Fixed STMPE1600 writeSYS_CTRL, enabled multi sensors support
Dependencies: ST_INTERFACES X_NUCLEO_COMMON
Fork of X_NUCLEO_53L0A1 by
Components/VL53L0X/vl53l0x_class.cpp
- Committer:
- johnAlexander
- Date:
- 2016-12-01
- Revision:
- 2:58b5e9097aa3
- Parent:
- 0:c523920bcc09
- Child:
- 7:35ecf3e060c3
File content as of revision 2:58b5e9097aa3:
/** ****************************************************************************** * @file vl53l0x_class.cpp * @author IMG * @version V0.0.1 * @date 28-June-2016 * @brief Implementation file for the VL53L0X driver class ****************************************************************************** * @attention * * <h2><center>© COPYRIGHT(c) 2016 STMicroelectronics</center></h2> * * Redistribution and use in source and binary forms, with or without modification, * are permitted provided that the following conditions are met: * 1. Redistributions of source code must retain the above copyright notice, * this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright notice, * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. * 3. Neither the name of STMicroelectronics nor the names of its contributors * may be used to endorse or promote products derived from this software * without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************** */ /* Includes */ #include <stdlib.h> #include "vl53l0x_class.h" //#include "vl53l0x_api_core.h" //#include "vl53l0x_api_calibration.h" //#include "vl53l0x_api_strings.h" #include "vl53l0x_interrupt_threshold_settings.h" #include "vl53l0x_tuning.h" #include "vl53l0x_types.h" /****************** define for i2c configuration *******************************/ #define TEMP_BUF_SIZE 64 /** Maximum buffer size to be used in i2c */ #define VL53L0X_MAX_I2C_XFER_SIZE 64 /* Maximum buffer size to be used in i2c */ #define VL53L0X_I2C_USER_VAR /* none but could be for a flag var to get/pass to mutex interruptible return flags and try again */ #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__) #ifdef VL53L0X_LOG_ENABLE #define trace_print(level, ...) trace_print_module_function(TRACE_MODULE_API, \ level, TRACE_FUNCTION_NONE, ##__VA_ARGS__) #endif #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::VL53L0X_device_read_strobe(VL53L0X_DEV Dev) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; uint8_t strobe; uint32_t LoopNb; LOG_FUNCTION_START(""); Status |= VL53L0X_WrByte(Dev, 0x83, 0x00); /* polling * use timeout to avoid deadlock*/ if (Status == VL53L0X_ERROR_NONE) { LoopNb = 0; do { Status = VL53L0X_RdByte(Dev, 0x83, &strobe); if ((strobe != 0x00) || Status != VL53L0X_ERROR_NONE) break; LoopNb = LoopNb + 1; } while (LoopNb < VL53L0X_DEFAULT_MAX_LOOP); if (LoopNb >= VL53L0X_DEFAULT_MAX_LOOP) Status = VL53L0X_ERROR_TIME_OUT; } Status |= VL53L0X_WrByte(Dev, 0x83, 0x01); LOG_FUNCTION_END(Status); return Status; } VL53L0X_Error VL53L0X::VL53L0X_get_info_from_device(VL53L0X_DEV Dev, uint8_t option) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; uint8_t byte; uint32_t TmpDWord; uint8_t ModuleId; uint8_t Revision; uint8_t ReferenceSpadCount = 0; uint8_t ReferenceSpadType = 0; uint32_t PartUIDUpper = 0; uint32_t PartUIDLower = 0; uint32_t OffsetFixed1104_mm = 0; int16_t OffsetMicroMeters = 0; uint32_t DistMeasTgtFixed1104_mm = 400 << 4; uint32_t DistMeasFixed1104_400_mm = 0; uint32_t SignalRateMeasFixed1104_400_mm = 0; char ProductId[19]; char *ProductId_tmp; uint8_t ReadDataFromDeviceDone; FixPoint1616_t SignalRateMeasFixed400mmFix = 0; uint8_t NvmRefGoodSpadMap[VL53L0X_REF_SPAD_BUFFER_SIZE]; int i; LOG_FUNCTION_START(""); ReadDataFromDeviceDone = VL53L0X_GETDEVICESPECIFICPARAMETER(Dev, ReadDataFromDeviceDone); /* This access is done only once after that a GetDeviceInfo or * datainit is done*/ if (ReadDataFromDeviceDone != 7) { Status |= VL53L0X_WrByte(Dev, 0x80, 0x01); Status |= VL53L0X_WrByte(Dev, 0xFF, 0x01); Status |= VL53L0X_WrByte(Dev, 0x00, 0x00); Status |= VL53L0X_WrByte(Dev, 0xFF, 0x06); Status |= VL53L0X_RdByte(Dev, 0x83, &byte); Status |= VL53L0X_WrByte(Dev, 0x83, byte|4); Status |= VL53L0X_WrByte(Dev, 0xFF, 0x07); Status |= VL53L0X_WrByte(Dev, 0x81, 0x01); Status |= VL53L0X_PollingDelay(Dev); Status |= VL53L0X_WrByte(Dev, 0x80, 0x01); if (((option & 1) == 1) && ((ReadDataFromDeviceDone & 1) == 0)) { Status |= VL53L0X_WrByte(Dev, 0x94, 0x6b); Status |= VL53L0X_device_read_strobe(Dev); Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord); ReferenceSpadCount = (uint8_t)((TmpDWord >> 8) & 0x07f); ReferenceSpadType = (uint8_t)((TmpDWord >> 15) & 0x01); Status |= VL53L0X_WrByte(Dev, 0x94, 0x24); Status |= VL53L0X_device_read_strobe(Dev); Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord); NvmRefGoodSpadMap[0] = (uint8_t)((TmpDWord >> 24) & 0xff); NvmRefGoodSpadMap[1] = (uint8_t)((TmpDWord >> 16) & 0xff); NvmRefGoodSpadMap[2] = (uint8_t)((TmpDWord >> 8) & 0xff); NvmRefGoodSpadMap[3] = (uint8_t)(TmpDWord & 0xff); Status |= VL53L0X_WrByte(Dev, 0x94, 0x25); Status |= VL53L0X_device_read_strobe(Dev); Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord); NvmRefGoodSpadMap[4] = (uint8_t)((TmpDWord >> 24) & 0xff); NvmRefGoodSpadMap[5] = (uint8_t)((TmpDWord >> 16) & 0xff); } if (((option & 2) == 2) && ((ReadDataFromDeviceDone & 2) == 0)) { Status |= VL53L0X_WrByte(Dev, 0x94, 0x02); Status |= VL53L0X_device_read_strobe(Dev); Status |= VL53L0X_RdByte(Dev, 0x90, &ModuleId); Status |= VL53L0X_WrByte(Dev, 0x94, 0x7B); Status |= VL53L0X_device_read_strobe(Dev); Status |= VL53L0X_RdByte(Dev, 0x90, &Revision); Status |= VL53L0X_WrByte(Dev, 0x94, 0x77); Status |= VL53L0X_device_read_strobe(Dev); Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord); ProductId[0] = (char)((TmpDWord >> 25) & 0x07f); ProductId[1] = (char)((TmpDWord >> 18) & 0x07f); ProductId[2] = (char)((TmpDWord >> 11) & 0x07f); ProductId[3] = (char)((TmpDWord >> 4) & 0x07f); byte = (uint8_t)((TmpDWord & 0x00f) << 3); Status |= VL53L0X_WrByte(Dev, 0x94, 0x78); Status |= VL53L0X_device_read_strobe(Dev); Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord); ProductId[4] = (char)(byte + ((TmpDWord >> 29) & 0x07f)); ProductId[5] = (char)((TmpDWord >> 22) & 0x07f); ProductId[6] = (char)((TmpDWord >> 15) & 0x07f); ProductId[7] = (char)((TmpDWord >> 8) & 0x07f); ProductId[8] = (char)((TmpDWord >> 1) & 0x07f); byte = (uint8_t)((TmpDWord & 0x001) << 6); Status |= VL53L0X_WrByte(Dev, 0x94, 0x79); Status |= VL53L0X_device_read_strobe(Dev); Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord); ProductId[9] = (char)(byte + ((TmpDWord >> 26) & 0x07f)); ProductId[10] = (char)((TmpDWord >> 19) & 0x07f); ProductId[11] = (char)((TmpDWord >> 12) & 0x07f); ProductId[12] = (char)((TmpDWord >> 5) & 0x07f); byte = (uint8_t)((TmpDWord & 0x01f) << 2); Status |= VL53L0X_WrByte(Dev, 0x94, 0x7A); Status |= VL53L0X_device_read_strobe(Dev); Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord); ProductId[13] = (char)(byte + ((TmpDWord >> 30) & 0x07f)); ProductId[14] = (char)((TmpDWord >> 23) & 0x07f); ProductId[15] = (char)((TmpDWord >> 16) & 0x07f); ProductId[16] = (char)((TmpDWord >> 9) & 0x07f); ProductId[17] = (char)((TmpDWord >> 2) & 0x07f); ProductId[18] = '\0'; } if (((option & 4) == 4) && ((ReadDataFromDeviceDone & 4) == 0)) { Status |= VL53L0X_WrByte(Dev, 0x94, 0x7B); Status |= VL53L0X_device_read_strobe(Dev); Status |= VL53L0X_RdDWord(Dev, 0x90, &PartUIDUpper); Status |= VL53L0X_WrByte(Dev, 0x94, 0x7C); Status |= VL53L0X_device_read_strobe(Dev); Status |= VL53L0X_RdDWord(Dev, 0x90, &PartUIDLower); Status |= VL53L0X_WrByte(Dev, 0x94, 0x73); Status |= VL53L0X_device_read_strobe(Dev); Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord); SignalRateMeasFixed1104_400_mm = (TmpDWord & 0x0000000ff) << 8; Status |= VL53L0X_WrByte(Dev, 0x94, 0x74); Status |= VL53L0X_device_read_strobe(Dev); Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord); SignalRateMeasFixed1104_400_mm |= ((TmpDWord & 0xff000000) >> 24); Status |= VL53L0X_WrByte(Dev, 0x94, 0x75); Status |= VL53L0X_device_read_strobe(Dev); Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord); DistMeasFixed1104_400_mm = (TmpDWord & 0x0000000ff) << 8; Status |= VL53L0X_WrByte(Dev, 0x94, 0x76); Status |= VL53L0X_device_read_strobe(Dev); Status |= VL53L0X_RdDWord(Dev, 0x90, &TmpDWord); DistMeasFixed1104_400_mm |= ((TmpDWord & 0xff000000) >> 24); } Status |= VL53L0X_WrByte(Dev, 0x81, 0x00); Status |= VL53L0X_WrByte(Dev, 0xFF, 0x06); Status |= VL53L0X_RdByte(Dev, 0x83, &byte); Status |= VL53L0X_WrByte(Dev, 0x83, byte&0xfb); Status |= VL53L0X_WrByte(Dev, 0xFF, 0x01); Status |= VL53L0X_WrByte(Dev, 0x00, 0x01); Status |= VL53L0X_WrByte(Dev, 0xFF, 0x00); Status |= VL53L0X_WrByte(Dev, 0x80, 0x00); } if ((Status == VL53L0X_ERROR_NONE) && (ReadDataFromDeviceDone != 7)) { /* Assign to variable if status is ok */ if (((option & 1) == 1) && ((ReadDataFromDeviceDone & 1) == 0)) { VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, ReferenceSpadCount, ReferenceSpadCount); VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, ReferenceSpadType, ReferenceSpadType); for (i = 0; i < VL53L0X_REF_SPAD_BUFFER_SIZE; i++) { Dev->Data.SpadData.RefGoodSpadMap[i] = NvmRefGoodSpadMap[i]; } } if (((option & 2) == 2) && ((ReadDataFromDeviceDone & 2) == 0)) { VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, ModuleId, ModuleId); VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, Revision, Revision); ProductId_tmp = VL53L0X_GETDEVICESPECIFICPARAMETER(Dev, ProductId); VL53L0X_COPYSTRING(ProductId_tmp, ProductId); } if (((option & 4) == 4) && ((ReadDataFromDeviceDone & 4) == 0)) { VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, PartUIDUpper, PartUIDUpper); VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, PartUIDLower, PartUIDLower); SignalRateMeasFixed400mmFix = VL53L0X_FIXPOINT97TOFIXPOINT1616( SignalRateMeasFixed1104_400_mm); VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, SignalRateMeasFixed400mm, SignalRateMeasFixed400mmFix); OffsetMicroMeters = 0; if (DistMeasFixed1104_400_mm != 0) { OffsetFixed1104_mm = DistMeasFixed1104_400_mm - DistMeasTgtFixed1104_mm; OffsetMicroMeters = (OffsetFixed1104_mm * 1000) >> 4; OffsetMicroMeters *= -1; } PALDevDataSet(Dev, Part2PartOffsetAdjustmentNVMMicroMeter, OffsetMicroMeters); } byte = (uint8_t)(ReadDataFromDeviceDone|option); VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, ReadDataFromDeviceDone, byte); } LOG_FUNCTION_END(Status); return Status; } VL53L0X_Error VL53L0X::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::VL53L0X_GetOffsetCalibrationDataMicroMeter(VL53L0X_DEV Dev, int32_t *pOffsetCalibrationDataMicroMeter) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; LOG_FUNCTION_START(""); Status = VL53L0X_get_offset_calibration_data_micro_meter(Dev, pOffsetCalibrationDataMicroMeter); LOG_FUNCTION_END(Status); return Status; } VL53L0X_Error VL53L0X::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::VL53L0X_SetOffsetCalibrationDataMicroMeter(VL53L0X_DEV Dev, int32_t OffsetCalibrationDataMicroMeter) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; LOG_FUNCTION_START(""); Status = VL53L0X_set_offset_calibration_data_micro_meter(Dev, OffsetCalibrationDataMicroMeter); LOG_FUNCTION_END(Status); return Status; } VL53L0X_Error VL53L0X::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; } VL53L0X_Error VL53L0X::VL53L0X_GetDeviceMode(VL53L0X_DEV Dev, VL53L0X_DeviceModes *pDeviceMode) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; LOG_FUNCTION_START(""); VL53L0X_GETPARAMETERFIELD(Dev, DeviceMode, *pDeviceMode); LOG_FUNCTION_END(Status); return Status; } VL53L0X_Error VL53L0X::VL53L0X_GetInterMeasurementPeriodMilliSeconds(VL53L0X_DEV Dev, uint32_t *pInterMeasurementPeriodMilliSeconds) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; uint16_t osc_calibrate_val; uint32_t IMPeriodMilliSeconds; LOG_FUNCTION_START(""); Status = VL53L0X_RdWord(Dev, VL53L0X_REG_OSC_CALIBRATE_VAL, &osc_calibrate_val); if (Status == VL53L0X_ERROR_NONE) { Status = VL53L0X_RdDWord(Dev, VL53L0X_REG_SYSTEM_INTERMEASUREMENT_PERIOD, &IMPeriodMilliSeconds); } if (Status == VL53L0X_ERROR_NONE) { if (osc_calibrate_val != 0) { *pInterMeasurementPeriodMilliSeconds = IMPeriodMilliSeconds / osc_calibrate_val; } VL53L0X_SETPARAMETERFIELD(Dev, InterMeasurementPeriodMilliSeconds, *pInterMeasurementPeriodMilliSeconds); } LOG_FUNCTION_END(Status); return Status; } VL53L0X_Error VL53L0X::VL53L0X_GetXTalkCompensationRateMegaCps(VL53L0X_DEV Dev, FixPoint1616_t *pXTalkCompensationRateMegaCps) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; uint16_t Value; FixPoint1616_t TempFix1616; LOG_FUNCTION_START(""); Status = VL53L0X_RdWord(Dev, VL53L0X_REG_CROSSTALK_COMPENSATION_PEAK_RATE_MCPS, (uint16_t *)&Value); if (Status == VL53L0X_ERROR_NONE) { if (Value == 0) { /* the Xtalk is disabled return value from memory */ VL53L0X_GETPARAMETERFIELD(Dev, XTalkCompensationRateMegaCps, TempFix1616); *pXTalkCompensationRateMegaCps = TempFix1616; VL53L0X_SETPARAMETERFIELD(Dev, XTalkCompensationEnable, 0); } else { TempFix1616 = VL53L0X_FIXPOINT313TOFIXPOINT1616(Value); *pXTalkCompensationRateMegaCps = TempFix1616; VL53L0X_SETPARAMETERFIELD(Dev, XTalkCompensationRateMegaCps, TempFix1616); VL53L0X_SETPARAMETERFIELD(Dev, XTalkCompensationEnable, 1); } } LOG_FUNCTION_END(Status); return Status; } VL53L0X_Error VL53L0X::VL53L0X_GetLimitCheckValue(VL53L0X_DEV Dev, uint16_t LimitCheckId, FixPoint1616_t *pLimitCheckValue) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; uint8_t EnableZeroValue = 0; uint16_t Temp16; FixPoint1616_t TempFix1616; LOG_FUNCTION_START(""); switch (LimitCheckId) { case VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE: /* internal computation: */ VL53L0X_GETARRAYPARAMETERFIELD(Dev, LimitChecksValue, VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, TempFix1616); EnableZeroValue = 0; break; case VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE: Status = VL53L0X_RdWord(Dev, VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT, &Temp16); if (Status == VL53L0X_ERROR_NONE) TempFix1616 = VL53L0X_FIXPOINT97TOFIXPOINT1616(Temp16); EnableZeroValue = 1; break; case VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP: /* internal computation: */ VL53L0X_GETARRAYPARAMETERFIELD(Dev, LimitChecksValue, VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, TempFix1616); EnableZeroValue = 0; break; case VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD: /* internal computation: */ VL53L0X_GETARRAYPARAMETERFIELD(Dev, LimitChecksValue, VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, TempFix1616); EnableZeroValue = 0; break; case VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC: case VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE: Status = VL53L0X_RdWord(Dev, VL53L0X_REG_PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT, &Temp16); if (Status == VL53L0X_ERROR_NONE) TempFix1616 = VL53L0X_FIXPOINT97TOFIXPOINT1616(Temp16); EnableZeroValue = 0; break; default: Status = VL53L0X_ERROR_INVALID_PARAMS; } if (Status == VL53L0X_ERROR_NONE) { if (EnableZeroValue == 1) { if (TempFix1616 == 0) { /* disabled: return value from memory */ VL53L0X_GETARRAYPARAMETERFIELD(Dev, LimitChecksValue, LimitCheckId, TempFix1616); *pLimitCheckValue = TempFix1616; VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksEnable, LimitCheckId, 0); } else { *pLimitCheckValue = TempFix1616; VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksValue, LimitCheckId, TempFix1616); VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksEnable, LimitCheckId, 1); } } else { *pLimitCheckValue = TempFix1616; } } LOG_FUNCTION_END(Status); return Status; } VL53L0X_Error VL53L0X::VL53L0X_GetLimitCheckEnable(VL53L0X_DEV Dev, uint16_t LimitCheckId, uint8_t *pLimitCheckEnable) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; uint8_t Temp8; LOG_FUNCTION_START(""); if (LimitCheckId >= VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS) { Status = VL53L0X_ERROR_INVALID_PARAMS; *pLimitCheckEnable = 0; } else { VL53L0X_GETARRAYPARAMETERFIELD(Dev, LimitChecksEnable, LimitCheckId, Temp8); *pLimitCheckEnable = Temp8; } LOG_FUNCTION_END(Status); return Status; } VL53L0X_Error VL53L0X::VL53L0X_GetWrapAroundCheckEnable(VL53L0X_DEV Dev, uint8_t *pWrapAroundCheckEnable) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; uint8_t data; LOG_FUNCTION_START(""); Status = VL53L0X_RdByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, &data); if (Status == VL53L0X_ERROR_NONE) { PALDevDataSet(Dev, SequenceConfig, data); if (data & (0x01 << 7)) *pWrapAroundCheckEnable = 0x01; else *pWrapAroundCheckEnable = 0x00; } if (Status == VL53L0X_ERROR_NONE) { VL53L0X_SETPARAMETERFIELD(Dev, WrapAroundCheckEnable, *pWrapAroundCheckEnable); } LOG_FUNCTION_END(Status); return Status; } VL53L0X_Error VL53L0X::sequence_step_enabled(VL53L0X_DEV Dev, VL53L0X_SequenceStepId SequenceStepId, uint8_t SequenceConfig, uint8_t *pSequenceStepEnabled) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; *pSequenceStepEnabled = 0; LOG_FUNCTION_START(""); switch (SequenceStepId) { case VL53L0X_SEQUENCESTEP_TCC: *pSequenceStepEnabled = (SequenceConfig & 0x10) >> 4; break; case VL53L0X_SEQUENCESTEP_DSS: *pSequenceStepEnabled = (SequenceConfig & 0x08) >> 3; break; case VL53L0X_SEQUENCESTEP_MSRC: *pSequenceStepEnabled = (SequenceConfig & 0x04) >> 2; break; case VL53L0X_SEQUENCESTEP_PRE_RANGE: *pSequenceStepEnabled = (SequenceConfig & 0x40) >> 6; break; case VL53L0X_SEQUENCESTEP_FINAL_RANGE: *pSequenceStepEnabled = (SequenceConfig & 0x80) >> 7; break; default: Status = VL53L0X_ERROR_INVALID_PARAMS; } LOG_FUNCTION_END(Status); return Status; } VL53L0X_Error VL53L0X::VL53L0X_GetSequenceStepEnables(VL53L0X_DEV Dev, VL53L0X_SchedulerSequenceSteps_t *pSchedulerSequenceSteps) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; uint8_t SequenceConfig = 0; LOG_FUNCTION_START(""); Status = VL53L0X_RdByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, &SequenceConfig); if (Status == VL53L0X_ERROR_NONE) { Status = sequence_step_enabled(Dev, VL53L0X_SEQUENCESTEP_TCC, SequenceConfig, &pSchedulerSequenceSteps->TccOn); } if (Status == VL53L0X_ERROR_NONE) { Status = sequence_step_enabled(Dev, VL53L0X_SEQUENCESTEP_DSS, SequenceConfig, &pSchedulerSequenceSteps->DssOn); } if (Status == VL53L0X_ERROR_NONE) { Status = sequence_step_enabled(Dev, VL53L0X_SEQUENCESTEP_MSRC, SequenceConfig, &pSchedulerSequenceSteps->MsrcOn); } if (Status == VL53L0X_ERROR_NONE) { Status = sequence_step_enabled(Dev, VL53L0X_SEQUENCESTEP_PRE_RANGE, SequenceConfig, &pSchedulerSequenceSteps->PreRangeOn); } if (Status == VL53L0X_ERROR_NONE) { Status = sequence_step_enabled(Dev, VL53L0X_SEQUENCESTEP_FINAL_RANGE, SequenceConfig, &pSchedulerSequenceSteps->FinalRangeOn); } LOG_FUNCTION_END(Status); return Status; } uint8_t VL53L0X::VL53L0X_decode_vcsel_period(uint8_t vcsel_period_reg) { /*! * Converts the encoded VCSEL period register value into the real * period in PLL clocks */ uint8_t vcsel_period_pclks = 0; vcsel_period_pclks = (vcsel_period_reg + 1) << 1; return vcsel_period_pclks; } uint8_t VL53L0X::VL53L0X_encode_vcsel_period(uint8_t vcsel_period_pclks) { /*! * Converts the encoded VCSEL period register value into the real period * in PLL clocks */ uint8_t vcsel_period_reg = 0; vcsel_period_reg = (vcsel_period_pclks >> 1) - 1; return vcsel_period_reg; } VL53L0X_Error VL53L0X::VL53L0X_set_vcsel_pulse_period(VL53L0X_DEV Dev, VL53L0X_VcselPeriod VcselPeriodType, uint8_t VCSELPulsePeriodPCLK) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; uint8_t vcsel_period_reg; uint8_t MinPreVcselPeriodPCLK = 12; uint8_t MaxPreVcselPeriodPCLK = 18; uint8_t MinFinalVcselPeriodPCLK = 8; uint8_t MaxFinalVcselPeriodPCLK = 14; uint32_t MeasurementTimingBudgetMicroSeconds; uint32_t FinalRangeTimeoutMicroSeconds; uint32_t PreRangeTimeoutMicroSeconds; uint32_t MsrcTimeoutMicroSeconds; uint8_t PhaseCalInt = 0; /* Check if valid clock period requested */ if ((VCSELPulsePeriodPCLK % 2) != 0) { /* Value must be an even number */ Status = VL53L0X_ERROR_INVALID_PARAMS; } else if (VcselPeriodType == VL53L0X_VCSEL_PERIOD_PRE_RANGE && (VCSELPulsePeriodPCLK < MinPreVcselPeriodPCLK || VCSELPulsePeriodPCLK > MaxPreVcselPeriodPCLK)) { Status = VL53L0X_ERROR_INVALID_PARAMS; } else if (VcselPeriodType == VL53L0X_VCSEL_PERIOD_FINAL_RANGE && (VCSELPulsePeriodPCLK < MinFinalVcselPeriodPCLK || VCSELPulsePeriodPCLK > MaxFinalVcselPeriodPCLK)) { Status = VL53L0X_ERROR_INVALID_PARAMS; } /* Apply specific settings for the requested clock period */ if (Status != VL53L0X_ERROR_NONE) return Status; if (VcselPeriodType == VL53L0X_VCSEL_PERIOD_PRE_RANGE) { /* Set phase check limits */ if (VCSELPulsePeriodPCLK == 12) { Status = VL53L0X_WrByte(Dev, VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x18); Status = VL53L0X_WrByte(Dev, VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW, 0x08); } else if (VCSELPulsePeriodPCLK == 14) { Status = VL53L0X_WrByte(Dev, VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x30); Status = VL53L0X_WrByte(Dev, VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW, 0x08); } else if (VCSELPulsePeriodPCLK == 16) { Status = VL53L0X_WrByte(Dev, VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x40); Status = VL53L0X_WrByte(Dev, VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW, 0x08); } else if (VCSELPulsePeriodPCLK == 18) { Status = VL53L0X_WrByte(Dev, VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x50); Status = VL53L0X_WrByte(Dev, VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW, 0x08); } } else if (VcselPeriodType == VL53L0X_VCSEL_PERIOD_FINAL_RANGE) { if (VCSELPulsePeriodPCLK == 8) { Status = VL53L0X_WrByte(Dev, VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x10); Status = VL53L0X_WrByte(Dev, VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08); Status |= VL53L0X_WrByte(Dev, VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x02); Status |= VL53L0X_WrByte(Dev, VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x0C); Status |= VL53L0X_WrByte(Dev, 0xff, 0x01); Status |= VL53L0X_WrByte(Dev, VL53L0X_REG_ALGO_PHASECAL_LIM, 0x30); Status |= VL53L0X_WrByte(Dev, 0xff, 0x00); } else if (VCSELPulsePeriodPCLK == 10) { Status = VL53L0X_WrByte(Dev, VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x28); Status = VL53L0X_WrByte(Dev, VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08); Status |= VL53L0X_WrByte(Dev, VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03); Status |= VL53L0X_WrByte(Dev, VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x09); Status |= VL53L0X_WrByte(Dev, 0xff, 0x01); Status |= VL53L0X_WrByte(Dev, VL53L0X_REG_ALGO_PHASECAL_LIM, 0x20); Status |= VL53L0X_WrByte(Dev, 0xff, 0x00); } else if (VCSELPulsePeriodPCLK == 12) { Status = VL53L0X_WrByte(Dev, VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x38); Status = VL53L0X_WrByte(Dev, VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08); Status |= VL53L0X_WrByte(Dev, VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03); Status |= VL53L0X_WrByte(Dev, VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x08); Status |= VL53L0X_WrByte(Dev, 0xff, 0x01); Status |= VL53L0X_WrByte(Dev, VL53L0X_REG_ALGO_PHASECAL_LIM, 0x20); Status |= VL53L0X_WrByte(Dev, 0xff, 0x00); } else if (VCSELPulsePeriodPCLK == 14) { Status = VL53L0X_WrByte(Dev, VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x048); Status = VL53L0X_WrByte(Dev, VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08); Status |= VL53L0X_WrByte(Dev, VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03); Status |= VL53L0X_WrByte(Dev, VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x07); Status |= VL53L0X_WrByte(Dev, 0xff, 0x01); Status |= VL53L0X_WrByte(Dev, VL53L0X_REG_ALGO_PHASECAL_LIM, 0x20); Status |= VL53L0X_WrByte(Dev, 0xff, 0x00); } } /* Re-calculate and apply timeouts, in macro periods */ if (Status == VL53L0X_ERROR_NONE) { vcsel_period_reg = VL53L0X_encode_vcsel_period((uint8_t) VCSELPulsePeriodPCLK); /* When the VCSEL period for the pre or final range is changed, * the corresponding timeout must be read from the device using * the current VCSEL period, then the new VCSEL period can be * applied. The timeout then must be written back to the device * using the new VCSEL period. * * For the MSRC timeout, the same applies - this timeout being * dependant on the pre-range vcsel period. */ switch (VcselPeriodType) { case VL53L0X_VCSEL_PERIOD_PRE_RANGE: Status = get_sequence_step_timeout(Dev, VL53L0X_SEQUENCESTEP_PRE_RANGE, &PreRangeTimeoutMicroSeconds); if (Status == VL53L0X_ERROR_NONE) Status = get_sequence_step_timeout(Dev, VL53L0X_SEQUENCESTEP_MSRC, &MsrcTimeoutMicroSeconds); if (Status == VL53L0X_ERROR_NONE) Status = VL53L0X_WrByte(Dev, VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD, vcsel_period_reg); if (Status == VL53L0X_ERROR_NONE) Status = set_sequence_step_timeout(Dev, VL53L0X_SEQUENCESTEP_PRE_RANGE, PreRangeTimeoutMicroSeconds); if (Status == VL53L0X_ERROR_NONE) Status = set_sequence_step_timeout(Dev, VL53L0X_SEQUENCESTEP_MSRC, MsrcTimeoutMicroSeconds); VL53L0X_SETDEVICESPECIFICPARAMETER( Dev, PreRangeVcselPulsePeriod, VCSELPulsePeriodPCLK); break; case VL53L0X_VCSEL_PERIOD_FINAL_RANGE: Status = get_sequence_step_timeout(Dev, VL53L0X_SEQUENCESTEP_FINAL_RANGE, &FinalRangeTimeoutMicroSeconds); if (Status == VL53L0X_ERROR_NONE) Status = VL53L0X_WrByte(Dev, VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD, vcsel_period_reg); if (Status == VL53L0X_ERROR_NONE) Status = set_sequence_step_timeout(Dev, VL53L0X_SEQUENCESTEP_FINAL_RANGE, FinalRangeTimeoutMicroSeconds); VL53L0X_SETDEVICESPECIFICPARAMETER( Dev, FinalRangeVcselPulsePeriod, VCSELPulsePeriodPCLK); break; default: Status = VL53L0X_ERROR_INVALID_PARAMS; } } /* Finally, the timing budget must be re-applied */ if (Status == VL53L0X_ERROR_NONE) { VL53L0X_GETPARAMETERFIELD(Dev, MeasurementTimingBudgetMicroSeconds, MeasurementTimingBudgetMicroSeconds); Status = VL53L0X_SetMeasurementTimingBudgetMicroSeconds(Dev, MeasurementTimingBudgetMicroSeconds); } /* Perform the phase calibration. This is needed after changing on * vcsel period. * get_data_enable = 0, restore_config = 1 */ if (Status == VL53L0X_ERROR_NONE) Status = VL53L0X_perform_phase_calibration( Dev, &PhaseCalInt, 0, 1); return Status; } VL53L0X_Error VL53L0X::VL53L0X_SetVcselPulsePeriod(VL53L0X_DEV Dev, VL53L0X_VcselPeriod VcselPeriodType, uint8_t VCSELPulsePeriodPCLK) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; LOG_FUNCTION_START(""); Status = VL53L0X_set_vcsel_pulse_period(Dev, VcselPeriodType, VCSELPulsePeriodPCLK); LOG_FUNCTION_END(Status); return Status; } VL53L0X_Error VL53L0X::VL53L0X_get_vcsel_pulse_period(VL53L0X_DEV Dev, VL53L0X_VcselPeriod VcselPeriodType, uint8_t *pVCSELPulsePeriodPCLK) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; uint8_t vcsel_period_reg; switch (VcselPeriodType) { case VL53L0X_VCSEL_PERIOD_PRE_RANGE: Status = VL53L0X_RdByte(Dev, VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD, &vcsel_period_reg); break; case VL53L0X_VCSEL_PERIOD_FINAL_RANGE: Status = VL53L0X_RdByte(Dev, VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD, &vcsel_period_reg); break; default: Status = VL53L0X_ERROR_INVALID_PARAMS; } if (Status == VL53L0X_ERROR_NONE) *pVCSELPulsePeriodPCLK = VL53L0X_decode_vcsel_period(vcsel_period_reg); return Status; } VL53L0X_Error VL53L0X::VL53L0X_GetVcselPulsePeriod(VL53L0X_DEV Dev, VL53L0X_VcselPeriod VcselPeriodType, uint8_t *pVCSELPulsePeriodPCLK) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; LOG_FUNCTION_START(""); Status = VL53L0X_get_vcsel_pulse_period(Dev, VcselPeriodType, pVCSELPulsePeriodPCLK); LOG_FUNCTION_END(Status); return Status; } uint32_t VL53L0X::VL53L0X_decode_timeout(uint16_t encoded_timeout) { /*! * Decode 16-bit timeout register value - format (LSByte * 2^MSByte) + 1 */ uint32_t timeout_macro_clks = 0; timeout_macro_clks = ((uint32_t) (encoded_timeout & 0x00FF) << (uint32_t) ((encoded_timeout & 0xFF00) >> 8)) + 1; return timeout_macro_clks; } uint32_t VL53L0X::VL53L0X_calc_macro_period_ps(VL53L0X_DEV Dev, uint8_t vcsel_period_pclks) { uint64_t PLL_period_ps; uint32_t macro_period_vclks; uint32_t macro_period_ps; LOG_FUNCTION_START(""); /* The above calculation will produce rounding errors, therefore set fixed value */ PLL_period_ps = 1655; macro_period_vclks = 2304; macro_period_ps = (uint32_t)(macro_period_vclks * vcsel_period_pclks * PLL_period_ps); LOG_FUNCTION_END(""); return macro_period_ps; } /* To convert register value into us */ uint32_t VL53L0X::VL53L0X_calc_timeout_us(VL53L0X_DEV Dev, uint16_t timeout_period_mclks, uint8_t vcsel_period_pclks) { uint32_t macro_period_ps; uint32_t macro_period_ns; uint32_t actual_timeout_period_us = 0; macro_period_ps = VL53L0X_calc_macro_period_ps(Dev, vcsel_period_pclks); macro_period_ns = (macro_period_ps + 500) / 1000; actual_timeout_period_us = ((timeout_period_mclks * macro_period_ns) + 500) / 1000; return actual_timeout_period_us; } VL53L0X_Error VL53L0X::get_sequence_step_timeout(VL53L0X_DEV Dev, VL53L0X_SequenceStepId SequenceStepId, uint32_t *pTimeOutMicroSecs) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; uint8_t CurrentVCSELPulsePeriodPClk; uint8_t EncodedTimeOutByte = 0; uint32_t TimeoutMicroSeconds = 0; uint16_t PreRangeEncodedTimeOut = 0; uint16_t MsrcTimeOutMClks; uint16_t PreRangeTimeOutMClks; uint16_t FinalRangeTimeOutMClks = 0; uint16_t FinalRangeEncodedTimeOut; VL53L0X_SchedulerSequenceSteps_t SchedulerSequenceSteps; if ((SequenceStepId == VL53L0X_SEQUENCESTEP_TCC) || (SequenceStepId == VL53L0X_SEQUENCESTEP_DSS) || (SequenceStepId == VL53L0X_SEQUENCESTEP_MSRC)) { Status = VL53L0X_GetVcselPulsePeriod(Dev, VL53L0X_VCSEL_PERIOD_PRE_RANGE, &CurrentVCSELPulsePeriodPClk); if (Status == VL53L0X_ERROR_NONE) { Status = VL53L0X_RdByte(Dev, VL53L0X_REG_MSRC_CONFIG_TIMEOUT_MACROP, &EncodedTimeOutByte); } MsrcTimeOutMClks = VL53L0X_decode_timeout(EncodedTimeOutByte); TimeoutMicroSeconds = VL53L0X_calc_timeout_us(Dev, MsrcTimeOutMClks, CurrentVCSELPulsePeriodPClk); } else if (SequenceStepId == VL53L0X_SEQUENCESTEP_PRE_RANGE) { /* Retrieve PRE-RANGE VCSEL Period */ Status = VL53L0X_GetVcselPulsePeriod(Dev, VL53L0X_VCSEL_PERIOD_PRE_RANGE, &CurrentVCSELPulsePeriodPClk); /* Retrieve PRE-RANGE Timeout in Macro periods (MCLKS) */ if (Status == VL53L0X_ERROR_NONE) { /* Retrieve PRE-RANGE VCSEL Period */ Status = VL53L0X_GetVcselPulsePeriod(Dev, VL53L0X_VCSEL_PERIOD_PRE_RANGE, &CurrentVCSELPulsePeriodPClk); if (Status == VL53L0X_ERROR_NONE) { Status = VL53L0X_RdWord(Dev, VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI, &PreRangeEncodedTimeOut); } PreRangeTimeOutMClks = VL53L0X_decode_timeout( PreRangeEncodedTimeOut); TimeoutMicroSeconds = VL53L0X_calc_timeout_us(Dev, PreRangeTimeOutMClks, CurrentVCSELPulsePeriodPClk); } } else if (SequenceStepId == VL53L0X_SEQUENCESTEP_FINAL_RANGE) { VL53L0X_GetSequenceStepEnables(Dev, &SchedulerSequenceSteps); PreRangeTimeOutMClks = 0; if (SchedulerSequenceSteps.PreRangeOn) { /* Retrieve PRE-RANGE VCSEL Period */ Status = VL53L0X_GetVcselPulsePeriod(Dev, VL53L0X_VCSEL_PERIOD_PRE_RANGE, &CurrentVCSELPulsePeriodPClk); /* Retrieve PRE-RANGE Timeout in Macro periods * (MCLKS) */ if (Status == VL53L0X_ERROR_NONE) { Status = VL53L0X_RdWord(Dev, VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI, &PreRangeEncodedTimeOut); PreRangeTimeOutMClks = VL53L0X_decode_timeout( PreRangeEncodedTimeOut); } } if (Status == VL53L0X_ERROR_NONE) { /* Retrieve FINAL-RANGE VCSEL Period */ Status = VL53L0X_GetVcselPulsePeriod(Dev, VL53L0X_VCSEL_PERIOD_FINAL_RANGE, &CurrentVCSELPulsePeriodPClk); } /* Retrieve FINAL-RANGE Timeout in Macro periods (MCLKS) */ if (Status == VL53L0X_ERROR_NONE) { Status = VL53L0X_RdWord(Dev, VL53L0X_REG_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI, &FinalRangeEncodedTimeOut); FinalRangeTimeOutMClks = VL53L0X_decode_timeout( FinalRangeEncodedTimeOut); } FinalRangeTimeOutMClks -= PreRangeTimeOutMClks; TimeoutMicroSeconds = VL53L0X_calc_timeout_us(Dev, FinalRangeTimeOutMClks, CurrentVCSELPulsePeriodPClk); } *pTimeOutMicroSecs = TimeoutMicroSeconds; return Status; } VL53L0X_Error VL53L0X::VL53L0X_get_measurement_timing_budget_micro_seconds(VL53L0X_DEV Dev, uint32_t *pMeasurementTimingBudgetMicroSeconds) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; VL53L0X_SchedulerSequenceSteps_t SchedulerSequenceSteps; uint32_t FinalRangeTimeoutMicroSeconds; uint32_t MsrcDccTccTimeoutMicroSeconds = 2000; uint32_t StartOverheadMicroSeconds = 1910; uint32_t EndOverheadMicroSeconds = 960; uint32_t MsrcOverheadMicroSeconds = 660; uint32_t TccOverheadMicroSeconds = 590; uint32_t DssOverheadMicroSeconds = 690; uint32_t PreRangeOverheadMicroSeconds = 660; uint32_t FinalRangeOverheadMicroSeconds = 550; uint32_t PreRangeTimeoutMicroSeconds = 0; LOG_FUNCTION_START(""); /* Start and end overhead times always present */ *pMeasurementTimingBudgetMicroSeconds = StartOverheadMicroSeconds + EndOverheadMicroSeconds; Status = VL53L0X_GetSequenceStepEnables(Dev, &SchedulerSequenceSteps); if (Status != VL53L0X_ERROR_NONE) { LOG_FUNCTION_END(Status); return Status; } if (SchedulerSequenceSteps.TccOn || SchedulerSequenceSteps.MsrcOn || SchedulerSequenceSteps.DssOn) { Status = get_sequence_step_timeout(Dev, VL53L0X_SEQUENCESTEP_MSRC, &MsrcDccTccTimeoutMicroSeconds); if (Status == VL53L0X_ERROR_NONE) { if (SchedulerSequenceSteps.TccOn) { *pMeasurementTimingBudgetMicroSeconds += MsrcDccTccTimeoutMicroSeconds + TccOverheadMicroSeconds; } if (SchedulerSequenceSteps.DssOn) { *pMeasurementTimingBudgetMicroSeconds += 2 * (MsrcDccTccTimeoutMicroSeconds + DssOverheadMicroSeconds); } else if (SchedulerSequenceSteps.MsrcOn) { *pMeasurementTimingBudgetMicroSeconds += MsrcDccTccTimeoutMicroSeconds + MsrcOverheadMicroSeconds; } } } if (Status == VL53L0X_ERROR_NONE) { if (SchedulerSequenceSteps.PreRangeOn) { Status = get_sequence_step_timeout(Dev, VL53L0X_SEQUENCESTEP_PRE_RANGE, &PreRangeTimeoutMicroSeconds); *pMeasurementTimingBudgetMicroSeconds += PreRangeTimeoutMicroSeconds + PreRangeOverheadMicroSeconds; } } if (Status == VL53L0X_ERROR_NONE) { if (SchedulerSequenceSteps.FinalRangeOn) { Status = get_sequence_step_timeout(Dev, VL53L0X_SEQUENCESTEP_FINAL_RANGE, &FinalRangeTimeoutMicroSeconds); *pMeasurementTimingBudgetMicroSeconds += (FinalRangeTimeoutMicroSeconds + FinalRangeOverheadMicroSeconds); } } if (Status == VL53L0X_ERROR_NONE) { VL53L0X_SETPARAMETERFIELD(Dev, MeasurementTimingBudgetMicroSeconds, *pMeasurementTimingBudgetMicroSeconds); } LOG_FUNCTION_END(Status); return Status; } VL53L0X_Error VL53L0X::VL53L0X_GetMeasurementTimingBudgetMicroSeconds(VL53L0X_DEV Dev, uint32_t *pMeasurementTimingBudgetMicroSeconds) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; LOG_FUNCTION_START(""); Status = VL53L0X_get_measurement_timing_budget_micro_seconds(Dev, pMeasurementTimingBudgetMicroSeconds); LOG_FUNCTION_END(Status); return Status; } VL53L0X_Error VL53L0X::VL53L0X_GetDeviceParameters(VL53L0X_DEV Dev, VL53L0X_DeviceParameters_t *pDeviceParameters) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; int i; LOG_FUNCTION_START(""); Status = VL53L0X_GetDeviceMode(Dev, &(pDeviceParameters->DeviceMode)); if (Status == VL53L0X_ERROR_NONE) Status = VL53L0X_GetInterMeasurementPeriodMilliSeconds(Dev, &(pDeviceParameters->InterMeasurementPeriodMilliSeconds)); if (Status == VL53L0X_ERROR_NONE) pDeviceParameters->XTalkCompensationEnable = 0; if (Status == VL53L0X_ERROR_NONE) Status = VL53L0X_GetXTalkCompensationRateMegaCps(Dev, &(pDeviceParameters->XTalkCompensationRateMegaCps)); if (Status == VL53L0X_ERROR_NONE) Status = VL53L0X_GetOffsetCalibrationDataMicroMeter(Dev, &(pDeviceParameters->RangeOffsetMicroMeters)); if (Status == VL53L0X_ERROR_NONE) { for (i = 0; i < VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS; i++) { /* get first the values, then the enables. * VL53L0X_GetLimitCheckValue will modify the enable * flags */ if (Status == VL53L0X_ERROR_NONE) { Status |= VL53L0X_GetLimitCheckValue(Dev, i, &(pDeviceParameters->LimitChecksValue[i])); } else { break; } if (Status == VL53L0X_ERROR_NONE) { Status |= VL53L0X_GetLimitCheckEnable(Dev, i, &(pDeviceParameters->LimitChecksEnable[i])); } else { break; } } } if (Status == VL53L0X_ERROR_NONE) { Status = VL53L0X_GetWrapAroundCheckEnable(Dev, &(pDeviceParameters->WrapAroundCheckEnable)); } /* Need to be done at the end as it uses VCSELPulsePeriod */ if (Status == VL53L0X_ERROR_NONE) { Status = VL53L0X_GetMeasurementTimingBudgetMicroSeconds(Dev, &(pDeviceParameters->MeasurementTimingBudgetMicroSeconds)); } LOG_FUNCTION_END(Status); return Status; } VL53L0X_Error VL53L0X::VL53L0X_SetLimitCheckValue(VL53L0X_DEV Dev, uint16_t LimitCheckId, FixPoint1616_t LimitCheckValue) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; uint8_t Temp8; LOG_FUNCTION_START(""); VL53L0X_GETARRAYPARAMETERFIELD(Dev, LimitChecksEnable, LimitCheckId, Temp8); if (Temp8 == 0) { /* disabled write only internal value */ VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksValue, LimitCheckId, LimitCheckValue); } else { switch (LimitCheckId) { case VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE: /* internal computation: */ VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksValue, VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, LimitCheckValue); break; case VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE: Status = VL53L0X_WrWord(Dev, VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT, VL53L0X_FIXPOINT1616TOFIXPOINT97( LimitCheckValue)); break; case VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP: /* internal computation: */ VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksValue, VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, LimitCheckValue); break; case VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD: /* internal computation: */ VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksValue, VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, LimitCheckValue); break; case VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC: case VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE: Status = VL53L0X_WrWord(Dev, VL53L0X_REG_PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT, VL53L0X_FIXPOINT1616TOFIXPOINT97( LimitCheckValue)); break; default: Status = VL53L0X_ERROR_INVALID_PARAMS; } if (Status == VL53L0X_ERROR_NONE) { VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksValue, LimitCheckId, LimitCheckValue); } } LOG_FUNCTION_END(Status); return Status; } VL53L0X_Error VL53L0X::VL53L0X_DataInit(VL53L0X_DEV Dev) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; VL53L0X_DeviceParameters_t CurrentParameters; int i; uint8_t StopVariable; LOG_FUNCTION_START(""); /* by default the I2C is running at 1V8 if you want to change it you * need to include this define at compilation level. */ #ifdef USE_I2C_2V8 Status = VL53L0X_UpdateByte(Dev, VL53L0X_REG_VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV, 0xFE, 0x01); #endif /* Set I2C standard mode */ if (Status == VL53L0X_ERROR_NONE) Status = VL53L0X_WrByte(Dev, 0x88, 0x00); VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, ReadDataFromDeviceDone, 0); #ifdef USE_IQC_STATION if (Status == VL53L0X_ERROR_NONE) Status = VL53L0X_apply_offset_adjustment(Dev); #endif /* Default value is 1000 for Linearity Corrective Gain */ PALDevDataSet(Dev, LinearityCorrectiveGain, 1000); /* Dmax default Parameter */ PALDevDataSet(Dev, DmaxCalRangeMilliMeter, 400); PALDevDataSet(Dev, DmaxCalSignalRateRtnMegaCps, (FixPoint1616_t)((0x00016B85))); /* 1.42 No Cover Glass*/ /* Set Default static parameters *set first temporary values 9.44MHz * 65536 = 618660 */ VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, OscFrequencyMHz, 618660); /* Set Default XTalkCompensationRateMegaCps to 0 */ VL53L0X_SETPARAMETERFIELD(Dev, XTalkCompensationRateMegaCps, 0); /* Get default parameters */ Status = VL53L0X_GetDeviceParameters(Dev, &CurrentParameters); if (Status == VL53L0X_ERROR_NONE) { /* initialize PAL values */ CurrentParameters.DeviceMode = VL53L0X_DEVICEMODE_SINGLE_RANGING; CurrentParameters.HistogramMode = VL53L0X_HISTOGRAMMODE_DISABLED; PALDevDataSet(Dev, CurrentParameters, CurrentParameters); } /* Sigma estimator variable */ PALDevDataSet(Dev, SigmaEstRefArray, 100); PALDevDataSet(Dev, SigmaEstEffPulseWidth, 900); PALDevDataSet(Dev, SigmaEstEffAmbWidth, 500); PALDevDataSet(Dev, targetRefRate, 0x0A00); /* 20 MCPS in 9:7 format */ /* Use internal default settings */ PALDevDataSet(Dev, UseInternalTuningSettings, 1); Status |= VL53L0X_WrByte(Dev, 0x80, 0x01); Status |= VL53L0X_WrByte(Dev, 0xFF, 0x01); Status |= VL53L0X_WrByte(Dev, 0x00, 0x00); Status |= VL53L0X_RdByte(Dev, 0x91, &StopVariable); PALDevDataSet(Dev, StopVariable, StopVariable); Status |= VL53L0X_WrByte(Dev, 0x00, 0x01); Status |= VL53L0X_WrByte(Dev, 0xFF, 0x00); Status |= VL53L0X_WrByte(Dev, 0x80, 0x00); /* Enable all check */ for (i = 0; i < VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS; i++) { if (Status == VL53L0X_ERROR_NONE) Status |= VL53L0X_SetLimitCheckEnable(Dev, i, 1); else break; } /* Disable the following checks */ if (Status == VL53L0X_ERROR_NONE) Status = VL53L0X_SetLimitCheckEnable(Dev, VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, 0); if (Status == VL53L0X_ERROR_NONE) Status = VL53L0X_SetLimitCheckEnable(Dev, VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, 0); if (Status == VL53L0X_ERROR_NONE) Status = VL53L0X_SetLimitCheckEnable(Dev, VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC, 0); if (Status == VL53L0X_ERROR_NONE) Status = VL53L0X_SetLimitCheckEnable(Dev, VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE, 0); /* Limit default values */ if (Status == VL53L0X_ERROR_NONE) { Status = VL53L0X_SetLimitCheckValue(Dev, VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, (FixPoint1616_t)(18 * 65536)); } if (Status == VL53L0X_ERROR_NONE) { Status = VL53L0X_SetLimitCheckValue(Dev, VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, (FixPoint1616_t)(25 * 65536 / 100)); /* 0.25 * 65536 */ } if (Status == VL53L0X_ERROR_NONE) { Status = VL53L0X_SetLimitCheckValue(Dev, VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, (FixPoint1616_t)(35 * 65536)); } if (Status == VL53L0X_ERROR_NONE) { Status = VL53L0X_SetLimitCheckValue(Dev, VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, (FixPoint1616_t)(0 * 65536)); } if (Status == VL53L0X_ERROR_NONE) { PALDevDataSet(Dev, SequenceConfig, 0xFF); Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0xFF); /* Set PAL state to tell that we are waiting for call to * VL53L0X_StaticInit */ PALDevDataSet(Dev, PalState, VL53L0X_STATE_WAIT_STATICINIT); } if (Status == VL53L0X_ERROR_NONE) VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, RefSpadsInitialised, 0); LOG_FUNCTION_END(Status); return Status; } VL53L0X_Error VL53L0X::VL53L0X_check_part_used(VL53L0X_DEV Dev, uint8_t *Revision, VL53L0X_DeviceInfo_t *pVL53L0X_DeviceInfo) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; uint8_t ModuleIdInt; char *ProductId_tmp; LOG_FUNCTION_START(""); Status = VL53L0X_get_info_from_device(Dev, 2); if (Status == VL53L0X_ERROR_NONE) { ModuleIdInt = VL53L0X_GETDEVICESPECIFICPARAMETER(Dev, ModuleId); if (ModuleIdInt == 0) { *Revision = 0; VL53L0X_COPYSTRING(pVL53L0X_DeviceInfo->ProductId, ""); } else { *Revision = VL53L0X_GETDEVICESPECIFICPARAMETER(Dev, Revision); ProductId_tmp = VL53L0X_GETDEVICESPECIFICPARAMETER(Dev, ProductId); VL53L0X_COPYSTRING(pVL53L0X_DeviceInfo->ProductId, ProductId_tmp); } } LOG_FUNCTION_END(Status); return Status; } VL53L0X_Error VL53L0X::VL53L0X_get_device_info(VL53L0X_DEV Dev, VL53L0X_DeviceInfo_t *pVL53L0X_DeviceInfo) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; uint8_t revision_id; uint8_t Revision; Status = VL53L0X_check_part_used(Dev, &Revision, pVL53L0X_DeviceInfo); if (Status == VL53L0X_ERROR_NONE) { if (Revision == 0) { VL53L0X_COPYSTRING(pVL53L0X_DeviceInfo->Name, VL53L0X_STRING_DEVICE_INFO_NAME_TS0); } else if ((Revision <= 34) && (Revision != 32)) { VL53L0X_COPYSTRING(pVL53L0X_DeviceInfo->Name, VL53L0X_STRING_DEVICE_INFO_NAME_TS1); } else if (Revision < 39) { VL53L0X_COPYSTRING(pVL53L0X_DeviceInfo->Name, VL53L0X_STRING_DEVICE_INFO_NAME_TS2); } else { VL53L0X_COPYSTRING(pVL53L0X_DeviceInfo->Name, VL53L0X_STRING_DEVICE_INFO_NAME_ES1); } VL53L0X_COPYSTRING(pVL53L0X_DeviceInfo->Type, VL53L0X_STRING_DEVICE_INFO_TYPE); } if (Status == VL53L0X_ERROR_NONE) { Status = VL53L0X_RdByte(Dev, VL53L0X_REG_IDENTIFICATION_MODEL_ID, &pVL53L0X_DeviceInfo->ProductType); } if (Status == VL53L0X_ERROR_NONE) { Status = VL53L0X_RdByte(Dev, VL53L0X_REG_IDENTIFICATION_REVISION_ID, &revision_id); pVL53L0X_DeviceInfo->ProductRevisionMajor = 1; pVL53L0X_DeviceInfo->ProductRevisionMinor = (revision_id & 0xF0) >> 4; } return Status; } VL53L0X_Error VL53L0X::VL53L0X_GetDeviceInfo(VL53L0X_DEV Dev, VL53L0X_DeviceInfo_t *pVL53L0X_DeviceInfo) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; LOG_FUNCTION_START(""); Status = VL53L0X_get_device_info(Dev, pVL53L0X_DeviceInfo); LOG_FUNCTION_END(Status); return Status; } VL53L0X_Error VL53L0X::VL53L0X_GetInterruptMaskStatus(VL53L0X_DEV Dev, uint32_t *pInterruptMaskStatus) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; uint8_t Byte; LOG_FUNCTION_START(""); Status = VL53L0X_RdByte(Dev, VL53L0X_REG_RESULT_INTERRUPT_STATUS, &Byte); *pInterruptMaskStatus = Byte & 0x07; if (Byte & 0x18) Status = VL53L0X_ERROR_RANGE_ERROR; LOG_FUNCTION_END(Status); return Status; } VL53L0X_Error VL53L0X::VL53L0X_GetMeasurementDataReady(VL53L0X_DEV Dev, uint8_t *pMeasurementDataReady) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; uint8_t SysRangeStatusRegister; uint8_t InterruptConfig; uint32_t InterruptMask; LOG_FUNCTION_START(""); InterruptConfig = VL53L0X_GETDEVICESPECIFICPARAMETER(Dev, Pin0GpioFunctionality); if (InterruptConfig == VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY) { Status = VL53L0X_GetInterruptMaskStatus(Dev, &InterruptMask); if (InterruptMask == VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY) *pMeasurementDataReady = 1; else *pMeasurementDataReady = 0; } else { Status = VL53L0X_RdByte(Dev, VL53L0X_REG_RESULT_RANGE_STATUS, &SysRangeStatusRegister); if (Status == VL53L0X_ERROR_NONE) { if (SysRangeStatusRegister & 0x01) *pMeasurementDataReady = 1; else *pMeasurementDataReady = 0; } } LOG_FUNCTION_END(Status); return Status; } VL53L0X_Error VL53L0X::VL53L0X_PollingDelay(VL53L0X_DEV Dev) { VL53L0X_Error status = VL53L0X_ERROR_NONE; // do nothing VL53L0X_OsDelay(); return status; } VL53L0X_Error VL53L0X::VL53L0X_measurement_poll_for_completion(VL53L0X_DEV Dev) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; uint8_t NewDataReady = 0; uint32_t LoopNb; LOG_FUNCTION_START(""); LoopNb = 0; do { Status = VL53L0X_GetMeasurementDataReady(Dev, &NewDataReady); if (Status != 0) break; /* the error is set */ if (NewDataReady == 1) break; /* done note that status == 0 */ LoopNb++; if (LoopNb >= VL53L0X_DEFAULT_MAX_LOOP) { Status = VL53L0X_ERROR_TIME_OUT; break; } VL53L0X_PollingDelay(Dev); } while (1); LOG_FUNCTION_END(Status); return Status; } /* Group PAL Interrupt Functions */ VL53L0X_Error VL53L0X::VL53L0X_ClearInterruptMask(VL53L0X_DEV Dev, uint32_t InterruptMask) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; uint8_t LoopCount; uint8_t Byte; LOG_FUNCTION_START(""); /* clear bit 0 range interrupt, bit 1 error interrupt */ LoopCount = 0; do { Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR, 0x01); Status |= VL53L0X_WrByte(Dev, VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR, 0x00); Status |= VL53L0X_RdByte(Dev, VL53L0X_REG_RESULT_INTERRUPT_STATUS, &Byte); LoopCount++; } while (((Byte & 0x07) != 0x00) && (LoopCount < 3) && (Status == VL53L0X_ERROR_NONE)); if (LoopCount >= 3) Status = VL53L0X_ERROR_INTERRUPT_NOT_CLEARED; LOG_FUNCTION_END(Status); return Status; } VL53L0X_Error VL53L0X::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::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::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::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::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; } void VL53L0X::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 VL53L0X::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 VL53L0X::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 VL53L0X::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 VL53L0X::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); // VL53L0X_Error status = VL53L0X_ERROR_NONE; // uint8_t count=0; // for (count = 0; count < 6; count++) // status = VL53L0X_RdByte(Dev, (VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0 + count), &refSpadArray[count]); return status; } VL53L0X_Error VL53L0X::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 VL53L0X::VL53L0X_SetDeviceMode(VL53L0X_DEV Dev, VL53L0X_DeviceModes DeviceMode) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; LOG_FUNCTION_START("%d", (int)DeviceMode); switch (DeviceMode) { case VL53L0X_DEVICEMODE_SINGLE_RANGING: case VL53L0X_DEVICEMODE_CONTINUOUS_RANGING: case VL53L0X_DEVICEMODE_CONTINUOUS_TIMED_RANGING: case VL53L0X_DEVICEMODE_GPIO_DRIVE: case VL53L0X_DEVICEMODE_GPIO_OSC: /* Supported modes */ VL53L0X_SETPARAMETERFIELD(Dev, DeviceMode, DeviceMode); break; default: /* Unsupported mode */ Status = VL53L0X_ERROR_MODE_NOT_SUPPORTED; } LOG_FUNCTION_END(Status); return Status; } VL53L0X_Error VL53L0X::VL53L0X_GetInterruptThresholds(VL53L0X_DEV Dev, VL53L0X_DeviceModes DeviceMode, FixPoint1616_t *pThresholdLow, FixPoint1616_t *pThresholdHigh) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; uint16_t Threshold16; LOG_FUNCTION_START(""); /* no dependency on DeviceMode for Ewok */ Status = VL53L0X_RdWord(Dev, VL53L0X_REG_SYSTEM_THRESH_LOW, &Threshold16); /* Need to multiply by 2 because the FW will apply a x2 */ *pThresholdLow = (FixPoint1616_t)((0x00fff & Threshold16) << 17); if (Status == VL53L0X_ERROR_NONE) { Status = VL53L0X_RdWord(Dev, VL53L0X_REG_SYSTEM_THRESH_HIGH, &Threshold16); /* Need to multiply by 2 because the FW will apply a x2 */ *pThresholdHigh = (FixPoint1616_t)((0x00fff & Threshold16) << 17); } LOG_FUNCTION_END(Status); return Status; } VL53L0X_Error VL53L0X::VL53L0X_load_tuning_settings(VL53L0X_DEV Dev, uint8_t *pTuningSettingBuffer) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; int i; int Index; uint8_t msb; uint8_t lsb; uint8_t SelectParam; uint8_t NumberOfWrites; uint8_t Address; uint8_t localBuffer[4]; /* max */ uint16_t Temp16; LOG_FUNCTION_START(""); Index = 0; while ((*(pTuningSettingBuffer + Index) != 0) && (Status == VL53L0X_ERROR_NONE)) { NumberOfWrites = *(pTuningSettingBuffer + Index); Index++; if (NumberOfWrites == 0xFF) { /* internal parameters */ SelectParam = *(pTuningSettingBuffer + Index); Index++; switch (SelectParam) { case 0: /* uint16_t SigmaEstRefArray -> 2 bytes */ msb = *(pTuningSettingBuffer + Index); Index++; lsb = *(pTuningSettingBuffer + Index); Index++; Temp16 = VL53L0X_MAKEUINT16(lsb, msb); PALDevDataSet(Dev, SigmaEstRefArray, Temp16); break; case 1: /* uint16_t SigmaEstEffPulseWidth -> 2 bytes */ msb = *(pTuningSettingBuffer + Index); Index++; lsb = *(pTuningSettingBuffer + Index); Index++; Temp16 = VL53L0X_MAKEUINT16(lsb, msb); PALDevDataSet(Dev, SigmaEstEffPulseWidth, Temp16); break; case 2: /* uint16_t SigmaEstEffAmbWidth -> 2 bytes */ msb = *(pTuningSettingBuffer + Index); Index++; lsb = *(pTuningSettingBuffer + Index); Index++; Temp16 = VL53L0X_MAKEUINT16(lsb, msb); PALDevDataSet(Dev, SigmaEstEffAmbWidth, Temp16); break; case 3: /* uint16_t targetRefRate -> 2 bytes */ msb = *(pTuningSettingBuffer + Index); Index++; lsb = *(pTuningSettingBuffer + Index); Index++; Temp16 = VL53L0X_MAKEUINT16(lsb, msb); PALDevDataSet(Dev, targetRefRate, Temp16); break; default: /* invalid parameter */ Status = VL53L0X_ERROR_INVALID_PARAMS; } } else if (NumberOfWrites <= 4) { Address = *(pTuningSettingBuffer + Index); Index++; for (i = 0; i < NumberOfWrites; i++) { localBuffer[i] = *(pTuningSettingBuffer + Index); Index++; } Status = VL53L0X_WriteMulti(Dev, Address, localBuffer, NumberOfWrites); } else { Status = VL53L0X_ERROR_INVALID_PARAMS; } } LOG_FUNCTION_END(Status); return Status; } VL53L0X_Error VL53L0X::VL53L0X_CheckAndLoadInterruptSettings(VL53L0X_DEV Dev, uint8_t StartNotStopFlag) { uint8_t InterruptConfig; FixPoint1616_t ThresholdLow; FixPoint1616_t ThresholdHigh; VL53L0X_Error Status = VL53L0X_ERROR_NONE; InterruptConfig = VL53L0X_GETDEVICESPECIFICPARAMETER(Dev, Pin0GpioFunctionality); if ((InterruptConfig == VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_LOW) || (InterruptConfig == VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_HIGH) || (InterruptConfig == VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_OUT)) { Status = VL53L0X_GetInterruptThresholds(Dev, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING, &ThresholdLow, &ThresholdHigh); if (((ThresholdLow > 255*65536) || (ThresholdHigh > 255*65536)) && (Status == VL53L0X_ERROR_NONE)) { if (StartNotStopFlag != 0) { Status = VL53L0X_load_tuning_settings(Dev, InterruptThresholdSettings); } else { Status |= VL53L0X_WrByte(Dev, 0xFF, 0x04); Status |= VL53L0X_WrByte(Dev, 0x70, 0x00); Status |= VL53L0X_WrByte(Dev, 0xFF, 0x00); Status |= VL53L0X_WrByte(Dev, 0x80, 0x00); } } } return Status; } VL53L0X_Error VL53L0X::VL53L0X_StartMeasurement(VL53L0X_DEV Dev) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; VL53L0X_DeviceModes DeviceMode; uint8_t Byte; uint8_t StartStopByte = VL53L0X_REG_SYSRANGE_MODE_START_STOP; uint32_t LoopNb; LOG_FUNCTION_START(""); /* Get Current DeviceMode */ VL53L0X_GetDeviceMode(Dev, &DeviceMode); Status = VL53L0X_WrByte(Dev, 0x80, 0x01); Status = VL53L0X_WrByte(Dev, 0xFF, 0x01); Status = VL53L0X_WrByte(Dev, 0x00, 0x00); Status = VL53L0X_WrByte(Dev, 0x91, PALDevDataGet(Dev, StopVariable)); Status = VL53L0X_WrByte(Dev, 0x00, 0x01); Status = VL53L0X_WrByte(Dev, 0xFF, 0x00); Status = VL53L0X_WrByte(Dev, 0x80, 0x00); switch (DeviceMode) { case VL53L0X_DEVICEMODE_SINGLE_RANGING: Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSRANGE_START, 0x01); Byte = StartStopByte; if (Status == VL53L0X_ERROR_NONE) { /* Wait until start bit has been cleared */ LoopNb = 0; do { if (LoopNb > 0) Status = VL53L0X_RdByte(Dev, VL53L0X_REG_SYSRANGE_START, &Byte); LoopNb = LoopNb + 1; } while (((Byte & StartStopByte) == StartStopByte) && (Status == VL53L0X_ERROR_NONE) && (LoopNb < VL53L0X_DEFAULT_MAX_LOOP)); if (LoopNb >= VL53L0X_DEFAULT_MAX_LOOP) Status = VL53L0X_ERROR_TIME_OUT; } break; case VL53L0X_DEVICEMODE_CONTINUOUS_RANGING: /* Back-to-back mode */ /* Check if need to apply interrupt settings */ if (Status == VL53L0X_ERROR_NONE) Status = VL53L0X_CheckAndLoadInterruptSettings(Dev, 1); Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSRANGE_START, VL53L0X_REG_SYSRANGE_MODE_BACKTOBACK); if (Status == VL53L0X_ERROR_NONE) { /* Set PAL State to Running */ PALDevDataSet(Dev, PalState, VL53L0X_STATE_RUNNING); } break; case VL53L0X_DEVICEMODE_CONTINUOUS_TIMED_RANGING: /* Continuous mode */ /* Check if need to apply interrupt settings */ if (Status == VL53L0X_ERROR_NONE) Status = VL53L0X_CheckAndLoadInterruptSettings(Dev, 1); Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSRANGE_START, VL53L0X_REG_SYSRANGE_MODE_TIMED); if (Status == VL53L0X_ERROR_NONE) { /* Set PAL State to Running */ PALDevDataSet(Dev, PalState, VL53L0X_STATE_RUNNING); } break; default: /* Selected mode not supported */ Status = VL53L0X_ERROR_MODE_NOT_SUPPORTED; } LOG_FUNCTION_END(Status); return Status; } /* Group PAL Measurement Functions */ VL53L0X_Error VL53L0X::VL53L0X_PerformSingleMeasurement(VL53L0X_DEV Dev) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; VL53L0X_DeviceModes DeviceMode; LOG_FUNCTION_START(""); /* Get Current DeviceMode */ Status = VL53L0X_GetDeviceMode(Dev, &DeviceMode); /* Start immediately to run a single ranging measurement in case of * single ranging or single histogram */ if (Status == VL53L0X_ERROR_NONE && DeviceMode == VL53L0X_DEVICEMODE_SINGLE_RANGING) Status = VL53L0X_StartMeasurement(Dev); if (Status == VL53L0X_ERROR_NONE) Status = VL53L0X_measurement_poll_for_completion(Dev); /* Change PAL State in case of single ranging or single histogram */ if (Status == VL53L0X_ERROR_NONE && DeviceMode == VL53L0X_DEVICEMODE_SINGLE_RANGING) PALDevDataSet(Dev, PalState, VL53L0X_STATE_IDLE); LOG_FUNCTION_END(Status); return Status; } VL53L0X_Error VL53L0X::VL53L0X_GetXTalkCompensationEnable(VL53L0X_DEV Dev, uint8_t *pXTalkCompensationEnable) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; uint8_t Temp8; LOG_FUNCTION_START(""); VL53L0X_GETPARAMETERFIELD(Dev, XTalkCompensationEnable, Temp8); *pXTalkCompensationEnable = Temp8; LOG_FUNCTION_END(Status); return Status; } VL53L0X_Error VL53L0X::VL53L0X_get_total_xtalk_rate(VL53L0X_DEV Dev, VL53L0X_RangingMeasurementData_t *pRangingMeasurementData, FixPoint1616_t *ptotal_xtalk_rate_mcps) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; uint8_t xtalkCompEnable; FixPoint1616_t totalXtalkMegaCps; FixPoint1616_t xtalkPerSpadMegaCps; *ptotal_xtalk_rate_mcps = 0; Status = VL53L0X_GetXTalkCompensationEnable(Dev, &xtalkCompEnable); if (Status == VL53L0X_ERROR_NONE) { if (xtalkCompEnable) { VL53L0X_GETPARAMETERFIELD( Dev, XTalkCompensationRateMegaCps, xtalkPerSpadMegaCps); /* FixPoint1616 * FixPoint 8:8 = FixPoint0824 */ totalXtalkMegaCps = pRangingMeasurementData->EffectiveSpadRtnCount * xtalkPerSpadMegaCps; /* FixPoint0824 >> 8 = FixPoint1616 */ *ptotal_xtalk_rate_mcps = (totalXtalkMegaCps + 0x80) >> 8; } } return Status; } VL53L0X_Error VL53L0X::VL53L0X_get_total_signal_rate(VL53L0X_DEV Dev, VL53L0X_RangingMeasurementData_t *pRangingMeasurementData, FixPoint1616_t *ptotal_signal_rate_mcps) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; FixPoint1616_t totalXtalkMegaCps; LOG_FUNCTION_START(""); *ptotal_signal_rate_mcps = pRangingMeasurementData->SignalRateRtnMegaCps; Status = VL53L0X_get_total_xtalk_rate( Dev, pRangingMeasurementData, &totalXtalkMegaCps); if (Status == VL53L0X_ERROR_NONE) *ptotal_signal_rate_mcps += totalXtalkMegaCps; return Status; } /* To convert ms into register value */ uint32_t VL53L0X::VL53L0X_calc_timeout_mclks(VL53L0X_DEV Dev, uint32_t timeout_period_us, uint8_t vcsel_period_pclks) { uint32_t macro_period_ps; uint32_t macro_period_ns; uint32_t timeout_period_mclks = 0; macro_period_ps = VL53L0X_calc_macro_period_ps(Dev, vcsel_period_pclks); macro_period_ns = (macro_period_ps + 500) / 1000; timeout_period_mclks = (uint32_t) (((timeout_period_us * 1000) + (macro_period_ns / 2)) / macro_period_ns); return timeout_period_mclks; } uint32_t VL53L0X::VL53L0X_isqrt(uint32_t num) { /* * Implements an integer square root * * From: http://en.wikipedia.org/wiki/Methods_of_computing_square_roots */ uint32_t res = 0; uint32_t bit = 1 << 30; /* The second-to-top bit is set: * 1 << 14 for 16-bits, 1 << 30 for 32 bits */ /* "bit" starts at the highest power of four <= the argument. */ while (bit > num) bit >>= 2; while (bit != 0) { if (num >= res + bit) { num -= res + bit; res = (res >> 1) + bit; } else res >>= 1; bit >>= 2; } return res; } VL53L0X_Error VL53L0X::VL53L0X_calc_dmax( VL53L0X_DEV Dev, FixPoint1616_t totalSignalRate_mcps, FixPoint1616_t totalCorrSignalRate_mcps, FixPoint1616_t pwMult, uint32_t sigmaEstimateP1, FixPoint1616_t sigmaEstimateP2, uint32_t peakVcselDuration_us, uint32_t *pdmax_mm) { const uint32_t cSigmaLimit = 18; const FixPoint1616_t cSignalLimit = 0x4000; /* 0.25 */ const FixPoint1616_t cSigmaEstRef = 0x00000042; /* 0.001 */ const uint32_t cAmbEffWidthSigmaEst_ns = 6; const uint32_t cAmbEffWidthDMax_ns = 7; uint32_t dmaxCalRange_mm; FixPoint1616_t dmaxCalSignalRateRtn_mcps; FixPoint1616_t minSignalNeeded; FixPoint1616_t minSignalNeeded_p1; FixPoint1616_t minSignalNeeded_p2; FixPoint1616_t minSignalNeeded_p3; FixPoint1616_t minSignalNeeded_p4; FixPoint1616_t sigmaLimitTmp; FixPoint1616_t sigmaEstSqTmp; FixPoint1616_t signalLimitTmp; FixPoint1616_t SignalAt0mm; FixPoint1616_t dmaxDark; FixPoint1616_t dmaxAmbient; FixPoint1616_t dmaxDarkTmp; FixPoint1616_t sigmaEstP2Tmp; uint32_t signalRateTemp_mcps; VL53L0X_Error Status = VL53L0X_ERROR_NONE; LOG_FUNCTION_START(""); dmaxCalRange_mm = PALDevDataGet(Dev, DmaxCalRangeMilliMeter); dmaxCalSignalRateRtn_mcps = PALDevDataGet(Dev, DmaxCalSignalRateRtnMegaCps); /* uint32 * FixPoint1616 = FixPoint1616 */ SignalAt0mm = dmaxCalRange_mm * dmaxCalSignalRateRtn_mcps; /* FixPoint1616 >> 8 = FixPoint2408 */ SignalAt0mm = (SignalAt0mm + 0x80) >> 8; SignalAt0mm *= dmaxCalRange_mm; minSignalNeeded_p1 = 0; if (totalCorrSignalRate_mcps > 0) { /* Shift by 10 bits to increase resolution prior to the * division */ signalRateTemp_mcps = totalSignalRate_mcps << 10; /* Add rounding value prior to division */ minSignalNeeded_p1 = signalRateTemp_mcps + (totalCorrSignalRate_mcps/2); /* FixPoint0626/FixPoint1616 = FixPoint2210 */ minSignalNeeded_p1 /= totalCorrSignalRate_mcps; /* Apply a factored version of the speed of light. Correction to be applied at the end */ minSignalNeeded_p1 *= 3; /* FixPoint2210 * FixPoint2210 = FixPoint1220 */ minSignalNeeded_p1 *= minSignalNeeded_p1; /* FixPoint1220 >> 16 = FixPoint2804 */ minSignalNeeded_p1 = (minSignalNeeded_p1 + 0x8000) >> 16; } minSignalNeeded_p2 = pwMult * sigmaEstimateP1; /* FixPoint1616 >> 16 = uint32 */ minSignalNeeded_p2 = (minSignalNeeded_p2 + 0x8000) >> 16; /* uint32 * uint32 = uint32 */ minSignalNeeded_p2 *= minSignalNeeded_p2; /* Check sigmaEstimateP2 * If this value is too high there is not enough signal rate * to calculate dmax value so set a suitable value to ensure * a very small dmax. */ sigmaEstP2Tmp = (sigmaEstimateP2 + 0x8000) >> 16; sigmaEstP2Tmp = (sigmaEstP2Tmp + cAmbEffWidthSigmaEst_ns/2)/ cAmbEffWidthSigmaEst_ns; sigmaEstP2Tmp *= cAmbEffWidthDMax_ns; if (sigmaEstP2Tmp > 0xffff) { minSignalNeeded_p3 = 0xfff00000; } else { /* DMAX uses a different ambient width from sigma, so apply * correction. * Perform division before multiplication to prevent overflow. */ sigmaEstimateP2 = (sigmaEstimateP2 + cAmbEffWidthSigmaEst_ns/2)/ cAmbEffWidthSigmaEst_ns; sigmaEstimateP2 *= cAmbEffWidthDMax_ns; /* FixPoint1616 >> 16 = uint32 */ minSignalNeeded_p3 = (sigmaEstimateP2 + 0x8000) >> 16; minSignalNeeded_p3 *= minSignalNeeded_p3; } /* FixPoint1814 / uint32 = FixPoint1814 */ sigmaLimitTmp = ((cSigmaLimit << 14) + 500) / 1000; /* FixPoint1814 * FixPoint1814 = FixPoint3628 := FixPoint0428 */ sigmaLimitTmp *= sigmaLimitTmp; /* FixPoint1616 * FixPoint1616 = FixPoint3232 */ sigmaEstSqTmp = cSigmaEstRef * cSigmaEstRef; /* FixPoint3232 >> 4 = FixPoint0428 */ sigmaEstSqTmp = (sigmaEstSqTmp + 0x08) >> 4; /* FixPoint0428 - FixPoint0428 = FixPoint0428 */ sigmaLimitTmp -= sigmaEstSqTmp; /* uint32_t * FixPoint0428 = FixPoint0428 */ minSignalNeeded_p4 = 4 * 12 * sigmaLimitTmp; /* FixPoint0428 >> 14 = FixPoint1814 */ minSignalNeeded_p4 = (minSignalNeeded_p4 + 0x2000) >> 14; /* uint32 + uint32 = uint32 */ minSignalNeeded = (minSignalNeeded_p2 + minSignalNeeded_p3); /* uint32 / uint32 = uint32 */ minSignalNeeded += (peakVcselDuration_us/2); minSignalNeeded /= peakVcselDuration_us; /* uint32 << 14 = FixPoint1814 */ minSignalNeeded <<= 14; /* FixPoint1814 / FixPoint1814 = uint32 */ minSignalNeeded += (minSignalNeeded_p4/2); minSignalNeeded /= minSignalNeeded_p4; /* FixPoint3200 * FixPoint2804 := FixPoint2804*/ minSignalNeeded *= minSignalNeeded_p1; /* Apply correction by dividing by 1000000. * This assumes 10E16 on the numerator of the equation * and 10E-22 on the denominator. * We do this because 32bit fix point calculation can't * handle the larger and smaller elements of this equation, * i.e. speed of light and pulse widths. */ minSignalNeeded = (minSignalNeeded + 500) / 1000; minSignalNeeded <<= 4; minSignalNeeded = (minSignalNeeded + 500) / 1000; /* FixPoint1616 >> 8 = FixPoint2408 */ signalLimitTmp = (cSignalLimit + 0x80) >> 8; /* FixPoint2408/FixPoint2408 = uint32 */ if (signalLimitTmp != 0) dmaxDarkTmp = (SignalAt0mm + (signalLimitTmp / 2)) / signalLimitTmp; else dmaxDarkTmp = 0; dmaxDark = VL53L0X_isqrt(dmaxDarkTmp); /* FixPoint2408/FixPoint2408 = uint32 */ if (minSignalNeeded != 0) dmaxAmbient = (SignalAt0mm + minSignalNeeded/2) / minSignalNeeded; else dmaxAmbient = 0; dmaxAmbient = VL53L0X_isqrt(dmaxAmbient); *pdmax_mm = dmaxDark; if (dmaxDark > dmaxAmbient) *pdmax_mm = dmaxAmbient; LOG_FUNCTION_END(Status); return Status; } VL53L0X_Error VL53L0X::VL53L0X_calc_sigma_estimate(VL53L0X_DEV Dev, VL53L0X_RangingMeasurementData_t *pRangingMeasurementData, FixPoint1616_t *pSigmaEstimate, uint32_t *pDmax_mm) { /* Expressed in 100ths of a ns, i.e. centi-ns */ const uint32_t cPulseEffectiveWidth_centi_ns = 800; /* Expressed in 100ths of a ns, i.e. centi-ns */ const uint32_t cAmbientEffectiveWidth_centi_ns = 600; const FixPoint1616_t cDfltFinalRangeIntegrationTimeMilliSecs = 0x00190000; /* 25ms */ const uint32_t cVcselPulseWidth_ps = 4700; /* pico secs */ const FixPoint1616_t cSigmaEstMax = 0x028F87AE; const FixPoint1616_t cSigmaEstRtnMax = 0xF000; const FixPoint1616_t cAmbToSignalRatioMax = 0xF0000000/ cAmbientEffectiveWidth_centi_ns; /* Time Of Flight per mm (6.6 pico secs) */ const FixPoint1616_t cTOF_per_mm_ps = 0x0006999A; const uint32_t c16BitRoundingParam = 0x00008000; const FixPoint1616_t cMaxXTalk_kcps = 0x00320000; const uint32_t cPllPeriod_ps = 1655; uint32_t vcselTotalEventsRtn; uint32_t finalRangeTimeoutMicroSecs; uint32_t preRangeTimeoutMicroSecs; uint32_t finalRangeIntegrationTimeMilliSecs; FixPoint1616_t sigmaEstimateP1; FixPoint1616_t sigmaEstimateP2; FixPoint1616_t sigmaEstimateP3; FixPoint1616_t deltaT_ps; FixPoint1616_t pwMult; FixPoint1616_t sigmaEstRtn; FixPoint1616_t sigmaEstimate; FixPoint1616_t xTalkCorrection; FixPoint1616_t ambientRate_kcps; FixPoint1616_t peakSignalRate_kcps; FixPoint1616_t xTalkCompRate_mcps; uint32_t xTalkCompRate_kcps; VL53L0X_Error Status = VL53L0X_ERROR_NONE; FixPoint1616_t diff1_mcps; FixPoint1616_t diff2_mcps; FixPoint1616_t sqr1; FixPoint1616_t sqr2; FixPoint1616_t sqrSum; FixPoint1616_t sqrtResult_centi_ns; FixPoint1616_t sqrtResult; FixPoint1616_t totalSignalRate_mcps; FixPoint1616_t correctedSignalRate_mcps; FixPoint1616_t sigmaEstRef; uint32_t vcselWidth; uint32_t finalRangeMacroPCLKS; uint32_t preRangeMacroPCLKS; uint32_t peakVcselDuration_us; uint8_t finalRangeVcselPCLKS; uint8_t preRangeVcselPCLKS; /*! \addtogroup calc_sigma_estimate * @{ * * Estimates the range sigma */ LOG_FUNCTION_START(""); VL53L0X_GETPARAMETERFIELD(Dev, XTalkCompensationRateMegaCps, xTalkCompRate_mcps); /* * We work in kcps rather than mcps as this helps keep within the * confines of the 32 Fix1616 type. */ ambientRate_kcps = (pRangingMeasurementData->AmbientRateRtnMegaCps * 1000) >> 16; correctedSignalRate_mcps = pRangingMeasurementData->SignalRateRtnMegaCps; Status = VL53L0X_get_total_signal_rate( Dev, pRangingMeasurementData, &totalSignalRate_mcps); Status = VL53L0X_get_total_xtalk_rate( Dev, pRangingMeasurementData, &xTalkCompRate_mcps); /* Signal rate measurement provided by device is the * peak signal rate, not average. */ peakSignalRate_kcps = (totalSignalRate_mcps * 1000); peakSignalRate_kcps = (peakSignalRate_kcps + 0x8000) >> 16; xTalkCompRate_kcps = xTalkCompRate_mcps * 1000; if (xTalkCompRate_kcps > cMaxXTalk_kcps) xTalkCompRate_kcps = cMaxXTalk_kcps; if (Status == VL53L0X_ERROR_NONE) { /* Calculate final range macro periods */ finalRangeTimeoutMicroSecs = VL53L0X_GETDEVICESPECIFICPARAMETER( Dev, FinalRangeTimeoutMicroSecs); finalRangeVcselPCLKS = VL53L0X_GETDEVICESPECIFICPARAMETER( Dev, FinalRangeVcselPulsePeriod); finalRangeMacroPCLKS = VL53L0X_calc_timeout_mclks( Dev, finalRangeTimeoutMicroSecs, finalRangeVcselPCLKS); /* Calculate pre-range macro periods */ preRangeTimeoutMicroSecs = VL53L0X_GETDEVICESPECIFICPARAMETER( Dev, PreRangeTimeoutMicroSecs); preRangeVcselPCLKS = VL53L0X_GETDEVICESPECIFICPARAMETER( Dev, PreRangeVcselPulsePeriod); preRangeMacroPCLKS = VL53L0X_calc_timeout_mclks( Dev, preRangeTimeoutMicroSecs, preRangeVcselPCLKS); vcselWidth = 3; if (finalRangeVcselPCLKS == 8) vcselWidth = 2; peakVcselDuration_us = vcselWidth * 2048 * (preRangeMacroPCLKS + finalRangeMacroPCLKS); peakVcselDuration_us = (peakVcselDuration_us + 500)/1000; peakVcselDuration_us *= cPllPeriod_ps; peakVcselDuration_us = (peakVcselDuration_us + 500)/1000; /* Fix1616 >> 8 = Fix2408 */ totalSignalRate_mcps = (totalSignalRate_mcps + 0x80) >> 8; /* Fix2408 * uint32 = Fix2408 */ vcselTotalEventsRtn = totalSignalRate_mcps * peakVcselDuration_us; /* Fix2408 >> 8 = uint32 */ vcselTotalEventsRtn = (vcselTotalEventsRtn + 0x80) >> 8; /* Fix2408 << 8 = Fix1616 = */ totalSignalRate_mcps <<= 8; } if (Status != VL53L0X_ERROR_NONE) { LOG_FUNCTION_END(Status); return Status; } if (peakSignalRate_kcps == 0) { *pSigmaEstimate = cSigmaEstMax; PALDevDataSet(Dev, SigmaEstimate, cSigmaEstMax); *pDmax_mm = 0; } else { if (vcselTotalEventsRtn < 1) vcselTotalEventsRtn = 1; sigmaEstimateP1 = cPulseEffectiveWidth_centi_ns; /* ((FixPoint1616 << 16)* uint32)/uint32 = FixPoint1616 */ sigmaEstimateP2 = (ambientRate_kcps << 16)/peakSignalRate_kcps; if (sigmaEstimateP2 > cAmbToSignalRatioMax) { /* Clip to prevent overflow. Will ensure safe * max result. */ sigmaEstimateP2 = cAmbToSignalRatioMax; } sigmaEstimateP2 *= cAmbientEffectiveWidth_centi_ns; sigmaEstimateP3 = 2 * VL53L0X_isqrt(vcselTotalEventsRtn * 12); /* uint32 * FixPoint1616 = FixPoint1616 */ deltaT_ps = pRangingMeasurementData->RangeMilliMeter * cTOF_per_mm_ps; /* * vcselRate - xtalkCompRate * (uint32 << 16) - FixPoint1616 = FixPoint1616. * Divide result by 1000 to convert to mcps. * 500 is added to ensure rounding when integer division * truncates. */ diff1_mcps = (((peakSignalRate_kcps << 16) - 2 * xTalkCompRate_kcps) + 500)/1000; /* vcselRate + xtalkCompRate */ diff2_mcps = ((peakSignalRate_kcps << 16) + 500)/1000; /* Shift by 8 bits to increase resolution prior to the * division */ diff1_mcps <<= 8; /* FixPoint0824/FixPoint1616 = FixPoint2408 */ // xTalkCorrection = abs(diff1_mcps/diff2_mcps); // abs is causing compiler overloading isue in C++, but unsigned types. So, redundant call anyway! xTalkCorrection = diff1_mcps/diff2_mcps; /* FixPoint2408 << 8 = FixPoint1616 */ xTalkCorrection <<= 8; if(pRangingMeasurementData->RangeStatus != 0){ pwMult = 1 << 16; } else { /* FixPoint1616/uint32 = FixPoint1616 */ pwMult = deltaT_ps/cVcselPulseWidth_ps; /* smaller than 1.0f */ /* * FixPoint1616 * FixPoint1616 = FixPoint3232, however both * values are small enough such that32 bits will not be * exceeded. */ pwMult *= ((1 << 16) - xTalkCorrection); /* (FixPoint3232 >> 16) = FixPoint1616 */ pwMult = (pwMult + c16BitRoundingParam) >> 16; /* FixPoint1616 + FixPoint1616 = FixPoint1616 */ pwMult += (1 << 16); /* * At this point the value will be 1.xx, therefore if we square * the value this will exceed 32 bits. To address this perform * a single shift to the right before the multiplication. */ pwMult >>= 1; /* FixPoint1715 * FixPoint1715 = FixPoint3430 */ pwMult = pwMult * pwMult; /* (FixPoint3430 >> 14) = Fix1616 */ pwMult >>= 14; } /* FixPoint1616 * uint32 = FixPoint1616 */ sqr1 = pwMult * sigmaEstimateP1; /* (FixPoint1616 >> 16) = FixPoint3200 */ sqr1 = (sqr1 + 0x8000) >> 16; /* FixPoint3200 * FixPoint3200 = FixPoint6400 */ sqr1 *= sqr1; sqr2 = sigmaEstimateP2; /* (FixPoint1616 >> 16) = FixPoint3200 */ sqr2 = (sqr2 + 0x8000) >> 16; /* FixPoint3200 * FixPoint3200 = FixPoint6400 */ sqr2 *= sqr2; /* FixPoint64000 + FixPoint6400 = FixPoint6400 */ sqrSum = sqr1 + sqr2; /* SQRT(FixPoin6400) = FixPoint3200 */ sqrtResult_centi_ns = VL53L0X_isqrt(sqrSum); /* (FixPoint3200 << 16) = FixPoint1616 */ sqrtResult_centi_ns <<= 16; /* * Note that the Speed Of Light is expressed in um per 1E-10 * seconds (2997) Therefore to get mm/ns we have to divide by * 10000 */ sigmaEstRtn = (((sqrtResult_centi_ns+50)/100) / sigmaEstimateP3); sigmaEstRtn *= VL53L0X_SPEED_OF_LIGHT_IN_AIR; /* Add 5000 before dividing by 10000 to ensure rounding. */ sigmaEstRtn += 5000; sigmaEstRtn /= 10000; if (sigmaEstRtn > cSigmaEstRtnMax) { /* Clip to prevent overflow. Will ensure safe * max result. */ sigmaEstRtn = cSigmaEstRtnMax; } finalRangeIntegrationTimeMilliSecs = (finalRangeTimeoutMicroSecs + preRangeTimeoutMicroSecs + 500)/1000; /* sigmaEstRef = 1mm * 25ms/final range integration time (inc pre-range) * sqrt(FixPoint1616/int) = FixPoint2408) */ sigmaEstRef = VL53L0X_isqrt((cDfltFinalRangeIntegrationTimeMilliSecs + finalRangeIntegrationTimeMilliSecs/2)/ finalRangeIntegrationTimeMilliSecs); /* FixPoint2408 << 8 = FixPoint1616 */ sigmaEstRef <<= 8; sigmaEstRef = (sigmaEstRef + 500)/1000; /* FixPoint1616 * FixPoint1616 = FixPoint3232 */ sqr1 = sigmaEstRtn * sigmaEstRtn; /* FixPoint1616 * FixPoint1616 = FixPoint3232 */ sqr2 = sigmaEstRef * sigmaEstRef; /* sqrt(FixPoint3232) = FixPoint1616 */ sqrtResult = VL53L0X_isqrt((sqr1 + sqr2)); /* * Note that the Shift by 4 bits increases resolution prior to * the sqrt, therefore the result must be shifted by 2 bits to * the right to revert back to the FixPoint1616 format. */ sigmaEstimate = 1000 * sqrtResult; if ((peakSignalRate_kcps < 1) || (vcselTotalEventsRtn < 1) || (sigmaEstimate > cSigmaEstMax)) { sigmaEstimate = cSigmaEstMax; } *pSigmaEstimate = (uint32_t)(sigmaEstimate); PALDevDataSet(Dev, SigmaEstimate, *pSigmaEstimate); Status = VL53L0X_calc_dmax( Dev, totalSignalRate_mcps, correctedSignalRate_mcps, pwMult, sigmaEstimateP1, sigmaEstimateP2, peakVcselDuration_us, pDmax_mm); } LOG_FUNCTION_END(Status); return Status; } VL53L0X_Error VL53L0X::VL53L0X_get_pal_range_status(VL53L0X_DEV Dev, uint8_t DeviceRangeStatus, FixPoint1616_t SignalRate, uint16_t EffectiveSpadRtnCount, VL53L0X_RangingMeasurementData_t *pRangingMeasurementData, uint8_t *pPalRangeStatus) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; uint8_t NoneFlag; uint8_t SigmaLimitflag = 0; uint8_t SignalRefClipflag = 0; uint8_t RangeIgnoreThresholdflag = 0; uint8_t SigmaLimitCheckEnable = 0; uint8_t SignalRateFinalRangeLimitCheckEnable = 0; uint8_t SignalRefClipLimitCheckEnable = 0; uint8_t RangeIgnoreThresholdLimitCheckEnable = 0; FixPoint1616_t SigmaEstimate; FixPoint1616_t SigmaLimitValue; FixPoint1616_t SignalRefClipValue; FixPoint1616_t RangeIgnoreThresholdValue; FixPoint1616_t SignalRatePerSpad; uint8_t DeviceRangeStatusInternal = 0; uint16_t tmpWord = 0; uint8_t Temp8; uint32_t Dmax_mm = 0; FixPoint1616_t LastSignalRefMcps; LOG_FUNCTION_START(""); /* * VL53L0X has a good ranging when the value of the * DeviceRangeStatus = 11. This function will replace the value 0 with * the value 11 in the DeviceRangeStatus. * In addition, the SigmaEstimator is not included in the VL53L0X * DeviceRangeStatus, this will be added in the PalRangeStatus. */ DeviceRangeStatusInternal = ((DeviceRangeStatus & 0x78) >> 3); if (DeviceRangeStatusInternal == 0 || DeviceRangeStatusInternal == 5 || DeviceRangeStatusInternal == 7 || DeviceRangeStatusInternal == 12 || DeviceRangeStatusInternal == 13 || DeviceRangeStatusInternal == 14 || DeviceRangeStatusInternal == 15 ) { NoneFlag = 1; } else { NoneFlag = 0; } /* * Check if Sigma limit is enabled, if yes then do comparison with limit * value and put the result back into pPalRangeStatus. */ if (Status == VL53L0X_ERROR_NONE) Status = VL53L0X_GetLimitCheckEnable(Dev, VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, &SigmaLimitCheckEnable); if ((SigmaLimitCheckEnable != 0) && (Status == VL53L0X_ERROR_NONE)) { /* * compute the Sigma and check with limit */ Status = VL53L0X_calc_sigma_estimate( Dev, pRangingMeasurementData, &SigmaEstimate, &Dmax_mm); if (Status == VL53L0X_ERROR_NONE) pRangingMeasurementData->RangeDMaxMilliMeter = Dmax_mm; if (Status == VL53L0X_ERROR_NONE) { Status = VL53L0X_GetLimitCheckValue(Dev, VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, &SigmaLimitValue); if ((SigmaLimitValue > 0) && (SigmaEstimate > SigmaLimitValue)) /* Limit Fail */ SigmaLimitflag = 1; } } /* * Check if Signal ref clip limit is enabled, if yes then do comparison * with limit value and put the result back into pPalRangeStatus. */ if (Status == VL53L0X_ERROR_NONE) Status = VL53L0X_GetLimitCheckEnable(Dev, VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, &SignalRefClipLimitCheckEnable); if ((SignalRefClipLimitCheckEnable != 0) && (Status == VL53L0X_ERROR_NONE)) { Status = VL53L0X_GetLimitCheckValue(Dev, VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, &SignalRefClipValue); /* Read LastSignalRefMcps from device */ 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, &tmpWord); if (Status == VL53L0X_ERROR_NONE) Status = VL53L0X_WrByte(Dev, 0xFF, 0x00); LastSignalRefMcps = VL53L0X_FIXPOINT97TOFIXPOINT1616(tmpWord); PALDevDataSet(Dev, LastSignalRefMcps, LastSignalRefMcps); if ((SignalRefClipValue > 0) && (LastSignalRefMcps > SignalRefClipValue)) { /* Limit Fail */ SignalRefClipflag = 1; } } /* * Check if Signal ref clip limit is enabled, if yes then do comparison * with limit value and put the result back into pPalRangeStatus. * EffectiveSpadRtnCount has a format 8.8 * If (Return signal rate < (1.5 x Xtalk x number of Spads)) : FAIL */ if (Status == VL53L0X_ERROR_NONE) Status = VL53L0X_GetLimitCheckEnable(Dev, VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, &RangeIgnoreThresholdLimitCheckEnable); if ((RangeIgnoreThresholdLimitCheckEnable != 0) && (Status == VL53L0X_ERROR_NONE)) { /* Compute the signal rate per spad */ if (EffectiveSpadRtnCount == 0) { SignalRatePerSpad = 0; } else { SignalRatePerSpad = (FixPoint1616_t)((256 * SignalRate) / EffectiveSpadRtnCount); } Status = VL53L0X_GetLimitCheckValue(Dev, VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, &RangeIgnoreThresholdValue); if ((RangeIgnoreThresholdValue > 0) && (SignalRatePerSpad < RangeIgnoreThresholdValue)) { /* Limit Fail add 2^6 to range status */ RangeIgnoreThresholdflag = 1; } } if (Status == VL53L0X_ERROR_NONE) { if (NoneFlag == 1) { *pPalRangeStatus = 255; /* NONE */ } else if (DeviceRangeStatusInternal == 1 || DeviceRangeStatusInternal == 2 || DeviceRangeStatusInternal == 3) { *pPalRangeStatus = 5; /* HW fail */ } else if (DeviceRangeStatusInternal == 6 || DeviceRangeStatusInternal == 9) { *pPalRangeStatus = 4; /* Phase fail */ } else if (DeviceRangeStatusInternal == 8 || DeviceRangeStatusInternal == 10 || SignalRefClipflag == 1) { *pPalRangeStatus = 3; /* Min range */ } else if (DeviceRangeStatusInternal == 4 || RangeIgnoreThresholdflag == 1) { *pPalRangeStatus = 2; /* Signal Fail */ } else if (SigmaLimitflag == 1) { *pPalRangeStatus = 1; /* Sigma Fail */ } else { *pPalRangeStatus = 0; /* Range Valid */ } } /* DMAX only relevant during range error */ if (*pPalRangeStatus == 0) pRangingMeasurementData->RangeDMaxMilliMeter = 0; /* fill the Limit Check Status */ Status = VL53L0X_GetLimitCheckEnable(Dev, VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, &SignalRateFinalRangeLimitCheckEnable); if (Status == VL53L0X_ERROR_NONE) { if ((SigmaLimitCheckEnable == 0) || (SigmaLimitflag == 1)) Temp8 = 1; else Temp8 = 0; VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksStatus, VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, Temp8); if ((DeviceRangeStatusInternal == 4) || (SignalRateFinalRangeLimitCheckEnable == 0)) Temp8 = 1; else Temp8 = 0; VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksStatus, VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, Temp8); if ((SignalRefClipLimitCheckEnable == 0) || (SignalRefClipflag == 1)) Temp8 = 1; else Temp8 = 0; VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksStatus, VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, Temp8); if ((RangeIgnoreThresholdLimitCheckEnable == 0) || (RangeIgnoreThresholdflag == 1)) Temp8 = 1; else Temp8 = 0; VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksStatus, VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, Temp8); } LOG_FUNCTION_END(Status); return Status; } VL53L0X_Error VL53L0X::VL53L0X_GetRangingMeasurementData(VL53L0X_DEV Dev, VL53L0X_RangingMeasurementData_t *pRangingMeasurementData) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; uint8_t DeviceRangeStatus; uint8_t RangeFractionalEnable; uint8_t PalRangeStatus; uint8_t XTalkCompensationEnable; uint16_t AmbientRate; FixPoint1616_t SignalRate; uint16_t XTalkCompensationRateMegaCps; uint16_t EffectiveSpadRtnCount; uint16_t tmpuint16; uint16_t XtalkRangeMilliMeter; uint16_t LinearityCorrectiveGain; uint8_t localBuffer[12]; VL53L0X_RangingMeasurementData_t LastRangeDataBuffer; LOG_FUNCTION_START(""); /* * use multi read even if some registers are not useful, result will * be more efficient * start reading at 0x14 dec20 * end reading at 0x21 dec33 total 14 bytes to read */ Status = VL53L0X_ReadMulti(Dev, 0x14, localBuffer, 12); if (Status == VL53L0X_ERROR_NONE) { pRangingMeasurementData->ZoneId = 0; /* Only one zone */ pRangingMeasurementData->TimeStamp = 0; /* Not Implemented */ tmpuint16 = VL53L0X_MAKEUINT16(localBuffer[11], localBuffer[10]); /* cut1.1 if SYSTEM__RANGE_CONFIG if 1 range is 2bits fractional *(format 11.2) else no fractional */ pRangingMeasurementData->MeasurementTimeUsec = 0; SignalRate = VL53L0X_FIXPOINT97TOFIXPOINT1616( VL53L0X_MAKEUINT16(localBuffer[7], localBuffer[6])); /* peak_signal_count_rate_rtn_mcps */ pRangingMeasurementData->SignalRateRtnMegaCps = SignalRate; AmbientRate = VL53L0X_MAKEUINT16(localBuffer[9], localBuffer[8]); pRangingMeasurementData->AmbientRateRtnMegaCps = VL53L0X_FIXPOINT97TOFIXPOINT1616(AmbientRate); EffectiveSpadRtnCount = VL53L0X_MAKEUINT16(localBuffer[3], localBuffer[2]); /* EffectiveSpadRtnCount is 8.8 format */ pRangingMeasurementData->EffectiveSpadRtnCount = EffectiveSpadRtnCount; DeviceRangeStatus = localBuffer[0]; /* Get Linearity Corrective Gain */ LinearityCorrectiveGain = PALDevDataGet(Dev, LinearityCorrectiveGain); /* Get ranging configuration */ RangeFractionalEnable = PALDevDataGet(Dev, RangeFractionalEnable); if (LinearityCorrectiveGain != 1000) { tmpuint16 = (uint16_t)((LinearityCorrectiveGain * tmpuint16 + 500) / 1000); /* Implement Xtalk */ VL53L0X_GETPARAMETERFIELD(Dev, XTalkCompensationRateMegaCps, XTalkCompensationRateMegaCps); VL53L0X_GETPARAMETERFIELD(Dev, XTalkCompensationEnable, XTalkCompensationEnable); if (XTalkCompensationEnable) { if ((SignalRate - ((XTalkCompensationRateMegaCps * EffectiveSpadRtnCount) >> 8)) <= 0) { if (RangeFractionalEnable) XtalkRangeMilliMeter = 8888; else XtalkRangeMilliMeter = 8888 << 2; } else { XtalkRangeMilliMeter = (tmpuint16 * SignalRate) / (SignalRate - ((XTalkCompensationRateMegaCps * EffectiveSpadRtnCount) >> 8)); } tmpuint16 = XtalkRangeMilliMeter; } } if (RangeFractionalEnable) { pRangingMeasurementData->RangeMilliMeter = (uint16_t)((tmpuint16) >> 2); pRangingMeasurementData->RangeFractionalPart = (uint8_t)((tmpuint16 & 0x03) << 6); } else { pRangingMeasurementData->RangeMilliMeter = tmpuint16; pRangingMeasurementData->RangeFractionalPart = 0; } /* * For a standard definition of RangeStatus, this should * return 0 in case of good result after a ranging * The range status depends on the device so call a device * specific function to obtain the right Status. */ Status |= VL53L0X_get_pal_range_status(Dev, DeviceRangeStatus, SignalRate, EffectiveSpadRtnCount, pRangingMeasurementData, &PalRangeStatus); if (Status == VL53L0X_ERROR_NONE) pRangingMeasurementData->RangeStatus = PalRangeStatus; } if (Status == VL53L0X_ERROR_NONE) { /* Copy last read data into Dev buffer */ LastRangeDataBuffer = PALDevDataGet(Dev, LastRangeMeasure); LastRangeDataBuffer.RangeMilliMeter = pRangingMeasurementData->RangeMilliMeter; LastRangeDataBuffer.RangeFractionalPart = pRangingMeasurementData->RangeFractionalPart; LastRangeDataBuffer.RangeDMaxMilliMeter = pRangingMeasurementData->RangeDMaxMilliMeter; LastRangeDataBuffer.MeasurementTimeUsec = pRangingMeasurementData->MeasurementTimeUsec; LastRangeDataBuffer.SignalRateRtnMegaCps = pRangingMeasurementData->SignalRateRtnMegaCps; LastRangeDataBuffer.AmbientRateRtnMegaCps = pRangingMeasurementData->AmbientRateRtnMegaCps; LastRangeDataBuffer.EffectiveSpadRtnCount = pRangingMeasurementData->EffectiveSpadRtnCount; LastRangeDataBuffer.RangeStatus = pRangingMeasurementData->RangeStatus; PALDevDataSet(Dev, LastRangeMeasure, LastRangeDataBuffer); } LOG_FUNCTION_END(Status); return Status; } VL53L0X_Error VL53L0X::VL53L0X_PerformSingleRangingMeasurement(VL53L0X_DEV Dev, VL53L0X_RangingMeasurementData_t *pRangingMeasurementData) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; LOG_FUNCTION_START(""); /* This function will do a complete single ranging * Here we fix the mode! */ Status = VL53L0X_SetDeviceMode(Dev, VL53L0X_DEVICEMODE_SINGLE_RANGING); if (Status == VL53L0X_ERROR_NONE) Status = VL53L0X_PerformSingleMeasurement(Dev); if (Status == VL53L0X_ERROR_NONE) Status = VL53L0X_GetRangingMeasurementData(Dev, pRangingMeasurementData); if (Status == VL53L0X_ERROR_NONE) Status = VL53L0X_ClearInterruptMask(Dev, 0); LOG_FUNCTION_END(Status); return Status; } VL53L0X_Error VL53L0X::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::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; } /* 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) { /* At this point we have enabled the maximum * number of Aperture spads. */ complete = 1; break; } (refSpadCount_int)++; 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::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::VL53L0X_WaitDeviceBooted(VL53L0X_DEV Dev) { VL53L0X_Error Status = VL53L0X_ERROR_NOT_IMPLEMENTED; LOG_FUNCTION_START(""); /* not implemented on VL53L0X */ LOG_FUNCTION_END(Status); return Status; } VL53L0X_Error VL53L0X::VL53L0X_PerformRefCalibration(VL53L0X_DEV Dev, uint8_t *pVhvSettings, uint8_t *pPhaseCal) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; LOG_FUNCTION_START(""); Status = VL53L0X_perform_ref_calibration(Dev, pVhvSettings, pPhaseCal, 1); LOG_FUNCTION_END(Status); return Status; } VL53L0X_Error VL53L0X::VL53L0X_PerformRefSpadManagement(VL53L0X_DEV Dev, uint32_t *refSpadCount, uint8_t *isApertureSpads) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; LOG_FUNCTION_START(""); Status = VL53L0X_perform_ref_spad_management(Dev, refSpadCount, isApertureSpads); LOG_FUNCTION_END(Status); return Status; } /* Group PAL Init Functions */ VL53L0X_Error VL53L0X::VL53L0X_SetDeviceAddress(VL53L0X_DEV Dev, uint8_t DeviceAddress) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; LOG_FUNCTION_START(""); Status = VL53L0X_WrByte(Dev, VL53L0X_REG_I2C_SLAVE_DEVICE_ADDRESS, DeviceAddress / 2); LOG_FUNCTION_END(Status); return Status; } VL53L0X_Error VL53L0X::VL53L0X_SetGpioConfig(VL53L0X_DEV Dev, uint8_t Pin, VL53L0X_DeviceModes DeviceMode, VL53L0X_GpioFunctionality Functionality, VL53L0X_InterruptPolarity Polarity) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; uint8_t data; LOG_FUNCTION_START(""); if (Pin != 0) { Status = VL53L0X_ERROR_GPIO_NOT_EXISTING; } else if (DeviceMode == VL53L0X_DEVICEMODE_GPIO_DRIVE) { if (Polarity == VL53L0X_INTERRUPTPOLARITY_LOW) data = 0x10; else data = 1; Status = VL53L0X_WrByte(Dev, VL53L0X_REG_GPIO_HV_MUX_ACTIVE_HIGH, data); } else if (DeviceMode == VL53L0X_DEVICEMODE_GPIO_OSC) { Status |= VL53L0X_WrByte(Dev, 0xff, 0x01); Status |= VL53L0X_WrByte(Dev, 0x00, 0x00); Status |= VL53L0X_WrByte(Dev, 0xff, 0x00); Status |= VL53L0X_WrByte(Dev, 0x80, 0x01); Status |= VL53L0X_WrByte(Dev, 0x85, 0x02); Status |= VL53L0X_WrByte(Dev, 0xff, 0x04); Status |= VL53L0X_WrByte(Dev, 0xcd, 0x00); Status |= VL53L0X_WrByte(Dev, 0xcc, 0x11); Status |= VL53L0X_WrByte(Dev, 0xff, 0x07); Status |= VL53L0X_WrByte(Dev, 0xbe, 0x00); Status |= VL53L0X_WrByte(Dev, 0xff, 0x06); Status |= VL53L0X_WrByte(Dev, 0xcc, 0x09); Status |= VL53L0X_WrByte(Dev, 0xff, 0x00); Status |= VL53L0X_WrByte(Dev, 0xff, 0x01); Status |= VL53L0X_WrByte(Dev, 0x00, 0x00); } else { if (Status == VL53L0X_ERROR_NONE) { switch (Functionality) { case VL53L0X_GPIOFUNCTIONALITY_OFF: data = 0x00; break; case VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_LOW: data = 0x01; break; case VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_HIGH: data = 0x02; break; case VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_OUT: data = 0x03; break; case VL53L0X_GPIOFUNCTIONALITY_NEW_MEASURE_READY: data = 0x04; break; default: Status = VL53L0X_ERROR_GPIO_FUNCTIONALITY_NOT_SUPPORTED; } } if (Status == VL53L0X_ERROR_NONE) Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSTEM_INTERRUPT_CONFIG_GPIO, data); if (Status == VL53L0X_ERROR_NONE) { if (Polarity == VL53L0X_INTERRUPTPOLARITY_LOW) data = 0; else data = (uint8_t)(1 << 4); Status = VL53L0X_UpdateByte(Dev, VL53L0X_REG_GPIO_HV_MUX_ACTIVE_HIGH, 0xEF, data); } if (Status == VL53L0X_ERROR_NONE) VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, Pin0GpioFunctionality, Functionality); if (Status == VL53L0X_ERROR_NONE) Status = VL53L0X_ClearInterruptMask(Dev, 0); } LOG_FUNCTION_END(Status); return Status; } VL53L0X_Error VL53L0X::VL53L0X_GetFractionEnable(VL53L0X_DEV Dev, uint8_t *pEnabled) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; LOG_FUNCTION_START(""); Status = VL53L0X_RdByte(Dev, VL53L0X_REG_SYSTEM_RANGE_CONFIG, pEnabled); if (Status == VL53L0X_ERROR_NONE) *pEnabled = (*pEnabled & 1); LOG_FUNCTION_END(Status); return Status; } uint16_t VL53L0X::VL53L0X_encode_timeout(uint32_t timeout_macro_clks) { /*! * Encode timeout in macro periods in (LSByte * 2^MSByte) + 1 format */ uint16_t encoded_timeout = 0; uint32_t ls_byte = 0; uint16_t ms_byte = 0; if (timeout_macro_clks > 0) { ls_byte = timeout_macro_clks - 1; while ((ls_byte & 0xFFFFFF00) > 0) { ls_byte = ls_byte >> 1; ms_byte++; } encoded_timeout = (ms_byte << 8) + (uint16_t) (ls_byte & 0x000000FF); } return encoded_timeout; } VL53L0X_Error VL53L0X::set_sequence_step_timeout(VL53L0X_DEV Dev, VL53L0X_SequenceStepId SequenceStepId, uint32_t TimeOutMicroSecs) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; uint8_t CurrentVCSELPulsePeriodPClk; uint8_t MsrcEncodedTimeOut; uint16_t PreRangeEncodedTimeOut; uint16_t PreRangeTimeOutMClks; uint16_t MsrcRangeTimeOutMClks; uint32_t FinalRangeTimeOutMClks; uint16_t FinalRangeEncodedTimeOut; VL53L0X_SchedulerSequenceSteps_t SchedulerSequenceSteps; if ((SequenceStepId == VL53L0X_SEQUENCESTEP_TCC) || (SequenceStepId == VL53L0X_SEQUENCESTEP_DSS) || (SequenceStepId == VL53L0X_SEQUENCESTEP_MSRC)) { Status = VL53L0X_GetVcselPulsePeriod(Dev, VL53L0X_VCSEL_PERIOD_PRE_RANGE, &CurrentVCSELPulsePeriodPClk); if (Status == VL53L0X_ERROR_NONE) { MsrcRangeTimeOutMClks = VL53L0X_calc_timeout_mclks(Dev, TimeOutMicroSecs, (uint8_t)CurrentVCSELPulsePeriodPClk); if (MsrcRangeTimeOutMClks > 256) MsrcEncodedTimeOut = 255; else MsrcEncodedTimeOut = (uint8_t)MsrcRangeTimeOutMClks - 1; VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, LastEncodedTimeout, MsrcEncodedTimeOut); } if (Status == VL53L0X_ERROR_NONE) { Status = VL53L0X_WrByte(Dev, VL53L0X_REG_MSRC_CONFIG_TIMEOUT_MACROP, MsrcEncodedTimeOut); } } else { if (SequenceStepId == VL53L0X_SEQUENCESTEP_PRE_RANGE) { if (Status == VL53L0X_ERROR_NONE) { Status = VL53L0X_GetVcselPulsePeriod(Dev, VL53L0X_VCSEL_PERIOD_PRE_RANGE, &CurrentVCSELPulsePeriodPClk); PreRangeTimeOutMClks = VL53L0X_calc_timeout_mclks(Dev, TimeOutMicroSecs, (uint8_t)CurrentVCSELPulsePeriodPClk); PreRangeEncodedTimeOut = VL53L0X_encode_timeout( PreRangeTimeOutMClks); VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, LastEncodedTimeout, PreRangeEncodedTimeOut); } if (Status == VL53L0X_ERROR_NONE) { Status = VL53L0X_WrWord(Dev, VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI, PreRangeEncodedTimeOut); } if (Status == VL53L0X_ERROR_NONE) { VL53L0X_SETDEVICESPECIFICPARAMETER( Dev, PreRangeTimeoutMicroSecs, TimeOutMicroSecs); } } else if (SequenceStepId == VL53L0X_SEQUENCESTEP_FINAL_RANGE) { /* For the final range timeout, the pre-range timeout * must be added. To do this both final and pre-range * timeouts must be expressed in macro periods MClks * because they have different vcsel periods. */ VL53L0X_GetSequenceStepEnables(Dev, &SchedulerSequenceSteps); PreRangeTimeOutMClks = 0; if (SchedulerSequenceSteps.PreRangeOn) { /* Retrieve PRE-RANGE VCSEL Period */ Status = VL53L0X_GetVcselPulsePeriod(Dev, VL53L0X_VCSEL_PERIOD_PRE_RANGE, &CurrentVCSELPulsePeriodPClk); /* Retrieve PRE-RANGE Timeout in Macro periods * (MCLKS) */ if (Status == VL53L0X_ERROR_NONE) { Status = VL53L0X_RdWord(Dev, 0x51, &PreRangeEncodedTimeOut); PreRangeTimeOutMClks = VL53L0X_decode_timeout( PreRangeEncodedTimeOut); } } /* Calculate FINAL RANGE Timeout in Macro Periods * (MCLKS) and add PRE-RANGE value */ if (Status == VL53L0X_ERROR_NONE) { Status = VL53L0X_GetVcselPulsePeriod(Dev, VL53L0X_VCSEL_PERIOD_FINAL_RANGE, &CurrentVCSELPulsePeriodPClk); } if (Status == VL53L0X_ERROR_NONE) { FinalRangeTimeOutMClks = VL53L0X_calc_timeout_mclks(Dev, TimeOutMicroSecs, (uint8_t) CurrentVCSELPulsePeriodPClk); FinalRangeTimeOutMClks += PreRangeTimeOutMClks; FinalRangeEncodedTimeOut = VL53L0X_encode_timeout(FinalRangeTimeOutMClks); if (Status == VL53L0X_ERROR_NONE) { Status = VL53L0X_WrWord(Dev, 0x71, FinalRangeEncodedTimeOut); } if (Status == VL53L0X_ERROR_NONE) { VL53L0X_SETDEVICESPECIFICPARAMETER( Dev, FinalRangeTimeoutMicroSecs, TimeOutMicroSecs); } } } else Status = VL53L0X_ERROR_INVALID_PARAMS; } return Status; } VL53L0X_Error VL53L0X::VL53L0X_set_measurement_timing_budget_micro_seconds(VL53L0X_DEV Dev, uint32_t MeasurementTimingBudgetMicroSeconds) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; uint32_t FinalRangeTimingBudgetMicroSeconds; VL53L0X_SchedulerSequenceSteps_t SchedulerSequenceSteps; uint32_t MsrcDccTccTimeoutMicroSeconds = 2000; uint32_t StartOverheadMicroSeconds = 1910; uint32_t EndOverheadMicroSeconds = 960; uint32_t MsrcOverheadMicroSeconds = 660; uint32_t TccOverheadMicroSeconds = 590; uint32_t DssOverheadMicroSeconds = 690; uint32_t PreRangeOverheadMicroSeconds = 660; uint32_t FinalRangeOverheadMicroSeconds = 550; uint32_t PreRangeTimeoutMicroSeconds = 0; uint32_t cMinTimingBudgetMicroSeconds = 20000; uint32_t SubTimeout = 0; LOG_FUNCTION_START(""); if (MeasurementTimingBudgetMicroSeconds < cMinTimingBudgetMicroSeconds) { Status = VL53L0X_ERROR_INVALID_PARAMS; return Status; } FinalRangeTimingBudgetMicroSeconds = MeasurementTimingBudgetMicroSeconds - (StartOverheadMicroSeconds + EndOverheadMicroSeconds); Status = VL53L0X_GetSequenceStepEnables(Dev, &SchedulerSequenceSteps); if (Status == VL53L0X_ERROR_NONE && (SchedulerSequenceSteps.TccOn || SchedulerSequenceSteps.MsrcOn || SchedulerSequenceSteps.DssOn)) { /* TCC, MSRC and DSS all share the same timeout */ Status = get_sequence_step_timeout(Dev, VL53L0X_SEQUENCESTEP_MSRC, &MsrcDccTccTimeoutMicroSeconds); /* Subtract the TCC, MSRC and DSS timeouts if they are * enabled. */ if (Status != VL53L0X_ERROR_NONE) return Status; /* TCC */ if (SchedulerSequenceSteps.TccOn) { SubTimeout = MsrcDccTccTimeoutMicroSeconds + TccOverheadMicroSeconds; if (SubTimeout < FinalRangeTimingBudgetMicroSeconds) { FinalRangeTimingBudgetMicroSeconds -= SubTimeout; } else { /* Requested timeout too big. */ Status = VL53L0X_ERROR_INVALID_PARAMS; } } if (Status != VL53L0X_ERROR_NONE) { LOG_FUNCTION_END(Status); return Status; } /* DSS */ if (SchedulerSequenceSteps.DssOn) { SubTimeout = 2 * (MsrcDccTccTimeoutMicroSeconds + DssOverheadMicroSeconds); if (SubTimeout < FinalRangeTimingBudgetMicroSeconds) { FinalRangeTimingBudgetMicroSeconds -= SubTimeout; } else { /* Requested timeout too big. */ Status = VL53L0X_ERROR_INVALID_PARAMS; } } else if (SchedulerSequenceSteps.MsrcOn) { /* MSRC */ SubTimeout = MsrcDccTccTimeoutMicroSeconds + MsrcOverheadMicroSeconds; if (SubTimeout < FinalRangeTimingBudgetMicroSeconds) { FinalRangeTimingBudgetMicroSeconds -= SubTimeout; } else { /* Requested timeout too big. */ Status = VL53L0X_ERROR_INVALID_PARAMS; } } } if (Status != VL53L0X_ERROR_NONE) { LOG_FUNCTION_END(Status); return Status; } if (SchedulerSequenceSteps.PreRangeOn) { /* Subtract the Pre-range timeout if enabled. */ Status = get_sequence_step_timeout(Dev, VL53L0X_SEQUENCESTEP_PRE_RANGE, &PreRangeTimeoutMicroSeconds); SubTimeout = PreRangeTimeoutMicroSeconds + PreRangeOverheadMicroSeconds; if (SubTimeout < FinalRangeTimingBudgetMicroSeconds) { FinalRangeTimingBudgetMicroSeconds -= SubTimeout; } else { /* Requested timeout too big. */ Status = VL53L0X_ERROR_INVALID_PARAMS; } } if (Status == VL53L0X_ERROR_NONE && SchedulerSequenceSteps.FinalRangeOn) { FinalRangeTimingBudgetMicroSeconds -= FinalRangeOverheadMicroSeconds; /* Final Range Timeout * Note that the final range timeout is determined by the timing * budget and the sum of all other timeouts within the sequence. * If there is no room for the final range timeout, then an error * will be set. Otherwise the remaining time will be applied to * the final range. */ Status = set_sequence_step_timeout(Dev, VL53L0X_SEQUENCESTEP_FINAL_RANGE, FinalRangeTimingBudgetMicroSeconds); VL53L0X_SETPARAMETERFIELD(Dev, MeasurementTimingBudgetMicroSeconds, MeasurementTimingBudgetMicroSeconds); } LOG_FUNCTION_END(Status); return Status; } VL53L0X_Error VL53L0X::VL53L0X_SetMeasurementTimingBudgetMicroSeconds(VL53L0X_DEV Dev, uint32_t MeasurementTimingBudgetMicroSeconds) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; LOG_FUNCTION_START(""); Status = VL53L0X_set_measurement_timing_budget_micro_seconds(Dev, MeasurementTimingBudgetMicroSeconds); LOG_FUNCTION_END(Status); return Status; } VL53L0X_Error VL53L0X::VL53L0X_SetSequenceStepEnable(VL53L0X_DEV Dev, VL53L0X_SequenceStepId SequenceStepId, uint8_t SequenceStepEnabled) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; uint8_t SequenceConfig = 0; uint8_t SequenceConfigNew = 0; uint32_t MeasurementTimingBudgetMicroSeconds; LOG_FUNCTION_START(""); Status = VL53L0X_RdByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, &SequenceConfig); SequenceConfigNew = SequenceConfig; if (Status == VL53L0X_ERROR_NONE) { if (SequenceStepEnabled == 1) { /* Enable requested sequence step */ switch (SequenceStepId) { case VL53L0X_SEQUENCESTEP_TCC: SequenceConfigNew |= 0x10; break; case VL53L0X_SEQUENCESTEP_DSS: SequenceConfigNew |= 0x28; break; case VL53L0X_SEQUENCESTEP_MSRC: SequenceConfigNew |= 0x04; break; case VL53L0X_SEQUENCESTEP_PRE_RANGE: SequenceConfigNew |= 0x40; break; case VL53L0X_SEQUENCESTEP_FINAL_RANGE: SequenceConfigNew |= 0x80; break; default: Status = VL53L0X_ERROR_INVALID_PARAMS; } } else { /* Disable requested sequence step */ switch (SequenceStepId) { case VL53L0X_SEQUENCESTEP_TCC: SequenceConfigNew &= 0xef; break; case VL53L0X_SEQUENCESTEP_DSS: SequenceConfigNew &= 0xd7; break; case VL53L0X_SEQUENCESTEP_MSRC: SequenceConfigNew &= 0xfb; break; case VL53L0X_SEQUENCESTEP_PRE_RANGE: SequenceConfigNew &= 0xbf; break; case VL53L0X_SEQUENCESTEP_FINAL_RANGE: SequenceConfigNew &= 0x7f; break; default: Status = VL53L0X_ERROR_INVALID_PARAMS; } } } if (SequenceConfigNew != SequenceConfig) { /* Apply New Setting */ if (Status == VL53L0X_ERROR_NONE) { Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, SequenceConfigNew); } if (Status == VL53L0X_ERROR_NONE) PALDevDataSet(Dev, SequenceConfig, SequenceConfigNew); /* Recalculate timing budget */ if (Status == VL53L0X_ERROR_NONE) { VL53L0X_GETPARAMETERFIELD(Dev, MeasurementTimingBudgetMicroSeconds, MeasurementTimingBudgetMicroSeconds); VL53L0X_SetMeasurementTimingBudgetMicroSeconds(Dev, MeasurementTimingBudgetMicroSeconds); } } LOG_FUNCTION_END(Status); return Status; } VL53L0X_Error VL53L0X::VL53L0X_SetLimitCheckEnable(VL53L0X_DEV Dev, uint16_t LimitCheckId, uint8_t LimitCheckEnable) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; FixPoint1616_t TempFix1616 = 0; uint8_t LimitCheckEnableInt = 0; uint8_t LimitCheckDisable = 0; uint8_t Temp8; LOG_FUNCTION_START(""); if (LimitCheckId >= VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS) { Status = VL53L0X_ERROR_INVALID_PARAMS; } else { if (LimitCheckEnable == 0) { TempFix1616 = 0; LimitCheckEnableInt = 0; LimitCheckDisable = 1; } else { VL53L0X_GETARRAYPARAMETERFIELD(Dev, LimitChecksValue, LimitCheckId, TempFix1616); LimitCheckDisable = 0; /* this to be sure to have either 0 or 1 */ LimitCheckEnableInt = 1; } switch (LimitCheckId) { case VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE: /* internal computation: */ VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksEnable, VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, LimitCheckEnableInt); break; case VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE: Status = VL53L0X_WrWord(Dev, VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT, VL53L0X_FIXPOINT1616TOFIXPOINT97(TempFix1616)); break; case VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP: /* internal computation: */ VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksEnable, VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, LimitCheckEnableInt); break; case VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD: /* internal computation: */ VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksEnable, VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, LimitCheckEnableInt); break; case VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC: Temp8 = (uint8_t)(LimitCheckDisable << 1); Status = VL53L0X_UpdateByte(Dev, VL53L0X_REG_MSRC_CONFIG_CONTROL, 0xFE, Temp8); break; case VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE: Temp8 = (uint8_t)(LimitCheckDisable << 4); Status = VL53L0X_UpdateByte(Dev, VL53L0X_REG_MSRC_CONFIG_CONTROL, 0xEF, Temp8); break; default: Status = VL53L0X_ERROR_INVALID_PARAMS; } } if (Status == VL53L0X_ERROR_NONE) { if (LimitCheckEnable == 0) { VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksEnable, LimitCheckId, 0); } else { VL53L0X_SETARRAYPARAMETERFIELD(Dev, LimitChecksEnable, LimitCheckId, 1); } } LOG_FUNCTION_END(Status); return Status; } VL53L0X_Error VL53L0X::VL53L0X_StaticInit(VL53L0X_DEV Dev) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; VL53L0X_DeviceParameters_t CurrentParameters = {0}; uint8_t *pTuningSettingBuffer; uint16_t tempword = 0; uint8_t tempbyte = 0; uint8_t UseInternalTuningSettings = 0; uint32_t count = 0; uint8_t isApertureSpads = 0; uint32_t refSpadCount = 0; uint8_t ApertureSpads = 0; uint8_t vcselPulsePeriodPCLK; uint32_t seqTimeoutMicroSecs; LOG_FUNCTION_START(""); Status = VL53L0X_get_info_from_device(Dev, 1); /* set the ref spad from NVM */ count = (uint32_t)VL53L0X_GETDEVICESPECIFICPARAMETER(Dev, ReferenceSpadCount); ApertureSpads = VL53L0X_GETDEVICESPECIFICPARAMETER(Dev, ReferenceSpadType); /* NVM value invalid */ if ((ApertureSpads > 1) || ((ApertureSpads == 1) && (count > 32)) || ((ApertureSpads == 0) && (count > 12))) Status = VL53L0X_perform_ref_spad_management(Dev, &refSpadCount, &isApertureSpads); else Status = VL53L0X_set_reference_spads(Dev, count, ApertureSpads); /* Initialize tuning settings buffer to prevent compiler warning. */ pTuningSettingBuffer = DefaultTuningSettings; if (Status == VL53L0X_ERROR_NONE) { UseInternalTuningSettings = PALDevDataGet(Dev, UseInternalTuningSettings); if (UseInternalTuningSettings == 0) pTuningSettingBuffer = PALDevDataGet(Dev, pTuningSettingsPointer); else pTuningSettingBuffer = DefaultTuningSettings; } if (Status == VL53L0X_ERROR_NONE) Status = VL53L0X_load_tuning_settings(Dev, pTuningSettingBuffer); /* Set interrupt config to new sample ready */ if (Status == VL53L0X_ERROR_NONE) { Status = VL53L0X_SetGpioConfig(Dev, 0, 0, VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY, VL53L0X_INTERRUPTPOLARITY_LOW); } if (Status == VL53L0X_ERROR_NONE) { Status = VL53L0X_WrByte(Dev, 0xFF, 0x01); Status |= VL53L0X_RdWord(Dev, 0x84, &tempword); Status |= VL53L0X_WrByte(Dev, 0xFF, 0x00); } if (Status == VL53L0X_ERROR_NONE) { VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, OscFrequencyMHz, VL53L0X_FIXPOINT412TOFIXPOINT1616(tempword)); } /* After static init, some device parameters may be changed, * so update them */ if (Status == VL53L0X_ERROR_NONE) Status = VL53L0X_GetDeviceParameters(Dev, &CurrentParameters); if (Status == VL53L0X_ERROR_NONE) { Status = VL53L0X_GetFractionEnable(Dev, &tempbyte); if (Status == VL53L0X_ERROR_NONE) PALDevDataSet(Dev, RangeFractionalEnable, tempbyte); } if (Status == VL53L0X_ERROR_NONE) PALDevDataSet(Dev, CurrentParameters, CurrentParameters); /* read the sequence config and save it */ if (Status == VL53L0X_ERROR_NONE) { Status = VL53L0X_RdByte(Dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, &tempbyte); if (Status == VL53L0X_ERROR_NONE) PALDevDataSet(Dev, SequenceConfig, tempbyte); } /* Disable MSRC and TCC by default */ if (Status == VL53L0X_ERROR_NONE) Status = VL53L0X_SetSequenceStepEnable(Dev, VL53L0X_SEQUENCESTEP_TCC, 0); if (Status == VL53L0X_ERROR_NONE) Status = VL53L0X_SetSequenceStepEnable(Dev, VL53L0X_SEQUENCESTEP_MSRC, 0); /* Set PAL State to standby */ if (Status == VL53L0X_ERROR_NONE) PALDevDataSet(Dev, PalState, VL53L0X_STATE_IDLE); /* Store pre-range vcsel period */ if (Status == VL53L0X_ERROR_NONE) { Status = VL53L0X_GetVcselPulsePeriod( Dev, VL53L0X_VCSEL_PERIOD_PRE_RANGE, &vcselPulsePeriodPCLK); } if (Status == VL53L0X_ERROR_NONE) { VL53L0X_SETDEVICESPECIFICPARAMETER( Dev, PreRangeVcselPulsePeriod, vcselPulsePeriodPCLK); } /* Store final-range vcsel period */ if (Status == VL53L0X_ERROR_NONE) { Status = VL53L0X_GetVcselPulsePeriod( Dev, VL53L0X_VCSEL_PERIOD_FINAL_RANGE, &vcselPulsePeriodPCLK); } if (Status == VL53L0X_ERROR_NONE) { VL53L0X_SETDEVICESPECIFICPARAMETER( Dev, FinalRangeVcselPulsePeriod, vcselPulsePeriodPCLK); } /* Store pre-range timeout */ if (Status == VL53L0X_ERROR_NONE) { Status = get_sequence_step_timeout( Dev, VL53L0X_SEQUENCESTEP_PRE_RANGE, &seqTimeoutMicroSecs); } if (Status == VL53L0X_ERROR_NONE) { VL53L0X_SETDEVICESPECIFICPARAMETER( Dev, PreRangeTimeoutMicroSecs, seqTimeoutMicroSecs); } /* Store final-range timeout */ if (Status == VL53L0X_ERROR_NONE) { Status = get_sequence_step_timeout( Dev, VL53L0X_SEQUENCESTEP_FINAL_RANGE, &seqTimeoutMicroSecs); } if (Status == VL53L0X_ERROR_NONE) { VL53L0X_SETDEVICESPECIFICPARAMETER( Dev, FinalRangeTimeoutMicroSecs, seqTimeoutMicroSecs); } LOG_FUNCTION_END(Status); return Status; } VL53L0X_Error VL53L0X::VL53L0X_StopMeasurement(VL53L0X_DEV Dev) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; LOG_FUNCTION_START(""); Status = VL53L0X_WrByte(Dev, VL53L0X_REG_SYSRANGE_START, VL53L0X_REG_SYSRANGE_MODE_SINGLESHOT); Status = VL53L0X_WrByte(Dev, 0xFF, 0x01); Status = VL53L0X_WrByte(Dev, 0x00, 0x00); Status = VL53L0X_WrByte(Dev, 0x91, 0x00); Status = VL53L0X_WrByte(Dev, 0x00, 0x01); Status = VL53L0X_WrByte(Dev, 0xFF, 0x00); if (Status == VL53L0X_ERROR_NONE) { /* Set PAL State to Idle */ PALDevDataSet(Dev, PalState, VL53L0X_STATE_IDLE); } /* Check if need to apply interrupt settings */ if (Status == VL53L0X_ERROR_NONE) Status = VL53L0X_CheckAndLoadInterruptSettings(Dev, 0); LOG_FUNCTION_END(Status); return Status; } VL53L0X_Error VL53L0X::VL53L0X_GetStopCompletedStatus(VL53L0X_DEV Dev, uint32_t *pStopStatus) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; uint8_t Byte = 0; LOG_FUNCTION_START(""); Status = VL53L0X_WrByte(Dev, 0xFF, 0x01); if (Status == VL53L0X_ERROR_NONE) Status = VL53L0X_RdByte(Dev, 0x04, &Byte); if (Status == VL53L0X_ERROR_NONE) Status = VL53L0X_WrByte(Dev, 0xFF, 0x0); *pStopStatus = Byte; if (Byte == 0) { Status = VL53L0X_WrByte(Dev, 0x80, 0x01); Status = VL53L0X_WrByte(Dev, 0xFF, 0x01); Status = VL53L0X_WrByte(Dev, 0x00, 0x00); Status = VL53L0X_WrByte(Dev, 0x91, PALDevDataGet(Dev, StopVariable)); Status = VL53L0X_WrByte(Dev, 0x00, 0x01); Status = VL53L0X_WrByte(Dev, 0xFF, 0x00); Status = VL53L0X_WrByte(Dev, 0x80, 0x00); } LOG_FUNCTION_END(Status); return Status; } /****************** Write and read functions from I2C *************************/ VL53L0X_Error VL53L0X::VL53L0X_WriteMulti(VL53L0X_DEV Dev, uint8_t index, uint8_t *pdata, uint32_t count) { int status; status = VL53L0X_I2CWrite(Dev->I2cDevAddr, index, pdata, (uint16_t)count); return status; } VL53L0X_Error VL53L0X::VL53L0X_ReadMulti(VL53L0X_DEV Dev, uint8_t index, uint8_t *pdata, uint32_t count) { int status; if (count>=VL53L0X_MAX_I2C_XFER_SIZE){ status = VL53L0X_ERROR_INVALID_PARAMS; } status = VL53L0X_I2CRead(Dev->I2cDevAddr, index, pdata, (uint16_t)count); return status; } VL53L0X_Error VL53L0X::VL53L0X_WrByte(VL53L0X_DEV Dev, uint8_t index, uint8_t data) { int status; status=VL53L0X_I2CWrite(Dev->I2cDevAddr, index, &data, 1); return status; } VL53L0X_Error VL53L0X::VL53L0X_WrWord(VL53L0X_DEV Dev, uint8_t index, uint16_t data) { int status; uint8_t buffer[2]; buffer[0] = data >> 8; buffer[1] = data & 0x00FF; status=VL53L0X_I2CWrite(Dev->I2cDevAddr, index, (uint8_t *)buffer, 2); return status; } VL53L0X_Error VL53L0X::VL53L0X_WrDWord(VL53L0X_DEV Dev, uint8_t index, uint32_t data) { int status; uint8_t buffer[4]; buffer[0] = (data >> 24) & 0xFF; buffer[1] = (data >> 16) & 0xFF; buffer[2] = (data >> 8) & 0xFF; buffer[3] = (data >> 0) & 0xFF; status=VL53L0X_I2CWrite(Dev->I2cDevAddr, index, (uint8_t *)buffer, 4); return status; } VL53L0X_Error VL53L0X::VL53L0X_RdByte(VL53L0X_DEV Dev, uint8_t index, uint8_t *data) { int status; status = VL53L0X_I2CRead(Dev->I2cDevAddr, index, data, 1); if(status) return -1; return 0; } VL53L0X_Error VL53L0X::VL53L0X_RdWord(VL53L0X_DEV Dev, uint8_t index, uint16_t *data) { int status; uint8_t buffer[2] = {0,0}; status = VL53L0X_I2CRead(Dev->I2cDevAddr, index, buffer, 2); if (!status) { *data = (buffer[0] << 8) + buffer[1]; } return status; } VL53L0X_Error VL53L0X::VL53L0X_RdDWord(VL53L0X_DEV Dev, uint8_t index, uint32_t *data) { int status; uint8_t buffer[4] = {0,0,0,0}; status = VL53L0X_I2CRead(Dev->I2cDevAddr, index, buffer, 4); if(!status) { *data = (buffer[0] << 24) + (buffer[1] << 16) + (buffer[2] << 8) + buffer[3]; } return status; } VL53L0X_Error VL53L0X::VL53L0X_UpdateByte(VL53L0X_DEV Dev, uint8_t index, uint8_t AndData, uint8_t OrData) { int status; uint8_t buffer = 0; /* read data direct onto buffer */ status = VL53L0X_I2CRead(Dev->I2cDevAddr, index, &buffer,1); if (!status) { buffer = (buffer & AndData) | OrData; status = VL53L0X_I2CWrite(Dev->I2cDevAddr, index, &buffer, (uint8_t)1); } return status; } VL53L0X_Error VL53L0X::VL53L0X_I2CWrite(uint8_t DeviceAddr, uint8_t RegisterAddr, uint8_t* pBuffer, uint16_t NumByteToWrite) { int ret; ret = dev_i2c.i2c_write(pBuffer, DeviceAddr, RegisterAddr, NumByteToWrite); if(ret) return -1; return 0; } VL53L0X_Error VL53L0X::VL53L0X_I2CRead(uint8_t DeviceAddr, uint8_t RegisterAddr, uint8_t* pBuffer, uint16_t NumByteToRead) { int ret; ret = dev_i2c.i2c_read(pBuffer, DeviceAddr, RegisterAddr, NumByteToRead); if(ret) return -1; return 0; } int VL53L0X::ReadID(uint8_t *id) { int status = 0; uint16_t rl_id=0; status = VL53L0X_RdWord(Device, VL53L0X_REG_IDENTIFICATION_MODEL_ID, &rl_id); if (rl_id == 0xEEAA) return status; return -1; } VL53L0X_Error VL53L0X::WaitMeasurementDataReady(VL53L0X_DEV Dev) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; uint8_t NewDatReady=0; uint32_t LoopNb; // Wait until it finished // use timeout to avoid deadlock if (Status == VL53L0X_ERROR_NONE) { LoopNb = 0; do { Status = VL53L0X_GetMeasurementDataReady(Dev, &NewDatReady); if ((NewDatReady == 0x01) || Status != VL53L0X_ERROR_NONE) { break; } LoopNb = LoopNb + 1; VL53L0X_PollingDelay(Dev); } while (LoopNb < VL53L0X_DEFAULT_MAX_LOOP); if (LoopNb >= VL53L0X_DEFAULT_MAX_LOOP) { Status = VL53L0X_ERROR_TIME_OUT; } } return Status; } VL53L0X_Error VL53L0X::WaitStopCompleted(VL53L0X_DEV Dev) { VL53L0X_Error Status = VL53L0X_ERROR_NONE; uint32_t StopCompleted=0; uint32_t LoopNb; // Wait until it finished // use timeout to avoid deadlock if (Status == VL53L0X_ERROR_NONE) { LoopNb = 0; do { Status = VL53L0X_GetStopCompletedStatus(Dev, &StopCompleted); if ((StopCompleted == 0x00) || Status != VL53L0X_ERROR_NONE) { break; } LoopNb = LoopNb + 1; VL53L0X_PollingDelay(Dev); } while (LoopNb < VL53L0X_DEFAULT_MAX_LOOP); if (LoopNb >= VL53L0X_DEFAULT_MAX_LOOP) { Status = VL53L0X_ERROR_TIME_OUT; } } return Status; } int VL53L0X::InitSensor(uint8_t NewAddr) { int status; VL53L0X_Off(); VL53L0X_On(); // status=VL53L0X_WaitDeviceBooted(Device); // if(status) // printf("WaitDeviceBooted fail\n\r"); status=IsPresent(); if(!status) { VL53L0X_DataInit(Device); if(status != VL53L0X_ERROR_NONE) { printf("Failed to init VL53L0X sensor!\n\r"); return status; } // deduce silicon version status = VL53L0X_GetDeviceInfo(&MyDevice, &DeviceInfo); status=Prepare(); if(status != VL53L0X_ERROR_NONE) { printf("Failed to prepare VL53L0X!\n\r"); return status; } /* if(NewAddr!=DEFAULT_DEVICE_ADDRESS) { status=SetDeviceAddress(NewAddr); if(status) { printf("Failed to change I2C address!\n\r"); return status; } } else { printf("Invalid new address!\n\r"); return VL53L0X_ERROR_INVALID_PARAMS; } */ // Device->Ready=1; } return status; } int VL53L0X::StartMeasurement(OperatingMode operating_mode, void (*fptr)(void)) { int Status = VL53L0X_ERROR_NONE; uint8_t VhvSettings; uint8_t PhaseCal; // *** from mass market cube expansion v1.1, ranging with satellites. // default settings, for normal range. FixPoint1616_t signalLimit = (FixPoint1616_t)(0.25*65536); FixPoint1616_t sigmaLimit = (FixPoint1616_t)(18*65536); uint32_t timingBudget = 33000; uint8_t preRangeVcselPeriod = 14; uint8_t finalRangeVcselPeriod = 10; if (operating_mode == range_single_shot_polling) { // singelshot, polled ranging if(Status == VL53L0X_ERROR_NONE) { // no need to do this when we use VL53L0X_PerformSingleRangingMeasurement Status = VL53L0X_SetDeviceMode(Device, VL53L0X_DEVICEMODE_SINGLE_RANGING); // Setup in single ranging mode } // Enable/Disable Sigma and Signal check if (Status == VL53L0X_ERROR_NONE) { Status = VL53L0X_SetLimitCheckEnable(Device, VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, 1); } if (Status == VL53L0X_ERROR_NONE) { Status = VL53L0X_SetLimitCheckEnable(Device, VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, 1); } // *** from mass market cube expansion v1.1, ranging with satellites. /* Ranging configuration */ //* // switch(rangingConfig) { // case LONG_RANGE: signalLimit = (FixPoint1616_t)(0.1*65536); sigmaLimit = (FixPoint1616_t)(60*65536); timingBudget = 33000; preRangeVcselPeriod = 18; finalRangeVcselPeriod = 14; /* break; case HIGH_ACCURACY: signalLimit = (FixPoint1616_t)(0.25*65536); sigmaLimit = (FixPoint1616_t)(18*65536); timingBudget = 200000; preRangeVcselPeriod = 14; finalRangeVcselPeriod = 10; break; case HIGH_SPEED: signalLimit = (FixPoint1616_t)(0.25*65536); sigmaLimit = (FixPoint1616_t)(32*65536); timingBudget = 20000; preRangeVcselPeriod = 14; finalRangeVcselPeriod = 10; break; default: debug_printf("Not Supported"); } */ if (Status == VL53L0X_ERROR_NONE) { Status = VL53L0X_SetLimitCheckValue(Device, VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, signalLimit); } if (Status == VL53L0X_ERROR_NONE) { Status = VL53L0X_SetLimitCheckValue(Device, VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, sigmaLimit); } if (Status == VL53L0X_ERROR_NONE) { Status = VL53L0X_SetMeasurementTimingBudgetMicroSeconds(Device, timingBudget); } if (Status == VL53L0X_ERROR_NONE) { Status = VL53L0X_SetVcselPulsePeriod(Device, VL53L0X_VCSEL_PERIOD_PRE_RANGE, preRangeVcselPeriod); } if (Status == VL53L0X_ERROR_NONE) { Status = VL53L0X_SetVcselPulsePeriod(Device, VL53L0X_VCSEL_PERIOD_FINAL_RANGE, finalRangeVcselPeriod); } if (Status == VL53L0X_ERROR_NONE) { Status = VL53L0X_PerformRefCalibration(Device, &VhvSettings, &PhaseCal); } } if (operating_mode == range_continuous_polling) { if(Status == VL53L0X_ERROR_NONE) { printf ("Call of VL53L0X_SetDeviceMode\n"); Status = VL53L0X_SetDeviceMode(Device, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING); // Setup in continuous ranging mode } if(Status == VL53L0X_ERROR_NONE) { printf ("Call of VL53L0X_StartMeasurement\n"); Status = VL53L0X_StartMeasurement(Device); } } return Status; } int VL53L0X::GetMeasurement(OperatingMode operating_mode, VL53L0X_RangingMeasurementData_t *Data) { int Status = VL53L0X_ERROR_NONE; if (operating_mode == range_single_shot_polling) { Status = VL53L0X_PerformSingleRangingMeasurement(Device, Data); } if (operating_mode == range_continuous_polling) { if (Status == VL53L0X_ERROR_NONE) Status = VL53L0X_measurement_poll_for_completion(Device); if(Status == VL53L0X_ERROR_NONE) { Status = VL53L0X_GetRangingMeasurementData(Device, Data); // Clear the interrupt VL53L0X_ClearInterruptMask(Device, VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY); VL53L0X_PollingDelay(Device); } } return Status; } int VL53L0X::StopMeasurement(OperatingMode operating_mode) { int status = VL53L0X_ERROR_NONE; // don't need to stop for a singleshot range! if (operating_mode==range_single_shot_polling) { } if (operating_mode==range_continuous_interrupt || operating_mode==range_continuous_polling) { // continuous mode if(status == VL53L0X_ERROR_NONE) { printf ("Call of VL53L0X_StopMeasurement\n"); status = VL53L0X_StopMeasurement(Device); } if(status == VL53L0X_ERROR_NONE) { printf ("Wait Stop to be competed\n"); status = WaitStopCompleted(Device); } if(status == VL53L0X_ERROR_NONE) status = VL53L0X_ClearInterruptMask(Device, VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY); } return status; } int VL53L0X::HandleIRQ(OperatingMode operating_mode, VL53L0X_RangingMeasurementData_t *Data) { int status; EnableInterruptMeasureDetectionIRQ(); status=GetMeasurement(operating_mode, Data); return status; } /******************************************************************************/