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