Diff: src/VL53L1CB.cpp

Revision:

18:0696efe39d08

Parent:

16:27131f13570d

--- a/src/VL53L1CB.cpp	Wed Jun 23 12:57:26 2021 +0000
+++ b/src/VL53L1CB.cpp	Wed Jul 21 17:06:38 2021 +0200
@@ -69,41 +69,21 @@
 {
     VL53L1CB_ERROR Status = 0;
 
-    pVersion->major = VL53L1_IMPLEMENTATION_VER_MAJOR;
-    pVersion->minor = VL53L1_IMPLEMENTATION_VER_MINOR;
-    pVersion->build = VL53L1_IMPLEMENTATION_VER_SUB;
-    pVersion->revision = VL53L1_IMPLEMENTATION_VER_REVISION;
+    pVersion->major = VL53L1CB_IMPLEMENTATION_VER_MAJOR;
+    pVersion->minor = VL53L1CB_IMPLEMENTATION_VER_MINOR;
+    pVersion->build = VL53L1CB_IMPLEMENTATION_VER_SUB;
+    pVersion->revision = VL53L1CB_IMPLEMENTATION_VER_REVISION;
     return Status;
 }
 
 VL53L1CB_ERROR VL53L1CB::VL53L1CB_SetI2CAddress(uint8_t new_address)
 {
     VL53L1CB_ERROR status = 0;
-  //  status = (VL53L1CB_ERROR)VL53L1_SetDeviceAddress(Device,new_address);
-    
-
-  //  Device->i2c_slave_address = new_address;  //~~ was
     if ( Device->i2c_slave_address != new_address)
     {
-        status = VL53L1_WrByte(Device, VL53L1_I2C_SLAVE__DEVICE_ADDRESS, new_address >> 1);   
+    	status = (VL53L1CB_ERROR)VL53L1_SetDeviceAddress(Device,new_address);
         printf("VL53L1_SetI2CAddress %d to %d status = %d\n", Device->i2c_slave_address,new_address,status);
-                Device->i2c_slave_address = new_address; 
-
-    }
-    return status;
-}
-
-int VL53L1CB::init_sensor(uint8_t new_addr)
-{
-    Device->i2c_slave_address = new_addr;
-    int status = 0;
-    VL53L1CB_Off();
-    VL53L1CB_On();
-
-    status = is_present();
-    if (!status) {
-        printf("Failed to init VL53L0X sensor!\n\r");
-        return status;
+        Device->i2c_slave_address = new_address;
     }
     return status;
 }
@@ -112,99 +92,17 @@
 VL53L1CB_ERROR VL53L1CB::VL53L1CB_SensorInit()
 {
     VL53L1CB_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 = VL53L1CB_StartRanging();
-    if (status != 0)
-    {
-        printf("start ranging failed - %d\r\n", status);
-    }
- 
-    status = VL53L1CB_ClearInterrupt();
-    status = VL53L1CB_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;
-}
-
-
-VL53L1CB_ERROR VL53L1CB::VL53L1CB_ClearInterrupt()
-{
-    VL53L1CB_ERROR status = 0;
-
-    status = VL53L1_WrByte(Device, SYSTEM__INTERRUPT_CLEAR, 0x01);
-//    printf("VL53L1CB::VL53L1X_ClearInterrupt()\n");
-    return status;
-}
-
-
-
-
-
-VL53L1CB_ERROR VL53L1CB::VL53L1CB_GetInterruptPolarity(uint8_t *pInterruptPolarity)
-{
-    uint8_t Temp;
-    VL53L1CB_ERROR status = 0;
 
-    status = VL53L1_RdByte(Device, GPIO_HV_MUX__CTRL, &Temp);
-    Temp = Temp & 0x10;
-    *pInterruptPolarity = !(Temp>>4);
-    return status;
-}
-
-
-
-VL53L1CB_ERROR VL53L1CB::VL53L1CB_StartRanging()
-{
-    VL53L1CB_ERROR status = 0;
-
-    status = VL53L1_WrByte(Device, SYSTEM__MODE_START, 0x40); 
-    return status;
-}
-
-
-VL53L1CB_ERROR VL53L1CB::VL53L1CB_StopRanging()
-{
-    VL53L1CB_ERROR status = 0;
+    /* Device Initialization and setting */
+    status = VL53L1CB_DataInit();
+    if (status != 0) {
+        return status;
+    }
+    status = VL53L1CB_StaticInit();
+    if (status != 0) {
+        return status;
+    }
 
-    status = VL53L1_WrByte(Device, SYSTEM__MODE_START, 0x00);   
-    return status;
-}
-
-
-
-
-VL53L1CB_ERROR VL53L1CB::VL53L1CB_BootState(uint8_t *state)
-{
-    VL53L1CB_ERROR status = 0;
-    uint8_t tmp = 0;
-
-    status = VL53L1_RdByte(Device,VL53L1_FIRMWARE__SYSTEM_STATUS, &tmp);
-    *state = tmp;
-    return status;
-}
-
-
-VL53L1CB_ERROR VL53L1CB::VL53L1CB_GetDistance(uint16_t *distance)
-{
-    VL53L1CB_ERROR status = 0;
-    uint16_t tmp;
-
-    status = (VL53L1_RdWord(Device,
-            VL53L1_RESULT__FINAL_CROSSTALK_CORRECTED_RANGE_MM_SD0, &tmp));
-    *distance = tmp;
     return status;
 }
 
