VL53L1X sensor class, for ARM Mbed platform. Based on Ultra-lite, Mass-market C Driver.
Dependencies: X_NUCLEO_COMMON ST_INTERFACES
vl53l1x_class.cpp
- Committer:
- JerrySzczurak
- Date:
- 2019-01-28
- Revision:
- 5:f16727052990
- Child:
- 6:aa13392d16bb
File content as of revision 5:f16727052990:
/** ****************************************************************************** * @file vl53l1x_class.cpp * @author JS * @version V0.0.1 * @date 15-January-2019 * @brief Implementation file for the VL53L1 sensor component driver class ****************************************************************************** * @attention * * <h2><center>© COPYRIGHT(c) 2018 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 "vl53l1x_class.h" #define ALGO__PART_TO_PART_RANGE_OFFSET_MM 0x001E #define MM_CONFIG__INNER_OFFSET_MM 0x0020 #define MM_CONFIG__OUTER_OFFSET_MM 0x0022 #include "vl53l1x_configuration.h" VL53L1X_ERROR VL53L1X::VL53L1X_GetSWVersion(VL53L1X_Version_t *pVersion) { VL53L1X_ERROR Status = 0; pVersion->major = VL53L1X_IMPLEMENTATION_VER_MAJOR; pVersion->minor = VL53L1X_IMPLEMENTATION_VER_MINOR; pVersion->build = VL53L1X_IMPLEMENTATION_VER_SUB; pVersion->revision = VL53L1X_IMPLEMENTATION_VER_REVISION; return Status; } VL53L1X_ERROR VL53L1X::VL53L1X_SetI2CAddress(uint8_t new_address) { VL53L1X_ERROR status = 0; Device->I2cDevAddr = new_address; status = VL53L1_WrByte(Device, VL53L1_I2C_SLAVE__DEVICE_ADDRESS, new_address >> 1); return status; } int VL53L1X::init_sensor(uint8_t new_addr) { Device->I2cDevAddr = new_addr; int status = 0; VL53L1_Off(); VL53L1_On(); status = is_present(); if (!status) { printf("Failed to init VL53L0X sensor!\n\r"); return status; } return status; } VL53L1X_ERROR VL53L1X::VL53L1X_SensorInit() { VL53L1X_ERROR status = 0; uint8_t Addr = 0x00; for (Addr = 0x2D; Addr <= 0x87; Addr++){ status = VL53L1_WrByte(Device, Addr, VL51L1X_DEFAULT_CONFIGURATION[Addr - 0x2D]); if (status != 0) { printf("Writing config failed - %d\r\n", status); } } uint16_t sensorID= 0; status = VL53L1X_GetSensorId(&sensorID); printf("Sensor id is - %d (%X)\r\n", sensorID, sensorID); status = VL53L1X_StartRanging(); if (status != 0) { printf("start ranging failed - %d\r\n", status); } status = VL53L1X_ClearInterrupt(); status = VL53L1X_StopRanging(); status = VL53L1_WrByte(Device, VL53L1_VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND, 0x09); /* two bounds VHV */ status = VL53L1_WrByte(Device, 0x0B, 0); /* start VHV from the previous temperature */ return status; } VL53L1X_ERROR VL53L1X::VL53L1X_ClearInterrupt() { VL53L1X_ERROR status = 0; status = VL53L1_WrByte(Device, SYSTEM__INTERRUPT_CLEAR, 0x01); return status; } VL53L1X_ERROR VL53L1X::VL53L1X_SetInterruptPolarity(uint8_t NewPolarity) { uint8_t Temp; VL53L1X_ERROR status = 0; status = VL53L1_RdByte(Device, GPIO_HV_MUX__CTRL, &Temp); Temp = Temp & 0xEF; status = VL53L1_WrByte(Device, GPIO_HV_MUX__CTRL, Temp | (!(NewPolarity & 1)) << 4); return status; } VL53L1X_ERROR VL53L1X::VL53L1X_GetInterruptPolarity(uint8_t *pInterruptPolarity) { uint8_t Temp; VL53L1X_ERROR status = 0; status = VL53L1_RdByte(Device, GPIO_HV_MUX__CTRL, &Temp); Temp = Temp & 0x10; *pInterruptPolarity = !(Temp>>4); return status; } VL53L1X_ERROR VL53L1X::VL53L1X_StartRanging() { VL53L1X_ERROR status = 0; /* uint8_t Addr = 0x00; for (Addr = 0x2D; Addr <= 0x87; Addr++){ status = VL53L1_WrByte(Device, Addr, VL51L1X_DEFAULT_CONFIGURATION[Addr - 0x2D]); } */ status = VL53L1_WrByte(Device, SYSTEM__MODE_START, 0x40); /* Enable VL53L1X */ return status; } VL53L1X_ERROR VL53L1X::VL53L1X_StopRanging() { VL53L1X_ERROR status = 0; status = VL53L1_WrByte(Device, SYSTEM__MODE_START, 0x00); /* Disable VL53L1X */ return status; } VL53L1X_ERROR VL53L1X::VL53L1X_CheckForDataReady(uint8_t *isDataReady) { uint8_t Temp; uint8_t IntPol; VL53L1X_ERROR status = 0; status = VL53L1X_GetInterruptPolarity(&IntPol); status = VL53L1_RdByte(Device, GPIO__TIO_HV_STATUS, &Temp); /* Read in the register to check if a new value is available */ if (status == 0){ if ((Temp & 1) == IntPol) *isDataReady = 1; else *isDataReady = 0; } return status; } VL53L1X_ERROR VL53L1X::VL53L1X_SetTimingBudgetInMs(uint16_t TimingBudgetInMs) { uint16_t DM; VL53L1X_ERROR status=0; status = VL53L1X_GetDistanceMode(&DM); if (DM == 0) return 1; else if (DM == 1) { /* Short DistanceMode */ switch (TimingBudgetInMs) { case 15: /* only available in short distance mode */ VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, 0x01D); VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, 0x0027); break; case 20: VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, 0x0051); VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, 0x006E); break; case 33: VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, 0x00D6); VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, 0x006E); break; case 50: VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, 0x1AE); VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, 0x01E8); break; case 100: VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, 0x02E1); VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, 0x0388); break; case 200: VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, 0x03E1); VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, 0x0496); break; case 500: VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, 0x0591); VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, 0x05C1); break; default: status = 1; break; } } else { switch (TimingBudgetInMs) { case 20: VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, 0x001E); VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, 0x0022); break; case 33: VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, 0x0060); VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, 0x006E); break; case 50: VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, 0x00AD); VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, 0x00C6); break; case 100: VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, 0x01CC); VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, 0x01EA); break; case 200: VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, 0x02D9); VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, 0x02F8); break; case 500: VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, 0x048F); VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, 0x04A4); break; default: status = 1; break; } } return status; } VL53L1X_ERROR VL53L1X::VL53L1X_GetTimingBudgetInMs(uint16_t *pTimingBudget) { uint16_t Temp; VL53L1X_ERROR status = 0; status = VL53L1_RdWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, &Temp); switch (Temp) { case 0x001D : *pTimingBudget = 15; break; case 0x0051 : case 0x001E : *pTimingBudget = 20; break; case 0x00D6 : case 0x0060 : *pTimingBudget = 33; break; case 0x1AE : case 0x00AD : *pTimingBudget = 50; break; case 0x02E1 : case 0x01CC : *pTimingBudget = 100; break; case 0x03E1 : case 0x02D9 : *pTimingBudget = 200; break; case 0x0591 : case 0x048F : *pTimingBudget = 500; break; default: *pTimingBudget = 0; break; } return status; } VL53L1X_ERROR VL53L1X::VL53L1X_SetDistanceMode(uint16_t DM) { uint16_t TB; VL53L1X_ERROR status = 0; status = VL53L1X_GetTimingBudgetInMs(&TB); switch (DM) { case 1: status = VL53L1_WrByte(Device, PHASECAL_CONFIG__TIMEOUT_MACROP, 0x14); status = VL53L1_WrByte(Device, RANGE_CONFIG__VCSEL_PERIOD_A, 0x07); status = VL53L1_WrByte(Device, RANGE_CONFIG__VCSEL_PERIOD_B, 0x05); status = VL53L1_WrByte(Device, RANGE_CONFIG__VALID_PHASE_HIGH, 0x38); status = VL53L1_WrWord(Device, SD_CONFIG__WOI_SD0, 0x0705); status = VL53L1_WrWord(Device, SD_CONFIG__INITIAL_PHASE_SD0, 0x0606); break; case 2: status = VL53L1_WrByte(Device, PHASECAL_CONFIG__TIMEOUT_MACROP, 0x0A); status = VL53L1_WrByte(Device, RANGE_CONFIG__VCSEL_PERIOD_A, 0x0F); status = VL53L1_WrByte(Device, RANGE_CONFIG__VCSEL_PERIOD_B, 0x0D); status = VL53L1_WrByte(Device, RANGE_CONFIG__VALID_PHASE_HIGH, 0xB8); status = VL53L1_WrWord(Device, SD_CONFIG__WOI_SD0, 0x0F0D); status = VL53L1_WrWord(Device, SD_CONFIG__INITIAL_PHASE_SD0, 0x0E0E); break; default: break; } status = VL53L1X_SetTimingBudgetInMs(TB); return status; } VL53L1X_ERROR VL53L1X::VL53L1X_GetDistanceMode(uint16_t *DM) { uint8_t TempDM, status=0; status = VL53L1_RdByte(Device,PHASECAL_CONFIG__TIMEOUT_MACROP, &TempDM); if (TempDM == 0x14) *DM=1; if(TempDM == 0x0A) *DM=2; return status; } VL53L1X_ERROR VL53L1X::VL53L1X_SetInterMeasurementInMs(uint16_t InterMeasMs) { uint16_t ClockPLL; VL53L1X_ERROR status = 0; status = VL53L1_RdWord(Device, VL53L1_RESULT__OSC_CALIBRATE_VAL, &ClockPLL); ClockPLL = ClockPLL&0x3FF; VL53L1_WrDWord(Device, VL53L1_SYSTEM__INTERMEASUREMENT_PERIOD, (uint32_t)(ClockPLL * InterMeasMs * 1.075)); return status; } VL53L1X_ERROR VL53L1X::VL53L1X_GetInterMeasurementInMs(uint16_t *pIM) { uint16_t ClockPLL; VL53L1X_ERROR status = 0; uint32_t tmp; status = VL53L1_RdDWord(Device,VL53L1_SYSTEM__INTERMEASUREMENT_PERIOD, &tmp); *pIM = (uint16_t)tmp; status = VL53L1_RdWord(Device, VL53L1_RESULT__OSC_CALIBRATE_VAL, &ClockPLL); ClockPLL = ClockPLL&0x3FF; *pIM= (uint16_t)(*pIM/(ClockPLL*1.065)); return status; } VL53L1X_ERROR VL53L1X::VL53L1X_BootState(uint8_t *state) { VL53L1X_ERROR status = 0; uint8_t tmp = 0; status = VL53L1_RdByte(Device,VL53L1_FIRMWARE__SYSTEM_STATUS, &tmp); *state = tmp; return status; } VL53L1X_ERROR VL53L1X::VL53L1X_GetSensorId(uint16_t *sensorId) { VL53L1X_ERROR status = 0; uint16_t tmp = 0; status = VL53L1_RdWord(Device, VL53L1_IDENTIFICATION__MODEL_ID, &tmp); *sensorId = tmp; return status; } VL53L1X_ERROR VL53L1X::VL53L1X_GetDistance(uint16_t *distance) { VL53L1X_ERROR status = 0; uint16_t tmp; status = (VL53L1_RdWord(Device, VL53L1_RESULT__FINAL_CROSSTALK_CORRECTED_RANGE_MM_SD0, &tmp)); *distance = tmp; return status; } VL53L1X_ERROR VL53L1X::VL53L1X_GetSignalPerSpad(uint16_t *signalRate) { VL53L1X_ERROR status = 0; uint16_t SpNb=1, signal; status = VL53L1_RdWord(Device, VL53L1_RESULT__PEAK_SIGNAL_COUNT_RATE_CROSSTALK_CORRECTED_MCPS_SD0, &signal); status = VL53L1_RdWord(Device, VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0, &SpNb); *signalRate = (uint16_t) (2000.0*signal/SpNb); return status; } VL53L1X_ERROR VL53L1X::VL53L1X_GetAmbientPerSpad(uint16_t *ambPerSp) { VL53L1X_ERROR status=0; uint16_t AmbientRate, SpNb=1; status = VL53L1_RdWord(Device, RESULT__AMBIENT_COUNT_RATE_MCPS_SD, &AmbientRate); status = VL53L1_RdWord(Device, VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0, &SpNb); *ambPerSp=(uint16_t) (2000.0 * AmbientRate / SpNb); return status; } VL53L1X_ERROR VL53L1X::VL53L1X_GetSignalRate(uint16_t *signal) { VL53L1X_ERROR status = 0; uint16_t tmp; status = VL53L1_RdWord(Device, VL53L1_RESULT__PEAK_SIGNAL_COUNT_RATE_CROSSTALK_CORRECTED_MCPS_SD0, &tmp); *signal = tmp*8; return status; } VL53L1X_ERROR VL53L1X::VL53L1X_GetSpadNb(uint16_t *spNb) { VL53L1X_ERROR status = 0; uint16_t tmp; status = VL53L1_RdWord(Device, VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0, &tmp); *spNb = tmp >> 8; return status; } VL53L1X_ERROR VL53L1X::VL53L1X_GetAmbientRate(uint16_t *ambRate) { VL53L1X_ERROR status = 0; uint16_t tmp; status = VL53L1_RdWord(Device, RESULT__AMBIENT_COUNT_RATE_MCPS_SD, &tmp); *ambRate = tmp*8; return status; } VL53L1X_ERROR VL53L1X::VL53L1X_GetRangeStatus(uint8_t *rangeStatus) { VL53L1X_ERROR status = 0; uint8_t RgSt; status = VL53L1_RdByte(Device, VL53L1_RESULT__RANGE_STATUS, &RgSt); RgSt = RgSt&0x1F; switch (RgSt) { case 9: RgSt = 0; break; case 6: RgSt = 1; break; case 4: RgSt = 2; break; case 8: RgSt = 3; break; case 5: RgSt = 4; break; case 3: RgSt = 5; break; case 19: RgSt = 6; break; case 7: RgSt = 7; break; case 12: RgSt = 9; break; case 18: RgSt = 10; break; case 22: RgSt = 11; break; case 23: RgSt = 12; break; case 13: RgSt = 13; break; default: RgSt = 255; break; } *rangeStatus = RgSt; return status; } VL53L1X_ERROR VL53L1X::VL53L1X_SetOffset(int16_t OffsetValue) { VL53L1X_ERROR status = 0; int16_t Temp; Temp = (OffsetValue*4); VL53L1_WrWord(Device, ALGO__PART_TO_PART_RANGE_OFFSET_MM, (uint16_t)Temp); VL53L1_WrWord(Device, MM_CONFIG__INNER_OFFSET_MM, 0x0); VL53L1_WrWord(Device, MM_CONFIG__OUTER_OFFSET_MM, 0x0); return status; } VL53L1X_ERROR VL53L1X::VL53L1X_GetOffset(int16_t *offset) { VL53L1X_ERROR status = 0; uint16_t Temp; status = VL53L1_RdWord(Device,ALGO__PART_TO_PART_RANGE_OFFSET_MM, &Temp); Temp = Temp<<3; Temp = Temp >>5; *offset = (int16_t)(Temp); return status; } VL53L1X_ERROR VL53L1X::VL53L1X_SetXtalk(uint16_t XtalkValue) { /* XTalkValue in count per second to avoid float type */ VL53L1X_ERROR status = 0; status = VL53L1_WrWord(Device, ALGO__CROSSTALK_COMPENSATION_X_PLANE_GRADIENT_KCPS, 0x0000); status = VL53L1_WrWord(Device, ALGO__CROSSTALK_COMPENSATION_Y_PLANE_GRADIENT_KCPS, 0x0000); status = VL53L1_WrWord(Device, ALGO__CROSSTALK_COMPENSATION_PLANE_OFFSET_KCPS, (XtalkValue<<9)/1000); /* * << 9 (7.9 format) and /1000 to convert cps to kpcs */ return status; } VL53L1X_ERROR VL53L1X::VL53L1X_GetXtalk(uint16_t *xtalk ) { VL53L1X_ERROR status = 0; uint16_t tmp; status = VL53L1_RdWord(Device,ALGO__CROSSTALK_COMPENSATION_PLANE_OFFSET_KCPS, &tmp); *xtalk = (tmp*1000)>>9; /* * 1000 to convert kcps to cps and >> 9 (7.9 format) */ return status; } VL53L1X_ERROR VL53L1X::VL53L1X_SetDistanceThreshold(uint16_t ThreshLow, uint16_t ThreshHigh, uint8_t Window, uint8_t IntOnNoTarget) { VL53L1X_ERROR status = 0; uint8_t Temp = 0; status = VL53L1_RdByte(Device, SYSTEM__INTERRUPT_CONFIG_GPIO, &Temp); Temp = Temp & 0x47; if (IntOnNoTarget == 0) { status = VL53L1_WrByte(Device, SYSTEM__INTERRUPT_CONFIG_GPIO, (Temp | (Window & 0x07))); } else { status = VL53L1_WrByte(Device, SYSTEM__INTERRUPT_CONFIG_GPIO, ((Temp | (Window & 0x07)) | 0x40)); } status = VL53L1_WrWord(Device, SYSTEM__THRESH_HIGH, ThreshHigh); status = VL53L1_WrWord(Device, SYSTEM__THRESH_LOW, ThreshLow); return status; } VL53L1X_ERROR VL53L1X::VL53L1X_GetDistanceThresholdWindow(uint16_t *window) { VL53L1X_ERROR status = 0; uint8_t tmp; status = VL53L1_RdByte(Device,SYSTEM__INTERRUPT_CONFIG_GPIO, &tmp); *window = (uint16_t)(tmp & 0x7); return status; } VL53L1X_ERROR VL53L1X::VL53L1X_GetDistanceThresholdLow(uint16_t *low) { VL53L1X_ERROR status = 0; uint16_t tmp; status = VL53L1_RdWord(Device,SYSTEM__THRESH_LOW, &tmp); *low = tmp; return status; } VL53L1X_ERROR VL53L1X::VL53L1X_GetDistanceThresholdHigh(uint16_t *high) { VL53L1X_ERROR status = 0; uint16_t tmp; status = VL53L1_RdWord(Device,SYSTEM__THRESH_HIGH, &tmp); *high = tmp; return status; } VL53L1X_ERROR VL53L1X::VL53L1X_SetROI(uint16_t X, uint16_t Y) { uint8_t OpticalCenter; VL53L1X_ERROR status = 0; status =VL53L1_RdByte(Device, VL53L1_ROI_CONFIG__MODE_ROI_CENTRE_SPAD, &OpticalCenter); if (X > 16) X = 16; if (Y > 16) Y = 16; if (X > 10 || Y > 10){ OpticalCenter = 199; } status = VL53L1_WrByte(Device, ROI_CONFIG__USER_ROI_CENTRE_SPAD, OpticalCenter); status = VL53L1_WrByte(Device, ROI_CONFIG__USER_ROI_REQUESTED_GLOBAL_XY_SIZE, (Y - 1) << 4 | (X - 1)); return status; } VL53L1X_ERROR VL53L1X::VL53L1X_GetROI_XY(uint16_t *ROI_X, uint16_t *ROI_Y) { VL53L1X_ERROR status = 0; uint8_t tmp; status = VL53L1_RdByte(Device,ROI_CONFIG__USER_ROI_REQUESTED_GLOBAL_XY_SIZE, &tmp); *ROI_X = ((uint16_t)tmp & 0x0F) + 1; *ROI_Y = (((uint16_t)tmp & 0xF0) >> 4) + 1; return status; } VL53L1X_ERROR VL53L1X::VL53L1X_SetSignalThreshold(uint16_t Signal) { VL53L1X_ERROR status = 0; VL53L1_WrWord(Device,RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS,Signal>>3); return status; } VL53L1X_ERROR VL53L1X::VL53L1X_GetSignalThreshold(uint16_t *signal) { VL53L1X_ERROR status = 0; uint16_t tmp; status = VL53L1_RdWord(Device, RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS, &tmp); *signal = tmp <<3; return status; } VL53L1X_ERROR VL53L1X::VL53L1X_SetSigmaThreshold(uint16_t Sigma) { VL53L1X_ERROR status = 0; if(Sigma>(0xFFFF>>2)){ return 1; } /* 16 bits register 14.2 format */ status = VL53L1_WrWord(Device,RANGE_CONFIG__SIGMA_THRESH,Sigma<<2); return status; } VL53L1X_ERROR VL53L1X::VL53L1X_GetSigmaThreshold(uint16_t *sigma) { VL53L1X_ERROR status = 0; uint16_t tmp; status = VL53L1_RdWord(Device,RANGE_CONFIG__SIGMA_THRESH, &tmp); *sigma = tmp >> 2; return status; } VL53L1X_ERROR VL53L1X::VL53L1X_StartTemperatureUpdate() { VL53L1X_ERROR status = 0; uint8_t tmp=0; status = VL53L1_WrByte(Device,VL53L1_VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND,0x81); /* full VHV */ status = VL53L1_WrByte(Device,0x0B,0x92); status = VL53L1X_StartRanging(); while(tmp==0){ status = VL53L1X_CheckForDataReady(&tmp); } tmp = 0; status = VL53L1X_ClearInterrupt(); status = VL53L1X_StopRanging(); status = VL53L1_WrByte(Device, VL53L1_VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND, 0x09); /* two bounds VHV */ status = VL53L1_WrByte(Device, 0x0B, 0); /* start VHV from the previous temperature */ return status; } /* VL53L1X_calibration.h functions */ int8_t VL53L1X::VL53L1X_CalibrateOffset(uint16_t TargetDistInMm, int16_t *offset) { uint8_t i = 0, tmp; int16_t AverageDistance = 0; uint16_t distance; VL53L1X_ERROR status = 0; status = VL53L1_WrWord(Device, ALGO__PART_TO_PART_RANGE_OFFSET_MM, 0x0); status = VL53L1_WrWord(Device, MM_CONFIG__INNER_OFFSET_MM, 0x0); status = VL53L1_WrWord(Device, MM_CONFIG__OUTER_OFFSET_MM, 0x0); status = VL53L1X_StartRanging(); /* Enable VL53L1X sensor */ for (i = 0; i < 50; i++) { while (tmp == 0){ status = VL53L1X_CheckForDataReady(&tmp); } tmp = 0; status = VL53L1X_GetDistance(&distance); status = VL53L1X_ClearInterrupt(); AverageDistance = AverageDistance + distance; } status = VL53L1X_StopRanging(); AverageDistance = AverageDistance / 50; *offset = TargetDistInMm - AverageDistance; status = VL53L1_WrWord(Device, ALGO__PART_TO_PART_RANGE_OFFSET_MM, *offset*4); return status; } int8_t VL53L1X::VL53L1X_CalibrateXtalk(uint16_t TargetDistInMm, uint16_t *xtalk) { uint8_t i, tmp= 0; float AverageSignalRate = 0; float AverageDistance = 0; float AverageSpadNb = 0; uint16_t distance = 0, spadNum; uint16_t sr; VL53L1X_ERROR status = 0; status = VL53L1_WrWord(Device, 0x0016,0); status = VL53L1X_StartRanging(); for (i = 0; i < 50; i++) { while (tmp == 0){ status = VL53L1X_CheckForDataReady(&tmp); } tmp=0; status= VL53L1X_GetSignalRate(&sr); status= VL53L1X_GetDistance(&distance); status = VL53L1X_ClearInterrupt(); AverageDistance = AverageDistance + distance; status = VL53L1X_GetSpadNb(&spadNum); AverageSpadNb = AverageSpadNb + spadNum; AverageSignalRate = AverageSignalRate + sr; } status = VL53L1X_StopRanging(); AverageDistance = AverageDistance / 50; AverageSpadNb = AverageSpadNb / 50; AverageSignalRate = AverageSignalRate / 50; /* Calculate Xtalk value */ *xtalk = (uint16_t)(512*(AverageSignalRate*(1-(AverageDistance/TargetDistInMm)))/AverageSpadNb); status = VL53L1_WrWord(Device, 0x0016, *xtalk); return status; } /* Write and read functions from I2C */ VL53L1X_ERROR VL53L1X::VL53L1_WriteMulti(VL53L1_DEV Dev, uint16_t index, uint8_t *pdata, uint32_t count) { int status; status = VL53L1_I2CWrite(Dev->I2cDevAddr, index, pdata, (uint16_t)count); return status; } VL53L1X_ERROR VL53L1X::VL53L1_ReadMulti(VL53L1_DEV Dev, uint16_t index, uint8_t *pdata, uint32_t count) { int status; status = VL53L1_I2CRead(Dev->I2cDevAddr, index, pdata, (uint16_t)count); return status; } VL53L1X_ERROR VL53L1X::VL53L1_WrByte(VL53L1_DEV Dev, uint16_t index, uint8_t data) { int status; status=VL53L1_I2CWrite(Dev->I2cDevAddr, index, &data, 1); return status; } VL53L1X_ERROR VL53L1X::VL53L1_WrWord(VL53L1_DEV Dev, uint16_t index, uint16_t data) { int status; uint8_t buffer[2]; buffer[0] = data >> 8; buffer[1] = data & 0x00FF; status=VL53L1_I2CWrite(Dev->I2cDevAddr, index, (uint8_t *)buffer, 2); return status; } VL53L1X_ERROR VL53L1X::VL53L1_WrDWord(VL53L1_DEV Dev, uint16_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=VL53L1_I2CWrite(Dev->I2cDevAddr, index, (uint8_t *)buffer, 4); return status; } VL53L1X_ERROR VL53L1X::VL53L1_RdByte(VL53L1_DEV Dev, uint16_t index, uint8_t *data) { int status; status = VL53L1_I2CRead(Dev->I2cDevAddr, index, data, 1); if(status) return -1; return 0; } VL53L1X_ERROR VL53L1X::VL53L1_RdWord(VL53L1_DEV Dev, uint16_t index, uint16_t *data) { int status; uint8_t buffer[2] = {0,0}; status = VL53L1_I2CRead(Dev->I2cDevAddr, index, buffer, 2); if (!status) { *data = (buffer[0] << 8) + buffer[1]; } return status; } VL53L1X_ERROR VL53L1X::VL53L1_RdDWord(VL53L1_DEV Dev, uint16_t index, uint32_t *data) { int status; uint8_t buffer[4] = {0,0,0,0}; status = VL53L1_I2CRead(Dev->I2cDevAddr, index, buffer, 4); if(!status) { *data = (buffer[0] << 24) + (buffer[1] << 16) + (buffer[2] << 8) + buffer[3]; } return status; } VL53L1X_ERROR VL53L1X::VL53L1_UpdateByte(VL53L1_DEV Dev, uint16_t index, uint8_t AndData, uint8_t OrData) { int status; uint8_t buffer = 0; /* read data direct onto buffer */ status = VL53L1_I2CRead(Dev->I2cDevAddr, index, &buffer,1); if (!status) { buffer = (buffer & AndData) | OrData; status = VL53L1_I2CWrite(Dev->I2cDevAddr, index, &buffer, (uint16_t)1); } return status; } VL53L1X_ERROR VL53L1X::VL53L1_I2CWrite(uint8_t DeviceAddr, uint16_t RegisterAddr, uint8_t* pBuffer, uint16_t NumByteToWrite) { int ret; ret = dev_i2c->v53l1x_i2c_write(pBuffer, DeviceAddr, RegisterAddr, NumByteToWrite); if (ret) { return -1; } return 0; } VL53L1X_ERROR VL53L1X::VL53L1_I2CRead(uint8_t DeviceAddr, uint16_t RegisterAddr, uint8_t* pBuffer, uint16_t NumByteToRead) { int ret; ret = dev_i2c->v53l1x_i2c_read(pBuffer, DeviceAddr, RegisterAddr, NumByteToRead); //ret = dev_i2c->i2c_read(pBuffer, DeviceAddr, RegisterAddr, NumByteToRead); if (ret) { return -1; } return 0; } VL53L1X_ERROR VL53L1X::VL53L1_GetTickCount( uint32_t *ptick_count_ms) { /* Returns current tick count in [ms] */ VL53L1X_ERROR status = VL53L1_ERROR_NONE; //*ptick_count_ms = timeGetTime(); *ptick_count_ms = 0; return status; } VL53L1X_ERROR VL53L1X::VL53L1_WaitUs(VL53L1_Dev_t *pdev, int32_t wait_us) { //(void)pdev; //wait_ms(wait_us/1000); return VL53L1_ERROR_NONE; } VL53L1X_ERROR VL53L1X::VL53L1_WaitMs(VL53L1_Dev_t *pdev, int32_t wait_ms) { //(void)pdev; //wait_ms(wait_ms); return VL53L1_ERROR_NONE; } VL53L1X_ERROR VL53L1X::VL53L1_WaitValueMaskEx( VL53L1_Dev_t *pdev, uint32_t timeout_ms, uint16_t index, uint8_t value, uint8_t mask, uint32_t poll_delay_ms) { /* * Platform implementation of WaitValueMaskEx V2WReg script command * * WaitValueMaskEx( * duration_ms, * index, * value, * mask, * poll_delay_ms); */ VL53L1_Error status = VL53L1_ERROR_NONE; uint32_t start_time_ms = 0; uint32_t current_time_ms = 0; uint32_t polling_time_ms = 0; uint8_t byte_value = 0; uint8_t found = 0; /* calculate time limit in absolute time */ VL53L1_GetTickCount(&start_time_ms); /* remember current trace functions and temporarily disable * function logging */ /* wait until value is found, timeout reached on error occurred */ while ((status == VL53L1_ERROR_NONE) && (polling_time_ms < timeout_ms) && (found == 0)) { if (status == VL53L1_ERROR_NONE) status = VL53L1_RdByte( pdev, index, &byte_value); if ((byte_value & mask) == value) found = 1; if (status == VL53L1_ERROR_NONE && found == 0 && poll_delay_ms > 0) status = VL53L1_WaitMs( pdev, poll_delay_ms); /* Update polling time (Compare difference rather than absolute to negate 32bit wrap around issue) */ VL53L1_GetTickCount(¤t_time_ms); polling_time_ms = current_time_ms - start_time_ms; } if (found == 0 && status == VL53L1_ERROR_NONE) status = VL53L1_ERROR_TIME_OUT; return status; }