VL53L0X World smallest Time-of-Flight (ToF) ranging sensor
Dependencies: X_NUCLEO_COMMON ST_INTERFACES
Dependents: HelloWorld_ST_Sensors mbed-os-mqtt-client Multi_VL53L0X DISCO-IOT01_HomeEnv ... more
Fork of VL53L0X by
Diff: vl53l1x_class.cpp
- Revision:
- 6:8ac15bf6d635
- Parent:
- 5:f16727052990
--- a/vl53l1x_class.cpp Mon Jan 28 10:14:03 2019 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1074 +0,0 @@ -/** - ****************************************************************************** - * @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; -} - - -