diff options
Diffstat (limited to 'Projects/STM32WB5MM-DK/Applications/BLE/BLE_Mesh_Model_Sensor/Core/Src/vl53l0x/vl53l0x_api_calibration.c')
| -rw-r--r-- | Projects/STM32WB5MM-DK/Applications/BLE/BLE_Mesh_Model_Sensor/Core/Src/vl53l0x/vl53l0x_api_calibration.c | 1259 |
1 files changed, 1259 insertions, 0 deletions
diff --git a/Projects/STM32WB5MM-DK/Applications/BLE/BLE_Mesh_Model_Sensor/Core/Src/vl53l0x/vl53l0x_api_calibration.c b/Projects/STM32WB5MM-DK/Applications/BLE/BLE_Mesh_Model_Sensor/Core/Src/vl53l0x/vl53l0x_api_calibration.c new file mode 100644 index 000000000..ba8243293 --- /dev/null +++ b/Projects/STM32WB5MM-DK/Applications/BLE/BLE_Mesh_Model_Sensor/Core/Src/vl53l0x/vl53l0x_api_calibration.c @@ -0,0 +1,1259 @@ +/** + ****************************************************************************** + * @attention + * + * Copyright (c) 2019-2021 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ + +#include "vl53l0x_api.h" +#include "vl53l0x_api_core.h" +#include "vl53l0x_api_calibration.h" + +#ifndef __KERNEL__ +#include <stdlib.h> +#endif + +#define LOG_FUNCTION_START(fmt, ...) \ + _LOG_FUNCTION_START(TRACE_MODULE_API, fmt, ##__VA_ARGS__) +#define LOG_FUNCTION_END(status, ...) \ + _LOG_FUNCTION_END(TRACE_MODULE_API, status, ##__VA_ARGS__) +#define LOG_FUNCTION_END_FMT(status, fmt, ...) \ + _LOG_FUNCTION_END_FMT(TRACE_MODULE_API, status, fmt, ##__VA_ARGS__) + +#define REF_ARRAY_SPAD_0 0 +#define REF_ARRAY_SPAD_5 5 +#define REF_ARRAY_SPAD_10 10 + +uint32_t refArrayQuadrants[4] = {REF_ARRAY_SPAD_10, REF_ARRAY_SPAD_5, + REF_ARRAY_SPAD_0, REF_ARRAY_SPAD_5 }; + +VL53L0X_Error VL53L0X_perform_xtalk_calibration(VL53L0X_DEV Dev, + FixPoint1616_t XTalkCalDistance, + FixPoint1616_t *pXTalkCompensationRateMegaCps) +{ + VL53L0X_Error Status = VL53L0X_ERROR_NONE; + uint16_t sum_ranging = 0; + uint16_t sum_spads = 0; + FixPoint1616_t sum_signalRate = 0; + FixPoint1616_t total_count = 0; + uint8_t xtalk_meas = 0; + VL53L0X_RangingMeasurementData_t RangingMeasurementData; + FixPoint1616_t xTalkStoredMeanSignalRate; + FixPoint1616_t xTalkStoredMeanRange; + FixPoint1616_t xTalkStoredMeanRtnSpads; + uint32_t signalXTalkTotalPerSpad; + uint32_t xTalkStoredMeanRtnSpadsAsInt; + uint32_t xTalkCalDistanceAsInt; + FixPoint1616_t XTalkCompensationRateMegaCps; + + if (XTalkCalDistance <= 0) + Status = VL53L0X_ERROR_INVALID_PARAMS; + + /* Disable the XTalk compensation */ + if (Status == VL53L0X_ERROR_NONE) + Status = VL53L0X_SetXTalkCompensationEnable(Dev, 0); + + /* Disable the RIT */ + if (Status == VL53L0X_ERROR_NONE) { + Status = VL53L0X_SetLimitCheckEnable(Dev, + VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, 0); + } + + /* Perform 50 measurements and compute the averages */ + if (Status == VL53L0X_ERROR_NONE) { + sum_ranging = 0; + sum_spads = 0; + sum_signalRate = 0; + total_count = 0; + for (xtalk_meas = 0; xtalk_meas < 50; xtalk_meas++) { + Status = VL53L0X_PerformSingleRangingMeasurement(Dev, + &RangingMeasurementData); + + if (Status != VL53L0X_ERROR_NONE) + break; + + /* The range is valid when RangeStatus = 0 */ + if (RangingMeasurementData.RangeStatus == 0) { + sum_ranging = sum_ranging + + RangingMeasurementData.RangeMilliMeter; + sum_signalRate = sum_signalRate + + RangingMeasurementData.SignalRateRtnMegaCps; + sum_spads = sum_spads + + RangingMeasurementData.EffectiveSpadRtnCount + / 256; + total_count = total_count + 1; + } + } + + /* no valid values found */ + if (total_count == 0) + Status = VL53L0X_ERROR_RANGE_ERROR; + + } + + + if (Status == VL53L0X_ERROR_NONE) { + /* FixPoint1616_t / uint16_t = FixPoint1616_t */ + xTalkStoredMeanSignalRate = sum_signalRate / total_count; + xTalkStoredMeanRange = (FixPoint1616_t)((uint32_t)( + sum_ranging << 16) / total_count); + xTalkStoredMeanRtnSpads = (FixPoint1616_t)((uint32_t)( + sum_spads << 16) / total_count); + + /* Round Mean Spads to Whole Number. + * Typically the calculated mean SPAD count is a whole number + * or very close to a whole + * number, therefore any truncation will not result in a + * significant loss in accuracy. + * Also, for a grey target at a typical distance of around + * 400mm, around 220 SPADs will + * be enabled, therefore, any truncation will result in a loss + * of accuracy of less than + * 0.5%. + */ + xTalkStoredMeanRtnSpadsAsInt = (xTalkStoredMeanRtnSpads + + 0x8000) >> 16; + + /* Round Cal Distance to Whole Number. + * Note that the cal distance is in mm, therefore no resolution + * is lost.*/ + xTalkCalDistanceAsInt = (XTalkCalDistance + 0x8000) >> 16; + + if (xTalkStoredMeanRtnSpadsAsInt == 0 || + xTalkCalDistanceAsInt == 0 || + xTalkStoredMeanRange >= XTalkCalDistance) { + XTalkCompensationRateMegaCps = 0; + } else { + /* Round Cal Distance to Whole Number. + Note that the cal distance is in mm, therefore no + resolution is lost.*/ + xTalkCalDistanceAsInt = (XTalkCalDistance + + 0x8000) >> 16; + + /* Apply division by mean spad count early in the + * calculation to keep the numbers small. + * This ensures we can maintain a 32bit calculation. + * Fixed1616 / int := Fixed1616 */ + signalXTalkTotalPerSpad = (xTalkStoredMeanSignalRate) / + xTalkStoredMeanRtnSpadsAsInt; + + /* Complete the calculation for total Signal XTalk per + * SPAD + * Fixed1616 * (Fixed1616 - Fixed1616/int) := + * (2^16 * Fixed1616) + */ + signalXTalkTotalPerSpad *= ((1 << 16) - + (xTalkStoredMeanRange / xTalkCalDistanceAsInt)); + + /* Round from 2^16 * Fixed1616, to Fixed1616. */ + XTalkCompensationRateMegaCps = (signalXTalkTotalPerSpad + + 0x8000) >> 16; + } + + *pXTalkCompensationRateMegaCps = XTalkCompensationRateMegaCps; + + /* Enable the XTalk compensation */ + if (Status == VL53L0X_ERROR_NONE) + Status = VL53L0X_SetXTalkCompensationEnable(Dev, 1); + + /* Enable the XTalk compensation */ + if (Status == VL53L0X_ERROR_NONE) + Status = VL53L0X_SetXTalkCompensationRateMegaCps(Dev, + XTalkCompensationRateMegaCps); + + } + + return Status; +} + +VL53L0X_Error VL53L0X_perform_offset_calibration(VL53L0X_DEV Dev, + FixPoint1616_t CalDistanceMilliMeter, + int32_t *pOffsetMicroMeter) +{ + VL53L0X_Error Status = VL53L0X_ERROR_NONE; + uint16_t sum_ranging = 0; + FixPoint1616_t total_count = 0; + VL53L0X_RangingMeasurementData_t RangingMeasurementData; + FixPoint1616_t StoredMeanRange; + uint32_t StoredMeanRangeAsInt; + uint32_t CalDistanceAsInt_mm; + uint8_t SequenceStepEnabled; + int meas = 0; + + if (CalDistanceMilliMeter <= 0) + Status = VL53L0X_ERROR_INVALID_PARAMS; + + if (Status == VL53L0X_ERROR_NONE) + Status = VL53L0X_SetOffsetCalibrationDataMicroMeter(Dev, 0); + + + /* Get the value of the TCC */ + if (Status == VL53L0X_ERROR_NONE) + Status = VL53L0X_GetSequenceStepEnable(Dev, + VL53L0X_SEQUENCESTEP_TCC, &SequenceStepEnabled); + + + /* Disable the TCC */ + if (Status == VL53L0X_ERROR_NONE) + Status = VL53L0X_SetSequenceStepEnable(Dev, + VL53L0X_SEQUENCESTEP_TCC, 0); + + + /* Disable the RIT */ + if (Status == VL53L0X_ERROR_NONE) + Status = VL53L0X_SetLimitCheckEnable(Dev, + VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, 0); + + /* Perform 50 measurements and compute the averages */ + if (Status == VL53L0X_ERROR_NONE) { + sum_ranging = 0; + total_count = 0; + for (meas = 0; meas < 50; meas++) { + Status = VL53L0X_PerformSingleRangingMeasurement(Dev, + &RangingMeasurementData); + + if (Status != VL53L0X_ERROR_NONE) + break; + + /* The range is valid when RangeStatus = 0 */ + if (RangingMeasurementData.RangeStatus == 0) { + sum_ranging = sum_ranging + + RangingMeasurementData.RangeMilliMeter; + total_count = total_count + 1; + } + } + + /* no valid values found */ + if (total_count == 0) + Status = VL53L0X_ERROR_RANGE_ERROR; + } + + + if (Status == VL53L0X_ERROR_NONE) { + /* FixPoint1616_t / uint16_t = FixPoint1616_t */ + StoredMeanRange = (FixPoint1616_t)((uint32_t)(sum_ranging << 16) + / total_count); + + StoredMeanRangeAsInt = (StoredMeanRange + 0x8000) >> 16; + + /* Round Cal Distance to Whole Number. + * Note that the cal distance is in mm, therefore no resolution + * is lost.*/ + CalDistanceAsInt_mm = (CalDistanceMilliMeter + 0x8000) >> 16; + + *pOffsetMicroMeter = (CalDistanceAsInt_mm - + StoredMeanRangeAsInt) * 1000; + + /* Apply the calculated offset */ + if (Status == VL53L0X_ERROR_NONE) { + VL53L0X_SETPARAMETERFIELD(Dev, RangeOffsetMicroMeters, + *pOffsetMicroMeter); + Status = VL53L0X_SetOffsetCalibrationDataMicroMeter(Dev, + *pOffsetMicroMeter); + } + + } + + /* Restore the TCC */ + if (Status == VL53L0X_ERROR_NONE) { + if (SequenceStepEnabled != 0) + Status = VL53L0X_SetSequenceStepEnable(Dev, + VL53L0X_SEQUENCESTEP_TCC, 1); + } + + return Status; +} + + +VL53L0X_Error VL53L0X_set_offset_calibration_data_micro_meter(VL53L0X_DEV Dev, + int32_t OffsetCalibrationDataMicroMeter) +{ + VL53L0X_Error Status = VL53L0X_ERROR_NONE; + int32_t cMaxOffsetMicroMeter = 511000; + int32_t cMinOffsetMicroMeter = -512000; + int16_t cOffsetRange = 4096; + uint32_t encodedOffsetVal; + + LOG_FUNCTION_START(""); + + if (OffsetCalibrationDataMicroMeter > cMaxOffsetMicroMeter) + OffsetCalibrationDataMicroMeter = cMaxOffsetMicroMeter; + else if (OffsetCalibrationDataMicroMeter < cMinOffsetMicroMeter) + OffsetCalibrationDataMicroMeter = cMinOffsetMicroMeter; + + /* The offset register is 10.2 format and units are mm + * therefore conversion is applied by a division of + * 250. + */ + if (OffsetCalibrationDataMicroMeter >= 0) { + encodedOffsetVal = + OffsetCalibrationDataMicroMeter/250; + } else { + encodedOffsetVal = + cOffsetRange + + OffsetCalibrationDataMicroMeter/250; + } + + Status = VL53L0X_WrWord(Dev, + VL53L0X_REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM, + encodedOffsetVal); + + LOG_FUNCTION_END(Status); + return Status; +} + +VL53L0X_Error VL53L0X_get_offset_calibration_data_micro_meter(VL53L0X_DEV Dev, + int32_t *pOffsetCalibrationDataMicroMeter) +{ + VL53L0X_Error Status = VL53L0X_ERROR_NONE; + uint16_t RangeOffsetRegister; + int16_t cMaxOffset = 2047; + int16_t cOffsetRange = 4096; + + /* Note that offset has 10.2 format */ + + Status = VL53L0X_RdWord(Dev, + VL53L0X_REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM, + &RangeOffsetRegister); + + if (Status == VL53L0X_ERROR_NONE) { + RangeOffsetRegister = (RangeOffsetRegister & 0x0fff); + + /* Apply 12 bit 2's compliment conversion */ + if (RangeOffsetRegister > cMaxOffset) + *pOffsetCalibrationDataMicroMeter = + (int16_t)(RangeOffsetRegister - cOffsetRange) + * 250; + else + *pOffsetCalibrationDataMicroMeter = + (int16_t)RangeOffsetRegister * 250; + + } + + return Status; +} + + +VL53L0X_Error VL53L0X_apply_offset_adjustment(VL53L0X_DEV Dev) +{ + VL53L0X_Error Status = VL53L0X_ERROR_NONE; + int32_t CorrectedOffsetMicroMeters; + int32_t CurrentOffsetMicroMeters; + + /* if we run on this function we can read all the NVM info + * used by the API */ + Status = VL53L0X_get_info_from_device(Dev, 7); + + /* Read back current device offset */ + if (Status == VL53L0X_ERROR_NONE) { + Status = VL53L0X_GetOffsetCalibrationDataMicroMeter(Dev, + &CurrentOffsetMicroMeters); + } + + /* Apply Offset Adjustment derived from 400mm measurements */ + if (Status == VL53L0X_ERROR_NONE) { + + /* Store initial device offset */ + PALDevDataSet(Dev, Part2PartOffsetNVMMicroMeter, + CurrentOffsetMicroMeters); + + CorrectedOffsetMicroMeters = CurrentOffsetMicroMeters + + (int32_t)PALDevDataGet(Dev, + Part2PartOffsetAdjustmentNVMMicroMeter); + + Status = VL53L0X_SetOffsetCalibrationDataMicroMeter(Dev, + CorrectedOffsetMicroMeters); + + /* store current, adjusted offset */ + if (Status == VL53L0X_ERROR_NONE) { + VL53L0X_SETPARAMETERFIELD(Dev, RangeOffsetMicroMeters, + CorrectedOffsetMicroMeters); + } + } + + return Status; +} + +void get_next_good_spad(uint8_t goodSpadArray[], uint32_t size, + uint32_t curr, int32_t *next) +{ + uint32_t startIndex; + uint32_t fineOffset; + uint32_t cSpadsPerByte = 8; + uint32_t coarseIndex; + uint32_t fineIndex; + uint8_t dataByte; + uint8_t success = 0; + + /* + * Starting with the current good spad, loop through the array to find + * the next. i.e. the next bit set in the sequence. + * + * The coarse index is the byte index of the array and the fine index is + * the index of the bit within each byte. + */ + + *next = -1; + + startIndex = curr / cSpadsPerByte; + fineOffset = curr % cSpadsPerByte; + + for (coarseIndex = startIndex; ((coarseIndex < size) && !success); + coarseIndex++) { + fineIndex = 0; + dataByte = goodSpadArray[coarseIndex]; + + if (coarseIndex == startIndex) { + /* locate the bit position of the provided current + * spad bit before iterating */ + dataByte >>= fineOffset; + fineIndex = fineOffset; + } + + while (fineIndex < cSpadsPerByte) { + if ((dataByte & 0x1) == 1) { + success = 1; + *next = coarseIndex * cSpadsPerByte + fineIndex; + break; + } + dataByte >>= 1; + fineIndex++; + } + } +} + + +uint8_t is_aperture(uint32_t spadIndex) +{ + /* + * This function reports if a given spad index is an aperture SPAD by + * deriving the quadrant. + */ + uint32_t quadrant; + uint8_t isAperture = 1; + quadrant = spadIndex >> 6; + if (refArrayQuadrants[quadrant] == REF_ARRAY_SPAD_0) + isAperture = 0; + + return isAperture; +} + + +VL53L0X_Error enable_spad_bit(uint8_t spadArray[], uint32_t size, + uint32_t spadIndex) +{ + VL53L0X_Error status = VL53L0X_ERROR_NONE; + uint32_t cSpadsPerByte = 8; + uint32_t coarseIndex; + uint32_t fineIndex; + + coarseIndex = spadIndex / cSpadsPerByte; + fineIndex = spadIndex % cSpadsPerByte; + if (coarseIndex >= size) + status = VL53L0X_ERROR_REF_SPAD_INIT; + else + spadArray[coarseIndex] |= (1 << fineIndex); + + return status; +} + +VL53L0X_Error count_enabled_spads(uint8_t spadArray[], + uint32_t byteCount, uint32_t maxSpads, + uint32_t *pTotalSpadsEnabled, uint8_t *pIsAperture) +{ + VL53L0X_Error status = VL53L0X_ERROR_NONE; + uint32_t cSpadsPerByte = 8; + uint32_t lastByte; + uint32_t lastBit; + uint32_t byteIndex = 0; + uint32_t bitIndex = 0; + uint8_t tempByte; + uint8_t spadTypeIdentified = 0; + + /* The entire array will not be used for spads, therefore the last + * byte and last bit is determined from the max spads value. + */ + + lastByte = maxSpads / cSpadsPerByte; + lastBit = maxSpads % cSpadsPerByte; + + /* Check that the max spads value does not exceed the array bounds. */ + if (lastByte >= byteCount) + status = VL53L0X_ERROR_REF_SPAD_INIT; + + *pTotalSpadsEnabled = 0; + + /* Count the bits enabled in the whole bytes */ + for (byteIndex = 0; byteIndex <= (lastByte - 1); byteIndex++) { + tempByte = spadArray[byteIndex]; + + for (bitIndex = 0; bitIndex <= cSpadsPerByte; bitIndex++) { + if ((tempByte & 0x01) == 1) { + (*pTotalSpadsEnabled)++; + + if (!spadTypeIdentified) { + *pIsAperture = 1; + if ((byteIndex < 2) && (bitIndex < 4)) + *pIsAperture = 0; + spadTypeIdentified = 1; + } + } + tempByte >>= 1; + } + } + + /* Count the number of bits enabled in the last byte accounting + * for the fact that not all bits in the byte may be used. + */ + tempByte = spadArray[lastByte]; + + for (bitIndex = 0; bitIndex <= lastBit; bitIndex++) { + if ((tempByte & 0x01) == 1) + (*pTotalSpadsEnabled)++; + } + + return status; +} + +VL53L0X_Error set_ref_spad_map(VL53L0X_DEV Dev, uint8_t *refSpadArray) +{ + VL53L0X_Error status = VL53L0X_WriteMulti(Dev, + VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0, + refSpadArray, 6); + return status; +} + +VL53L0X_Error get_ref_spad_map(VL53L0X_DEV Dev, uint8_t *refSpadArray) +{ + VL53L0X_Error status = VL53L0X_ReadMulti(Dev, + VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0, + refSpadArray, + 6); + return status; +} + +VL53L0X_Error enable_ref_spads(VL53L0X_DEV Dev, + uint8_t apertureSpads, + uint8_t goodSpadArray[], + uint8_t spadArray[], + uint32_t size, + uint32_t start, + uint32_t offset, + uint32_t spadCount, + uint32_t *lastSpad) +{ + VL53L0X_Error status = VL53L0X_ERROR_NONE; + uint32_t index; + uint32_t i; + int32_t nextGoodSpad = offset; + uint32_t currentSpad; + uint8_t checkSpadArray[6]; + + /* + * This function takes in a spad array which may or may not have SPADS + * already enabled and appends from a given offset a requested number + * of new SPAD enables. The 'good spad map' is applied to + * determine the next SPADs to enable. + * + * This function applies to only aperture or only non-aperture spads. + * Checks are performed to ensure this. + */ + + currentSpad = offset; + for (index = 0; index < spadCount; index++) { + get_next_good_spad(goodSpadArray, size, currentSpad, + &nextGoodSpad); + + if (nextGoodSpad == -1) { + status = VL53L0X_ERROR_REF_SPAD_INIT; + break; + } + + /* Confirm that the next good SPAD is non-aperture */ + if (is_aperture(start + nextGoodSpad) != apertureSpads) { + /* if we can't get the required number of good aperture + * spads from the current quadrant then this is an error + */ + status = VL53L0X_ERROR_REF_SPAD_INIT; + break; + } + currentSpad = (uint32_t)nextGoodSpad; + enable_spad_bit(spadArray, size, currentSpad); + currentSpad++; + } + *lastSpad = currentSpad; + + if (status == VL53L0X_ERROR_NONE) + status = set_ref_spad_map(Dev, spadArray); + + + if (status == VL53L0X_ERROR_NONE) { + status = get_ref_spad_map(Dev, checkSpadArray); + + i = 0; + + /* Compare spad maps. If not equal report error. */ + while (i < size) { + if (spadArray[i] != checkSpadArray[i]) { + status = VL53L0X_ERROR_REF_SPAD_INIT; + break; + } + i++; + } + } + return status; +} + + +VL53L0X_Error perform_ref_signal_measurement(VL53L0X_DEV Dev, + uint16_t *refSignalRate) +{ + VL53L0X_Error status = VL53L0X_ERROR_NONE; + VL53L0X_RangingMeasurementData_t rangingMeasurementData; + + uint8_t SequenceConfig = 0; + + /* store the value of the sequence config, + * this will be reset before the end of the function + */ + + SequenceConfig = PALDevDataGet(Dev, SequenceConfig); + + /* + * This function performs a reference signal rate measurement. + */ + if (status == VL53L0X_ERROR_NONE) + status = VL53L0X_WrByte(Dev, + VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0xC0); + + if (status == VL53L0X_ERROR_NONE) + status = VL53L0X_PerformSingleRangingMeasurement(Dev, + &rangingMeasurementData); + + if (status == VL53L0X_ERROR_NONE) + status = VL53L0X_WrByte(Dev, 0xFF, 0x01); + + if (status == VL53L0X_ERROR_NONE) + status = VL53L0X_RdWord(Dev, + VL53L0X_REG_RESULT_PEAK_SIGNAL_RATE_REF, + refSignalRate); + + if (status == VL53L0X_ERROR_NONE) + status = VL53L0X_WrByte(Dev, 0xFF, 0x00); + + if (status == VL53L0X_ERROR_NONE) { + /* restore the previous Sequence Config */ + status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, + SequenceConfig); + if (status == VL53L0X_ERROR_NONE) + PALDevDataSet(Dev, SequenceConfig, SequenceConfig); + } + + return status; +} + +VL53L0X_Error VL53L0X_perform_ref_spad_management(VL53L0X_DEV Dev, + uint32_t *refSpadCount, + uint8_t *isApertureSpads) +{ + VL53L0X_Error Status = VL53L0X_ERROR_NONE; + uint8_t lastSpadArray[6]; + uint8_t startSelect = 0xB4; + uint32_t minimumSpadCount = 3; + uint32_t maxSpadCount = 44; + uint32_t currentSpadIndex = 0; + uint32_t lastSpadIndex = 0; + int32_t nextGoodSpad = 0; + uint16_t targetRefRate = 0x0A00; /* 20 MCPS in 9:7 format */ + uint16_t peakSignalRateRef; + uint32_t needAptSpads = 0; + uint32_t index = 0; + uint32_t spadArraySize = 6; + uint32_t signalRateDiff = 0; + uint32_t lastSignalRateDiff = 0; + uint8_t complete = 0; + uint8_t VhvSettings = 0; + uint8_t PhaseCal = 0; + uint32_t refSpadCount_int = 0; + uint8_t isApertureSpads_int = 0; + + /* + * The reference SPAD initialization procedure determines the minimum + * amount of reference spads to be enables to achieve a target reference + * signal rate and should be performed once during initialization. + * + * Either aperture or non-aperture spads are applied but never both. + * Firstly non-aperture spads are set, begining with 5 spads, and + * increased one spad at a time until the closest measurement to the + * target rate is achieved. + * + * If the target rate is exceeded when 5 non-aperture spads are enabled, + * initialization is performed instead with aperture spads. + * + * When setting spads, a 'Good Spad Map' is applied. + * + * This procedure operates within a SPAD window of interest of a maximum + * 44 spads. + * The start point is currently fixed to 180, which lies towards the end + * of the non-aperture quadrant and runs in to the adjacent aperture + * quadrant. + */ + + + targetRefRate = PALDevDataGet(Dev, targetRefRate); + + /* + * Initialize Spad arrays. + * Currently the good spad map is initialised to 'All good'. + * This is a short term implementation. The good spad map will be + * provided as an input. + * Note that there are 6 bytes. Only the first 44 bits will be used to + * represent spads. + */ + for (index = 0; index < spadArraySize; index++) + Dev->Data.SpadData.RefSpadEnables[index] = 0; + + + Status = VL53L0X_WrByte(Dev, 0xFF, 0x01); + + if (Status == VL53L0X_ERROR_NONE) + Status = VL53L0X_WrByte(Dev, + VL53L0X_REG_DYNAMIC_SPAD_REF_EN_START_OFFSET, 0x00); + + if (Status == VL53L0X_ERROR_NONE) + Status = VL53L0X_WrByte(Dev, + VL53L0X_REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD, 0x2C); + + if (Status == VL53L0X_ERROR_NONE) + Status = VL53L0X_WrByte(Dev, 0xFF, 0x00); + + if (Status == VL53L0X_ERROR_NONE) + Status = VL53L0X_WrByte(Dev, + VL53L0X_REG_GLOBAL_CONFIG_REF_EN_START_SELECT, + startSelect); + + + if (Status == VL53L0X_ERROR_NONE) + Status = VL53L0X_WrByte(Dev, + VL53L0X_REG_POWER_MANAGEMENT_GO1_POWER_FORCE, 0); + + /* Perform ref calibration */ + if (Status == VL53L0X_ERROR_NONE) + Status = VL53L0X_perform_ref_calibration(Dev, &VhvSettings, + &PhaseCal, 0); + + if (Status == VL53L0X_ERROR_NONE) { + /* Enable Minimum NON-APERTURE Spads */ + currentSpadIndex = 0; + lastSpadIndex = currentSpadIndex; + needAptSpads = 0; + Status = enable_ref_spads(Dev, + needAptSpads, + Dev->Data.SpadData.RefGoodSpadMap, + Dev->Data.SpadData.RefSpadEnables, + spadArraySize, + startSelect, + currentSpadIndex, + minimumSpadCount, + &lastSpadIndex); + } + + if (Status == VL53L0X_ERROR_NONE) { + currentSpadIndex = lastSpadIndex; + + Status = perform_ref_signal_measurement(Dev, + &peakSignalRateRef); + if ((Status == VL53L0X_ERROR_NONE) && + (peakSignalRateRef > targetRefRate)) { + /* Signal rate measurement too high, + * switch to APERTURE SPADs */ + + for (index = 0; index < spadArraySize; index++) + Dev->Data.SpadData.RefSpadEnables[index] = 0; + + + /* Increment to the first APERTURE spad */ + while ((is_aperture(startSelect + currentSpadIndex) + == 0) && (currentSpadIndex < maxSpadCount)) { + currentSpadIndex++; + } + + needAptSpads = 1; + + Status = enable_ref_spads(Dev, + needAptSpads, + Dev->Data.SpadData.RefGoodSpadMap, + Dev->Data.SpadData.RefSpadEnables, + spadArraySize, + startSelect, + currentSpadIndex, + minimumSpadCount, + &lastSpadIndex); + + if (Status == VL53L0X_ERROR_NONE) { + currentSpadIndex = lastSpadIndex; + Status = perform_ref_signal_measurement(Dev, + &peakSignalRateRef); + + if ((Status == VL53L0X_ERROR_NONE) && + (peakSignalRateRef > targetRefRate)) { + /* Signal rate still too high after + * setting the minimum number of + * APERTURE spads. Can do no more + * therefore set the min number of + * aperture spads as the result. + */ + isApertureSpads_int = 1; + refSpadCount_int = minimumSpadCount; + } + } + } else { + needAptSpads = 0; + } + } + + if ((Status == VL53L0X_ERROR_NONE) && + (peakSignalRateRef < targetRefRate)) { + /* At this point, the minimum number of either aperture + * or non-aperture spads have been set. Proceed to add + * spads and perform measurements until the target + * reference is reached. + */ + isApertureSpads_int = needAptSpads; + refSpadCount_int = minimumSpadCount; + + memcpy(lastSpadArray, Dev->Data.SpadData.RefSpadEnables, + spadArraySize); + lastSignalRateDiff = abs(peakSignalRateRef - + targetRefRate); + complete = 0; + + while (!complete) { + get_next_good_spad( + Dev->Data.SpadData.RefGoodSpadMap, + spadArraySize, currentSpadIndex, + &nextGoodSpad); + + if (nextGoodSpad == -1) { + Status = VL53L0X_ERROR_REF_SPAD_INIT; + break; + } + + (refSpadCount_int)++; + + /* Cannot combine Aperture and Non-Aperture spads, so + * ensure the current spad is of the correct type. + */ + if (is_aperture((uint32_t)startSelect + nextGoodSpad) != + needAptSpads) { + Status = VL53L0X_ERROR_REF_SPAD_INIT; + break; + } + + currentSpadIndex = nextGoodSpad; + Status = enable_spad_bit( + Dev->Data.SpadData.RefSpadEnables, + spadArraySize, currentSpadIndex); + + if (Status == VL53L0X_ERROR_NONE) { + currentSpadIndex++; + /* Proceed to apply the additional spad and + * perform measurement. */ + Status = set_ref_spad_map(Dev, + Dev->Data.SpadData.RefSpadEnables); + } + + if (Status != VL53L0X_ERROR_NONE) + break; + + Status = perform_ref_signal_measurement(Dev, + &peakSignalRateRef); + + if (Status != VL53L0X_ERROR_NONE) + break; + + signalRateDiff = abs(peakSignalRateRef - targetRefRate); + + if (peakSignalRateRef > targetRefRate) { + /* Select the spad map that provides the + * measurement closest to the target rate, + * either above or below it. + */ + if (signalRateDiff > lastSignalRateDiff) { + /* Previous spad map produced a closer + * measurement, so choose this. */ + Status = set_ref_spad_map(Dev, + lastSpadArray); + memcpy( + Dev->Data.SpadData.RefSpadEnables, + lastSpadArray, spadArraySize); + + (refSpadCount_int)--; + } + complete = 1; + } else { + /* Continue to add spads */ + lastSignalRateDiff = signalRateDiff; + memcpy(lastSpadArray, + Dev->Data.SpadData.RefSpadEnables, + spadArraySize); + } + + } /* while */ + } + + if (Status == VL53L0X_ERROR_NONE) { + *refSpadCount = refSpadCount_int; + *isApertureSpads = isApertureSpads_int; + + VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, RefSpadsInitialised, 1); + VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, + ReferenceSpadCount, (uint8_t)(*refSpadCount)); + VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, + ReferenceSpadType, *isApertureSpads); + } + + return Status; +} + +VL53L0X_Error VL53L0X_set_reference_spads(VL53L0X_DEV Dev, + uint32_t count, uint8_t isApertureSpads) +{ + VL53L0X_Error Status = VL53L0X_ERROR_NONE; + uint32_t currentSpadIndex = 0; + uint8_t startSelect = 0xB4; + uint32_t spadArraySize = 6; + uint32_t maxSpadCount = 44; + uint32_t lastSpadIndex; + uint32_t index; + + /* + * This function applies a requested number of reference spads, either + * aperture or + * non-aperture, as requested. + * The good spad map will be applied. + */ + + Status = VL53L0X_WrByte(Dev, 0xFF, 0x01); + + if (Status == VL53L0X_ERROR_NONE) + Status = VL53L0X_WrByte(Dev, + VL53L0X_REG_DYNAMIC_SPAD_REF_EN_START_OFFSET, 0x00); + + if (Status == VL53L0X_ERROR_NONE) + Status = VL53L0X_WrByte(Dev, + VL53L0X_REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD, 0x2C); + + if (Status == VL53L0X_ERROR_NONE) + Status = VL53L0X_WrByte(Dev, 0xFF, 0x00); + + if (Status == VL53L0X_ERROR_NONE) + Status = VL53L0X_WrByte(Dev, + VL53L0X_REG_GLOBAL_CONFIG_REF_EN_START_SELECT, + startSelect); + + for (index = 0; index < spadArraySize; index++) + Dev->Data.SpadData.RefSpadEnables[index] = 0; + + if (isApertureSpads) { + /* Increment to the first APERTURE spad */ + while ((is_aperture(startSelect + currentSpadIndex) == 0) && + (currentSpadIndex < maxSpadCount)) { + currentSpadIndex++; + } + } + Status = enable_ref_spads(Dev, + isApertureSpads, + Dev->Data.SpadData.RefGoodSpadMap, + Dev->Data.SpadData.RefSpadEnables, + spadArraySize, + startSelect, + currentSpadIndex, + count, + &lastSpadIndex); + + if (Status == VL53L0X_ERROR_NONE) { + VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, RefSpadsInitialised, 1); + VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, + ReferenceSpadCount, (uint8_t)(count)); + VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, + ReferenceSpadType, isApertureSpads); + } + + return Status; +} + +VL53L0X_Error VL53L0X_get_reference_spads(VL53L0X_DEV Dev, + uint32_t *pSpadCount, uint8_t *pIsApertureSpads) +{ + VL53L0X_Error Status = VL53L0X_ERROR_NONE; + uint8_t refSpadsInitialised; + uint8_t refSpadArray[6]; + uint32_t cMaxSpadCount = 44; + uint32_t cSpadArraySize = 6; + uint32_t spadsEnabled; + uint8_t isApertureSpads = 0; + + refSpadsInitialised = VL53L0X_GETDEVICESPECIFICPARAMETER(Dev, + RefSpadsInitialised); + + if (refSpadsInitialised == 1) { + + *pSpadCount = (uint32_t)VL53L0X_GETDEVICESPECIFICPARAMETER(Dev, + ReferenceSpadCount); + *pIsApertureSpads = VL53L0X_GETDEVICESPECIFICPARAMETER(Dev, + ReferenceSpadType); + } else { + + /* obtain spad info from device.*/ + Status = get_ref_spad_map(Dev, refSpadArray); + + if (Status == VL53L0X_ERROR_NONE) { + /* count enabled spads within spad map array and + * determine if Aperture or Non-Aperture. + */ + Status = count_enabled_spads(refSpadArray, + cSpadArraySize, + cMaxSpadCount, + &spadsEnabled, + &isApertureSpads); + + if (Status == VL53L0X_ERROR_NONE) { + + *pSpadCount = spadsEnabled; + *pIsApertureSpads = isApertureSpads; + + VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, + RefSpadsInitialised, 1); + VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, + ReferenceSpadCount, + (uint8_t)spadsEnabled); + VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, + ReferenceSpadType, isApertureSpads); + } + } + } + + return Status; +} + + +VL53L0X_Error VL53L0X_perform_single_ref_calibration(VL53L0X_DEV Dev, + uint8_t vhv_init_byte) +{ + VL53L0X_Error Status = VL53L0X_ERROR_NONE; + + if (Status == VL53L0X_ERROR_NONE) + Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSRANGE_START, + VL53L0X_REG_SYSRANGE_MODE_START_STOP | + vhv_init_byte); + + if (Status == VL53L0X_ERROR_NONE) + Status = VL53L0X_measurement_poll_for_completion(Dev); + + if (Status == VL53L0X_ERROR_NONE) + Status = VL53L0X_ClearInterruptMask(Dev, 0); + + if (Status == VL53L0X_ERROR_NONE) + Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSRANGE_START, 0x00); + + return Status; +} + + +VL53L0X_Error VL53L0X_ref_calibration_io(VL53L0X_DEV Dev, uint8_t read_not_write, + uint8_t VhvSettings, uint8_t PhaseCal, + uint8_t *pVhvSettings, uint8_t *pPhaseCal, + const uint8_t vhv_enable, const uint8_t phase_enable) +{ + VL53L0X_Error Status = VL53L0X_ERROR_NONE; + uint8_t PhaseCalint = 0; + + /* Read VHV from device */ + Status |= VL53L0X_WrByte(Dev, 0xFF, 0x01); + Status |= VL53L0X_WrByte(Dev, 0x00, 0x00); + Status |= VL53L0X_WrByte(Dev, 0xFF, 0x00); + + if (read_not_write) { + if (vhv_enable) + Status |= VL53L0X_RdByte(Dev, 0xCB, pVhvSettings); + if (phase_enable) + Status |= VL53L0X_RdByte(Dev, 0xEE, &PhaseCalint); + } else { + if (vhv_enable) + Status |= VL53L0X_WrByte(Dev, 0xCB, VhvSettings); + if (phase_enable) + Status |= VL53L0X_UpdateByte(Dev, 0xEE, 0x80, PhaseCal); + } + + Status |= VL53L0X_WrByte(Dev, 0xFF, 0x01); + Status |= VL53L0X_WrByte(Dev, 0x00, 0x01); + Status |= VL53L0X_WrByte(Dev, 0xFF, 0x00); + + *pPhaseCal = (uint8_t)(PhaseCalint&0xEF); + + return Status; +} + + +VL53L0X_Error VL53L0X_perform_vhv_calibration(VL53L0X_DEV Dev, + uint8_t *pVhvSettings, const uint8_t get_data_enable, + const uint8_t restore_config) +{ + VL53L0X_Error Status = VL53L0X_ERROR_NONE; + uint8_t SequenceConfig = 0; + uint8_t VhvSettings = 0; + uint8_t PhaseCal = 0; + uint8_t PhaseCalInt = 0; + + /* store the value of the sequence config, + * this will be reset before the end of the function + */ + + if (restore_config) + SequenceConfig = PALDevDataGet(Dev, SequenceConfig); + + /* Run VHV */ + Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0x01); + + if (Status == VL53L0X_ERROR_NONE) + Status = VL53L0X_perform_single_ref_calibration(Dev, 0x40); + + /* Read VHV from device */ + if ((Status == VL53L0X_ERROR_NONE) && (get_data_enable == 1)) { + Status = VL53L0X_ref_calibration_io(Dev, 1, + VhvSettings, PhaseCal, /* Not used here */ + pVhvSettings, &PhaseCalInt, + 1, 0); + } else + *pVhvSettings = 0; + + + if ((Status == VL53L0X_ERROR_NONE) && restore_config) { + /* restore the previous Sequence Config */ + Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, + SequenceConfig); + if (Status == VL53L0X_ERROR_NONE) + PALDevDataSet(Dev, SequenceConfig, SequenceConfig); + + } + + return Status; +} + +VL53L0X_Error VL53L0X_perform_phase_calibration(VL53L0X_DEV Dev, + uint8_t *pPhaseCal, const uint8_t get_data_enable, + const uint8_t restore_config) +{ + VL53L0X_Error Status = VL53L0X_ERROR_NONE; + uint8_t SequenceConfig = 0; + uint8_t VhvSettings = 0; + uint8_t PhaseCal = 0; + uint8_t VhvSettingsint; + + /* store the value of the sequence config, + * this will be reset before the end of the function + */ + + if (restore_config) + SequenceConfig = PALDevDataGet(Dev, SequenceConfig); + + /* Run PhaseCal */ + Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0x02); + + if (Status == VL53L0X_ERROR_NONE) + Status = VL53L0X_perform_single_ref_calibration(Dev, 0x0); + + /* Read PhaseCal from device */ + if ((Status == VL53L0X_ERROR_NONE) && (get_data_enable == 1)) { + Status = VL53L0X_ref_calibration_io(Dev, 1, + VhvSettings, PhaseCal, /* Not used here */ + &VhvSettingsint, pPhaseCal, + 0, 1); + } else + *pPhaseCal = 0; + + + if ((Status == VL53L0X_ERROR_NONE) && restore_config) { + /* restore the previous Sequence Config */ + Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, + SequenceConfig); + if (Status == VL53L0X_ERROR_NONE) + PALDevDataSet(Dev, SequenceConfig, SequenceConfig); + + } + + return Status; +} + +VL53L0X_Error VL53L0X_perform_ref_calibration(VL53L0X_DEV Dev, + uint8_t *pVhvSettings, uint8_t *pPhaseCal, uint8_t get_data_enable) +{ + VL53L0X_Error Status = VL53L0X_ERROR_NONE; + uint8_t SequenceConfig = 0; + + /* store the value of the sequence config, + * this will be reset before the end of the function + */ + + SequenceConfig = PALDevDataGet(Dev, SequenceConfig); + + /* In the following function we don't save the config to optimize + * writes on device. Config is saved and restored only once. */ + Status = VL53L0X_perform_vhv_calibration( + Dev, pVhvSettings, get_data_enable, 0); + + + if (Status == VL53L0X_ERROR_NONE) + Status = VL53L0X_perform_phase_calibration( + Dev, pPhaseCal, get_data_enable, 0); + + + if (Status == VL53L0X_ERROR_NONE) { + /* restore the previous Sequence Config */ + Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, + SequenceConfig); + if (Status == VL53L0X_ERROR_NONE) + PALDevDataSet(Dev, SequenceConfig, SequenceConfig); + + } + + return Status; +} + +VL53L0X_Error VL53L0X_set_ref_calibration(VL53L0X_DEV Dev, + uint8_t VhvSettings, uint8_t PhaseCal) +{ + VL53L0X_Error Status = VL53L0X_ERROR_NONE; + uint8_t pVhvSettings; + uint8_t pPhaseCal; + + Status = VL53L0X_ref_calibration_io(Dev, 0, + VhvSettings, PhaseCal, + &pVhvSettings, &pPhaseCal, + 1, 1); + + return Status; +} + +VL53L0X_Error VL53L0X_get_ref_calibration(VL53L0X_DEV Dev, + uint8_t *pVhvSettings, uint8_t *pPhaseCal) +{ + VL53L0X_Error Status = VL53L0X_ERROR_NONE; + uint8_t VhvSettings = 0; + uint8_t PhaseCal = 0; + + Status = VL53L0X_ref_calibration_io(Dev, 1, + VhvSettings, PhaseCal, + pVhvSettings, pPhaseCal, + 1, 1); + + return Status; +} |