@@ -212,225 +110,13 @@
     /* Write and read functions from I2C */
 
 
-VL53L1CB_ERROR VL53L1CB::VL53L1CB_WriteMulti(VL53L1_DEV Dev, uint16_t index, uint8_t *pdata, uint32_t count)
-{
-   int  status;
-//printf(" class VL53L1_WriteMulti \n");
-   status = VL53L1CB_I2CWrite(Dev->i2c_slave_address, index, pdata, (uint16_t)count);
-   return status;
-}
-
-VL53L1CB_ERROR VL53L1CB::VL53L1CB_ReadMulti(VL53L1_DEV Dev, uint16_t index, uint8_t *pdata, uint32_t count)
-{
-    int status;
-
-    status = VL53L1CB_I2CRead(Dev->i2c_slave_address, index, pdata, (uint16_t)count);
-
-    return status;
-}
-
-
-VL53L1CB_ERROR VL53L1CB::VL53L1_WrByte(VL53L1_DEV Dev, uint16_t index, uint8_t data)
-{
-   int  status;
-
-   status=VL53L1CB_I2CWrite(Dev->i2c_slave_address, index, &data, 1);
-   return status;
-}
-
-
-VL53L1CB_ERROR VL53L1CB::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=VL53L1CB_I2CWrite(Dev->i2c_slave_address, index, (uint8_t *)buffer, 2);
-   return status;
-}
-
-VL53L1CB_ERROR VL53L1CB::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=VL53L1CB_I2CWrite(Dev->i2c_slave_address, index, (uint8_t *)buffer, 4);
-   return status;
-}
-
-
-VL53L1CB_ERROR VL53L1CB::VL53L1_RdByte(VL53L1_DEV Dev, uint16_t index, uint8_t *data)
-{
-   int  status;
-
-   status = VL53L1CB_I2CRead(Dev->i2c_slave_address, index, data, 1);
-
-   if(status)
-     return -1;
-
-   return 0;
-}
-
-VL53L1CB_ERROR VL53L1CB::VL53L1_RdWord(VL53L1_DEV Dev, uint16_t index, uint16_t *data)
-{
-   int  status;
-   uint8_t buffer[2] = {0,0};
-
-   status = VL53L1CB_I2CRead(Dev->i2c_slave_address, index, buffer, 2);
-   if (!status)
-   {
-       *data = (buffer[0] << 8) + buffer[1];
-   }
- //  printf("VL53L1_RdWord %d %d %d \n",Dev->i2c_slave_address,index,status);
-   return status;
-
-}
-
-VL53L1CB_ERROR VL53L1CB::VL53L1_RdDWord(VL53L1_DEV Dev, uint16_t index, uint32_t *data)
-{
-   int status;
-   uint8_t buffer[4] = {0,0,0,0};
-
-   status = VL53L1CB_I2CRead(Dev->i2c_slave_address, index, buffer, 4);
-   if(!status)
-   {
-       *data = (buffer[0] << 24) + (buffer[1] << 16) + (buffer[2] << 8) + buffer[3];
-   }
-   return status;
-
-}
-
-VL53L1CB_ERROR VL53L1CB::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 = VL53L1CB_I2CRead(Dev->i2c_slave_address, index, &buffer,1);
-   if (!status)
-   {
-      buffer = (buffer & AndData) | OrData;
-      status = VL53L1CB_I2CWrite(Dev->i2c_slave_address, index, &buffer, (uint16_t)1);
-   }
-   return status;
-}
-
-VL53L1CB_ERROR VL53L1CB::VL53L1CB_I2CWrite(uint8_t DeviceAddr, uint16_t RegisterAddr, uint8_t* pBuffer, uint16_t NumByteToWrite)
-{
-    int ret;
-    ret = dev_i2c->ToF_i2c_write(pBuffer, DeviceAddr, RegisterAddr, NumByteToWrite);
-    if (ret) {
-        return -1;
-    }
-    return 0;
-
-}
-
-VL53L1CB_ERROR VL53L1CB::VL53L1CB_I2CRead(uint8_t DeviceAddr, uint16_t RegisterAddr, uint8_t* pBuffer, uint16_t NumByteToRead)
-{
-    int ret;
-
-    ret = dev_i2c->ToF_i2c_read(pBuffer, DeviceAddr, RegisterAddr, NumByteToRead);
-    //ret = dev_i2c->i2c_read(pBuffer, DeviceAddr, RegisterAddr, NumByteToRead);
-    if (ret) {
-        return -1;
-    }
-    return 0;
-}
-
-
-VL53L1CB_ERROR VL53L1CB::VL53L1CB_GetTickCount(
-    uint32_t *ptick_count_ms)
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_WaitDeviceBooted()
 {
 
-    /* Returns current tick count in [ms] */
-
-    VL53L1CB_ERROR status  = VL53L1_ERROR_NONE;
-
-    //*ptick_count_ms = timeGetTime();
-    *ptick_count_ms = us_ticker_read() / 1000;
-
-    return status;
-}
-
-
-
-VL53L1CB_ERROR VL53L1CB::VL53L1CB_WaitUs(VL53L1_Dev_t *pdev, int32_t wait_time)
-{
-    //(void)pdev;
-      wait_us(wait_time);
-    return VL53L1_ERROR_NONE;
-}
-
-
-VL53L1CB_ERROR VL53L1CB::VL53L1CB_WaitMs(VL53L1_Dev_t *pdev, int32_t wait_time)
-{
-    //(void)pdev;
-
-#if (MBED_VERSION  > 60300) 
-    thread_sleep_for(wait_time);
-#else
-    wait_ms(wait_time);  // NEEDS A DELAY BETWEEN SENSORS
-#endif
-    return VL53L1_ERROR_NONE;
+    return VL53L1_WaitDeviceBooted(Device);
 }
 
 
