vl53l1x_api.c

Go to the documentation of this file.

/*
 Copyright (c) 2017, STMicroelectronics - All Rights Reserved

 This file : part of VL53L1 Core and : dual licensed,
 either 'STMicroelectronics
 Proprietary license'
 or 'BSD 3-clause "New" or "Revised" License' , at your option.

*******************************************************************************

 'STMicroelectronics Proprietary license'

*******************************************************************************

 License terms: STMicroelectronics Proprietary in accordance with licensing
 terms at www.st.com/sla0081

 STMicroelectronics confidential
 Reproduction and Communication of this document : strictly prohibited unless
 specifically authorized in writing by STMicroelectronics.


*******************************************************************************

 Alternatively, VL53L1 Core may be distributed under the terms of
 'BSD 3-clause "New" or "Revised" License', in which case the following
 provisions apply instead of the ones mentioned above :

*******************************************************************************

 License terms: BSD 3-clause "New" or "Revised" License.

 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 the copyright holder 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.
*******************************************************************************
*/

#include "vl53l1x_api.h"
#include <string.h>

#if 0
uint8_t VL51L1X_NVM_CONFIGURATION[] = {
  0x00, /* 0x00 : not user-modifiable */
  0x29, /* 0x01 : 7 bits I2C address (default=0x29), use SetI2CAddress(). Warning: after changing the register value to a new I2C address, the device will only answer to the new address */
  0x00, /* 0x02 : not user-modifiable */
  0x00, /* 0x03 : not user-modifiable */
  0x00, /* 0x04 : not user-modifiable */
  0x00, /* 0x05 : not user-modifiable */
  0x00, /* 0x06 : not user-modifiable */
  0x00, /* 0x07 : not user-modifiable */
  0x00, /* 0x08 : not user-modifiable */
  0x50, /* 0x09 : not user-modifiable */
  0x00, /* 0x0A : not user-modifiable */
  0x00, /* 0x0B : not user-modifiable */
  0x00, /* 0x0C : not user-modifiable */
  0x00, /* 0x0D : not user-modifiable */
  0x0a, /* 0x0E : not user-modifiable */
  0x00, /* 0x0F : not user-modifiable */
  0x00, /* 0x10 : not user-modifiable */
  0x00, /* 0x11 : not user-modifiable */
  0x00, /* 0x12 : not user-modifiable */
  0x00, /* 0x13 : not user-modifiable */
  0x00, /* 0x14 : not user-modifiable */
  0x00, /* 0x15 : not user-modifiable */
  0x00, /* 0x16 : Xtalk calibration value MSB (7.9 format in kcps), use SetXtalk() */
  0x00, /* 0x17 : Xtalk calibration value LSB */
  0x00, /* 0x18 : not user-modifiable */
  0x00, /* 0x19 : not user-modifiable */
  0x00, /* 0x1a : not user-modifiable */
  0x00, /* 0x1b : not user-modifiable */
  0x00, /* 0x1e : Part to Part offset x4 MSB (in mm), use SetOffset() */
  0x50, /* 0x1f : Part to Part offset x4 LSB */
  0x00, /* 0x20 : not user-modifiable */
  0x00, /* 0x21 : not user-modifiable */
  0x00, /* 0x22 : not user-modifiable */
  0x00, /* 0x23 : not user-modifiable */
}
#endif

