Diff: src/VL53L1CB.cpp

Revision:

11:be14e3591d65

Parent:

10:3687b5e79f98

Child:

14:f06785b2a964

diff -r 3687b5e79f98 -r be14e3591d65 src/VL53L1CB.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/VL53L1CB.cpp	Mon Jun 14 07:46:41 2021 +0000
@@ -0,0 +1,977 @@
+/**
+ ******************************************************************************
+ * @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>&copy; 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 "VL53L1CB.h"
+/************************************************/
+#include "vl53l1_platform_user_config.h"
+#include "vl53l1_def.h"
+#include "vl53l1_wait.h"
+#include "vl53l1_api.h"
+#include "vl53l1_api_debug.h"
+#include "vl53l1_api_strings.h"
+#include "vl53l1_preset_setup.h"
+#include "vl53l1_api_calibration.h"
+#include "vl53l1_nvm_structs.h"
+#include "vl53l1_nvm.h"
+#include "vl53l1_core.h"
+#include "vl53l1_register_funcs.h"
+/***********************************************************/
+#include "vl53l1_api_core.h"
+
+#include "vl53l1_configuration.h"
+
+
+#ifndef MIN
+#define MIN(v1, v2) ((v1) < (v2) ? (v1) : (v2))
+#endif
+#ifndef MAX
+#define MAX(v1, v2) ((v1) < (v2) ? (v2) : (v1))
+#endif
+
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_GetSWVersion(VL53L1_Version_t *pVersion)
+{
+    VL53L1CB_ERROR Status = 0;
+
+    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 = VL53L1CB_WrByte(Device, VL53L1CB_I2C_SLAVE__DEVICE_ADDRESS, new_address >> 1);   
+        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;
+    }
+    return status;
+}
+
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_SensorInit()
+{
+    VL53L1CB_ERROR status = 0;
+    uint8_t Addr = 0x00;
+
+    for (Addr = 0x2D; Addr <= 0x87; Addr++){
+        status = VL53L1CB_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 = VL53L1CB_WrByte(Device, SYSTEM__INTERRUPT_CLEAR, 0x01);
+//    printf("VL53L1CB::VL53L1CBX_ClearInterrupt()\n");
+    return status;
+}
+
+
+
+
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_GetInterruptPolarity(uint8_t *pInterruptPolarity)
+{
+    uint8_t Temp;
+    VL53L1CB_ERROR status = 0;
+
+    status = VL53L1CB_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 = VL53L1CB_WrByte(Device, SYSTEM__MODE_START, 0x40); 
+    return status;
+}
+
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_StopRanging()
+{
+    VL53L1CB_ERROR status = 0;
+
+    status = VL53L1CB_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 = VL53L1CB_RdByte(Device,VL53L1CB_FIRMWARE__SYSTEM_STATUS, &tmp);
+    *state = tmp;
+    return status;
+}
+
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_GetDistance(uint16_t *distance)
+{
+    VL53L1CB_ERROR status = 0;
+    uint16_t tmp;
+
+    status = (VL53L1CB_RdWord(Device,
+            VL53L1CB_RESULT__FINAL_CROSSTALK_CORRECTED_RANGE_MM_SD0, &tmp));
+    *distance = tmp;
+    return status;
+}
+
+
+    /* 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::VL53L1CB_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::VL53L1CB_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::VL53L1CB_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::VL53L1CB_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::VL53L1CB_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::VL53L1CB_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::VL53L1CB_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)
+{
+
+    /* 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;
+}
+
+
+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::VL53L1CB_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)
+{
+
+    return VL53L1_GetVersion(pVersion);
+}
+
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_GetProductRevision(
+    uint8_t *pProductRevisionMajor, uint8_t *pProductRevisionMinor)
+{
+    return VL53L1_GetProductRevision(Device,pProductRevisionMajor,pProductRevisionMinor);
+}
+
+//******************************************************************
+
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_GetDeviceInfo(
+    VL53L1_DeviceInfo_t *pVL53L1_DeviceInfo)
+{
+    return VL53L1_GetDeviceInfo(Device,pVL53L1_DeviceInfo);
+}
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_GetUID( uint64_t *pUid)
+{
+    return VL53L1_GetUID(Device,pUid);
+}
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_GetRangeStatusString(uint8_t RangeStatus,
+    char *pRangeStatusString)
+{
+    return VL53L1_GetRangeStatusString(RangeStatus,
+        pRangeStatusString);
+}
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_GetPalErrorString(VL53L1CB_ERROR PalErrorCode,
+    char *pPalErrorString)
+{
+    return VL53L1_GetPalErrorString(PalErrorCode,pPalErrorString);
+}
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_GetPalStateString(VL53L1_State PalStateCode,
+    char *pPalStateString)
+{
+    return VL53L1_GetPalStateString(PalStateCode, pPalStateString);
+}
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_GetPalState(VL53L1_DEV Dev, VL53L1_State *pPalState)
+{
+    return VL53L1_GetPalState(Dev,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, VL53L1CB_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);
+}
+
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_StaticInit()
+{
+    return VL53L1_StaticInit( Device);
+}
+
+
+ VL53L1CB_ERROR VL53L1CB::VL53L1CB_SetPresetMode(VL53L1_PresetModes PresetMode)
+{
+    return VL53L1_SetPresetMode(Device,PresetMode);
+}
+
+
+ VL53L1CB_ERROR VL53L1CB::VL53L1CB_GetPresetMode( VL53L1_PresetModes *pPresetMode)
+{
+    return VL53L1_GetPresetMode(Device,pPresetMode);
+}
+
+
+ VL53L1CB_ERROR VL53L1CB::VL53L1CB_SetDistanceMode(VL53L1_DistanceModes DistanceMode)
+{
+    return VL53L1_SetDistanceMode(Device,DistanceMode);
+}
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_GetDistanceMode(VL53L1_DistanceModes *pDistanceMode)
+{
+    return VL53L1_GetDistanceMode(Device,pDistanceMode);
+}
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_SetOutputMode(VL53L1_OutputModes OutputMode)
+{
+    return VL53L1_SetOutputMode(Device,OutputMode);
+}
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_GetOutputMode(VL53L1_OutputModes *pOutputMode)
+{
+    return VL53L1_GetOutputMode(Device,pOutputMode);
+}
+
+
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_SetMeasurementTimingBudgetMicroSeconds(
+                                   uint32_t MeasurementTimingBudgetMicroSeconds)
+{
+    return VL53L1_SetMeasurementTimingBudgetMicroSeconds(Device,
+                                           MeasurementTimingBudgetMicroSeconds);
+}
+
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_GetMeasurementTimingBudgetMicroSeconds(
+                                 uint32_t *pMeasurementTimingBudgetMicroSeconds)
+{
+    return VL53L1_GetMeasurementTimingBudgetMicroSeconds(Device,
+                                        pMeasurementTimingBudgetMicroSeconds);
+}
+
+
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_SetInterMeasurementPeriodMilliSeconds(
+    uint32_t InterMeasurementPeriodMilliSeconds)
+{   
+    return VL53L1_SetInterMeasurementPeriodMilliSeconds(Device,InterMeasurementPeriodMilliSeconds);
+}
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_GetInterMeasurementPeriodMilliSeconds(
+    uint32_t *pInterMeasurementPeriodMilliSeconds)
+{
+    return VL53L1_GetInterMeasurementPeriodMilliSeconds(Device,pInterMeasurementPeriodMilliSeconds);
+}
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_SetDmaxReflectance(FixPoint1616_t DmaxReflectance)
+{
+
+    return VL53L1_SetDmaxReflectance(Device,DmaxReflectance);
+}
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_GetDmaxReflectance(
+                                           FixPoint1616_t *pDmaxReflectance)
+{
+    return VL53L1_GetDmaxReflectance(Device,pDmaxReflectance);
+}
+
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_SetDmaxMode(VL53L1_DeviceDmaxModes DmaxMode)
+{
+    return VL53L1_SetDmaxMode(Device,DmaxMode);
+}
+
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_GetDmaxMode(
+    VL53L1_DeviceDmaxModes *pDmaxMode)
+{
+    return VL53L1_GetDmaxMode(Device,pDmaxMode);
+}
+
+
+
+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;
+}
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_GetLimitCheckInfo(uint16_t LimitCheckId,
+    char *pLimitCheckString)
+{
+    return VL53L1_GetLimitCheckInfo(LimitCheckId,
+        pLimitCheckString);
+
+}
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_GetLimitCheckStatus(uint16_t LimitCheckId,
+    uint8_t *pLimitCheckStatus)
+{
+    return VL53L1_GetLimitCheckStatus(Device,LimitCheckId,pLimitCheckStatus);
+}
+
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_SetLimitCheckEnable(uint16_t LimitCheckId,
+    uint8_t LimitCheckEnable)
+{
+
+    return VL53L1_SetLimitCheckEnable(Device,LimitCheckId,LimitCheckEnable);
+}
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_GetLimitCheckEnable(uint16_t LimitCheckId,
+    uint8_t *pLimitCheckEnable)
+{
+    return VL53L1_GetLimitCheckEnable(Device,LimitCheckId,pLimitCheckEnable);
+}
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_SetLimitCheckValue( uint16_t LimitCheckId,
+    FixPoint1616_t LimitCheckValue)
+{
+    return VL53L1_SetLimitCheckValue(Device,LimitCheckId,LimitCheckValue);
+}
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_GetLimitCheckValue( uint16_t LimitCheckId,
+    FixPoint1616_t *pLimitCheckValue)
+{
+    return VL53L1_GetLimitCheckValue(Device,LimitCheckId,pLimitCheckValue);
+}
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_GetLimitCheckCurrent( uint16_t LimitCheckId,
+    FixPoint1616_t *pLimitCheckCurrent)
+{
+    return VL53L1_GetLimitCheckCurrent(Device,LimitCheckId,pLimitCheckCurrent);
+}
+
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_GetMaxNumberOfROI( uint8_t *pMaxNumberOfROI)
+{
+    return VL53L1_GetMaxNumberOfROI(Device,pMaxNumberOfROI);
+}
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_SetROI( VL53L1_RoiConfig_t *pRoiConfig)
+{
+
+    return VL53L1_SetROI(Device,pRoiConfig);
+}
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_GetROI(VL53L1_RoiConfig_t *pRoiConfig)
+{
+    return VL53L1_GetROI(Device,pRoiConfig);
+}
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_GetNumberOfSequenceSteps(uint8_t *pNumberOfSequenceSteps)
+{
+    VL53L1CB_ERROR Status = VL53L1_ERROR_NONE;
+
+ //   SUPPRESS_UNUSED_WARNING(Dev);
+
+ //   LOG_FUNCTION_START("");
+
+    *pNumberOfSequenceSteps = VL53L1_SEQUENCESTEP_NUMBER_OF_ITEMS;
+
+//    LOG_FUNCTION_END(Status);
+    return Status;
+}
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_GetSequenceStepsInfo(VL53L1_SequenceStepId SequenceStepId,
+    char *pSequenceStepsString)
+{
+    return VL53L1_GetSequenceStepsInfo(SequenceStepId,pSequenceStepsString);
+}
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_SetSequenceStepEnable(VL53L1_SequenceStepId SequenceStepId,
+                                                    uint8_t SequenceStepEnabled)
+{
+
+    return VL53L1_SetSequenceStepEnable(Device,SequenceStepId,SequenceStepEnabled);
+}
+
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_GetSequenceStepEnable(VL53L1_SequenceStepId SequenceStepId, 
+                                                    uint8_t *pSequenceStepEnabled)
+{
+    return VL53L1_GetSequenceStepEnable(Device,SequenceStepId,pSequenceStepEnabled);
+}
+
+
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_StartMeasurement()
+{
+    return VL53L1_StartMeasurement(Device);
+}
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_StopMeasurement()
+{
+    return VL53L1_StopMeasurement(Device);
+}
+
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_ClearInterruptAndStartMeasurement()
+{
+
+    return VL53L1_ClearInterruptAndStartMeasurement(Device);
+}
+
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_GetMeasurementDataReady(uint8_t *pMeasurementDataReady)
+{
+    return VL53L1_GetMeasurementDataReady(Device, pMeasurementDataReady);
+}
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_WaitMeasurementDataReady()
+{
+    return VL53L1_WaitMeasurementDataReady(Device);
+}
+
+
+//******************************************************************
+
+
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_GetCalibrationData(
+        VL53L1_CalibrationData_t  *pCalibrationData)
+{
+    
+    VL53L1CB_ERROR Status = VL53L1_ERROR_NONE;
+    Status = VL53L1_GetCalibrationData(Device,pCalibrationData);
+
+    return Status;
+}
+
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_GetRangingMeasurementData(
+    VL53L1_RangingMeasurementData_t *pRangingMeasurementData)
+{
+//              printf("   VL53L1_GetRangingMeasurementData 000  \n");  
+    VL53L1CB_ERROR Status = VL53L1_ERROR_NONE;
+    Status = VL53L1_GetRangingMeasurementData(Device,pRangingMeasurementData);
+
+    return Status;
+}
+
+
+
+
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_GetMultiRangingData(
+        VL53L1_MultiRangingData_t *pMultiRangingData)
+{
+    return VL53L1_GetMultiRangingData(Device,pMultiRangingData);
+}
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_GetAdditionalData(
+        VL53L1_AdditionalData_t *pAdditionalData)
+{
+    return VL53L1_GetAdditionalData(Device, pAdditionalData);
+}
+
+
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_SetTuningParameter(
+        uint16_t TuningParameterId, int32_t TuningParameterValue)
+{
+    return VL53L1_SetTuningParameter(Device,TuningParameterId,TuningParameterValue);
+}
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_GetTuningParameter(
+        uint16_t TuningParameterId, int32_t *pTuningParameterValue)
+{
+
+    return VL53L1_GetTuningParameter(Device,TuningParameterId,pTuningParameterValue);
+}
+
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_SetXTalkCompensationEnable(
+    uint8_t XTalkCompensationEnable)
+{
+
+    return  VL53L1_SetXTalkCompensationEnable(Device,XTalkCompensationEnable);
+}
+
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_GetXTalkCompensationEnable(
+    uint8_t *pXTalkCompensationEnable)
+{
+    VL53L1CB_ERROR Status = VL53L1_ERROR_NONE;
+
+  //  LOG_FUNCTION_START("");
+
+    VL53L1_GetXTalkCompensationEnable(
+        Device,
+        pXTalkCompensationEnable);
+
+  //  LOG_FUNCTION_END(Status);
+    return Status;
+}
+
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_PerformXTalkCalibration(
+        uint8_t CalibrationOption)
+{
+
+    return VL53L1_PerformXTalkCalibration(Device,CalibrationOption);
+}
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_SetOffsetCalibrationMode(
+        VL53L1_OffsetCalibrationModes OffsetCalibrationMode)
+{
+    return VL53L1_SetOffsetCalibrationMode(Device,OffsetCalibrationMode);
+}
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_SetOffsetCorrectionMode(
+        VL53L1_OffsetCorrectionModes OffsetCorrectionMode)
+{
+    return  VL53L1_SetOffsetCorrectionMode(Device,OffsetCorrectionMode);
+}
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_PerformOffsetCalibration(
+    int32_t CalDistanceMilliMeter, FixPoint1616_t CalReflectancePercent)
+{
+    return VL53L1_PerformOffsetCalibration(Device,CalDistanceMilliMeter,CalReflectancePercent);
+}
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_PerformOffsetSimpleCalibration(
+    int32_t CalDistanceMilliMeter)
+{
+    return VL53L1_PerformOffsetSimpleCalibration(Device,CalDistanceMilliMeter);
+}
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_PerformOffsetZeroDistanceCalibration()
+{
+    return VL53L1_PerformOffsetZeroDistanceCalibration(Device);
+}
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_SetCalibrationData(
+        VL53L1_CalibrationData_t *pCalibrationData)
+{
+    return VL53L1_SetCalibrationData(Device,pCalibrationData);
+}
+
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_SetZoneCalibrationData(
+        VL53L1_ZoneCalibrationData_t *pZoneCalibrationData)
+{
+    return VL53L1_SetZoneCalibrationData(Device, pZoneCalibrationData);
+}
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_GetZoneCalibrationData(
+        VL53L1_ZoneCalibrationData_t  *pZoneCalibrationData)
+{
+    return VL53L1_GetZoneCalibrationData(Device, pZoneCalibrationData);
+}
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_SmudgeCorrectionEnable(
+        VL53L1_SmudgeCorrectionModes Mode)
+        
+{
+    return VL53L1_SmudgeCorrectionEnable(Device, Mode);
+}
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_GetOpticalCenter(
+        FixPoint1616_t *pOpticalCenterX,
+        FixPoint1616_t *pOpticalCenterY)
+{
+    return VL53L1_GetOpticalCenter(Device,pOpticalCenterX,pOpticalCenterY);
+}
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_SetThresholdConfig(VL53L1_DetectionConfig_t *pConfig)
+{
+    return VL53L1_SetThresholdConfig(Device,pConfig);
+}
+
+
+VL53L1CB_ERROR VL53L1CB::VL53L1CB_GetThresholdConfig(VL53L1_DetectionConfig_t *pConfig)
+{
+    return VL53L1_GetThresholdConfig(Device,pConfig);
+}
+
+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;
+    status = get_measurement(distance);
+    enable_interrupt_measure_detection_irq();
+    return status;
+}
+
+int VL53L1CB::get_measurement(uint16_t *distance)
+{
+    int status = 0;
+
+    status = VL53L1CB_GetDistance(distance);
+    status = VL53L1CB_ClearInterrupt();
+
+    return status;
+}
+
+int VL53L1CB::start_measurement(void (*fptr)(void))
+{
+    int status = 0;
+
+    if (_gpio1Int == NULL) {
+        printf("GPIO1 Error\r\n");
+        return 1;
+    }
+
+    status = VL53L1CB_StopRanging(); // it is safer to do this while sensor is stopped
+
+    if (status == 0) {
+        attach_interrupt_measure_detection_irq(fptr);
+        enable_interrupt_measure_detection_irq();
+    }
+
+    if (status == 0) {
+        status = VL53L1CB_StartRanging();
+    }
+
+    return status;
+}
+
+int VL53L1CB::stop_measurement()
+{
+    int status = 0;
+
+    if (status == 0) {
+        printf("Call of VL53L1_StopMeasurement\n");
+        status = VL53L1CB_StopRanging();
+    }
+
+    if (status == 0)
+        status = VL53L1CB_ClearInterrupt();
+
+    return status;
+}
+
+