-VL53L1CB_ERROR VL53L1CB::VL53L1CB_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);
-     */
-
-
-    return VL53L1_WaitValueMaskEx( pdev, timeout_ms,index, value, mask, poll_delay_ms);
-}
-
-
-/***************************************************************************/
-//VL53L1CB_ERROR VL53L1CB::VL53L1_WaitValueMaskEx(
-
-VL53L1CB_ERROR VL53L1CB::VL53L1CB_WaitDeviceBooted(VL53L1_DEV Dev)
-{
-
-    return VL53L1_WaitDeviceBooted(Dev);
-}
-
-
-static int32_t BDTable[VL53L1_TUNING_MAX_TUNABLE_KEY] = {
-        TUNING_VERSION,
-        TUNING_PROXY_MIN,
-        TUNING_SINGLE_TARGET_XTALK_TARGET_DISTANCE_MM,
-        TUNING_SINGLE_TARGET_XTALK_SAMPLE_NUMBER,
-        TUNING_MIN_AMBIENT_DMAX_VALID,
-        TUNING_MAX_SIMPLE_OFFSET_CALIBRATION_SAMPLE_NUMBER,
-        TUNING_XTALK_FULL_ROI_TARGET_DISTANCE_MM,
-        TUNING_SIMPLE_OFFSET_CALIBRATION_REPEAT,
-        TUNING_XTALK_FULL_ROI_BIN_SUM_MARGIN,
-        TUNING_XTALK_FULL_ROI_DEFAULT_OFFSET,
-        TUNING_ZERO_DISTANCE_OFFSET_NON_LINEAR_FACTOR_DEFAULT,
-};
-
-
 VL53L1CB_ERROR VL53L1CB::VL53L1CB_GetVersion(VL53L1_Version_t *pVersion)
 {
 
@@ -446,7 +132,6 @@
 
 //******************************************************************
 
-
 VL53L1CB_ERROR VL53L1CB::VL53L1CB_GetDeviceInfo(
     VL53L1_DeviceInfo_t *pVL53L1_DeviceInfo)
 {
@@ -477,28 +162,14 @@
     return VL53L1_GetPalStateString(PalStateCode, pPalStateString);
 }
 
-VL53L1CB_ERROR VL53L1CB::VL53L1CB_GetPalState(VL53L1_DEV Dev, VL53L1_State *pPalState)
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_GetPalState(VL53L1_State *pPalState)
 {
-    return VL53L1_GetPalState(Dev,pPalState);
+    return VL53L1_GetPalState(Device, pPalState);
 }
 
 
-VL53L1CB_ERROR VL53L1CB::VL53L1CB_SetDeviceAddress(VL53L1_DEV Dev, uint8_t DeviceAddress)
-{
-    VL53L1CB_ERROR Status = VL53L1_ERROR_NONE;
-
- //   LOG_FUNCTION_START("");
-
-    Status = VL53L1_WrByte(Dev, VL53L1_I2C_SLAVE__DEVICE_ADDRESS,
-            DeviceAddress / 2);
-
-//    LOG_FUNCTION_END(Status);
-    return Status;
-}
-
 VL53L1CB_ERROR VL53L1CB::VL53L1CB_DataInit()
 {
-    printf("vl53L1_DataInit %d \n",Device->i2c_slave_address);
     return VL53L1_DataInit( Device);
 }
 
@@ -598,17 +269,9 @@
 }
 
 