const uint8_t VL51L1X_DEFAULT_CONFIGURATION[] = {
  0x00, /* 0x2d : set bit 2 and 5 to 1 for fast plus mode (1MHz I2C), else don't touch */
  0x00, /* 0x2e : bit 0 if I2C pulled up at 1.8V, else set bit 0 to 1 (pull up at AVDD) */
  0x00, /* 0x2f : bit 0 if GPIO pulled up at 1.8V, else set bit 0 to 1 (pull up at AVDD) */
  0x01, /* 0x30 : set bit 4 to 0 for active high interrupt and 1 for active low (bits 3:0 must be 0x1), use SetInterruptPolarity() */
  0x02, /* 0x31 : bit 1 = interrupt depending on the polarity, use CheckForDataReady() */
  0x00, /* 0x32 : not user-modifiable */
  0x02, /* 0x33 : not user-modifiable */
  0x08, /* 0x34 : not user-modifiable */
  0x00, /* 0x35 : not user-modifiable */
  0x08, /* 0x36 : not user-modifiable */
  0x10, /* 0x37 : not user-modifiable */
  0x01, /* 0x38 : not user-modifiable */
  0x01, /* 0x39 : not user-modifiable */
  0x00, /* 0x3a : not user-modifiable */
  0x00, /* 0x3b : not user-modifiable */
  0x00, /* 0x3c : not user-modifiable */
  0x00, /* 0x3d : not user-modifiable */
  0xff, /* 0x3e : not user-modifiable */
  0x00, /* 0x3f : not user-modifiable */
  0x0F, /* 0x40 : not user-modifiable */
  0x00, /* 0x41 : not user-modifiable */
  0x00, /* 0x42 : not user-modifiable */
  0x00, /* 0x43 : not user-modifiable */
  0x00, /* 0x44 : not user-modifiable */
  0x00, /* 0x45 : not user-modifiable */
  0x20, /* 0x46 : interrupt configuration 0->level low detection, 1-> level high, 2-> Out of window, 3->In window, 0x20-> New sample ready , TBC */
  0x0b, /* 0x47 : not user-modifiable */
  0x00, /* 0x48 : not user-modifiable */
  0x00, /* 0x49 : not user-modifiable */
  0x02, /* 0x4a : not user-modifiable */
  0x0a, /* 0x4b : not user-modifiable */
  0x21, /* 0x4c : not user-modifiable */
  0x00, /* 0x4d : not user-modifiable */
  0x00, /* 0x4e : not user-modifiable */
  0x05, /* 0x4f : not user-modifiable */
  0x00, /* 0x50 : not user-modifiable */
  0x00, /* 0x51 : not user-modifiable */
  0x00, /* 0x52 : not user-modifiable */
  0x00, /* 0x53 : not user-modifiable */
  0xc8, /* 0x54 : not user-modifiable */
  0x00, /* 0x55 : not user-modifiable */
  0x00, /* 0x56 : not user-modifiable */
  0x38, /* 0x57 : not user-modifiable */
  0xff, /* 0x58 : not user-modifiable */
  0x01, /* 0x59 : not user-modifiable */
  0x00, /* 0x5a : not user-modifiable */
  0x08, /* 0x5b : not user-modifiable */
  0x00, /* 0x5c : not user-modifiable */
  0x00, /* 0x5d : not user-modifiable */
  0x01, /* 0x5e : not user-modifiable */
  0xcc, /* 0x5f : not user-modifiable */
  0x0f, /* 0x60 : not user-modifiable */
  0x01, /* 0x61 : not user-modifiable */
  0xf1, /* 0x62 : not user-modifiable */
  0x0d, /* 0x63 : not user-modifiable */
  0x01, /* 0x64 : Sigma threshold MSB (mm in 14.2 format for MSB+LSB), use SetSigmaThreshold(), default value 90 mm  */
  0x68, /* 0x65 : Sigma threshold LSB */
  0x00, /* 0x66 : Min count Rate MSB (MCPS in 9.7 format for MSB+LSB), use SetSignalThreshold() */
  0x80, /* 0x67 : Min count Rate LSB */
  0x08, /* 0x68 : not user-modifiable */
  0xb8, /* 0x69 : not user-modifiable */
  0x00, /* 0x6a : not user-modifiable */
  0x00, /* 0x6b : not user-modifiable */
  0x00, /* 0x6c : Intermeasurement period MSB, 32 bits register, use SetIntermeasurementInMs() */
  0x00, /* 0x6d : Intermeasurement period */
  0x0f, /* 0x6e : Intermeasurement period */
  0x89, /* 0x6f : Intermeasurement period LSB */
  0x00, /* 0x70 : not user-modifiable */
  0x00, /* 0x71 : not user-modifiable */
  0x00, /* 0x72 : distance threshold high MSB (in mm, MSB+LSB), use SetD:tanceThreshold() */
  0x00, /* 0x73 : distance threshold high LSB */
  0x00, /* 0x74 : distance threshold low MSB ( in mm, MSB+LSB), use SetD:tanceThreshold() */
  0x00, /* 0x75 : distance threshold low LSB */
  0x00, /* 0x76 : not user-modifiable */
  0x01, /* 0x77 : not user-modifiable */
  0x0f, /* 0x78 : not user-modifiable */
  0x0d, /* 0x79 : not user-modifiable */
  0x0e, /* 0x7a : not user-modifiable */
  0x0e, /* 0x7b : not user-modifiable */
  0x00, /* 0x7c : not user-modifiable */
  0x00, /* 0x7d : not user-modifiable */
  0x02, /* 0x7e : not user-modifiable */
  0xc7, /* 0x7f : ROI center, use SetROI() */
  0xff, /* 0x80 : XY ROI (X=Width, Y=Height), use SetROI() */
  0x9B, /* 0x81 : not user-modifiable */
  0x00, /* 0x82 : not user-modifiable */
  0x00, /* 0x83 : not user-modifiable */
  0x00, /* 0x84 : not user-modifiable */
  0x01, /* 0x85 : not user-modifiable */
  0x00, /* 0x86 : clear interrupt, use ClearInterrupt() */
  0x00  /* 0x87 : start ranging, use StartRanging() or StopRanging(), If you want an automatic start after VL53L1X_init() call, put 0x40 in location 0x87 */
};

