VL53L0X.cpp

00001 /**
00002  ******************************************************************************
00003  * @file    Class.cpp
00004  * @author  IMG
00005  * @version V0.0.1
00006  * @date    28-June-2016
00007  * @brief   Implementation file for the VL53L0X driver class
00008  ******************************************************************************
00009  * @attention
00010  *
00011  * <h2><center>&copy; COPYRIGHT(c) 2016 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 // Some example regex that were used to replace useless macros
00039 // \QVL53L0X_SETARRAYPARAMETERFIELD(\E([A-Z\d]+)[[:punct:]](\s*)([A-Z\d_]+)[[:punct:]](\s*)([A-Z\d_]+)\Q);\E
00040 // _device->CurrParams.\1[\3] = \5; 
00041 // to replace this "#define VL53L0X_SETARRAYPARAMETERFIELD(field, index, value)"  by  "_device->CurrParams.field[index] = value" 
00042 
00043 // to replace "Read_Byte(0x90,&module_id);"  by  "module_id = Read_Byte(0x90);" search and replace
00044 // \QRead_Byte(\E([A-Za-z_\d]+)[[:punct:]](\s*)\Q&\E([A-Za-z\d_]+)\Q);\E
00045 // \3 = Read_Byte\(\1\); 
00046 
00047 /* Includes */
00048 #include <stdlib.h>
00049 #include "VL53L0X.h"
00050 #include "VL53L0X_tuning.h"
00051 
00052 /******************************************************************************/
00053 /******************************************************************************/
00054 /****************** All initialization functions      *************************/
00055 /******************************************************************************/
00056 /******************************************************************************/
00057 
00058 // Function Data_init and Init_Sensor is united into Start_Sensor
00059 VL53L0X_Error VL53L0X::Start_Sensor(uint8_t new_addr)
00060 {   ErrState = VL53L0X_OK;
00061 
00062     if (_gpio0) {   // Can the shutdown pin be controlled?
00063        *_gpio0 = 0; wait_ms(1); // quick shutdown
00064        *_gpio0 = 1; wait_ms(10); // and back ON again
00065        }
00066 
00067     /* Setup the I2C bus.  By default the I2C is running at 1V8 if you 
00068      * want to change it you need to include this define at compilation level. */
00069 #ifdef USE_I2C_2V8
00070     VL53L0X_UpdateByte(REG_VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV, 0xFE, 0x01);
00071 #endif
00072     /* Set I2C standard mode */
00073     Write_Byte(0x88,0x00);
00074 
00075     // read and check the device ID from the ID register
00076     Device_Info.ProductType  = Read_Byte(REG_IDENTIFICATION_MODEL_ID); 
00077     if ( (ErrState == VL53L0X_OK)  && (Device_Info.ProductType  != 0xEEAA) ) 
00078         {return VL53L0X_ERROR_I2C_WRONG_DEV_ID; }
00079     
00080     // reconfigure the address with a new address if requested
00081     if ( (ErrState == VL53L0X_OK) && (new_addr != VL53L0X_DEFAULT_ADDRESS) )
00082         { Write_Byte(REG_I2C_SLAVE_DEVICE_ADDRESS, new_addr / 2);
00083           I2cDevAddr = new_addr;
00084         } 
00085     // quite if an error was raised 
00086     if (ErrState != VL53L0X_OK)  {return ErrState; } 
00087     
00088     /* Set Default static parameters
00089     *set first temporary values 9.44MHz * 65536 = 618660 */
00090     OscFrequencyMHz = 618660;
00091     RefSPADSInitialised = 0;
00092     
00093     // Read All NVM from device
00094     ReadNVMDataFromDeviceDone = 0;
00095     Get_all_NVM_info_from_device();
00096 
00097 #ifdef USE_IQC_STATION
00098     // must make sure that first your read all NVM info used by the API */
00099     VL53L0X_Apply_Offset_Cal();
00100 #endif
00101 
00102     /* Default value is 1000 for Linearity Corrective Gain */
00103     LinearityCorrectiveGain = 1000; 
00104 
00105     /* Dmax default Parameter */
00106     DmaxCalRangeMilliMeter = 400; 
00107     DmaxCalSignalRateRtnMHz = (TFP1616)((0x00016B85)); /* 1.42 No Cover Glass*/
00108 
00109     /* Get default parameters */
00110     CurrParams = Get_device_parameters();
00111     
00112     /* Set Default Xtalk_CompRate_MHz to 0  */
00113     CurrParams.Xtalk_CompRate_MHz  = 0; 
00114 
00115     /* initialize CurrParams values */
00116     CurrParams.DeviceMode  = VL53L0X_DEVICEMODE_SINGLE_RANGING;
00117     CurrParams.HistogramMode  = VL53L0X_HISTOGRAMMODE_DISABLED;
00118 
00119     /* Sigma estimator variable */
00120     SigmaEstRefArray       = 100; 
00121     SigmaEstEffPulseWidth  = 900; 
00122     SigmaEstEffAmbWidth    = 500; 
00123     targetRefRate         = 0x0A00;  /* 20 MHz in 9:7 format */
00124 
00125     /* Use internal default settings */
00126     UseInternalTuningSettings = 1; 
00127     Write_Byte(0x80,0x01);
00128     Write_Byte(0xFF,0x01);
00129     Write_Byte(0x00,0x00);
00130     StopVariable = Read_Byte(0x91); 
00131     Write_Byte(0x00,0x01);
00132     Write_Byte(0xFF,0x00);
00133     Write_Byte(0x80,0x00);
00134 
00135     // quite if an error was raised 
00136     if (ErrState != VL53L0X_OK)  {return ErrState; } 
00137 
00138     /* Disable the following SW-internal checks plaus set some values */
00139     CurrParams.Limit_Chk_En  [VL53L0X_CHECKEN_SIG_REF_CLIP] = 0; 
00140     CurrParams.Limit_Chk_Val [VL53L0X_CHECKEN_SIG_REF_CLIP] = (35 * 65536);
00141     CurrParams.Limit_Chk_En  [VL53L0X_CHECKEN_RANGE_IGNORE_THRESHOLD] = 0; 
00142     CurrParams.Limit_Chk_Val [VL53L0X_CHECKEN_RANGE_IGNORE_THRESHOLD] = 0;
00143 
00144     /* Disable the following Device-Internal Checks: */
00145     CurrParams.Limit_Chk_En [VL53L0X_CHECKEN_SIG_RATE_MSRC] = 0; 
00146     CurrParams.Limit_Chk_En [VL53L0X_CHECKEN_SIG_RATE_PRE_RANGE] = 0; 
00147     Register_BitMask(REG_MSRC_CONFIG_CONTROL,0xEE, 0);
00148 
00149     /* Only enable this  internal Check : */
00150     CurrParams.Limit_Chk_En  [VL53L0X_CHECKEN_SIGMA_FINAL_RANGE] = 1; 
00151     CurrParams.Limit_Chk_Val [VL53L0X_CHECKEN_SIGMA_FINAL_RANGE] = (18 * 65536);
00152 
00153     /* Plus Enable VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE check */
00154     Set_limit_chk_en(VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE,1);
00155                                       
00156     if (ErrState == VL53L0X_OK) {  /* 0.25 in FP1616 notation 65536 */
00157         Set_limit_chk_val(VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE,
00158                                                (TFP1616)(25 * 65536 / 100)); }
00159 
00160     // quit if an error was raised 
00161     if (ErrState != VL53L0X_OK)  {return ErrState; } 
00162 
00163     // Preset the Config States 
00164     SequenceConfig = 0xFF ; 
00165 
00166     /* Set Device state to tell that we are waiting for call to VL53L0X_StaticInit */
00167     Current_State  = VL53L0X_STATE_WAIT_STATICINIT ;
00168 
00169     Fill_device_info(); // Retrieve Silicon version, stored in Device_Info
00170 
00171     uint32_t ref_SPAD_count;
00172     uint8_t is_aperture_SPADS;
00173     uint8_t vhv_settings;
00174     uint8_t phase_cal;
00175 
00176     // make sure the Get_all_NVM_info_from_device was called before calling static init
00177     if (ErrState == VL53L0X_OK) { Static_init(); } // Device Initialization
00178 
00179     if (ErrState == VL53L0X_OK) {  // Device Calibration
00180            Perf_Ref_calibration( &vhv_settings, &phase_cal, 1);  }
00181 
00182     if (ErrState == VL53L0X_OK) {  // SPAD Configuration
00183         Perf_Ref_SPAD_management( &ref_SPAD_count, &is_aperture_SPADS); }
00184    
00185     return ErrState;
00186 }                   
00187  
00188 void VL53L0X::Fill_device_info()
00189 {   uint8_t revision;
00190 
00191     if (ErrState == VL53L0X_OK) 
00192       { if (ModuleId == 0) 
00193             { revision = 0;
00194               strcpy(Device_Info.ProductId ,""); }
00195           else 
00196             { revision = Revision; 
00197               strcpy(Device_Info.ProductId ,ProductId); 
00198             }
00199         if (revision == 0) 
00200             { strcpy(Device_Info.Name ,VL53L0X_STRING_DEVICE_INFO_NAME_TS0); } 
00201           else if ((revision <= 34) && (revision != 32)) 
00202             { strcpy(Device_Info.Name ,VL53L0X_STRING_DEVICE_INFO_NAME_TS1); }
00203           else if (revision < 39)
00204             { strcpy(Device_Info.Name ,VL53L0X_STRING_DEVICE_INFO_NAME_TS2); }
00205           else { strcpy(Device_Info.Name ,VL53L0X_STRING_DEVICE_INFO_NAME_ES1); }
00206         strcpy(Device_Info.Type ,VL53L0X_STRING_DEVICE_INFO_TYPE);
00207      }
00208  
00209     Device_Info.ProductRevisionMajor  = 1;
00210     Device_Info.ProductRevisionMinor  = 
00211                  (Read_Byte(REG_IDENTIFICATION_REVISION_ID) & 0xF0) >> 4;
00212 }
00213  
00214 /* That was 'Get_info_from_device( unit8_t option)' where option was composed of 
00215   1, 2, 4 depending what info was requested.
00216   It was decided to combine all that into a single Get ALL infos from the device
00217   that is called once at Sensor Init so all the job is done. 
00218   In addition, originally values would first be read into local variables and then
00219   only copied to class fields if no errors. 
00220   However, if at device initialization an error is raised the whole class instance
00221   cannot be used anyway, so decided that we directly read into class fields.  */
00222 
00223 void VL53L0X::Get_all_NVM_info_from_device()
00224 {   uint32_t tmp_dword;
00225     uint32_t offset_fixed1104_mm = 0;
00226     int16_t  offset_um = 0;
00227     uint32_t dist_meas_tgt_fixed1104_mm = 400 << 4;
00228     uint32_t dist_meas_fixed1104_400_mm = 0;
00229     uint32_t signal_rate_meas_fixed1104_400_mm = 0;
00230     int i;
00231 
00232     /* This access is done only once after that a GetDeviceInfo or datainit is done*/
00233     if (ReadNVMDataFromDeviceDone == 7) { return ; }
00234     
00235     Write_Byte(0x80,0x01);
00236     Write_Byte(0xFF,0x01);
00237     Write_Byte(0x00,0x00);
00238     Write_Byte(0xFF,0x06);
00239     Register_BitMask(0x83,0xFF,0x04)
00240     Write_Byte(0xFF,0x07);
00241     Write_Byte(0x81,0x01);
00242     Polling_delay(); // warning, does nothing!!
00243     Write_Byte(0x80,0x01);
00244 
00245     /*  *************    First Block of NVM data is read:   ************/
00246     tmp_dword = Get_NVM_DWord(0x6b); 
00247     ReferenceSPADCount = (uint8_t)((tmp_dword >>  8) & 0x7f);
00248     ReferenceSPADType  = (uint8_t)((tmp_dword >> 15) & 0x01);
00249 
00250     tmp_dword = Get_NVM_DWord(0x24); 
00251     SPADData.RefGoodSPADMap[0] = (uint8_t)((tmp_dword >> 24)& 0xff);
00252     SPADData.RefGoodSPADMap[1] = (uint8_t)((tmp_dword >> 16)& 0xff);
00253     SPADData.RefGoodSPADMap[2] = (uint8_t)((tmp_dword >>  8)& 0xff);
00254     SPADData.RefGoodSPADMap[3] = (uint8_t)(tmp_dword & 0xff);
00255 
00256     tmp_dword = Get_NVM_DWord(0x25); 
00257     SPADData.RefGoodSPADMap[4] = (uint8_t)((tmp_dword >> 24)& 0xff);
00258     SPADData.RefGoodSPADMap[5] = (uint8_t)((tmp_dword >> 16)& 0xff);
00259 
00260     /*  *************    Second Block of NVM data is read:   ************/
00261     ModuleId = Get_NVM_Byte(0x02); 
00262     Revision  = Get_NVM_Byte(0x02); 
00263 
00264     tmp_dword = Get_NVM_DWord(0x77); 
00265     ProductId[0] = (char)((tmp_dword >> 25) & 0x07f);
00266     ProductId[1] = (char)((tmp_dword >> 18) & 0x07f);
00267     ProductId[2] = (char)((tmp_dword >> 11) & 0x07f);
00268     ProductId[3] = (char)((tmp_dword >>  4) & 0x07f);
00269     ProductId[4] = (char)((tmp_dword <<  3) & 0x07f);
00270 
00271     tmp_dword = Get_NVM_DWord(0x78); 
00272     ProductId[4] = ProductId[4] | 
00273                     (char)((tmp_dword >> 29) & 0x07f));
00274     ProductId[5] = (char)((tmp_dword >> 22) & 0x07f);
00275     ProductId[6] = (char)((tmp_dword >> 15) & 0x07f);
00276     ProductId[7] = (char)((tmp_dword >>  8) & 0x07f);
00277     ProductId[8] = (char)((tmp_dword >>  1) & 0x07f);
00278     ProductId[9] = (char)((tmp_dword <<  6) & 0x07f);
00279 
00280     tmp_dword = Get_NVM_DWord(0x79); 
00281     ProductId[9]  = ProductId[9] | 
00282                      (char)((tmp_dword >> 26) & 0x07f);
00283     ProductId[10] = (char)((tmp_dword >> 19) & 0x07f);
00284     ProductId[11] = (char)((tmp_dword >> 12) & 0x07f);
00285     ProductId[12] = (char)((tmp_dword >>  5) & 0x07f);
00286     ProductId[13] = (char)((tmp_dword <<  2) & 0x07f);
00287 
00288     tmp_dword = Get_NVM_DWord(0x7A); 
00289     ProductId[13] = ProductId[13] |
00290                      (char)((tmp_dword >> 30) & 0x07f));
00291     ProductId[14] = (char)((tmp_dword >> 23) & 0x07f);
00292     ProductId[15] = (char)((tmp_dword >> 16) & 0x07f);
00293     ProductId[16] = (char)((tmp_dword >>  9) & 0x07f);
00294     ProductId[17] = (char)((tmp_dword >>  2) & 0x07f);
00295     ProductId[18] = '\0';
00296 
00297     /*  *************    Third Block of NVM data is read:   ************/
00298     PartUIDUpper  = Get_NVM_DWord(0x7B); 
00299     PartUIDLower  = Get_NVM_DWord(0x7C); 
00300     SignalRateMeasFixed400mm  =  // convert from FP97_TO_FP1616
00301        ( (( Get_NVM_DWord(0x73) << 17) & 0x1fE0000) | 
00302          (( Get_NVM_DWord(0x74) >> 15) & 0x001fE00) ) ; 
00303 
00304     dist_meas_fixed1104_400_mm = 
00305         (( Get_NVM_DWord(0x75) <<  8) & 0xff00) | 
00306         (( Get_NVM_DWord(0x76) >> 24) & 0x00ff);
00307     
00308     if (dist_meas_fixed1104_400_mm != 0) {
00309         offset_fixed1104_mm = dist_meas_fixed1104_400_mm -
00310             dist_meas_tgt_fixed1104_mm;
00311         NVM_Offset_Cal_um = (offset_fixed1104_mm * 1000) >> 4;
00312         NVM_Offset_Cal_um *= -1; }
00313       else { NVM_Offset_Cal_um = 0; }
00314 
00315     Write_Byte(0x81,0x00);
00316     Write_Byte(0xFF,0x06);
00317     Register_BitMask(0x83,0xfb,0x00)
00318     Write_Byte(0xFF,0x01);
00319     Write_Byte(0x00,0x01);
00320     Write_Byte(0xFF,0x00);
00321     Write_Byte(0x80,0x00);
00322     ReadNVMDataFromDeviceDone = 7;
00323 }
00324 
00325 uint32_t VL53L0X::Get_distance()
00326 {   ErrState = VL53L0X_OK;
00327     TRangeResults p_ranging_results;
00328 
00329     Start_Measurement(op_single_shot_poll, NULL);
00330     if (ErrState==VL53L0X_OK) 
00331        { p_ranging_results = Get_Measurement(op_single_shot_poll); }
00332 
00333     Stop_Measurement(op_single_shot_poll);
00334 
00335     if (p_ranging_results.RangeStatus == 0) // we have a valid range ?
00336        { return p_ranging_results.RangeMilliMeter; } 
00337       else 
00338        { ErrState = VL53L0X_ERROR_RANGE_ERROR; return 0;}
00339 }
00340 
00341 /******************************************************************************/
00342 /******************************************************************************/
00343 /****************** Actual Measurement functions      *************************/
00344 /******************************************************************************/
00345 /******************************************************************************/
00346 
00347 TRangeResults VL53L0X::Get_Measurement(TOperatingMode operating_mode)
00348 {   TRangeResults p_data;
00349 
00350     switch (operating_mode) {
00351         case op_single_shot_poll: 
00352             Perf_single_ranging_measurement(&p_data);
00353             break;
00354         case op_poll:
00355             Poll_Measure_Completion();
00356             Get_ranging_results(&p_data);
00357             if (ErrState == VL53L0X_OK) { // Clear the interrupt
00358                 Clear_interrupt_mask(REG_SYSINT_GPIO_NEW_SAMPLE_READY);
00359                 Polling_delay();
00360               }
00361             break;
00362         case op_INT:
00363             Get_ranging_results(&p_data);
00364             Clear_interrupt_mask(REG_SYSINT_CLEAR | REG_RESULT_INTERRUPT_STATUS);
00365       } // switch
00366     return p_data;
00367 }
00368 
00369 /** Get part to part calibration offset;   Should only be used after a 
00370     successful call to @a VL53L0X_DataInit to backup device NVM value **/
00371 int32_t VL53L0X::Get_Offset_Cal_um()
00372 {   uint16_t range_offset_register;
00373     int16_t c_max_offset = 2047;
00374     int16_t c_offset_range = 4096;
00375 
00376     /* Note that offset has 10.2 format */
00377     range_offset_register = Read_Word(REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM); 
00378 
00379     if (ErrState == VL53L0X_OK) {
00380         range_offset_register = (range_offset_register & 0x0fff);
00381 
00382         /* Apply 12 bit 2's complement conversion */
00383         if (range_offset_register > c_max_offset) 
00384             { return (int16_t)(range_offset_register - c_offset_range)  * 250; } 
00385           else 
00386             { return (int16_t)range_offset_register * 250;}
00387     }
00388     else return 0; 
00389 }
00390 
00391 void VL53L0X::Set_Offset_Cal_um(int32_t Offset_Cal_um)
00392 {   int32_t c_max_offset_um =  511000;
00393     int32_t c_min_offset_um = -512000;
00394     int16_t c_offset_range  =    4096;
00395     uint32_t encoded_offset_val;
00396 
00397     if (Offset_Cal_um > c_max_offset_um) { Offset_Cal_um = c_max_offset_um; } 
00398       else
00399         if (Offset_Cal_um < c_min_offset_um) { Offset_Cal_um = c_min_offset_um; }
00400 
00401     /* The offset register is 10.2 format and units are mm
00402      * therefore conversion is applied by a division of 250. */
00403     if (Offset_Cal_um >= 0) { encoded_offset_val = Offset_Cal_um / 250; } 
00404       else { encoded_offset_val = c_offset_range + Offset_Cal_um / 250; }
00405 
00406     Write_Word(REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM,  encoded_offset_val);
00407 }
00408 
00409 void VL53L0X::VL53L0X_Apply_Offset_Cal()
00410 {   int32_t Summed_Offset_Cal_um;
00411 
00412     /* Read back current device offset, and remember in case later someone wants to use it */
00413     if (ErrState == VL53L0X_OK) { Last_Offset_Cal_um = Get_Offset_Cal_um(); }
00414 
00415     /* Apply Offset Adjustment derived from 400mm measurements */
00416     if (ErrState == VL53L0X_OK) 
00417      {  Summed_Offset_Cal_um = Last_Offset_Cal_um + (int32_t) NVM_Offset_Cal_um;
00418         Set_Offset_Cal_um(Summed_Offset_Cal_um);
00419         /* remember current,adjusted offset */
00420         if (ErrState == VL53L0X_OK) { CurrParams.Offset_Cal_um  = Summed_Offset_Cal_um; }
00421      }
00422 }
00423 
00424 void VL53L0X::Get_measure_period_ms(uint32_t *p_measure_period_ms)
00425 {   uint16_t osc_calibrate_val;
00426     uint32_t im_period_ms;
00427 
00428     osc_calibrate_val = Read_Word(REG_OSC_CALIBRATE_VAL); 
00429 
00430     if (ErrState == VL53L0X_OK)  { im_period_ms = Read_DWord(REG_SYSTEM_MEASURE_PERIOD); }
00431 
00432     if (ErrState == VL53L0X_OK) {
00433         if (osc_calibrate_val != 0) 
00434           {*p_measure_period_ms =  im_period_ms / osc_calibrate_val; }
00435            CurrParams.Measure_Period_ms  = *p_measure_period_ms;
00436     }
00437 }
00438 
00439 void VL53L0X::Get_Xtalk_CompRate_MHz( TFP1616 *p_Xtalk_CompRate_MHz)
00440 {   uint16_t value;
00441     TFP1616 temp_fix1616;
00442 
00443     value = Read_Word(REG_XTALK_COMPENS_RATE_MHz);
00444     
00445     if (ErrState == VL53L0X_OK) {
00446         if (value == 0) {
00447             /* the Xtalk is disabled return value from memory */
00448             temp_fix1616 = CurrParams.Xtalk_CompRate_MHz ; 
00449             *p_Xtalk_CompRate_MHz = temp_fix1616;
00450             CurrParams.XTalk_Compens_En  = 0; 
00451         } else {
00452             temp_fix1616 = FP313_TO_FP1616(value);
00453             *p_Xtalk_CompRate_MHz = temp_fix1616;
00454             CurrParams.Xtalk_CompRate_MHz  = temp_fix1616; 
00455             CurrParams.XTalk_Compens_En  = 1; 
00456         }
00457     }
00458 }
00459 
00460 TFP1616 VL53L0X::Get_limit_chk_val( uint16_t limit_check_id )
00461 {   uint16_t temp16;
00462     TFP1616 temp_fix1616;
00463 
00464     switch (limit_check_id) {
00465         case VL53L0X_CHECKEN_SIGMA_FINAL_RANGE:  /* only internal computations: */
00466         case VL53L0X_CHECKEN_SIG_REF_CLIP: 
00467         case VL53L0X_CHECKEN_RANGE_IGNORE_THRESHOLD:
00468             return CurrParams.Limit_Chk_Val [limit_check_id];// need no more 'break';
00469 
00470         case VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE:
00471             temp16 = Read_Word(REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT); 
00472             temp_fix1616 = FP97_TO_FP1616(temp16);
00473             if (temp_fix1616 == 0)  /* disabled: return value from memory instead*/
00474                { temp_fix1616 = CurrParams.Limit_Chk_Val [limit_check_id];
00475                  CurrParams.Limit_Chk_En [limit_check_id] = 0;  } 
00476              else 
00477                 { CurrParams.Limit_Chk_Val [limit_check_id] = temp_fix1616; 
00478                   CurrParams.Limit_Chk_En [limit_check_id] = 1; }
00479             return temp_fix1616; // need no more 'break';
00480 
00481         case VL53L0X_CHECKEN_SIG_RATE_MSRC:
00482         case VL53L0X_CHECKEN_SIG_RATE_PRE_RANGE:
00483             temp16 = Read_Word(REG_PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT); 
00484             return FP97_TO_FP1616(temp16); // need no more break;
00485 
00486         default:
00487             ErrState = VL53L0X_ERROR_INVALID_PARAMS;
00488             return 0;
00489     }
00490 }
00491 
00492 uint8_t VL53L0X::Get_limit_chk_en(uint16_t limit_check_id )
00493 {   if (limit_check_id >= VL53L0X_CHECKEN_NUMBER_OF_CHECKS) 
00494         { ErrState = VL53L0X_ERROR_INVALID_PARAMS;
00495           return 0; } 
00496      else { return CurrParams.Limit_Chk_En [limit_check_id]; }
00497 }
00498 
00499 uint8_t VL53L0X::Get_Wrap_Around_Chk_En()
00500 {   /* Now using the private state field SequenceConfig instead of reading from device:
00501     uint8_t SequenceConfig;
00502     SequenceConfig = Read_Byte(REG_SYSTEM_SEQUENCE_CONFIG);
00503     Set_SequenceConfig( SequenceConfig ); // checks for ErrState   
00504     
00505     if (ErrState == VL53L0X_OK) {    
00506     */
00507         CurrParams.Wrap_Around_Chk_En  = (SequenceConfig >> 7) & 0x01;
00508         return CurrParams.Wrap_Around_Chk_En ;
00509     // } else return 0;
00510 }
00511 
00512 VL53L0X_Sequence_Steps_t VL53L0X::Get_sequence_step_enables()
00513 {   VL53L0X_Sequence_Steps_t p_sequence_steps;
00514     /* Now using the private state field SequenceConfig instead of reading from device:
00515     uint8_t SequenceConfig;
00516 
00517     SequenceConfig = Read_Byte(REG_SYSTEM_SEQUENCE_CONFIG);
00518     
00519     if (ErrState == VL53L0X_OK) {
00520     */
00521         p_sequence_steps.TccOn        = (SequenceConfig & 0x10) >> 4;
00522         p_sequence_steps.DssOn        = (SequenceConfig & 0x08) >> 3;
00523         p_sequence_steps.MsrcOn       = (SequenceConfig & 0x04) >> 2;
00524         p_sequence_steps.PreRangeOn   = (SequenceConfig & 0x40) >> 6;
00525         p_sequence_steps.FinalRangeOn = (SequenceConfig & 0x80) >> 7;
00526     // }
00527     return p_sequence_steps;
00528 }
00529 
00530 void VL53L0X::Set_vcsel_PPeriod(VL53L0X_Range_Phase Vcsel_Range_Phase, uint8_t vcsel_PPeriod_pclk)
00531 {   uint8_t vcsel_period_reg;
00532     uint8_t min_pre_vcsel_period_pclk = 12;
00533     uint8_t max_pre_vcsel_period_pclk = 18;
00534     uint8_t min_final_vcsel_period_pclk = 8;
00535     uint8_t max_final_vcsel_period_pclk = 14;
00536     uint32_t final_range_timeout_us;
00537     uint32_t pre_range_timeout_us;
00538     uint32_t msrc_timeout_us;
00539     uint8_t phase_cal_int = 0;
00540 
00541     /* Check if valid clock period requested */
00542     if ( ((vcsel_PPeriod_pclk % 2) != 0 ) /* Value must be an even number */
00543         ||
00544        ( Vcsel_Range_Phase == VL53L0X_VCSEL_PRE_RANGE &&
00545         ((vcsel_PPeriod_pclk < min_pre_vcsel_period_pclk)||
00546          (vcsel_PPeriod_pclk > max_pre_vcsel_period_pclk)  ) ) 
00547         ||
00548        ( Vcsel_Range_Phase == VL53L0X_VCSEL_FINAL_RANGE &&
00549         (vcsel_PPeriod_pclk < min_final_vcsel_period_pclk ||
00550          vcsel_PPeriod_pclk > max_final_vcsel_period_pclk) )  )
00551        { ErrState = VL53L0X_ERROR_INVALID_PARAMS; 
00552          return;}
00553 
00554     /* Apply specific settings for the requested clock period */
00555     if (Vcsel_Range_Phase == VL53L0X_VCSEL_PRE_RANGE) {
00556         /* Set phase check limits for pre-ranging*/
00557         if (vcsel_PPeriod_pclk == 12) {
00558             Write_Byte(REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,0x18);
00559             Write_Byte(REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW ,0x08);
00560         } else if (vcsel_PPeriod_pclk == 14) {
00561             Write_Byte(REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,0x30);
00562             Write_Byte(REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW ,0x08);
00563         } else if (vcsel_PPeriod_pclk == 16) {
00564             Write_Byte(REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,0x40);
00565             Write_Byte(REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW ,0x08);
00566         } else if (vcsel_PPeriod_pclk == 18) {
00567             Write_Byte(REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,0x50);
00568             Write_Byte(REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW ,0x08);
00569         }
00570     } else if (Vcsel_Range_Phase == VL53L0X_VCSEL_FINAL_RANGE) {
00571         if (vcsel_PPeriod_pclk == 8) {
00572             Write_Byte(REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,0x10);
00573             Write_Byte(REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW ,0x08);
00574             Write_Byte(REG_GLOBAL_CONFIG_VCSEL_WIDTH   ,0x02);
00575             Write_Byte(REG_ALGO_PHASECAL_CONFIG_TIMEOUT,0x0C);
00576             Write_Byte(0xff,0x01);
00577             Write_Byte(REG_ALGO_PHASECAL_LIM,0x30);
00578             Write_Byte(0xff,0x00);
00579         } else if (vcsel_PPeriod_pclk == 10) {
00580             Write_Byte(REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,0x28);
00581             Write_Byte(REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,0x08);
00582             Write_Byte(REG_GLOBAL_CONFIG_VCSEL_WIDTH,0x03);
00583             Write_Byte(REG_ALGO_PHASECAL_CONFIG_TIMEOUT,0x09);
00584             Write_Byte(0xff,0x01);
00585             Write_Byte(REG_ALGO_PHASECAL_LIM,0x20);
00586             Write_Byte(0xff,0x00);
00587         } else if (vcsel_PPeriod_pclk == 12) {
00588             Write_Byte(REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,0x38);
00589             Write_Byte(REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,0x08);
00590             Write_Byte(REG_GLOBAL_CONFIG_VCSEL_WIDTH,0x03);
00591             Write_Byte(REG_ALGO_PHASECAL_CONFIG_TIMEOUT,0x08);
00592             Write_Byte(0xff,0x01);
00593             Write_Byte(REG_ALGO_PHASECAL_LIM,0x20);
00594             Write_Byte(0xff,0x00);
00595         } else if (vcsel_PPeriod_pclk == 14) {
00596             Write_Byte(REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,0x048);
00597             Write_Byte(REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,0x08);
00598             Write_Byte(REG_GLOBAL_CONFIG_VCSEL_WIDTH,0x03);
00599             Write_Byte(REG_ALGO_PHASECAL_CONFIG_TIMEOUT,0x07);
00600             Write_Byte(0xff,0x01);
00601             Write_Byte(REG_ALGO_PHASECAL_LIM, 0x20);
00602             Write_Byte(0xff,0x00);
00603         }
00604     }
00605 
00606     /* Re-calculate and apply timeouts,in macro periods */
00607     if (ErrState == VL53L0X_OK) {
00608         /* Converts the encoded VCSEL period register value into the real period in PLL clocks */
00609         /* Flattened from procedure called Encode_vcsel_period */
00610         vcsel_period_reg = (vcsel_PPeriod_pclk >> 1) - 1;
00611         
00612         /* When the VCSEL period for the pre or final range is changed,
00613         * the corresponding timeout must be read from the device using
00614         * the current VCSEL period,then the new VCSEL period can be
00615         * applied. The timeout then must be written back to the device
00616         * using the new VCSEL period.
00617         * For the MSRC timeout,the same applies - this timeout being
00618         * dependant on the pre-range vcsel period.
00619         */
00620         switch (Vcsel_Range_Phase) {
00621           case VL53L0X_VCSEL_PRE_RANGE:
00622             Get_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_PRE_RANGE,&pre_range_timeout_us);
00623 
00624             if (ErrState == VL53L0X_OK)
00625                 Get_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_MSRC,&msrc_timeout_us);
00626 
00627             Write_Byte(REG_PRE_RANGE_CONFIG_VCSEL_PERIOD,vcsel_period_reg);
00628 
00629             if (ErrState == VL53L0X_OK)
00630                 Set_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_PRE_RANGE,pre_range_timeout_us);
00631 
00632             if (ErrState == VL53L0X_OK)
00633                 Set_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_MSRC,msrc_timeout_us);
00634 
00635             PreRangeVcselPPeriod  = vcsel_PPeriod_pclk;
00636             break;
00637             
00638           case VL53L0X_VCSEL_FINAL_RANGE:
00639             Get_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_FINAL_RANGE,&final_range_timeout_us);
00640 
00641             Write_Byte(REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD,vcsel_period_reg);
00642 
00643             if (ErrState == VL53L0X_OK)
00644                 Set_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_FINAL_RANGE,final_range_timeout_us);
00645 
00646             FinalRangeVcselPPeriod  = vcsel_PPeriod_pclk;
00647             break;
00648           default: ErrState = VL53L0X_ERROR_INVALID_PARAMS;
00649         }
00650     }
00651 
00652     /* Finally,the timing budget is re-applied */
00653     if (ErrState == VL53L0X_OK) 
00654       { Set_Measure_Time_Budget_us(CurrParams.Measure_Time_Budget_us); }
00655 
00656     /* Perform the phase calibration. This is needed after changing on  vcsel period.
00657      * get_data_enable = 0,restore_config = 1 */
00658      Perf_phase_calibration(&phase_cal_int,0,1);
00659 }
00660 
00661 #define VL53L0X_MACRO_PERIOD_NS 3813; // = ( VL53L0X_PLL_PERIOD_PS * VL53L0X_MACRO_PERIOD_VCLKS / 1000 )
00662 
00663 /* To convert register value into us */
00664 uint32_t VL53L0X::Calc_timeout_us(uint16_t timeout_period_mclks,
00665         uint8_t vcsel_period_pclks)
00666 {
00667     uint32_t macro_period_ns;
00668     uint32_t actual_timeout_period_us = 0;
00669 
00670     macro_period_ns = (uint32_t) (vcsel_period_pclks ) *  VL53L0X_MACRO_PERIOD_NS;
00671 
00672     actual_timeout_period_us = ((timeout_period_mclks * macro_period_ns) + 500) / 1000;
00673 
00674     return actual_timeout_period_us;
00675 }
00676 
00677 void VL53L0X::Get_Sequence_Step_Timeout(VL53L0X_SequenceStepId sequence_step_id,
00678         uint32_t *p_time_out_micro_secs)
00679 {   uint8_t current_vcsel_PPeriod_p_clk;
00680     uint8_t encoded_time_out_byte = 0;
00681     uint32_t timeout_us = 0;
00682     uint16_t pre_range_encoded_time_out = 0;
00683     uint16_t msrc_time_out_m_clks;
00684     uint16_t pre_range_time_out_m_clks;
00685     uint16_t final_range_time_out_m_clks = 0;
00686     uint16_t final_range_encoded_time_out;
00687     VL53L0X_Sequence_Steps_t sequence_steps;
00688 
00689     if ((sequence_step_id == VL53L0X_SEQUENCESTEP_TCC ) ||
00690         (sequence_step_id == VL53L0X_SEQUENCESTEP_DSS ) ||
00691         (sequence_step_id == VL53L0X_SEQUENCESTEP_MSRC)   ) {
00692 
00693         current_vcsel_PPeriod_p_clk = /*  Gets and converts the VCSEL period register into actual clock periods */
00694                 ( Read_Byte(REG_PRE_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1;
00695 
00696         if (ErrState == VL53L0X_OK) {
00697             encoded_time_out_byte = Read_Byte(REG_MSRC_CONFIG_TIMEOUT_MACROP); 
00698         }
00699         msrc_time_out_m_clks = Decode_timeout(encoded_time_out_byte);
00700 
00701         timeout_us = Calc_timeout_us(msrc_time_out_m_clks,
00702                                 current_vcsel_PPeriod_p_clk);
00703     } else if (sequence_step_id == VL53L0X_SEQUENCESTEP_PRE_RANGE) {
00704 
00705         current_vcsel_PPeriod_p_clk = /*  Gets and converts the VCSEL period register into actual clock periods */
00706                 ( Read_Byte(REG_PRE_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1;
00707 
00708         /* Retrieve PRE-RANGE Timeout in Macro periods (MCLKS) */
00709         if (ErrState == VL53L0X_OK) {
00710 
00711             pre_range_encoded_time_out = Read_Word(REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI); 
00712 
00713             pre_range_time_out_m_clks = Decode_timeout(pre_range_encoded_time_out);
00714 
00715             timeout_us = Calc_timeout_us(pre_range_time_out_m_clks,
00716                                     current_vcsel_PPeriod_p_clk);
00717         }
00718     } else if (sequence_step_id == VL53L0X_SEQUENCESTEP_FINAL_RANGE) {
00719 
00720         sequence_steps = Get_sequence_step_enables();
00721         pre_range_time_out_m_clks = 0;
00722 
00723         if (sequence_steps.PreRangeOn) {
00724             current_vcsel_PPeriod_p_clk = /*  Gets and converts the VCSEL period register into actual clock periods */
00725                 ( Read_Byte(REG_PRE_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1;
00726 
00727             /* Retrieve PRE-RANGE Timeout in Macro periods (MCLKS) */
00728             if (ErrState == VL53L0X_OK) {
00729                 pre_range_encoded_time_out = Read_Word(REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI); 
00730                 pre_range_time_out_m_clks = Decode_timeout(pre_range_encoded_time_out);
00731             }
00732         }
00733 
00734         if (ErrState == VL53L0X_OK) {
00735             current_vcsel_PPeriod_p_clk =  /*  Get and converts the VCSEL period register into actual clock periods */
00736                 ( Read_Byte(REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1; 
00737 
00738         }
00739 
00740         /* Retrieve FINAL-RANGE Timeout in Macro periods (MCLKS) */
00741         if (ErrState == VL53L0X_OK) {
00742             final_range_encoded_time_out = Read_Word(REG_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI); 
00743             final_range_time_out_m_clks = Decode_timeout(final_range_encoded_time_out);
00744         }
00745 
00746         final_range_time_out_m_clks -= pre_range_time_out_m_clks;
00747         timeout_us = Calc_timeout_us(final_range_time_out_m_clks,current_vcsel_PPeriod_p_clk);
00748     }
00749 
00750     *p_time_out_micro_secs = timeout_us;
00751 }
00752 
00753 uint32_t VL53L0X::Get_Measure_Time_Budget_us()
00754 {   VL53L0X_Sequence_Steps_t sequence_steps;
00755     uint32_t p_Measure_Time_Budget_us;
00756     uint32_t final_range_timeout_us;
00757     uint32_t msrc_dcc_tcc_timeout_us= 2000;
00758     uint32_t start_overhead_us      = 1910;
00759     uint32_t end_overhead_us        = 960;
00760     uint32_t msrc_overhead_us       = 660;
00761     uint32_t tcc_overhead_us        = 590;
00762     uint32_t dss_overhead_us        = 690;
00763     uint32_t pre_range_overhead_us  = 660;
00764     uint32_t final_range_overhead_us= 550;
00765     uint32_t pre_range_timeout_us   = 0;
00766 
00767     if (ErrState != VL53L0X_OK)  {return 0; } // do nothing while in Error State!!!!
00768 
00769     /* Start and end overhead times always present */
00770     p_Measure_Time_Budget_us = start_overhead_us + end_overhead_us;
00771 
00772     sequence_steps = Get_sequence_step_enables();
00773 
00774     if (sequence_steps.TccOn  || sequence_steps.MsrcOn || sequence_steps.DssOn) 
00775       { Get_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_MSRC, &msrc_dcc_tcc_timeout_us);
00776 
00777         if (ErrState == VL53L0X_OK) {
00778             if (sequence_steps.TccOn)
00779                 { p_Measure_Time_Budget_us += msrc_dcc_tcc_timeout_us + tcc_overhead_us; }
00780 
00781             if (sequence_steps.DssOn) {
00782                 p_Measure_Time_Budget_us += 2 * (msrc_dcc_tcc_timeout_us + dss_overhead_us);
00783             } else if (sequence_steps.MsrcOn) {
00784                 p_Measure_Time_Budget_us +=  msrc_dcc_tcc_timeout_us + msrc_overhead_us;
00785             }
00786         }
00787     }
00788 
00789     if ( (ErrState == VL53L0X_OK) && sequence_steps.PreRangeOn) {
00790             Get_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_PRE_RANGE, &pre_range_timeout_us);
00791             p_Measure_Time_Budget_us += pre_range_timeout_us + pre_range_overhead_us;
00792         }
00793 
00794     if (ErrState == VL53L0X_OK) {
00795         if (sequence_steps.FinalRangeOn) {
00796             Get_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_FINAL_RANGE, &final_range_timeout_us);
00797             p_Measure_Time_Budget_us += (final_range_timeout_us + final_range_overhead_us);
00798         }
00799     }
00800 
00801     if (ErrState == VL53L0X_OK) 
00802        { CurrParams.Measure_Time_Budget_us = p_Measure_Time_Budget_us;  }
00803 
00804     return p_Measure_Time_Budget_us;
00805 }
00806 
00807 VL53L0X_DeviceParams_t VL53L0X::Get_device_parameters()
00808 {   VL53L0X_DeviceParams_t device_params  = {0};
00809     int i;
00810 
00811     if (ErrState != VL53L0X_OK)  {return device_params; } // do nothing while in Error State!!!!
00812 
00813     device_params.DeviceMode  = CurrParams.DeviceMode ;
00814     device_params.XTalk_Compens_En  = 0; 
00815     device_params.Offset_Cal_um  = Get_Offset_Cal_um();
00816     
00817     Get_measure_period_ms(&(device_params.Measure_Period_ms ));
00818 
00819     if (ErrState == VL53L0X_OK)
00820         Get_Xtalk_CompRate_MHz(&(device_params.Xtalk_CompRate_MHz ));
00821 
00822     if (ErrState == VL53L0X_OK) {
00823         for (i = 0; i < VL53L0X_CHECKEN_NUMBER_OF_CHECKS; i++) 
00824           {/* get first the values,then the enables.  GetLimitCheckValue will 
00825            modify the enable flags  */
00826             if (ErrState == VL53L0X_OK) 
00827               { device_params.Limit_Chk_Val [i] = Get_limit_chk_val(i); } 
00828               else { break; }
00829             if (ErrState == VL53L0X_OK) 
00830               { device_params.Limit_Chk_En [i]= Get_limit_chk_en(i);} 
00831               else { break; }
00832         }
00833     }
00834 
00835     if (ErrState == VL53L0X_OK) {
00836         device_params.Wrap_Around_Chk_En  = Get_Wrap_Around_Chk_En();}
00837 
00838     /* Need to be done at the end as it uses VCSELPPeriod */
00839     if (ErrState == VL53L0X_OK) {
00840         device_params.Measure_Time_Budget_us = Get_Measure_Time_Budget_us();  }
00841 
00842     return device_params;
00843 }
00844 
00845 void VL53L0X::Set_limit_chk_val(uint16_t limit_check_id, TFP1616 limit_chk_val)
00846 {  /* first verify that the ID is within bounds .. */
00847     if (limit_check_id>=VL53L0X_CHECKEN_NUMBER_OF_CHECKS)
00848         {  ErrState = VL53L0X_ERROR_INVALID_PARAMS;  return; }
00849 
00850     /*  Under all other circumstances store value in local array:  */
00851     CurrParams.Limit_Chk_Val [limit_check_id] = limit_chk_val;
00852     
00853     /* in addition, if enabled, then write the external ones also to the Registers  */
00854     if (CurrParams.Limit_Chk_En [ limit_check_id ])  
00855         switch (limit_check_id) {
00856             case VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE:
00857                 Write_Word(REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT,
00858                                             FP1616_TO_FP97(limit_chk_val));
00859                 break;
00860             case VL53L0X_CHECKEN_SIG_RATE_MSRC:
00861             case VL53L0X_CHECKEN_SIG_RATE_PRE_RANGE:
00862                 Write_Word(REG_PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT, 
00863                                               FP1616_TO_FP97(limit_chk_val));
00864                 break;
00865         } // switch
00866 }
00867 
00868 
00869 
00870 
00871 void VL53L0X::Get_interrupt_mask_status(uint32_t *p_interrupt_mask_status)
00872 {   uint8_t intStat;
00873 
00874     intStat = Read_Byte(REG_RESULT_INTERRUPT_STATUS); 
00875     *p_interrupt_mask_status = intStat & 0x07;
00876     if (intStat & 0x18) { ErrState =  VL53L0X_ERROR_RANGE_ERROR;  }
00877 }
00878 
00879 uint8_t VL53L0X::Get_Measurement_Ready()
00880 {   uint8_t sys_range_status_register;
00881     uint32_t interrupt_mask;
00882 
00883     if (GpioFunctionality == REG_SYSINT_GPIO_NEW_SAMPLE_READY) 
00884       { Get_interrupt_mask_status(&interrupt_mask);
00885         if (interrupt_mask ==  REG_SYSINT_GPIO_NEW_SAMPLE_READY) 
00886            { return 1; } else { return 0; }
00887       } 
00888       else 
00889       { sys_range_status_register = Read_Byte(REG_RESULT_RANGE_STATUS); 
00890         if ( ( ErrState == VL53L0X_OK ) & (sys_range_status_register & 0x01) )
00891             { return 1; } else { return 0; }
00892       }
00893 }
00894 
00895 void VL53L0X::Polling_delay()
00896 {
00897    // do nothing VL53L0X_OsDelay();
00898 }
00899 
00900 void VL53L0X::Poll_Measure_Completion()
00901 {   uint8_t new_data_ready;
00902     uint32_t loop_nb = 0;
00903     
00904     if (ErrState != VL53L0X_OK) { return; } // Do nothing if not Cleared error
00905 
00906     new_data_ready = Get_Measurement_Ready();
00907 
00908     while ((ErrState==0) && (new_data_ready != 1) )
00909       { Polling_delay();
00910         new_data_ready = Get_Measurement_Ready();
00911         if (loop_nb++ >= VL53L0X_DEFAULT_MAX_LOOP) ErrState=VL53L0X_ERROR_TIME_OUT;
00912       } // while ;
00913 }
00914 
00915 /* Group Device Interrupt Functions */
00916 void VL53L0X::Clear_interrupt_mask(uint32_t interrupt_mask)
00917 {   uint8_t loop_count = 0;
00918     uint8_t byte;
00919 
00920     if (ErrState != VL53L0X_OK) { return; } // Do nothing if not Cleared error
00921 
00922     /* clear bit 0 range interrupt,bit 1 error interrupt */
00923     do {
00924         Write_Byte(REG_SYSINT_CLEAR,0x01);
00925         Write_Byte(REG_SYSINT_CLEAR,0x00);
00926         byte = Read_Byte(REG_RESULT_INTERRUPT_STATUS); 
00927         if (loop_count++ > 3) {ErrState =VL53L0X_ERROR_INTERRUPT_NOT_CLEARED;}
00928         } while (((byte & 0x07) != 0x00)  && (ErrState == VL53L0X_OK));    
00929 
00930 }
00931 
00932 void VL53L0X::Perf_single_Ref_calibration(uint8_t vhv_init_byte)
00933 {   if (ErrState != VL53L0X_OK)  {return; } // no activity while in Error State!!!!
00934     Write_Byte(REG_SYSRANGE_START, REG_SYSRANGE_MODE_START_STOP | vhv_init_byte);
00935     Poll_Measure_Completion();
00936     Clear_interrupt_mask(0);
00937     Write_Byte(REG_SYSRANGE_START,0x00);
00938 }
00939 
00940 void VL53L0X::Ref_calibration_io(uint8_t read_not_write,
00941         uint8_t vhv_settings,uint8_t phase_cal,
00942         uint8_t *p_vhv_settings,uint8_t *p_phase_cal,
00943         const uint8_t vhv_enable,const uint8_t phase_enable)
00944 {   uint8_t phase_calint = 0;
00945 
00946     /* Read VHV from device */
00947     Write_Byte(0xFF,0x01);
00948     Write_Byte(0x00,0x00);
00949     Write_Byte(0xFF,0x00);
00950 
00951     if (read_not_write) {
00952         if (vhv_enable  ) { *p_vhv_settings = Read_Byte(0xCB); }
00953         if (phase_enable) { phase_calint = Read_Byte(0xEE); }
00954       } 
00955       else {
00956         if (vhv_enable  ) { Write_Byte(0xCB,vhv_settings); }
00957         if (phase_enable) { Register_BitMask(0xEE,0x80,phase_cal); }
00958       }
00959 
00960     Write_Byte(0xFF,0x01);
00961     Write_Byte(0x00,0x01);
00962     Write_Byte(0xFF,0x00);
00963 
00964     *p_phase_cal = (uint8_t)(phase_calint & 0xEF);
00965 }
00966 
00967 void VL53L0X::Perf_vhv_calibration(uint8_t *p_vhv_settings,
00968         const uint8_t get_data_enable,   const uint8_t restore_config)
00969 {   uint8_t orig_sequence_config = 0;
00970     uint8_t vhv_settings = 0;
00971     uint8_t phase_cal = 0;
00972     uint8_t phase_cal_int = 0;
00973 
00974     /* store the value of the sequence config,
00975      * this will be reset before the end of the function  */
00976     orig_sequence_config = SequenceConfig;
00977 
00978     /* Run VHV */
00979     Set_SequenceConfig( 0x01 );
00980     Perf_single_Ref_calibration(0x40);
00981 
00982     /* Read VHV from device */
00983     if ((ErrState == VL53L0X_OK) && (get_data_enable == 1)) 
00984         { Ref_calibration_io(1,vhv_settings,phase_cal,/* Not used here */
00985                             p_vhv_settings,&phase_cal_int, 1,0); }
00986       else { *p_vhv_settings = 0; }
00987 
00988     if (restore_config) { /* restore the previous Sequence Config */
00989         Set_SequenceConfig( orig_sequence_config ); } // checks for ErrState
00990 }
00991 
00992 void VL53L0X::Perf_phase_calibration(uint8_t *p_phase_cal,const uint8_t get_data_enable,
00993         const uint8_t restore_config)
00994 {   uint8_t orig_sequence_config;
00995     uint8_t vhv_settings = 0;
00996     uint8_t phase_cal = 0;
00997     uint8_t vhv_settingsint;
00998 
00999     if (ErrState != VL53L0X_OK) { return; } // Do nothing if not Cleared error
01000     
01001     /* store the value of the sequence config, this will be reset before the end of the function  */
01002     orig_sequence_config = SequenceConfig;
01003 
01004     /* Run PhaseCal: */ 
01005     Set_SequenceConfig( 0x02 ); // sets REG_SYSTEM_SEQUENCE_CONFIG
01006     Perf_single_Ref_calibration(0x0);
01007 
01008     /* Read PhaseCal from device */
01009     if ((ErrState == VL53L0X_OK) && (get_data_enable == 1)) 
01010        { Ref_calibration_io(1,vhv_settings,phase_cal,/* Not used here */
01011                             &vhv_settingsint,p_phase_cal,  0,1); }
01012        else {  *p_phase_cal = 0; }
01013 
01014     if (restore_config) { /* restore the previous Sequence Config */
01015         Set_SequenceConfig( orig_sequence_config ); }
01016 }
01017 
01018 void VL53L0X::Perf_Ref_calibration(uint8_t *p_vhv_settings,
01019                                uint8_t *p_phase_cal,  uint8_t get_data_enable)
01020 {   uint8_t orig_sequence_config;
01021 
01022     /* store the value of the sequence config,
01023      * this will be reset before the end of the function */
01024     orig_sequence_config =  SequenceConfig;
01025 
01026     /* In the following function we don't save the config to optimize
01027      * writes on device. Config is saved and restored only once. */
01028     Perf_vhv_calibration(p_vhv_settings,get_data_enable,0);
01029     Perf_phase_calibration(p_phase_cal,get_data_enable,0);
01030 
01031     /* restore the previous Sequence Config */
01032     Set_SequenceConfig( orig_sequence_config ); // sets REG_SYSTEM_SEQUENCE_CONFIG
01033 }
01034 
01035 void VL53L0X::Get_Next_Good_SPAD(uint8_t good_SPAD_array[],uint32_t size,
01036                                  uint32_t curr,int32_t *p_next)
01037 {   uint32_t start_index;
01038     uint32_t fine_offset;
01039     uint32_t c_SPADS_per_byte = 8;
01040     uint32_t coarse_index;
01041     uint32_t fine_index;
01042     uint8_t data_byte;
01043     uint8_t success = 0;
01044 
01045     /* Starting with the current good SPAD,loop through the array to find
01046      * the next. i.e. the next bit set in the sequence.
01047      * The coarse index is the byte index of the array and the fine index is
01048      * the index of the bit within each byte.  */
01049     *p_next = -1;
01050 
01051     start_index = curr / c_SPADS_per_byte;
01052     fine_offset = curr % c_SPADS_per_byte;
01053 
01054     for (coarse_index = start_index; ((coarse_index < size) && !success);
01055             coarse_index++) {
01056         fine_index = 0;
01057         data_byte = good_SPAD_array[coarse_index];
01058 
01059         if (coarse_index == start_index) {
01060             /* locate the bit position of the provided current
01061              * SPAD bit before iterating */
01062             data_byte >>= fine_offset;
01063             fine_index = fine_offset;
01064         }
01065 
01066         while (fine_index < c_SPADS_per_byte) {
01067             if ((data_byte & 0x1) == 1) {
01068                 success = 1;
01069                 *p_next = coarse_index * c_SPADS_per_byte + fine_index;
01070                 break;
01071             }
01072             data_byte >>= 1;
01073             fine_index++;
01074         }
01075     }
01076 }
01077 
01078 void VL53L0X::Enable_SPAD_bit(uint8_t SPAD_array[],uint32_t size,uint32_t SPAD_index)
01079 {   uint32_t c_SPADS_per_byte = 8;
01080     uint32_t coarse_index;
01081     uint32_t fine_index;
01082 
01083     coarse_index = SPAD_index / c_SPADS_per_byte;
01084     fine_index = SPAD_index % c_SPADS_per_byte;
01085     if (coarse_index >= size) { ErrState = VL53L0X_ERROR_REF_SPAD_INIT; }
01086        else { SPAD_array[coarse_index] |= (1 << fine_index); }
01087 }
01088 
01089 void VL53L0X::Enable_Ref_SPADS( uint8_t aperture_SPADS, uint8_t good_SPAD_array[], 
01090         uint8_t SPAD_array[],  uint32_t size, uint32_t start, uint32_t offset,
01091         uint32_t SPAD_count,  uint32_t *p_last_SPAD )
01092 {   uint32_t index;
01093     uint32_t i;
01094     int32_t  next_good_SPAD = offset;
01095     uint32_t current_SPAD;
01096     uint8_t  check_SPAD_array[6];
01097 
01098     /* This function takes in a SPAD array which may or may not have SPADS
01099      * already enabled and appends from a given offset a requested number
01100      * of new SPAD enables. The 'good SPAD map' is applied to
01101      * determine the next SPADS to enable.
01102      *
01103      * This function applies to only aperture or only non-aperture SPADS.
01104      * Checks are performed to ensure this.
01105      */
01106 
01107     current_SPAD = offset;
01108     for (index = 0; index < SPAD_count; index++) {
01109         Get_Next_Good_SPAD(good_SPAD_array,size,current_SPAD, &next_good_SPAD);
01110 
01111         if (next_good_SPAD == -1) 
01112           {  ErrState = VL53L0X_ERROR_REF_SPAD_INIT;
01113             break;  }
01114        
01115 
01116         /* Confirm that the next good SPAD is non-aperture */
01117         if (Is_ApertureSPAD(start + next_good_SPAD) != aperture_SPADS) {
01118             /* if we can't get the required number of good aperture
01119              * SPADS from the current quadrant then this is an error */
01120             ErrState = VL53L0X_ERROR_REF_SPAD_INIT;
01121             break;}
01122             
01123         current_SPAD = (uint32_t)next_good_SPAD;
01124         Enable_SPAD_bit(SPAD_array,size,current_SPAD);
01125         current_SPAD++;
01126     }
01127     *p_last_SPAD = current_SPAD;
01128 
01129     if (ErrState == VL53L0X_OK) 
01130       { I2c_Write(REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0, SPAD_array,6); } // set_Ref_SPAD_map()
01131 
01132     if (ErrState == VL53L0X_OK) {
01133         // Get the ref_SPAD_map from the device
01134         I2c_Read(REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0,check_SPAD_array,6);
01135 
01136         /* Compare SPAD maps. If not equal report error. */
01137         i = 0;
01138         while (i < size) {
01139             if (SPAD_array[i] != check_SPAD_array[i]) {
01140                 ErrState = VL53L0X_ERROR_REF_SPAD_INIT;
01141                 break;
01142             }
01143             i++;
01144         }
01145     }
01146 }
01147 
01148 void VL53L0X::Set_device_mode(VL53L0X_DeviceModes device_mode)
01149 {   if (ErrState != VL53L0X_OK)  {return; } // no reaction while in Error State!!!!
01150 
01151     switch (device_mode) {
01152         case VL53L0X_DEVICEMODE_SINGLE_RANGING:
01153         case VL53L0X_DEVICEMODE_CONTINUOUS_RANGING:
01154         case VL53L0X_DEVICEMODE_CONTINUOUS_TIMED_RANGING:
01155         case VL53L0X_DEVICEMODE_GPIO_DRIVE:
01156         case VL53L0X_DEVICEMODE_GPIO_OSC: /* Supported modes */
01157             CurrParams.DeviceMode  = device_mode; 
01158             break;
01159         default:  /* Unsupported mode */
01160             ErrState = VL53L0X_ERROR_MODE_NOT_SUPPORTED;
01161     }
01162 }
01163 
01164 void VL53L0X::Set_interrupt_thresholds(VL53L0X_DeviceModes device_mode,TFP1616 threshold_low,
01165         TFP1616 threshold_high)
01166 {   uint16_t threshold16;
01167 
01168     /* no dependency on DeviceMode for FlightSense */
01169     /* Need to divide by 2 because the FW will apply a x2 */
01170     threshold16 = (uint16_t)((threshold_low >> 17) & 0x00fff);
01171     Write_Word(REG_SYSTEM_THRESH_LOW,threshold16);
01172 
01173     /* Need to divide by 2 because the FW will apply a x2 */
01174     threshold16 = (uint16_t)((threshold_high >> 17) & 0x00fff);
01175     Write_Word(REG_SYSTEM_THRESH_HIGH,threshold16);
01176 }
01177 
01178 void VL53L0X::Get_interrupt_thresholds(VL53L0X_DeviceModes device_mode,TFP1616 *p_threshold_low,
01179         TFP1616 *p_threshold_high)
01180 {   uint16_t threshold16;
01181 
01182     /* no dependency on DeviceMode for FlightSense */
01183     threshold16 = Read_Word(REG_SYSTEM_THRESH_LOW); 
01184     /* Need to multiply by 2 because the FW will apply a x2 */
01185     *p_threshold_low = (TFP1616)((0x00fff & threshold16) << 17);
01186 
01187     if (ErrState == VL53L0X_OK) {
01188         threshold16 = Read_Word(REG_SYSTEM_THRESH_HIGH); 
01189         /* Need to multiply by 2 because the FW will apply a x2 */
01190         *p_threshold_high =  (TFP1616)((0x00fff & threshold16) << 17);
01191     }
01192 }
01193 
01194 void VL53L0X::Load_tuning_settings(uint8_t *p_tuning_setting_buffer)
01195 {   int i;
01196     int index;
01197     uint8_t msb;
01198     uint8_t lsb;
01199     uint8_t select_param;
01200     uint8_t number_of_writes;
01201     uint8_t address;
01202     uint8_t local_buffer[4]; /* max */
01203     uint16_t temp16;
01204 
01205     index = 0;
01206 
01207     while ((*(p_tuning_setting_buffer + index) != 0) &&
01208             (ErrState == VL53L0X_OK)) {
01209         number_of_writes = *(p_tuning_setting_buffer + index);
01210         index++;
01211         if (number_of_writes == 0xFF) {
01212             /* internal parameters */
01213             select_param = *(p_tuning_setting_buffer + index);
01214             index++;
01215             switch (select_param) {
01216                 case 0: /* uint16_t SigmaEstRefArray -> 2 bytes */
01217                     msb = *(p_tuning_setting_buffer + index);
01218                     index++;
01219                     lsb = *(p_tuning_setting_buffer + index);
01220                     index++;
01221                     temp16 = VL53L0X_MAKEUINT16(lsb,msb);
01222                     SigmaEstRefArray  = temp16; 
01223                     break;
01224                 case 1: /* uint16_t SigmaEstEffPulseWidth -> 2 bytes */
01225                     msb = *(p_tuning_setting_buffer + index);
01226                     index++;
01227                     lsb = *(p_tuning_setting_buffer + index);
01228                     index++;
01229                     temp16 = VL53L0X_MAKEUINT16(lsb,msb);
01230                     SigmaEstEffPulseWidth  = temp16; 
01231                     break;
01232                 case 2: /* uint16_t SigmaEstEffAmbWidth -> 2 bytes */
01233                     msb = *(p_tuning_setting_buffer + index);
01234                     index++;
01235                     lsb = *(p_tuning_setting_buffer + index);
01236                     index++;
01237                     temp16 = VL53L0X_MAKEUINT16(lsb,msb);
01238                     SigmaEstEffAmbWidth  = temp16; 
01239                     break;
01240                 case 3: /* uint16_t targetRefRate -> 2 bytes */
01241                     msb = *(p_tuning_setting_buffer + index);
01242                     index++;
01243                     lsb = *(p_tuning_setting_buffer + index);
01244                     index++;
01245                     temp16 = VL53L0X_MAKEUINT16(lsb,msb);
01246                     targetRefRate = temp16; 
01247                     break;
01248                 default: /* invalid parameter */
01249                     ErrState = VL53L0X_ERROR_INVALID_PARAMS;
01250             }
01251         } else if (number_of_writes <= 4) {
01252             address = *(p_tuning_setting_buffer + index);
01253             index++;
01254             for (i = 0; i < number_of_writes; i++) {
01255                 local_buffer[i] = *(p_tuning_setting_buffer + index);
01256                 index++;
01257             }
01258             I2c_Write(address,local_buffer,number_of_writes);
01259         } else {
01260             ErrState = VL53L0X_ERROR_INVALID_PARAMS;
01261         }
01262     }
01263 }
01264 
01265 void VL53L0X::Check_and_load_interrupt_settings(uint8_t start_not_stopflag)
01266 {   uint8_t interrupt_config;
01267     TFP1616 threshold_low;
01268     TFP1616 threshold_high;
01269     
01270     if (ErrState != VL53L0X_OK) { return; } // Do nothing if not Cleared error
01271 
01272     interrupt_config = GpioFunctionality; 
01273 
01274     if ((interrupt_config == GPIO_FUNC_THRESHOLD_CROSSED_LOW  ) ||
01275         (interrupt_config == GPIO_FUNC_THRESHOLD_CROSSED_HIGH ) ||
01276         (interrupt_config == GPIO_FUNC_THRESHOLD_CROSSED_OUT  )) {
01277         Get_interrupt_thresholds(VL53L0X_DEVICEMODE_CONTINUOUS_RANGING,
01278                  &threshold_low,&threshold_high);
01279 
01280         if (((threshold_low > 255 * 65536) || (threshold_high > 255 * 65536)) &&
01281             (ErrState == VL53L0X_OK)) 
01282           { if (start_not_stopflag != 0) 
01283                {Load_tuning_settings(InterruptThresholdSettings); } 
01284              else 
01285                {Write_Byte(0xFF,0x04);
01286                 Write_Byte(0x70,0x00);
01287                 Write_Byte(0xFF,0x00);
01288                 Write_Byte(0x80,0x00);
01289               }
01290         }
01291     }
01292 }
01293 
01294 void VL53L0X::Start_Measurement()
01295 {   VL53L0X_DeviceModes device_mode;
01296     uint8_t byte;
01297     uint8_t start_stop_byte = REG_SYSRANGE_MODE_START_STOP;
01298     uint32_t loop_nb;
01299 
01300     if (ErrState != VL53L0X_OK)  {return; } // no activity while in Error State!!!!
01301 
01302     /* Get Current DeviceMode */
01303     device_mode = CurrParams.DeviceMode ;
01304 
01305     Write_Byte(0x80,0x01);
01306     Write_Byte(0xFF,0x01);
01307     Write_Byte(0x00,0x00);
01308     Write_Byte(0x91,StopVariable);
01309     Write_Byte(0x00,0x01);
01310     Write_Byte(0xFF,0x00);
01311     Write_Byte(0x80,0x00);
01312 
01313     switch (device_mode) {
01314         case VL53L0X_DEVICEMODE_SINGLE_RANGING:
01315             Write_Byte(REG_SYSRANGE_START,0x01);
01316 
01317             byte = start_stop_byte;
01318             if (ErrState == VL53L0X_OK) {
01319                 /* Wait until start bit has been cleared */
01320                 loop_nb = 0;
01321                 do {
01322                     if (loop_nb > 0)
01323                         byte = Read_Byte(REG_SYSRANGE_START); 
01324                     loop_nb = loop_nb + 1;
01325                 } while (((byte & start_stop_byte) == start_stop_byte)
01326                          && (ErrState == VL53L0X_OK)
01327                          && (loop_nb < VL53L0X_DEFAULT_MAX_LOOP));
01328 
01329                 if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP) {
01330                     ErrState = VL53L0X_ERROR_TIME_OUT;
01331                 }
01332             }
01333             break;
01334             
01335         case VL53L0X_DEVICEMODE_CONTINUOUS_RANGING:
01336             /* Back-to-back mode, Check if need to apply interrupt settings */
01337             Check_and_load_interrupt_settings(1);
01338             Write_Byte(REG_SYSRANGE_START,REG_SYSRANGE_MODE_BACKTOBACK);
01339             Set_Current_State( VL53L0X_STATE_RUNNING );
01340             break;
01341         case VL53L0X_DEVICEMODE_CONTINUOUS_TIMED_RANGING:
01342             /* Continuous mode; Check if need to apply interrupt settings */
01343             Check_and_load_interrupt_settings(1);
01344             Write_Byte(REG_SYSRANGE_START, REG_SYSRANGE_MODE_TIMED);
01345             Set_Current_State( VL53L0X_STATE_RUNNING ); 
01346             break;
01347         default:
01348             /* Selected mode not supported */
01349             ErrState = VL53L0X_ERROR_MODE_NOT_SUPPORTED;
01350     }
01351 }
01352 
01353 /* Group Device Measurement Functions */
01354 void VL53L0X::Perf_single_measurement()
01355 {   VL53L0X_DeviceModes device_mode;
01356 
01357     if (ErrState != VL53L0X_OK)  {return; } // no activity while in Error State!!!!
01358 
01359     /* Get Current DeviceMode */
01360     device_mode = CurrParams.DeviceMode ;
01361 
01362     /* Start immediately to run a single ranging measurement in case of
01363      * single ranging or single histogram */
01364     if (device_mode == VL53L0X_DEVICEMODE_SINGLE_RANGING) {Start_Measurement();}
01365 
01366     Poll_Measure_Completion();
01367 
01368     /* Change Device State in case of single ranging or single histogram */
01369     if (device_mode == VL53L0X_DEVICEMODE_SINGLE_RANGING) 
01370        { Set_Current_State( VL53L0X_STATE_IDLE );  }
01371 }
01372 
01373 TFP1616 VL53L0X::Get_total_xtalk_rate(TRangeResults *p_ranging_results)
01374 {   TFP1616 total_xtalk_MHz;
01375 
01376     // CurrParams.XTalk_Compens_En was Get_xtalk_compensation_enable
01377     if ( (ErrState == VL53L0X_OK) & (CurrParams.XTalk_Compens_En  ) ) 
01378       { /* FixPoint1616 * FixPoint 8:8 = FixPoint0824 */
01379         total_xtalk_MHz = p_ranging_results->EffectiveSPADRtnCount *
01380             CurrParams.Xtalk_CompRate_MHz ;
01381 
01382         /* FixPoint0824 >> 8 = FixPoint1616 */
01383         return (total_xtalk_MHz + 0x80) >> 8;
01384       }
01385       else { return 0; }
01386 }
01387 
01388 void VL53L0X::Get_total_SIG_rate(TRangeResults *p_ranging_results,
01389         TFP1616 *p_total_SIG_rate_mcps)
01390 {   TFP1616 total_xtalk_MHz;
01391 
01392     *p_total_SIG_rate_mcps = p_ranging_results->SignalRateRtnMHz;
01393     total_xtalk_MHz = Get_total_xtalk_rate(p_ranging_results);
01394     
01395     if (ErrState == VL53L0X_OK) { *p_total_SIG_rate_mcps += total_xtalk_MHz;}
01396 }
01397 
01398 /* To convert ms into register value */
01399 uint32_t VL53L0X::Calc_timeout_mclks(uint32_t timeout_period_us,
01400         uint8_t vcsel_period_pclks)
01401 {   uint32_t macro_period_ns;
01402     
01403     macro_period_ns = (uint32_t)(vcsel_period_pclks) * VL53L0X_MACRO_PERIOD_NS;
01404 
01405     return (uint32_t)(((timeout_period_us * 1000)
01406                      + (macro_period_ns / 2)) / macro_period_ns);
01407 }
01408 
01409 uint32_t VL53L0X::ISQRT(uint32_t num)
01410 {   /* Implements an integer square root
01411      * From: http://en.wikipedia.org/wiki/Methods_of_computing_square_roots */
01412     uint32_t  res = 0;
01413     uint32_t  bit = 1 << 30;
01414     /* The second-to-top bit is set:    1 << 14 for 16-bits,1 << 30 for 32 bits */
01415 
01416     /* "bit" starts at the highest power of four <= the argument. */
01417     while (bit > num) { bit >>= 2; }
01418 
01419     while (bit != 0) {
01420         if (num >= res + bit) {
01421             num -= res + bit;
01422             res = (res >> 1) + bit;
01423         } else { res >>= 1;  }
01424         bit >>= 2;
01425     }
01426     return res;
01427 }
01428 
01429 void VL53L0X::Calc_dmax(TFP1616 total_SIG_rate_mcps,
01430     TFP1616 total_corr_SIG_rate_mcps,
01431     TFP1616 pw_mult,
01432     uint32_t sigma_estimate_p1,
01433     TFP1616 sigma_estimate_p2,
01434     uint32_t peak_vcsel_duration_us,
01435     uint32_t *pd_max_mm)
01436 {   const uint32_t c_sigma_limit        = 18;
01437     const TFP1616 c_SIG_limit   = 0x4000; /* 0.25 */
01438     const TFP1616 c_sigma_est_Ref   = 0x00000042; /* 0.001 */
01439     const uint32_t c_amb_eff_width_sigma_est_ns = 6;
01440     const uint32_t c_amb_eff_width_d_max_ns    = 7;
01441     uint32_t dmax_cal_range_mm;
01442     TFP1616 dmax_cal_SIG_rate_rtn_mcps;
01443     TFP1616 min_SIG_needed;
01444     TFP1616 min_SIG_needed_p1;
01445     TFP1616 min_SIG_needed_p2;
01446     TFP1616 min_SIG_needed_p3;
01447     TFP1616 min_SIG_needed_p4;
01448     TFP1616 sigma_limit_tmp;
01449     TFP1616 sigma_est_sq_tmp;
01450     TFP1616 signal_limit_tmp;
01451     TFP1616 signal_at0_mm;
01452     TFP1616 dmax_dark;
01453     TFP1616 dmax_ambient;
01454     TFP1616 dmax_dark_tmp;
01455     TFP1616 sigma_est_p2_tmp;
01456     uint32_t signal_rate_temp_mcps;
01457 
01458     dmax_cal_range_mm = DmaxCalRangeMilliMeter;
01459 
01460     dmax_cal_SIG_rate_rtn_mcps = DmaxCalSignalRateRtnMHz;
01461 
01462     /* uint32 * FixPoint1616 = FixPoint1616 */
01463     signal_at0_mm = dmax_cal_range_mm * dmax_cal_SIG_rate_rtn_mcps;
01464 
01465     /* FixPoint1616 >> 8 = FixPoint2408 */
01466     signal_at0_mm = (signal_at0_mm + 0x80) >> 8;
01467     signal_at0_mm *= dmax_cal_range_mm;
01468 
01469     min_SIG_needed_p1 = 0;
01470     if (total_corr_SIG_rate_mcps > 0) {
01471         /* Shift by 10 bits to increase resolution prior to the  division */
01472         signal_rate_temp_mcps = total_SIG_rate_mcps << 10;
01473 
01474         /* Add rounding value prior to division */
01475         min_SIG_needed_p1 = signal_rate_temp_mcps + (total_corr_SIG_rate_mcps / 2);
01476 
01477         /* FixPoint0626/FixPoint1616 = FixPoint2210 */
01478         min_SIG_needed_p1 /= total_corr_SIG_rate_mcps;
01479 
01480         /* Apply a factored version of the speed of light.
01481          Correction to be applied at the end */
01482         min_SIG_needed_p1 *= 3;
01483 
01484         /* FixPoint2210 * FixPoint2210 = FixPoint1220 */
01485         min_SIG_needed_p1 *= min_SIG_needed_p1;
01486 
01487         /* FixPoint1220 >> 16 = FixPoint2804 */
01488         min_SIG_needed_p1 = (min_SIG_needed_p1 + 0x8000) >> 16;
01489     }
01490 
01491     min_SIG_needed_p2 = pw_mult * sigma_estimate_p1;
01492 
01493     /* FixPoint1616 >> 16 =  uint32 */
01494     min_SIG_needed_p2 = (min_SIG_needed_p2 + 0x8000) >> 16;
01495 
01496     /* uint32 * uint32  =  uint32 */
01497     min_SIG_needed_p2 *= min_SIG_needed_p2;
01498 
01499     /* Check sigmaEstimateP2; If this value is too high, there is not enough
01500      * signal rate to calculate dmax value so set a suitable value to ensure
01501      * a very small dmax. */
01502     sigma_est_p2_tmp = (sigma_estimate_p2 + 0x8000) >> 16;
01503     sigma_est_p2_tmp = (sigma_est_p2_tmp + c_amb_eff_width_sigma_est_ns / 2) /
01504                        c_amb_eff_width_sigma_est_ns;
01505     sigma_est_p2_tmp *= c_amb_eff_width_d_max_ns;
01506 
01507     if (sigma_est_p2_tmp > 0xffff) {
01508         min_SIG_needed_p3 = 0xfff00000;
01509     } else {
01510         /* DMAX uses a different ambient width from sigma,so apply correction.
01511          * Perform division before multiplication to prevent overflow.  */
01512         sigma_estimate_p2 = (sigma_estimate_p2 + c_amb_eff_width_sigma_est_ns / 2) /
01513                             c_amb_eff_width_sigma_est_ns;
01514         sigma_estimate_p2 *= c_amb_eff_width_d_max_ns;
01515 
01516         /* FixPoint1616 >> 16 = uint32 */
01517         min_SIG_needed_p3 = (sigma_estimate_p2 + 0x8000) >> 16;
01518         min_SIG_needed_p3 *= min_SIG_needed_p3;
01519     }
01520 
01521     /* FixPoint1814 / uint32 = FixPoint1814 */
01522     sigma_limit_tmp = ((c_sigma_limit << 14) + 500) / 1000;
01523 
01524     /* FixPoint1814 * FixPoint1814 = FixPoint3628 := FixPoint0428 */
01525     sigma_limit_tmp *= sigma_limit_tmp;
01526 
01527     /* FixPoint1616 * FixPoint1616 = FixPoint3232 */
01528     sigma_est_sq_tmp = c_sigma_est_Ref * c_sigma_est_Ref;
01529 
01530     /* FixPoint3232 >> 4 = FixPoint0428 */
01531     sigma_est_sq_tmp = (sigma_est_sq_tmp + 0x08) >> 4;
01532 
01533     /* FixPoint0428 - FixPoint0428  = FixPoint0428 */
01534     sigma_limit_tmp -=  sigma_est_sq_tmp;
01535 
01536     /* uint32_t * FixPoint0428 = FixPoint0428 */
01537     min_SIG_needed_p4 = 4 * 12 * sigma_limit_tmp;
01538 
01539     /* FixPoint0428 >> 14 = FixPoint1814 */
01540     min_SIG_needed_p4 = (min_SIG_needed_p4 + 0x2000) >> 14;
01541 
01542     /* uint32 + uint32 = uint32 */
01543     min_SIG_needed = (min_SIG_needed_p2 + min_SIG_needed_p3);
01544 
01545     /* uint32 / uint32 = uint32 */
01546     min_SIG_needed += (peak_vcsel_duration_us / 2);
01547     min_SIG_needed /= peak_vcsel_duration_us;
01548 
01549     /* uint32 << 14 = FixPoint1814 */
01550     min_SIG_needed <<= 14;
01551 
01552     /* FixPoint1814 / FixPoint1814 = uint32 */
01553     min_SIG_needed += (min_SIG_needed_p4 / 2);
01554     min_SIG_needed /= min_SIG_needed_p4;
01555 
01556     /* FixPoint3200 * FixPoint2804 := FixPoint2804*/
01557     min_SIG_needed *= min_SIG_needed_p1;
01558 
01559     /* Apply correction by dividing by 1000000.
01560      * This assumes 10E16 on the numerator of the equation and 10E-22 on the denominator.
01561      * We do this because 32bit fix point calculation can't
01562      * handle the larger and smaller elements of this equation,
01563      * i.e. speed of light and pulse widths.
01564      */
01565     min_SIG_needed = (min_SIG_needed + 500) / 1000;
01566     min_SIG_needed <<= 4;
01567 
01568     min_SIG_needed = (min_SIG_needed + 500) / 1000;
01569 
01570     /* FixPoint1616 >> 8 = FixPoint2408 */
01571     signal_limit_tmp = (c_SIG_limit + 0x80) >> 8;
01572 
01573     /* FixPoint2408/FixPoint2408 = uint32 */
01574     if (signal_limit_tmp != 0) {
01575         dmax_dark_tmp = (signal_at0_mm + (signal_limit_tmp / 2))
01576                         / signal_limit_tmp;
01577     } else { dmax_dark_tmp = 0; }
01578 
01579     dmax_dark = ISQRT(dmax_dark_tmp);
01580 
01581     /* FixPoint2408/FixPoint2408 = uint32 */
01582     if (min_SIG_needed != 0) 
01583        { dmax_ambient = (signal_at0_mm + min_SIG_needed / 2) / min_SIG_needed; }
01584       else { dmax_ambient = 0; }
01585 
01586     dmax_ambient = ISQRT(dmax_ambient);
01587 
01588     *pd_max_mm = dmax_dark;
01589     if (dmax_dark > dmax_ambient) { *pd_max_mm = dmax_ambient; }
01590 }
01591 
01592 void VL53L0X::Calc_sigma_estimate(TRangeResults *p_ranging_results,
01593         TFP1616 *p_sigma_estimate, uint32_t *p_dmax_mm)
01594 {   /* Expressed in 100ths of a ns,i.e. centi-ns */
01595     const uint32_t c_pulse_effective_width_centi_ns   = 800;
01596     /* Expressed in 100ths of a ns,i.e. centi-ns */
01597     const uint32_t c_ambient_effective_width_centi_ns = 600;
01598     const TFP1616 c_dflt_final_range_integration_time_milli_secs    = 0x00190000; /* 25ms */
01599     const uint32_t c_vcsel_pulse_width_ps   = 4700; /* pico secs */
01600     const TFP1616 c_sigma_est_max   = 0x028F87AE;
01601     const TFP1616 c_sigma_est_rtn_max   = 0xF000;
01602     const TFP1616 c_amb_to_SIG_ratio_max = 0xF0000000 /
01603             c_ambient_effective_width_centi_ns;
01604     /* Time Of Flight per mm (6.6 pico secs) */
01605     const TFP1616 c_tof_per_mm_ps       = 0x0006999A;
01606     const uint32_t c_16bit_rounding_param       = 0x00008000;
01607     const TFP1616 c_max_xtalk_kcps      = 0x00320000;
01608     const uint32_t c_pll_period_ps          = 1655;
01609 
01610     uint32_t vcsel_total_events_rtn;
01611     uint32_t final_range_timeout_micro_secs;
01612     uint32_t pre_range_timeout_micro_secs;
01613     uint32_t final_range_integration_time_milli_secs;
01614     TFP1616 sigma_estimate_p1;
01615     TFP1616 sigma_estimate_p2;
01616     TFP1616 sigma_estimate_p3;
01617     TFP1616 delta_t_ps;
01618     TFP1616 pw_mult;
01619     TFP1616 sigma_est_rtn;
01620     TFP1616 sigma_estimate;
01621     TFP1616 xtalk_correction;
01622     TFP1616 ambient_rate_kcps;
01623     TFP1616 peak_SIG_rate_kcps;
01624     TFP1616 xtalk_comp_rate_mcps;
01625     uint32_t xtalk_comp_rate_kcps;
01626     
01627     TFP1616 diff1_mcps;
01628     TFP1616 diff2_mcps;
01629     TFP1616 sqr1;
01630     TFP1616 sqr2;
01631     TFP1616 sqr_sum;
01632     TFP1616 sqrt_result_centi_ns;
01633     TFP1616 sqrt_result;
01634     TFP1616 total_SIG_rate_mcps;
01635     TFP1616 corrected_SIG_rate_mcps;
01636     TFP1616 sigma_est_Ref;
01637     uint32_t vcsel_width;
01638     uint32_t final_range_macro_pclks;
01639     uint32_t pre_range_macro_pclks;
01640     uint32_t peak_vcsel_duration_us;
01641     uint8_t final_range_vcsel_pclks;
01642     uint8_t pre_range_vcsel_pclks;
01643     /*! \addtogroup calc_sigma_estimate
01644      * @{
01645      * Estimates the range sigma */
01646 
01647     xtalk_comp_rate_mcps = CurrParams.Xtalk_CompRate_MHz ; 
01648 
01649     /* We work in kcps rather than mcps as this helps keep within the
01650      * confines of the 32 Fix1616 type.  */
01651     ambient_rate_kcps = (p_ranging_results->AmbientRateRtnMHz * 1000) >> 16;
01652 
01653     corrected_SIG_rate_mcps =  p_ranging_results->SignalRateRtnMHz;
01654 
01655     Get_total_SIG_rate(p_ranging_results,&total_SIG_rate_mcps);
01656     xtalk_comp_rate_mcps = Get_total_xtalk_rate(p_ranging_results);
01657 
01658     /* Signal rate measurement provided by device is the
01659      * peak signal rate,not average.  */
01660     peak_SIG_rate_kcps = (total_SIG_rate_mcps * 1000);
01661     peak_SIG_rate_kcps = (peak_SIG_rate_kcps + 0x8000) >> 16;
01662 
01663     xtalk_comp_rate_kcps = xtalk_comp_rate_mcps * 1000;
01664 
01665     if (xtalk_comp_rate_kcps > c_max_xtalk_kcps) 
01666         {  xtalk_comp_rate_kcps = c_max_xtalk_kcps; }
01667 
01668     if (ErrState == VL53L0X_OK) {
01669         /* Calculate final range macro periods */
01670         final_range_timeout_micro_secs = FinalRangeTimeoutMicroSecs ; 
01671         final_range_vcsel_pclks = FinalRangeVcselPPeriod ; 
01672         final_range_macro_pclks = Calc_timeout_mclks(final_range_timeout_micro_secs,final_range_vcsel_pclks);
01673 
01674         /* Calculate pre-range macro periods */
01675         pre_range_timeout_micro_secs = PreRangeTimeoutMicroSecs ; 
01676         pre_range_vcsel_pclks = PreRangeVcselPPeriod ; 
01677         pre_range_macro_pclks = Calc_timeout_mclks(pre_range_timeout_micro_secs,pre_range_vcsel_pclks);
01678         vcsel_width = 3;
01679         if (final_range_vcsel_pclks == 8) {  vcsel_width = 2; }
01680 
01681         peak_vcsel_duration_us = vcsel_width * 2048 *
01682                                  (pre_range_macro_pclks + final_range_macro_pclks);
01683         peak_vcsel_duration_us = (peak_vcsel_duration_us + 500) / 1000;
01684         peak_vcsel_duration_us *= c_pll_period_ps;
01685         peak_vcsel_duration_us = (peak_vcsel_duration_us + 500) / 1000;
01686 
01687         /* Fix1616 >> 8 = Fix2408 */
01688         total_SIG_rate_mcps = (total_SIG_rate_mcps + 0x80) >> 8;
01689 
01690         /* Fix2408 * uint32 = Fix2408 */
01691         vcsel_total_events_rtn = total_SIG_rate_mcps *  peak_vcsel_duration_us;
01692 
01693         /* Fix2408 >> 8 = uint32 */
01694         vcsel_total_events_rtn = (vcsel_total_events_rtn + 0x80) >> 8;
01695 
01696         /* Fix2408 << 8 = Fix1616 = */
01697         total_SIG_rate_mcps <<= 8;
01698     }
01699 
01700     if (ErrState != VL53L0X_OK) { return ; }
01701 
01702     if (peak_SIG_rate_kcps == 0) {
01703         *p_sigma_estimate = c_sigma_est_max;
01704         SigmaEstimate = c_sigma_est_max; 
01705         *p_dmax_mm = 0;
01706     } else {
01707         if (vcsel_total_events_rtn < 1) {vcsel_total_events_rtn = 1; }
01708 
01709         sigma_estimate_p1 = c_pulse_effective_width_centi_ns;
01710 
01711         /* ((FixPoint1616 << 16)* uint32)/uint32 = FixPoint1616 */
01712         sigma_estimate_p2 = (ambient_rate_kcps << 16) / peak_SIG_rate_kcps;
01713         if (sigma_estimate_p2 > c_amb_to_SIG_ratio_max) 
01714             /* Clip to prevent overflow. Will ensure safe max result. */
01715             { sigma_estimate_p2 = c_amb_to_SIG_ratio_max; }
01716         sigma_estimate_p2 *= c_ambient_effective_width_centi_ns;
01717         sigma_estimate_p3 = 2 * ISQRT(vcsel_total_events_rtn * 12);
01718 
01719         /* uint32 * FixPoint1616 = FixPoint1616 */
01720         delta_t_ps = p_ranging_results->RangeMilliMeter * c_tof_per_mm_ps;
01721 
01722         /* vcselRate - xtalkCompRate
01723          * (uint32 << 16) - FixPoint1616 = FixPoint1616.
01724          * Divide result by 1000 to convert to mcps.
01725          * 500 is added to ensure rounding when integer division truncates. */
01726         diff1_mcps = (((peak_SIG_rate_kcps << 16) - 2 * xtalk_comp_rate_kcps) + 500) / 1000;
01727 
01728         /* vcselRate + xtalkCompRate */
01729         diff2_mcps = ((peak_SIG_rate_kcps << 16) + 500) / 1000;
01730 
01731         /* Shift by 8 bits to increase resolution prior to the division */
01732         diff1_mcps <<= 8;
01733 
01734         /* FixPoint0824/FixPoint1616 = FixPoint2408 */
01735 //      xTalkCorrection  = abs(diff1_mcps/diff2_mcps);
01736 // abs is causing compiler overloading isue in C++, but unsigned types. So,redundant call anyway!
01737         xtalk_correction     = diff1_mcps / diff2_mcps;
01738 
01739         /* FixPoint2408 << 8 = FixPoint1616 */
01740         xtalk_correction <<= 8;
01741 
01742         if (p_ranging_results->RangeStatus != 0) 
01743           { pw_mult = 1 << 16; }
01744          else {
01745             /* FixPoint1616/uint32 = FixPoint1616 */
01746             pw_mult = delta_t_ps / c_vcsel_pulse_width_ps; /* smaller than 1.0f */
01747 
01748             /* FixPoint1616 * FixPoint1616 = FixPoint3232,however both
01749              * values are small enough such that32 bits will not be
01750              * exceeded. */
01751             pw_mult *= ((1 << 16) - xtalk_correction);
01752 
01753             /* (FixPoint3232 >> 16) = FixPoint1616 */
01754             pw_mult = (pw_mult + c_16bit_rounding_param) >> 16;
01755 
01756             /* FixPoint1616 + FixPoint1616 = FixPoint1616 */
01757             pw_mult += (1 << 16);
01758 
01759             /* At this point the value will be 1.xx,therefore if we square
01760              * the value this will exceed 32 bits. To address this perform
01761              * a single shift to the right before the multiplication.  */
01762             pw_mult >>= 1;
01763             /* FixPoint1715 * FixPoint1715 = FixPoint3430 */
01764             pw_mult = pw_mult * pw_mult;
01765 
01766             /* (FixPoint3430 >> 14) = Fix1616 */
01767             pw_mult >>= 14;
01768         }
01769 
01770         /* FixPoint1616 * uint32 = FixPoint1616 */
01771         sqr1 = pw_mult * sigma_estimate_p1;
01772 
01773         /* (FixPoint1616 >> 16) = FixPoint3200 */
01774         sqr1 = (sqr1 + 0x8000) >> 16;
01775 
01776         /* FixPoint3200 * FixPoint3200 = FixPoint6400 */
01777         sqr1 *= sqr1;
01778         sqr2 = sigma_estimate_p2;
01779 
01780         /* (FixPoint1616 >> 16) = FixPoint3200 */
01781         sqr2 = (sqr2 + 0x8000) >> 16;
01782 
01783         /* FixPoint3200 * FixPoint3200 = FixPoint6400 */
01784         sqr2 *= sqr2;
01785 
01786         /* FixPoint64000 + FixPoint6400 = FixPoint6400 */
01787         sqr_sum = sqr1 + sqr2;
01788 
01789         /* SQRT(FixPoin6400) = FixPoint3200 */
01790         sqrt_result_centi_ns = ISQRT(sqr_sum);
01791 
01792         /* (FixPoint3200 << 16) = FixPoint1616 */
01793         sqrt_result_centi_ns <<= 16;
01794 
01795         /* Note that the Speed Of Light is expressed in um per 1E-10
01796          * seconds (2997) Therefore to get mm/ns we have to divide by 10000 */
01797         sigma_est_rtn = (((sqrt_result_centi_ns + 50) / 100) /
01798                          sigma_estimate_p3);
01799         sigma_est_rtn        *= VL53L0X_SPEED_OF_LIGHT_IN_AIR;
01800 
01801         /* Add 5000 before dividing by 10000 to ensure rounding. */
01802         sigma_est_rtn  = (sigma_est_rtn + 5000) / 10000;
01803 
01804         if (sigma_est_rtn > c_sigma_est_rtn_max) 
01805             /* Clip to prevent overflow. Will ensure safe max result. */
01806             { sigma_est_rtn = c_sigma_est_rtn_max; }
01807 
01808         final_range_integration_time_milli_secs =
01809             (final_range_timeout_micro_secs + pre_range_timeout_micro_secs + 500) / 1000;
01810 
01811         /* sigmaEstRef = 1mm * 25ms/final range integration time (inc pre-range)
01812          * sqrt(FixPoint1616/int) = FixPoint2408) */
01813         sigma_est_Ref =
01814             ISQRT((c_dflt_final_range_integration_time_milli_secs +
01815                            final_range_integration_time_milli_secs / 2) /
01816                           final_range_integration_time_milli_secs);
01817 
01818         /* FixPoint2408 << 8 = FixPoint1616 */
01819         sigma_est_Ref <<= 8;
01820         sigma_est_Ref = (sigma_est_Ref + 500) / 1000;
01821 
01822         /* FixPoint1616 * FixPoint1616 = FixPoint3232 */
01823         sqr1 = sigma_est_rtn * sigma_est_rtn;
01824         /* FixPoint1616 * FixPoint1616 = FixPoint3232 */
01825         sqr2 = sigma_est_Ref * sigma_est_Ref;
01826 
01827         /* sqrt(FixPoint3232) = FixPoint1616 */
01828         sqrt_result = ISQRT((sqr1 + sqr2));
01829         /* Note that the Shift by 4 bits increases resolution prior to
01830          * the sqrt,therefore the result must be shifted by 2 bits to
01831          * the right to revert back to the FixPoint1616 format.  */
01832         sigma_estimate   = 1000 * sqrt_result;
01833 
01834         if ((peak_SIG_rate_kcps < 1) || (vcsel_total_events_rtn < 1) ||
01835                 (sigma_estimate > c_sigma_est_max)) {
01836             sigma_estimate = c_sigma_est_max; }
01837 
01838         *p_sigma_estimate = (uint32_t)(sigma_estimate);
01839         SigmaEstimate = *p_sigma_estimate;
01840         Calc_dmax(total_SIG_rate_mcps,
01841                      corrected_SIG_rate_mcps,
01842                      pw_mult,
01843                      sigma_estimate_p1,
01844                      sigma_estimate_p2,
01845                      peak_vcsel_duration_us,
01846                      p_dmax_mm);
01847     }
01848 }
01849 
01850 void VL53L0X::Get_Device_range_status(uint8_t device_range_status,
01851         TFP1616 signal_rate,
01852         uint16_t effective_SPAD_rtn_count,
01853         TRangeResults *p_ranging_results,
01854         uint8_t *p_Device_range_status)
01855 {   uint8_t none_flag;
01856     uint8_t sigma_limitflag = 0;
01857     uint8_t signal_Ref_clipflag = 0;
01858     uint8_t range_ignore_thresholdflag = 0;
01859     uint8_t sigma_limit_chk_en = 0;
01860     uint8_t signal_rate_final_range_limit_chk_en = 0;
01861     uint8_t signal_Ref_clip_limit_chk_en = 0;
01862     uint8_t range_ignore_threshold_chk_en = 0;
01863     TFP1616 sigma_estimate;
01864     TFP1616 sigma_limit_value;
01865     TFP1616 signal_Ref_clip_value;
01866     TFP1616 range_ignore_threshold;
01867     TFP1616 signal_rate_per_SPAD;
01868     uint8_t device_range_status_internal = 0;
01869     uint8_t temp8;
01870     uint32_t dmax_mm = 0;
01871 
01872     /* VL53L0X has a good ranging when the value of the
01873      * DeviceRangeStatus = 11. This function will replace the value 0 with
01874      * the value 11 in the DeviceRangeStatus.
01875      * In addition,the SigmaEstimator is not included in the VL53L0X
01876      * DeviceRangeStatus,this will be added in the DeviceRangeStatus.  */
01877 
01878     device_range_status_internal = ((device_range_status & 0x78) >> 3);
01879 
01880     if ( device_range_status_internal == 0  ||
01881          device_range_status_internal == 5  ||
01882          device_range_status_internal == 7  ||
01883          device_range_status_internal == 12 ||
01884          device_range_status_internal == 13 ||
01885          device_range_status_internal == 14 ||
01886          device_range_status_internal == 15    )
01887         { none_flag = 1; } 
01888         else { none_flag = 0; }
01889 
01890     /* Check if Sigma limit is enabled,if yes then do comparison with limit
01891      * value and put the result back into pDeviceRangeStatus. */
01892     if (ErrState == VL53L0X_OK) 
01893        { sigma_limit_chk_en = Get_limit_chk_en(VL53L0X_CHECKEN_SIGMA_FINAL_RANGE); }
01894 
01895     if ((sigma_limit_chk_en != 0) && (ErrState == VL53L0X_OK)) {
01896         /* compute the Sigma and check with limit */
01897         Calc_sigma_estimate(p_ranging_results,  &sigma_estimate, &dmax_mm);
01898         if (ErrState == VL53L0X_OK) 
01899           { p_ranging_results->RangeDMaxMilliMeter = dmax_mm; }
01900 
01901         if (ErrState == VL53L0X_OK) 
01902           { sigma_limit_value = Get_limit_chk_val(VL53L0X_CHECKEN_SIGMA_FINAL_RANGE);
01903 
01904             if ((sigma_limit_value > 0) && (sigma_estimate > sigma_limit_value)) 
01905               { sigma_limitflag = 1; }/* Limit Fail */
01906           }
01907       }
01908 
01909     /* Check if Signal ref clip limit is enabled,if yes then do comparison
01910      * with limit value and put the result back into pDeviceRangeStatus. */
01911     if (ErrState == VL53L0X_OK) 
01912       {signal_Ref_clip_limit_chk_en = Get_limit_chk_en(VL53L0X_CHECKEN_SIG_REF_CLIP);}
01913 
01914     if ((signal_Ref_clip_limit_chk_en != 0) && (ErrState == VL53L0X_OK)) 
01915       { signal_Ref_clip_value = Get_limit_chk_val(VL53L0X_CHECKEN_SIG_REF_CLIP);
01916 
01917         /* Read LastSignalRefMcps from device */
01918         Write_Byte(0xFF,0x01);
01919         LastSignalRefMcps = FP97_TO_FP1616( Read_Word(REG_RESULT_PEAK_SIG_RATE_REF));
01920         Write_Byte(0xFF,0x00);
01921  
01922         if ((signal_Ref_clip_value > 0) && (LastSignalRefMcps > signal_Ref_clip_value)) \
01923           { signal_Ref_clipflag = 1; /* Limit Fail */ }
01924       }
01925 
01926     /* Check if Signal ref clip limit is enabled,if yes then do comparison
01927      * with limit value and put the result back into pDeviceRangeStatus.
01928      * EffectiveSPADRtnCount has a format 8.8
01929      * If (Return signal rate < (1.5 x Xtalk x number of SPADS)) : FAIL  */
01930     if (ErrState == VL53L0X_OK) 
01931        { range_ignore_threshold_chk_en = Get_limit_chk_en(VL53L0X_CHECKEN_RANGE_IGNORE_THRESHOLD); }
01932 
01933     if ((range_ignore_threshold_chk_en != 0) && (ErrState == VL53L0X_OK)) 
01934        {/* Compute the signal rate per SPAD */
01935         if (effective_SPAD_rtn_count == 0) {  signal_rate_per_SPAD = 0; }
01936         else { signal_rate_per_SPAD = 
01937                     (TFP1616)((256 * signal_rate) / effective_SPAD_rtn_count); }
01938 
01939         range_ignore_threshold=Get_limit_chk_val(VL53L0X_CHECKEN_RANGE_IGNORE_THRESHOLD);
01940 
01941         if ((range_ignore_threshold > 0) && (signal_rate_per_SPAD < range_ignore_threshold)) {
01942             /* Limit Fail add 2^6 to range ErrState */
01943             range_ignore_thresholdflag = 1;
01944         }
01945     }
01946 
01947     if (ErrState == VL53L0X_OK) {
01948         if (none_flag == 1) {
01949             *p_Device_range_status = 255;    /* NONE */
01950         } else if (device_range_status_internal == 1 ||
01951                    device_range_status_internal == 2 ||
01952                    device_range_status_internal == 3) {
01953             *p_Device_range_status = 5; /* HW fail */
01954         } else if (device_range_status_internal == 6 ||
01955                    device_range_status_internal == 9) {
01956             *p_Device_range_status = 4;  /* Phase fail */
01957         } else if (device_range_status_internal == 8 ||
01958                    device_range_status_internal == 10 ||
01959                    signal_Ref_clipflag == 1) {
01960             *p_Device_range_status = 3;  /* Min range */
01961         } else if (device_range_status_internal == 4 ||
01962                    range_ignore_thresholdflag == 1) {
01963             *p_Device_range_status = 2;  /* Signal Fail */
01964         } else if (sigma_limitflag == 1) {
01965             *p_Device_range_status = 1;  /* Sigma    Fail */
01966         } else {
01967             *p_Device_range_status = 0; /* Range Valid */
01968         }
01969     }
01970 
01971     /* DMAX only relevant during range error */
01972     if (*p_Device_range_status == 0) { p_ranging_results->RangeDMaxMilliMeter = 0; }
01973 
01974     /* fill the Limit Check ErrState */
01975     signal_rate_final_range_limit_chk_en = Get_limit_chk_en(VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE);
01976 
01977     if (ErrState == VL53L0X_OK) {
01978         if ((sigma_limit_chk_en == 0) || (sigma_limitflag == 1))
01979             { temp8 = 1; } else { temp8 = 0; }
01980          CurrParams.LimitChecksStatus [VL53L0X_CHECKEN_SIGMA_FINAL_RANGE] = temp8; 
01981 
01982         if ((device_range_status_internal == 4) || (signal_rate_final_range_limit_chk_en == 0)) 
01983             { temp8 = 1; } else { temp8 = 0; }
01984          CurrParams.LimitChecksStatus [VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE] = temp8; 
01985 
01986         if ((signal_Ref_clip_limit_chk_en == 0) || (signal_Ref_clipflag == 1)) 
01987             { temp8 = 1; } else { temp8 = 0; }
01988          CurrParams.LimitChecksStatus [VL53L0X_CHECKEN_SIG_REF_CLIP] = temp8; 
01989 
01990         if ((range_ignore_threshold_chk_en == 0) || (range_ignore_thresholdflag == 1)) 
01991             { temp8 = 1; } else { temp8 = 0;}
01992          CurrParams.LimitChecksStatus [VL53L0X_CHECKEN_RANGE_IGNORE_THRESHOLD] = temp8; 
01993     }
01994 }
01995 
01996 void VL53L0X::Get_ranging_results(TRangeResults *p_ranging_results)
01997 {   uint8_t device_range_status;
01998     uint8_t range_fractional_enable;
01999     uint8_t Device_range_status;
02000     uint8_t xtalk_compensation_enable;
02001     uint16_t ambient_rate;
02002     TFP1616 signal_rate;
02003     uint16_t Xtalk_CompRate_MHz;
02004     uint16_t effective_SPAD_rtn_count;
02005     uint16_t tmpuint16;
02006     uint16_t xtalk_range_milli_meter;
02007     uint16_t linearity_corrective_gain;
02008     uint8_t localBuffer[12];
02009     TRangeResults last_range_data_buffer;
02010 
02011     if (ErrState != VL53L0X_OK) { return; } // Do nothing while in error state
02012     
02013     /* use multi read even if some registers are not useful,result will
02014      * be more efficient start reading at REG_RESULT_RANGE_STATUS = 0x14 
02015      * end reading at 0x21 dec33 total 14 bytes to read */
02016     I2c_Read(REG_RESULT_RANGE_STATUS, localBuffer,12);
02017 
02018     if (ErrState == VL53L0X_OK) {
02019         p_ranging_results->ZoneId = 0; /* Only one zone */
02020         p_ranging_results->TimeStamp = 0; /* Not Implemented */
02021 
02022         tmpuint16 = VL53L0X_MAKEUINT16(localBuffer[11],localBuffer[10]);
02023         /* cut1.1 if SYSTEM__RANGE_CONFIG if 1 range is 2bits fractional
02024          *(format 11.2) else no fractional     */
02025 
02026         p_ranging_results->MeasurementTimeUsec = 0;
02027 
02028         signal_rate = FP97_TO_FP1616(VL53L0X_MAKEUINT16(localBuffer[7],localBuffer[6]));
02029         /* peak_SIG_count_rate_rtn_mcps */
02030         p_ranging_results->SignalRateRtnMHz = signal_rate;
02031 
02032         ambient_rate = VL53L0X_MAKEUINT16(localBuffer[9],localBuffer[8]);
02033         p_ranging_results->AmbientRateRtnMHz = FP97_TO_FP1616(ambient_rate);
02034 
02035         effective_SPAD_rtn_count = VL53L0X_MAKEUINT16(localBuffer[3], localBuffer[2]);
02036         /* EffectiveSPADRtnCount is 8.8 format */
02037         p_ranging_results->EffectiveSPADRtnCount = effective_SPAD_rtn_count;
02038 
02039         device_range_status = localBuffer[0];
02040 
02041         /* Get Linearity Corrective Gain */
02042         linearity_corrective_gain = LinearityCorrectiveGain;
02043 
02044         /* Get ranging configuration */
02045         range_fractional_enable = RangeFractionalEnable;
02046 
02047         if (linearity_corrective_gain != 1000) {
02048             tmpuint16 = (uint16_t)((linearity_corrective_gain
02049                                     * tmpuint16 + 500) / 1000);
02050 
02051             /* Implement Xtalk */
02052             Xtalk_CompRate_MHz = CurrParams.Xtalk_CompRate_MHz ; 
02053             xtalk_compensation_enable = CurrParams.XTalk_Compens_En ; 
02054 
02055             if (xtalk_compensation_enable) {
02056                 if ((signal_rate - ((Xtalk_CompRate_MHz
02057                             * effective_SPAD_rtn_count) >> 8))  <= 0) { 
02058                     if (range_fractional_enable) { xtalk_range_milli_meter = 8888;
02059                     } else { xtalk_range_milli_meter = 8888 << 2; }
02060                 } else {
02061                     xtalk_range_milli_meter = (tmpuint16 * signal_rate)
02062                         / (signal_rate - ((Xtalk_CompRate_MHz * effective_SPAD_rtn_count) >> 8));
02063                 }
02064                 tmpuint16 = xtalk_range_milli_meter;
02065             }
02066         }
02067 
02068         if (range_fractional_enable) {
02069             p_ranging_results->RangeMilliMeter = (uint16_t)((tmpuint16) >> 2);
02070             p_ranging_results->RangeFractionalPart =
02071                 (uint8_t)((tmpuint16 & 0x03) << 6);
02072         } else {
02073             p_ranging_results->RangeMilliMeter = tmpuint16;
02074             p_ranging_results->RangeFractionalPart = 0;
02075         }
02076 
02077         /* For a standard definition of RangeStatus,this should
02078          * return 0 in case of good result after a ranging
02079          * The range ErrState depends on the device so call a device
02080          * specific function to obtain the right ErrState. */
02081         Get_Device_range_status(device_range_status,signal_rate,effective_SPAD_rtn_count,
02082                                                p_ranging_results,&Device_range_status);
02083         if (ErrState == VL53L0X_OK) {
02084             p_ranging_results->RangeStatus = Device_range_status;
02085         }
02086     }
02087 
02088     if (ErrState == VL53L0X_OK) {
02089         /* Copy last read data into device+ buffer */
02090         last_range_data_buffer =  LastRangeMeasure;
02091         last_range_data_buffer.RangeMilliMeter =
02092             p_ranging_results->RangeMilliMeter;
02093         last_range_data_buffer.RangeFractionalPart =
02094             p_ranging_results->RangeFractionalPart;
02095         last_range_data_buffer.RangeDMaxMilliMeter =
02096             p_ranging_results->RangeDMaxMilliMeter;
02097         last_range_data_buffer.MeasurementTimeUsec =
02098             p_ranging_results->MeasurementTimeUsec;
02099         last_range_data_buffer.SignalRateRtnMHz =
02100             p_ranging_results->SignalRateRtnMHz;
02101         last_range_data_buffer.AmbientRateRtnMHz =
02102             p_ranging_results->AmbientRateRtnMHz;
02103         last_range_data_buffer.EffectiveSPADRtnCount =
02104             p_ranging_results->EffectiveSPADRtnCount;
02105         last_range_data_buffer.RangeStatus =
02106             p_ranging_results->RangeStatus;
02107         LastRangeMeasure = last_range_data_buffer; 
02108     }
02109 }
02110 
02111 void VL53L0X::Perf_single_ranging_measurement(
02112                           TRangeResults *p_ranging_results)
02113 {   if (ErrState != VL53L0X_OK)  {return; } // no activity while in Error State!!!!
02114 
02115     /* This function will do a complete single ranging  Here we fix the mode! */
02116     Set_device_mode(VL53L0X_DEVICEMODE_SINGLE_RANGING);
02117     
02118     Perf_single_measurement();
02119     
02120     Get_ranging_results(p_ranging_results);
02121     
02122     Clear_interrupt_mask(0);
02123 }
02124 
02125 uint16_t VL53L0X::Get_Perf_Ref_SIG_measurement()
02126 {   TRangeResults ranging_results;
02127     uint8_t orig_sequence_config;
02128     uint16_t Ref_SIG_rate ;
02129 
02130     /* store the value of the sequence config,
02131      * this will be reset before the end of the function*/
02132     orig_sequence_config =  SequenceConfig;
02133 
02134     /* This function performs a reference signal rate measurement.*/
02135     Set_SequenceConfig( 0xC0 ); // sets REG_SYSTEM_SEQUENCE_CONFIG
02136     
02137     Perf_single_ranging_measurement(&ranging_results);
02138 
02139     Write_Byte(0xFF,0x01);
02140     Ref_SIG_rate = Read_Word(REG_RESULT_PEAK_SIG_RATE_REF);
02141     Write_Byte(0xFF,0x00);
02142 
02143     /* restore the previous Sequence Config */
02144     Set_SequenceConfig( orig_sequence_config ); // resets REG_SYSTEM_SEQUENCE_CONFIG
02145     
02146     return Ref_SIG_rate;
02147 }
02148 
02149 void VL53L0X::Perf_Ref_SPAD_management(uint32_t *ref_SPAD_count,
02150         uint8_t *is_aperture_SPADS)
02151 {   uint8_t last_SPAD_array[6];
02152     uint8_t start_select = 0xB4;
02153     uint32_t minimum_SPAD_count = 3;
02154     uint32_t max_SPAD_count = 44;
02155     uint32_t current_SPAD_index = 0;
02156     uint32_t last_SPAD_index = 0;
02157     int32_t next_good_SPAD = 0;
02158     uint16_t target_Ref_rate = 0x0A00; /* 20 MHz in 9:7 format */
02159     uint16_t peak_SIG_rate_Ref;
02160     uint32_t need_apt_SPADS = 0;
02161     uint32_t index = 0;
02162     uint32_t REF_SPAD_ARRAY_SIZE = 6;
02163     uint32_t signal_rate_diff = 0;
02164     uint32_t last_SIG_rate_diff = 0;
02165     uint8_t complete = 0;
02166     uint8_t vhv_settings = 0;
02167     uint8_t phase_cal = 0;
02168     uint32_t ref_SPAD_count_int = 0;
02169     uint8_t  is_aperture_SPADS_int = 0;
02170 
02171     /*
02172      * The reference SPAD initialization procedure determines the minimum
02173      * amount of reference SPADS to be enables to achieve a target reference
02174      * signal rate and should be performed once during initialization.
02175      *
02176      * Either aperture or non-aperture SPADS are applied but never both.
02177      * Firstly non-aperture SPADS are set,begining with 5 SPADS,and
02178      * increased one SPAD at a time until the closest measurement to the
02179      * target rate is achieved.
02180      *
02181      * If the target rate is exceeded when 5 non-aperture SPADS are enabled,
02182      * initialization is performed instead with aperture SPADS.
02183      *
02184      * When setting SPADS,a 'Good SPAD Map' is applied.
02185      *
02186      * This procedure operates within a SPAD window of interest of a maximum
02187      * 44 SPADS.
02188      * The start point is currently fixed to 180,which lies towards the end
02189      * of the non-aperture quadrant and runs in to the adjacent aperture
02190      * quadrant. */
02191     target_Ref_rate =  targetRefRate;
02192 
02193     /* Initialize SPAD arrays.
02194      * Currently the good SPAD map is initialised to 'All good'.
02195      * This is a short term implementation. The good SPAD map will be
02196      * provided as an input.
02197      * Note that there are 6 bytes. Only the first 44 bits will be used to
02198      * represent SPADS. */
02199     for (index = 0; index < REF_SPAD_ARRAY_SIZE; index++) {
02200         SPADData.RefSPADEnables [index] = 0; }
02201 
02202     Write_Byte(0xFF,0x01);
02203     Write_Byte(REG_DYNAMIC_SPAD_REF_EN_START_OFFSET,0x00);
02204     Write_Byte(REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD,0x2C);
02205     Write_Byte(0xFF,0x00);
02206     Write_Byte(REG_GLOBAL_CONFIG_REF_EN_START_SELECT,start_select);
02207     Write_Byte(REG_POWER_MANAGEMENT_GO1_POWER_FORCE,0);
02208 
02209     /* Perform ref calibration */
02210     if (ErrState == VL53L0X_OK)
02211        {Perf_Ref_calibration(&vhv_settings, &phase_cal, 0);}
02212 
02213     if (ErrState == VL53L0X_OK) {
02214         /* Enable Minimum NON-APERTURE SPADS */
02215         current_SPAD_index = 0;
02216         last_SPAD_index = current_SPAD_index;
02217         need_apt_SPADS = 0;
02218         Enable_Ref_SPADS(need_apt_SPADS,
02219                                   SPADData.RefGoodSPADMap ,
02220                                   SPADData.RefSPADEnables ,
02221                                   REF_SPAD_ARRAY_SIZE,
02222                                   start_select,
02223                                   current_SPAD_index,
02224                                   minimum_SPAD_count,
02225                                   &last_SPAD_index);
02226     }
02227 
02228     if (ErrState == VL53L0X_OK) {
02229         current_SPAD_index = last_SPAD_index;
02230 
02231         peak_SIG_rate_Ref = Get_Perf_Ref_SIG_measurement();
02232         if ((ErrState == VL53L0X_OK) && (peak_SIG_rate_Ref > target_Ref_rate)) 
02233          {  /* Signal rate measurement too high, switch to APERTURE SPADS */
02234             for (index = 0; index < REF_SPAD_ARRAY_SIZE; index++) 
02235               {  SPADData.RefSPADEnables [index] = 0; }
02236 
02237             /* Increment to the first APERTURE SPAD */
02238             while ((Is_ApertureSPAD(start_select + current_SPAD_index)
02239                     == 0) && (current_SPAD_index < max_SPAD_count)) 
02240                { current_SPAD_index++; }
02241 
02242             need_apt_SPADS = 1;
02243 
02244             Enable_Ref_SPADS(need_apt_SPADS,
02245                                       SPADData.RefGoodSPADMap ,
02246                                       SPADData.RefSPADEnables ,
02247                                       REF_SPAD_ARRAY_SIZE,
02248                                       start_select,
02249                                       current_SPAD_index,
02250                                       minimum_SPAD_count,
02251                                       &last_SPAD_index);
02252 
02253             if (ErrState == VL53L0X_OK) {
02254                 current_SPAD_index = last_SPAD_index;
02255                 peak_SIG_rate_Ref = Get_Perf_Ref_SIG_measurement();
02256 
02257                 if ((ErrState == VL53L0X_OK) && (peak_SIG_rate_Ref > target_Ref_rate)) 
02258                   { /* Signal rate still too high after  setting the minimum number of
02259                      * APERTURE SPADS. Can do no more therefore set the min number of
02260                      * aperture SPADS as the result.        */
02261                     is_aperture_SPADS_int = 1;
02262                     ref_SPAD_count_int = minimum_SPAD_count;
02263                 }
02264             }
02265         } else { need_apt_SPADS = 0;}
02266     }
02267 
02268     if ((ErrState == VL53L0X_OK) && (peak_SIG_rate_Ref < target_Ref_rate)) 
02269      {  /* At this point,the minimum number of either aperture
02270          * or non-aperture SPADS have been set. Proceed to add
02271          * SPADS and perform measurements until the target reference is reached.*/
02272         is_aperture_SPADS_int = need_apt_SPADS;
02273         ref_SPAD_count_int  = minimum_SPAD_count;
02274 
02275         memcpy(last_SPAD_array,SPADData.RefSPADEnables , REF_SPAD_ARRAY_SIZE);
02276         last_SIG_rate_diff = abs(peak_SIG_rate_Ref - target_Ref_rate);
02277         complete = 0;
02278 
02279         while (!complete) {
02280             Get_Next_Good_SPAD(SPADData.RefGoodSPADMap ,
02281                 REF_SPAD_ARRAY_SIZE,current_SPAD_index, &next_good_SPAD);
02282 
02283             if (next_good_SPAD == -1) {
02284                 ErrState = VL53L0X_ERROR_REF_SPAD_INIT;
02285                 break;
02286             }
02287 
02288             /* Cannot combine Aperture and Non-Aperture SPADS,so
02289              * ensure the current SPAD is of the correct type. */
02290             if (Is_ApertureSPAD((uint32_t)start_select + next_good_SPAD) !=
02291                     need_apt_SPADS) {
02292                 /* At this point we have enabled the maximum number of Aperture SPADS.  */
02293                 complete = 1;
02294                 break;
02295             }
02296 
02297             (ref_SPAD_count_int)++;
02298 
02299             current_SPAD_index = next_good_SPAD;
02300             Enable_SPAD_bit(SPADData.RefSPADEnables ,
02301                          REF_SPAD_ARRAY_SIZE,current_SPAD_index);
02302 
02303             if (ErrState == VL53L0X_OK) {
02304                 current_SPAD_index++;
02305                 /* Proceed to apply the additional SPAD and perform measurement. */
02306                 I2c_Write(REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0, SPADData.RefSPADEnables ,6); //Set_Ref_SPAD_map
02307             }
02308 
02309             if (ErrState != VL53L0X_OK) { break; }
02310 
02311             peak_SIG_rate_Ref = Get_Perf_Ref_SIG_measurement();
02312 
02313             if (ErrState != VL53L0X_OK) { break; }
02314 
02315             signal_rate_diff = abs(peak_SIG_rate_Ref - target_Ref_rate);
02316 
02317             if (peak_SIG_rate_Ref > target_Ref_rate) {
02318                 /* Select the SPAD map that provides the
02319                  * measurement closest to the target rate,
02320                  * either above or below it. */
02321                 if (signal_rate_diff > last_SIG_rate_diff) {
02322                     /* Previous SPAD map produced a closer measurement,so choose this. */
02323                     I2c_Write(REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0, last_SPAD_array,6); // Set_Ref_SPAD_map();
02324                     memcpy(SPADData.RefSPADEnables ,last_SPAD_array,REF_SPAD_ARRAY_SIZE);
02325                     (ref_SPAD_count_int)--;
02326                 }
02327                 complete = 1;
02328             } else {
02329                 /* Continue to add SPADS */
02330                 last_SIG_rate_diff = signal_rate_diff;
02331                 memcpy(last_SPAD_array, SPADData.RefSPADEnables ,REF_SPAD_ARRAY_SIZE);
02332             }
02333         } /* while */
02334     }
02335 
02336     if (ErrState == VL53L0X_OK) {
02337         *ref_SPAD_count = ref_SPAD_count_int;
02338         *is_aperture_SPADS = is_aperture_SPADS_int;
02339         RefSPADSInitialised = 1;
02340         ReferenceSPADCount = (uint8_t)(*ref_SPAD_count);
02341         ReferenceSPADType = *is_aperture_SPADS;
02342     }
02343 }
02344 
02345 void VL53L0X::Set_Reference_SPADS(uint32_t count,uint8_t is_aperture_SPADS)
02346 {   uint32_t current_SPAD_index = 0;
02347     uint8_t  start_select    = 0xB4;
02348     uint32_t max_SPAD_count  = 44;
02349     uint32_t last_SPAD_index;
02350     uint32_t index;
02351 
02352     /* This function applies a requested number of reference SPADS,either
02353      * aperture or non-aperture,as requested. The good SPAD map will be applied.*/
02354     Write_Byte(0xFF,0x01);
02355     Write_Byte(REG_DYNAMIC_SPAD_REF_EN_START_OFFSET,0x00);
02356     Write_Byte(REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD,0x2C);
02357     Write_Byte(0xFF,0x00); 
02358     Write_Byte(REG_GLOBAL_CONFIG_REF_EN_START_SELECT, start_select);
02359 
02360     for (index = 0; index < REF_SPAD_ARRAY_SIZE; index++) {
02361         SPADData.RefSPADEnables [index] = 0; }
02362 
02363     if (is_aperture_SPADS) {
02364         /* Increment to the first APERTURE SPAD */
02365         while ((Is_ApertureSPAD(start_select + current_SPAD_index) == 0) &&
02366                 (current_SPAD_index < max_SPAD_count)) {
02367             current_SPAD_index++;
02368         }
02369     }
02370     Enable_Ref_SPADS(is_aperture_SPADS,  SPADData.RefGoodSPADMap ,
02371                               SPADData.RefSPADEnables ,
02372                               REF_SPAD_ARRAY_SIZE,
02373                               start_select,
02374                               current_SPAD_index,
02375                               count,
02376                               &last_SPAD_index);
02377 
02378     if (ErrState == VL53L0X_OK) {
02379         RefSPADSInitialised = 1;
02380         ReferenceSPADCount = (uint8_t)(count);
02381         ReferenceSPADType = is_aperture_SPADS;
02382     }
02383 }
02384 
02385 void VL53L0X::Set_GPIO_config(VL53L0X_DeviceModes device_mode,
02386                 TGPIO_Func  functionality,  VL53L0X_InterruptPolarity polarity)
02387 {   uint8_t pol_data;
02388 
02389     if (polarity == VL53L0X_INTERRUPTPOLARITY_LOW) 
02390         { pol_data = 0x00;} else { pol_data = 0x10;}
02391 
02392     switch ( device_mode ) {
02393       case VL53L0X_DEVICEMODE_GPIO_DRIVE: 
02394         Write_Byte(REG_GPIO_HV_MUX_ACTIVE_HIGH,pol_data);
02395         break;
02396       case VL53L0X_DEVICEMODE_GPIO_OSC: 
02397         Write_Byte(0xff,0x01);
02398         Write_Byte(0x00,0x00);
02399         Write_Byte(0xff,0x00);
02400         Write_Byte(0x80,0x01);
02401         Write_Byte(0x85,0x02);
02402         Write_Byte(0xff,0x04);
02403         Write_Byte(0xcd,0x00);
02404         Write_Byte(0xcc,0x11);
02405         Write_Byte(0xff,0x07);
02406         Write_Byte(0xbe,0x00);
02407         Write_Byte(0xff,0x06);
02408         Write_Byte(0xcc,0x09);
02409         Write_Byte(0xff,0x00);
02410         Write_Byte(0xff,0x01);
02411         Write_Byte(0x00,0x00);
02412         break;
02413       default:  
02414         if (functionality>GPIO_FUNC_NEW_MEASURE_READY )
02415             { ErrState = VL53L0X_ERROR_GPIO_FUNC_NOT_SUPPORTED; }
02416             else { Write_Byte(REG_SYSINT_CONFIG_GPIO,functionality); }
02417 
02418             if (ErrState == VL53L0X_OK) 
02419                { Register_BitMask(REG_GPIO_HV_MUX_ACTIVE_HIGH,0xEF,pol_data); }
02420 
02421             if (ErrState == VL53L0X_OK) 
02422                {GpioFunctionality = functionality; }
02423 
02424             Clear_interrupt_mask(0);
02425      } // switch 
02426 } // Set_GPIO_config
02427 
02428 /* Encode timeout in macro periods in (LSByte * 2^MSByte) + 1 format */
02429 uint16_t VL53L0X::Encode_timeout(uint32_t timeout_macro_clks)
02430 {   uint16_t encoded_timeout = 0;
02431     uint16_t ms_byte = 0;
02432 
02433     if (timeout_macro_clks > 0) {
02434         timeout_macro_clks = timeout_macro_clks - 1;
02435         while ((timeout_macro_clks & 0xFFFFFF00) > 0) {
02436             timeout_macro_clks = timeout_macro_clks >> 1;
02437             ms_byte++;
02438           } // while
02439         encoded_timeout = (ms_byte << 8) + (uint16_t)(timeout_macro_clks & 0x000000FF);
02440       }
02441     return encoded_timeout;
02442 }
02443 
02444 void VL53L0X::Set_Sequence_Step_Timeout(VL53L0X_SequenceStepId sequence_step_id,
02445         uint32_t timeout_micro_secs)
02446 {   uint8_t current_vcsel_PPeriod_p_clk;
02447     uint8_t msrc_encoded_time_out;
02448     uint16_t pre_range_encoded_time_out;
02449     uint16_t pre_range_time_out_m_clks;
02450     uint16_t msrc_range_time_out_m_clks;
02451     uint32_t final_range_time_out_m_clks;
02452     uint16_t final_range_encoded_time_out;
02453     VL53L0X_Sequence_Steps_t sequence_steps;
02454 
02455     switch (sequence_step_id) {
02456       case VL53L0X_SEQUENCESTEP_TCC:
02457       case VL53L0X_SEQUENCESTEP_DSS:
02458       case VL53L0X_SEQUENCESTEP_MSRC:
02459         current_vcsel_PPeriod_p_clk = /*  Gets and converts the VCSEL period register into actual clock periods */
02460                 ( Read_Byte(REG_PRE_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1;
02461 
02462         if (ErrState == VL53L0X_OK) {
02463                msrc_range_time_out_m_clks = Calc_timeout_mclks(timeout_micro_secs,
02464                                          (uint8_t)current_vcsel_PPeriod_p_clk);
02465 
02466             if (msrc_range_time_out_m_clks > 256) { msrc_encoded_time_out = 255;} 
02467                else {msrc_encoded_time_out = (uint8_t)msrc_range_time_out_m_clks - 1; }
02468 
02469             LastEncodedTimeout = msrc_encoded_time_out;
02470           }
02471         Write_Byte(REG_MSRC_CONFIG_TIMEOUT_MACROP,msrc_encoded_time_out);
02472         break; 
02473         
02474       case VL53L0X_SEQUENCESTEP_PRE_RANGE:
02475             current_vcsel_PPeriod_p_clk = /*  Gets and converts the VCSEL period register into actual clock periods */
02476                   ( Read_Byte(REG_PRE_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1;
02477             
02478             pre_range_time_out_m_clks = Calc_timeout_mclks(timeout_micro_secs,
02479                                            (uint8_t)current_vcsel_PPeriod_p_clk);
02480             pre_range_encoded_time_out = Encode_timeout(pre_range_time_out_m_clks);
02481 
02482             LastEncodedTimeout = pre_range_encoded_time_out;
02483         
02484             Write_Word(REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI,pre_range_encoded_time_out);
02485 
02486             if (ErrState == VL53L0X_OK) 
02487                {PreRangeTimeoutMicroSecs =timeout_micro_secs; }
02488             break;
02489             
02490       case VL53L0X_SEQUENCESTEP_FINAL_RANGE:
02491             /* For the final range timeout,the pre-range timeout must be added. 
02492              * To do this both final and pre-range timeouts must be expressed in 
02493              * macro periods MClks because they have different vcsel periods.*/
02494             sequence_steps = Get_sequence_step_enables();
02495             pre_range_time_out_m_clks = 0;
02496             
02497             if (sequence_steps.PreRangeOn) 
02498               { current_vcsel_PPeriod_p_clk = /*  Gets and converts the VCSEL period register into actual clock periods */
02499                     ( Read_Byte(REG_PRE_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1;
02500 
02501                 /* Retrieve PRE-RANGE Timeout in Macro periods (MCLKS) */
02502                 if (ErrState == VL53L0X_OK) 
02503                  {  pre_range_encoded_time_out = Read_Word(0x51); 
02504                     pre_range_time_out_m_clks = Decode_timeout(pre_range_encoded_time_out);
02505                   } 
02506               }
02507 
02508             /* Calculate FINAL RANGE Timeout in Macro Periode (MCLKS) and add PRE-RANGE value */
02509             if (ErrState == VL53L0X_OK) 
02510               { current_vcsel_PPeriod_p_clk  /*  Get and converts the VCSEL period register into actual clock periods */
02511                     = ( Read_Byte(REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1; }
02512                     
02513             if (ErrState == VL53L0X_OK) 
02514               { final_range_time_out_m_clks = Calc_timeout_mclks(timeout_micro_secs,
02515                                     (uint8_t) current_vcsel_PPeriod_p_clk);
02516 
02517                 final_range_time_out_m_clks += pre_range_time_out_m_clks;
02518 
02519                 final_range_encoded_time_out = Encode_timeout(final_range_time_out_m_clks);
02520 
02521                 Write_Word(0x71,final_range_encoded_time_out); 
02522               }
02523 
02524             if (ErrState == VL53L0X_OK) 
02525               { FinalRangeTimeoutMicroSecs  = timeout_micro_secs; }
02526             break;
02527           
02528         default: ErrState = VL53L0X_ERROR_INVALID_PARAMS;
02529       } // switch (sequence_step_id)
02530  }
02531 
02532 void VL53L0X::Set_Measure_Time_Budget_us (uint32_t Measure_Time_Budget_us)
02533 {   uint32_t final_range_timing_budget_us;
02534     VL53L0X_Sequence_Steps_t sequence_steps;
02535     uint32_t msrc_dcc_tcc_timeout_us= 2000;
02536     uint32_t start_overhead_us      = 1910;
02537     uint32_t end_overhead_us        = 960;
02538     uint32_t msrc_overhead_us       = 660;
02539     uint32_t tcc_overhead_us        = 590;
02540     uint32_t dss_overhead_us        = 690;
02541     uint32_t pre_range_overhead_us  = 660;
02542     uint32_t final_range_overhead_us= 550;
02543     uint32_t pre_range_timeout_us   = 0;
02544     uint32_t c_min_timing_budget_us = 20000;
02545     uint32_t sub_timeout = 0;
02546 
02547     if (Measure_Time_Budget_us < c_min_timing_budget_us) 
02548         { ErrState = VL53L0X_ERROR_INVALID_PARAMS;
02549           return ; }
02550 
02551     final_range_timing_budget_us = Measure_Time_Budget_us - 
02552                                       (start_overhead_us + end_overhead_us);
02553 
02554     sequence_steps = Get_sequence_step_enables();
02555 
02556     if (ErrState == VL53L0X_OK &&
02557             (sequence_steps.TccOn  ||
02558              sequence_steps.MsrcOn ||
02559              sequence_steps.DssOn)) {
02560 
02561         /* TCC,MSRC and DSS all share the same timeout */
02562         Get_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_MSRC,
02563                                            &msrc_dcc_tcc_timeout_us);
02564 
02565         /* Subtract the TCC,MSRC and DSS timeouts if they are  enabled. */
02566         if (ErrState != VL53L0X_OK) {return ; }
02567 
02568         /* TCC */
02569         if (sequence_steps.TccOn) {
02570 
02571             sub_timeout = msrc_dcc_tcc_timeout_us + tcc_overhead_us;
02572 
02573             if (sub_timeout < final_range_timing_budget_us) {
02574                 final_range_timing_budget_us -= sub_timeout;
02575             } else { /* Requested timeout too big. */
02576                 ErrState = VL53L0X_ERROR_INVALID_PARAMS;
02577             }
02578         }
02579 
02580         if (ErrState != VL53L0X_OK) {return;  }
02581 
02582         /* DSS */
02583         if (sequence_steps.DssOn) 
02584           { sub_timeout = 2 * (msrc_dcc_tcc_timeout_us + dss_overhead_us);
02585 
02586             if (sub_timeout < final_range_timing_budget_us) 
02587                 { final_range_timing_budget_us  -= sub_timeout;  } 
02588               else { /* Requested timeout too big. */
02589                 ErrState = VL53L0X_ERROR_INVALID_PARAMS; }
02590           } 
02591          else if (sequence_steps.MsrcOn)  /* MSRC */
02592           { sub_timeout = msrc_dcc_tcc_timeout_us + msrc_overhead_us;
02593 
02594             if (sub_timeout < final_range_timing_budget_us) 
02595                { final_range_timing_budget_us -= sub_timeout; } 
02596             else  /* Requested timeout too big. */
02597                { ErrState = VL53L0X_ERROR_INVALID_PARAMS; } 
02598           }
02599     }
02600 
02601     if (ErrState != VL53L0X_OK) {return; }
02602 
02603     if (sequence_steps.PreRangeOn) {
02604         /* Subtract the Pre-range timeout if enabled. */
02605         Get_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_PRE_RANGE,
02606                                            &pre_range_timeout_us);
02607         sub_timeout = pre_range_timeout_us + pre_range_overhead_us;
02608 
02609         if (sub_timeout < final_range_timing_budget_us) {
02610             final_range_timing_budget_us -= sub_timeout;
02611         } else {
02612             /* Requested timeout too big. */
02613             ErrState = VL53L0X_ERROR_INVALID_PARAMS;
02614         }
02615     }
02616 
02617     if (ErrState == VL53L0X_OK && sequence_steps.FinalRangeOn) 
02618       { final_range_timing_budget_us -= final_range_overhead_us;
02619 
02620         /* Final Range Timeout
02621          * Note that the final range timeout is determined by the timing
02622          * budget and the sum of all other timeouts within the sequence.
02623          * If there is no room for the final range timeout,then an error
02624          * will be set. Otherwise the remaining time will be applied to
02625          * the final range.
02626          */
02627         Set_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_FINAL_RANGE,
02628                                            final_range_timing_budget_us);
02629 
02630         CurrParams.Measure_Time_Budget_us = Measure_Time_Budget_us; 
02631     }
02632 }
02633 
02634 const uint8_t SEQUENCESTEP_MASK[] =
02635 {  0x10, //VL53L0X_SEQUENCESTEP_TCC = 0
02636    0x28, //VL53L0X_SEQUENCESTEP_DSS = 1
02637    0x04, //VL53L0X_SEQUENCESTEP_MSRC= 2 
02638    0x40, //VL53L0X_SEQUENCESTEP_PRE_RANGE= 3
02639    0x80}; //VL53L0X_SEQUENCESTEP_FINAL_RANGE = 4 
02640   
02641 void VL53L0X::Set_sequence_step_enable(VL53L0X_SequenceStepId sequence_step_id,
02642                                        uint8_t sequence_step_enabled)
02643 {   uint8_t new_config = 0;
02644 
02645     // SequenceConfig = Read_Byte(REG_SYSTEM_SEQUENCE_CONFIG); 
02646     // instead of reading from the device, use the SequenceConfig local data field!!
02647     
02648     if (sequence_step_enabled == 1)  /* Enable requested sequence step  */
02649          { new_config = SequenceConfig | SEQUENCESTEP_MASK[sequence_step_id]; }
02650        else  /* Disable requested sequence step  */ 
02651          { new_config = SequenceConfig & (0xff - SEQUENCESTEP_MASK[sequence_step_id]); }
02652 
02653     if (new_config != SequenceConfig) {  /* Apply New Setting */
02654         Set_SequenceConfig( new_config ); 
02655         if (ErrState == VL53L0X_OK)   /* Recalculate timing budget */
02656            {  Set_Measure_Time_Budget_us(CurrParams.Measure_Time_Budget_us); }
02657     } // if (new_config != sequence_config)
02658 }
02659 
02660 void VL53L0X::Set_limit_chk_en(uint16_t limit_check_id,  uint8_t limit_chk_en)
02661 {   TFP1616 temp_fix1616 = 0;
02662     if (limit_chk_en!=0) {limit_chk_en=1;} // make sure we only have 0 or 1 as values!!!
02663 
02664     switch (limit_check_id) {
02665         case VL53L0X_CHECKEN_SIGMA_FINAL_RANGE:      /* internal computation: */
02666         case VL53L0X_CHECKEN_SIG_REF_CLIP:           /* internal computation: */
02667         case VL53L0X_CHECKEN_RANGE_IGNORE_THRESHOLD: /* internal computation: */
02668             CurrParams.Limit_Chk_En [limit_check_id] = limit_chk_en; 
02669             break;
02670 
02671         case VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE:
02672             temp_fix1616 = limit_chk_en * CurrParams.Limit_Chk_Val [limit_check_id];
02673             Write_Word(REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT,
02674                            FP1616_TO_FP97(temp_fix1616));
02675             break;
02676 
02677         case VL53L0X_CHECKEN_SIG_RATE_MSRC:
02678             Register_BitMask(REG_MSRC_CONFIG_CONTROL,0xFE, (1-limit_chk_en)<< 1);
02679             break;
02680 
02681         case VL53L0X_CHECKEN_SIG_RATE_PRE_RANGE:
02682             Register_BitMask(REG_MSRC_CONFIG_CONTROL,0xEF, (1-limit_chk_en)<< 4);
02683             break;
02684 
02685         default: ErrState = VL53L0X_ERROR_INVALID_PARAMS; 
02686      } // switch
02687 
02688     if (ErrState == VL53L0X_OK) { CurrParams.Limit_Chk_En [limit_check_id] = limit_chk_en; } 
02689 }
02690 
02691 void VL53L0X::Static_init()
02692 {   VL53L0X_DeviceParams_t new_curr_parameters;
02693     uint8_t  *p_tuning_setting_buffer;
02694     uint16_t tempword = 0;
02695     uint8_t  tempbyte = 0;
02696     uint32_t count = 0;
02697     uint8_t  is_aperture_SPADS = 0;
02698     uint32_t ref_SPAD_count = 0;
02699     uint8_t  aperture_SPADS = 0;
02700     uint8_t  vcsel_PPeriod_pclk;
02701     uint32_t seq_timeout_micro_secs;
02702 
02703     /* set the ref SPAD from NVM */
02704     count   = (uint32_t)ReferenceSPADCount; 
02705     aperture_SPADS = ReferenceSPADType; 
02706 
02707     /* NVM value invalid */
02708     if ((aperture_SPADS > 1) || ((aperture_SPADS == 1) && (count > 32)) ||
02709        ((aperture_SPADS == 0) && (count > 12))) 
02710        { Perf_Ref_SPAD_management(&ref_SPAD_count, &is_aperture_SPADS); }
02711      else 
02712        { Set_Reference_SPADS(count,aperture_SPADS);  }
02713 
02714     /* Initialize tuning settings buffer to prevent compiler warning. */
02715     p_tuning_setting_buffer = DefaultTuningSettings;
02716 
02717     if (ErrState == VL53L0X_OK) {
02718         if (UseInternalTuningSettings == 0) 
02719            { p_tuning_setting_buffer = pTuningSettingsPointer; }
02720          else 
02721            { p_tuning_setting_buffer = DefaultTuningSettings; }
02722       }
02723 
02724     if (ErrState == VL53L0X_OK) 
02725        {  Load_tuning_settings(p_tuning_setting_buffer); }
02726        
02727     /*  Set interrupt config to new sample ready */
02728     if (ErrState == VL53L0X_OK) 
02729        { Set_GPIO_config(0,GPIO_FUNC_NEW_MEASURE_READY ,VL53L0X_INTERRUPTPOLARITY_LOW); }
02730 
02731     Write_Byte(0xFF,0x01);
02732     tempword = Read_Word(0x84); 
02733     Write_Byte(0xFF,0x00);
02734 
02735     if (ErrState == VL53L0X_OK) 
02736       { OscFrequencyMHz=FP412_TO_FP1616(tempword); }
02737 
02738     /* After static init,some device parameters may be changed, so update them */
02739     new_curr_parameters = Get_device_parameters(); 
02740 
02741     if (ErrState == VL53L0X_OK) { tempbyte = Read_Byte(REG_SYSTEM_RANGE_CONFIG); }
02742     if (ErrState == VL53L0X_OK) { RangeFractionalEnable = (tempbyte & 1); }
02743     if (ErrState == VL53L0X_OK) { CurrParams = new_curr_parameters; }
02744 
02745     /* read the sequence config and save it */
02746     Set_SequenceConfig( Read_Byte(REG_SYSTEM_SEQUENCE_CONFIG) ); // checks for ErrState
02747 
02748     /* Disable MSRC and TCC by default */
02749     if (ErrState == VL53L0X_OK) 
02750        { Set_sequence_step_enable(VL53L0X_SEQUENCESTEP_TCC,0); }
02751 
02752     if (ErrState == VL53L0X_OK) 
02753        { Set_sequence_step_enable(VL53L0X_SEQUENCESTEP_MSRC,0); }
02754 
02755     /* Set State to standby */
02756     Set_Current_State( VL53L0X_STATE_IDLE) ;
02757 
02758     /* Store pre-range vcsel period */
02759     if (ErrState == VL53L0X_OK)/*  Gets and converts the VCSEL period register into actual clock periods */ 
02760        { vcsel_PPeriod_pclk = (Read_Byte(REG_PRE_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1; }
02761 
02762     if ( ErrState == VL53L0X_OK) 
02763        { PreRangeVcselPPeriod  = vcsel_PPeriod_pclk; }
02764 
02765     /* Store final-range vcsel period */
02766     if (ErrState == VL53L0X_OK)
02767        { vcsel_PPeriod_pclk  /*  Get and convert the VCSEL period register into actual clock periods */
02768             = ( Read_Byte(REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1; }
02769 
02770     if (ErrState == VL53L0X_OK) 
02771        { FinalRangeVcselPPeriod  = vcsel_PPeriod_pclk; }
02772 
02773     /* Store pre-range timeout */
02774     if (ErrState == VL53L0X_OK) 
02775        { Get_Sequence_Step_Timeout( VL53L0X_SEQUENCESTEP_PRE_RANGE, 
02776                                                      &seq_timeout_micro_secs); }
02777 
02778     if (ErrState == VL53L0X_OK) 
02779        { PreRangeTimeoutMicroSecs  = seq_timeout_micro_secs; }
02780 
02781     /* Store final-range timeout */
02782     if (ErrState == VL53L0X_OK) 
02783        { Get_Sequence_Step_Timeout( VL53L0X_SEQUENCESTEP_FINAL_RANGE, 
02784                                                       &seq_timeout_micro_secs);}
02785 
02786     if (ErrState == VL53L0X_OK) 
02787        { FinalRangeTimeoutMicroSecs  = seq_timeout_micro_secs; }
02788 }
02789 
02790 void VL53L0X::Stop_Measurement()
02791 {   Write_Byte(REG_SYSRANGE_START, REG_SYSRANGE_MODE_SINGLESHOT);
02792     Write_Byte(0xFF,0x01);
02793     Write_Byte(0x00,0x00);
02794     Write_Byte(0x91,0x00);
02795     Write_Byte(0x00,0x01);
02796     Write_Byte(0xFF,0x00);
02797 
02798     Set_Current_State( VL53L0X_STATE_IDLE );
02799 
02800     /* Check if need to apply interrupt settings */
02801     Check_and_load_interrupt_settings(0); 
02802 }
02803 
02804 uint8_t VL53L0X::Get_Stop_Completed()
02805 {   uint8_t Abyte = 0;
02806 
02807     Write_Byte(0xFF,0x01);
02808     Abyte = Read_Byte(0x04); 
02809     Write_Byte(0xFF,0x0); 
02810     
02811     if ((ErrState == VL53L0X_OK) & (Abyte == 0))
02812      {  Write_Byte(0x80,0x01);
02813         Write_Byte(0xFF,0x01);
02814         Write_Byte(0x00,0x00);
02815         Write_Byte(0x91,StopVariable);
02816         Write_Byte(0x00,0x01);
02817         Write_Byte(0xFF,0x00);
02818         Write_Byte(0x80,0x00);
02819       }
02820     return Abyte;
02821 }
02822 
02823 void VL53L0X::Wait_Measurement_Ready()
02824 {   uint32_t loop_nb = 0;
02825 
02826     // Wait until it finished, or loopo count reached = avoids deadlock
02827     while ( !Get_Measurement_Ready() & (ErrState == VL53L0X_OK)  )
02828       { if (loop_nb++ >= VL53L0X_DEFAULT_MAX_LOOP) 
02829              { ErrState = VL53L0X_ERROR_TIME_OUT;} 
02830            else { Polling_delay(); }  
02831       } // while ends
02832 }
02833 
02834 void VL53L0X::Wait_Stop_Completed()
02835 {   uint32_t loop_nb = 0;
02836 
02837     // Wait until Stop_Completed, or loopo count reached = avoids deadlock
02838     while ( (ErrState == VL53L0X_OK) & !Get_Stop_Completed() )
02839       { if (loop_nb++ >= VL53L0X_DEFAULT_MAX_LOOP) 
02840              { ErrState = VL53L0X_ERROR_TIME_OUT;} 
02841            else { Polling_delay(); }  
02842       } // while ends
02843 }
02844 
02845 void VL53L0X::Range_meas_int_continuous_mode(void (*fptr)(void))
02846 {   Stop_Measurement(); // it is safer to do this while sensor is stopped
02847 
02848     Set_GPIO_config(VL53L0X_DEVICEMODE_CONTINUOUS_RANGING,
02849             GPIO_FUNC_NEW_MEASURE_READY , VL53L0X_INTERRUPTPOLARITY_HIGH);
02850     if (ErrState==VL53L0X_OK) {
02851         Attach_interrupt_measure_detection_irq(fptr);
02852         Enable_interrupt_measure_detection_irq();   }
02853 
02854     Clear_interrupt_mask(REG_RESULT_INTERRUPT_STATUS | REG_RESULT_RANGE_STATUS);
02855     // NB: return value was previously only passed to logging macro,but did not get passed back 
02856 
02857     if (ErrState==VL53L0X_OK) { Range_start_continuous_mode(); }
02858 }
02859 
02860 
02861 VL53L0X_Error VL53L0X::Start_Measurement(TOperatingMode operating_mode, void (*fptr)(void))
02862 {   uint8_t VhvSettings;
02863     uint8_t PhaseCal;
02864     // *** from mass market cube expansion v1.1,ranging with satellites.
02865     // default settings,for normal range.
02866     TFP1616 signalLimit = (TFP1616)(0.25 * 65536);
02867     TFP1616 sigmaLimit = (TFP1616)(18 * 65536);
02868     uint32_t timingBudget = 33000;
02869     uint8_t preRangeVcselPeriod = 14;
02870     uint8_t finalRangeVcselPeriod = 10;
02871 
02872     switch (operating_mode) {
02873      case op_INT:
02874         if (_gpio1Int == NULL) { ErrState=1; return ErrState; }
02875         Stop_Measurement(); // it is safer to do this while sensor is stopped
02876 
02877         Set_GPIO_config(VL53L0X_DEVICEMODE_CONTINUOUS_RANGING,
02878              GPIO_FUNC_NEW_MEASURE_READY , VL53L0X_INTERRUPTPOLARITY_HIGH);
02879 
02880         if (ErrState == VL53L0X_OK) 
02881          {  Attach_interrupt_measure_detection_irq(fptr);
02882             Enable_interrupt_measure_detection_irq(); }
02883 
02884         Clear_interrupt_mask(REG_RESULT_INTERRUPT_STATUS | REG_RESULT_RANGE_STATUS);
02885         // NB: return value was previously only passed to logging macro, but did not get passed back 
02886 
02887         // Setup in continuous ranging mode
02888         Set_device_mode(VL53L0X_DEVICEMODE_CONTINUOUS_RANGING);
02889         Start_Measurement();
02890         break;
02891 
02892     case op_single_shot_poll:
02893         // singelshot,polled ranging; no need to do this when we use VL53L0X_PerformSingleRangingMeasurement
02894         Set_device_mode(VL53L0X_DEVICEMODE_SINGLE_RANGING); // Setup in single ranging mode
02895 
02896         // Enable/Disable Sigma and Signal check
02897         if (ErrState == VL53L0X_OK) 
02898           { Set_limit_chk_en(VL53L0X_CHECKEN_SIGMA_FINAL_RANGE,1); }
02899         
02900         if (ErrState == VL53L0X_OK) 
02901           { Set_limit_chk_en(VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE,1); }
02902         
02903 
02904         /* Ranging configuration */
02905         // *** from mass market cube expansion v1.1,ranging with satellites.
02906         //        switch(rangingConfig) {
02907         //        case LONG_RANGE:
02908         signalLimit = (TFP1616)(0.1 * 65536);
02909         sigmaLimit = (TFP1616)(60 * 65536);
02910         timingBudget = 33000;
02911         preRangeVcselPeriod = 18;
02912         finalRangeVcselPeriod = 14;
02913         /*          break;
02914                 case HIGH_ACCURACY:
02915                     signalLimit = (TFP1616)(0.25*65536);
02916                     sigmaLimit = (TFP1616)(18*65536);
02917                     timingBudget = 200000;
02918                     preRangeVcselPeriod = 14;
02919                     finalRangeVcselPeriod = 10;
02920                     break;
02921                 case HIGH_SPEED:
02922                     signalLimit = (TFP1616)(0.25*65536);
02923                     sigmaLimit = (TFP1616)(32*65536);
02924                     timingBudget = 20000;
02925                     preRangeVcselPeriod = 14;
02926                     finalRangeVcselPeriod = 10;
02927                     break;
02928                 default:
02929                     debug_printf("Not Supported");
02930                 }
02931         */
02932 
02933         if (ErrState == VL53L0X_OK) 
02934            { Set_limit_chk_val(VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE,signalLimit);}
02935 
02936         if (ErrState == VL53L0X_OK) 
02937            { Set_limit_chk_val(VL53L0X_CHECKEN_SIGMA_FINAL_RANGE,sigmaLimit);}
02938 
02939         if (ErrState == VL53L0X_OK) 
02940            { Set_Measure_Time_Budget_us(timingBudget);}
02941 
02942         if (ErrState == VL53L0X_OK) 
02943            { Set_vcsel_PPeriod(VL53L0X_VCSEL_PRE_RANGE,preRangeVcselPeriod); }
02944 
02945         if (ErrState == VL53L0X_OK) 
02946            { Set_vcsel_PPeriod(VL53L0X_VCSEL_FINAL_RANGE,finalRangeVcselPeriod);}
02947 
02948         if (ErrState == VL53L0X_OK) 
02949            { Perf_Ref_calibration(&VhvSettings,&PhaseCal,1); }
02950         break;
02951     case  op_poll:  // Setup in continuous ranging mode
02952         Set_device_mode(VL53L0X_DEVICEMODE_CONTINUOUS_RANGING);
02953         Start_Measurement(); 
02954     } // switch
02955     return ErrState;
02956 }
02957 
02958 VL53L0X_Error VL53L0X::Stop_Measurement(TOperatingMode operating_mode)
02959 {   if ((ErrState == VL53L0X_OK) &
02960         (operating_mode == op_INT || operating_mode == op_poll) )
02961       { // only stop if in one of the continuous modes !!!!
02962         Stop_Measurement();
02963         Wait_Stop_Completed();
02964         Clear_interrupt_mask( REG_SYSINT_GPIO_NEW_SAMPLE_READY);
02965       }
02966     return ErrState;
02967 }
02968 
02969 TRangeResults VL53L0X::Handle_irq(TOperatingMode operating_mode)
02970 {   TRangeResults RangeResults;
02971     RangeResults = Get_Measurement(operating_mode);
02972     Enable_interrupt_measure_detection_irq();
02973     return RangeResults;
02974 }
02975 
02976 /****************** Private device functions *************************/
02977 
02978 void VL53L0X::Wait_read_strobe()
02979 {   uint32_t loop_nb = 0;
02980 
02981     Write_Byte(0x83,0x00); // set strobe register to 0
02982 
02983     /* polling while no error, no strobe, and not reached max number of loop to avoid deadlock*/
02984     while ((ErrState == VL53L0X_OK) && (Read_Byte(0x83) == 0x00) ) 
02985       {  if (loop_nb++ >= VL53L0X_DEFAULT_MAX_LOOP) 
02986           { ErrState = VL53L0X_ERROR_TIME_OUT; } } 
02987 
02988     Write_Byte(0x83,0x01); // set strobe register back to 1 'manually'
02989 }
02990 
02991 /******************************************************************************/
02992 /****************** Small Service and wrapper functions  **********************/
02993 /******************************************************************************/
02994 uint32_t refArrayQuadrants[4] = {REF_ARRAY_SPAD_10,REF_ARRAY_SPAD_5,
02995                                  REF_ARRAY_SPAD_0,REF_ARRAY_SPAD_5 }; 
02996 
02997 
02998 uint32_t VL53L0X::Decode_timeout(uint16_t encoded_timeout)
02999 {  /*Decode 16-bit timeout register value - format (LSByte * 2^MSByte) + 1 */
03000     return ((uint32_t) (encoded_timeout & 0x00FF)            
03001          << (uint32_t)((encoded_timeout & 0xFF00) >> 8)) + 1;
03002 }
03003 
03004 uint8_t VL53L0X::Is_ApertureSPAD(uint32_t SPAD_index)
03005 {   /* This function reports if a given SPAD index is an aperture SPAD by
03006      * deriving the quadrant = SPAD_index >> 6. */
03007     if (refArrayQuadrants[SPAD_index >> 6] == REF_ARRAY_SPAD_0) 
03008        { return 0; } else { return 1; }
03009 }
03010 
03011 /******************************************************************************/
03012 /****************** Write and read functions from I2C *************************/
03013 /******************************************************************************/
03014 
03015 
03016 uint32_t VL53L0X::Get_NVM_DWord(uint8_t NVM_Address)
03017 {   Write_Byte(0x94,NVM_Address);
03018     Wait_read_strobe();
03019     return Read_DWord(0x90); 
03020 }
03021 uint16_t VL53L0X::Get_NVM_Word(uint8_t NVM_Address)
03022 {   Write_Byte(0x94,NVM_Address);
03023     Wait_read_strobe();
03024     return Read_Word(0x90); 
03025 }
03026 uint8_t VL53L0X::Get_NVM_Byte(uint8_t NVM_Address)
03027 {   Write_Byte(0x94,NVM_Address);
03028     Wait_read_strobe();
03029     return Read_Byte(0x90); 
03030 }
03031 void VL53L0X::Write_Byte(uint8_t index, uint8_t data)
03032 {   I2c_Write(index,&data,1);   }
03033 
03034 void VL53L0X::Write_Word(uint8_t index,uint16_t data)
03035 {   uint8_t buffer[2];
03036     buffer[0] = data >> 8;
03037     buffer[1] = data & 0x00FF;
03038     I2c_Write(index,(uint8_t *)buffer,2);
03039 }
03040 
03041 void VL53L0X::Write_DWord(uint8_t index, uint32_t data)
03042 {  uint8_t buffer[4];
03043     buffer[0] = (data >> 24) & 0xFF;
03044     buffer[1] = (data >> 16) & 0xFF;
03045     buffer[2] = (data >>  8) & 0xFF;
03046     buffer[3] = (data >>  0) & 0xFF;
03047     I2c_Write(index,(uint8_t *)buffer,4);
03048 }
03049 
03050 uint8_t VL53L0X::Read_Byte(uint8_t index)
03051 {  uint8_t result;
03052    I2c_Read(index,&result,1);
03053    return result;
03054 }
03055 
03056 uint16_t VL53L0X::Read_Word(uint8_t index)
03057 {   uint8_t buffer[2] = {0,0};
03058     I2c_Read(index, &buffer[0], 2);
03059     return (buffer[0] << 8) + buffer[1];
03060 }
03061 
03062 uint32_t VL53L0X::Read_DWord(uint8_t index)
03063 {   uint8_t buffer[4] = {0,0,0,0};
03064     I2c_Read(index,buffer,4);
03065     return (buffer[0] << 24) + (buffer[1] << 16) + (buffer[2] <<  8) +  buffer[3];
03066 }
03067 
03068 void VL53L0X::Register_BitMask(uint8_t index,uint8_t and_mask,uint8_t or_mask)
03069 {   uint8_t buffer = 0;
03070     /* read data direct onto buffer */
03071     I2c_Read(index,&buffer,1);
03072     if (ErrState==VL53L0X_OK) {
03073         buffer = (buffer & and_mask) | or_mask;
03074         I2c_Write(index,&buffer,(uint8_t)1);
03075     }
03076 }
03077 
03078 /**
03079  * @brief  Writes a buffer towards the I2C peripheral device.
03080  * @param  RegisterAddr specifies the internal address register
03081  * @param  p_data pointer to the byte-array data to send
03082  *         where to start writing to (must be correctly masked).
03083  * @param  NumByteToWrite number of bytes to be written.
03084  * @note   On some devices if NumByteToWrite is greater
03085  *         than one, the RegisterAddr must be masked correctly!  */
03086 void VL53L0X::I2c_Write( uint8_t  RegisterAddr,  uint8_t  *p_data,
03087                             uint16_t NumByteToWrite )
03088 {   int ret;
03089     uint8_t tmp[TEMP_BUF_SIZE];
03090 
03091     if (ErrState != VL53L0X_OK)  {return; } // no comms while in Error State!!!!
03092     
03093     if (NumByteToWrite >= TEMP_BUF_SIZE) 
03094         {ErrState = VL53L0X_ERROR_I2C_BUF_OVERFLOW; return; };
03095 
03096     /* First, send device address. Then, send data and terminate with STOP condition */
03097     tmp[0] = RegisterAddr;
03098     memcpy(tmp+1, p_data, NumByteToWrite);
03099     ret = _dev_i2c->write(I2cDevAddr, (const char*)tmp, NumByteToWrite+1, false);
03100 
03101     if (ret) { ErrState = VL53L0X_ERROR_CONTROL_INTERFACE;  }
03102 }
03103 
03104     /**@brief  Reads a buffer from the I2C peripheral device.
03105      * @param  pBuffer pointer to the byte-array to read data in to
03106      * @param  I2cDevAddr specifies the peripheral device slave address.
03107      * @param  RegisterAddr specifies the internal address register
03108      *         where to start reading from (must be correctly masked).
03109      * @param  NumByteToRead number of bytes to be read, maximum VL53L0X_MAX_I2C_XFER_SIZE
03110      * @note   On some devices if NumByteToWrite is greater
03111      *         than one, the RegisterAddr must be masked correctly!
03112      */
03113 void VL53L0X::I2c_Read(uint8_t RegisterAddr,  uint8_t *p_data,
03114                           uint16_t NumByteToRead)
03115 {   int ret;
03116     if (ErrState != VL53L0X_OK)  {return; } // no comms while in Error State, return value undefined!!!!
03117     
03118      /*  Send device address, without STOP condition  */
03119     ret = _dev_i2c->write(I2cDevAddr, (const char*)&RegisterAddr, 1, true);
03120     if(!ret) /*   Read data, with STOP condition   */
03121        { ret = _dev_i2c->read(I2cDevAddr, (char*)p_data, NumByteToRead, false); }
03122 
03123     if (ret) { ErrState = VL53L0X_ERROR_CONTROL_INTERFACE;  }
03124 }
03125 
03126 /******************************************************************************/