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     status = VL53L1_WrByte(Device, VL53L1_I2C_SLAVE__DEVICE_ADDRESS, new_address >> 1);
00066     Device->I2cDevAddr = new_address;
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     status = VL53L1_WrByte(Device, SYSTEM__MODE_START, 0x40);   /* Enable VL53L1X */
00157     return status;
00158 }
00159 
00160 VL53L1X_ERROR VL53L1X::VL53L1X_StopRanging()
00161 {
00162     VL53L1X_ERROR status = 0;
00163 
00164     status = VL53L1_WrByte(Device, SYSTEM__MODE_START, 0x00);   /* Disable VL53L1X */
00165     return status;
00166 }
00167 
00168 
00169 
00170 VL53L1X_ERROR VL53L1X::VL53L1X_CheckForDataReady(uint8_t *isDataReady)
00171 {
00172     uint8_t Temp;
00173     uint8_t IntPol;
00174     VL53L1X_ERROR status = 0;
00175 
00176     status = VL53L1X_GetInterruptPolarity(&IntPol);
00177     status = VL53L1_RdByte(Device, GPIO__TIO_HV_STATUS, &Temp);
00178     /* Read in the register to check if a new value is available */
00179     if (status == 0){
00180         if ((Temp & 1) == IntPol)
00181             *isDataReady = 1;
00182         else
00183             *isDataReady = 0;
00184     }
00185     return status;
00186 }
00187 
00188 
00189 VL53L1X_ERROR VL53L1X::VL53L1X_SetTimingBudgetInMs(uint16_t TimingBudgetInMs)
00190 {
00191     uint16_t DM;
00192     VL53L1X_ERROR  status=0;
00193 
00194     status = VL53L1X_GetDistanceMode(&DM);
00195     if (DM == 0)
00196         return 1;
00197     else if (DM == 1) { /* Short DistanceMode */
00198         switch (TimingBudgetInMs) {
00199         case 15: /* only available in short distance mode */
00200             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
00201                     0x01D);
00202             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
00203                     0x0027);
00204             break;
00205         case 20:
00206             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
00207                     0x0051);
00208             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
00209                     0x006E);
00210             break;
00211         case 33:
00212             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
00213                     0x00D6);
00214             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
00215                     0x006E);
00216             break;
00217         case 50:
00218             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
00219                     0x1AE);
00220             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
00221                     0x01E8);
00222             break;
00223         case 100:
00224             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
00225                     0x02E1);
00226             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
00227                     0x0388);
00228             break;
00229         case 200:
00230             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
00231                     0x03E1);
00232             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
00233                     0x0496);
00234             break;
00235         case 500:
00236             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
00237                     0x0591);
00238             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
00239                     0x05C1);
00240             break;
00241         default:
00242             status = 1;
00243             break;
00244         }
00245     } else {
00246         switch (TimingBudgetInMs) {
00247         case 20:
00248             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
00249                     0x001E);
00250             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
00251                     0x0022);
00252             break;
00253         case 33:
00254             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
00255                     0x0060);
00256             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
00257                     0x006E);
00258             break;
00259         case 50:
00260             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
00261                     0x00AD);
00262             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
00263                     0x00C6);
00264             break;
00265         case 100:
00266             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
00267                     0x01CC);
00268             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
00269                     0x01EA);
00270             break;
00271         case 200:
00272             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
00273                     0x02D9);
00274             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
00275                     0x02F8);
00276             break;
00277         case 500:
00278             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI,
00279                     0x048F);
00280             VL53L1_WrWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_B_HI,
00281                     0x04A4);
00282             break;
00283         default:
00284             status = 1;
00285             break;
00286         }
00287     }
00288     return status;
00289 }
00290 
00291 VL53L1X_ERROR VL53L1X::VL53L1X_GetTimingBudgetInMs(uint16_t *pTimingBudget)
00292 {
00293     uint16_t Temp;
00294     VL53L1X_ERROR status = 0;
00295 
00296     status = VL53L1_RdWord(Device, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, &Temp);
00297     switch (Temp) {
00298         case 0x001D :
00299             *pTimingBudget = 15;
00300             break;
00301         case 0x0051 :
00302         case 0x001E :
00303             *pTimingBudget = 20;
00304             break;
00305         case 0x00D6 :
00306         case 0x0060 :
00307             *pTimingBudget = 33;
00308             break;
00309         case 0x1AE :
00310         case 0x00AD :
00311             *pTimingBudget = 50;
00312             break;
00313         case 0x02E1 :
00314         case 0x01CC :
00315             *pTimingBudget = 100;
00316             break;
00317         case 0x03E1 :
00318         case 0x02D9 :
00319             *pTimingBudget = 200;
00320             break;
00321         case 0x0591 :
00322         case 0x048F :
00323             *pTimingBudget = 500;
00324             break;
00325         default:
00326             *pTimingBudget = 0;
00327             break;
00328     }
00329     return status;
00330 }
00331 
00332 
00333 VL53L1X_ERROR VL53L1X::VL53L1X_SetDistanceMode(uint16_t DM)
00334 {
00335     uint16_t TB;
00336     VL53L1X_ERROR status = 0;
00337 
00338     status = VL53L1X_GetTimingBudgetInMs(&TB);
00339     
00340     
00341     switch (DM) {
00342     case 1:
00343         status = VL53L1_WrByte(Device, PHASECAL_CONFIG__TIMEOUT_MACROP, 0x14);
00344         status = VL53L1_WrByte(Device, RANGE_CONFIG__VCSEL_PERIOD_A, 0x07);
00345         status = VL53L1_WrByte(Device, RANGE_CONFIG__VCSEL_PERIOD_B, 0x05);
00346         status = VL53L1_WrByte(Device, RANGE_CONFIG__VALID_PHASE_HIGH, 0x38);
00347         status = VL53L1_WrWord(Device, SD_CONFIG__WOI_SD0, 0x0705);
00348         status = VL53L1_WrWord(Device, SD_CONFIG__INITIAL_PHASE_SD0, 0x0606);
00349         break;
00350     case 2:
00351         status = VL53L1_WrByte(Device, PHASECAL_CONFIG__TIMEOUT_MACROP, 0x0A);
00352         status = VL53L1_WrByte(Device, RANGE_CONFIG__VCSEL_PERIOD_A, 0x0F);
00353         status = VL53L1_WrByte(Device, RANGE_CONFIG__VCSEL_PERIOD_B, 0x0D);
00354         status = VL53L1_WrByte(Device, RANGE_CONFIG__VALID_PHASE_HIGH, 0xB8);
00355         status = VL53L1_WrWord(Device, SD_CONFIG__WOI_SD0, 0x0F0D);
00356         status = VL53L1_WrWord(Device, SD_CONFIG__INITIAL_PHASE_SD0, 0x0E0E);
00357         break;
00358     default:
00359         break;
00360     }
00361     status = VL53L1X_SetTimingBudgetInMs(TB);
00362     return status;
00363 }
00364 
00365 
00366 
00367 
00368 VL53L1X_ERROR VL53L1X::VL53L1X_GetDistanceMode(uint16_t *DM)
00369 {
00370     uint8_t TempDM, status=0;
00371 
00372     status = VL53L1_RdByte(Device,PHASECAL_CONFIG__TIMEOUT_MACROP, &TempDM);
00373     if (TempDM == 0x14)
00374         *DM=1;
00375     if(TempDM == 0x0A)
00376         *DM=2;
00377     return status;
00378 }
00379 
00380 
00381 
00382 VL53L1X_ERROR VL53L1X::VL53L1X_SetInterMeasurementInMs(uint16_t InterMeasMs)
00383 {
00384     uint16_t ClockPLL;
00385     VL53L1X_ERROR status = 0;
00386 
00387     status = VL53L1_RdWord(Device, VL53L1_RESULT__OSC_CALIBRATE_VAL, &ClockPLL);
00388     ClockPLL = ClockPLL&0x3FF;
00389     VL53L1_WrDWord(Device, VL53L1_SYSTEM__INTERMEASUREMENT_PERIOD,
00390             (uint32_t)(ClockPLL * InterMeasMs * 1.075));
00391     return status;
00392 
00393 }
00394 
00395 
00396 VL53L1X_ERROR VL53L1X::VL53L1X_GetInterMeasurementInMs(uint16_t *pIM)
00397 {
00398     uint16_t ClockPLL;
00399     VL53L1X_ERROR status = 0;
00400     uint32_t tmp;
00401 
00402     status = VL53L1_RdDWord(Device,VL53L1_SYSTEM__INTERMEASUREMENT_PERIOD, &tmp);
00403     *pIM = (uint16_t)tmp;
00404     status = VL53L1_RdWord(Device, VL53L1_RESULT__OSC_CALIBRATE_VAL, &ClockPLL);
00405     ClockPLL = ClockPLL&0x3FF;
00406     *pIM= (uint16_t)(*pIM/(ClockPLL*1.065));
00407     return status;
00408 }
00409 
00410 
00411 VL53L1X_ERROR VL53L1X::VL53L1X_BootState(uint8_t *state)
00412 {
00413     VL53L1X_ERROR status = 0;
00414     uint8_t tmp = 0;
00415 
00416     status = VL53L1_RdByte(Device,VL53L1_FIRMWARE__SYSTEM_STATUS, &tmp);
00417     *state = tmp;
00418     return status;
00419 }
00420 
00421 
00422 VL53L1X_ERROR VL53L1X::VL53L1X_GetSensorId(uint16_t *sensorId)
00423 {
00424     VL53L1X_ERROR status = 0;
00425     uint16_t tmp = 0;
00426 
00427     status = VL53L1_RdWord(Device, VL53L1_IDENTIFICATION__MODEL_ID, &tmp);
00428     *sensorId = tmp;
00429     return status;
00430 }
00431 
00432 
00433 VL53L1X_ERROR VL53L1X::VL53L1X_GetDistance(uint16_t *distance)
00434 {
00435     VL53L1X_ERROR status = 0;
00436     uint16_t tmp;
00437 
00438     status = (VL53L1_RdWord(Device,
00439             VL53L1_RESULT__FINAL_CROSSTALK_CORRECTED_RANGE_MM_SD0, &tmp));
00440     *distance = tmp;
00441     return status;
00442 }
00443 
00444 VL53L1X_ERROR VL53L1X::VL53L1X_GetSignalPerSpad(uint16_t *signalRate)
00445 {
00446     VL53L1X_ERROR status = 0;
00447     uint16_t SpNb=1, signal;
00448 
00449     status = VL53L1_RdWord(Device,
00450         VL53L1_RESULT__PEAK_SIGNAL_COUNT_RATE_CROSSTALK_CORRECTED_MCPS_SD0, &signal);
00451     status = VL53L1_RdWord(Device,
00452         VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0, &SpNb);
00453     *signalRate = (uint16_t) (2000.0*signal/SpNb);
00454     return status;
00455 }
00456 
00457 
00458 VL53L1X_ERROR VL53L1X::VL53L1X_GetAmbientPerSpad(uint16_t *ambPerSp)
00459 {
00460     VL53L1X_ERROR status=0;
00461     uint16_t AmbientRate, SpNb=1;
00462 
00463     status = VL53L1_RdWord(Device, RESULT__AMBIENT_COUNT_RATE_MCPS_SD, &AmbientRate);
00464     status = VL53L1_RdWord(Device, VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0, &SpNb);
00465     *ambPerSp=(uint16_t) (2000.0 * AmbientRate / SpNb);
00466     return status;
00467 }
00468 
00469 
00470 VL53L1X_ERROR VL53L1X::VL53L1X_GetSignalRate(uint16_t *signal)
00471 {
00472     VL53L1X_ERROR status = 0;
00473     uint16_t tmp;
00474 
00475     status = VL53L1_RdWord(Device,
00476         VL53L1_RESULT__PEAK_SIGNAL_COUNT_RATE_CROSSTALK_CORRECTED_MCPS_SD0, &tmp);
00477     *signal = tmp*8;
00478     return status;
00479 }
00480 
00481 
00482 VL53L1X_ERROR VL53L1X::VL53L1X_GetSpadNb(uint16_t *spNb)
00483 {
00484     VL53L1X_ERROR status = 0;
00485     uint16_t tmp;
00486 
00487     status = VL53L1_RdWord(Device,
00488                   VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0, &tmp);
00489     *spNb = tmp >> 8;
00490     return status;
00491 }
00492 
00493 
00494 VL53L1X_ERROR VL53L1X::VL53L1X_GetAmbientRate(uint16_t *ambRate)
00495 {
00496     VL53L1X_ERROR status = 0;
00497     uint16_t tmp;
00498 
00499     status = VL53L1_RdWord(Device, RESULT__AMBIENT_COUNT_RATE_MCPS_SD, &tmp);
00500     *ambRate = tmp*8;
00501     return status;
00502 }
00503 
00504 
00505 VL53L1X_ERROR VL53L1X::VL53L1X_GetRangeStatus(uint8_t *rangeStatus)
00506 {
00507     VL53L1X_ERROR status = 0;
00508     uint8_t RgSt;
00509 
00510     status = VL53L1_RdByte(Device, VL53L1_RESULT__RANGE_STATUS, &RgSt);
00511     RgSt = RgSt&0x1F;
00512     switch (RgSt) {
00513     case 9:
00514         RgSt = 0;
00515         break;
00516     case 6:
00517         RgSt = 1;
00518         break;
00519     case 4:
00520         RgSt = 2;
00521         break;
00522     case 8:
00523         RgSt = 3;
00524         break;
00525     case 5:
00526         RgSt = 4;
00527         break;
00528     case 3:
00529         RgSt = 5;
00530         break;
00531     case 19:
00532         RgSt = 6;
00533         break;
00534     case 7:
00535         RgSt = 7;
00536         break;
00537     case 12:
00538         RgSt = 9;
00539         break;
00540     case 18:
00541         RgSt = 10;
00542         break;
00543     case 22:
00544         RgSt = 11;
00545         break;
00546     case 23:
00547         RgSt = 12;
00548         break;
00549     case 13:
00550         RgSt = 13;
00551         break;
00552     default:
00553         RgSt = 255;
00554         break;
00555     }
00556     *rangeStatus = RgSt;
00557     return status;
00558 }
00559 
00560 
00561 VL53L1X_ERROR VL53L1X::VL53L1X_SetOffset(int16_t OffsetValue)
00562 {
00563     VL53L1X_ERROR status = 0;
00564     int16_t Temp;
00565 
00566     Temp = (OffsetValue*4);
00567     VL53L1_WrWord(Device, ALGO__PART_TO_PART_RANGE_OFFSET_MM,
00568             (uint16_t)Temp);
00569     VL53L1_WrWord(Device, MM_CONFIG__INNER_OFFSET_MM, 0x0);
00570     VL53L1_WrWord(Device, MM_CONFIG__OUTER_OFFSET_MM, 0x0);
00571     return status;
00572 }
00573 
00574 
00575 VL53L1X_ERROR  VL53L1X::VL53L1X_GetOffset(int16_t *offset)
00576 {
00577     VL53L1X_ERROR status = 0;
00578     uint16_t Temp;
00579 
00580     status = VL53L1_RdWord(Device,ALGO__PART_TO_PART_RANGE_OFFSET_MM, &Temp);
00581     Temp = Temp<<3;
00582     Temp = Temp >>5;
00583     *offset = (int16_t)(Temp);
00584     return status;
00585 }
00586 
00587 VL53L1X_ERROR VL53L1X::VL53L1X_SetXtalk(uint16_t XtalkValue)
00588 {
00589     /* XTalkValue in count per second to avoid float type */
00590     VL53L1X_ERROR status = 0;
00591 
00592     status = VL53L1_WrWord(Device,
00593             ALGO__CROSSTALK_COMPENSATION_X_PLANE_GRADIENT_KCPS,
00594             0x0000);
00595     status = VL53L1_WrWord(Device, ALGO__CROSSTALK_COMPENSATION_Y_PLANE_GRADIENT_KCPS,
00596             0x0000);
00597     status = VL53L1_WrWord(Device, ALGO__CROSSTALK_COMPENSATION_PLANE_OFFSET_KCPS,
00598             (XtalkValue<<9)/1000); /* * << 9 (7.9 format) and /1000 to convert cps to kpcs */
00599     return status;
00600 }
00601 
00602 
00603 VL53L1X_ERROR VL53L1X::VL53L1X_GetXtalk(uint16_t *xtalk )
00604 {
00605     VL53L1X_ERROR status = 0;
00606     uint16_t tmp;
00607 
00608     status = VL53L1_RdWord(Device,ALGO__CROSSTALK_COMPENSATION_PLANE_OFFSET_KCPS, &tmp);
00609     *xtalk = (tmp*1000)>>9; /* * 1000 to convert kcps to cps and >> 9 (7.9 format) */
00610     return status;
00611 }
00612 
00613 
00614 VL53L1X_ERROR VL53L1X::VL53L1X_SetDistanceThreshold(uint16_t ThreshLow,
00615                   uint16_t ThreshHigh, uint8_t Window,
00616                   uint8_t IntOnNoTarget)
00617 {
00618     VL53L1X_ERROR status = 0;
00619     uint8_t Temp = 0;
00620 
00621     status = VL53L1_RdByte(Device, SYSTEM__INTERRUPT_CONFIG_GPIO, &Temp);
00622     Temp = Temp & 0x47;
00623     if (IntOnNoTarget == 0) {
00624         status = VL53L1_WrByte(Device, SYSTEM__INTERRUPT_CONFIG_GPIO,
00625                    (Temp | (Window & 0x07)));
00626     } else {
00627         status = VL53L1_WrByte(Device, SYSTEM__INTERRUPT_CONFIG_GPIO,
00628                    ((Temp | (Window & 0x07)) | 0x40));
00629     }
00630     status = VL53L1_WrWord(Device, SYSTEM__THRESH_HIGH, ThreshHigh);
00631     status = VL53L1_WrWord(Device, SYSTEM__THRESH_LOW, ThreshLow);
00632     return status;
00633 }
00634 
00635 
00636 VL53L1X_ERROR VL53L1X::VL53L1X_GetDistanceThresholdWindow(uint16_t *window)
00637 {
00638     VL53L1X_ERROR status = 0;
00639     uint8_t tmp;
00640     status = VL53L1_RdByte(Device,SYSTEM__INTERRUPT_CONFIG_GPIO, &tmp);
00641     *window = (uint16_t)(tmp & 0x7);
00642     return status;
00643 }
00644 
00645 
00646 VL53L1X_ERROR VL53L1X::VL53L1X_GetDistanceThresholdLow(uint16_t *low)
00647 {
00648     VL53L1X_ERROR status = 0;
00649     uint16_t tmp;
00650 
00651     status = VL53L1_RdWord(Device,SYSTEM__THRESH_LOW, &tmp);
00652     *low = tmp;
00653     return status;
00654 }
00655 
00656 VL53L1X_ERROR VL53L1X::VL53L1X_GetDistanceThresholdHigh(uint16_t *high)
00657 {
00658     VL53L1X_ERROR status = 0;
00659     uint16_t tmp;
00660 
00661     status = VL53L1_RdWord(Device,SYSTEM__THRESH_HIGH, &tmp);
00662     *high = tmp;
00663     return status;
00664 }
00665 
00666 VL53L1X_ERROR VL53L1X::VL53L1X_SetROI(uint16_t X, uint16_t Y)
00667 {
00668     uint8_t OpticalCenter;
00669     VL53L1X_ERROR status = 0;
00670 
00671     status =VL53L1_RdByte(Device, VL53L1_ROI_CONFIG__MODE_ROI_CENTRE_SPAD, &OpticalCenter);
00672     if (X > 16)
00673         X = 16;
00674     if (Y > 16)
00675         Y = 16;
00676     if (X > 10 || Y > 10){
00677         OpticalCenter = 199;
00678     }
00679     status = VL53L1_WrByte(Device, ROI_CONFIG__USER_ROI_CENTRE_SPAD, OpticalCenter);
00680     status = VL53L1_WrByte(Device, ROI_CONFIG__USER_ROI_REQUESTED_GLOBAL_XY_SIZE,
00681                (Y - 1) << 4 | (X - 1));
00682     return status;
00683 }
00684 
00685 VL53L1X_ERROR VL53L1X::VL53L1X_GetROI_XY(uint16_t *ROI_X, uint16_t *ROI_Y)
00686 {
00687     VL53L1X_ERROR status = 0;
00688     uint8_t tmp;
00689 
00690     status = VL53L1_RdByte(Device,ROI_CONFIG__USER_ROI_REQUESTED_GLOBAL_XY_SIZE, &tmp);
00691     *ROI_X = ((uint16_t)tmp & 0x0F) + 1;
00692     *ROI_Y = (((uint16_t)tmp & 0xF0) >> 4) + 1;
00693     return status;
00694 }
00695 
00696 VL53L1X_ERROR VL53L1X::VL53L1X_SetSignalThreshold(uint16_t Signal)
00697 {
00698     VL53L1X_ERROR status = 0;
00699 
00700     VL53L1_WrWord(Device,RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS,Signal>>3);
00701     return status;
00702 }
00703 
00704 VL53L1X_ERROR VL53L1X::VL53L1X_GetSignalThreshold(uint16_t *signal)
00705 {
00706     VL53L1X_ERROR status = 0;
00707     uint16_t tmp;
00708 
00709     status = VL53L1_RdWord(Device,
00710                 RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS, &tmp);
00711     *signal = tmp <<3;
00712     return status;
00713 }
00714 
00715 
00716 VL53L1X_ERROR VL53L1X::VL53L1X_SetSigmaThreshold(uint16_t Sigma)
00717 {
00718     VL53L1X_ERROR status = 0;
00719 
00720     if(Sigma>(0xFFFF>>2)){
00721         return 1;
00722     }
00723     /* 16 bits register 14.2 format */
00724     status = VL53L1_WrWord(Device,RANGE_CONFIG__SIGMA_THRESH,Sigma<<2);
00725     return status;
00726 }
00727 
00728 VL53L1X_ERROR VL53L1X::VL53L1X_GetSigmaThreshold(uint16_t *sigma)
00729 {
00730     VL53L1X_ERROR status = 0;
00731     uint16_t tmp;
00732 
00733     status = VL53L1_RdWord(Device,RANGE_CONFIG__SIGMA_THRESH, &tmp);
00734     *sigma = tmp >> 2;
00735     return status;
00736 
00737 }
00738 
00739 VL53L1X_ERROR VL53L1X::VL53L1X_StartTemperatureUpdate()
00740 {
00741     VL53L1X_ERROR status = 0;
00742     uint8_t tmp=0;
00743 
00744     status = VL53L1_WrByte(Device,VL53L1_VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND,0x81); /* full VHV */
00745     status = VL53L1_WrByte(Device,0x0B,0x92);
00746     status = VL53L1X_StartRanging();
00747     while(tmp==0){
00748         status = VL53L1X_CheckForDataReady(&tmp);
00749     }
00750     tmp  = 0;
00751     status = VL53L1X_ClearInterrupt();
00752     status = VL53L1X_StopRanging();
00753     status = VL53L1_WrByte(Device, VL53L1_VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND, 0x09); /* two bounds VHV */
00754     status = VL53L1_WrByte(Device, 0x0B, 0); /* start VHV from the previous temperature */
00755     return status;
00756 }
00757 
00758     /* VL53L1X_calibration.h functions */
00759     
00760 int8_t VL53L1X::VL53L1X_CalibrateOffset(uint16_t TargetDistInMm, int16_t *offset)
00761 {
00762     uint8_t i = 0, tmp;
00763     int16_t AverageDistance = 0;
00764     uint16_t distance;
00765     VL53L1X_ERROR status = 0;
00766 
00767     status = VL53L1_WrWord(Device, ALGO__PART_TO_PART_RANGE_OFFSET_MM, 0x0);
00768     status = VL53L1_WrWord(Device, MM_CONFIG__INNER_OFFSET_MM, 0x0);
00769     status = VL53L1_WrWord(Device, MM_CONFIG__OUTER_OFFSET_MM, 0x0);
00770     status = VL53L1X_StartRanging();    /* Enable VL53L1X sensor */
00771     for (i = 0; i < 50; i++) {
00772         while (tmp == 0){
00773             status = VL53L1X_CheckForDataReady(&tmp);
00774         }
00775         tmp = 0;
00776         status = VL53L1X_GetDistance(&distance);
00777         status = VL53L1X_ClearInterrupt();
00778         AverageDistance = AverageDistance + distance;
00779     }
00780     status = VL53L1X_StopRanging();
00781     AverageDistance = AverageDistance / 50;
00782     *offset = TargetDistInMm - AverageDistance;
00783     status = VL53L1_WrWord(Device, ALGO__PART_TO_PART_RANGE_OFFSET_MM, *offset*4);
00784     return status;
00785 }
00786 
00787 
00788 int8_t VL53L1X::VL53L1X_CalibrateXtalk(uint16_t TargetDistInMm, uint16_t *xtalk)
00789 {
00790     uint8_t i, tmp= 0;
00791     float AverageSignalRate = 0;
00792     float AverageDistance = 0;
00793     float AverageSpadNb = 0;
00794     uint16_t distance = 0, spadNum;
00795     uint16_t sr;
00796     VL53L1X_ERROR status = 0;
00797 
00798     status = VL53L1_WrWord(Device, 0x0016,0);
00799     status = VL53L1X_StartRanging();
00800     for (i = 0; i < 50; i++) {
00801         while (tmp == 0){
00802             status = VL53L1X_CheckForDataReady(&tmp);
00803         }
00804         tmp=0;
00805         status= VL53L1X_GetSignalRate(&sr);
00806         status= VL53L1X_GetDistance(&distance);
00807         status = VL53L1X_ClearInterrupt();
00808         AverageDistance = AverageDistance + distance;
00809         status = VL53L1X_GetSpadNb(&spadNum);
00810         AverageSpadNb = AverageSpadNb + spadNum;
00811         AverageSignalRate =
00812             AverageSignalRate + sr;
00813     }
00814     status = VL53L1X_StopRanging();
00815     AverageDistance = AverageDistance / 50;
00816     AverageSpadNb = AverageSpadNb / 50;
00817     AverageSignalRate = AverageSignalRate / 50;
00818     /* Calculate Xtalk value */
00819     *xtalk = (uint16_t)(512*(AverageSignalRate*(1-(AverageDistance/TargetDistInMm)))/AverageSpadNb);
00820     status = VL53L1_WrWord(Device, 0x0016, *xtalk);
00821     return status;
00822 }
00823 
00824 
00825 
00826 
00827     /* Write and read functions from I2C */
00828 
00829 
00830 VL53L1X_ERROR VL53L1X::VL53L1_WriteMulti(VL53L1_DEV Dev, uint16_t index, uint8_t *pdata, uint32_t count)
00831 {
00832    int  status;
00833 
00834    status = VL53L1_I2CWrite(Dev->I2cDevAddr, index, pdata, (uint16_t)count);
00835    return status;
00836 }
00837 
00838 VL53L1X_ERROR VL53L1X::VL53L1_ReadMulti(VL53L1_DEV Dev, uint16_t index, uint8_t *pdata, uint32_t count)
00839 {
00840     int status;
00841 
00842     status = VL53L1_I2CRead(Dev->I2cDevAddr, index, pdata, (uint16_t)count);
00843 
00844     return status;
00845 }
00846 
00847 
00848 VL53L1X_ERROR VL53L1X::VL53L1_WrByte(VL53L1_DEV Dev, uint16_t index, uint8_t data)
00849 {
00850    int  status;
00851 
00852    status=VL53L1_I2CWrite(Dev->I2cDevAddr, index, &data, 1);
00853    return status;
00854 }
00855 
00856 VL53L1X_ERROR VL53L1X::VL53L1_WrWord(VL53L1_DEV Dev, uint16_t index, uint16_t data)
00857 {
00858    int  status;
00859    uint8_t buffer[2];
00860 
00861      buffer[0] = data >> 8;
00862      buffer[1] = data & 0x00FF;
00863    status=VL53L1_I2CWrite(Dev->I2cDevAddr, index, (uint8_t *)buffer, 2);
00864    return status;
00865 }
00866 
00867 VL53L1X_ERROR VL53L1X::VL53L1_WrDWord(VL53L1_DEV Dev, uint16_t index, uint32_t data)
00868 {
00869    int  status;
00870    uint8_t buffer[4];
00871 
00872      buffer[0] = (data >> 24) & 0xFF;
00873      buffer[1] = (data >> 16) & 0xFF;
00874      buffer[2] = (data >>  8) & 0xFF;
00875      buffer[3] = (data >>  0) & 0xFF;
00876    status=VL53L1_I2CWrite(Dev->I2cDevAddr, index, (uint8_t *)buffer, 4);
00877    return status;
00878 }
00879 
00880 
00881 VL53L1X_ERROR VL53L1X::VL53L1_RdByte(VL53L1_DEV Dev, uint16_t index, uint8_t *data)
00882 {
00883    int  status;
00884 
00885    status = VL53L1_I2CRead(Dev->I2cDevAddr, index, data, 1);
00886 
00887    if(status)
00888      return -1;
00889 
00890    return 0;
00891 }
00892 
00893 VL53L1X_ERROR VL53L1X::VL53L1_RdWord(VL53L1_DEV Dev, uint16_t index, uint16_t *data)
00894 {
00895    int  status;
00896    uint8_t buffer[2] = {0,0};
00897 
00898    status = VL53L1_I2CRead(Dev->I2cDevAddr, index, buffer, 2);
00899    if (!status)
00900    {
00901        *data = (buffer[0] << 8) + buffer[1];
00902    }
00903    return status;
00904 
00905 }
00906 
00907 VL53L1X_ERROR VL53L1X::VL53L1_RdDWord(VL53L1_DEV Dev, uint16_t index, uint32_t *data)
00908 {
00909    int status;
00910    uint8_t buffer[4] = {0,0,0,0};
00911 
00912    status = VL53L1_I2CRead(Dev->I2cDevAddr, index, buffer, 4);
00913    if(!status)
00914    {
00915        *data = (buffer[0] << 24) + (buffer[1] << 16) + (buffer[2] << 8) + buffer[3];
00916    }
00917    return status;
00918 
00919 }
00920 
00921 VL53L1X_ERROR VL53L1X::VL53L1_UpdateByte(VL53L1_DEV Dev, uint16_t index, uint8_t AndData, uint8_t OrData)
00922 {
00923    int  status;
00924    uint8_t buffer = 0;
00925 
00926    /* read data direct onto buffer */
00927    status = VL53L1_I2CRead(Dev->I2cDevAddr, index, &buffer,1);
00928    if (!status)
00929    {
00930       buffer = (buffer & AndData) | OrData;
00931       status = VL53L1_I2CWrite(Dev->I2cDevAddr, index, &buffer, (uint16_t)1);
00932    }
00933    return status;
00934 }
00935 
00936 VL53L1X_ERROR VL53L1X::VL53L1_I2CWrite(uint8_t DeviceAddr, uint16_t RegisterAddr, uint8_t* pBuffer, uint16_t NumByteToWrite)
00937 {
00938     int ret;
00939     ret = dev_i2c->v53l1x_i2c_write(pBuffer, DeviceAddr, RegisterAddr, NumByteToWrite);
00940     if (ret) {
00941         return -1;
00942     }
00943     return 0;
00944 
00945 }
00946 
00947 VL53L1X_ERROR VL53L1X::VL53L1_I2CRead(uint8_t DeviceAddr, uint16_t RegisterAddr, uint8_t* pBuffer, uint16_t NumByteToRead)
00948 {
00949     int ret;
00950 
00951     ret = dev_i2c->v53l1x_i2c_read(pBuffer, DeviceAddr, RegisterAddr, NumByteToRead);
00952 
00953     if (ret) {
00954         return -1;
00955     }
00956     return 0;
00957 }
00958 
00959 
00960 VL53L1X_ERROR VL53L1X::VL53L1_GetTickCount(
00961     uint32_t *ptick_count_ms)
00962 {
00963 
00964     /* Returns current tick count in [ms] */
00965 
00966     VL53L1X_ERROR status  = VL53L1_ERROR_NONE;
00967 
00968     *ptick_count_ms = 0;
00969 
00970     return status;
00971 }
00972 
00973 
00974 
00975 VL53L1X_ERROR VL53L1X::VL53L1_WaitUs(VL53L1_Dev_t *pdev, int32_t wait_us)
00976 {
00977     return VL53L1_ERROR_NONE;
00978 }
00979 
00980 
00981 VL53L1X_ERROR VL53L1X::VL53L1_WaitMs(VL53L1_Dev_t *pdev, int32_t wait_ms)
00982 {
00983     return VL53L1_ERROR_NONE;
00984 }
00985 
00986 
00987 VL53L1X_ERROR VL53L1X::VL53L1_WaitValueMaskEx(
00988     VL53L1_Dev_t *pdev,
00989     uint32_t      timeout_ms,
00990     uint16_t      index,
00991     uint8_t       value,
00992     uint8_t       mask,
00993     uint32_t      poll_delay_ms)
00994 {
00995 
00996     /*
00997      * Platform implementation of WaitValueMaskEx V2WReg script command
00998      *
00999      * WaitValueMaskEx(
01000      *          duration_ms,
01001      *          index,
01002      *          value,
01003      *          mask,
01004      *          poll_delay_ms);
01005      */
01006 
01007     VL53L1_Error status         = VL53L1_ERROR_NONE;
01008     uint32_t     start_time_ms = 0;
01009     uint32_t     current_time_ms = 0;
01010     uint32_t     polling_time_ms = 0;
01011     uint8_t      byte_value      = 0;
01012     uint8_t      found           = 0;
01013 
01014 
01015 
01016     /* calculate time limit in absolute time */
01017 
01018      VL53L1_GetTickCount(&start_time_ms);
01019 
01020     /* remember current trace functions and temporarily disable
01021      * function logging
01022      */
01023 
01024 
01025     /* wait until value is found, timeout reached on error occurred */
01026 
01027     while ((status == VL53L1_ERROR_NONE) &&
01028            (polling_time_ms < timeout_ms) &&
01029            (found == 0)) {
01030 
01031         if (status == VL53L1_ERROR_NONE)
01032             status = VL53L1_RdByte(
01033                             pdev,
01034                             index,
01035                             &byte_value);
01036 
01037         if ((byte_value & mask) == value)
01038             found = 1;
01039 
01040         if (status == VL53L1_ERROR_NONE  &&
01041             found == 0 &&
01042             poll_delay_ms > 0)
01043             status = VL53L1_WaitMs(
01044                     pdev,
01045                     poll_delay_ms);
01046 
01047         /* Update polling time (Compare difference rather than absolute to
01048         negate 32bit wrap around issue) */
01049         VL53L1_GetTickCount(&current_time_ms);
01050         polling_time_ms = current_time_ms - start_time_ms;
01051 
01052     }
01053     
01054 
01055     if (found == 0 && status == VL53L1_ERROR_NONE)
01056         status = VL53L1_ERROR_TIME_OUT;
01057 
01058     return status;
01059 }
01060 
01061 int VL53L1X::handle_irq(uint16_t *distance)
01062 {
01063     int status;
01064     status = get_measurement(distance);
01065     enable_interrupt_measure_detection_irq();
01066     return status;
01067 }
01068 
01069 int VL53L1X::get_measurement(uint16_t *distance)
01070 {
01071     int status = 0;
01072 
01073     status = VL53L1X_GetDistance(distance);
01074     status = VL53L1X_ClearInterrupt();
01075 
01076     return status;
01077 }
01078 
01079 int VL53L1X::start_measurement(void (*fptr)(void))
01080 {
01081     int status = 0;
01082 
01083     if (_gpio1Int == NULL) {
01084         printf("GPIO1 Error\r\n");
01085         return 1;
01086     }
01087 
01088     status = VL53L1X_StopRanging(); // it is safer to do this while sensor is stopped
01089 
01090     if (status == 0) {
01091         attach_interrupt_measure_detection_irq(fptr);
01092         enable_interrupt_measure_detection_irq();
01093     }
01094 
01095     if (status == 0) {
01096         status = VL53L1X_StartRanging();
01097     }
01098 
01099     return status;
01100 }
01101 
01102 int VL53L1X::stop_measurement()
01103 {
01104     int status = 0;
01105 
01106     if (status == 0) {
01107         printf("Call of VL53L0X_StopMeasurement\n");
01108         status = VL53L1X_StopRanging();
01109     }
01110 
01111     if (status == 0)
01112         status = VL53L1X_ClearInterrupt();
01113 
01114     return status;
01115 }