static const uint8_t status_rtn[24] = { 255, 255, 255, 5, 2, 4, 1, 7, 3, 0,
                                        255, 255, 9, 13, 255, 255, 255, 255, 10, 6,
                                        255, 255, 11, 12
                                      };

#if VL53L1X_AUTO_INCR_ADDR
// start after default address
static uint8_t auto_incr_addr = VL53L1_DEFAULT_ADDRESS + 2;
#endif

VL53L1X_ERROR 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_SetI2CAddress(VL53L1_DEV dev, uint8_t new_address)
{
  VL53L1X_ERROR status = 0;

  status = VL53L1_WrByte(dev, VL53L1_I2C_SLAVE__DEVICE_ADDRESS, new_address >> 1);
  if (!status) { dev->i2c_trans.slave_addr = new_address; }
  return status;
}

VL53L1X_ERROR VL53L1X_SensorInit(VL53L1_DEV dev)
{
  VL53L1X_ERROR status = 0;
  uint8_t Addr = 0x00, tmp;

  for (Addr = 0x2D; Addr <= 0x87; Addr++) {
    status = VL53L1_WrByte(dev, Addr, VL51L1X_DEFAULT_CONFIGURATION[Addr - 0x2D]);
  }
  status = VL53L1X_StartRanging(dev);
  tmp  = 0;
  while (tmp == 0) {
    status = VL53L1X_CheckForDataReady(dev, &tmp);
  }
  status = VL53L1X_ClearInterrupt(dev);
  status = VL53L1X_StopRanging(dev);
  status = VL53L1_WrByte(dev, VL53L1_VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND, 0x09); /* two bounds VHV */
  status = VL53L1_WrByte(dev, 0x0B, 0); /* start VHV from the previous temperature */
#if VL53L1X_AUTO_INCR_ADDR
  status = VL53L1X_SetI2CAddress(dev, auto_incr_addr);
  auto_incr_addr += 2; // auto increment by 2 (+1 on 7 bits address)
#endif
  return status;
}

void VL53L1X_BootDevice(VL53L1_DEV dev, uint16_t TimingBudgetInMs, uint16_t DistanceMode, uint32_t InterMeasurementInMs)
{
  uint8_t state;
  do {
    VL53L1X_BootState(dev, &state);
  } while (!state);
  VL53L1X_SensorInit(dev);
  /* Configure sensor */
  VL53L1X_SetTimingBudgetInMs(dev, TimingBudgetInMs);
  VL53L1X_SetDistanceMode(dev, DistanceMode);
  VL53L1X_SetInterMeasurementInMs(dev, InterMeasurementInMs);
  /* Start measurement */
  VL53L1X_StartRanging(dev);
}


VL53L1X_ERROR VL53L1X_ClearInterrupt(VL53L1_DEV dev)
{
  VL53L1X_ERROR status = 0;

  status = VL53L1_WrByte(dev, SYSTEM__INTERRUPT_CLEAR, 0x01);
  return status;
}