-
 VL53L1CB_ERROR VL53L1CB::VL53L1CB_GetNumberOfLimitCheck(uint16_t *pNumberOfLimitCheck)
 {
-    VL53L1CB_ERROR Status = VL53L1_ERROR_NONE;
-
-  //  LOG_FUNCTION_START("");
-
-    *pNumberOfLimitCheck = VL53L1_CHECKENABLE_NUMBER_OF_CHECKS;
-
- //   LOG_FUNCTION_END(Status);
-    return Status;
+    return VL53L1_GetNumberOfLimitCheck(pNumberOfLimitCheck);
 }
 
 VL53L1CB_ERROR VL53L1CB::VL53L1CB_GetLimitCheckInfo(uint16_t LimitCheckId,
@@ -859,6 +522,11 @@
     return VL53L1_PerformOffsetZeroDistanceCalibration(Device);
 }
 
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_PerformOffsetPerVcselCalibration(int32_t CalDistanceMilliMeter)
+{
+    return VL53L1_PerformOffsetPerVcselCalibration(Device,CalDistanceMilliMeter);
+}
+
 VL53L1CB_ERROR VL53L1CB::VL53L1CB_SetCalibrationData(
         VL53L1_CalibrationData_t *pCalibrationData)
 {
@@ -892,11 +560,6 @@
     return VL53L1_GetOpticalCenter(Device,pOpticalCenterX,pOpticalCenterY);
 }
 
-
-
-
-
-
 VL53L1CB_ERROR VL53L1CB::VL53L1CB_SetThresholdConfig(VL53L1_DetectionConfig_t *pConfig)
 {
     return VL53L1_SetThresholdConfig(Device,pConfig);
@@ -910,26 +573,6 @@
 
 
 
-
-VL53L1CB_ERROR VL53L1CB::VL53L1CB_PerformOffsetPerVcselCalibration(int32_t CalDistanceMilliMeter)
-{
-    return VL53L1_PerformOffsetPerVcselCalibration(Device,CalDistanceMilliMeter);
-}
-
-
-
-// from vl53l1_api_debug.c
-
-
-VL53L1CB_ERROR VL53L1CB::VL53L1CB_get_additional_data(
-    VL53L1_DEV                      Dev,
-    VL53L1_additional_data_t        *pdata)
-{
-    return VL53L1_get_additional_data(Dev,pdata);
-}
-
-
-
 int VL53L1CB::handle_irq(uint16_t *distance)
 {
     int status;
@@ -941,9 +584,11 @@
 int VL53L1CB::get_measurement(uint16_t *distance)
 {
     int status = 0;
+    VL53L1_RangingMeasurementData_t RangingMeasurementData;
 
-    status = VL53L1CB_GetDistance(distance);
-    status = VL53L1CB_ClearInterrupt();
+    status = VL53L1CB_GetRangingMeasurementData(&RangingMeasurementData);
+    *distance = RangingMeasurementData.RangeMilliMeter;
+    status = VL53L1CB_ClearInterruptAndStartMeasurement();
 
     return status;
 }
@@ -957,16 +602,12 @@
         return 1;
     }
 
-    status = VL53L1CB_StopRanging(); // it is safer to do this while sensor is stopped
+    VL53L1CB_StopMeasurement(); // it is safer to do this while sensor is stopped
 
-    if (status == 0) {
-        attach_interrupt_measure_detection_irq(fptr);
-        enable_interrupt_measure_detection_irq();
-    }
+    attach_interrupt_measure_detection_irq(fptr);
+    enable_interrupt_measure_detection_irq();
 
-    if (status == 0) {
-        status = VL53L1CB_StartRanging();
-    }
+    status = VL53L1CB_StartMeasurement();
 
     return status;
 }
@@ -976,14 +617,8 @@
     int status = 0;
 
     if (status == 0) {
-        printf("Call of VL53L1_StopMeasurement\n");
-        status = VL53L1CB_StopRanging();
+        status = VL53L1CB_StopMeasurement();
     }
 
-    if (status == 0)
-        status = VL53L1CB_ClearInterrupt();
-
     return status;
 }
-
-