File content as of revision 10:3687b5e79f98:

/**
 ******************************************************************************
 * @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;
}