VL53L1X_ERROR VL53L1X_SetInterruptPolarity(VL53L1_DEV dev, uint8_t NewPolarity)
{
  uint8_t Temp;
  VL53L1X_ERROR status = 0;

  status = VL53L1_RdByte(dev, GPIO_HV_MUX__CTRL, &Temp);
  Temp = Temp & 0xEF;
  status = VL53L1_WrByte(dev, GPIO_HV_MUX__CTRL, Temp | (!(NewPolarity & 1)) << 4);
  return status;
}

VL53L1X_ERROR VL53L1X_GetInterruptPolarity(VL53L1_DEV dev, uint8_t *pInterruptPolarity)
{
  uint8_t Temp;
  VL53L1X_ERROR status = 0;

  status = VL53L1_RdByte(dev, GPIO_HV_MUX__CTRL, &Temp);
  Temp = Temp & 0x10;
  *pInterruptPolarity = !(Temp >> 4);
  return status;
}

VL53L1X_ERROR VL53L1X_StartRanging(VL53L1_DEV dev)
{
  VL53L1X_ERROR status = 0;

  status = VL53L1_WrByte(dev, SYSTEM__MODE_START, 0x40);  /* Enable VL53L1X */
  return status;
}

VL53L1X_ERROR VL53L1X_StopRanging(VL53L1_DEV dev)
{
  VL53L1X_ERROR status = 0;

  status = VL53L1_WrByte(dev, SYSTEM__MODE_START, 0x00);  /* Disable VL53L1X */
  return status;
}

VL53L1X_ERROR VL53L1X_CheckForDataReady(VL53L1_DEV dev, uint8_t *isDataReady)
{
  uint8_t Temp;
  uint8_t IntPol;
  VL53L1X_ERROR status = 0;

  status = VL53L1X_GetInterruptPolarity(dev, &IntPol);
  status = VL53L1_RdByte(dev, 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_SetTimingBudgetInMs(VL53L1_DEV dev, uint16_t TimingBudgetInMs)
{
  uint16_t DM;
  VL53L1X_ERROR  status = 0;

  status = VL53L1X_GetDistanceMode(dev, &DM);
  if (DM == 0) {
    return 1;
  } else if (DM == 1) { /* Short DistanceMode */
    switch (TimingBudgetInMs) {
      case 15: /* only available in short distance mode */
        VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
                      0x01D);
        VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
                      0x0027);
        break;
      case 20:
        VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
                      0x0051);
        VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
                      0x006E);
        break;
      case 33:
        VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
                      0x00D6);
        VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
                      0x006E);
        break;
      case 50:
        VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
                      0x1AE);
        VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
                      0x01E8);
        break;
      case 100:
        VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
                      0x02E1);
        VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
                      0x0388);
        break;
      case 200:
        VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
                      0x03E1);
        VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
                      0x0496);
        break;
      case 500:
        VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
                      0x0591);
        VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
                      0x05C1);
        break;
      default:
        status = 1;
        break;
    }
  } else {
    switch (TimingBudgetInMs) {
      case 20:
        VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
                      0x001E);
        VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
                      0x0022);
        break;
      case 33:
        VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
                      0x0060);
        VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
                      0x006E);
        break;
      case 50:
        VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
                      0x00AD);
        VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
                      0x00C6);
        break;
      case 100:
        VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
                      0x01CC);
        VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
                      0x01EA);
        break;
      case 200:
        VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
                      0x02D9);
        VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
                      0x02F8);
        break;
      case 500:
        VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
                      0x048F);
        VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
                      0x04A4);
        break;
      default:
        status = 1;
        break;
    }
  }
  return status;
}

VL53L1X_ERROR VL53L1X_GetTimingBudgetInMs(VL53L1_DEV dev, uint16_t *pTimingBudget)
{
  uint16_t Temp;
  VL53L1X_ERROR status = 0;

  status = VL53L1_RdWord(dev, 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:
      status = 1;
      *pTimingBudget = 0;
  }
  return status;
}

