vl53l1x_class.cpp

00001 /**
00002  ******************************************************************************
00003  * @file    vl53l1x_class.cpp
00004  * @author  JS
00005  * @version V0.0.1
00006  * @date    15-January-2019
00007  * @brief   Implementation file for the VL53L1 sensor component driver class
00008  ******************************************************************************
00009  * @attention
00010  *
00011  * <h2><center>&copy; COPYRIGHT(c) 2018 STMicroelectronics</center></h2>
00012  *
00013  * Redistribution and use in source and binary forms, with or without modification,
00014  * are permitted provided that the following conditions are met:
00015  *   1. Redistributions of source code must retain the above copyright notice,
00016  *      this list of conditions and the following disclaimer.
00017  *   2. Redistributions in binary form must reproduce the above copyright notice,
00018  *      this list of conditions and the following disclaimer in the documentation
00019  *      and/or other materials provided with the distribution.
00020  *   3. Neither the name of STMicroelectronics nor the names of its contributors
00021  *      may be used to endorse or promote products derived from this software
00022  *      without specific prior written permission.
00023  *
00024  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00025  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00026  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00027  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
00028  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
00029  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
00030  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
00032  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
00033  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00034  *
00035  ******************************************************************************
00036 */
00037 
00038 /* Includes */
00039 #include <stdlib.h>
00040 #include "vl53l1x_class.h"
00041 
00042 
00043 #define ALGO__PART_TO_PART_RANGE_OFFSET_MM  0x001E
00044 #define MM_CONFIG__INNER_OFFSET_MM          0x0020
00045 #define MM_CONFIG__OUTER_OFFSET_MM          0x0022
00046 
00047 #include "vl53l1x_configuration.h"
00048 
00049 
00050 VL53L1X_ERROR VL53L1X::VL53L1X_GetSWVersion(VL53L1X_Version_t *pVersion)
00051 {
00052     VL53L1X_ERROR Status = 0;
00053 
00054     pVersion->major  = VL53L1X_IMPLEMENTATION_VER_MAJOR;
00055     pVersion->minor  = VL53L1X_IMPLEMENTATION_VER_MINOR;
00056     pVersion->build  = VL53L1X_IMPLEMENTATION_VER_SUB;
00057     pVersion->revision  = VL53L1X_IMPLEMENTATION_VER_REVISION;
00058     return Status;
00059 }
00060 
00061 VL53L1X_ERROR VL53L1X::VL53L1X_SetI2CAddress(uint8_t new_address)
00062 {
00063     VL53L1X_ERROR status = 0;
00064 
00065     Device->I2cDevAddr = new_address;
00066     status = VL53L1_WrByte(Device, VL53L1_I2C_SLAVE__DEVICE_ADDRESS, new_address >> 1);    
00067     return status;
00068 }
00069 
00070 int VL53L1X::init_sensor(uint8_t new_addr)
00071 {
00072     Device->I2cDevAddr = new_addr;
00073     int status = 0;
00074     VL53L1_Off();
00075     VL53L1_On();
00076 
00077     status = is_present();
00078     if (!status) {
00079         printf("Failed to init VL53L0X sensor!\n\r");
00080         return status;
00081     }
00082     return status;
00083 }
00084 
00085 
00086 VL53L1X_ERROR VL53L1X::VL53L1X_SensorInit()
00087 {
00088     VL53L1X_ERROR status = 0;
00089     uint8_t Addr = 0x00;
00090 
00091     for (Addr = 0x2D; Addr <= 0x87; Addr++){
00092         status = VL53L1_WrByte(Device, Addr, VL51L1X_DEFAULT_CONFIGURATION[Addr - 0x2D]);
00093         if (status != 0)
00094         {
00095             printf("Writing config failed - %d\r\n", status);
00096         }
00097     }
00098     
00099     uint16_t sensorID= 0;
00100     status = VL53L1X_GetSensorId(&sensorID);
00101     printf("Sensor id is - %d (%X)\r\n", sensorID, sensorID);
00102     
00103     status = VL53L1X_StartRanging();
00104     if (status != 0)
00105     {
00106         printf("start ranging failed - %d\r\n", status);
00107     }
00108  
00109     status = VL53L1X_ClearInterrupt();
00110     status = VL53L1X_StopRanging();
00111     status = VL53L1_WrByte(Device, VL53L1_VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND, 0x09); /* two bounds VHV */
00112     status = VL53L1_WrByte(Device, 0x0B, 0); /* start VHV from the previous temperature */
00113     return status;
00114 }
00115 
00116 
00117 VL53L1X_ERROR VL53L1X::VL53L1X_ClearInterrupt()
00118 {
00119     VL53L1X_ERROR status = 0;
00120 
00121     status = VL53L1_WrByte(Device, SYSTEM__INTERRUPT_CLEAR, 0x01);
00122     return status;
00123 }
00124 
00125 
00126 VL53L1X_ERROR VL53L1X::VL53L1X_SetInterruptPolarity(uint8_t NewPolarity)
00127 {
00128     uint8_t Temp;
00129     VL53L1X_ERROR status = 0;
00130 
00131     status = VL53L1_RdByte(Device, GPIO_HV_MUX__CTRL, &Temp);
00132     Temp = Temp & 0xEF;
00133     status = VL53L1_WrByte(Device, GPIO_HV_MUX__CTRL, Temp | (!(NewPolarity & 1)) << 4);
00134     return status;
00135 }
00136 
00137 
00138 
00139 VL53L1X_ERROR VL53L1X::VL53L1X_GetInterruptPolarity(uint8_t *pInterruptPolarity)
00140 {
00141     uint8_t Temp;
00142     VL53L1X_ERROR status = 0;
00143 
00144     status = VL53L1_RdByte(Device, GPIO_HV_MUX__CTRL, &Temp);
00145     Temp = Temp & 0x10;
00146     *pInterruptPolarity = !(Temp>>4);
00147     return status;
00148 }
00149 
00150 
00151 
00152 VL53L1X_ERROR VL53L1X::VL53L1X_StartRanging()
00153 {
00154     VL53L1X_ERROR status = 0;
00155 /*
00156     uint8_t Addr = 0x00;
00157 
00158     for (Addr = 0x2D; Addr <= 0x87; Addr++){
00159         status = VL53L1_WrByte(Device, Addr, VL51L1X_DEFAULT_CONFIGURATION[Addr - 0x2D]);
00160     }
00161 */
00162     status = VL53L1_WrByte(Device, SYSTEM__MODE_START, 0x40);   /* Enable VL53L1X */
00163     return status;
00164 }
00165 
00166 VL53L1X_ERROR VL53L1X::VL53L1X_StopRanging()
00167 {
00168     VL53L1X_ERROR status = 0;
00169 
00170     status = VL53L1_WrByte(Device, SYSTEM__MODE_START, 0x00);   /* Disable VL53L1X */
00171     return status;
00172 }
00173 
00174 
00175 
00176 VL53L1X_ERROR VL53L1X::VL53L1X_CheckForDataReady(uint8_t *isDataReady)
00177 {
00178     uint8_t Temp;
00179     uint8_t IntPol;
00180     VL53L1X_ERROR status = 0;
00181 
00182     status = VL53L1X_GetInterruptPolarity(&IntPol);
00183     status = VL53L1_RdByte(Device, GPIO__TIO_HV_STATUS, &Temp);
00184     /* Read in the register to check if a new value is available */
00185     if (status == 0){
00186         if ((Temp & 1) == IntPol)
00187             *isDataReady = 1;
00188         else
00189             *isDataReady = 0;
00190     }
00191     return status;
00192 }
00193 
00194 
00195 VL53L1X_ERROR VL53L1X::VL53L1X_SetTimingBudgetInMs(uint16_t TimingBudgetInMs)
00196 {
00197     uint16_t DM;
00198     VL53L1X_ERROR  status=0;
00199 
00200     status = VL53L1X_GetDistanceMode(&DM);
00201     if (DM == 0)
00202         return 1;
00203     else if (DM == 1) { /* Short DistanceMode */
00204         switch (TimingBudgetInMs) {
00205         case 15: /* only available in short distance mode */
00206             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
00207                     0x01D);
00208             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
00209                     0x0027);
00210             break;
00211         case 20:
00212             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
00213                     0x0051);
00214             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
00215                     0x006E);
00216             break;
00217         case 33:
00218             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
00219                     0x00D6);
00220             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
00221                     0x006E);
00222             break;
00223         case 50:
00224             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
00225                     0x1AE);
00226             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
00227                     0x01E8);
00228             break;
00229         case 100:
00230             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
00231                     0x02E1);
00232             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
00233                     0x0388);
00234             break;
00235         case 200:
00236             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
00237                     0x03E1);
00238             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
00239                     0x0496);
00240             break;
00241         case 500:
00242             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
00243                     0x0591);
00244             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
00245                     0x05C1);
00246             break;
00247         default:
00248             status = 1;
00249             break;
00250         }
00251     } else {
00252         switch (TimingBudgetInMs) {
00253         case 20:
00254             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
00255                     0x001E);
00256             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
00257                     0x0022);
00258             break;
00259         case 33:
00260             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
00261                     0x0060);
00262             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
00263                     0x006E);
00264             break;
00265         case 50:
00266             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
00267                     0x00AD);
00268             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
00269                     0x00C6);
00270             break;
00271         case 100:
00272             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
00273                     0x01CC);
00274             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
00275                     0x01EA);
00276             break;
00277         case 200:
00278             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
00279                     0x02D9);
00280             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
00281                     0x02F8);
00282             break;
00283         case 500:
00284             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
00285                     0x048F);
00286             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
00287                     0x04A4);
00288             break;
00289         default:
00290             status = 1;
00291             break;
00292         }
00293     }
00294     return status;
00295 }
00296 
00297 VL53L1X_ERROR VL53L1X::VL53L1X_GetTimingBudgetInMs(uint16_t *pTimingBudget)
00298 {
00299     uint16_t Temp;
00300     VL53L1X_ERROR status = 0;
00301 
00302     status = VL53L1_RdWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, &Temp);
00303     switch (Temp) {
00304         case 0x001D :
00305             *pTimingBudget = 15;
00306             break;
00307         case 0x0051 :
00308         case 0x001E :
00309             *pTimingBudget = 20;
00310             break;
00311         case 0x00D6 :
00312         case 0x0060 :
00313             *pTimingBudget = 33;
00314             break;
00315         case 0x1AE :
00316         case 0x00AD :
00317             *pTimingBudget = 50;
00318             break;
00319         case 0x02E1 :
00320         case 0x01CC :
00321             *pTimingBudget = 100;
00322             break;
00323         case 0x03E1 :
00324         case 0x02D9 :
00325             *pTimingBudget = 200;
00326             break;
00327         case 0x0591 :
00328         case 0x048F :
00329             *pTimingBudget = 500;
00330             break;
00331         default:
00332             *pTimingBudget = 0;
00333             break;
00334     }
00335     return status;
00336 }
00337 
00338 
00339 VL53L1X_ERROR VL53L1X::VL53L1X_SetDistanceMode(uint16_t DM)
00340 {
00341     uint16_t TB;
00342     VL53L1X_ERROR status = 0;
00343 
00344     status = VL53L1X_GetTimingBudgetInMs(&TB);
00345     
00346     
00347     switch (DM) {
00348     case 1:
00349         status = VL53L1_WrByte(Device, PHASECAL_CONFIG__TIMEOUT_MACROP, 0x14);
00350         status = VL53L1_WrByte(Device, RANGE_CONFIG__VCSEL_PERIOD_A, 0x07);
00351         status = VL53L1_WrByte(Device, RANGE_CONFIG__VCSEL_PERIOD_B, 0x05);
00352         status = VL53L1_WrByte(Device, RANGE_CONFIG__VALID_PHASE_HIGH, 0x38);
00353         status = VL53L1_WrWord(Device, SD_CONFIG__WOI_SD0, 0x0705);
00354         status = VL53L1_WrWord(Device, SD_CONFIG__INITIAL_PHASE_SD0, 0x0606);
00355         break;
00356     case 2:
00357         status = VL53L1_WrByte(Device, PHASECAL_CONFIG__TIMEOUT_MACROP, 0x0A);
00358         status = VL53L1_WrByte(Device, RANGE_CONFIG__VCSEL_PERIOD_A, 0x0F);
00359         status = VL53L1_WrByte(Device, RANGE_CONFIG__VCSEL_PERIOD_B, 0x0D);
00360         status = VL53L1_WrByte(Device, RANGE_CONFIG__VALID_PHASE_HIGH, 0xB8);
00361         status = VL53L1_WrWord(Device, SD_CONFIG__WOI_SD0, 0x0F0D);
00362         status = VL53L1_WrWord(Device, SD_CONFIG__INITIAL_PHASE_SD0, 0x0E0E);
00363         break;
00364     default:
00365         break;
00366     }
00367     status = VL53L1X_SetTimingBudgetInMs(TB);
00368     return status;
00369 }
00370 
00371 
00372 
00373 
00374 VL53L1X_ERROR VL53L1X::VL53L1X_GetDistanceMode(uint16_t *DM)
00375 {
00376     uint8_t TempDM, status=0;
00377 
00378     status = VL53L1_RdByte(Device,PHASECAL_CONFIG__TIMEOUT_MACROP, &TempDM);
00379     if (TempDM == 0x14)
00380         *DM=1;
00381     if(TempDM == 0x0A)
00382         *DM=2;
00383     return status;
00384 }
00385 
00386 
00387 
00388 VL53L1X_ERROR VL53L1X::VL53L1X_SetInterMeasurementInMs(uint16_t InterMeasMs)
00389 {
00390     uint16_t ClockPLL;
00391     VL53L1X_ERROR status = 0;
00392 
00393     status = VL53L1_RdWord(Device, VL53L1_RESULT__OSC_CALIBRATE_VAL, &ClockPLL);
00394     ClockPLL = ClockPLL&0x3FF;
00395     VL53L1_WrDWord(Device, VL53L1_SYSTEM__INTERMEASUREMENT_PERIOD,
00396             (uint32_t)(ClockPLL * InterMeasMs * 1.075));
00397     return status;
00398 
00399 }
00400 
00401 
00402 VL53L1X_ERROR VL53L1X::VL53L1X_GetInterMeasurementInMs(uint16_t *pIM)
00403 {
00404     uint16_t ClockPLL;
00405     VL53L1X_ERROR status = 0;
00406     uint32_t tmp;
00407 
00408     status = VL53L1_RdDWord(Device,VL53L1_SYSTEM__INTERMEASUREMENT_PERIOD, &tmp);
00409     *pIM = (uint16_t)tmp;
00410     status = VL53L1_RdWord(Device, VL53L1_RESULT__OSC_CALIBRATE_VAL, &ClockPLL);
00411     ClockPLL = ClockPLL&0x3FF;
00412     *pIM= (uint16_t)(*pIM/(ClockPLL*1.065));
00413     return status;
00414 }
00415 
00416 
00417 VL53L1X_ERROR VL53L1X::VL53L1X_BootState(uint8_t *state)
00418 {
00419     VL53L1X_ERROR status = 0;
00420     uint8_t tmp = 0;
00421 
00422     status = VL53L1_RdByte(Device,VL53L1_FIRMWARE__SYSTEM_STATUS, &tmp);
00423     *state = tmp;
00424     return status;
00425 }
00426 
00427 
00428 VL53L1X_ERROR VL53L1X::VL53L1X_GetSensorId(uint16_t *sensorId)
00429 {
00430     VL53L1X_ERROR status = 0;
00431     uint16_t tmp = 0;
00432 
00433     status = VL53L1_RdWord(Device, VL53L1_IDENTIFICATION__MODEL_ID, &tmp);
00434     *sensorId = tmp;
00435     return status;
00436 }
00437 
00438 
00439 VL53L1X_ERROR VL53L1X::VL53L1X_GetDistance(uint16_t *distance)
00440 {
00441     VL53L1X_ERROR status = 0;
00442     uint16_t tmp;
00443 
00444     status = (VL53L1_RdWord(Device,
00445             VL53L1_RESULT__FINAL_CROSSTALK_CORRECTED_RANGE_MM_SD0, &tmp));
00446     *distance = tmp;
00447     return status;
00448 }
00449 
00450 VL53L1X_ERROR VL53L1X::VL53L1X_GetSignalPerSpad(uint16_t *signalRate)
00451 {
00452     VL53L1X_ERROR status = 0;
00453     uint16_t SpNb=1, signal;
00454 
00455     status = VL53L1_RdWord(Device,
00456         VL53L1_RESULT__PEAK_SIGNAL_COUNT_RATE_CROSSTALK_CORRECTED_MCPS_SD0, &signal);
00457     status = VL53L1_RdWord(Device,
00458         VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0, &SpNb);
00459     *signalRate = (uint16_t) (2000.0*signal/SpNb);
00460     return status;
00461 }
00462 
00463 
00464 VL53L1X_ERROR VL53L1X::VL53L1X_GetAmbientPerSpad(uint16_t *ambPerSp)
00465 {
00466     VL53L1X_ERROR status=0;
00467     uint16_t AmbientRate, SpNb=1;
00468 
00469     status = VL53L1_RdWord(Device, RESULT__AMBIENT_COUNT_RATE_MCPS_SD, &AmbientRate);
00470     status = VL53L1_RdWord(Device, VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0, &SpNb);
00471     *ambPerSp=(uint16_t) (2000.0 * AmbientRate / SpNb);
00472     return status;
00473 }
00474 
00475 
00476 VL53L1X_ERROR VL53L1X::VL53L1X_GetSignalRate(uint16_t *signal)
00477 {
00478     VL53L1X_ERROR status = 0;
00479     uint16_t tmp;
00480 
00481     status = VL53L1_RdWord(Device,
00482         VL53L1_RESULT__PEAK_SIGNAL_COUNT_RATE_CROSSTALK_CORRECTED_MCPS_SD0, &tmp);
00483     *signal = tmp*8;
00484     return status;
00485 }
00486 
00487 
00488 VL53L1X_ERROR VL53L1X::VL53L1X_GetSpadNb(uint16_t *spNb)
00489 {
00490     VL53L1X_ERROR status = 0;
00491     uint16_t tmp;
00492 
00493     status = VL53L1_RdWord(Device,
00494                   VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0, &tmp);
00495     *spNb = tmp >> 8;
00496     return status;
00497 }
00498 
00499 
00500 VL53L1X_ERROR VL53L1X::VL53L1X_GetAmbientRate(uint16_t *ambRate)
00501 {
00502     VL53L1X_ERROR status = 0;
00503     uint16_t tmp;
00504 
00505     status = VL53L1_RdWord(Device, RESULT__AMBIENT_COUNT_RATE_MCPS_SD, &tmp);
00506     *ambRate = tmp*8;
00507     return status;
00508 }
00509 
00510 
00511 VL53L1X_ERROR VL53L1X::VL53L1X_GetRangeStatus(uint8_t *rangeStatus)
00512 {
00513     VL53L1X_ERROR status = 0;
00514     uint8_t RgSt;
00515 
00516     status = VL53L1_RdByte(Device, VL53L1_RESULT__RANGE_STATUS, &RgSt);
00517     RgSt = RgSt&0x1F;
00518     switch (RgSt) {
00519     case 9:
00520         RgSt = 0;
00521         break;
00522     case 6:
00523         RgSt = 1;
00524         break;
00525     case 4:
00526         RgSt = 2;
00527         break;
00528     case 8:
00529         RgSt = 3;
00530         break;
00531     case 5:
00532         RgSt = 4;
00533         break;
00534     case 3:
00535         RgSt = 5;
00536         break;
00537     case 19:
00538         RgSt = 6;
00539         break;
00540     case 7:
00541         RgSt = 7;
00542         break;
00543     case 12:
00544         RgSt = 9;
00545         break;
00546     case 18:
00547         RgSt = 10;
00548         break;
00549     case 22:
00550         RgSt = 11;
00551         break;
00552     case 23:
00553         RgSt = 12;
00554         break;
00555     case 13:
00556         RgSt = 13;
00557         break;
00558     default:
00559         RgSt = 255;
00560         break;
00561     }
00562     *rangeStatus = RgSt;
00563     return status;
00564 }
00565 
00566 
00567 VL53L1X_ERROR VL53L1X::VL53L1X_SetOffset(int16_t OffsetValue)
00568 {
00569     VL53L1X_ERROR status = 0;
00570     int16_t Temp;
00571 
00572     Temp = (OffsetValue*4);
00573     VL53L1_WrWord(Device, ALGO__PART_TO_PART_RANGE_OFFSET_MM,
00574             (uint16_t)Temp);
00575     VL53L1_WrWord(Device, MM_CONFIG__INNER_OFFSET_MM, 0x0);
00576     VL53L1_WrWord(Device, MM_CONFIG__OUTER_OFFSET_MM, 0x0);
00577     return status;
00578 }
00579 
00580 
00581 VL53L1X_ERROR  VL53L1X::VL53L1X_GetOffset(int16_t *offset)
00582 {
00583     VL53L1X_ERROR status = 0;
00584     uint16_t Temp;
00585 
00586     status = VL53L1_RdWord(Device,ALGO__PART_TO_PART_RANGE_OFFSET_MM, &Temp);
00587     Temp = Temp<<3;
00588     Temp = Temp >>5;
00589     *offset = (int16_t)(Temp);
00590     return status;
00591 }
00592 
00593 VL53L1X_ERROR VL53L1X::VL53L1X_SetXtalk(uint16_t XtalkValue)
00594 {
00595 /* XTalkValue in count per second to avoid float type */
00596     VL53L1X_ERROR status = 0;
00597 
00598     status = VL53L1_WrWord(Device,
00599             ALGO__CROSSTALK_COMPENSATION_X_PLANE_GRADIENT_KCPS,
00600             0x0000);
00601     status = VL53L1_WrWord(Device, ALGO__CROSSTALK_COMPENSATION_Y_PLANE_GRADIENT_KCPS,
00602             0x0000);
00603     status = VL53L1_WrWord(Device, ALGO__CROSSTALK_COMPENSATION_PLANE_OFFSET_KCPS,
00604             (XtalkValue<<9)/1000); /* * << 9 (7.9 format) and /1000 to convert cps to kpcs */
00605     return status;
00606 }
00607 
00608 
00609 VL53L1X_ERROR VL53L1X::VL53L1X_GetXtalk(uint16_t *xtalk )
00610 {
00611     VL53L1X_ERROR status = 0;
00612     uint16_t tmp;
00613 
00614     status = VL53L1_RdWord(Device,ALGO__CROSSTALK_COMPENSATION_PLANE_OFFSET_KCPS, &tmp);
00615     *xtalk = (tmp*1000)>>9; /* * 1000 to convert kcps to cps and >> 9 (7.9 format) */
00616     return status;
00617 }
00618 
00619 
00620 VL53L1X_ERROR VL53L1X::VL53L1X_SetDistanceThreshold(uint16_t ThreshLow,
00621                   uint16_t ThreshHigh, uint8_t Window,
00622                   uint8_t IntOnNoTarget)
00623 {
00624     VL53L1X_ERROR status = 0;
00625     uint8_t Temp = 0;
00626 
00627     status = VL53L1_RdByte(Device, SYSTEM__INTERRUPT_CONFIG_GPIO, &Temp);
00628     Temp = Temp & 0x47;
00629     if (IntOnNoTarget == 0) {
00630         status = VL53L1_WrByte(Device, SYSTEM__INTERRUPT_CONFIG_GPIO,
00631                    (Temp | (Window & 0x07)));
00632     } else {
00633         status = VL53L1_WrByte(Device, SYSTEM__INTERRUPT_CONFIG_GPIO,
00634                    ((Temp | (Window & 0x07)) | 0x40));
00635     }
00636     status = VL53L1_WrWord(Device, SYSTEM__THRESH_HIGH, ThreshHigh);
00637     status = VL53L1_WrWord(Device, SYSTEM__THRESH_LOW, ThreshLow);
00638     return status;
00639 }
00640 
00641 
00642 VL53L1X_ERROR VL53L1X::VL53L1X_GetDistanceThresholdWindow(uint16_t *window)
00643 {
00644     VL53L1X_ERROR status = 0;
00645     uint8_t tmp;
00646     status = VL53L1_RdByte(Device,SYSTEM__INTERRUPT_CONFIG_GPIO, &tmp);
00647     *window = (uint16_t)(tmp & 0x7);
00648     return status;
00649 }
00650 
00651 
00652 VL53L1X_ERROR VL53L1X::VL53L1X_GetDistanceThresholdLow(uint16_t *low)
00653 {
00654     VL53L1X_ERROR status = 0;
00655     uint16_t tmp;
00656 
00657     status = VL53L1_RdWord(Device,SYSTEM__THRESH_LOW, &tmp);
00658     *low = tmp;
00659     return status;
00660 }
00661 
00662 VL53L1X_ERROR VL53L1X::VL53L1X_GetDistanceThresholdHigh(uint16_t *high)
00663 {
00664     VL53L1X_ERROR status = 0;
00665     uint16_t tmp;
00666 
00667     status = VL53L1_RdWord(Device,SYSTEM__THRESH_HIGH, &tmp);
00668     *high = tmp;
00669     return status;
00670 }
00671 
00672 VL53L1X_ERROR VL53L1X::VL53L1X_SetROI(uint16_t X, uint16_t Y)
00673 {
00674     uint8_t OpticalCenter;
00675     VL53L1X_ERROR status = 0;
00676 
00677     status =VL53L1_RdByte(Device, VL53L1_ROI_CONFIG__MODE_ROI_CENTRE_SPAD, &OpticalCenter);
00678     if (X > 16)
00679         X = 16;
00680     if (Y > 16)
00681         Y = 16;
00682     if (X > 10 || Y > 10){
00683         OpticalCenter = 199;
00684     }
00685     status = VL53L1_WrByte(Device, ROI_CONFIG__USER_ROI_CENTRE_SPAD, OpticalCenter);
00686     status = VL53L1_WrByte(Device, ROI_CONFIG__USER_ROI_REQUESTED_GLOBAL_XY_SIZE,
00687                (Y - 1) << 4 | (X - 1));
00688     return status;
00689 }
00690 
00691 VL53L1X_ERROR VL53L1X::VL53L1X_GetROI_XY(uint16_t *ROI_X, uint16_t *ROI_Y)
00692 {
00693     VL53L1X_ERROR status = 0;
00694     uint8_t tmp;
00695 
00696     status = VL53L1_RdByte(Device,ROI_CONFIG__USER_ROI_REQUESTED_GLOBAL_XY_SIZE, &tmp);
00697     *ROI_X = ((uint16_t)tmp & 0x0F) + 1;
00698     *ROI_Y = (((uint16_t)tmp & 0xF0) >> 4) + 1;
00699     return status;
00700 }
00701 
00702 VL53L1X_ERROR VL53L1X::VL53L1X_SetSignalThreshold(uint16_t Signal)
00703 {
00704     VL53L1X_ERROR status = 0;
00705 
00706     VL53L1_WrWord(Device,RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS,Signal>>3);
00707     return status;
00708 }
00709 
00710 VL53L1X_ERROR VL53L1X::VL53L1X_GetSignalThreshold(uint16_t *signal)
00711 {
00712     VL53L1X_ERROR status = 0;
00713     uint16_t tmp;
00714 
00715     status = VL53L1_RdWord(Device,
00716                 RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS, &tmp);
00717     *signal = tmp <<3;
00718     return status;
00719 }
00720 
00721 
00722 VL53L1X_ERROR VL53L1X::VL53L1X_SetSigmaThreshold(uint16_t Sigma)
00723 {
00724     VL53L1X_ERROR status = 0;
00725 
00726     if(Sigma>(0xFFFF>>2)){
00727         return 1;
00728     }
00729     /* 16 bits register 14.2 format */
00730     status = VL53L1_WrWord(Device,RANGE_CONFIG__SIGMA_THRESH,Sigma<<2);
00731     return status;
00732 }
00733 
00734 VL53L1X_ERROR VL53L1X::VL53L1X_GetSigmaThreshold(uint16_t *sigma)
00735 {
00736     VL53L1X_ERROR status = 0;
00737     uint16_t tmp;
00738 
00739     status = VL53L1_RdWord(Device,RANGE_CONFIG__SIGMA_THRESH, &tmp);
00740     *sigma = tmp >> 2;
00741     return status;
00742 
00743 }
00744 
00745 VL53L1X_ERROR VL53L1X::VL53L1X_StartTemperatureUpdate()
00746 {
00747     VL53L1X_ERROR status = 0;
00748     uint8_t tmp=0;
00749 
00750     status = VL53L1_WrByte(Device,VL53L1_VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND,0x81); /* full VHV */
00751     status = VL53L1_WrByte(Device,0x0B,0x92);
00752     status = VL53L1X_StartRanging();
00753     while(tmp==0){
00754         status = VL53L1X_CheckForDataReady(&tmp);
00755     }
00756     tmp  = 0;
00757     status = VL53L1X_ClearInterrupt();
00758     status = VL53L1X_StopRanging();
00759     status = VL53L1_WrByte(Device, VL53L1_VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND, 0x09); /* two bounds VHV */
00760     status = VL53L1_WrByte(Device, 0x0B, 0); /* start VHV from the previous temperature */
00761     return status;
00762 }
00763 
00764     /* VL53L1X_calibration.h functions */
00765     
00766 int8_t VL53L1X::VL53L1X_CalibrateOffset(uint16_t TargetDistInMm, int16_t *offset)
00767 {
00768     uint8_t i = 0, tmp;
00769     int16_t AverageDistance = 0;
00770     uint16_t distance;
00771     VL53L1X_ERROR status = 0;
00772 
00773     status = VL53L1_WrWord(Device, ALGO__PART_TO_PART_RANGE_OFFSET_MM, 0x0);
00774     status = VL53L1_WrWord(Device, MM_CONFIG__INNER_OFFSET_MM, 0x0);
00775     status = VL53L1_WrWord(Device, MM_CONFIG__OUTER_OFFSET_MM, 0x0);
00776     status = VL53L1X_StartRanging();    /* Enable VL53L1X sensor */
00777     for (i = 0; i < 50; i++) {
00778         while (tmp == 0){
00779             status = VL53L1X_CheckForDataReady(&tmp);
00780         }
00781         tmp = 0;
00782         status = VL53L1X_GetDistance(&distance);
00783         status = VL53L1X_ClearInterrupt();
00784         AverageDistance = AverageDistance + distance;
00785     }
00786     status = VL53L1X_StopRanging();
00787     AverageDistance = AverageDistance / 50;
00788     *offset = TargetDistInMm - AverageDistance;
00789     status = VL53L1_WrWord(Device, ALGO__PART_TO_PART_RANGE_OFFSET_MM, *offset*4);
00790     return status;
00791 }
00792 
00793 
00794 int8_t VL53L1X::VL53L1X_CalibrateXtalk(uint16_t TargetDistInMm, uint16_t *xtalk)
00795 {
00796     uint8_t i, tmp= 0;
00797     float AverageSignalRate = 0;
00798     float AverageDistance = 0;
00799     float AverageSpadNb = 0;
00800     uint16_t distance = 0, spadNum;
00801     uint16_t sr;
00802     VL53L1X_ERROR status = 0;
00803 
00804     status = VL53L1_WrWord(Device, 0x0016,0);
00805     status = VL53L1X_StartRanging();
00806     for (i = 0; i < 50; i++) {
00807         while (tmp == 0){
00808             status = VL53L1X_CheckForDataReady(&tmp);
00809         }
00810         tmp=0;
00811         status= VL53L1X_GetSignalRate(&sr);
00812         status= VL53L1X_GetDistance(&distance);
00813         status = VL53L1X_ClearInterrupt();
00814         AverageDistance = AverageDistance + distance;
00815         status = VL53L1X_GetSpadNb(&spadNum);
00816         AverageSpadNb = AverageSpadNb + spadNum;
00817         AverageSignalRate =
00818             AverageSignalRate + sr;
00819     }
00820     status = VL53L1X_StopRanging();
00821     AverageDistance = AverageDistance / 50;
00822     AverageSpadNb = AverageSpadNb / 50;
00823     AverageSignalRate = AverageSignalRate / 50;
00824     /* Calculate Xtalk value */
00825     *xtalk = (uint16_t)(512*(AverageSignalRate*(1-(AverageDistance/TargetDistInMm)))/AverageSpadNb);
00826     status = VL53L1_WrWord(Device, 0x0016, *xtalk);
00827     return status;
00828 }
00829 
00830 
00831 
00832 
00833     /* Write and read functions from I2C */
00834 
00835 
00836 VL53L1X_ERROR VL53L1X::VL53L1_WriteMulti(VL53L1_DEV Dev, uint16_t index, uint8_t *pdata, uint32_t count)
00837 {
00838    int  status;
00839 
00840    status = VL53L1_I2CWrite(Dev->I2cDevAddr, index, pdata, (uint16_t)count);
00841    return status;
00842 }
00843 
00844 VL53L1X_ERROR VL53L1X::VL53L1_ReadMulti(VL53L1_DEV Dev, uint16_t index, uint8_t *pdata, uint32_t count)
00845 {
00846     int status;
00847 
00848     status = VL53L1_I2CRead(Dev->I2cDevAddr, index, pdata, (uint16_t)count);
00849 
00850     return status;
00851 }
00852 
00853 
00854 VL53L1X_ERROR VL53L1X::VL53L1_WrByte(VL53L1_DEV Dev, uint16_t index, uint8_t data)
00855 {
00856    int  status;
00857 
00858    status=VL53L1_I2CWrite(Dev->I2cDevAddr, index, &data, 1);
00859    return status;
00860 }
00861 
00862 VL53L1X_ERROR VL53L1X::VL53L1_WrWord(VL53L1_DEV Dev, uint16_t index, uint16_t data)
00863 {
00864    int  status;
00865    uint8_t buffer[2];
00866 
00867      buffer[0] = data >> 8;
00868      buffer[1] = data & 0x00FF;
00869    status=VL53L1_I2CWrite(Dev->I2cDevAddr, index, (uint8_t *)buffer, 2);
00870    return status;
00871 }
00872 
00873 VL53L1X_ERROR VL53L1X::VL53L1_WrDWord(VL53L1_DEV Dev, uint16_t index, uint32_t data)
00874 {
00875    int  status;
00876    uint8_t buffer[4];
00877 
00878      buffer[0] = (data >> 24) & 0xFF;
00879      buffer[1] = (data >> 16) & 0xFF;
00880      buffer[2] = (data >>  8) & 0xFF;
00881      buffer[3] = (data >>  0) & 0xFF;
00882    status=VL53L1_I2CWrite(Dev->I2cDevAddr, index, (uint8_t *)buffer, 4);
00883    return status;
00884 }
00885 
00886 
00887 VL53L1X_ERROR VL53L1X::VL53L1_RdByte(VL53L1_DEV Dev, uint16_t index, uint8_t *data)
00888 {
00889    int  status;
00890 
00891    status = VL53L1_I2CRead(Dev->I2cDevAddr, index, data, 1);
00892 
00893    if(status)
00894      return -1;
00895 
00896    return 0;
00897 }
00898 
00899 VL53L1X_ERROR VL53L1X::VL53L1_RdWord(VL53L1_DEV Dev, uint16_t index, uint16_t *data)
00900 {
00901    int  status;
00902    uint8_t buffer[2] = {0,0};
00903 
00904    status = VL53L1_I2CRead(Dev->I2cDevAddr, index, buffer, 2);
00905    if (!status)
00906    {
00907        *data = (buffer[0] << 8) + buffer[1];
00908    }
00909    return status;
00910 
00911 }
00912 
00913 VL53L1X_ERROR VL53L1X::VL53L1_RdDWord(VL53L1_DEV Dev, uint16_t index, uint32_t *data)
00914 {
00915    int status;
00916    uint8_t buffer[4] = {0,0,0,0};
00917 
00918    status = VL53L1_I2CRead(Dev->I2cDevAddr, index, buffer, 4);
00919    if(!status)
00920    {
00921        *data = (buffer[0] << 24) + (buffer[1] << 16) + (buffer[2] << 8) + buffer[3];
00922    }
00923    return status;
00924 
00925 }
00926 
00927 VL53L1X_ERROR VL53L1X::VL53L1_UpdateByte(VL53L1_DEV Dev, uint16_t index, uint8_t AndData, uint8_t OrData)
00928 {
00929    int  status;
00930    uint8_t buffer = 0;
00931 
00932    /* read data direct onto buffer */
00933    status = VL53L1_I2CRead(Dev->I2cDevAddr, index, &buffer,1);
00934    if (!status)
00935    {
00936       buffer = (buffer & AndData) | OrData;
00937       status = VL53L1_I2CWrite(Dev->I2cDevAddr, index, &buffer, (uint16_t)1);
00938    }
00939    return status;
00940 }
00941 
00942 VL53L1X_ERROR VL53L1X::VL53L1_I2CWrite(uint8_t DeviceAddr, uint16_t RegisterAddr, uint8_t* pBuffer, uint16_t NumByteToWrite)
00943 {
00944     int ret;
00945     ret = dev_i2c->v53l1x_i2c_write(pBuffer, DeviceAddr, RegisterAddr, NumByteToWrite);
00946     if (ret) {
00947         return -1;
00948     }
00949     return 0;
00950 
00951 }
00952 
00953 VL53L1X_ERROR VL53L1X::VL53L1_I2CRead(uint8_t DeviceAddr, uint16_t RegisterAddr, uint8_t* pBuffer, uint16_t NumByteToRead)
00954 {
00955     int ret;
00956 
00957     ret = dev_i2c->v53l1x_i2c_read(pBuffer, DeviceAddr, RegisterAddr, NumByteToRead);
00958     //ret = dev_i2c->i2c_read(pBuffer, DeviceAddr, RegisterAddr, NumByteToRead);
00959 
00960     if (ret) {
00961         return -1;
00962     }
00963     return 0;
00964 }
00965 
00966 
00967 VL53L1X_ERROR VL53L1X::VL53L1_GetTickCount(
00968     uint32_t *ptick_count_ms)
00969 {
00970 
00971     /* Returns current tick count in [ms] */
00972 
00973     VL53L1X_ERROR status  = VL53L1_ERROR_NONE;
00974 
00975     //*ptick_count_ms = timeGetTime();
00976     *ptick_count_ms = 0;
00977 
00978     return status;
00979 }
00980 
00981 
00982 
00983 VL53L1X_ERROR VL53L1X::VL53L1_WaitUs(VL53L1_Dev_t *pdev, int32_t wait_us)
00984 {
00985     //(void)pdev;
00986     //wait_ms(wait_us/1000);
00987     return VL53L1_ERROR_NONE;
00988 }
00989 
00990 
00991 VL53L1X_ERROR VL53L1X::VL53L1_WaitMs(VL53L1_Dev_t *pdev, int32_t wait_ms)
00992 {
00993     //(void)pdev;
00994     //wait_ms(wait_ms);
00995     return VL53L1_ERROR_NONE;
00996 }
00997 
00998 
00999 VL53L1X_ERROR VL53L1X::VL53L1_WaitValueMaskEx(
01000     VL53L1_Dev_t *pdev,
01001     uint32_t      timeout_ms,
01002     uint16_t      index,
01003     uint8_t       value,
01004     uint8_t       mask,
01005     uint32_t      poll_delay_ms)
01006 {
01007 
01008     /*
01009      * Platform implementation of WaitValueMaskEx V2WReg script command
01010      *
01011      * WaitValueMaskEx(
01012      *          duration_ms,
01013      *          index,
01014      *          value,
01015      *          mask,
01016      *          poll_delay_ms);
01017      */
01018 
01019     VL53L1_Error status         = VL53L1_ERROR_NONE;
01020     uint32_t     start_time_ms = 0;
01021     uint32_t     current_time_ms = 0;
01022     uint32_t     polling_time_ms = 0;
01023     uint8_t      byte_value      = 0;
01024     uint8_t      found           = 0;
01025 
01026 
01027 
01028     /* calculate time limit in absolute time */
01029 
01030      VL53L1_GetTickCount(&start_time_ms);
01031 
01032     /* remember current trace functions and temporarily disable
01033      * function logging
01034      */
01035 
01036 
01037     /* wait until value is found, timeout reached on error occurred */
01038 
01039     while ((status == VL53L1_ERROR_NONE) &&
01040            (polling_time_ms < timeout_ms) &&
01041            (found == 0)) {
01042 
01043         if (status == VL53L1_ERROR_NONE)
01044             status = VL53L1_RdByte(
01045                             pdev,
01046                             index,
01047                             &byte_value);
01048 
01049         if ((byte_value & mask) == value)
01050             found = 1;
01051 
01052         if (status == VL53L1_ERROR_NONE  &&
01053             found == 0 &&
01054             poll_delay_ms > 0)
01055             status = VL53L1_WaitMs(
01056                     pdev,
01057                     poll_delay_ms);
01058 
01059         /* Update polling time (Compare difference rather than absolute to
01060         negate 32bit wrap around issue) */
01061         VL53L1_GetTickCount(&current_time_ms);
01062         polling_time_ms = current_time_ms - start_time_ms;
01063 
01064     }
01065     
01066 
01067     if (found == 0 && status == VL53L1_ERROR_NONE)
01068         status = VL53L1_ERROR_TIME_OUT;
01069 
01070     return status;
01071 }
01072 
01073 
01074