Tarek Lule
/
VL53L0X_Condensed2
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Embed:
(wiki syntax)
Show/hide line numbers
VL53L0X.cpp
00001 /** 00002 ****************************************************************************** 00003 * @file VL53L0X_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>© 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 /* 00039 Simplifications versus the original library: 00040 00041 Replace: 00042 * "MicroSeconds" or "micro_seconds" by "us" or "_us" 00043 * "MilliSeconds" or "milli_seconds" by "ms" or "_ms" 00044 * "MegaCps" or "MCps" or "_mega_cps" by "MHz" or "_MHz" 00045 * "MicroMeter" by "um" or "_um" 00046 * "FIXEDPNT" by "FP" 00047 00048 Everything related to histogram_mode seems completely not implemented, so all definitions removed. 00049 00050 Everything related to x_talk_compensation seems also not implemented, all removed 00051 00052 Some example regular expressinos used to simplify the code: 00053 b) Search for: \QRead_Byte(\E([A-Za-z_\d]+)[[:punct:]](\s*)\Q&\E([A-Za-z\d_]+)\Q);\E 00054 Replace by: \3 = Read_Byte\(\1\); 00055 to replace: Read_Byte(0x90,&module_id); 00056 by this: module_id = Read_Byte(0x90); 00057 00058 c) Search for: ([A-Za-z_\d]+)\Q(\E\r\n(\s*) 00059 Replace by: \1\( 00060 To join lines where the first line has an open bracket, and the next line starts listing the parameters. 00061 for example: Status = VL53L0X_UpdateByte(V 00062 L53L0X_REG_VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV, .... 00063 becomes: Status = VL53L0X_UpdateByte(VL53L0X_REG_VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV, .... 00064 00065 */ 00066 00067 /* Includes */ 00068 #include <stdlib.h> 00069 #include "VL53L0X.h" 00070 00071 void Report_Range_Infos(VL53L0X_RangingMeasurementData_t RangeResults, Serial *aSerial ) 00072 { 00073 aSerial->printf("\n\r Reporting All Fields of VL53L0X_RangingMeasurementData_t structure \n\r" ); 00074 aSerial->printf(" .Range_mm = %dmm; Ranged distance. \n\r", RangeResults.Range_mm ); 00075 aSerial->printf(" .RangeDMax_mm = %dmm; maximum detection distance in current setup and environment conditions \n\r", RangeResults.RangeDMax_mm ); 00076 aSerial->printf(" .SignalRateRtn_MHz = %3.3fMHz; effectively a measure of target reflectance \n\r", RangeResults.SignalRateRtn_MHz / 65535.01); 00077 aSerial->printf(" .AmbientRateRtn_MHz = %3.3fMHz; effectively a measure of the ambient light \n\r", RangeResults.AmbientRateRtn_MHz / 65535.01 ); 00078 aSerial->printf(" .EffectiveSpadRtnCount = %3.3f; effective SPAD count for the return signal \n\r", RangeResults.EffectiveSpadRtnCount / 256.001 ); 00079 aSerial->printf(" .RangeFractionalPart = %d; Fractional part of range distance. \n\r", RangeResults.RangeFractionalPart >> 6 ); 00080 aSerial->printf(" .RangeStatus = %d[u8]; Status for the current measurement, 0 = value is valid \n\r", RangeResults.RangeStatus ); 00081 aSerial->printf(" .SigmaEstimate = %3.2f; Estimated Sigma - based on ambient & VCSEL rates and signal_total_events \n\r", RangeResults.SigmaEstimate/ 65535.01 ); 00082 }; 00083 00084 void Report_Deep_Infos(VL53L0X TOF1, Serial *aSerial) 00085 { 00086 aSerial->printf("\n\r Reporting All Top Level Infos of the class \n\r" ); 00087 aSerial->printf("I2cDevAddr = %d. \n\r", TOF1.I2cDevAddr ); 00088 aSerial->printf("comms_type = %d. Type of comms: 1=VL53L0X_COMMS_I2C or VL53L0X_COMMS_SPI \n\r", TOF1.comms_type ); 00089 aSerial->printf("comms_speed = %d. Communication speed [kHz] : typically 400kHz for I2C \n\r", TOF1.comms_speed_khz ); 00090 00091 aSerial->printf("\n\r Reporting All Infos of the Device_Info structure: \n\r" ); 00092 aSerial->printf("Device_Info.ProductType = 0x%2X. VL53L0X = 1, VL53L1 = 2 \n\r", TOF1.Device_Info.ProductType ); 00093 aSerial->printf("Device_Info.ProductRevision = %d.%d. Revision NR, major.minor \n\r", 00094 TOF1.Device_Info.ProductRevisionMajor , TOF1.Device_Info.ProductRevisionMinor ); 00095 aSerial->printf("Device_Info.Name = %s. Name of Device e.g. Left_Distance\n\r", TOF1.Device_Info.Name ); 00096 aSerial->printf("Device_Info.Type = %s. Type of Device e.g VL53L0X \n\r", TOF1.Device_Info.Type ); 00097 aSerial->printf("Device_Info.ProductId = %s. Product Identifier String \n\r", TOF1.Device_Info.ProductId ); 00098 00099 aSerial->printf("\n\r Reporting All Fields of CurrentParameters \n\r" ); 00100 aSerial->printf(" .DeviceMode = %d. Defines type of measurement to be done for the next measurement \n\r", 00101 TOF1.CurrentParameters .DeviceMode ); 00102 aSerial->printf(" .Measure_Time_Budget_us= %dus. Allowed total time for a single measurement \n\r", 00103 TOF1.CurrentParameters .MeasurementTimingBudget_us ); 00104 aSerial->printf(" .Measure_Period_ms = %dms. Time between two consecutive measurements \n\r", 00105 TOF1.CurrentParameters .InterMeasurementPeriod_ms ); 00106 aSerial->printf(" .XTalk_Compens_En = %d. Crosstalk compensation enable or not (0, default) \n\r", 00107 TOF1.CurrentParameters .XTalkCompensationEnable ); 00108 aSerial->printf(" .XTalk_CompRange_mm = %dmm. CrossTalk compensation range, seems never used \n\r", 00109 TOF1.CurrentParameters .XTalkCompensationRange_mm ); 00110 aSerial->printf(" .XTalk_CompRate_MHz = %3.2fMHz. CrossTalk compensation rate . \n\r", 00111 (float) TOF1.CurrentParameters .XTalkCompensationRate_MHz / 65536); 00112 aSerial->printf(" .RangeOffset_um = %d. Range offset adjustment (um) last programmed.\n\r", 00113 TOF1.CurrentParameters .RangeOffset_um ); 00114 aSerial->printf(" .LimitChecks ... = SIGMA_FINAL, SIGNAL_RATE_FINAL, SIGNAL_REF_CLIP, IGNORE_THRESHOLD, SIGNAL_RATE_MSRC, SIGNAL_RATE_PRE.\n\r"); 00115 aSerial->printf(" .LimitChecksEnable[x] = %d %d %d %d %d %d. The Limit Checks enabled or not.\n\r", 00116 TOF1.CurrentParameters .LimitChecksEnable [0],TOF1.CurrentParameters .LimitChecksEnable [1] ,TOF1.CurrentParameters .LimitChecksEnable [2], 00117 TOF1.CurrentParameters .LimitChecksEnable [3],TOF1.CurrentParameters .LimitChecksEnable [4] ,TOF1.CurrentParameters .LimitChecksEnable [5] ); 00118 aSerial->printf(" .LimitChecksStatus[x] = %d %d %d %d %d %d. Status of checks of last measurement.\n\r", 00119 TOF1.CurrentParameters .LimitChecksStatus [0],TOF1.CurrentParameters .LimitChecksStatus [1] ,TOF1.CurrentParameters .LimitChecksStatus [2], 00120 TOF1.CurrentParameters .LimitChecksStatus [3],TOF1.CurrentParameters .LimitChecksStatus [4] ,TOF1.CurrentParameters .LimitChecksStatus [5] ); 00121 aSerial->printf(" .LimitChecksValue[x] = %d %d %d %d %d %d [FP1616]. The Limit Check values \n\r", 00122 TOF1.CurrentParameters .LimitChecksValue [0],TOF1.CurrentParameters .LimitChecksValue [1] ,TOF1.CurrentParameters .LimitChecksValue [2], 00123 TOF1.CurrentParameters .LimitChecksValue [3],TOF1.CurrentParameters .LimitChecksValue [4] ,TOF1.CurrentParameters .LimitChecksValue [5] ); 00124 aSerial->printf(" .WrapAroundCheckEnable = %d. Wrap Around Check enabled or not \n\r", 00125 TOF1.CurrentParameters .WrapAroundCheckEnable ); 00126 00127 aSerial->printf("\n\r Reporting All Fields of VL53L0X_DevData_t Data structure \n\r" ); 00128 aSerial->printf(" .OscFrequency_MHz = %3.2fMHz; Frequency used \n\r", (float) TOF1.Data.OscFrequency_MHz/65536 ); 00129 aSerial->printf(" .LastEncodedTimeout = %d[u16]; Last encoded Time out used for timing budget \n\r", TOF1.Data.LastEncodedTimeout ); 00130 aSerial->printf(" .Pin0GpioFunctionality = %d[u8]; functionality of the GPIO: pin0 \n\r", TOF1.Data.Pin0GpioFunctionality ); 00131 aSerial->printf(" .FinalRangeTimeout_us = %d[u32]; Execution time of the final ranging \n\r", TOF1.Data.FinalRangeTimeout_us ); 00132 aSerial->printf(" .FinalRangeVcselPulsePeriod= %d[u8]; Vcsel pulse period (pll clocks) for the final range measurement \n\r", TOF1.Data.FinalRangeVcselPulsePeriod ); 00133 aSerial->printf(" .PreRangeTimeout_us = %d[u32]; Execution time of the final range \n\r", TOF1.Data.PreRangeTimeout_us ); 00134 aSerial->printf(" .PreRangeVcselPulsePeriod = %d[u8]; Vcsel pulse period (pll clocks) for the pre-range measurement \n\r", TOF1.Data.PreRangeVcselPulsePeriod ); 00135 aSerial->printf(" .ReadDataFromDeviceDone = %2d; reads from device has been done (>0) or not. \n\r", TOF1.Data.ReadDataFromDeviceDone ); 00136 aSerial->printf(" .ModuleId = %X; Module ID \n\r", TOF1.Data.ModuleId ); 00137 aSerial->printf(" .Revision = %d[u8]; test Revision \n\r", TOF1.Data.Revision ); 00138 aSerial->printf(" .ProductId = %s[char*]; Product Identifier String \n\r", TOF1.Data.ProductId ); 00139 aSerial->printf(" .ReferenceSpadCount = %d[u8]; used for ref spad management \n\r", TOF1.Data.ReferenceSpadCount ); 00140 aSerial->printf(" .ReferenceSpadType = %d[u8]; used for ref spad management \n\r", TOF1.Data.ReferenceSpadType ); 00141 aSerial->printf(" .RefSpadsInitialised = %d[u8]; reports if ref spads are initialised. \n\r", TOF1.Data.RefSpadsInitialised ); 00142 aSerial->printf(" .PartUIDUpper = %d[u32]; Unique Part ID Upper \n\r", TOF1.Data.PartUIDUpper ); 00143 aSerial->printf(" .PartUIDLower = %d[u32]; Unique Part ID Lower \n\r", TOF1.Data.PartUIDLower ); 00144 aSerial->printf(" .SignalRateMeasFixed400mm = %3.3f; Peak Signal rate at 400 mm \n\r", 1.0 / 65535.0 * TOF1.Data.SignalRateMeasFixed400mm ); 00145 aSerial->printf(" .RefSpadEnables[x] = %X %X %X %X %X %X[hex8]; Reference Spad Enables \n\r", 00146 TOF1.Data.RefSpadEnables[0], TOF1.Data.RefSpadEnables[1], TOF1.Data.RefSpadEnables[2], 00147 TOF1.Data.RefSpadEnables[3], TOF1.Data.RefSpadEnables[4], TOF1.Data.RefSpadEnables[5] ); 00148 aSerial->printf(" .RefGoodSpadMap[x] = %X %X %X %X %X %X[hex8]; Reference Spad Good Spad Map\n\r", 00149 TOF1.Data.RefGoodSpadMap[0], TOF1.Data.RefGoodSpadMap[1], TOF1.Data.RefGoodSpadMap[2], 00150 TOF1.Data.RefGoodSpadMap[3], TOF1.Data.RefGoodSpadMap[4], TOF1.Data.RefGoodSpadMap[5] ); 00151 aSerial->printf(" .Part2PartOffsetNVM_um = %d[i32]; backed up NVM value \n\r", TOF1.Data.Part2PartOffsetNVM_um ); 00152 aSerial->printf(" .Part2PartOffsetAdjustNVM_um= %d[i32]; backed up NVM value of additional offset adjustment \n\r", TOF1.Data.Part2PartOffsetAdjustNVM_um ); 00153 aSerial->printf(" .SequenceConfig = %d[u8]; Internal value for the sequence config \n\r", TOF1.Data.SequenceConfig ); 00154 aSerial->printf(" .RangeFractionalEnable = %d[u8]; Enable/Disable fractional part of range data \n\r", TOF1.Data.RangeFractionalEnable); 00155 aSerial->printf(" .PalState = %d[u8]; Current state of the PAL \n\r", TOF1.Data.PalState ); 00156 aSerial->printf(" .PowerMode = %d[u8]; Current Power Mode; Stdby1/2, Idle1/2 \n\r", TOF1.Data.PowerMode ); 00157 aSerial->printf(" .SigmaEstRefArray = %d[u16]; Reference array sigma value in 1/100th of [mm] \n\r", TOF1.Data.SigmaEstRefArray ); 00158 aSerial->printf(" .SigmaEstEffPulseWidth = %d[u16]; Effective Pulse width for sigma estimate in 1/100th of ns \n\r", TOF1.Data.SigmaEstEffPulseWidth ); 00159 aSerial->printf(" .SigmaEstEffAmbWidth = %d. Effective Ambient width for sigma estimate in 1/100th of ns \n\r", TOF1.Data.SigmaEstEffAmbWidth ); 00160 aSerial->printf(" .StopVariable = %d[u8]; StopVariable used during the stop sequence \n\r", TOF1.Data.StopVariable ); 00161 aSerial->printf(" .targetRefRate = %d. Target Ambient Rate for Ref spad management \n\r", TOF1.Data.targetRefRate ); 00162 aSerial->printf(" .LastSignalRef_MHz = %3.3fMHz; Latest Signal ref \n\r", TOF1.Data.LastSignalRef_MHz / 65535.01 ); 00163 aSerial->printf(" .UseInternalTuningSetting = %d[u8]; Indicate if we use Tuning Settings table \n\r", TOF1.Data.UseInternalTuningSettings ); 00164 aSerial->printf(" .LinearityCorrectiveGain = %d[u8]; Linearity Corrective Gain value in x1000 \n\r", TOF1.Data.LinearityCorrectiveGain ); 00165 aSerial->printf(" .DmaxCalRange_mm = %dmm; Dmax Calibration Range \n\r", TOF1.Data.DmaxCalRange_mm ); 00166 aSerial->printf(" .DmaxCalSignalRateRtn_MHz = %3.3fMHz; Dmax Calibration Signal Rate Return \n\r", TOF1.Data.DmaxCalSignalRateRtn_MHz / 65535.01 ); 00167 } 00168 00169 int VL53L0X::read_id(uint8_t *id) 00170 { int status = 0; 00171 uint16_t rl_id = 0; 00172 00173 status = VL53L0X_read_word(VL53L0X_REG_IDENTIFICATION_MODEL_ID, &rl_id); 00174 if (rl_id == 0xEEAA) { 00175 return status; 00176 } 00177 return -1; 00178 } 00179 00180 int VL53L0X::init_sensor(uint8_t new_addr) 00181 { int status; 00182 00183 VL53L0X_off(); 00184 VL53L0X_on(); 00185 00186 // Verify if the device is actually present 00187 uint8_t id = 0; 00188 status = read_id(&id); 00189 if (status != 0) { 00190 //aSerial->printf("VL53L0X sensor is not present!\n\r"); 00191 return 99; } // device is not present 00192 00193 status = VL53L0X_data_init(); 00194 if (status != VL53L0X_ERROR_NONE) { 00195 //aSerial->printf("Failed to init VL53L0X sensor!\n\r"); 00196 return status; 00197 } 00198 00199 // deduce silicon version 00200 status = VL53L0X_get_device_info(); 00201 00202 status = prepare(); 00203 if (status != VL53L0X_ERROR_NONE) { 00204 //aSerial->printf("Failed to prepare VL53L0X!\n\r"); 00205 return status; 00206 } 00207 00208 if (new_addr != VL53L0X_DEFAULT_ADDRESS) { 00209 status = set_device_address(new_addr); 00210 if (status) { 00211 //aSerial->printf("Failed to change I2C address!\n\r"); 00212 return status; 00213 } 00214 } 00215 return status; 00216 } 00217 00218 VL53L0X_Error VL53L0X::VL53L0X_data_init(void) 00219 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 00220 VL53L0X_DeviceParameters_t CurrentParameters ; 00221 int i; 00222 uint8_t StopVariable; 00223 00224 /* by default the I2C is running at 1V8 if you want to change it you 00225 * need to include this define at compilation level. */ 00226 #ifdef USE_I2C_2V8 00227 Status = VL53L0X_UpdateByte(VL53L0X_REG_VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV,0xFE,0x01); 00228 #endif 00229 00230 /* Set I2C standard mode */ 00231 if (status == VL53L0X_ERROR_NONE) { 00232 status = VL53L0X_write_byte( 0x88, 0x00); } 00233 00234 Data.ReadDataFromDeviceDone = 0; 00235 Data.ReadDataFromDeviceDone = 0; 00236 00237 #ifdef USE_IQC_STATION 00238 if (Status == VL53L0X_ERROR_NONE) { 00239 Status = VL53L0X_apply_offset_adjustment(); 00240 } 00241 #endif 00242 00243 /* Default value is 1000 for Linearity Corrective Gain */ 00244 Data.LinearityCorrectiveGain = 1000; 00245 00246 /* Dmax default Parameter */ 00247 Data.DmaxCalRange_mm = 400; 00248 Data.DmaxCalSignalRateRtn_MHz = (FixPoint1616_t)((0x00016B85)); /* 1.42 No Cover Glass*/ 00249 00250 /* Set Default static parameters 00251 *set first temporary values 9.44_MHz * 65536 = 618660 */ 00252 Data.OscFrequency_MHz = 618660; 00253 00254 /* Set Default XTalkCompensationRate_MHz to 0 */ 00255 CurrentParameters.XTalkCompensationRate_MHz = 0; 00256 00257 /* Get default parameters */ 00258 status = VL53L0X_get_device_parameters( &CurrentParameters); 00259 if (status == VL53L0X_ERROR_NONE) { 00260 /* initialize PAL values */ 00261 CurrentParameters.DeviceMode = VL53L0X_DEVICEMODE_SINGLE_RANGING; 00262 CurrentParameters = CurrentParameters ; 00263 } 00264 00265 /* Sigma estimator variable */ 00266 Data.SigmaEstRefArray = 100; 00267 Data.SigmaEstEffPulseWidth = 900; 00268 Data.SigmaEstEffAmbWidth = 500; 00269 Data.targetRefRate = 0x0A00; /* 20 MHz in 9:7 format */ 00270 00271 /* Use internal default settings */ 00272 Data.UseInternalTuningSettings = 1; 00273 00274 status |= VL53L0X_write_byte( 0x80, 0x01); 00275 status |= VL53L0X_write_byte( 0xFF, 0x01); 00276 status |= VL53L0X_write_byte( 0x00, 0x00); 00277 status |= VL53L0X_read_byte( 0x91, &StopVariable); 00278 Data.StopVariable = StopVariable; 00279 status |= VL53L0X_write_byte( 0x00, 0x01); 00280 status |= VL53L0X_write_byte( 0xFF, 0x00); 00281 status |= VL53L0X_write_byte( 0x80, 0x00); 00282 00283 /* Enable all check */ 00284 for (i = 0; i < VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS; i++) { 00285 if (status == VL53L0X_ERROR_NONE) { 00286 status |= VL53L0X_set_limit_check_enable( i, 1); 00287 } else { break; } 00288 } 00289 00290 /* Disable the following checks */ 00291 if (status == VL53L0X_ERROR_NONE) 00292 status = VL53L0X_set_limit_check_enable(VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, 0); 00293 00294 if (status == VL53L0X_ERROR_NONE) 00295 status = VL53L0X_set_limit_check_enable(VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, 0); 00296 00297 if (status == VL53L0X_ERROR_NONE) 00298 status = VL53L0X_set_limit_check_enable(VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC, 0); 00299 00300 if (status == VL53L0X_ERROR_NONE) 00301 status = VL53L0X_set_limit_check_enable(VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE, 0); 00302 00303 /* Limit default values */ 00304 if (status == VL53L0X_ERROR_NONE) { 00305 status = VL53L0X_set_limit_check_value(VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, 00306 (FixPoint1616_t)(18 * 65536)); 00307 } 00308 if (status == VL53L0X_ERROR_NONE) { 00309 status = VL53L0X_set_limit_check_value(VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, 00310 (FixPoint1616_t)(25 * 65536 / 100)); /* 0.25 * 65536 */ 00311 } 00312 00313 if (status == VL53L0X_ERROR_NONE) { 00314 status = VL53L0X_set_limit_check_value(VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, 00315 (FixPoint1616_t)(35 * 65536)); 00316 } 00317 00318 if (status == VL53L0X_ERROR_NONE) { 00319 status = VL53L0X_set_limit_check_value(VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, 00320 (FixPoint1616_t)(0 * 65536)); 00321 } 00322 00323 if (status == VL53L0X_ERROR_NONE) { 00324 Data.SequenceConfig = 0xFF; 00325 status = VL53L0X_write_byte( VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,0xFF); 00326 00327 /* Set PAL state to tell that we are waiting for call to VL53L0X_StaticInit */ 00328 Data.PalState = VL53L0X_STATE_WAIT_STATICINIT; 00329 } 00330 00331 if (status == VL53L0X_ERROR_NONE) { 00332 Data.RefSpadsInitialised = 0; 00333 } 00334 00335 return status; 00336 } 00337 00338 int VL53L0X::prepare() 00339 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 00340 uint32_t ref_spad_count; 00341 uint8_t is_aperture_spads; 00342 uint8_t vhv_settings; 00343 uint8_t phase_cal; 00344 00345 if (status == VL53L0X_ERROR_NONE) { 00346 status = VL53L0X_static_init(); // Device Initialization 00347 } 00348 00349 if (status == VL53L0X_ERROR_NONE) { 00350 status = VL53L0X_perform_ref_calibration(&vhv_settings, &phase_cal); // Device Initialization 00351 } 00352 00353 if (status == VL53L0X_ERROR_NONE) { 00354 status = VL53L0X_perform_ref_spad_management(&ref_spad_count, &is_aperture_spads); // Device Initialization 00355 } 00356 00357 return status; 00358 } 00359 00360 int VL53L0X::start_measurement(OperatingMode operating_mode, void (*fptr)(void), 00361 VL53L0X_RangingConfig rangingConfig) 00362 { int Status = VL53L0X_ERROR_NONE; 00363 int ClrStatus; 00364 00365 uint8_t VhvSettings; 00366 uint8_t PhaseCal; 00367 // default settings, for normal range. 00368 FixPoint1616_t signalLimit = (FixPoint1616_t)(0.25 * 65536); 00369 FixPoint1616_t sigmaLimit = (FixPoint1616_t)(25 * 65536); 00370 uint32_t timingBudget = 33000; 00371 uint8_t preRangeVcselPeriod = 14; 00372 uint8_t finalRangeVcselPeriod = 10; 00373 00374 if (operating_mode == range_continuous_interrupt) { 00375 if (_gpio1Int == NULL) { 00376 //aSerial->printf("GPIO1 Error\r\n"); 00377 return 1; 00378 } 00379 00380 Status = VL53L0X_stop_measurement(); // it is safer to do this while sensor is stopped 00381 00382 // Status = VL53L0X_SetInterruptThresholds(Device, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING, 0, 300); 00383 00384 Status = VL53L0X_set_gpio_config(0, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING, 00385 VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY, 00386 VL53L0X_INTERRUPTPOLARITY_HIGH); 00387 00388 if (Status == VL53L0X_ERROR_NONE) { 00389 attach_interrupt_measure_detection_irq(fptr); 00390 enable_interrupt_measure_detection_irq(); 00391 } 00392 00393 ClrStatus = clear_interrupt(VL53L0X_REG_RESULT_INTERRUPT_STATUS | VL53L0X_REG_RESULT_RANGE_STATUS); 00394 if (ClrStatus) { Status = 97; } // VL53L0X_ClearErrorInterrupt fail 00395 00396 if (Status == VL53L0X_ERROR_NONE) { 00397 CurrentParameters .DeviceMode = VL53L0X_DEVICEMODE_CONTINUOUS_RANGING; // Setup in continuous ranging mode 00398 Status = VL53L0X_start_measurement(); 00399 } 00400 } 00401 00402 if (operating_mode == range_single_shot_polling) { 00403 // singelshot, polled ranging 00404 if (Status == VL53L0X_ERROR_NONE) { 00405 // no need to do this when we use VL53L0X_PerformSingleRangingMeasurement 00406 CurrentParameters .DeviceMode = VL53L0X_DEVICEMODE_SINGLE_RANGING; // Setup in single ranging mode 00407 // Enable/Disable Sigma and Signal check 00408 Status = VL53L0X_set_limit_check_enable(VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, 1); 00409 } 00410 if (Status == VL53L0X_ERROR_NONE) { 00411 Status = VL53L0X_set_limit_check_enable(VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, 1); 00412 } 00413 00414 /* Preselected Ranging configurations */ 00415 switch(rangingConfig) { 00416 case Range_Config_DEFAULT: 00417 // default settings, for normal range. 00418 signalLimit = (FixPoint1616_t)(0.25 * 65536); 00419 sigmaLimit = (FixPoint1616_t)(16 * 65536); 00420 timingBudget = 33000; 00421 preRangeVcselPeriod = 14; 00422 finalRangeVcselPeriod = 10; 00423 break; 00424 case Range_Config_LONG_RANGE: // *** from mass market cube expansion v1.1, ranging with satellites. 00425 signalLimit = (FixPoint1616_t)(0.1 * 65536); 00426 sigmaLimit = (FixPoint1616_t)(60 * 65536); 00427 timingBudget = 33000; 00428 preRangeVcselPeriod = 18; 00429 finalRangeVcselPeriod = 14; 00430 break; 00431 case Range_Config_HIGH_ACCURACY: 00432 signalLimit = (FixPoint1616_t)(0.25*65536); 00433 sigmaLimit = (FixPoint1616_t)(18*65536); 00434 timingBudget = 200000; 00435 preRangeVcselPeriod = 14; 00436 finalRangeVcselPeriod = 10; 00437 break; 00438 case Range_Config_HIGH_SPEED: 00439 signalLimit = (FixPoint1616_t)(0.25*65536); 00440 sigmaLimit = (FixPoint1616_t)(60*65536); 00441 timingBudget = 20000; 00442 preRangeVcselPeriod = 14; 00443 finalRangeVcselPeriod = 10; 00444 break; 00445 default: 00446 Status = 96; // Config Not Supported 00447 } 00448 00449 if (Status == VL53L0X_ERROR_NONE) { 00450 Status = VL53L0X_set_limit_check_value(VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, signalLimit);} 00451 00452 if (Status == VL53L0X_ERROR_NONE) { 00453 Status = VL53L0X_set_limit_check_value(VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, sigmaLimit);} 00454 00455 if (Status == VL53L0X_ERROR_NONE) { 00456 Status = VL53L0X_set_measurement_timing_budget_us( timingBudget);} 00457 00458 if (Status == VL53L0X_ERROR_NONE) { 00459 Status = VL53L0X_set_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_PRE_RANGE, preRangeVcselPeriod);} 00460 00461 if (Status == VL53L0X_ERROR_NONE) { 00462 Status = VL53L0X_set_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_FINAL_RANGE, finalRangeVcselPeriod);} 00463 00464 if (Status == VL53L0X_ERROR_NONE) { 00465 Status = VL53L0X_perform_ref_calibration( &VhvSettings, &PhaseCal);} 00466 00467 } 00468 00469 if (operating_mode == range_continuous_polling) { 00470 if (Status == VL53L0X_ERROR_NONE) { 00471 CurrentParameters .DeviceMode = VL53L0X_DEVICEMODE_CONTINUOUS_RANGING; // Setup in continuous ranging mode 00472 Status = VL53L0X_start_measurement(); 00473 } 00474 } 00475 return Status; 00476 } 00477 00478 int VL53L0X::range_meas_int_continuous_mode(void (*fptr)(void)) 00479 { int status, clr_status; 00480 00481 status = VL53L0X_stop_measurement(); // it is safer to do this while sensor is stopped 00482 00483 // status = VL53L0X_SetInterruptThresholds(Device, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING, 0, 300); 00484 00485 status = VL53L0X_set_gpio_config( 0, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING, 00486 VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY, VL53L0X_INTERRUPTPOLARITY_HIGH); 00487 00488 if (!status) { 00489 attach_interrupt_measure_detection_irq(fptr); 00490 enable_interrupt_measure_detection_irq(); 00491 } 00492 00493 clr_status = clear_interrupt(VL53L0X_REG_RESULT_INTERRUPT_STATUS | VL53L0X_REG_RESULT_RANGE_STATUS); 00494 if (clr_status!=0) { status = 98; } // VL53L0X_ClearErrorInterrupt_fail; 00495 00496 if (!status) { 00497 status = range_start_continuous_mode(); 00498 } 00499 return status; 00500 } 00501 00502 VL53L0X_Error VL53L0X::wait_measurement_data_ready(void) 00503 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 00504 uint8_t new_dat_ready = 0; 00505 uint32_t loop_nb; 00506 00507 // Wait until it finished 00508 // use timeout to avoid deadlock 00509 if (status == VL53L0X_ERROR_NONE) { 00510 loop_nb = 0; 00511 do { 00512 status = VL53L0X_get_measurement_data_ready( &new_dat_ready); 00513 if ((new_dat_ready == 0x01) || status != VL53L0X_ERROR_NONE) { 00514 break; 00515 } 00516 loop_nb = loop_nb + 1; 00517 VL53L0X_polling_delay(); 00518 } while (loop_nb < VL53L0X_DEFAULT_MAX_LOOP); 00519 00520 if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP) { 00521 status = VL53L0X_ERROR_TIME_OUT; 00522 } 00523 } 00524 00525 return status; 00526 } 00527 00528 int VL53L0X::get_distance(uint32_t *p_data) 00529 { 00530 int status = 0; 00531 VL53L0X_RangingMeasurementData_t p_ranging_measurement_data; 00532 00533 status = start_measurement(range_single_shot_polling, NULL, Range_Config_DEFAULT); 00534 if (!status) { 00535 status = get_measurement(range_single_shot_polling, &p_ranging_measurement_data); 00536 } 00537 if (p_ranging_measurement_data.RangeStatus == 0) { // we have a valid range. 00538 *p_data = p_ranging_measurement_data.Range_mm; 00539 } else { 00540 *p_data = 0; 00541 status = VL53L0X_ERROR_RANGE_ERROR; 00542 } 00543 stop_measurement(range_single_shot_polling); 00544 return status; 00545 } 00546 00547 VL53L0X_Error VL53L0X::wait_stop_completed(void) 00548 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 00549 uint32_t stop_completed = 0; 00550 uint32_t loop_nb; 00551 00552 // Wait until it finished 00553 // use timeout to avoid deadlock 00554 if (status == VL53L0X_ERROR_NONE) { 00555 loop_nb = 0; 00556 do { 00557 status = VL53L0X_get_stop_completed_status( &stop_completed); 00558 if ((stop_completed == 0x00) || status != VL53L0X_ERROR_NONE) { 00559 break; 00560 } 00561 loop_nb = loop_nb + 1; 00562 VL53L0X_polling_delay(); 00563 } while (loop_nb < VL53L0X_DEFAULT_MAX_LOOP); 00564 00565 if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP) { 00566 status = VL53L0X_ERROR_TIME_OUT; 00567 } 00568 } 00569 00570 return status; 00571 } 00572 00573 int VL53L0X::get_measurement(OperatingMode operating_mode, VL53L0X_RangingMeasurementData_t *p_data) 00574 { int Status = VL53L0X_ERROR_NONE; 00575 00576 if (operating_mode == range_single_shot_polling) { 00577 Status = VL53L0X_perform_single_ranging_measurement( p_data); 00578 } 00579 00580 if (operating_mode == range_continuous_polling) { 00581 if (Status == VL53L0X_ERROR_NONE) { 00582 Status = VL53L0X_measurement_poll_for_completion(); 00583 } 00584 00585 if (Status == VL53L0X_ERROR_NONE) { 00586 Status = VL53L0X_get_ranging_measurement_data( p_data); 00587 00588 // Clear the interrupt 00589 VL53L0X_clear_interrupt_mask( VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY); 00590 VL53L0X_polling_delay(); 00591 } 00592 } 00593 00594 if (operating_mode == range_continuous_interrupt) { 00595 Status = VL53L0X_get_ranging_measurement_data( p_data); 00596 VL53L0X_clear_interrupt_mask( VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR | VL53L0X_REG_RESULT_INTERRUPT_STATUS); 00597 } 00598 00599 return Status; 00600 } 00601 00602 int VL53L0X::stop_measurement(OperatingMode operating_mode) 00603 { int status = VL53L0X_ERROR_NONE; 00604 00605 // don't need to stop for a singleshot range! 00606 if (operating_mode == range_single_shot_polling) { 00607 } 00608 00609 if (operating_mode == range_continuous_interrupt || operating_mode == range_continuous_polling) { 00610 // continuous mode 00611 if (status == VL53L0X_ERROR_NONE) { 00612 //aSerial->printf("Call of VL53L0X_StopMeasurement\n"); 00613 status = VL53L0X_stop_measurement(); 00614 } 00615 00616 if (status == VL53L0X_ERROR_NONE) { 00617 //aSerial->printf("Wait Stop to be competed\n"); 00618 status = wait_stop_completed(); 00619 } 00620 00621 if (status == VL53L0X_ERROR_NONE) 00622 status = VL53L0X_clear_interrupt_mask(VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY); 00623 } 00624 00625 return status; 00626 } 00627 00628 int VL53L0X::handle_irq(OperatingMode operating_mode, VL53L0X_RangingMeasurementData_t *data) 00629 { int status; 00630 status = get_measurement(operating_mode, data); 00631 enable_interrupt_measure_detection_irq(); 00632 return status; 00633 } 00634 00635 int VL53L0X::range_start_continuous_mode() 00636 { CurrentParameters .DeviceMode = VL53L0X_DEVICEMODE_CONTINUOUS_RANGING; 00637 00638 return VL53L0X_start_measurement(); 00639 00640 } 00641 00642 VL53L0X_Error VL53L0X::VL53L0X_device_read_strobe(void) 00643 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 00644 uint8_t strobe; 00645 uint32_t loop_nb; 00646 00647 status |= VL53L0X_write_byte( 0x83, 0x00); 00648 00649 /* polling 00650 * use timeout to avoid deadlock*/ 00651 if (status == VL53L0X_ERROR_NONE) { 00652 loop_nb = 0; 00653 do { 00654 status = VL53L0X_read_byte( 0x83, &strobe); 00655 if ((strobe != 0x00) || status != VL53L0X_ERROR_NONE) { 00656 break; 00657 } 00658 00659 loop_nb = loop_nb + 1; 00660 } while (loop_nb < VL53L0X_DEFAULT_MAX_LOOP); 00661 00662 if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP) { 00663 status = VL53L0X_ERROR_TIME_OUT; 00664 } 00665 } 00666 00667 status |= VL53L0X_write_byte( 0x83, 0x01); 00668 00669 return status; 00670 } 00671 00672 VL53L0X_Error VL53L0X::VL53L0X_get_info_from_device( uint8_t option) 00673 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 00674 uint8_t byte; 00675 uint32_t tmp_dword; 00676 uint8_t module_id; 00677 uint8_t revision; 00678 uint8_t reference_spad_count = 0; 00679 uint8_t reference_spad_type = 0; 00680 uint32_t part_uid_upper = 0; 00681 uint32_t part_uid_lower = 0; 00682 uint32_t offset_fixed1104_mm = 0; 00683 int16_t offset_micro_meters = 0; 00684 uint32_t dist_meas_tgt_fixed1104_mm = 400 << 4; 00685 uint32_t dist_meas_fixed1104_400_mm = 0; 00686 uint32_t signal_rate_meas_fixed1104_400_mm = 0; 00687 char product_id[19]; 00688 char *product_id_tmp; 00689 uint8_t read_data_from_device_done; 00690 FixPoint1616_t signal_rate_meas_fixed400_mm_fix = 0; 00691 uint8_t nvm_ref_good_spad_map[VL53L0X_REF_SPAD_BUFFER_SIZE]; 00692 int i; 00693 00694 read_data_from_device_done = Data.ReadDataFromDeviceDone; 00695 read_data_from_device_done = Data.ReadDataFromDeviceDone; 00696 read_data_from_device_done = Data.ReadDataFromDeviceDone; 00697 00698 /* This access is done only once after that a GetDeviceInfo or 00699 * datainit is done*/ 00700 if (read_data_from_device_done != 7) { 00701 00702 status |= VL53L0X_write_byte( 0x80, 0x01); 00703 status |= VL53L0X_write_byte( 0xFF, 0x01); 00704 status |= VL53L0X_write_byte( 0x00, 0x00); 00705 status |= VL53L0X_write_byte( 0xFF, 0x06); 00706 status |= VL53L0X_read_byte ( 0x83, &byte); 00707 status |= VL53L0X_write_byte( 0x83, byte | 4); 00708 status |= VL53L0X_write_byte( 0xFF, 0x07); 00709 status |= VL53L0X_write_byte( 0x81, 0x01); 00710 00711 status |= VL53L0X_polling_delay(); 00712 00713 status |= VL53L0X_write_byte( 0x80, 0x01); 00714 00715 if (((option & 1) == 1) && 00716 ((read_data_from_device_done & 1) == 0)) { 00717 status |= VL53L0X_write_byte( 0x94, 0x6b); 00718 status |= VL53L0X_device_read_strobe(); 00719 status |= VL53L0X_read_dword( 0x90, &tmp_dword); 00720 00721 reference_spad_count = (uint8_t)((tmp_dword >> 8) & 0x07f); 00722 reference_spad_type = (uint8_t)((tmp_dword >> 15) & 0x01); 00723 00724 status |= VL53L0X_write_byte( 0x94, 0x24); 00725 status |= VL53L0X_device_read_strobe(); 00726 status |= VL53L0X_read_dword( 0x90, &tmp_dword); 00727 00728 nvm_ref_good_spad_map[0] = (uint8_t)((tmp_dword >> 24) & 0xff); 00729 nvm_ref_good_spad_map[1] = (uint8_t)((tmp_dword >> 16) & 0xff); 00730 nvm_ref_good_spad_map[2] = (uint8_t)((tmp_dword >> 8) & 0xff); 00731 nvm_ref_good_spad_map[3] = (uint8_t)(tmp_dword & 0xff); 00732 00733 status |= VL53L0X_write_byte( 0x94, 0x25); 00734 status |= VL53L0X_device_read_strobe(); 00735 status |= VL53L0X_read_dword( 0x90, &tmp_dword); 00736 00737 nvm_ref_good_spad_map[4] = (uint8_t)((tmp_dword >> 24) & 0xff); 00738 nvm_ref_good_spad_map[5] = (uint8_t)((tmp_dword >> 16) & 0xff); 00739 } 00740 00741 if (((option & 2) == 2) && 00742 ((read_data_from_device_done & 2) == 0)) { 00743 00744 status |= VL53L0X_write_byte( 0x94, 0x02); 00745 status |= VL53L0X_device_read_strobe(); 00746 status |= VL53L0X_read_byte( 0x90, &module_id); 00747 00748 status |= VL53L0X_write_byte( 0x94, 0x7B); 00749 status |= VL53L0X_device_read_strobe(); 00750 status |= VL53L0X_read_byte( 0x90, &revision); 00751 00752 status |= VL53L0X_write_byte( 0x94, 0x77); 00753 status |= VL53L0X_device_read_strobe(); 00754 status |= VL53L0X_read_dword( 0x90, &tmp_dword); 00755 00756 product_id[0] = (char)((tmp_dword >> 25) & 0x07f); 00757 product_id[1] = (char)((tmp_dword >> 18) & 0x07f); 00758 product_id[2] = (char)((tmp_dword >> 11) & 0x07f); 00759 product_id[3] = (char)((tmp_dword >> 4) & 0x07f); 00760 00761 byte = (uint8_t)((tmp_dword & 0x00f) << 3); 00762 00763 status |= VL53L0X_write_byte( 0x94, 0x78); 00764 status |= VL53L0X_device_read_strobe(); 00765 status |= VL53L0X_read_dword( 0x90, &tmp_dword); 00766 00767 product_id[4] = (char)(byte + 00768 ((tmp_dword >> 29) & 0x07f)); 00769 product_id[5] = (char)((tmp_dword >> 22) & 0x07f); 00770 product_id[6] = (char)((tmp_dword >> 15) & 0x07f); 00771 product_id[7] = (char)((tmp_dword >> 8) & 0x07f); 00772 product_id[8] = (char)((tmp_dword >> 1) & 0x07f); 00773 00774 byte = (uint8_t)((tmp_dword & 0x001) << 6); 00775 00776 status |= VL53L0X_write_byte( 0x94, 0x79); 00777 00778 status |= VL53L0X_device_read_strobe(); 00779 00780 status |= VL53L0X_read_dword( 0x90, &tmp_dword); 00781 00782 product_id[9] = (char)(byte + 00783 ((tmp_dword >> 26) & 0x07f)); 00784 product_id[10] = (char)((tmp_dword >> 19) & 0x07f); 00785 product_id[11] = (char)((tmp_dword >> 12) & 0x07f); 00786 product_id[12] = (char)((tmp_dword >> 5) & 0x07f); 00787 00788 byte = (uint8_t)((tmp_dword & 0x01f) << 2); 00789 00790 status |= VL53L0X_write_byte( 0x94, 0x7A); 00791 00792 status |= VL53L0X_device_read_strobe(); 00793 00794 status |= VL53L0X_read_dword( 0x90, &tmp_dword); 00795 00796 product_id[13] = (char)(byte + 00797 ((tmp_dword >> 30) & 0x07f)); 00798 product_id[14] = (char)((tmp_dword >> 23) & 0x07f); 00799 product_id[15] = (char)((tmp_dword >> 16) & 0x07f); 00800 product_id[16] = (char)((tmp_dword >> 9) & 0x07f); 00801 product_id[17] = (char)((tmp_dword >> 2) & 0x07f); 00802 product_id[18] = '\0'; 00803 00804 } 00805 00806 if (((option & 4) == 4) && 00807 ((read_data_from_device_done & 4) == 0)) { 00808 00809 status |= VL53L0X_write_byte( 0x94, 0x7B); 00810 status |= VL53L0X_device_read_strobe(); 00811 status |= VL53L0X_read_dword( 0x90, &part_uid_upper); 00812 status |= VL53L0X_write_byte( 0x94, 0x7C); 00813 status |= VL53L0X_device_read_strobe(); 00814 status |= VL53L0X_read_dword( 0x90, &part_uid_lower); 00815 00816 status |= VL53L0X_write_byte( 0x94, 0x73); 00817 status |= VL53L0X_device_read_strobe(); 00818 status |= VL53L0X_read_dword( 0x90, &tmp_dword); 00819 signal_rate_meas_fixed1104_400_mm = (tmp_dword & 0x0000000ff) << 8; 00820 00821 status |= VL53L0X_write_byte( 0x94, 0x74); 00822 status |= VL53L0X_device_read_strobe(); 00823 status |= VL53L0X_read_dword( 0x90, &tmp_dword); 00824 signal_rate_meas_fixed1104_400_mm |= ((tmp_dword & 0xff000000) >> 24); 00825 00826 status |= VL53L0X_write_byte( 0x94, 0x75); 00827 status |= VL53L0X_device_read_strobe(); 00828 status |= VL53L0X_read_dword( 0x90, &tmp_dword); 00829 dist_meas_fixed1104_400_mm = (tmp_dword & 0x0000000ff) << 8; 00830 00831 status |= VL53L0X_write_byte( 0x94, 0x76); 00832 status |= VL53L0X_device_read_strobe(); 00833 status |= VL53L0X_read_dword( 0x90, &tmp_dword); 00834 dist_meas_fixed1104_400_mm |= ((tmp_dword & 0xff000000) >> 24); 00835 } 00836 00837 status |= VL53L0X_write_byte( 0x81, 0x00); 00838 status |= VL53L0X_write_byte( 0xFF, 0x06); 00839 status |= VL53L0X_read_byte( 0x83, &byte); 00840 status |= VL53L0X_write_byte( 0x83, byte & 0xfb); 00841 status |= VL53L0X_write_byte( 0xFF, 0x01); 00842 status |= VL53L0X_write_byte( 0x00, 0x01); 00843 status |= VL53L0X_write_byte( 0xFF, 0x00); 00844 status |= VL53L0X_write_byte( 0x80, 0x00); 00845 } 00846 00847 if ((status == VL53L0X_ERROR_NONE) && 00848 (read_data_from_device_done != 7)) { 00849 /* Assign to variable if status is ok */ 00850 if (((option & 1) == 1) && 00851 ((read_data_from_device_done & 1) == 0)) { 00852 Data.ReferenceSpadCount = reference_spad_count; 00853 Data.ReferenceSpadType = reference_spad_type; 00854 00855 for (i = 0; i < VL53L0X_REF_SPAD_BUFFER_SIZE; i++) { 00856 Data.RefGoodSpadMap[i] = 00857 nvm_ref_good_spad_map[i]; 00858 } 00859 } 00860 00861 if (((option & 2) == 2) && 00862 ((read_data_from_device_done & 2) == 0)) { 00863 Data.ModuleId = module_id; 00864 Data.Revision = revision; 00865 product_id_tmp = Data.ProductId; 00866 VL53L0X_COPYSTRING(product_id_tmp, product_id); 00867 } 00868 00869 if (((option & 4) == 4) && 00870 ((read_data_from_device_done & 4) == 0)) { 00871 Data.PartUIDUpper = part_uid_upper; 00872 Data.PartUIDLower = part_uid_lower; 00873 signal_rate_meas_fixed400_mm_fix = 00874 VL53L0X_FP97TOFP1616(signal_rate_meas_fixed1104_400_mm); 00875 Data.SignalRateMeasFixed400mm = signal_rate_meas_fixed400_mm_fix; 00876 00877 offset_micro_meters = 0; 00878 if (dist_meas_fixed1104_400_mm != 0) { 00879 offset_fixed1104_mm = 00880 dist_meas_fixed1104_400_mm - 00881 dist_meas_tgt_fixed1104_mm; 00882 offset_micro_meters = (offset_fixed1104_mm 00883 * 1000) >> 4; 00884 offset_micro_meters *= -1; 00885 } 00886 00887 Data.Part2PartOffsetAdjustNVM_um = offset_micro_meters; 00888 } 00889 byte = (uint8_t)(read_data_from_device_done | option); 00890 Data.ReadDataFromDeviceDone = byte; 00891 } 00892 return status; 00893 } 00894 00895 VL53L0X_Error VL53L0X::VL53L0X_get_offset_calibration_data_micro_meter(int32_t *p_offset_calibration_data_micro_meter) 00896 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 00897 uint16_t range_offset_register; 00898 int16_t c_max_offset = 2047; 00899 int16_t c_offset_range = 4096; 00900 00901 /* Note, that offset has 10.2 format */ 00902 status = VL53L0X_read_word(VL53L0X_REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM, 00903 &range_offset_register); 00904 00905 if (status == VL53L0X_ERROR_NONE) { 00906 range_offset_register = (range_offset_register & 0x0fff); 00907 00908 /* Apply 12 bit 2's compliment conversion */ 00909 if (range_offset_register > c_max_offset) { 00910 *p_offset_calibration_data_micro_meter = 00911 (int16_t)(range_offset_register - c_offset_range) * 250; 00912 } else { 00913 *p_offset_calibration_data_micro_meter = 00914 (int16_t)range_offset_register * 250; } 00915 } 00916 00917 return status; 00918 } 00919 00920 VL53L0X_Error VL53L0X::VL53L0X_set_offset_calibration_data_micro_meter(int32_t offset_calibration_data_micro_meter) 00921 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 00922 int32_t c_max_offset_micro_meter = 511000; 00923 int32_t c_min_offset_micro_meter = -512000; 00924 int16_t c_offset_range = 4096; 00925 uint32_t encoded_offset_val; 00926 00927 if (offset_calibration_data_micro_meter > c_max_offset_micro_meter) { 00928 offset_calibration_data_micro_meter = c_max_offset_micro_meter; 00929 } else { 00930 if (offset_calibration_data_micro_meter < c_min_offset_micro_meter) { 00931 offset_calibration_data_micro_meter = c_min_offset_micro_meter; 00932 } 00933 } 00934 00935 /* The offset register is 10.2 format and units are mm 00936 * therefore conversion is applied by a division of 250. */ 00937 if (offset_calibration_data_micro_meter >= 0) { 00938 encoded_offset_val = offset_calibration_data_micro_meter / 250; 00939 } else { 00940 encoded_offset_val = 00941 c_offset_range + offset_calibration_data_micro_meter / 250; 00942 } 00943 00944 status = VL53L0X_write_word(VL53L0X_REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM, 00945 encoded_offset_val); 00946 return status; 00947 } 00948 00949 VL53L0X_Error VL53L0X::VL53L0X_apply_offset_adjustment(void) 00950 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 00951 int32_t corrected_offset_micro_meters; 00952 int32_t current_offset_micro_meters; 00953 00954 /* if we run on this function we can read all the NVM info used by the API */ 00955 status = VL53L0X_get_info_from_device( 7); 00956 00957 /* Read back current device offset */ 00958 if (status == VL53L0X_ERROR_NONE) { 00959 status = VL53L0X_get_offset_calibration_data_micro_meter(¤t_offset_micro_meters); 00960 } 00961 00962 /* Apply Offset Adjustment derived from 400mm measurements */ 00963 if (status == VL53L0X_ERROR_NONE) { 00964 00965 /* Store initial device offset */ 00966 Data.Part2PartOffsetNVM_um = current_offset_micro_meters; 00967 00968 corrected_offset_micro_meters = current_offset_micro_meters + 00969 (int32_t)Data.Part2PartOffsetAdjustNVM_um; 00970 00971 status = VL53L0X_set_offset_calibration_data_micro_meter(corrected_offset_micro_meters); 00972 00973 /* store current, adjusted offset */ 00974 if (status == VL53L0X_ERROR_NONE) { 00975 CurrentParameters .RangeOffset_um = corrected_offset_micro_meters; 00976 } 00977 } 00978 return status; 00979 } 00980 00981 VL53L0X_Error VL53L0X::VL53L0X_get_inter_measurement_period_ms(uint32_t *p_inter_measurement_period_ms) 00982 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 00983 uint16_t osc_calibrate_val; 00984 uint32_t im_period_ms; 00985 00986 00987 00988 status = VL53L0X_read_word( VL53L0X_REG_OSC_CALIBRATE_VAL, 00989 &osc_calibrate_val); 00990 00991 if (status == VL53L0X_ERROR_NONE) { 00992 status = VL53L0X_read_dword(VL53L0X_REG_SYSTEM_INTERMEASUREMENT_PERIOD, 00993 &im_period_ms); 00994 } 00995 00996 if (status == VL53L0X_ERROR_NONE) { 00997 if (osc_calibrate_val != 0) { 00998 *p_inter_measurement_period_ms = 00999 im_period_ms / osc_calibrate_val; 01000 } 01001 CurrentParameters .InterMeasurementPeriod_ms = *p_inter_measurement_period_ms; 01002 } 01003 01004 return status; 01005 } 01006 01007 VL53L0X_Error VL53L0X::VL53L0X_get_x_talk_compensation_rate_MHz(FixPoint1616_t *p_xtalk_compensation_rate_MHz) 01008 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 01009 uint16_t value; 01010 FixPoint1616_t temp_fix1616; 01011 01012 status = VL53L0X_read_word(VL53L0X_REG_CROSSTALK_COMPENSATION_PEAK_RATE_MHz, (uint16_t *)&value); 01013 if (status == VL53L0X_ERROR_NONE) { 01014 if (value == 0) { 01015 /* the Xtalk is disabled return value from memory */ 01016 temp_fix1616 = CurrentParameters .XTalkCompensationRate_MHz ; 01017 *p_xtalk_compensation_rate_MHz = temp_fix1616; 01018 CurrentParameters .XTalkCompensationEnable = 0; 01019 } else { 01020 temp_fix1616 = VL53L0X_FP313TOFP1616(value); 01021 *p_xtalk_compensation_rate_MHz = temp_fix1616; 01022 CurrentParameters .XTalkCompensationRate_MHz = temp_fix1616; 01023 CurrentParameters .XTalkCompensationEnable = 1; 01024 } 01025 } 01026 01027 return status; 01028 } 01029 01030 VL53L0X_Error VL53L0X::VL53L0X_get_limit_check_value( uint16_t limit_check_id, 01031 FixPoint1616_t *p_limit_check_value) 01032 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 01033 uint8_t enable_zero_value = 0; 01034 uint16_t temp16; 01035 FixPoint1616_t temp_fix1616; 01036 01037 switch (limit_check_id) { 01038 01039 case VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE: 01040 /* internal computation: */ 01041 temp_fix1616 = CurrentParameters .LimitChecksValue [VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE]; 01042 enable_zero_value = 0; 01043 break; 01044 01045 case VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE: 01046 status = VL53L0X_read_word(VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT, &temp16); 01047 if (status == VL53L0X_ERROR_NONE) { 01048 temp_fix1616 = VL53L0X_FP97TOFP1616(temp16); 01049 } 01050 enable_zero_value = 1; 01051 break; 01052 01053 case VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP: 01054 /* internal computation: */ 01055 temp_fix1616 = CurrentParameters .LimitChecksValue [VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP]; 01056 enable_zero_value = 0; 01057 break; 01058 01059 case VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD: 01060 /* internal computation: */ 01061 temp_fix1616 = CurrentParameters .LimitChecksValue [VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD]; 01062 enable_zero_value = 0; 01063 break; 01064 01065 case VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC: 01066 case VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE: 01067 status = VL53L0X_read_word(VL53L0X_REG_PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT, 01068 &temp16); 01069 if (status == VL53L0X_ERROR_NONE) { 01070 temp_fix1616 = VL53L0X_FP97TOFP1616(temp16); 01071 } 01072 01073 enable_zero_value = 0; 01074 break; 01075 01076 default: 01077 status = VL53L0X_ERROR_INVALID_PARAMS; 01078 01079 } 01080 01081 if (status == VL53L0X_ERROR_NONE) { 01082 if (enable_zero_value == 1) { 01083 01084 if (temp_fix1616 == 0) { 01085 /* disabled: return value from memory */ 01086 temp_fix1616 = CurrentParameters .LimitChecksValue [limit_check_id]; 01087 *p_limit_check_value = temp_fix1616; 01088 CurrentParameters .LimitChecksEnable [limit_check_id] = 0; 01089 } else { 01090 *p_limit_check_value = temp_fix1616; 01091 CurrentParameters .LimitChecksValue [limit_check_id] = temp_fix1616; 01092 CurrentParameters .LimitChecksEnable [limit_check_id] = 1; 01093 } 01094 } else { *p_limit_check_value = temp_fix1616; } 01095 } 01096 01097 return status; 01098 } 01099 01100 VL53L0X_Error VL53L0X::VL53L0X_get_limit_check_enable( uint16_t limit_check_id, 01101 uint8_t *p_limit_check_enable) 01102 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 01103 uint8_t temp8; 01104 01105 if (limit_check_id >= VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS) { 01106 status = VL53L0X_ERROR_INVALID_PARAMS; 01107 *p_limit_check_enable = 0; 01108 } else { 01109 temp8 = CurrentParameters .LimitChecksEnable [limit_check_id]; 01110 *p_limit_check_enable = temp8; 01111 } 01112 01113 return status; 01114 } 01115 01116 VL53L0X_Error VL53L0X::VL53L0X_get_wrap_around_check_enable(uint8_t *p_wrap_around_check_enable) 01117 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 01118 uint8_t data; 01119 01120 status = VL53L0X_read_byte( VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, &data); 01121 if (status == VL53L0X_ERROR_NONE) { 01122 Data.SequenceConfig = data; 01123 if (data & (0x01 << 7)) { 01124 *p_wrap_around_check_enable = 0x01; 01125 } else { 01126 *p_wrap_around_check_enable = 0x00; 01127 } 01128 } 01129 if (status == VL53L0X_ERROR_NONE) { 01130 CurrentParameters .WrapAroundCheckEnable = *p_wrap_around_check_enable; 01131 } 01132 01133 return status; 01134 } 01135 01136 VL53L0X_Error VL53L0X::sequence_step_enabled(VL53L0X_SequenceStepId sequence_step_id, uint8_t sequence_config, 01137 uint8_t *p_sequence_step_enabled) 01138 { VL53L0X_Error Status = VL53L0X_ERROR_NONE; 01139 *p_sequence_step_enabled = 0; 01140 01141 switch (sequence_step_id) { 01142 case VL53L0X_SEQUENCESTEP_TCC: 01143 *p_sequence_step_enabled = (sequence_config & 0x10) >> 4; 01144 break; 01145 case VL53L0X_SEQUENCESTEP_DSS: 01146 *p_sequence_step_enabled = (sequence_config & 0x08) >> 3; 01147 break; 01148 case VL53L0X_SEQUENCESTEP_MSRC: 01149 *p_sequence_step_enabled = (sequence_config & 0x04) >> 2; 01150 break; 01151 case VL53L0X_SEQUENCESTEP_PRE_RANGE: 01152 *p_sequence_step_enabled = (sequence_config & 0x40) >> 6; 01153 break; 01154 case VL53L0X_SEQUENCESTEP_FINAL_RANGE: 01155 *p_sequence_step_enabled = (sequence_config & 0x80) >> 7; 01156 break; 01157 default: 01158 Status = VL53L0X_ERROR_INVALID_PARAMS; 01159 } 01160 return Status; 01161 } 01162 01163 VL53L0X_Error VL53L0X::VL53L0X_get_sequence_step_enables(VL53L0X_SchedulerSequenceSteps_t *p_scheduler_sequence_steps) 01164 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 01165 uint8_t sequence_config = 0; 01166 01167 status = VL53L0X_read_byte( VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 01168 &sequence_config); 01169 01170 if (status == VL53L0X_ERROR_NONE) { 01171 status = sequence_step_enabled(VL53L0X_SEQUENCESTEP_TCC, sequence_config, 01172 &p_scheduler_sequence_steps->TccOn); 01173 } 01174 if (status == VL53L0X_ERROR_NONE) { 01175 status = sequence_step_enabled(VL53L0X_SEQUENCESTEP_DSS, sequence_config, 01176 &p_scheduler_sequence_steps->DssOn); 01177 } 01178 if (status == VL53L0X_ERROR_NONE) { 01179 status = sequence_step_enabled(VL53L0X_SEQUENCESTEP_MSRC, sequence_config, 01180 &p_scheduler_sequence_steps->MsrcOn); 01181 } 01182 if (status == VL53L0X_ERROR_NONE) { 01183 status = sequence_step_enabled(VL53L0X_SEQUENCESTEP_PRE_RANGE, sequence_config, 01184 &p_scheduler_sequence_steps->PreRangeOn); 01185 } 01186 if (status == VL53L0X_ERROR_NONE) { 01187 status = sequence_step_enabled(VL53L0X_SEQUENCESTEP_FINAL_RANGE, sequence_config, 01188 &p_scheduler_sequence_steps->FinalRangeOn); 01189 } 01190 return status; 01191 } 01192 01193 uint8_t VL53L0X::VL53L0X_decode_vcsel_period (uint8_t vcsel_period_reg) 01194 { /*! Converts the encoded VCSEL period register value into the real period in PLL clocks */ 01195 uint8_t vcsel_period_pclks = 0; 01196 01197 vcsel_period_pclks = (vcsel_period_reg + 1) << 1; 01198 01199 return vcsel_period_pclks; 01200 } 01201 01202 uint8_t VL53L0X::lv53l0x_encode_vcsel_period (uint8_t vcsel_period_pclks) 01203 { /*! Converts the encoded VCSEL period register value into the real period in PLL clocks */ 01204 01205 uint8_t vcsel_period_reg = 0; 01206 01207 vcsel_period_reg = (vcsel_period_pclks >> 1) - 1; 01208 01209 return vcsel_period_reg; 01210 } 01211 01212 VL53L0X_Error VL53L0X::wrapped_VL53L0X_set_vcsel_pulse_period(VL53L0X_VcselPeriod vcsel_period_type, uint8_t vcsel_pulse_period_pclk) 01213 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 01214 uint8_t vcsel_period_reg; 01215 uint8_t min_pre_vcsel_period_pclk = 12; 01216 uint8_t max_pre_vcsel_period_pclk = 18; 01217 uint8_t min_final_vcsel_period_pclk = 8; 01218 uint8_t max_final_vcsel_period_pclk = 14; 01219 uint32_t measurement_timing_budget_us; 01220 uint32_t final_range_timeout_us; 01221 uint32_t pre_range_timeout_us; 01222 uint32_t msrc_timeout_us; 01223 uint8_t phase_cal_int = 0; 01224 01225 /* Check if valid clock period requested */ 01226 01227 if ((vcsel_pulse_period_pclk % 2) != 0) { 01228 /* Value must be an even number */ 01229 status = VL53L0X_ERROR_INVALID_PARAMS; 01230 } else if (vcsel_period_type == VL53L0X_VCSEL_PERIOD_PRE_RANGE && 01231 (vcsel_pulse_period_pclk < min_pre_vcsel_period_pclk || 01232 vcsel_pulse_period_pclk > max_pre_vcsel_period_pclk)) { 01233 status = VL53L0X_ERROR_INVALID_PARAMS; 01234 } else if (vcsel_period_type == VL53L0X_VCSEL_PERIOD_FINAL_RANGE && 01235 (vcsel_pulse_period_pclk < min_final_vcsel_period_pclk || 01236 vcsel_pulse_period_pclk > max_final_vcsel_period_pclk)) { 01237 01238 status = VL53L0X_ERROR_INVALID_PARAMS; 01239 } 01240 01241 /* Apply specific settings for the requested clock period */ 01242 01243 if (status != VL53L0X_ERROR_NONE) { return status; } 01244 01245 if (vcsel_period_type == VL53L0X_VCSEL_PERIOD_PRE_RANGE) { 01246 01247 /* Set phase check limits */ 01248 if (vcsel_pulse_period_pclk == 12) { 01249 01250 status = VL53L0X_write_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 01251 0x18); 01252 status = VL53L0X_write_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW, 01253 0x08); 01254 } else if (vcsel_pulse_period_pclk == 14) { 01255 01256 status = VL53L0X_write_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 01257 0x30); 01258 status = VL53L0X_write_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW, 01259 0x08); 01260 } else if (vcsel_pulse_period_pclk == 16) { 01261 01262 status = VL53L0X_write_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 01263 0x40); 01264 status = VL53L0X_write_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW, 01265 0x08); 01266 } else if (vcsel_pulse_period_pclk == 18) { 01267 01268 status = VL53L0X_write_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 01269 0x50); 01270 status = VL53L0X_write_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW, 01271 0x08); 01272 } 01273 } else if (vcsel_period_type == VL53L0X_VCSEL_PERIOD_FINAL_RANGE) { 01274 if (vcsel_pulse_period_pclk == 8) { 01275 status = VL53L0X_write_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,0x10); 01276 status = VL53L0X_write_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,0x08); 01277 status |= VL53L0X_write_byte(VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x02); 01278 status |= VL53L0X_write_byte(VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x0C); 01279 status |= VL53L0X_write_byte( 0xff, 0x01); 01280 status |= VL53L0X_write_byte(VL53L0X_REG_ALGO_PHASECAL_LIM,0x30); 01281 status |= VL53L0X_write_byte( 0xff, 0x00); 01282 } else if (vcsel_pulse_period_pclk == 10) { 01283 status = VL53L0X_write_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,0x28); 01284 status = VL53L0X_write_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,0x08); 01285 status |= VL53L0X_write_byte(VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03); 01286 status |= VL53L0X_write_byte(VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x09); 01287 status |= VL53L0X_write_byte( 0xff, 0x01); 01288 status |= VL53L0X_write_byte(VL53L0X_REG_ALGO_PHASECAL_LIM,0x20); 01289 status |= VL53L0X_write_byte( 0xff, 0x00); 01290 } else if (vcsel_pulse_period_pclk == 12) { 01291 status = VL53L0X_write_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x38); 01292 status = VL53L0X_write_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08); 01293 status |= VL53L0X_write_byte(VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03); 01294 status |= VL53L0X_write_byte(VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x08); 01295 status |= VL53L0X_write_byte( 0xff, 0x01); 01296 status |= VL53L0X_write_byte(VL53L0X_REG_ALGO_PHASECAL_LIM,0x20); 01297 status |= VL53L0X_write_byte( 0xff, 0x00); 01298 } else if (vcsel_pulse_period_pclk == 14) { 01299 status = VL53L0X_write_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,0x048); 01300 status = VL53L0X_write_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,0x08); 01301 status |= VL53L0X_write_byte(VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03); 01302 status |= VL53L0X_write_byte(VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x07); 01303 status |= VL53L0X_write_byte( 0xff, 0x01); 01304 status |= VL53L0X_write_byte(VL53L0X_REG_ALGO_PHASECAL_LIM,0x20); 01305 status |= VL53L0X_write_byte( 0xff, 0x00); 01306 } 01307 } 01308 01309 /* Re-calculate and apply timeouts, in macro periods */ 01310 01311 if (status == VL53L0X_ERROR_NONE) { 01312 vcsel_period_reg = lv53l0x_encode_vcsel_period ((uint8_t) vcsel_pulse_period_pclk); 01313 01314 /* When the VCSEL period for the pre or final range is changed, 01315 * the corresponding timeout must be read from the device using 01316 * the current VCSEL period, then the new VCSEL period can be 01317 * applied. The timeout then must be written back to the device 01318 * using the new VCSEL period. 01319 * 01320 * For the MSRC timeout, the same applies - this timeout being 01321 * dependant on the pre-range vcsel period. 01322 */ 01323 switch (vcsel_period_type) { 01324 case VL53L0X_VCSEL_PERIOD_PRE_RANGE: 01325 status = get_sequence_step_timeout(VL53L0X_SEQUENCESTEP_PRE_RANGE, 01326 &pre_range_timeout_us); 01327 01328 if (status == VL53L0X_ERROR_NONE) 01329 status = get_sequence_step_timeout(VL53L0X_SEQUENCESTEP_MSRC, 01330 &msrc_timeout_us); 01331 01332 if (status == VL53L0X_ERROR_NONE) 01333 status = VL53L0X_write_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD, 01334 vcsel_period_reg); 01335 01336 if (status == VL53L0X_ERROR_NONE) 01337 status = set_sequence_step_timeout(VL53L0X_SEQUENCESTEP_PRE_RANGE, 01338 pre_range_timeout_us); 01339 01340 if (status == VL53L0X_ERROR_NONE) 01341 status = set_sequence_step_timeout(VL53L0X_SEQUENCESTEP_MSRC, 01342 msrc_timeout_us); 01343 01344 Data.PreRangeVcselPulsePeriod = vcsel_pulse_period_pclk; 01345 break; 01346 case VL53L0X_VCSEL_PERIOD_FINAL_RANGE: 01347 status = get_sequence_step_timeout(VL53L0X_SEQUENCESTEP_FINAL_RANGE, 01348 &final_range_timeout_us); 01349 01350 if (status == VL53L0X_ERROR_NONE) 01351 status = VL53L0X_write_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD, 01352 vcsel_period_reg); 01353 01354 if (status == VL53L0X_ERROR_NONE) 01355 status = set_sequence_step_timeout(VL53L0X_SEQUENCESTEP_FINAL_RANGE, 01356 final_range_timeout_us); 01357 01358 Data.FinalRangeVcselPulsePeriod = vcsel_pulse_period_pclk; 01359 break; 01360 default: 01361 status = VL53L0X_ERROR_INVALID_PARAMS; 01362 } 01363 } 01364 01365 /* Finally, the timing budget must be re-applied */ 01366 if (status == VL53L0X_ERROR_NONE) { 01367 measurement_timing_budget_us = CurrentParameters .MeasurementTimingBudget_us ; 01368 status = VL53L0X_set_measurement_timing_budget_us(measurement_timing_budget_us); 01369 } 01370 01371 /* Perform the phase calibration. This is needed after changing on 01372 * vcsel period. get_data_enable = 0, restore_config = 1 */ 01373 if (status == VL53L0X_ERROR_NONE) 01374 status = VL53L0X_perform_phase_calibration(&phase_cal_int, 0, 1); 01375 01376 return status; 01377 } 01378 01379 VL53L0X_Error VL53L0X::VL53L0X_set_vcsel_pulse_period(VL53L0X_VcselPeriod vcsel_period_type, uint8_t vcsel_pulse_period) 01380 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 01381 01382 status = wrapped_VL53L0X_set_vcsel_pulse_period( vcsel_period_type, vcsel_pulse_period); 01383 01384 return status; 01385 } 01386 01387 VL53L0X_Error VL53L0X::VL53L0X_get_vcsel_pulse_period(VL53L0X_VcselPeriod vcsel_period_type, uint8_t *p_vcsel_pulse_period_pclk) 01388 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 01389 uint8_t vcsel_period_reg; 01390 01391 switch (vcsel_period_type) { 01392 case VL53L0X_VCSEL_PERIOD_PRE_RANGE: 01393 status = VL53L0X_read_byte(VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD, 01394 &vcsel_period_reg); 01395 break; 01396 case VL53L0X_VCSEL_PERIOD_FINAL_RANGE: 01397 status = VL53L0X_read_byte(VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD, 01398 &vcsel_period_reg); 01399 break; 01400 default: 01401 status = VL53L0X_ERROR_INVALID_PARAMS; 01402 } 01403 01404 if (status == VL53L0X_ERROR_NONE) 01405 *p_vcsel_pulse_period_pclk = VL53L0X_decode_vcsel_period (vcsel_period_reg); 01406 01407 return status; 01408 } 01409 01410 uint32_t VL53L0X::VL53L0X_decode_timeout (uint16_t encoded_timeout) 01411 { /*! Decode 16-bit timeout register value - format (LSByte * 2^MSByte) + 1 */ 01412 uint32_t timeout_macro_clks = 0; 01413 01414 timeout_macro_clks = ((uint32_t)(encoded_timeout & 0x00FF) 01415 << (uint32_t)((encoded_timeout & 0xFF00) >> 8)) + 1; 01416 01417 return timeout_macro_clks; 01418 } 01419 01420 uint32_t VL53L0X::VL53L0X_calc_macro_period_ps( uint8_t vcsel_period_pclks) 01421 { uint64_t pll_period_ps; 01422 uint32_t macro_period_vclks; 01423 uint32_t macro_period_ps; 01424 01425 /* The above calculation will produce rounding errors, therefore set fixed value*/ 01426 pll_period_ps = 1655; 01427 macro_period_vclks = 2304; 01428 macro_period_ps = (uint32_t)(macro_period_vclks 01429 * vcsel_period_pclks * pll_period_ps); 01430 return macro_period_ps; 01431 } 01432 01433 /* To convert register value into us */ 01434 uint32_t VL53L0X::VL53L0X_calc_timeout_us(uint16_t timeout_period_mclks, 01435 uint8_t vcsel_period_pclks) 01436 { uint32_t macro_period_ps; 01437 uint32_t macro_period_ns; 01438 uint32_t actual_timeout_period_us = 0; 01439 01440 macro_period_ps = VL53L0X_calc_macro_period_ps( vcsel_period_pclks); 01441 macro_period_ns = (macro_period_ps + 500) / 1000; 01442 01443 actual_timeout_period_us = 01444 ((timeout_period_mclks * macro_period_ns) + 500) / 1000; 01445 01446 return actual_timeout_period_us; 01447 } 01448 01449 VL53L0X_Error VL53L0X::get_sequence_step_timeout(VL53L0X_SequenceStepId sequence_step_id, 01450 uint32_t *p_time_out_micro_secs) 01451 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 01452 uint8_t current_vcsel_pulse_period_p_clk; 01453 uint8_t encoded_time_out_byte = 0; 01454 uint32_t timeout_us = 0; 01455 uint16_t pre_range_encoded_time_out = 0; 01456 uint16_t msrc_time_out_m_clks; 01457 uint16_t pre_range_time_out_m_clks; 01458 uint16_t final_range_time_out_m_clks = 0; 01459 uint16_t final_range_encoded_time_out; 01460 VL53L0X_SchedulerSequenceSteps_t scheduler_sequence_steps; 01461 01462 if ((sequence_step_id == VL53L0X_SEQUENCESTEP_TCC) || 01463 (sequence_step_id == VL53L0X_SEQUENCESTEP_DSS) || 01464 (sequence_step_id == VL53L0X_SEQUENCESTEP_MSRC)) { 01465 01466 status = VL53L0X_get_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_PRE_RANGE, 01467 ¤t_vcsel_pulse_period_p_clk); 01468 if (status == VL53L0X_ERROR_NONE) { 01469 status = VL53L0X_read_byte(VL53L0X_REG_MSRC_CONFIG_TIMEOUT_MACROP, 01470 &encoded_time_out_byte); 01471 } 01472 msrc_time_out_m_clks = VL53L0X_decode_timeout (encoded_time_out_byte); 01473 01474 timeout_us = VL53L0X_calc_timeout_us(msrc_time_out_m_clks, 01475 current_vcsel_pulse_period_p_clk); 01476 } else if (sequence_step_id == VL53L0X_SEQUENCESTEP_PRE_RANGE) { 01477 /* Retrieve PRE-RANGE VCSEL Period */ 01478 status = VL53L0X_get_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_PRE_RANGE, 01479 ¤t_vcsel_pulse_period_p_clk); 01480 01481 /* Retrieve PRE-RANGE Timeout in Macro periods (MCLKS) */ 01482 if (status == VL53L0X_ERROR_NONE) { 01483 01484 /* Retrieve PRE-RANGE VCSEL Period */ 01485 status = VL53L0X_get_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_PRE_RANGE, 01486 ¤t_vcsel_pulse_period_p_clk); 01487 01488 if (status == VL53L0X_ERROR_NONE) { 01489 status = VL53L0X_read_word(VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI, 01490 &pre_range_encoded_time_out); 01491 } 01492 01493 pre_range_time_out_m_clks = VL53L0X_decode_timeout (pre_range_encoded_time_out); 01494 01495 timeout_us = VL53L0X_calc_timeout_us(pre_range_time_out_m_clks, 01496 current_vcsel_pulse_period_p_clk); 01497 } 01498 } else if (sequence_step_id == VL53L0X_SEQUENCESTEP_FINAL_RANGE) { 01499 01500 VL53L0X_get_sequence_step_enables( &scheduler_sequence_steps); 01501 pre_range_time_out_m_clks = 0; 01502 01503 if (scheduler_sequence_steps.PreRangeOn) { 01504 /* Retrieve PRE-RANGE VCSEL Period */ 01505 status = VL53L0X_get_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_PRE_RANGE, 01506 ¤t_vcsel_pulse_period_p_clk); 01507 01508 /* Retrieve PRE-RANGE Timeout in Macro periods 01509 * (MCLKS) */ 01510 if (status == VL53L0X_ERROR_NONE) { 01511 status = VL53L0X_read_word(VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI, 01512 &pre_range_encoded_time_out); 01513 pre_range_time_out_m_clks = VL53L0X_decode_timeout (pre_range_encoded_time_out); 01514 } 01515 } 01516 01517 if (status == VL53L0X_ERROR_NONE) { 01518 /* Retrieve FINAL-RANGE VCSEL Period */ 01519 status = VL53L0X_get_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_FINAL_RANGE, 01520 ¤t_vcsel_pulse_period_p_clk); 01521 } 01522 01523 /* Retrieve FINAL-RANGE Timeout in Macro periods (MCLKS) */ 01524 if (status == VL53L0X_ERROR_NONE) { 01525 status = VL53L0X_read_word(VL53L0X_REG_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI, 01526 &final_range_encoded_time_out); 01527 final_range_time_out_m_clks = VL53L0X_decode_timeout (final_range_encoded_time_out); 01528 } 01529 01530 final_range_time_out_m_clks -= pre_range_time_out_m_clks; 01531 timeout_us = VL53L0X_calc_timeout_us(final_range_time_out_m_clks, 01532 current_vcsel_pulse_period_p_clk); 01533 } 01534 01535 *p_time_out_micro_secs = timeout_us; 01536 01537 return status; 01538 } 01539 01540 VL53L0X_Error VL53L0X::VL53L0X_get_measurement_timing_budget_us(uint32_t *p_measurement_timing_budget_us) 01541 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 01542 VL53L0X_SchedulerSequenceSteps_t scheduler_sequence_steps; 01543 uint32_t final_range_timeout_us; 01544 uint32_t msrc_dcc_tcc_timeout_us = 2000; 01545 uint32_t start_overhead_us = 1910; 01546 uint32_t end_overhead_us = 960; 01547 uint32_t msrc_overhead_us = 660; 01548 uint32_t tcc_overhead_us = 590; 01549 uint32_t dss_overhead_us = 690; 01550 uint32_t pre_range_overhead_us = 660; 01551 uint32_t final_range_overhead_us = 550; 01552 uint32_t pre_range_timeout_us = 0; 01553 01554 /* Start and end overhead times always present */ 01555 *p_measurement_timing_budget_us 01556 = start_overhead_us + end_overhead_us; 01557 01558 status = VL53L0X_get_sequence_step_enables( &scheduler_sequence_steps); 01559 01560 if (status != VL53L0X_ERROR_NONE) { return status; } 01561 01562 if (scheduler_sequence_steps.TccOn || 01563 scheduler_sequence_steps.MsrcOn || 01564 scheduler_sequence_steps.DssOn) { 01565 01566 status = get_sequence_step_timeout(VL53L0X_SEQUENCESTEP_MSRC, 01567 &msrc_dcc_tcc_timeout_us); 01568 01569 if (status == VL53L0X_ERROR_NONE) { 01570 if (scheduler_sequence_steps.TccOn) { 01571 *p_measurement_timing_budget_us += 01572 msrc_dcc_tcc_timeout_us + tcc_overhead_us; 01573 } 01574 01575 if (scheduler_sequence_steps.DssOn) { 01576 *p_measurement_timing_budget_us += 01577 2 * (msrc_dcc_tcc_timeout_us + dss_overhead_us); 01578 } else if (scheduler_sequence_steps.MsrcOn) { 01579 *p_measurement_timing_budget_us += 01580 msrc_dcc_tcc_timeout_us + msrc_overhead_us; 01581 } 01582 } 01583 } 01584 01585 if (status == VL53L0X_ERROR_NONE) { 01586 if (scheduler_sequence_steps.PreRangeOn) { 01587 status = get_sequence_step_timeout(VL53L0X_SEQUENCESTEP_PRE_RANGE, 01588 &pre_range_timeout_us); 01589 *p_measurement_timing_budget_us += 01590 pre_range_timeout_us + pre_range_overhead_us; 01591 } 01592 } 01593 01594 if (status == VL53L0X_ERROR_NONE) { 01595 if (scheduler_sequence_steps.FinalRangeOn) { 01596 status = get_sequence_step_timeout(VL53L0X_SEQUENCESTEP_FINAL_RANGE, 01597 &final_range_timeout_us); 01598 *p_measurement_timing_budget_us += 01599 (final_range_timeout_us + final_range_overhead_us); 01600 } 01601 } 01602 01603 if (status == VL53L0X_ERROR_NONE) { 01604 CurrentParameters .MeasurementTimingBudget_us = *p_measurement_timing_budget_us;} 01605 01606 return status; 01607 } 01608 01609 VL53L0X_Error VL53L0X::VL53L0X_get_device_parameters(VL53L0X_DeviceParameters_t *p_device_parameters) 01610 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 01611 int i; 01612 01613 p_device_parameters->DeviceMode = CurrentParameters .DeviceMode ; 01614 01615 if (status == VL53L0X_ERROR_NONE) 01616 status = VL53L0X_get_inter_measurement_period_ms(&(p_device_parameters->InterMeasurementPeriod_ms )); 01617 01618 if (status == VL53L0X_ERROR_NONE) { 01619 p_device_parameters->XTalkCompensationEnable = 0; 01620 } 01621 01622 if (status == VL53L0X_ERROR_NONE) 01623 status = VL53L0X_get_x_talk_compensation_rate_MHz(&(p_device_parameters->XTalkCompensationRate_MHz )); 01624 01625 if (status == VL53L0X_ERROR_NONE) 01626 status = VL53L0X_get_offset_calibration_data_micro_meter(&(p_device_parameters->RangeOffset_um )); 01627 01628 if (status == VL53L0X_ERROR_NONE) { 01629 for (i = 0; i < VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS; i++) { 01630 /* get first the values, then the enables. 01631 * VL53L0X_GetLimitCheckValue will modify the enable 01632 * flags 01633 */ 01634 if (status == VL53L0X_ERROR_NONE) { 01635 status |= VL53L0X_get_limit_check_value( i, 01636 &(p_device_parameters->LimitChecksValue [i])); 01637 } else { 01638 break; 01639 } 01640 if (status == VL53L0X_ERROR_NONE) { 01641 status |= VL53L0X_get_limit_check_enable( i, 01642 &(p_device_parameters->LimitChecksEnable [i])); 01643 } else { 01644 break; 01645 } 01646 } 01647 } 01648 01649 if (status == VL53L0X_ERROR_NONE) { 01650 status = VL53L0X_get_wrap_around_check_enable(&(p_device_parameters->WrapAroundCheckEnable )); 01651 } 01652 01653 /* Need to be done at the end as it uses VCSELPulsePeriod */ 01654 if (status == VL53L0X_ERROR_NONE) { 01655 status = VL53L0X_get_measurement_timing_budget_us(&(p_device_parameters->MeasurementTimingBudget_us )); 01656 } 01657 01658 01659 return status; 01660 } 01661 01662 VL53L0X_Error VL53L0X::VL53L0X_set_limit_check_value( uint16_t limit_check_id, 01663 FixPoint1616_t limit_check_value) 01664 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 01665 uint8_t temp8; 01666 01667 temp8 = CurrentParameters .LimitChecksEnable [limit_check_id]; 01668 01669 if (temp8 == 0) { /* disabled write only internal value */ 01670 CurrentParameters .LimitChecksValue [limit_check_id] = limit_check_value; 01671 } else { 01672 01673 switch (limit_check_id) { 01674 01675 case VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE:/* internal computation: */ 01676 CurrentParameters .LimitChecksValue [VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE] = limit_check_value; 01677 break; 01678 01679 case VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE: 01680 status = VL53L0X_write_word(VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT, 01681 VL53L0X_FP1616TOFP97(limit_check_value)); 01682 break; 01683 01684 case VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP:/* internal computation: */ 01685 CurrentParameters .LimitChecksValue [VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP] = limit_check_value; 01686 break; 01687 01688 case VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD:/* internal computation: */ 01689 CurrentParameters .LimitChecksValue [VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD] = limit_check_value; 01690 break; 01691 01692 case VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC: 01693 case VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE: 01694 status = VL53L0X_write_word(VL53L0X_REG_PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT, 01695 VL53L0X_FP1616TOFP97(limit_check_value)); 01696 break; 01697 01698 default: 01699 status = VL53L0X_ERROR_INVALID_PARAMS; 01700 } 01701 01702 if (status == VL53L0X_ERROR_NONE) { 01703 CurrentParameters .LimitChecksValue [limit_check_id] = limit_check_value; 01704 } 01705 } 01706 return status; 01707 } 01708 01709 // instead of passing VL53L0X_DeviceInfo_t *p_VL53L0X_device_info, directly fill Device_Info 01710 VL53L0X_Error VL53L0X::VL53L0X_get_device_info() 01711 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 01712 uint8_t revision_id; 01713 uint8_t revision; 01714 char *product_id_tmp; 01715 01716 status = VL53L0X_get_info_from_device( 2); 01717 01718 if (status == VL53L0X_ERROR_NONE) { 01719 if (Data.ModuleId == 0) { 01720 revision = 0; 01721 VL53L0X_COPYSTRING(Device_Info.ProductId , ""); 01722 } else { 01723 revision = Data.Revision; 01724 product_id_tmp = Data.ProductId; 01725 VL53L0X_COPYSTRING(Device_Info.ProductId , product_id_tmp); 01726 } 01727 } 01728 01729 if (status == VL53L0X_ERROR_NONE) { 01730 if (revision == 0) { 01731 VL53L0X_COPYSTRING(Device_Info.Name , 01732 VL53L0X_STRING_DEVICE_INFO_NAME_TS0); 01733 } else if ((revision <= 34) && (revision != 32)) { 01734 VL53L0X_COPYSTRING(Device_Info.Name , 01735 VL53L0X_STRING_DEVICE_INFO_NAME_TS1); 01736 } else if (revision < 39) { 01737 VL53L0X_COPYSTRING(Device_Info.Name , 01738 VL53L0X_STRING_DEVICE_INFO_NAME_TS2); 01739 } else {VL53L0X_COPYSTRING(Device_Info.Name , 01740 VL53L0X_STRING_DEVICE_INFO_NAME_ES1); 01741 } 01742 01743 VL53L0X_COPYSTRING(Device_Info.Type , VL53L0X_STRING_DEVICE_INFO_TYPE); 01744 } 01745 01746 if (status == VL53L0X_ERROR_NONE) { 01747 status = VL53L0X_read_byte( VL53L0X_REG_IDENTIFICATION_MODEL_ID, 01748 &Device_Info.ProductType );} 01749 01750 if (status == VL53L0X_ERROR_NONE) { 01751 status = VL53L0X_read_byte(VL53L0X_REG_IDENTIFICATION_REVISION_ID, 01752 &revision_id); 01753 Device_Info.ProductRevisionMajor = 1; 01754 Device_Info.ProductRevisionMinor = 01755 (revision_id & 0xF0) >> 4; 01756 } 01757 return status; 01758 } 01759 01760 VL53L0X_Error VL53L0X::VL53L0X_get_interrupt_mask_status(uint32_t *p_interrupt_mask_status) 01761 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 01762 uint8_t byte; 01763 01764 status = VL53L0X_read_byte( VL53L0X_REG_RESULT_INTERRUPT_STATUS, &byte); 01765 *p_interrupt_mask_status = byte & 0x07; 01766 01767 if (byte & 0x18) { status = VL53L0X_ERROR_RANGE_ERROR;} 01768 01769 return status; 01770 } 01771 01772 VL53L0X_Error VL53L0X::VL53L0X_get_measurement_data_ready(uint8_t *p_measurement_data_ready) 01773 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 01774 uint8_t sys_range_status_register; 01775 uint8_t interrupt_config; 01776 uint32_t interrupt_mask; 01777 01778 interrupt_config = Data.Pin0GpioFunctionality; 01779 01780 if (interrupt_config == 01781 VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY) { 01782 status = VL53L0X_get_interrupt_mask_status( &interrupt_mask); 01783 if (interrupt_mask == 01784 VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY) { 01785 *p_measurement_data_ready = 1; 01786 } else { 01787 *p_measurement_data_ready = 0; 01788 } 01789 } else { 01790 status = VL53L0X_read_byte( VL53L0X_REG_RESULT_RANGE_STATUS, 01791 &sys_range_status_register); 01792 if (status == VL53L0X_ERROR_NONE) { 01793 if (sys_range_status_register & 0x01) { 01794 *p_measurement_data_ready = 1; 01795 } else { *p_measurement_data_ready = 0; } 01796 } 01797 } 01798 01799 return status; 01800 } 01801 01802 VL53L0X_Error VL53L0X::VL53L0X_polling_delay(void) 01803 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 01804 01805 // do nothing 01806 VL53L0X_OsDelay(); 01807 return status; 01808 } 01809 01810 VL53L0X_Error VL53L0X::VL53L0X_measurement_poll_for_completion(void) 01811 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 01812 uint8_t new_data_ready = 0; 01813 uint32_t loop_nb; 01814 01815 loop_nb = 0; 01816 01817 do { 01818 status = VL53L0X_get_measurement_data_ready( &new_data_ready); 01819 if (status != 0) { 01820 break; /* the error is set */ 01821 } 01822 01823 if (new_data_ready == 1) { 01824 break; /* done note that status == 0 */ 01825 } 01826 01827 loop_nb++; 01828 if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP) { 01829 status = VL53L0X_ERROR_TIME_OUT; 01830 break; 01831 } 01832 01833 VL53L0X_polling_delay(); 01834 } while (1); 01835 01836 return status; 01837 } 01838 01839 /* Group PAL Interrupt Functions */ 01840 VL53L0X_Error VL53L0X::VL53L0X_clear_interrupt_mask( uint32_t interrupt_mask) 01841 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 01842 uint8_t loop_count; 01843 uint8_t byte; 01844 01845 /* clear bit 0 range interrupt, bit 1 error interrupt */ 01846 loop_count = 0; 01847 do { 01848 status = VL53L0X_write_byte(VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR, 0x01); 01849 status |= VL53L0X_write_byte(VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR, 0x00); 01850 status |= VL53L0X_read_byte(VL53L0X_REG_RESULT_INTERRUPT_STATUS, &byte); 01851 loop_count++; 01852 } while (((byte & 0x07) != 0x00) 01853 && (loop_count < 3) 01854 && (status == VL53L0X_ERROR_NONE)); 01855 01856 if (loop_count >= 3) { 01857 status = VL53L0X_ERROR_INTERRUPT_NOT_CLEARED; 01858 } 01859 01860 01861 return status; 01862 } 01863 01864 VL53L0X_Error VL53L0X::VL53L0X_perform_single_ref_calibration(uint8_t vhv_init_byte) 01865 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 01866 01867 if (status == VL53L0X_ERROR_NONE) { 01868 status = VL53L0X_write_byte( VL53L0X_REG_SYSRANGE_START, 01869 VL53L0X_REG_SYSRANGE_MODE_START_STOP | 01870 vhv_init_byte); 01871 } 01872 01873 if (status == VL53L0X_ERROR_NONE) { 01874 status = VL53L0X_measurement_poll_for_completion();} 01875 01876 if (status == VL53L0X_ERROR_NONE) { 01877 status = VL53L0X_clear_interrupt_mask( 0);} 01878 01879 if (status == VL53L0X_ERROR_NONE) { 01880 status = VL53L0X_write_byte( VL53L0X_REG_SYSRANGE_START, 0x00); 01881 } 01882 01883 return status; 01884 } 01885 01886 VL53L0X_Error VL53L0X::VL53L0X_ref_calibration_io( uint8_t read_not_write, 01887 uint8_t vhv_settings, uint8_t phase_cal, 01888 uint8_t *p_vhv_settings, uint8_t *p_phase_cal, 01889 const uint8_t vhv_enable, const uint8_t phase_enable) 01890 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 01891 uint8_t phase_calint = 0; 01892 01893 /* Read VHV from device */ 01894 status |= VL53L0X_write_byte( 0xFF, 0x01); 01895 status |= VL53L0X_write_byte( 0x00, 0x00); 01896 status |= VL53L0X_write_byte( 0xFF, 0x00); 01897 01898 if (read_not_write) { 01899 if (vhv_enable) { 01900 status |= VL53L0X_read_byte( 0xCB, p_vhv_settings);} 01901 if (phase_enable) { 01902 status |= VL53L0X_read_byte( 0xEE, &phase_calint);} 01903 } else { 01904 if (vhv_enable) { 01905 status |= VL53L0X_write_byte( 0xCB, vhv_settings);} 01906 if (phase_enable) { 01907 status |= VL53L0X_update_byte( 0xEE, 0x80, phase_cal);} 01908 } 01909 01910 status |= VL53L0X_write_byte( 0xFF, 0x01); 01911 status |= VL53L0X_write_byte( 0x00, 0x01); 01912 status |= VL53L0X_write_byte( 0xFF, 0x00); 01913 01914 *p_phase_cal = (uint8_t)(phase_calint & 0xEF); 01915 01916 return status; 01917 } 01918 01919 VL53L0X_Error VL53L0X::VL53L0X_perform_vhv_calibration(uint8_t *p_vhv_settings, const uint8_t get_data_enable, 01920 const uint8_t restore_config) 01921 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 01922 uint8_t sequence_config = 0; 01923 uint8_t vhv_settings = 0; 01924 uint8_t phase_cal = 0; 01925 uint8_t phase_cal_int = 0; 01926 01927 /* store the value of the sequence config, 01928 * this will be reset before the end of the function 01929 */ 01930 01931 if (restore_config) { 01932 sequence_config = Data.SequenceConfig; 01933 } 01934 01935 /* Run VHV */ 01936 status = VL53L0X_write_byte( VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0x01); 01937 01938 if (status == VL53L0X_ERROR_NONE) { 01939 status = VL53L0X_perform_single_ref_calibration( 0x40);} 01940 01941 /* Read VHV from device */ 01942 if ((status == VL53L0X_ERROR_NONE) && (get_data_enable == 1)) { 01943 status = VL53L0X_ref_calibration_io( 1, vhv_settings, phase_cal, /* Not used here */ 01944 p_vhv_settings, &phase_cal_int, 1, 0); 01945 } else {*p_vhv_settings = 0; } 01946 01947 if ((status == VL53L0X_ERROR_NONE) && restore_config) { 01948 /* restore the previous Sequence Config */ 01949 status = VL53L0X_write_byte( VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 01950 sequence_config); 01951 if (status == VL53L0X_ERROR_NONE) { 01952 Data.SequenceConfig = sequence_config; } 01953 } 01954 01955 return status; 01956 } 01957 01958 VL53L0X_Error VL53L0X::VL53L0X_perform_phase_calibration(uint8_t *p_phase_cal, const uint8_t get_data_enable, 01959 const uint8_t restore_config) 01960 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 01961 uint8_t sequence_config = 0; 01962 uint8_t vhv_settings = 0; 01963 uint8_t phase_cal = 0; 01964 uint8_t vhv_settingsint; 01965 01966 /* store the value of the sequence config, 01967 * this will be reset before the end of the function */ 01968 01969 if (restore_config) { 01970 sequence_config = Data.SequenceConfig; 01971 } 01972 01973 /* Run PhaseCal */ 01974 status = VL53L0X_write_byte( VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0x02); 01975 01976 if (status == VL53L0X_ERROR_NONE) { 01977 status = VL53L0X_perform_single_ref_calibration( 0x0); 01978 } 01979 01980 /* Read PhaseCal from device */ 01981 if ((status == VL53L0X_ERROR_NONE) && (get_data_enable == 1)) { 01982 status = VL53L0X_ref_calibration_io( 1, 01983 vhv_settings, phase_cal, /* Not used here */ 01984 &vhv_settingsint, p_phase_cal, 01985 0, 1); 01986 } else { 01987 *p_phase_cal = 0; 01988 } 01989 01990 if ((status == VL53L0X_ERROR_NONE) && restore_config) { 01991 /* restore the previous Sequence Config */ 01992 status = VL53L0X_write_byte( VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 01993 sequence_config); 01994 if (status == VL53L0X_ERROR_NONE) { 01995 Data.SequenceConfig = sequence_config; 01996 } 01997 01998 } 01999 02000 return status; 02001 } 02002 02003 VL53L0X_Error VL53L0X::VL53L0X_perform_ref_calibration(uint8_t *p_vhv_settings, uint8_t *p_phase_cal, uint8_t get_data_enable) 02004 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 02005 uint8_t sequence_config = 0; 02006 02007 /* store the value of the sequence config, 02008 * this will be reset before the end of the function */ 02009 02010 sequence_config = Data.SequenceConfig; 02011 02012 /* In the following function we don't save the config to optimize 02013 * writes on device. Config is saved and restored only once. */ 02014 status = VL53L0X_perform_vhv_calibration(p_vhv_settings, get_data_enable, 0); 02015 02016 if (status == VL53L0X_ERROR_NONE) { 02017 status = VL53L0X_perform_phase_calibration(p_phase_cal, get_data_enable, 0); } 02018 02019 if (status == VL53L0X_ERROR_NONE) { 02020 /* restore the previous Sequence Config */ 02021 status = VL53L0X_write_byte( VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 02022 sequence_config); 02023 if (status == VL53L0X_ERROR_NONE) { 02024 Data.SequenceConfig = sequence_config; } 02025 } 02026 02027 return status; 02028 } 02029 02030 void VL53L0X::get_next_good_spad(uint8_t good_spad_array[], uint32_t size, 02031 uint32_t curr, int32_t *p_next) 02032 { uint32_t start_index; 02033 uint32_t fine_offset; 02034 uint32_t c_spads_per_byte = 8; 02035 uint32_t coarse_index; 02036 uint32_t fine_index; 02037 uint8_t data_byte; 02038 uint8_t success = 0; 02039 02040 /* Starting with the current good spad, loop through the array to find 02041 * the next. i.e. the next bit set in the sequence. 02042 * 02043 * The coarse index is the byte index of the array and the fine index is 02044 * the index of the bit within each byte. */ 02045 02046 *p_next = -1; 02047 02048 start_index = curr / c_spads_per_byte; 02049 fine_offset = curr % c_spads_per_byte; 02050 02051 for (coarse_index = start_index; ((coarse_index < size) && !success); 02052 coarse_index++) { 02053 fine_index = 0; 02054 data_byte = good_spad_array[coarse_index]; 02055 02056 if (coarse_index == start_index) { 02057 /* locate the bit position of the provided current 02058 * spad bit before iterating */ 02059 data_byte >>= fine_offset; 02060 fine_index = fine_offset; 02061 } 02062 02063 while (fine_index < c_spads_per_byte) { 02064 if ((data_byte & 0x1) == 1) { 02065 success = 1; 02066 *p_next = coarse_index * c_spads_per_byte + fine_index; 02067 break; 02068 } 02069 data_byte >>= 1; 02070 fine_index++; 02071 } 02072 } 02073 } 02074 02075 uint8_t VL53L0X::is_aperture(uint32_t spad_index) 02076 { /* This function reports if a given spad index is an aperture SPAD by 02077 * deriving the quadrant.*/ 02078 uint32_t quadrant; 02079 uint8_t is_aperture = 1; 02080 quadrant = spad_index >> 6; 02081 if (refArrayQuadrants[quadrant] == REF_ARRAY_SPAD_0) { 02082 is_aperture = 0; 02083 } 02084 02085 return is_aperture; 02086 } 02087 02088 VL53L0X_Error VL53L0X::enable_spad_bit(uint8_t spad_array[], uint32_t size, 02089 uint32_t spad_index) 02090 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 02091 uint32_t c_spads_per_byte = 8; 02092 uint32_t coarse_index; 02093 uint32_t fine_index; 02094 02095 coarse_index = spad_index / c_spads_per_byte; 02096 fine_index = spad_index % c_spads_per_byte; 02097 if (coarse_index >= size) { 02098 status = VL53L0X_ERROR_REF_SPAD_INIT; 02099 } else { 02100 spad_array[coarse_index] |= (1 << fine_index); 02101 } 02102 02103 return status; 02104 } 02105 02106 VL53L0X_Error VL53L0X::set_ref_spad_map( uint8_t *p_ref_spad_array) 02107 { VL53L0X_Error status = VL53L0X_i2c_write(VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0, 02108 p_ref_spad_array, 6); 02109 02110 return status; 02111 } 02112 02113 VL53L0X_Error VL53L0X::get_ref_spad_map( uint8_t *p_ref_spad_array) 02114 { VL53L0X_Error status = VL53L0X_read_multi(VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0, 02115 p_ref_spad_array, 02116 6); 02117 // VL53L0X_Error status = VL53L0X_ERROR_NONE; 02118 // uint8_t count=0; 02119 02120 // for (count = 0; count < 6; count++) 02121 // status = VL53L0X_RdByte( (VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0 + count), &refSpadArray[count]); 02122 return status; 02123 } 02124 02125 VL53L0X_Error VL53L0X::enable_ref_spads(uint8_t aperture_spads, 02126 uint8_t good_spad_array[], 02127 uint8_t spad_array[], 02128 uint32_t size, 02129 uint32_t start, 02130 uint32_t offset, 02131 uint32_t spad_count, 02132 uint32_t *p_last_spad) 02133 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 02134 uint32_t index; 02135 uint32_t i; 02136 int32_t next_good_spad = offset; 02137 uint32_t current_spad; 02138 uint8_t check_spad_array[6]; 02139 02140 /* 02141 * This function takes in a spad array which may or may not have SPADS 02142 * already enabled and appends from a given offset a requested number 02143 * of new SPAD enables. The 'good spad map' is applied to 02144 * determine the next SPADs to enable. 02145 * 02146 * This function applies to only aperture or only non-aperture spads. 02147 * Checks are performed to ensure this. 02148 */ 02149 02150 current_spad = offset; 02151 for (index = 0; index < spad_count; index++) { 02152 get_next_good_spad(good_spad_array, size, current_spad, 02153 &next_good_spad); 02154 02155 if (next_good_spad == -1) { 02156 status = VL53L0X_ERROR_REF_SPAD_INIT; 02157 break; 02158 } 02159 02160 /* Confirm that the next good SPAD is non-aperture */ 02161 if (is_aperture(start + next_good_spad) != aperture_spads) { 02162 /* if we can't get the required number of good aperture 02163 * spads from the current quadrant then this is an error 02164 */ 02165 status = VL53L0X_ERROR_REF_SPAD_INIT; 02166 break; 02167 } 02168 current_spad = (uint32_t)next_good_spad; 02169 enable_spad_bit(spad_array, size, current_spad); 02170 current_spad++; 02171 } 02172 *p_last_spad = current_spad; 02173 02174 if (status == VL53L0X_ERROR_NONE) { 02175 status = set_ref_spad_map( spad_array); 02176 } 02177 02178 if (status == VL53L0X_ERROR_NONE) { 02179 status = get_ref_spad_map( check_spad_array); 02180 02181 i = 0; 02182 02183 /* Compare spad maps. If not equal report error. */ 02184 while (i < size) { 02185 if (spad_array[i] != check_spad_array[i]) { 02186 status = VL53L0X_ERROR_REF_SPAD_INIT; 02187 break; 02188 } 02189 i++; 02190 } 02191 } 02192 return status; 02193 } 02194 02195 VL53L0X_Error VL53L0X::VL53L0X_set_device_mode( VL53L0X_DeviceModes device_mode) 02196 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 02197 02198 switch (device_mode) { 02199 case VL53L0X_DEVICEMODE_SINGLE_RANGING: 02200 case VL53L0X_DEVICEMODE_CONTINUOUS_RANGING: 02201 case VL53L0X_DEVICEMODE_CONTINUOUS_TIMED_RANGING: 02202 case VL53L0X_DEVICEMODE_GPIO_DRIVE: 02203 case VL53L0X_DEVICEMODE_GPIO_OSC: 02204 /* Supported modes */ 02205 CurrentParameters .DeviceMode = device_mode; 02206 break; 02207 default: 02208 /* Unsupported mode */ 02209 status = VL53L0X_ERROR_MODE_NOT_SUPPORTED; 02210 } 02211 02212 02213 return status; 02214 } 02215 02216 VL53L0X_Error VL53L0X::VL53L0X_set_interrupt_thresholds(VL53L0X_DeviceModes device_mode, FixPoint1616_t threshold_low, 02217 FixPoint1616_t threshold_high) 02218 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 02219 uint16_t threshold16; 02220 02221 02222 /* no dependency on DeviceMode for Ewok */ 02223 /* Need to divide by 2 because the FW will apply a x2 */ 02224 threshold16 = (uint16_t)((threshold_low >> 17) & 0x00fff); 02225 status = VL53L0X_write_word( VL53L0X_REG_SYSTEM_THRESH_LOW, threshold16); 02226 02227 if (status == VL53L0X_ERROR_NONE) { 02228 /* Need to divide by 2 because the FW will apply a x2 */ 02229 threshold16 = (uint16_t)((threshold_high >> 17) & 0x00fff); 02230 status = VL53L0X_write_word( VL53L0X_REG_SYSTEM_THRESH_HIGH, 02231 threshold16); 02232 } 02233 02234 02235 return status; 02236 } 02237 02238 VL53L0X_Error VL53L0X::VL53L0X_get_interrupt_thresholds(VL53L0X_DeviceModes device_mode, FixPoint1616_t *p_threshold_low, 02239 FixPoint1616_t *p_threshold_high) 02240 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 02241 uint16_t threshold16; 02242 02243 02244 /* no dependency on DeviceMode for Ewok */ 02245 02246 status = VL53L0X_read_word( VL53L0X_REG_SYSTEM_THRESH_LOW, &threshold16); 02247 /* Need to multiply by 2 because the FW will apply a x2 */ 02248 *p_threshold_low = (FixPoint1616_t)((0x00fff & threshold16) << 17); 02249 02250 if (status == VL53L0X_ERROR_NONE) { 02251 status = VL53L0X_read_word( VL53L0X_REG_SYSTEM_THRESH_HIGH, 02252 &threshold16); 02253 /* Need to multiply by 2 because the FW will apply a x2 */ 02254 *p_threshold_high = 02255 (FixPoint1616_t)((0x00fff & threshold16) << 17); 02256 } 02257 02258 02259 return status; 02260 } 02261 02262 VL53L0X_Error VL53L0X::VL53L0X_load_tuning_settings(uint8_t *p_tuning_setting_buffer) 02263 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 02264 int i; 02265 int index; 02266 uint8_t msb; 02267 uint8_t lsb; 02268 uint8_t select_param; 02269 uint16_t number_of_writes; 02270 uint8_t address; 02271 uint8_t local_buffer[4]; /* max */ 02272 uint16_t temp16; 02273 02274 index = 0; 02275 02276 while ((*(p_tuning_setting_buffer + index) != 0) && 02277 (status == VL53L0X_ERROR_NONE)) { 02278 number_of_writes = *(p_tuning_setting_buffer + index); 02279 index++; 02280 if (number_of_writes == 0xFF) { 02281 /* internal parameters */ 02282 select_param = *(p_tuning_setting_buffer + index); 02283 index++; 02284 switch (select_param) { 02285 case 0: /* uint16_t SigmaEstRefArray -> 2 bytes */ 02286 msb = *(p_tuning_setting_buffer + index); 02287 index++; 02288 lsb = *(p_tuning_setting_buffer + index); 02289 index++; 02290 temp16 = VL53L0X_MAKEUINT16(lsb, msb); 02291 Data.SigmaEstRefArray = temp16; 02292 break; 02293 case 1: /* uint16_t SigmaEstEffPulseWidth -> 2 bytes */ 02294 msb = *(p_tuning_setting_buffer + index); 02295 index++; 02296 lsb = *(p_tuning_setting_buffer + index); 02297 index++; 02298 temp16 = VL53L0X_MAKEUINT16(lsb, msb); 02299 Data.SigmaEstEffPulseWidth = temp16; 02300 break; 02301 case 2: /* uint16_t SigmaEstEffAmbWidth -> 2 bytes */ 02302 msb = *(p_tuning_setting_buffer + index); 02303 index++; 02304 lsb = *(p_tuning_setting_buffer + index); 02305 index++; 02306 temp16 = VL53L0X_MAKEUINT16(lsb, msb); 02307 Data.SigmaEstEffAmbWidth = temp16; 02308 break; 02309 case 3: /* uint16_t targetRefRate -> 2 bytes */ 02310 msb = *(p_tuning_setting_buffer + index); 02311 index++; 02312 lsb = *(p_tuning_setting_buffer + index); 02313 index++; 02314 temp16 = VL53L0X_MAKEUINT16(lsb, msb); 02315 Data.targetRefRate = temp16; 02316 break; 02317 default: /* invalid parameter */ 02318 status = VL53L0X_ERROR_INVALID_PARAMS; 02319 } 02320 02321 } else if (number_of_writes <= 4) { 02322 address = *(p_tuning_setting_buffer + index); 02323 index++; 02324 02325 for (i = 0; i < number_of_writes; i++) { 02326 local_buffer[i] = *(p_tuning_setting_buffer + 02327 index); 02328 index++; 02329 } 02330 02331 status = VL53L0X_i2c_write( address, local_buffer, number_of_writes); 02332 02333 } else { status = VL53L0X_ERROR_INVALID_PARAMS; } 02334 } 02335 return status; 02336 } 02337 02338 VL53L0X_Error VL53L0X::VL53L0X_check_and_load_interrupt_settings(uint8_t start_not_stopflag) 02339 { uint8_t interrupt_config; 02340 FixPoint1616_t threshold_low; 02341 FixPoint1616_t threshold_high; 02342 VL53L0X_Error status = VL53L0X_ERROR_NONE; 02343 02344 interrupt_config = Data.Pin0GpioFunctionality; 02345 02346 if ((interrupt_config == 02347 VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_LOW) || 02348 (interrupt_config == 02349 VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_HIGH) || 02350 (interrupt_config == 02351 VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_OUT)) { 02352 02353 status = VL53L0X_get_interrupt_thresholds(VL53L0X_DEVICEMODE_CONTINUOUS_RANGING, 02354 &threshold_low, &threshold_high); 02355 02356 if (((threshold_low > 255 * 65536) || 02357 (threshold_high > 255 * 65536)) && 02358 (status == VL53L0X_ERROR_NONE)) { 02359 02360 if (start_not_stopflag != 0) { 02361 status = VL53L0X_load_tuning_settings(InterruptThresholdSettings); 02362 } else { 02363 status |= VL53L0X_write_byte( 0xFF, 0x04); 02364 status |= VL53L0X_write_byte( 0x70, 0x00); 02365 status |= VL53L0X_write_byte( 0xFF, 0x00); 02366 status |= VL53L0X_write_byte( 0x80, 0x00); 02367 } 02368 } 02369 } 02370 return status; 02371 } 02372 02373 VL53L0X_Error VL53L0X::VL53L0X_start_measurement(void) 02374 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 02375 VL53L0X_DeviceModes device_mode; 02376 uint8_t byte; 02377 uint8_t start_stop_byte = VL53L0X_REG_SYSRANGE_MODE_START_STOP; 02378 uint32_t loop_nb; 02379 02380 02381 /* Get Current DeviceMode */ 02382 device_mode = CurrentParameters .DeviceMode ; 02383 02384 status = VL53L0X_write_byte( 0x80, 0x01); 02385 status = VL53L0X_write_byte( 0xFF, 0x01); 02386 status = VL53L0X_write_byte( 0x00, 0x00); 02387 status = VL53L0X_write_byte( 0x91, Data.StopVariable); 02388 status = VL53L0X_write_byte( 0x00, 0x01); 02389 status = VL53L0X_write_byte( 0xFF, 0x00); 02390 status = VL53L0X_write_byte( 0x80, 0x00); 02391 02392 switch (device_mode) { 02393 case VL53L0X_DEVICEMODE_SINGLE_RANGING: 02394 status = VL53L0X_write_byte( VL53L0X_REG_SYSRANGE_START, 0x01); 02395 02396 byte = start_stop_byte; 02397 if (status == VL53L0X_ERROR_NONE) { 02398 /* Wait until start bit has been cleared */ 02399 loop_nb = 0; 02400 do { 02401 if (loop_nb > 0) 02402 status = VL53L0X_read_byte(VL53L0X_REG_SYSRANGE_START, &byte); 02403 loop_nb = loop_nb + 1; 02404 } while (((byte & start_stop_byte) == start_stop_byte) 02405 && (status == VL53L0X_ERROR_NONE) 02406 && (loop_nb < VL53L0X_DEFAULT_MAX_LOOP)); 02407 02408 if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP) { 02409 status = VL53L0X_ERROR_TIME_OUT; 02410 } 02411 } 02412 02413 break; 02414 case VL53L0X_DEVICEMODE_CONTINUOUS_RANGING: /* Back-to-back mode */ 02415 02416 /* Check if need to apply interrupt settings */ 02417 if (status == VL53L0X_ERROR_NONE) 02418 { status = VL53L0X_check_and_load_interrupt_settings( 1); } 02419 02420 status = VL53L0X_write_byte(VL53L0X_REG_SYSRANGE_START, 02421 VL53L0X_REG_SYSRANGE_MODE_BACKTOBACK); 02422 if (status == VL53L0X_ERROR_NONE) { 02423 /* Set PAL State to Running */ 02424 Data.PalState = VL53L0X_STATE_RUNNING; 02425 } 02426 break; 02427 case VL53L0X_DEVICEMODE_CONTINUOUS_TIMED_RANGING: 02428 /* Continuous mode */ 02429 /* Check if need to apply interrupt settings */ 02430 if (status == VL53L0X_ERROR_NONE) { 02431 status = VL53L0X_check_and_load_interrupt_settings( 1); 02432 } 02433 02434 status = VL53L0X_write_byte(VL53L0X_REG_SYSRANGE_START, 02435 VL53L0X_REG_SYSRANGE_MODE_TIMED); 02436 02437 if (status == VL53L0X_ERROR_NONE)/* Set PAL State to Running */ 02438 { Data.PalState = VL53L0X_STATE_RUNNING; } 02439 break; 02440 default: 02441 /* Selected mode not supported */ 02442 status = VL53L0X_ERROR_MODE_NOT_SUPPORTED; 02443 } 02444 02445 return status; 02446 } 02447 02448 /* Group PAL Measurement Functions */ 02449 VL53L0X_Error VL53L0X::VL53L0X_perform_single_measurement(void) 02450 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 02451 VL53L0X_DeviceModes device_mode; 02452 02453 /* Get Current DeviceMode */ 02454 device_mode = CurrentParameters .DeviceMode ; 02455 02456 /* Start immediately to run a single ranging measurement in case of 02457 * single ranging or single histogram */ 02458 if (status == VL53L0X_ERROR_NONE && device_mode == VL53L0X_DEVICEMODE_SINGLE_RANGING) { 02459 status = VL53L0X_start_measurement(); } 02460 02461 if (status == VL53L0X_ERROR_NONE) { 02462 status = VL53L0X_measurement_poll_for_completion(); } 02463 02464 /* Change PAL State in case of single ranging or single histogram */ 02465 if (status == VL53L0X_ERROR_NONE && device_mode == VL53L0X_DEVICEMODE_SINGLE_RANGING) { 02466 Data.PalState = VL53L0X_STATE_IDLE; } 02467 02468 return status; 02469 } 02470 02471 VL53L0X_Error VL53L0X::VL53L0X_get_x_talk_compensation_enable(uint8_t *p_x_talk_compensation_enable) 02472 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 02473 uint8_t temp8; 02474 02475 temp8 = CurrentParameters .XTalkCompensationEnable ; 02476 *p_x_talk_compensation_enable = temp8; 02477 02478 return status; 02479 } 02480 02481 VL53L0X_Error VL53L0X::VL53L0X_get_total_xtalk_rate(VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data, 02482 FixPoint1616_t *p_total_xtalk_rate_MHz) 02483 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 02484 02485 uint8_t xtalk_comp_enable; 02486 FixPoint1616_t total_xtalk_MHz; 02487 FixPoint1616_t xtalk_per_spad_MHz; 02488 02489 *p_total_xtalk_rate_MHz = 0; 02490 02491 status = VL53L0X_get_x_talk_compensation_enable( &xtalk_comp_enable); 02492 if (status == VL53L0X_ERROR_NONE) { 02493 02494 if (xtalk_comp_enable) { 02495 02496 xtalk_per_spad_MHz = CurrentParameters .XTalkCompensationRate_MHz ; 02497 02498 /* FixPoint1616 * FixPoint 8:8 = FixPoint0824 */ 02499 total_xtalk_MHz = 02500 p_ranging_measurement_data->EffectiveSpadRtnCount * 02501 xtalk_per_spad_MHz; 02502 02503 /* FixPoint0824 >> 8 = FixPoint1616 */ 02504 *p_total_xtalk_rate_MHz = 02505 (total_xtalk_MHz + 0x80) >> 8; 02506 } 02507 } 02508 return status; 02509 } 02510 02511 VL53L0X_Error VL53L0X::VL53L0X_get_total_signal_rate(VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data, 02512 FixPoint1616_t *p_total_signal_rate_MHz) 02513 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 02514 FixPoint1616_t total_xtalk_MHz; 02515 02516 *p_total_signal_rate_MHz = 02517 p_ranging_measurement_data->SignalRateRtn_MHz; 02518 02519 status = VL53L0X_get_total_xtalk_rate(p_ranging_measurement_data, &total_xtalk_MHz); 02520 02521 if (status == VL53L0X_ERROR_NONE) { 02522 *p_total_signal_rate_MHz += total_xtalk_MHz; } 02523 02524 return status; 02525 } 02526 02527 /* To convert ms into register value */ 02528 uint32_t VL53L0X::VL53L0X_calc_timeout_mclks(uint32_t timeout_period_us, 02529 uint8_t vcsel_period_pclks) 02530 { uint32_t macro_period_ps; 02531 uint32_t macro_period_ns; 02532 uint32_t timeout_period_mclks = 0; 02533 02534 macro_period_ps = VL53L0X_calc_macro_period_ps( vcsel_period_pclks); 02535 macro_period_ns = (macro_period_ps + 500) / 1000; 02536 02537 timeout_period_mclks = (uint32_t)(((timeout_period_us * 1000) 02538 + (macro_period_ns / 2)) / macro_period_ns); 02539 02540 return timeout_period_mclks; 02541 } 02542 02543 uint32_t VL53L0X::VL53L0X_isqrt(uint32_t num) 02544 { /* Implements an integer square root 02545 * From: http://en.wikipedia.org/wiki/Methods_of_computing_square_roots */ 02546 02547 uint32_t res = 0; 02548 uint32_t bit = 1 << 30; 02549 /* The second-to-top bit is set: 1 << 14 for 16-bits, 1 << 30 for 32 bits */ 02550 02551 /* "bit" starts at the highest power of four <= the argument. */ 02552 while (bit > num) { bit >>= 2; } 02553 02554 while (bit != 0) { 02555 if (num >= res + bit) { 02556 num -= res + bit; 02557 res = (res >> 1) + bit; 02558 } 02559 else { res >>= 1; } 02560 bit >>= 2; 02561 } 02562 return res; 02563 } 02564 02565 VL53L0X_Error VL53L0X::VL53L0X_calc_dmax(FixPoint1616_t total_signal_rate_MHz, 02566 FixPoint1616_t total_corr_signal_rate_MHz, 02567 FixPoint1616_t pw_mult, 02568 uint32_t sigma_estimate_p1, 02569 FixPoint1616_t sigma_estimate_p2, 02570 uint32_t peak_vcsel_duration_us, 02571 uint32_t *pd_max_mm) 02572 { const uint32_t c_sigma_limit = 18; 02573 const FixPoint1616_t c_signal_limit = 0x4000; /* 0.25 */ 02574 const FixPoint1616_t c_sigma_est_ref = 0x00000042; /* 0.001 */ 02575 const uint32_t c_amb_eff_width_sigma_est_ns = 6; 02576 const uint32_t c_amb_eff_width_d_max_ns = 7; 02577 uint32_t dmax_cal_range_mm; 02578 FixPoint1616_t dmax_cal_signal_rate_rtn_MHz; 02579 FixPoint1616_t min_signal_needed; 02580 FixPoint1616_t min_signal_needed_p1; 02581 FixPoint1616_t min_signal_needed_p2; 02582 FixPoint1616_t min_signal_needed_p3; 02583 FixPoint1616_t min_signal_needed_p4; 02584 FixPoint1616_t sigma_limit_tmp; 02585 FixPoint1616_t sigma_est_sq_tmp; 02586 FixPoint1616_t signal_limit_tmp; 02587 FixPoint1616_t signal_at0_mm; 02588 FixPoint1616_t dmax_dark; 02589 FixPoint1616_t dmax_ambient; 02590 FixPoint1616_t dmax_dark_tmp; 02591 FixPoint1616_t sigma_est_p2_tmp; 02592 uint32_t signal_rate_temp_MHz; 02593 02594 VL53L0X_Error status = VL53L0X_ERROR_NONE; 02595 02596 dmax_cal_range_mm = Data.DmaxCalRange_mm; 02597 02598 dmax_cal_signal_rate_rtn_MHz = Data.DmaxCalSignalRateRtn_MHz; 02599 02600 /* uint32 * FixPoint1616 = FixPoint1616 */ 02601 signal_at0_mm = dmax_cal_range_mm * dmax_cal_signal_rate_rtn_MHz; 02602 02603 /* FixPoint1616 >> 8 = FixPoint2408 */ 02604 signal_at0_mm = (signal_at0_mm + 0x80) >> 8; 02605 signal_at0_mm *= dmax_cal_range_mm; 02606 02607 min_signal_needed_p1 = 0; 02608 if (total_corr_signal_rate_MHz > 0) { 02609 02610 /* Shift by 10 bits to increase resolution prior to the division */ 02611 signal_rate_temp_MHz = total_signal_rate_MHz << 10; 02612 02613 /* Add rounding value prior to division */ 02614 min_signal_needed_p1 = signal_rate_temp_MHz + 02615 (total_corr_signal_rate_MHz / 2); 02616 02617 /* FixPoint0626/FixPoint1616 = FixPoint2210 */ 02618 min_signal_needed_p1 /= total_corr_signal_rate_MHz; 02619 02620 /* Apply a factored version of the speed of light. 02621 Correction to be applied at the end */ 02622 min_signal_needed_p1 *= 3; 02623 02624 /* FixPoint2210 * FixPoint2210 = FixPoint1220 */ 02625 min_signal_needed_p1 *= min_signal_needed_p1; 02626 02627 /* FixPoint1220 >> 16 = FixPoint2804 */ 02628 min_signal_needed_p1 = (min_signal_needed_p1 + 0x8000) >> 16; 02629 } 02630 min_signal_needed_p2 = pw_mult * sigma_estimate_p1; 02631 02632 /* FixPoint1616 >> 16 = uint32 */ 02633 min_signal_needed_p2 = (min_signal_needed_p2 + 0x8000) >> 16; 02634 02635 /* uint32 * uint32 = uint32 */ 02636 min_signal_needed_p2 *= min_signal_needed_p2; 02637 02638 /* Check sigmaEstimateP2 02639 * If this value is too high there is not enough signal rate 02640 * to calculate dmax value so set a suitable value to ensure 02641 * a very small dmax. */ 02642 sigma_est_p2_tmp = (sigma_estimate_p2 + 0x8000) >> 16; 02643 sigma_est_p2_tmp = (sigma_est_p2_tmp + c_amb_eff_width_sigma_est_ns / 2) / 02644 c_amb_eff_width_sigma_est_ns; 02645 sigma_est_p2_tmp *= c_amb_eff_width_d_max_ns; 02646 02647 if (sigma_est_p2_tmp > 0xffff) { 02648 min_signal_needed_p3 = 0xfff00000; 02649 } else { 02650 /* DMAX uses a different ambient width from sigma, so apply correction. 02651 * Perform division before multiplication to prevent overflow. */ 02652 sigma_estimate_p2 = (sigma_estimate_p2 + c_amb_eff_width_sigma_est_ns / 2) / 02653 c_amb_eff_width_sigma_est_ns; 02654 sigma_estimate_p2 *= c_amb_eff_width_d_max_ns; 02655 02656 /* FixPoint1616 >> 16 = uint32 */ 02657 min_signal_needed_p3 = (sigma_estimate_p2 + 0x8000) >> 16; 02658 min_signal_needed_p3 *= min_signal_needed_p3; 02659 } 02660 02661 /* FixPoint1814 / uint32 = FixPoint1814 */ 02662 sigma_limit_tmp = ((c_sigma_limit << 14) + 500) / 1000; 02663 02664 /* FixPoint1814 * FixPoint1814 = FixPoint3628 := FixPoint0428 */ 02665 sigma_limit_tmp *= sigma_limit_tmp; 02666 02667 /* FixPoint1616 * FixPoint1616 = FixPoint3232 */ 02668 sigma_est_sq_tmp = c_sigma_est_ref * c_sigma_est_ref; 02669 02670 /* FixPoint3232 >> 4 = FixPoint0428 */ 02671 sigma_est_sq_tmp = (sigma_est_sq_tmp + 0x08) >> 4; 02672 02673 /* FixPoint0428 - FixPoint0428 = FixPoint0428 */ 02674 sigma_limit_tmp -= sigma_est_sq_tmp; 02675 02676 /* uint32_t * FixPoint0428 = FixPoint0428 */ 02677 min_signal_needed_p4 = 4 * 12 * sigma_limit_tmp; 02678 02679 /* FixPoint0428 >> 14 = FixPoint1814 */ 02680 min_signal_needed_p4 = (min_signal_needed_p4 + 0x2000) >> 14; 02681 02682 /* uint32 + uint32 = uint32 */ 02683 min_signal_needed = (min_signal_needed_p2 + min_signal_needed_p3); 02684 02685 /* uint32 / uint32 = uint32 */ 02686 min_signal_needed += (peak_vcsel_duration_us / 2); 02687 min_signal_needed /= peak_vcsel_duration_us; 02688 02689 /* uint32 << 14 = FixPoint1814 */ 02690 min_signal_needed <<= 14; 02691 02692 /* FixPoint1814 / FixPoint1814 = uint32 */ 02693 min_signal_needed += (min_signal_needed_p4 / 2); 02694 min_signal_needed /= min_signal_needed_p4; 02695 02696 /* FixPoint3200 * FixPoint2804 := FixPoint2804*/ 02697 min_signal_needed *= min_signal_needed_p1; 02698 02699 /* Apply correction by dividing by 1000000. 02700 * This assumes 10E16 on the numerator of the equation 02701 * and 10E-22 on the denominator. 02702 * We do this because 32bit fix point calculation can't 02703 * handle the larger and smaller elements of this equation, 02704 * i.e. speed of light and pulse widths. 02705 */ 02706 min_signal_needed = (min_signal_needed + 500) / 1000; 02707 min_signal_needed <<= 4; 02708 02709 min_signal_needed = (min_signal_needed + 500) / 1000; 02710 02711 /* FixPoint1616 >> 8 = FixPoint2408 */ 02712 signal_limit_tmp = (c_signal_limit + 0x80) >> 8; 02713 02714 /* FixPoint2408/FixPoint2408 = uint32 */ 02715 if (signal_limit_tmp != 0) { 02716 dmax_dark_tmp = (signal_at0_mm + (signal_limit_tmp / 2)) 02717 / signal_limit_tmp; 02718 } else { dmax_dark_tmp = 0; } 02719 02720 dmax_dark = VL53L0X_isqrt(dmax_dark_tmp); 02721 02722 /* FixPoint2408/FixPoint2408 = uint32 */ 02723 if (min_signal_needed != 0) { 02724 dmax_ambient = (signal_at0_mm + min_signal_needed / 2) 02725 / min_signal_needed; 02726 } else { dmax_ambient = 0; } 02727 02728 dmax_ambient = VL53L0X_isqrt(dmax_ambient); 02729 02730 *pd_max_mm = dmax_dark; 02731 if (dmax_dark > dmax_ambient) { *pd_max_mm = dmax_ambient; } 02732 02733 return status; 02734 } 02735 02736 VL53L0X_Error VL53L0X::VL53L0X_calc_sigma_estimate(VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data, 02737 FixPoint1616_t *p_sigma_estimate, uint32_t *p_dmax_mm) 02738 { /* Expressed in 100ths of a ns, i.e. centi-ns */ 02739 const uint32_t c_pulse_effective_width_centi_ns = 800; 02740 /* Expressed in 100ths of a ns, i.e. centi-ns */ 02741 const uint32_t c_ambient_effective_width_centi_ns = 600; 02742 const FixPoint1616_t c_dflt_final_range_integration_time_milli_secs = 0x00190000; /* 25ms */ 02743 const uint32_t c_vcsel_pulse_width_ps = 4700; /* pico secs */ 02744 const FixPoint1616_t c_sigma_est_max = 0x028F87AE; 02745 const FixPoint1616_t c_sigma_est_rtn_max = 0xF000; 02746 const FixPoint1616_t c_amb_to_signal_ratio_max = 0xF0000000 / 02747 c_ambient_effective_width_centi_ns; 02748 /* Time Of Flight per mm (6.6 pico secs) */ 02749 const FixPoint1616_t c_tof_per_mm_ps = 0x0006999A; 02750 const uint32_t c_16bit_rounding_param = 0x00008000; 02751 const FixPoint1616_t c_max_x_talk_kcps = 0x00320000; 02752 const uint32_t c_pll_period_ps = 1655; 02753 02754 uint32_t vcsel_total_events_rtn; 02755 uint32_t final_range_timeout_micro_secs; 02756 uint32_t pre_range_timeout_micro_secs; 02757 uint32_t final_range_integration_time_milli_secs; 02758 FixPoint1616_t sigma_estimate_p1; 02759 FixPoint1616_t sigma_estimate_p2; 02760 FixPoint1616_t sigma_estimate_p3; 02761 FixPoint1616_t delta_t_ps; 02762 FixPoint1616_t pw_mult; 02763 FixPoint1616_t sigma_est_rtn; 02764 FixPoint1616_t sigma_estimate; 02765 FixPoint1616_t x_talk_correction; 02766 FixPoint1616_t ambient_rate_kcps; 02767 FixPoint1616_t peak_signal_rate_kcps; 02768 FixPoint1616_t x_talk_comp_rate_MHz; 02769 uint32_t x_talk_comp_rate_kcps; 02770 VL53L0X_Error status = VL53L0X_ERROR_NONE; 02771 FixPoint1616_t diff1_MHz; 02772 FixPoint1616_t diff2_MHz; 02773 FixPoint1616_t sqr1; 02774 FixPoint1616_t sqr2; 02775 FixPoint1616_t sqr_sum; 02776 FixPoint1616_t sqrt_result_centi_ns; 02777 FixPoint1616_t sqrt_result; 02778 FixPoint1616_t total_signal_rate_MHz; 02779 FixPoint1616_t corrected_signal_rate_MHz; 02780 FixPoint1616_t sigma_est_ref; 02781 uint32_t vcsel_width; 02782 uint32_t final_range_macro_pclks; 02783 uint32_t pre_range_macro_pclks; 02784 uint32_t peak_vcsel_duration_us; 02785 uint8_t final_range_vcsel_pclks; 02786 uint8_t pre_range_vcsel_pclks; 02787 /*! \addtogroup calc_sigma_estimate 02788 * @{ 02789 * 02790 * Estimates the range sigma 02791 */ 02792 02793 x_talk_comp_rate_MHz = CurrentParameters .XTalkCompensationRate_MHz ; 02794 02795 /* 02796 * We work in kcps rather than MHz as this helps keep within the 02797 * confines of the 32 Fix1616 type. 02798 */ 02799 02800 ambient_rate_kcps = (p_ranging_measurement_data->AmbientRateRtn_MHz * 1000) >> 16; 02801 02802 corrected_signal_rate_MHz = p_ranging_measurement_data->SignalRateRtn_MHz; 02803 02804 status = VL53L0X_get_total_signal_rate(p_ranging_measurement_data, &total_signal_rate_MHz); 02805 status = VL53L0X_get_total_xtalk_rate(p_ranging_measurement_data, &x_talk_comp_rate_MHz); 02806 02807 /* Signal rate measurement provided by device is the 02808 * peak signal rate, not average. 02809 */ 02810 peak_signal_rate_kcps = (total_signal_rate_MHz * 1000); 02811 peak_signal_rate_kcps = (peak_signal_rate_kcps + 0x8000) >> 16; 02812 02813 x_talk_comp_rate_kcps = x_talk_comp_rate_MHz * 1000; 02814 02815 if (x_talk_comp_rate_kcps > c_max_x_talk_kcps) { 02816 x_talk_comp_rate_kcps = c_max_x_talk_kcps; 02817 } 02818 02819 if (status == VL53L0X_ERROR_NONE) { 02820 02821 /* Calculate final range macro periods */ 02822 final_range_timeout_micro_secs = Data.FinalRangeTimeout_us; 02823 final_range_vcsel_pclks = Data.FinalRangeVcselPulsePeriod; 02824 final_range_macro_pclks = VL53L0X_calc_timeout_mclks( final_range_timeout_micro_secs, final_range_vcsel_pclks); 02825 02826 /* Calculate pre-range macro periods */ 02827 pre_range_timeout_micro_secs = Data.PreRangeTimeout_us; 02828 pre_range_vcsel_pclks = Data.PreRangeVcselPulsePeriod; 02829 02830 pre_range_macro_pclks = VL53L0X_calc_timeout_mclks(pre_range_timeout_micro_secs, pre_range_vcsel_pclks); 02831 02832 vcsel_width = 3; 02833 if (final_range_vcsel_pclks == 8) { 02834 vcsel_width = 2; 02835 } 02836 02837 peak_vcsel_duration_us = vcsel_width * 2048 * 02838 (pre_range_macro_pclks + final_range_macro_pclks); 02839 peak_vcsel_duration_us = (peak_vcsel_duration_us + 500) / 1000; 02840 peak_vcsel_duration_us *= c_pll_period_ps; 02841 peak_vcsel_duration_us = (peak_vcsel_duration_us + 500) / 1000; 02842 02843 /* Fix1616 >> 8 = Fix2408 */ 02844 total_signal_rate_MHz = (total_signal_rate_MHz + 0x80) >> 8; 02845 02846 /* Fix2408 * uint32 = Fix2408 */ 02847 vcsel_total_events_rtn = total_signal_rate_MHz * 02848 peak_vcsel_duration_us; 02849 02850 /* Fix2408 >> 8 = uint32 */ 02851 vcsel_total_events_rtn = (vcsel_total_events_rtn + 0x80) >> 8; 02852 02853 /* Fix2408 << 8 = Fix1616 = */ 02854 total_signal_rate_MHz <<= 8; 02855 } 02856 02857 if (status != VL53L0X_ERROR_NONE) { return status; } 02858 02859 if (peak_signal_rate_kcps == 0) { 02860 *p_sigma_estimate = c_sigma_est_max; 02861 p_ranging_measurement_data->SigmaEstimate = c_sigma_est_max; 02862 *p_dmax_mm = 0; 02863 } else { 02864 if (vcsel_total_events_rtn < 1) { vcsel_total_events_rtn = 1; } 02865 02866 sigma_estimate_p1 = c_pulse_effective_width_centi_ns; 02867 02868 /* ((FixPoint1616 << 16)* uint32)/uint32 = FixPoint1616 */ 02869 sigma_estimate_p2 = (ambient_rate_kcps << 16) / peak_signal_rate_kcps; 02870 if (sigma_estimate_p2 > c_amb_to_signal_ratio_max) { 02871 /* Clip to prevent overflow. Will ensure safe 02872 * max result. */ 02873 sigma_estimate_p2 = c_amb_to_signal_ratio_max; 02874 } 02875 sigma_estimate_p2 *= c_ambient_effective_width_centi_ns; 02876 02877 sigma_estimate_p3 = 2 * VL53L0X_isqrt(vcsel_total_events_rtn * 12); 02878 02879 /* uint32 * FixPoint1616 = FixPoint1616 */ 02880 delta_t_ps = p_ranging_measurement_data->Range_mm * c_tof_per_mm_ps; 02881 02882 /* vcselRate - xtalkCompRate 02883 * (uint32 << 16) - FixPoint1616 = FixPoint1616. 02884 * Divide result by 1000 to convert to MHz. 02885 * 500 is added to ensure rounding when integer division truncates. */ 02886 diff1_MHz = (((peak_signal_rate_kcps << 16) - 02887 2 * x_talk_comp_rate_kcps) + 500) / 1000; 02888 02889 /* vcselRate + xtalkCompRate */ 02890 diff2_MHz = ((peak_signal_rate_kcps << 16) + 500) / 1000; 02891 02892 /* Shift by 8 bits to increase resolution prior to the division */ 02893 diff1_MHz <<= 8; 02894 02895 /* FixPoint0824/FixPoint1616 = FixPoint2408 */ 02896 x_talk_correction = diff1_MHz / diff2_MHz; 02897 02898 /* FixPoint2408 << 8 = FixPoint1616 */ 02899 x_talk_correction <<= 8; 02900 02901 if (p_ranging_measurement_data->RangeStatus != 0) { 02902 pw_mult = 1 << 16; 02903 } else { 02904 /* FixPoint1616/uint32 = FixPoint1616 */ 02905 pw_mult = delta_t_ps / c_vcsel_pulse_width_ps; /* smaller than 1.0f */ 02906 02907 /* FixPoint1616 * FixPoint1616 = FixPoint3232, however both 02908 * values are small enough such that32 bits will not be exceeded. */ 02909 pw_mult *= ((1 << 16) - x_talk_correction); 02910 02911 /* (FixPoint3232 >> 16) = FixPoint1616 */ 02912 pw_mult = (pw_mult + c_16bit_rounding_param) >> 16; 02913 02914 /* FixPoint1616 + FixPoint1616 = FixPoint1616 */ 02915 pw_mult += (1 << 16); 02916 02917 /* At this point the value will be 1.xx, therefore if we square 02918 * the value this will exceed 32 bits. To address this perform 02919 * a single shift to the right before the multiplication. */ 02920 pw_mult >>= 1; 02921 /* FixPoint1715 * FixPoint1715 = FixPoint3430 */ 02922 pw_mult = pw_mult * pw_mult; 02923 02924 /* (FixPoint3430 >> 14) = Fix1616 */ 02925 pw_mult >>= 14; 02926 } 02927 02928 /* FixPoint1616 * uint32 = FixPoint1616 */ 02929 sqr1 = pw_mult * sigma_estimate_p1; 02930 02931 /* (FixPoint1616 >> 16) = FixPoint3200 */ 02932 sqr1 = (sqr1 + 0x8000) >> 16; 02933 02934 /* FixPoint3200 * FixPoint3200 = FixPoint6400 */ 02935 sqr1 *= sqr1; 02936 02937 sqr2 = sigma_estimate_p2; 02938 02939 /* (FixPoint1616 >> 16) = FixPoint3200 */ 02940 sqr2 = (sqr2 + 0x8000) >> 16; 02941 02942 /* FixPoint3200 * FixPoint3200 = FixPoint6400 */ 02943 sqr2 *= sqr2; 02944 02945 /* FixPoint64000 + FixPoint6400 = FixPoint6400 */ 02946 sqr_sum = sqr1 + sqr2; 02947 02948 /* SQRT(FixPoin6400) = FixPoint3200 */ 02949 sqrt_result_centi_ns = VL53L0X_isqrt(sqr_sum); 02950 02951 /* (FixPoint3200 << 16) = FixPoint1616 */ 02952 sqrt_result_centi_ns <<= 16; 02953 02954 /* Note that the Speed Of Light is expressed in um per 1E-10 02955 * seconds (2997) Therefore to get mm/ns we have to divide by 02956 * 10000 02957 */ 02958 sigma_est_rtn = (((sqrt_result_centi_ns + 50) / 100) / 02959 sigma_estimate_p3); 02960 sigma_est_rtn *= VL53L0X_SPEED_OF_LIGHT_IN_AIR; 02961 02962 /* Add 5000 before dividing by 10000 to ensure rounding. */ 02963 sigma_est_rtn += 5000; 02964 sigma_est_rtn /= 10000; 02965 02966 if (sigma_est_rtn > c_sigma_est_rtn_max) { 02967 /* Clip to prevent overflow. Will ensure safe 02968 * max result. */ 02969 sigma_est_rtn = c_sigma_est_rtn_max; 02970 } 02971 final_range_integration_time_milli_secs = 02972 (final_range_timeout_micro_secs + pre_range_timeout_micro_secs + 500) / 1000; 02973 02974 /* sigmaEstRef = 1mm * 25ms/final range integration time (inc pre-range) 02975 * sqrt(FixPoint1616/int) = FixPoint2408) 02976 */ 02977 sigma_est_ref = 02978 VL53L0X_isqrt((c_dflt_final_range_integration_time_milli_secs + 02979 final_range_integration_time_milli_secs / 2) / 02980 final_range_integration_time_milli_secs); 02981 02982 /* FixPoint2408 << 8 = FixPoint1616 */ 02983 sigma_est_ref <<= 8; 02984 sigma_est_ref = (sigma_est_ref + 500) / 1000; 02985 02986 /* FixPoint1616 * FixPoint1616 = FixPoint3232 */ 02987 sqr1 = sigma_est_rtn * sigma_est_rtn; 02988 /* FixPoint1616 * FixPoint1616 = FixPoint3232 */ 02989 sqr2 = sigma_est_ref * sigma_est_ref; 02990 02991 /* sqrt(FixPoint3232) = FixPoint1616 */ 02992 sqrt_result = VL53L0X_isqrt((sqr1 + sqr2)); 02993 /* 02994 * Note that the Shift by 4 bits increases resolution prior to 02995 * the sqrt, therefore the result must be shifted by 2 bits to 02996 * the right to revert back to the FixPoint1616 format. 02997 */ 02998 02999 sigma_estimate = 1000 * sqrt_result; 03000 03001 if ((peak_signal_rate_kcps < 1) || (vcsel_total_events_rtn < 1) || 03002 (sigma_estimate > c_sigma_est_max)) { 03003 sigma_estimate = c_sigma_est_max; 03004 } 03005 03006 *p_sigma_estimate = (uint32_t)(sigma_estimate); 03007 p_ranging_measurement_data->SigmaEstimate = *p_sigma_estimate; 03008 status = VL53L0X_calc_dmax(total_signal_rate_MHz, 03009 corrected_signal_rate_MHz, 03010 pw_mult, 03011 sigma_estimate_p1, 03012 sigma_estimate_p2, 03013 peak_vcsel_duration_us, 03014 p_dmax_mm); 03015 } 03016 03017 return status; 03018 } 03019 03020 VL53L0X_Error VL53L0X::VL53L0X_get_pal_range_status(uint8_t device_range_status, 03021 FixPoint1616_t signal_rate, 03022 uint16_t effective_spad_rtn_count, 03023 VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data, 03024 uint8_t *p_pal_range_status) 03025 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 03026 uint8_t none_flag; 03027 uint8_t sigma_limitflag = 0; 03028 uint8_t signal_ref_clipflag = 0; 03029 uint8_t range_ignore_thresholdflag = 0; 03030 uint8_t sigma_limit_check_enable = 0; 03031 uint8_t signal_rate_final_range_limit_check_enable = 0; 03032 uint8_t signal_ref_clip_limit_check_enable = 0; 03033 uint8_t range_ignore_threshold_limit_check_enable = 0; 03034 FixPoint1616_t sigma_estimate; 03035 FixPoint1616_t sigma_limit_value; 03036 FixPoint1616_t signal_ref_clip_value; 03037 FixPoint1616_t range_ignore_threshold_value; 03038 FixPoint1616_t signal_rate_per_spad; 03039 uint8_t device_range_status_internal = 0; 03040 uint16_t tmp_word = 0; 03041 uint8_t temp8; 03042 uint32_t dmax_mm = 0; 03043 FixPoint1616_t last_signal_ref_MHz; 03044 03045 /* VL53L0X has a good ranging when the value of the 03046 * DeviceRangeStatus = 11. This function will replace the value 0 with 03047 * the value 11 in the DeviceRangeStatus. 03048 * In addition, the SigmaEstimator is not included in the VL53L0X 03049 * DeviceRangeStatus, this will be added in the PalRangeStatus. */ 03050 03051 device_range_status_internal = ((device_range_status & 0x78) >> 3); 03052 03053 if (device_range_status_internal == 0 || 03054 device_range_status_internal == 5 || 03055 device_range_status_internal == 7 || 03056 device_range_status_internal == 12 || 03057 device_range_status_internal == 13 || 03058 device_range_status_internal == 14 || 03059 device_range_status_internal == 15 03060 ) { 03061 none_flag = 1; 03062 } else { 03063 none_flag = 0; 03064 } 03065 03066 /* Check if Sigma limit is enabled, if yes then do comparison with limit 03067 * value and put the result back into pPalRangeStatus. */ 03068 if (status == VL53L0X_ERROR_NONE) { 03069 status = VL53L0X_get_limit_check_enable(VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, 03070 &sigma_limit_check_enable); 03071 } 03072 03073 if ((sigma_limit_check_enable != 0) && (status == VL53L0X_ERROR_NONE)) { 03074 /* compute the Sigma and check with limit */ 03075 status = VL53L0X_calc_sigma_estimate(p_ranging_measurement_data, &sigma_estimate, &dmax_mm); 03076 if (status == VL53L0X_ERROR_NONE) { p_ranging_measurement_data->RangeDMax_mm = dmax_mm; } 03077 03078 if (status == VL53L0X_ERROR_NONE) { 03079 status = VL53L0X_get_limit_check_value(VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, 03080 &sigma_limit_value); 03081 03082 if ((sigma_limit_value > 0) && (sigma_estimate > sigma_limit_value)) 03083 { sigma_limitflag = 1; } /* Limit Fail */ 03084 } 03085 } 03086 03087 /* Check if Signal ref clip limit is enabled, if yes then do comparison 03088 * with limit value and put the result back into pPalRangeStatus. */ 03089 if (status == VL53L0X_ERROR_NONE) { 03090 status = VL53L0X_get_limit_check_enable(VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, 03091 &signal_ref_clip_limit_check_enable); 03092 } 03093 03094 if ((signal_ref_clip_limit_check_enable != 0) && 03095 (status == VL53L0X_ERROR_NONE)) { 03096 03097 status = VL53L0X_get_limit_check_value(VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, 03098 &signal_ref_clip_value); 03099 03100 /* Read LastSignalRef_MHz from device */ 03101 if (status == VL53L0X_ERROR_NONE) { 03102 status = VL53L0X_write_byte( 0xFF, 0x01); 03103 } 03104 03105 if (status == VL53L0X_ERROR_NONE) { 03106 status = VL53L0X_read_word(VL53L0X_REG_RESULT_PEAK_SIGNAL_RATE_REF, 03107 &tmp_word); 03108 } 03109 03110 if (status == VL53L0X_ERROR_NONE) { 03111 status = VL53L0X_write_byte( 0xFF, 0x00); 03112 } 03113 03114 last_signal_ref_MHz = VL53L0X_FP97TOFP1616(tmp_word); 03115 Data.LastSignalRef_MHz = last_signal_ref_MHz; 03116 03117 if ((signal_ref_clip_value > 0) && 03118 (last_signal_ref_MHz > signal_ref_clip_value)) { 03119 /* Limit Fail */ 03120 signal_ref_clipflag = 1; 03121 } 03122 } 03123 03124 /* 03125 * Check if Signal ref clip limit is enabled, if yes then do comparison 03126 * with limit value and put the result back into pPalRangeStatus. 03127 * EffectiveSpadRtnCount has a format 8.8 03128 * If (Return signal rate < (1.5 x Xtalk x number of Spads)) : FAIL 03129 */ 03130 if (status == VL53L0X_ERROR_NONE) { 03131 status = VL53L0X_get_limit_check_enable(VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, 03132 &range_ignore_threshold_limit_check_enable); 03133 } 03134 03135 if ((range_ignore_threshold_limit_check_enable != 0) && 03136 (status == VL53L0X_ERROR_NONE)) { 03137 03138 /* Compute the signal rate per spad */ 03139 if (effective_spad_rtn_count == 0) { 03140 signal_rate_per_spad = 0; 03141 } else { 03142 signal_rate_per_spad = (FixPoint1616_t)((256 * signal_rate) 03143 / effective_spad_rtn_count); 03144 } 03145 03146 status = VL53L0X_get_limit_check_value(VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, 03147 &range_ignore_threshold_value); 03148 03149 if ((range_ignore_threshold_value > 0) && 03150 (signal_rate_per_spad < range_ignore_threshold_value)) { 03151 /* Limit Fail add 2^6 to range status */ 03152 range_ignore_thresholdflag = 1; 03153 } 03154 } 03155 03156 if (status == VL53L0X_ERROR_NONE) { 03157 if (none_flag == 1) { 03158 *p_pal_range_status = 255; /* NONE */ 03159 } else if (device_range_status_internal == 1 || 03160 device_range_status_internal == 2 || 03161 device_range_status_internal == 3) { 03162 *p_pal_range_status = 5; /* HW fail */ 03163 } else if (device_range_status_internal == 6 || 03164 device_range_status_internal == 9) { 03165 *p_pal_range_status = 4; /* Phase fail */ 03166 } else if (device_range_status_internal == 8 || 03167 device_range_status_internal == 10 || 03168 signal_ref_clipflag == 1) { 03169 *p_pal_range_status = 3; /* Min range */ 03170 } else if (device_range_status_internal == 4 || 03171 range_ignore_thresholdflag == 1) { 03172 *p_pal_range_status = 2; /* Signal Fail */ 03173 } else if (sigma_limitflag == 1) { 03174 *p_pal_range_status = 1; /* Sigma Fail */ 03175 } else { 03176 *p_pal_range_status = 0; /* Range Valid */ 03177 } 03178 } 03179 03180 /* DMAX only relevant during range error */ 03181 if (*p_pal_range_status == 0) { 03182 p_ranging_measurement_data->RangeDMax_mm = 0; 03183 } 03184 03185 /* fill the Limit Check Status */ 03186 03187 status = VL53L0X_get_limit_check_enable(VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, 03188 &signal_rate_final_range_limit_check_enable); 03189 03190 if (status == VL53L0X_ERROR_NONE) { 03191 if ((sigma_limit_check_enable == 0) || (sigma_limitflag == 1)) { 03192 temp8 = 1; 03193 } else { 03194 temp8 = 0; 03195 } 03196 CurrentParameters .LimitChecksStatus [VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE] = temp8; 03197 03198 if ((device_range_status_internal == 4) || 03199 (signal_rate_final_range_limit_check_enable == 0)) { 03200 temp8 = 1; 03201 } else { 03202 temp8 = 0; 03203 } 03204 CurrentParameters .LimitChecksStatus [VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE] = temp8; 03205 03206 if ((signal_ref_clip_limit_check_enable == 0) || 03207 (signal_ref_clipflag == 1)) { 03208 temp8 = 1; 03209 } else { 03210 temp8 = 0; 03211 } 03212 03213 CurrentParameters .LimitChecksStatus [VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP] = temp8; 03214 03215 if ((range_ignore_threshold_limit_check_enable == 0) || 03216 (range_ignore_thresholdflag == 1)) { 03217 temp8 = 1; 03218 } else { 03219 temp8 = 0; 03220 } 03221 03222 CurrentParameters .LimitChecksStatus [VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD] = temp8; 03223 } 03224 03225 return status; 03226 } 03227 03228 VL53L0X_Error VL53L0X::VL53L0X_get_ranging_measurement_data(VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data) 03229 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 03230 uint8_t device_range_status; 03231 uint8_t range_fractional_enable; 03232 uint8_t pal_range_status; 03233 uint8_t x_talk_compensation_enable; 03234 uint16_t ambient_rate; 03235 FixPoint1616_t signal_rate; 03236 uint16_t x_talk_compensation_rate_MHz; 03237 uint16_t effective_spad_rtn_count; 03238 uint16_t tmpuint16; 03239 uint16_t xtalk_range_milli_meter; 03240 uint16_t linearity_corrective_gain; 03241 uint8_t localBuffer[12]; 03242 03243 /* use multi read even if some registers are not useful, result will 03244 * be more efficient start reading at 0x14 dec20 03245 * end reading at 0x21 dec33 total 14 bytes to read */ 03246 status = VL53L0X_read_multi( 0x14, localBuffer, 12); 03247 03248 if (status == VL53L0X_ERROR_NONE) { 03249 03250 tmpuint16 = VL53L0X_MAKEUINT16(localBuffer[11], localBuffer[10]); 03251 /* cut1.1 if SYSTEM__RANGE_CONFIG if 1 range is 2bits fractional 03252 *(format 11.2) else no fractional */ 03253 03254 signal_rate = VL53L0X_FP97TOFP1616(VL53L0X_MAKEUINT16(localBuffer[7], localBuffer[6])); 03255 /* peak_signal_count_rate_rtn_MHz */ 03256 p_ranging_measurement_data->SignalRateRtn_MHz = signal_rate; 03257 03258 ambient_rate = VL53L0X_MAKEUINT16(localBuffer[9], localBuffer[8]); 03259 p_ranging_measurement_data->AmbientRateRtn_MHz = 03260 VL53L0X_FP97TOFP1616(ambient_rate); 03261 03262 effective_spad_rtn_count = VL53L0X_MAKEUINT16(localBuffer[3], 03263 localBuffer[2]); 03264 /* EffectiveSpadRtnCount is 8.8 format */ 03265 p_ranging_measurement_data->EffectiveSpadRtnCount = 03266 effective_spad_rtn_count; 03267 03268 device_range_status = localBuffer[0]; 03269 03270 /* Get Linearity Corrective Gain */ 03271 linearity_corrective_gain = Data.LinearityCorrectiveGain; 03272 03273 /* Get ranging configuration */ 03274 range_fractional_enable = Data.RangeFractionalEnable; 03275 03276 if (linearity_corrective_gain != 1000) { 03277 03278 tmpuint16 = (uint16_t)((linearity_corrective_gain * tmpuint16 + 500) / 1000); 03279 03280 /* Implement Xtalk */ 03281 x_talk_compensation_rate_MHz = CurrentParameters .XTalkCompensationRate_MHz ; 03282 x_talk_compensation_enable = CurrentParameters .XTalkCompensationEnable ; 03283 03284 if (x_talk_compensation_enable) { 03285 03286 if ((signal_rate 03287 - ((x_talk_compensation_rate_MHz 03288 * effective_spad_rtn_count) >> 8)) 03289 <= 0) { 03290 if (range_fractional_enable) { 03291 xtalk_range_milli_meter = 8888; 03292 } else { 03293 xtalk_range_milli_meter = 8888 << 2; 03294 } 03295 } else { 03296 xtalk_range_milli_meter = 03297 (tmpuint16 * signal_rate) 03298 / (signal_rate 03299 - ((x_talk_compensation_rate_MHz 03300 * effective_spad_rtn_count) 03301 >> 8)); 03302 } 03303 tmpuint16 = xtalk_range_milli_meter; 03304 } 03305 } 03306 03307 if (range_fractional_enable) { 03308 p_ranging_measurement_data->Range_mm = 03309 (uint16_t)((tmpuint16) >> 2); 03310 p_ranging_measurement_data->RangeFractionalPart = 03311 (uint8_t)((tmpuint16 & 0x03) << 6); 03312 } else { 03313 p_ranging_measurement_data->Range_mm = tmpuint16; 03314 p_ranging_measurement_data->RangeFractionalPart = 0; 03315 } 03316 03317 /* For a standard definition of RangeStatus, this should 03318 * return 0 in case of good result after a ranging 03319 * The range status depends on the device so call a device 03320 * specific function to obtain the right Status. 03321 */ 03322 status |= VL53L0X_get_pal_range_status( device_range_status, 03323 signal_rate, effective_spad_rtn_count, 03324 p_ranging_measurement_data, &pal_range_status); 03325 03326 if (status == VL53L0X_ERROR_NONE) { 03327 p_ranging_measurement_data->RangeStatus = pal_range_status;} 03328 03329 } 03330 03331 if (status == VL53L0X_ERROR_NONE) { /* Copy last read data into Device buffer */ 03332 LastRangeMeasure .Range_mm = p_ranging_measurement_data->Range_mm; 03333 LastRangeMeasure .RangeFractionalPart = p_ranging_measurement_data->RangeFractionalPart; 03334 LastRangeMeasure .RangeDMax_mm = p_ranging_measurement_data->RangeDMax_mm; 03335 LastRangeMeasure .SignalRateRtn_MHz = p_ranging_measurement_data->SignalRateRtn_MHz; 03336 LastRangeMeasure .AmbientRateRtn_MHz = p_ranging_measurement_data->AmbientRateRtn_MHz; 03337 LastRangeMeasure .EffectiveSpadRtnCount = p_ranging_measurement_data->EffectiveSpadRtnCount; 03338 LastRangeMeasure .RangeStatus = p_ranging_measurement_data->RangeStatus; 03339 } 03340 03341 return status; 03342 } 03343 03344 VL53L0X_Error VL53L0X::VL53L0X_perform_single_ranging_measurement(VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data) 03345 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 03346 03347 /* This function will do a complete single ranging 03348 * Here we fix the mode! */ 03349 CurrentParameters .DeviceMode = VL53L0X_DEVICEMODE_SINGLE_RANGING; 03350 03351 if (status == VL53L0X_ERROR_NONE) { 03352 status = VL53L0X_perform_single_measurement(); } 03353 03354 if (status == VL53L0X_ERROR_NONE) { 03355 status = VL53L0X_get_ranging_measurement_data(p_ranging_measurement_data); } 03356 03357 if (status == VL53L0X_ERROR_NONE) { 03358 status = VL53L0X_clear_interrupt_mask( 0);} 03359 03360 return status; 03361 } 03362 03363 VL53L0X_Error VL53L0X::perform_ref_signal_measurement(uint16_t *p_ref_signal_rate) 03364 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 03365 VL53L0X_RangingMeasurementData_t ranging_measurement_data; 03366 03367 uint8_t sequence_config = 0; 03368 03369 /* store the value of the sequence config, 03370 * this will be reset before the end of the function*/ 03371 sequence_config = Data.SequenceConfig; 03372 03373 /* 03374 * This function performs a reference signal rate measurement. 03375 */ 03376 if (status == VL53L0X_ERROR_NONE) { 03377 status = VL53L0X_write_byte(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0xC0);} 03378 03379 if (status == VL53L0X_ERROR_NONE) { 03380 status = VL53L0X_perform_single_ranging_measurement(&ranging_measurement_data); } 03381 03382 if (status == VL53L0X_ERROR_NONE) { 03383 status = VL53L0X_write_byte( 0xFF, 0x01); } 03384 03385 if (status == VL53L0X_ERROR_NONE) { 03386 status = VL53L0X_read_word(VL53L0X_REG_RESULT_PEAK_SIGNAL_RATE_REF, 03387 p_ref_signal_rate);} 03388 03389 if (status == VL53L0X_ERROR_NONE) { 03390 status = VL53L0X_write_byte( 0xFF, 0x00);} 03391 03392 if (status == VL53L0X_ERROR_NONE) { 03393 /* restore the previous Sequence Config */ 03394 status = VL53L0X_write_byte( VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 03395 sequence_config); 03396 if (status == VL53L0X_ERROR_NONE) { 03397 Data.SequenceConfig = sequence_config; 03398 } 03399 } 03400 return status; 03401 } 03402 03403 VL53L0X_Error VL53L0X::wrapped_VL53L0X_perform_ref_spad_management(uint32_t *ref_spad_count, 03404 uint8_t *is_aperture_spads) 03405 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 03406 uint8_t last_spad_array[6]; 03407 uint8_t start_select = 0xB4; 03408 uint32_t minimum_spad_count = 3; 03409 uint32_t max_spad_count = 44; 03410 uint32_t current_spad_index = 0; 03411 uint32_t last_spad_index = 0; 03412 int32_t next_good_spad = 0; 03413 uint16_t target_ref_rate = 0x0A00; /* 20 MHz in 9:7 format */ 03414 uint16_t peak_signal_rate_ref; 03415 uint32_t need_apt_spads = 0; 03416 uint32_t index = 0; 03417 uint32_t spad_array_size = 6; 03418 uint32_t signal_rate_diff = 0; 03419 uint32_t last_signal_rate_diff = 0; 03420 uint8_t complete = 0; 03421 uint8_t vhv_settings = 0; 03422 uint8_t phase_cal = 0; 03423 uint32_t ref_spad_count_int = 0; 03424 uint8_t is_aperture_spads_int = 0; 03425 03426 /* 03427 * The reference SPAD initialization procedure determines the minimum 03428 * amount of reference spads to be enables to achieve a target reference 03429 * signal rate and should be performed once during initialization. 03430 * 03431 * Either aperture or non-aperture spads are applied but never both. 03432 * Firstly non-aperture spads are set, begining with 5 spads, and 03433 * increased one spad at a time until the closest measurement to the 03434 * target rate is achieved. 03435 * 03436 * If the target rate is exceeded when 5 non-aperture spads are enabled, 03437 * initialization is performed instead with aperture spads. 03438 * 03439 * When setting spads, a 'Good Spad Map' is applied. 03440 * 03441 * This procedure operates within a SPAD window of interest of a maximum 03442 * 44 spads. 03443 * The start point is currently fixed to 180, which lies towards the end 03444 * of the non-aperture quadrant and runs in to the adjacent aperture 03445 * quadrant. 03446 */ 03447 target_ref_rate = Data.targetRefRate; 03448 03449 /* 03450 * Initialize Spad arrays. 03451 * Currently the good spad map is initialised to 'All good'. 03452 * This is a short term implementation. The good spad map will be 03453 * provided as an input. 03454 * Note that there are 6 bytes. Only the first 44 bits will be used to 03455 * represent spads. 03456 */ 03457 for (index = 0; index < spad_array_size; index++) { 03458 Data.RefSpadEnables[index] = 0; 03459 } 03460 03461 status = VL53L0X_write_byte( 0xFF, 0x01); 03462 03463 if (status == VL53L0X_ERROR_NONE) { 03464 status = VL53L0X_write_byte(VL53L0X_REG_DYNAMIC_SPAD_REF_EN_START_OFFSET, 0x00); 03465 } 03466 03467 if (status == VL53L0X_ERROR_NONE) { 03468 status = VL53L0X_write_byte(VL53L0X_REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD, 0x2C); 03469 } 03470 03471 if (status == VL53L0X_ERROR_NONE) { 03472 status = VL53L0X_write_byte( 0xFF, 0x00); 03473 } 03474 03475 if (status == VL53L0X_ERROR_NONE) { 03476 status = VL53L0X_write_byte(VL53L0X_REG_GLOBAL_CONFIG_REF_EN_START_SELECT, 03477 start_select); 03478 } 03479 03480 if (status == VL53L0X_ERROR_NONE) { 03481 status = VL53L0X_write_byte(VL53L0X_REG_POWER_MANAGEMENT_GO1_POWER_FORCE, 0); 03482 } 03483 03484 /* Perform ref calibration */ 03485 if (status == VL53L0X_ERROR_NONE) { 03486 status = VL53L0X_perform_ref_calibration( &vhv_settings, 03487 &phase_cal, 0); 03488 } 03489 03490 if (status == VL53L0X_ERROR_NONE) { 03491 /* Enable Minimum NON-APERTURE Spads */ 03492 current_spad_index = 0; 03493 last_spad_index = current_spad_index; 03494 need_apt_spads = 0; 03495 status = enable_ref_spads(need_apt_spads, 03496 Data.RefGoodSpadMap, 03497 Data.RefSpadEnables, 03498 spad_array_size, 03499 start_select, 03500 current_spad_index, 03501 minimum_spad_count, 03502 &last_spad_index); 03503 } 03504 03505 if (status == VL53L0X_ERROR_NONE) { 03506 current_spad_index = last_spad_index; 03507 03508 status = perform_ref_signal_measurement(&peak_signal_rate_ref); 03509 if ((status == VL53L0X_ERROR_NONE) && 03510 (peak_signal_rate_ref > target_ref_rate)) { 03511 /* Signal rate measurement too high, 03512 * switch to APERTURE SPADs */ 03513 03514 for (index = 0; index < spad_array_size; index++) { 03515 Data.RefSpadEnables[index] = 0; 03516 } 03517 03518 /* Increment to the first APERTURE spad */ 03519 while ((is_aperture(start_select + current_spad_index) 03520 == 0) && (current_spad_index < max_spad_count)) { 03521 current_spad_index++; 03522 } 03523 03524 need_apt_spads = 1; 03525 03526 status = enable_ref_spads(need_apt_spads, 03527 Data.RefGoodSpadMap, 03528 Data.RefSpadEnables, 03529 spad_array_size, 03530 start_select, 03531 current_spad_index, 03532 minimum_spad_count, 03533 &last_spad_index); 03534 03535 if (status == VL53L0X_ERROR_NONE) { 03536 current_spad_index = last_spad_index; 03537 status = perform_ref_signal_measurement(&peak_signal_rate_ref); 03538 03539 if ((status == VL53L0X_ERROR_NONE) && 03540 (peak_signal_rate_ref > target_ref_rate)) { 03541 /* Signal rate still too high after 03542 * setting the minimum number of 03543 * APERTURE spads. Can do no more 03544 * therefore set the min number of 03545 * aperture spads as the result. 03546 */ 03547 is_aperture_spads_int = 1; 03548 ref_spad_count_int = minimum_spad_count; 03549 } 03550 } 03551 } else { 03552 need_apt_spads = 0; 03553 } 03554 } 03555 03556 if ((status == VL53L0X_ERROR_NONE) && 03557 (peak_signal_rate_ref < target_ref_rate)) { 03558 /* At this point, the minimum number of either aperture 03559 * or non-aperture spads have been set. Proceed to add 03560 * spads and perform measurements until the target 03561 * reference is reached. 03562 */ 03563 is_aperture_spads_int = need_apt_spads; 03564 ref_spad_count_int = minimum_spad_count; 03565 03566 memcpy(last_spad_array, Data.RefSpadEnables, 03567 spad_array_size); 03568 last_signal_rate_diff = abs(peak_signal_rate_ref - 03569 target_ref_rate); 03570 complete = 0; 03571 03572 while (!complete) { 03573 get_next_good_spad(Data.RefGoodSpadMap, 03574 spad_array_size, current_spad_index, 03575 &next_good_spad); 03576 03577 if (next_good_spad == -1) { 03578 status = VL53L0X_ERROR_REF_SPAD_INIT; 03579 break; 03580 } 03581 03582 /* Cannot combine Aperture and Non-Aperture spads, so 03583 * ensure the current spad is of the correct type. 03584 */ 03585 if (is_aperture((uint32_t)start_select + next_good_spad) != 03586 need_apt_spads) { 03587 /* At this point we have enabled the maximum 03588 * number of Aperture spads. 03589 */ 03590 complete = 1; 03591 break; 03592 } 03593 03594 (ref_spad_count_int)++; 03595 03596 current_spad_index = next_good_spad; 03597 status = enable_spad_bit(Data.RefSpadEnables, 03598 spad_array_size, current_spad_index); 03599 03600 if (status == VL53L0X_ERROR_NONE) { 03601 current_spad_index++; 03602 /* Proceed to apply the additional spad and 03603 * perform measurement. */ 03604 status = set_ref_spad_map(Data.RefSpadEnables); 03605 } 03606 03607 if (status != VL53L0X_ERROR_NONE) { 03608 break; 03609 } 03610 03611 status = perform_ref_signal_measurement(&peak_signal_rate_ref); 03612 03613 if (status != VL53L0X_ERROR_NONE) { 03614 break; 03615 } 03616 03617 signal_rate_diff = abs(peak_signal_rate_ref - target_ref_rate); 03618 03619 if (peak_signal_rate_ref > target_ref_rate) { 03620 /* Select the spad map that provides the 03621 * measurement closest to the target rate, 03622 * either above or below it. 03623 */ 03624 if (signal_rate_diff > last_signal_rate_diff) { 03625 /* Previous spad map produced a closer 03626 * measurement, so choose this. */ 03627 status = set_ref_spad_map(last_spad_array); 03628 memcpy(Data.RefSpadEnables, 03629 last_spad_array, spad_array_size); 03630 (ref_spad_count_int)--; 03631 } 03632 complete = 1; 03633 } else { 03634 /* Continue to add spads */ 03635 last_signal_rate_diff = signal_rate_diff; 03636 memcpy(last_spad_array, 03637 Data.RefSpadEnables, 03638 spad_array_size); 03639 } 03640 03641 } /* while */ 03642 } 03643 03644 if (status == VL53L0X_ERROR_NONE) { 03645 *ref_spad_count = ref_spad_count_int; 03646 *is_aperture_spads = is_aperture_spads_int; 03647 Data.RefSpadsInitialised = 1; 03648 Data.ReferenceSpadCount = (uint8_t)(*ref_spad_count); 03649 Data.ReferenceSpadType = *is_aperture_spads; 03650 } 03651 03652 return status; 03653 } 03654 03655 VL53L0X_Error VL53L0X::VL53L0X_set_reference_spads(uint32_t count, uint8_t is_aperture_spads) 03656 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 03657 uint32_t current_spad_index = 0; 03658 uint8_t start_select = 0xB4; 03659 uint32_t spad_array_size = 6; 03660 uint32_t max_spad_count = 44; 03661 uint32_t last_spad_index; 03662 uint32_t index; 03663 03664 /* 03665 * This function applies a requested number of reference spads, either 03666 * aperture or 03667 * non-aperture, as requested. 03668 * The good spad map will be applied. 03669 */ 03670 03671 status = VL53L0X_write_byte( 0xFF, 0x01); 03672 03673 if (status == VL53L0X_ERROR_NONE) { 03674 status = VL53L0X_write_byte(VL53L0X_REG_DYNAMIC_SPAD_REF_EN_START_OFFSET, 0x00); 03675 } 03676 03677 if (status == VL53L0X_ERROR_NONE) { 03678 status = VL53L0X_write_byte( VL53L0X_REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD, 0x2C); 03679 } 03680 03681 if (status == VL53L0X_ERROR_NONE) { 03682 status = VL53L0X_write_byte( 0xFF, 0x00); 03683 } 03684 03685 if (status == VL53L0X_ERROR_NONE) { 03686 status = VL53L0X_write_byte(VL53L0X_REG_GLOBAL_CONFIG_REF_EN_START_SELECT, 03687 start_select); 03688 } 03689 03690 for (index = 0; index < spad_array_size; index++) { 03691 Data.RefSpadEnables[index] = 0; 03692 } 03693 03694 if (is_aperture_spads) { 03695 /* Increment to the first APERTURE spad */ 03696 while ((is_aperture(start_select + current_spad_index) == 0) && 03697 (current_spad_index < max_spad_count)) { 03698 current_spad_index++; 03699 } 03700 } 03701 status = enable_ref_spads(is_aperture_spads, 03702 Data.RefGoodSpadMap, 03703 Data.RefSpadEnables, 03704 spad_array_size, 03705 start_select, 03706 current_spad_index, 03707 count, 03708 &last_spad_index); 03709 03710 if (status == VL53L0X_ERROR_NONE) { 03711 Data.RefSpadsInitialised = 1; 03712 Data.ReferenceSpadCount = (uint8_t)(count); 03713 Data.ReferenceSpadType = is_aperture_spads; 03714 } 03715 03716 return status; 03717 } 03718 03719 VL53L0X_Error VL53L0X::VL53L0X_perform_ref_calibration( uint8_t *p_vhv_settings, 03720 uint8_t *p_phase_cal) 03721 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 03722 03723 status = VL53L0X_perform_ref_calibration( p_vhv_settings, p_phase_cal, 1); 03724 03725 return status; 03726 } 03727 03728 VL53L0X_Error VL53L0X::VL53L0X_perform_ref_spad_management(uint32_t *ref_spad_count, uint8_t *is_aperture_spads) 03729 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 03730 03731 status = wrapped_VL53L0X_perform_ref_spad_management( ref_spad_count, 03732 is_aperture_spads); 03733 03734 return status; 03735 } 03736 03737 /* Group PAL Init Functions */ 03738 VL53L0X_Error VL53L0X::VL53L0X_set_device_address( uint8_t device_address) 03739 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 03740 03741 status = VL53L0X_write_byte( VL53L0X_REG_I2C_SLAVE_DEVICE_ADDRESS, 03742 device_address / 2); 03743 return status; 03744 } 03745 03746 VL53L0X_Error VL53L0X::VL53L0X_set_gpio_config( uint8_t pin, 03747 VL53L0X_DeviceModes device_mode, VL53L0X_GpioFunctionality functionality, 03748 VL53L0X_InterruptPolarity polarity) 03749 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 03750 uint8_t data; 03751 03752 if (pin != 0) { 03753 status = VL53L0X_ERROR_GPIO_NOT_EXISTING; 03754 } else if (device_mode == VL53L0X_DEVICEMODE_GPIO_DRIVE) { 03755 if (polarity == VL53L0X_INTERRUPTPOLARITY_LOW) { 03756 data = 0x10; 03757 } else {data = 1;} 03758 03759 status = VL53L0X_write_byte(VL53L0X_REG_GPIO_HV_MUX_ACTIVE_HIGH, data); 03760 03761 } else { 03762 if (device_mode == VL53L0X_DEVICEMODE_GPIO_OSC) { 03763 03764 status |= VL53L0X_write_byte( 0xff, 0x01); 03765 status |= VL53L0X_write_byte( 0x00, 0x00); 03766 status |= VL53L0X_write_byte( 0xff, 0x00); 03767 status |= VL53L0X_write_byte( 0x80, 0x01); 03768 status |= VL53L0X_write_byte( 0x85, 0x02); 03769 status |= VL53L0X_write_byte( 0xff, 0x04); 03770 status |= VL53L0X_write_byte( 0xcd, 0x00); 03771 status |= VL53L0X_write_byte( 0xcc, 0x11); 03772 status |= VL53L0X_write_byte( 0xff, 0x07); 03773 status |= VL53L0X_write_byte( 0xbe, 0x00); 03774 status |= VL53L0X_write_byte( 0xff, 0x06); 03775 status |= VL53L0X_write_byte( 0xcc, 0x09); 03776 status |= VL53L0X_write_byte( 0xff, 0x00); 03777 status |= VL53L0X_write_byte( 0xff, 0x01); 03778 status |= VL53L0X_write_byte( 0x00, 0x00); 03779 03780 } else { 03781 03782 if (status == VL53L0X_ERROR_NONE) { 03783 switch (functionality) { 03784 case VL53L0X_GPIOFUNCTIONALITY_OFF: 03785 data = 0x00; 03786 break; 03787 case VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_LOW: 03788 data = 0x01; 03789 break; 03790 case VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_HIGH: 03791 data = 0x02; 03792 break; 03793 case VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_OUT: 03794 data = 0x03; 03795 break; 03796 case VL53L0X_GPIOFUNCTIONALITY_NEW_MEASURE_READY: 03797 data = 0x04; 03798 break; 03799 default: 03800 status = VL53L0X_ERROR_GPIO_FUNCTIONALITY_NOT_SUPPORTED; 03801 } 03802 } 03803 03804 if (status == VL53L0X_ERROR_NONE) { 03805 status = VL53L0X_write_byte(VL53L0X_REG_SYSTEM_INTERRUPT_CONFIG_GPIO, data); 03806 } 03807 03808 if (status == VL53L0X_ERROR_NONE) { 03809 if (polarity == VL53L0X_INTERRUPTPOLARITY_LOW) { 03810 data = 0; 03811 } else { data = (uint8_t)(1 << 4); } 03812 status = VL53L0X_update_byte(VL53L0X_REG_GPIO_HV_MUX_ACTIVE_HIGH, 0xEF, data); 03813 } 03814 03815 if (status == VL53L0X_ERROR_NONE) { 03816 Data.Pin0GpioFunctionality = functionality; } 03817 03818 if (status == VL53L0X_ERROR_NONE) { status = VL53L0X_clear_interrupt_mask( 0); } 03819 } 03820 } 03821 return status; 03822 } 03823 03824 VL53L0X_Error VL53L0X::VL53L0X_get_fraction_enable( uint8_t *p_enabled) 03825 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 03826 status = VL53L0X_read_byte( VL53L0X_REG_SYSTEM_RANGE_CONFIG, p_enabled); 03827 if (status == VL53L0X_ERROR_NONE) { *p_enabled = (*p_enabled & 1); } 03828 return status; 03829 } 03830 03831 uint16_t VL53L0X::VL53L0X_encode_timeout (uint32_t timeout_macro_clks) 03832 { /*!Encode timeout in macro periods in (LSByte * 2^MSByte) + 1 format*/ 03833 03834 uint16_t encoded_timeout = 0; 03835 uint32_t ls_byte = 0; 03836 uint16_t ms_byte = 0; 03837 03838 if (timeout_macro_clks > 0) { 03839 ls_byte = timeout_macro_clks - 1; 03840 03841 while ((ls_byte & 0xFFFFFF00) > 0) { 03842 ls_byte = ls_byte >> 1; 03843 ms_byte++; 03844 } 03845 encoded_timeout = (ms_byte << 8) + (uint16_t)(ls_byte & 0x000000FF); 03846 } 03847 return encoded_timeout; 03848 } 03849 03850 VL53L0X_Error VL53L0X::set_sequence_step_timeout(VL53L0X_SequenceStepId sequence_step_id, 03851 uint32_t timeout_micro_secs) 03852 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 03853 uint8_t current_vcsel_pulse_period_p_clk; 03854 uint8_t msrc_encoded_time_out; 03855 uint16_t pre_range_encoded_time_out; 03856 uint16_t pre_range_time_out_m_clks; 03857 uint16_t msrc_range_time_out_m_clks; 03858 uint32_t final_range_time_out_m_clks; 03859 uint16_t final_range_encoded_time_out; 03860 VL53L0X_SchedulerSequenceSteps_t scheduler_sequence_steps; 03861 03862 if ((sequence_step_id == VL53L0X_SEQUENCESTEP_TCC) || 03863 (sequence_step_id == VL53L0X_SEQUENCESTEP_DSS) || 03864 (sequence_step_id == VL53L0X_SEQUENCESTEP_MSRC)) { 03865 03866 status = VL53L0X_get_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_PRE_RANGE, 03867 ¤t_vcsel_pulse_period_p_clk); 03868 03869 if (status == VL53L0X_ERROR_NONE) { 03870 msrc_range_time_out_m_clks = VL53L0X_calc_timeout_mclks(timeout_micro_secs, 03871 (uint8_t)current_vcsel_pulse_period_p_clk); 03872 03873 if (msrc_range_time_out_m_clks > 256) { 03874 msrc_encoded_time_out = 255; 03875 } else { 03876 msrc_encoded_time_out = 03877 (uint8_t)msrc_range_time_out_m_clks - 1; 03878 } 03879 Data.LastEncodedTimeout = msrc_encoded_time_out; 03880 } 03881 03882 if (status == VL53L0X_ERROR_NONE) { 03883 status = VL53L0X_write_byte(VL53L0X_REG_MSRC_CONFIG_TIMEOUT_MACROP, 03884 msrc_encoded_time_out); 03885 } 03886 } else { 03887 03888 if (sequence_step_id == VL53L0X_SEQUENCESTEP_PRE_RANGE) { 03889 03890 if (status == VL53L0X_ERROR_NONE) { 03891 status = VL53L0X_get_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_PRE_RANGE, 03892 ¤t_vcsel_pulse_period_p_clk); 03893 pre_range_time_out_m_clks = 03894 VL53L0X_calc_timeout_mclks(timeout_micro_secs, 03895 (uint8_t)current_vcsel_pulse_period_p_clk); 03896 pre_range_encoded_time_out = VL53L0X_encode_timeout (pre_range_time_out_m_clks); 03897 Data.LastEncodedTimeout = pre_range_encoded_time_out; 03898 } 03899 03900 if (status == VL53L0X_ERROR_NONE) { 03901 status = VL53L0X_write_word(VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI, 03902 pre_range_encoded_time_out); 03903 } 03904 03905 if (status == VL53L0X_ERROR_NONE) { 03906 Data.PreRangeTimeout_us = timeout_micro_secs; 03907 } 03908 } else if (sequence_step_id == VL53L0X_SEQUENCESTEP_FINAL_RANGE) { 03909 03910 /* For the final range timeout, the pre-range timeout 03911 * must be added. To do this both final and pre-range 03912 * timeouts must be expressed in macro periods MClks 03913 * because they have different vcsel periods. */ 03914 03915 VL53L0X_get_sequence_step_enables(&scheduler_sequence_steps); 03916 pre_range_time_out_m_clks = 0; 03917 if (scheduler_sequence_steps.PreRangeOn) { 03918 03919 /* Retrieve PRE-RANGE VCSEL Period */ 03920 status = VL53L0X_get_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_PRE_RANGE, 03921 ¤t_vcsel_pulse_period_p_clk); 03922 03923 /* Retrieve PRE-RANGE Timeout in Macro periods 03924 * (MCLKS) */ 03925 if (status == VL53L0X_ERROR_NONE) { 03926 status = VL53L0X_read_word( 0x51, &pre_range_encoded_time_out); 03927 pre_range_time_out_m_clks = 03928 VL53L0X_decode_timeout ( pre_range_encoded_time_out); 03929 } 03930 } 03931 03932 /* Calculate FINAL RANGE Timeout in Macro Periods 03933 * (MCLKS) and add PRE-RANGE value 03934 */ 03935 if (status == VL53L0X_ERROR_NONE) { 03936 status = VL53L0X_get_vcsel_pulse_period( VL53L0X_VCSEL_PERIOD_FINAL_RANGE, 03937 ¤t_vcsel_pulse_period_p_clk); 03938 } 03939 if (status == VL53L0X_ERROR_NONE) { 03940 final_range_time_out_m_clks = 03941 VL53L0X_calc_timeout_mclks( timeout_micro_secs, 03942 (uint8_t) current_vcsel_pulse_period_p_clk); 03943 03944 final_range_time_out_m_clks += pre_range_time_out_m_clks; 03945 final_range_encoded_time_out = 03946 VL53L0X_encode_timeout (final_range_time_out_m_clks); 03947 03948 if (status == VL53L0X_ERROR_NONE) { 03949 status = VL53L0X_write_word( 0x71, final_range_encoded_time_out); 03950 } 03951 03952 if (status == VL53L0X_ERROR_NONE) { 03953 Data.FinalRangeTimeout_us = timeout_micro_secs; 03954 } 03955 } 03956 } else { 03957 status = VL53L0X_ERROR_INVALID_PARAMS; 03958 } 03959 } 03960 return status; 03961 } 03962 03963 VL53L0X_Error VL53L0X::wrapped_VL53L0X_set_measurement_timing_budget_us(uint32_t measurement_timing_budget_us) 03964 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 03965 uint32_t final_range_timing_budget_us; 03966 VL53L0X_SchedulerSequenceSteps_t scheduler_sequence_steps; 03967 uint32_t msrc_dcc_tcc_timeout_us = 2000; 03968 uint32_t start_overhead_us = 1910; 03969 uint32_t end_overhead_us = 960; 03970 uint32_t msrc_overhead_us = 660; 03971 uint32_t tcc_overhead_us = 590; 03972 uint32_t dss_overhead_us = 690; 03973 uint32_t pre_range_overhead_us = 660; 03974 uint32_t final_range_overhead_us = 550; 03975 uint32_t pre_range_timeout_us = 0; 03976 uint32_t c_min_timing_budget_us = 20000; 03977 uint32_t sub_timeout = 0; 03978 03979 if (measurement_timing_budget_us < c_min_timing_budget_us) 03980 { status = VL53L0X_ERROR_INVALID_PARAMS; 03981 return status; 03982 } 03983 03984 final_range_timing_budget_us = 03985 measurement_timing_budget_us - (start_overhead_us + end_overhead_us); 03986 03987 status = VL53L0X_get_sequence_step_enables( &scheduler_sequence_steps); 03988 03989 if (status == VL53L0X_ERROR_NONE && 03990 (scheduler_sequence_steps.TccOn || 03991 scheduler_sequence_steps.MsrcOn || 03992 scheduler_sequence_steps.DssOn)) { 03993 03994 /* TCC, MSRC and DSS all share the same timeout */ 03995 status = get_sequence_step_timeout( VL53L0X_SEQUENCESTEP_MSRC, 03996 &msrc_dcc_tcc_timeout_us); 03997 03998 /* Subtract the TCC, MSRC and DSS timeouts if they are 03999 * enabled. */ 04000 04001 if (status != VL53L0X_ERROR_NONE) { 04002 return status; 04003 } 04004 04005 /* TCC */ 04006 if (scheduler_sequence_steps.TccOn) { 04007 04008 sub_timeout = msrc_dcc_tcc_timeout_us 04009 + tcc_overhead_us; 04010 04011 if (sub_timeout < 04012 final_range_timing_budget_us) { 04013 final_range_timing_budget_us -= 04014 sub_timeout; 04015 } else { 04016 /* Requested timeout too big. */ 04017 status = VL53L0X_ERROR_INVALID_PARAMS; 04018 } 04019 } 04020 04021 if (status != VL53L0X_ERROR_NONE) {return status;} 04022 04023 /* DSS */ 04024 if (scheduler_sequence_steps.DssOn) { 04025 04026 sub_timeout = 2 * (msrc_dcc_tcc_timeout_us + 04027 dss_overhead_us); 04028 04029 if (sub_timeout < final_range_timing_budget_us) { 04030 final_range_timing_budget_us 04031 -= sub_timeout; 04032 } else { 04033 /* Requested timeout too big. */ 04034 status = VL53L0X_ERROR_INVALID_PARAMS; 04035 } 04036 } else if (scheduler_sequence_steps.MsrcOn) { 04037 /* MSRC */ 04038 sub_timeout = msrc_dcc_tcc_timeout_us + 04039 msrc_overhead_us; 04040 04041 if (sub_timeout < final_range_timing_budget_us) { 04042 final_range_timing_budget_us 04043 -= sub_timeout; 04044 } else { 04045 /* Requested timeout too big. */ 04046 status = VL53L0X_ERROR_INVALID_PARAMS; 04047 } 04048 } 04049 } 04050 04051 if (status != VL53L0X_ERROR_NONE) { return status; } 04052 04053 if (scheduler_sequence_steps.PreRangeOn) { 04054 04055 /* Subtract the Pre-range timeout if enabled. */ 04056 04057 status = get_sequence_step_timeout(VL53L0X_SEQUENCESTEP_PRE_RANGE, 04058 &pre_range_timeout_us); 04059 04060 sub_timeout = pre_range_timeout_us + 04061 pre_range_overhead_us; 04062 04063 if (sub_timeout < final_range_timing_budget_us) { 04064 final_range_timing_budget_us -= sub_timeout; 04065 } else { 04066 /* Requested timeout too big. */ 04067 status = VL53L0X_ERROR_INVALID_PARAMS; 04068 } 04069 } 04070 04071 if (status == VL53L0X_ERROR_NONE && 04072 scheduler_sequence_steps.FinalRangeOn) { 04073 04074 final_range_timing_budget_us -= 04075 final_range_overhead_us; 04076 04077 /* Final Range Timeout 04078 * Note that the final range timeout is determined by the timing 04079 * budget and the sum of all other timeouts within the sequence. 04080 * If there is no room for the final range timeout, then an error 04081 * will be set. Otherwise the remaining time will be applied to 04082 * the final range. 04083 */ 04084 status = set_sequence_step_timeout(VL53L0X_SEQUENCESTEP_FINAL_RANGE, 04085 final_range_timing_budget_us); 04086 CurrentParameters .MeasurementTimingBudget_us = measurement_timing_budget_us; 04087 } 04088 04089 return status; 04090 } 04091 04092 VL53L0X_Error VL53L0X::VL53L0X_set_measurement_timing_budget_us(uint32_t measurement_timing_budget_us) 04093 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 04094 status = wrapped_VL53L0X_set_measurement_timing_budget_us(measurement_timing_budget_us); 04095 return status; 04096 } 04097 04098 VL53L0X_Error VL53L0X::VL53L0X_set_sequence_step_enable(VL53L0X_SequenceStepId sequence_step_id, uint8_t sequence_step_enabled) 04099 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 04100 uint8_t sequence_config = 0; 04101 uint8_t sequence_config_new = 0; 04102 uint32_t measurement_timing_budget_us; 04103 04104 status = VL53L0X_read_byte( VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, &sequence_config); 04105 04106 sequence_config_new = sequence_config; 04107 04108 if (status == VL53L0X_ERROR_NONE) { 04109 if (sequence_step_enabled == 1) { 04110 04111 /* Enable requested sequence step 04112 */ 04113 switch (sequence_step_id) { 04114 case VL53L0X_SEQUENCESTEP_TCC: 04115 sequence_config_new |= 0x10; 04116 break; 04117 case VL53L0X_SEQUENCESTEP_DSS: 04118 sequence_config_new |= 0x28; 04119 break; 04120 case VL53L0X_SEQUENCESTEP_MSRC: 04121 sequence_config_new |= 0x04; 04122 break; 04123 case VL53L0X_SEQUENCESTEP_PRE_RANGE: 04124 sequence_config_new |= 0x40; 04125 break; 04126 case VL53L0X_SEQUENCESTEP_FINAL_RANGE: 04127 sequence_config_new |= 0x80; 04128 break; 04129 default: 04130 status = VL53L0X_ERROR_INVALID_PARAMS; 04131 } 04132 } else { 04133 /* Disable requested sequence step */ 04134 switch (sequence_step_id) { 04135 case VL53L0X_SEQUENCESTEP_TCC: 04136 sequence_config_new &= 0xef; 04137 break; 04138 case VL53L0X_SEQUENCESTEP_DSS: 04139 sequence_config_new &= 0xd7; 04140 break; 04141 case VL53L0X_SEQUENCESTEP_MSRC: 04142 sequence_config_new &= 0xfb; 04143 break; 04144 case VL53L0X_SEQUENCESTEP_PRE_RANGE: 04145 sequence_config_new &= 0xbf; 04146 break; 04147 case VL53L0X_SEQUENCESTEP_FINAL_RANGE: 04148 sequence_config_new &= 0x7f; 04149 break; 04150 default: 04151 status = VL53L0X_ERROR_INVALID_PARAMS; 04152 } 04153 } 04154 } 04155 04156 if (sequence_config_new != sequence_config) { 04157 /* Apply New Setting */ 04158 if (status == VL53L0X_ERROR_NONE) { 04159 status = VL53L0X_write_byte(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, sequence_config_new); 04160 } 04161 if (status == VL53L0X_ERROR_NONE) { 04162 Data.SequenceConfig = sequence_config_new;} 04163 04164 /* Recalculate timing budget */ 04165 if (status == VL53L0X_ERROR_NONE) { 04166 measurement_timing_budget_us = CurrentParameters .MeasurementTimingBudget_us ; 04167 VL53L0X_set_measurement_timing_budget_us(measurement_timing_budget_us); 04168 } 04169 } 04170 04171 return status; 04172 } 04173 04174 VL53L0X_Error VL53L0X::VL53L0X_set_limit_check_enable( uint16_t limit_check_id, 04175 uint8_t limit_check_enable) 04176 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 04177 FixPoint1616_t temp_fix1616 = 0; 04178 uint8_t limit_check_enable_int = 0; 04179 uint8_t limit_check_disable = 0; 04180 uint8_t temp8; 04181 04182 if (limit_check_id >= VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS) { 04183 status = VL53L0X_ERROR_INVALID_PARAMS; 04184 } else { 04185 if (limit_check_enable == 0) { 04186 temp_fix1616 = 0; 04187 limit_check_enable_int = 0; 04188 limit_check_disable = 1; 04189 } else { 04190 temp_fix1616 = CurrentParameters .LimitChecksValue [limit_check_id]; 04191 limit_check_disable = 0; 04192 /* this to be sure to have either 0 or 1 */ 04193 limit_check_enable_int = 1; 04194 } 04195 04196 switch (limit_check_id) { 04197 04198 case VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE: 04199 /* internal computation: */ 04200 CurrentParameters .LimitChecksEnable [VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE] = limit_check_enable_int; 04201 break; 04202 04203 case VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE: 04204 status = VL53L0X_write_word( VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT, 04205 VL53L0X_FP1616TOFP97(temp_fix1616)); 04206 break; 04207 04208 case VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP:/* internal computation: */ 04209 CurrentParameters .LimitChecksEnable [VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP] = limit_check_enable_int; 04210 break; 04211 04212 case VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD:/* internal computation: */ 04213 CurrentParameters .LimitChecksEnable [VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD] = limit_check_enable_int; 04214 break; 04215 04216 case VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC: 04217 temp8 = (uint8_t)(limit_check_disable << 1); 04218 status = VL53L0X_update_byte(VL53L0X_REG_MSRC_CONFIG_CONTROL, 04219 0xFE, temp8); 04220 break; 04221 04222 case VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE: 04223 04224 temp8 = (uint8_t)(limit_check_disable << 4); 04225 status = VL53L0X_update_byte(VL53L0X_REG_MSRC_CONFIG_CONTROL, 04226 0xEF, temp8); 04227 break; 04228 04229 default: 04230 status = VL53L0X_ERROR_INVALID_PARAMS; 04231 } 04232 } 04233 04234 if (status == VL53L0X_ERROR_NONE) { 04235 if (limit_check_enable == 0) { 04236 CurrentParameters .LimitChecksEnable [limit_check_id] = 0; 04237 } else { 04238 CurrentParameters .LimitChecksEnable [limit_check_id] = 1; 04239 } 04240 } 04241 return status; 04242 } 04243 04244 VL53L0X_Error VL53L0X::VL53L0X_static_init(void) 04245 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 04246 VL53L0X_DeviceParameters_t current_parameters = {0}; 04247 uint8_t *p_tuning_setting_buffer; 04248 uint16_t tempword = 0; 04249 uint8_t tempbyte = 0; 04250 uint8_t use_internal_tuning_settings = 0; 04251 uint32_t count = 0; 04252 uint8_t is_aperture_spads = 0; 04253 uint32_t ref_spad_count = 0; 04254 uint8_t aperture_spads = 0; 04255 uint8_t vcsel_pulse_period_pclk; 04256 uint32_t seq_timeout_micro_secs; 04257 04258 status = VL53L0X_get_info_from_device( 1); 04259 04260 /* set the ref spad from NVM */ 04261 count = (uint32_t)Data.ReferenceSpadCount; 04262 aperture_spads = Data.ReferenceSpadType; 04263 04264 /* NVM value invalid */ 04265 if ((aperture_spads > 1) || 04266 ((aperture_spads == 1) && (count > 32)) || 04267 ((aperture_spads == 0) && (count > 12))) { 04268 status = wrapped_VL53L0X_perform_ref_spad_management( &ref_spad_count, 04269 &is_aperture_spads); 04270 } else { 04271 status = VL53L0X_set_reference_spads( count, aperture_spads); 04272 } 04273 04274 /* Initialize tuning settings buffer to prevent compiler warning. */ 04275 p_tuning_setting_buffer = DefaultTuningSettings; 04276 04277 if (status == VL53L0X_ERROR_NONE) { 04278 use_internal_tuning_settings = Data.UseInternalTuningSettings; 04279 04280 if (use_internal_tuning_settings == 0) { 04281 p_tuning_setting_buffer = Data.pTuningSettingsPointer; } 04282 else { p_tuning_setting_buffer = DefaultTuningSettings; } 04283 04284 } 04285 04286 if (status == VL53L0X_ERROR_NONE) { 04287 status = VL53L0X_load_tuning_settings( p_tuning_setting_buffer); } 04288 04289 /* Set interrupt config to new sample ready */ 04290 if (status == VL53L0X_ERROR_NONE) { 04291 status = VL53L0X_set_gpio_config( 0, 0, 04292 VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY, 04293 VL53L0X_INTERRUPTPOLARITY_LOW); 04294 } 04295 04296 if (status == VL53L0X_ERROR_NONE) { 04297 status = VL53L0X_write_byte( 0xFF, 0x01); 04298 status |= VL53L0X_read_word ( 0x84, &tempword); 04299 status |= VL53L0X_write_byte( 0xFF, 0x00); 04300 } 04301 04302 if (status == VL53L0X_ERROR_NONE) { 04303 Data.OscFrequency_MHz = VL53L0X_FP412TOFP1616(tempword) ; 04304 } 04305 04306 /* After static init, some device parameters may be changed, 04307 * so update them */ 04308 if (status == VL53L0X_ERROR_NONE) { 04309 status = VL53L0X_get_device_parameters( ¤t_parameters); 04310 } 04311 04312 if (status == VL53L0X_ERROR_NONE) { 04313 status = VL53L0X_get_fraction_enable( &tempbyte); 04314 if (status == VL53L0X_ERROR_NONE) { 04315 Data.RangeFractionalEnable = tempbyte; 04316 } 04317 } 04318 04319 if (status == VL53L0X_ERROR_NONE) { 04320 CurrentParameters = current_parameters; 04321 } 04322 04323 /* read the sequence config and save it */ 04324 if (status == VL53L0X_ERROR_NONE) { 04325 status = VL53L0X_read_byte(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, &tempbyte); 04326 if (status == VL53L0X_ERROR_NONE) { 04327 Data.SequenceConfig = tempbyte; 04328 } 04329 } 04330 04331 /* Disable MSRC and TCC by default */ 04332 if (status == VL53L0X_ERROR_NONE) { 04333 status = VL53L0X_set_sequence_step_enable(VL53L0X_SEQUENCESTEP_TCC, 0); 04334 } 04335 04336 if (status == VL53L0X_ERROR_NONE) { 04337 status = VL53L0X_set_sequence_step_enable(VL53L0X_SEQUENCESTEP_MSRC, 0); 04338 } 04339 04340 /* Set PAL State to standby */ 04341 if (status == VL53L0X_ERROR_NONE) { 04342 Data.PalState = VL53L0X_STATE_IDLE; 04343 } 04344 04345 /* Store pre-range vcsel period */ 04346 if (status == VL53L0X_ERROR_NONE) { 04347 status = VL53L0X_get_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_PRE_RANGE,&vcsel_pulse_period_pclk); 04348 } 04349 04350 if (status == VL53L0X_ERROR_NONE) { 04351 Data.PreRangeVcselPulsePeriod = vcsel_pulse_period_pclk; 04352 } 04353 04354 /* Store final-range vcsel period */ 04355 if (status == VL53L0X_ERROR_NONE) { 04356 status = VL53L0X_get_vcsel_pulse_period(VL53L0X_VCSEL_PERIOD_FINAL_RANGE, 04357 &vcsel_pulse_period_pclk); 04358 } 04359 04360 if (status == VL53L0X_ERROR_NONE) { 04361 Data.FinalRangeVcselPulsePeriod = vcsel_pulse_period_pclk; 04362 } 04363 04364 /* Store pre-range timeout */ 04365 if (status == VL53L0X_ERROR_NONE) { 04366 status = get_sequence_step_timeout(VL53L0X_SEQUENCESTEP_PRE_RANGE,&seq_timeout_micro_secs); 04367 } 04368 04369 if (status == VL53L0X_ERROR_NONE) { 04370 Data.PreRangeTimeout_us = seq_timeout_micro_secs; 04371 } 04372 04373 /* Store final-range timeout */ 04374 if (status == VL53L0X_ERROR_NONE) { 04375 status = get_sequence_step_timeout(VL53L0X_SEQUENCESTEP_FINAL_RANGE,&seq_timeout_micro_secs); 04376 } 04377 04378 if (status == VL53L0X_ERROR_NONE) { 04379 Data.FinalRangeTimeout_us = seq_timeout_micro_secs; 04380 } 04381 return status; 04382 } 04383 04384 VL53L0X_Error VL53L0X::VL53L0X_stop_measurement(void) 04385 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 04386 04387 status = VL53L0X_write_byte( VL53L0X_REG_SYSRANGE_START, 04388 VL53L0X_REG_SYSRANGE_MODE_SINGLESHOT); 04389 status = VL53L0X_write_byte( 0xFF, 0x01); 04390 status = VL53L0X_write_byte( 0x00, 0x00); 04391 status = VL53L0X_write_byte( 0x91, 0x00); 04392 status = VL53L0X_write_byte( 0x00, 0x01); 04393 status = VL53L0X_write_byte( 0xFF, 0x00); 04394 04395 if (status == VL53L0X_ERROR_NONE) { 04396 /* Set PAL State to Idle */ 04397 Data.PalState = VL53L0X_STATE_IDLE; 04398 } 04399 04400 /* Check if need to apply interrupt settings */ 04401 if (status == VL53L0X_ERROR_NONE) { 04402 status = VL53L0X_check_and_load_interrupt_settings( 0); 04403 } 04404 return status; 04405 } 04406 04407 VL53L0X_Error VL53L0X::VL53L0X_get_stop_completed_status(uint32_t *p_stop_status) 04408 { VL53L0X_Error status = VL53L0X_ERROR_NONE; 04409 uint8_t byte = 0; 04410 04411 04412 status = VL53L0X_write_byte( 0xFF, 0x01); 04413 04414 if (status == VL53L0X_ERROR_NONE) { 04415 status = VL53L0X_read_byte( 0x04, &byte);} 04416 04417 if (status == VL53L0X_ERROR_NONE) { 04418 status = VL53L0X_write_byte( 0xFF, 0x0); } 04419 04420 *p_stop_status = byte; 04421 04422 if (byte == 0) { 04423 status = VL53L0X_write_byte( 0x80, 0x01); 04424 status = VL53L0X_write_byte( 0xFF, 0x01); 04425 status = VL53L0X_write_byte( 0x00, 0x00); 04426 status = VL53L0X_write_byte( 0x91,Data.StopVariable); 04427 status = VL53L0X_write_byte( 0x00, 0x01); 04428 status = VL53L0X_write_byte( 0xFF, 0x00); 04429 status = VL53L0X_write_byte( 0x80, 0x00); 04430 } 04431 return status; 04432 } 04433 04434 /******************************************************************************/ 04435 04436 /****************** Write and read functions from I2C *************************/ 04437 04438 VL53L0X_Error VL53L0X::VL53L0X_read_multi( uint8_t index, uint8_t *p_data, uint32_t count) 04439 { if (count >= VL53L0X_MAX_I2C_XFER_SIZE) { 04440 return VL53L0X_ERROR_INVALID_PARAMS;} 04441 else { return VL53L0X_i2c_read(index, p_data, (uint16_t)count); } 04442 } 04443 04444 VL53L0X_Error VL53L0X::VL53L0X_write_byte( uint8_t index, uint8_t data) 04445 { return VL53L0X_i2c_write(index, &data, 1); 04446 } 04447 04448 VL53L0X_Error VL53L0X::VL53L0X_write_word( uint8_t index, uint16_t data) 04449 { int status; 04450 uint8_t buffer[2]; 04451 04452 buffer[0] = data >> 8; 04453 buffer[1] = data & 0x00FF; 04454 status = VL53L0X_i2c_write(index, (uint8_t *)buffer, 2); 04455 return status; 04456 } 04457 04458 VL53L0X_Error VL53L0X::VL53L0X_write_dword( uint8_t index, uint32_t data) 04459 { int status; 04460 uint8_t buffer[4]; 04461 04462 buffer[0] = (data >> 24) & 0xFF; 04463 buffer[1] = (data >> 16) & 0xFF; 04464 buffer[2] = (data >> 8) & 0xFF; 04465 buffer[3] = (data >> 0) & 0xFF; 04466 status = VL53L0X_i2c_write(index, (uint8_t *)buffer, 4); 04467 return status; 04468 } 04469 04470 VL53L0X_Error VL53L0X::VL53L0X_read_byte( uint8_t index, uint8_t *p_data) 04471 { return VL53L0X_i2c_read(index, p_data, 1); } 04472 04473 VL53L0X_Error VL53L0X::VL53L0X_read_word( uint8_t index, uint16_t *p_data) 04474 { int status; 04475 uint8_t buffer[2] = {0, 0}; 04476 04477 status = VL53L0X_i2c_read(index, buffer, 2); 04478 if (!status) {*p_data = (buffer[0] << 8) + buffer[1];} 04479 return status; 04480 } 04481 04482 VL53L0X_Error VL53L0X::VL53L0X_read_dword( uint8_t index, uint32_t *p_data) 04483 { int status; 04484 uint8_t buffer[4] = {0, 0, 0, 0}; 04485 04486 status = VL53L0X_i2c_read(index, buffer, 4); 04487 if (!status) { *p_data = (buffer[0] << 24) + (buffer[1] << 16) + (buffer[2] << 8) + buffer[3]; } 04488 return status; 04489 } 04490 04491 VL53L0X_Error VL53L0X::VL53L0X_update_byte( uint8_t index, uint8_t and_data, uint8_t or_data) 04492 { int status; 04493 uint8_t buffer = 0; 04494 04495 /* read data direct onto buffer */ 04496 status = VL53L0X_i2c_read(index, &buffer, 1); 04497 if (!status) { 04498 buffer = (buffer & and_data) | or_data; 04499 status = VL53L0X_i2c_write(index, &buffer, (uint8_t)1); 04500 } 04501 return status; 04502 } 04503 04504 VL53L0X_Error VL53L0X::VL53L0X_i2c_write(uint8_t RegisterAddr, uint8_t *p_data, 04505 uint16_t NumByteToWrite) 04506 { /** Writes a buffer towards the I2C peripheral device. */ 04507 static uint8_t tmp[VL53L0X_MAX_I2C_XFER_SIZE]; 04508 04509 if(NumByteToWrite >= VL53L0X_MAX_I2C_XFER_SIZE) return -2; 04510 04511 /* First, send device address. Then, send data and STOP condition */ 04512 tmp[0] = RegisterAddr; 04513 memcpy(tmp+1, p_data, NumByteToWrite); 04514 04515 if (_dev_i2c->write(I2cDevAddr , (const char*)tmp, NumByteToWrite+1, false) != 0 ) 04516 { return -1; } 04517 return 0; 04518 } 04519 04520 VL53L0X_Error VL53L0X::VL53L0X_i2c_read(uint8_t RegisterAddr, uint8_t *p_data, uint16_t NumByteToRead) 04521 { /** Reads a buffer from the I2C peripheral device. */ 04522 04523 /* First Send device address, with no STOP condition */ 04524 int ret = _dev_i2c->write(I2cDevAddr , (const char*)&RegisterAddr, 1, true); 04525 04526 /* all ok ? then Read data, with STOP condition */ 04527 if (ret == 0) { ret = _dev_i2c->read(I2cDevAddr , (char*)p_data, NumByteToRead, false); } 04528 04529 if (ret != 0 ){ return -1; } 04530 return 0; 04531 } 04532
Generated on Thu Jul 14 2022 23:17:38 by
1.7.2