VL53L1X_ERROR VL53L1X_SetDistanceMode(VL53L1_DEV dev, uint16_t DM)
{
  uint16_t TB;
  VL53L1X_ERROR status = 0;

  status = VL53L1X_GetTimingBudgetInMs(dev, &TB);
  if (status != 0) {
    return 1;
  }
  switch (DM) {
    case 1:
      status = VL53L1_WrByte(dev, PHASECAL_CONFIG__TIMEOUT_MACROP, 0x14);
      status = VL53L1_WrByte(dev, RANGE_CONFIG__VCSEL_PERIOD_A, 0x07);
      status = VL53L1_WrByte(dev, RANGE_CONFIG__VCSEL_PERIOD_B, 0x05);
      status = VL53L1_WrByte(dev, RANGE_CONFIG__VALID_PHASE_HIGH, 0x38);
      status = VL53L1_WrWord(dev, SD_CONFIG__WOI_SD0, 0x0705);
      status = VL53L1_WrWord(dev, SD_CONFIG__INITIAL_PHASE_SD0, 0x0606);
      break;
    case 2:
      status = VL53L1_WrByte(dev, PHASECAL_CONFIG__TIMEOUT_MACROP, 0x0A);
      status = VL53L1_WrByte(dev, RANGE_CONFIG__VCSEL_PERIOD_A, 0x0F);
      status = VL53L1_WrByte(dev, RANGE_CONFIG__VCSEL_PERIOD_B, 0x0D);
      status = VL53L1_WrByte(dev, RANGE_CONFIG__VALID_PHASE_HIGH, 0xB8);
      status = VL53L1_WrWord(dev, SD_CONFIG__WOI_SD0, 0x0F0D);
      status = VL53L1_WrWord(dev, SD_CONFIG__INITIAL_PHASE_SD0, 0x0E0E);
      break;
    default:
      status = 1;
      break;
  }

  if (status == 0) {
    status = VL53L1X_SetTimingBudgetInMs(dev, TB);
  }
  return status;
}

VL53L1X_ERROR VL53L1X_GetDistanceMode(VL53L1_DEV dev, uint16_t *DM)
{
  uint8_t TempDM, status = 0;

  status = VL53L1_RdByte(dev, PHASECAL_CONFIG__TIMEOUT_MACROP, &TempDM);
  if (TempDM == 0x14) {
    *DM = 1;
  }
  if (TempDM == 0x0A) {
    *DM = 2;
  }
  return status;
}

VL53L1X_ERROR VL53L1X_SetInterMeasurementInMs(VL53L1_DEV dev, uint32_t InterMeasMs)
{
  uint16_t ClockPLL;
  VL53L1X_ERROR status = 0;

  status = VL53L1_RdWord(dev, VL53L1_RESULT__OSC_CALIBRATE_VAL, &ClockPLL);
  ClockPLL = ClockPLL & 0x3FF;
  VL53L1_WrDWord(dev, VL53L1_SYSTEM__INTERMEASUREMENT_PERIOD,
                 (uint32_t)(ClockPLL * InterMeasMs * 1.075));
  return status;

}

VL53L1X_ERROR VL53L1X_GetInterMeasurementInMs(VL53L1_DEV dev, uint16_t *pIM)
{
  uint16_t ClockPLL;
  VL53L1X_ERROR status = 0;
  uint32_t tmp;

  status = VL53L1_RdDWord(dev, VL53L1_SYSTEM__INTERMEASUREMENT_PERIOD, &tmp);
  *pIM = (uint16_t)tmp;
  status = VL53L1_RdWord(dev, VL53L1_RESULT__OSC_CALIBRATE_VAL, &ClockPLL);
  ClockPLL = ClockPLL & 0x3FF;
  *pIM = (uint16_t)(*pIM / (ClockPLL * 1.065));
  return status;
}

VL53L1X_ERROR VL53L1X_BootState(VL53L1_DEV dev, uint8_t *state)
{
  VL53L1X_ERROR status = 0;
  uint8_t tmp = 0;

  status = VL53L1_RdByte(dev, VL53L1_FIRMWARE__SYSTEM_STATUS, &tmp);
  *state = tmp;
  return status;
}

VL53L1X_ERROR VL53L1X_GetSensorId(VL53L1_DEV dev, uint16_t *sensorId)
{
  VL53L1X_ERROR status = 0;
  uint16_t tmp = 0;

  status = VL53L1_RdWord(dev, VL53L1_IDENTIFICATION__MODEL_ID, &tmp);
  *sensorId = tmp;
  return status;
}

VL53L1X_ERROR VL53L1X_GetDistance(VL53L1_DEV dev, uint16_t *distance)
{
  VL53L1X_ERROR status = 0;
  uint16_t tmp;

  status = (VL53L1_RdWord(dev,
                          VL53L1_RESULT__FINAL_CROSSTALK_CORRECTED_RANGE_MM_SD0, &tmp));
  *distance = tmp;
  return status;
}

VL53L1X_ERROR VL53L1X_GetSignalPerSpad(VL53L1_DEV dev, uint16_t *signalRate)
{
  VL53L1X_ERROR status = 0;
  uint16_t SpNb = 1, signal;

  status = VL53L1_RdWord(dev,
                         VL53L1_RESULT__PEAK_SIGNAL_COUNT_RATE_CROSSTALK_CORRECTED_MCPS_SD0, &signal);
  status = VL53L1_RdWord(dev,
                         VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0, &SpNb);
  *signalRate = (uint16_t)(2000.0 * signal / SpNb);
  return status;
}

VL53L1X_ERROR VL53L1X_GetAmbientPerSpad(VL53L1_DEV dev, uint16_t *ambPerSp)
{
  VL53L1X_ERROR status = 0;
  uint16_t AmbientRate, SpNb = 1;

  status = VL53L1_RdWord(dev, RESULT__AMBIENT_COUNT_RATE_MCPS_SD, &AmbientRate);
  status = VL53L1_RdWord(dev, VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0, &SpNb);
  *ambPerSp = (uint16_t)(2000.0 * AmbientRate / SpNb);
  return status;
}

VL53L1X_ERROR VL53L1X_GetSignalRate(VL53L1_DEV dev, uint16_t *signal)
{
  VL53L1X_ERROR status = 0;
  uint16_t tmp;

  status = VL53L1_RdWord(dev,
                         VL53L1_RESULT__PEAK_SIGNAL_COUNT_RATE_CROSSTALK_CORRECTED_MCPS_SD0, &tmp);
  *signal = tmp * 8;
  return status;
}

VL53L1X_ERROR VL53L1X_GetSpadNb(VL53L1_DEV dev, uint16_t *spNb)
{
  VL53L1X_ERROR status = 0;
  uint16_t tmp;

  status = VL53L1_RdWord(dev,
                         VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0, &tmp);
  *spNb = tmp >> 8;
  return status;
}

VL53L1X_ERROR VL53L1X_GetAmbientRate(VL53L1_DEV dev, uint16_t *ambRate)
{
  VL53L1X_ERROR status = 0;
  uint16_t tmp;

  status = VL53L1_RdWord(dev, RESULT__AMBIENT_COUNT_RATE_MCPS_SD, &tmp);
  *ambRate = tmp * 8;
  return status;
}

VL53L1X_ERROR VL53L1X_GetRangeStatus(VL53L1_DEV dev, uint8_t *rangeStatus)
{
  VL53L1X_ERROR status = 0;
  uint8_t RgSt;

  *rangeStatus = 255;
  status = VL53L1_RdByte(dev, VL53L1_RESULT__RANGE_STATUS, &RgSt);
  RgSt = RgSt & 0x1F;
  if (RgSt < 24) {
    *rangeStatus = status_rtn[RgSt];
  }
  return status;
}

VL53L1X_ERROR VL53L1X_GetResult(VL53L1_DEV dev, VL53L1X_Result_t *pResult)
{
  VL53L1X_ERROR status = 0;
  uint8_t Temp[17];
  uint8_t RgSt = 255;

  status = VL53L1_ReadMulti(dev, VL53L1_RESULT__RANGE_STATUS, Temp, 17);
  RgSt = Temp[0] & 0x1F;
  if (RgSt < 24) {
    RgSt = status_rtn[RgSt];
  }
  pResult->Status = RgSt;
  pResult->Ambient = (Temp[7] << 8 | Temp[8]) * 8;
  pResult->NumSPADs = Temp[3];
  pResult->SigPerSPAD = (Temp[15] << 8 | Temp[16]) * 8;
  pResult->Distance = Temp[13] << 8 | Temp[14];

  return status;
}

VL53L1X_ERROR VL53L1X_SetOffset(VL53L1_DEV dev, int16_t OffsetValue)
{
  VL53L1X_ERROR status = 0;
  int16_t Temp;

  Temp = (OffsetValue * 4);
  VL53L1_WrWord(dev, ALGO__PART_TO_PART_RANGE_OFFSET_MM,
                (uint16_t)Temp);
  VL53L1_WrWord(dev, MM_CONFIG__INNER_OFFSET_MM, 0x0);
  VL53L1_WrWord(dev, MM_CONFIG__OUTER_OFFSET_MM, 0x0);
  return status;
}

VL53L1X_ERROR  VL53L1X_GetOffset(VL53L1_DEV dev, int16_t *offset)
{
  VL53L1X_ERROR status = 0;
  uint16_t Temp;

  status = VL53L1_RdWord(dev, ALGO__PART_TO_PART_RANGE_OFFSET_MM, &Temp);
  Temp = Temp << 3;
  Temp = Temp >> 5;
  *offset = (int16_t)(Temp);
  return status;
}

VL53L1X_ERROR VL53L1X_SetXtalk(VL53L1_DEV dev, uint16_t XtalkValue)
{
  /* XTalkValue in count per second to avoid float type */
  VL53L1X_ERROR status = 0;

  status = VL53L1_WrWord(dev,
                         ALGO__CROSSTALK_COMPENSATION_X_PLANE_GRADIENT_KCPS,
                         0x0000);
  status = VL53L1_WrWord(dev, ALGO__CROSSTALK_COMPENSATION_Y_PLANE_GRADIENT_KCPS,
                         0x0000);
  status = VL53L1_WrWord(dev, 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_GetXtalk(VL53L1_DEV dev, uint16_t *xtalk)
{
  VL53L1X_ERROR status = 0;
  uint32_t tmp;

  status = VL53L1_RdDWord(dev, ALGO__CROSSTALK_COMPENSATION_PLANE_OFFSET_KCPS, &tmp);
  *xtalk = (uint16_t)(tmp * 1000) >> 9; /* * 1000 to convert kcps to cps and >> 9 (7.9 format) */
  return status;
}

VL53L1X_ERROR VL53L1X_SetDistanceThreshold(VL53L1_DEV dev, uint16_t ThreshLow,
    uint16_t ThreshHigh, uint8_t Window,
    uint8_t IntOnNoTarget)
{
  VL53L1X_ERROR status = 0;
  uint8_t Temp = 0;

  status = VL53L1_RdByte(dev, SYSTEM__INTERRUPT_CONFIG_GPIO, &Temp);
  Temp = Temp & 0x47;
  if (IntOnNoTarget == 0) {
    status = VL53L1_WrByte(dev, SYSTEM__INTERRUPT_CONFIG_GPIO,
                           (Temp | (Window & 0x07)));
  } else {
    status = VL53L1_WrByte(dev, SYSTEM__INTERRUPT_CONFIG_GPIO,
                           ((Temp | (Window & 0x07)) | 0x40));
  }
  status = VL53L1_WrWord(dev, SYSTEM__THRESH_HIGH, ThreshHigh);
  status = VL53L1_WrWord(dev, SYSTEM__THRESH_LOW, ThreshLow);
  return status;
}

VL53L1X_ERROR VL53L1X_GetDistanceThresholdWindow(VL53L1_DEV dev, uint16_t *window)
{
  VL53L1X_ERROR status = 0;
  uint8_t tmp;
  status = VL53L1_RdByte(dev, SYSTEM__INTERRUPT_CONFIG_GPIO, &tmp);
  *window = (uint16_t)(tmp & 0x7);
  return status;
}

VL53L1X_ERROR VL53L1X_GetDistanceThresholdLow(VL53L1_DEV dev, uint16_t *low)
{
  VL53L1X_ERROR status = 0;
  uint16_t tmp;

  status = VL53L1_RdWord(dev, SYSTEM__THRESH_LOW, &tmp);
  *low = tmp;
  return status;
}

VL53L1X_ERROR VL53L1X_GetDistanceThresholdHigh(VL53L1_DEV dev, uint16_t *high)
{
  VL53L1X_ERROR status = 0;
  uint16_t tmp;

  status = VL53L1_RdWord(dev, SYSTEM__THRESH_HIGH, &tmp);
  *high = tmp;
  return status;
}

VL53L1X_ERROR VL53L1X_SetROICenter(VL53L1_DEV dev, uint8_t ROICenter)
{
  VL53L1X_ERROR status = 0;
  status = VL53L1_WrByte(dev, ROI_CONFIG__USER_ROI_CENTRE_SPAD, ROICenter);
  return status;
}

VL53L1X_ERROR VL53L1X_GetROICenter(VL53L1_DEV dev, uint8_t *ROICenter)
{
  VL53L1X_ERROR status = 0;
  uint8_t tmp;
  status = VL53L1_RdByte(dev, ROI_CONFIG__USER_ROI_CENTRE_SPAD, &tmp);
  *ROICenter = tmp;
  return status;
}

VL53L1X_ERROR VL53L1X_SetROI(VL53L1_DEV dev, uint16_t X, uint16_t Y)
{
  uint8_t OpticalCenter;
  VL53L1X_ERROR status = 0;

  status = VL53L1_RdByte(dev, 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(dev, ROI_CONFIG__USER_ROI_CENTRE_SPAD, OpticalCenter);
  status = VL53L1_WrByte(dev, ROI_CONFIG__USER_ROI_REQUESTED_GLOBAL_XY_SIZE,
                         (Y - 1) << 4 | (X - 1));
  return status;
}

VL53L1X_ERROR VL53L1X_GetROI_XY(VL53L1_DEV dev, uint16_t *ROI_X, uint16_t *ROI_Y)
{
  VL53L1X_ERROR status = 0;
  uint8_t tmp;

  status = VL53L1_RdByte(dev, 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_SetSignalThreshold(VL53L1_DEV dev, uint16_t Signal)
{
  VL53L1X_ERROR status = 0;

  VL53L1_WrWord(dev, RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS, Signal >> 3);
  return status;
}

VL53L1X_ERROR VL53L1X_GetSignalThreshold(VL53L1_DEV dev, uint16_t *signal)
{
  VL53L1X_ERROR status = 0;
  uint16_t tmp;

  status = VL53L1_RdWord(dev,
                         RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS, &tmp);
  *signal = tmp << 3;
  return status;
}

VL53L1X_ERROR VL53L1X_SetSigmaThreshold(VL53L1_DEV dev, uint16_t Sigma)
{
  VL53L1X_ERROR status = 0;

  if (Sigma > (0xFFFF >> 2)) {
    return 1;
  }
  /* 16 bits register 14.2 format */
  status = VL53L1_WrWord(dev, RANGE_CONFIG__SIGMA_THRESH, Sigma << 2);
  return status;
}

VL53L1X_ERROR VL53L1X_GetSigmaThreshold(VL53L1_DEV dev, uint16_t *sigma)
{
  VL53L1X_ERROR status = 0;
  uint16_t tmp;

  status = VL53L1_RdWord(dev, RANGE_CONFIG__SIGMA_THRESH, &tmp);
  *sigma = tmp >> 2;
  return status;

}

VL53L1X_ERROR VL53L1X_StartTemperatureUpdate(VL53L1_DEV dev)
{
  VL53L1X_ERROR status = 0;
  uint8_t tmp = 0;

  status = VL53L1_WrByte(dev, VL53L1_VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND, 0x81); /* full VHV */
  status = VL53L1_WrByte(dev, 0x0B, 0x92);
  status = VL53L1X_StartRanging(dev);
  while (tmp == 0) {
    status = VL53L1X_CheckForDataReady(dev, &tmp);
  }
  tmp  = 0;
  status = VL53L1X_ClearInterrupt(dev);
  status = VL53L1X_StopRanging(dev);
  status = VL53L1_WrByte(dev, VL53L1_VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND, 0x09); /* two bounds VHV */
  status = VL53L1_WrByte(dev, 0x0B, 0); /* start VHV from the previous temperature */
  return status;
}