Condensed Version of Public VL53L0X

Dependents:   ToF-Only-Tryout

Committer:
sepp_nepp
Date:
Wed Apr 10 19:03:16 2019 +0000
Revision:
12:aa177f0e4c10
Parent:
11:c6f95a42d4d7
Troy a gain

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nikapov 0:a1a69d32f310 1 /**
nikapov 0:a1a69d32f310 2 ******************************************************************************
sepp_nepp 11:c6f95a42d4d7 3 * @file Class.cpp
nikapov 0:a1a69d32f310 4 * @author IMG
nikapov 0:a1a69d32f310 5 * @version V0.0.1
nikapov 0:a1a69d32f310 6 * @date 28-June-2016
nikapov 0:a1a69d32f310 7 * @brief Implementation file for the VL53L0X driver class
nikapov 0:a1a69d32f310 8 ******************************************************************************
nikapov 0:a1a69d32f310 9 * @attention
nikapov 0:a1a69d32f310 10 *
nikapov 0:a1a69d32f310 11 * <h2><center>&copy; COPYRIGHT(c) 2016 STMicroelectronics</center></h2>
nikapov 0:a1a69d32f310 12 *
sepp_nepp 7:41cbc431e1f4 13 * Redistribution and use in source and binary forms,with or without modification,
nikapov 0:a1a69d32f310 14 * are permitted provided that the following conditions are met:
nikapov 0:a1a69d32f310 15 * 1. Redistributions of source code must retain the above copyright notice,
nikapov 0:a1a69d32f310 16 * this list of conditions and the following disclaimer.
nikapov 0:a1a69d32f310 17 * 2. Redistributions in binary form must reproduce the above copyright notice,
nikapov 0:a1a69d32f310 18 * this list of conditions and the following disclaimer in the documentation
nikapov 0:a1a69d32f310 19 * and/or other materials provided with the distribution.
nikapov 0:a1a69d32f310 20 * 3. Neither the name of STMicroelectronics nor the names of its contributors
nikapov 0:a1a69d32f310 21 * may be used to endorse or promote products derived from this software
nikapov 0:a1a69d32f310 22 * without specific prior written permission.
nikapov 0:a1a69d32f310 23 *
nikapov 0:a1a69d32f310 24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
sepp_nepp 7:41cbc431e1f4 25 * AND ANY EXPRESS OR IMPLIED WARRANTIES,INCLUDING,BUT NOT LIMITED TO,THE
nikapov 0:a1a69d32f310 26 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
nikapov 0:a1a69d32f310 27 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
sepp_nepp 7:41cbc431e1f4 28 * FOR ANY DIRECT,INDIRECT,INCIDENTAL,SPECIAL,EXEMPLARY,OR CONSEQUENTIAL
sepp_nepp 7:41cbc431e1f4 29 * DAMAGES (INCLUDING,BUT NOT LIMITED TO,PROCUREMENT OF SUBSTITUTE GOODS OR
sepp_nepp 7:41cbc431e1f4 30 * SERVICES; LOSS OF USE,DATA,OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
sepp_nepp 7:41cbc431e1f4 31 * CAUSED AND ON ANY THEORY OF LIABILITY,WHETHER IN CONTRACT,STRICT LIABILITY,
nikapov 0:a1a69d32f310 32 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
sepp_nepp 7:41cbc431e1f4 33 * OF THIS SOFTWARE,EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
nikapov 0:a1a69d32f310 34 *
nikapov 0:a1a69d32f310 35 ******************************************************************************
nikapov 0:a1a69d32f310 36 */
nikapov 0:a1a69d32f310 37
sepp_nepp 7:41cbc431e1f4 38 // Some example regex that were used to replace useless macros
sepp_nepp 7:41cbc431e1f4 39 // \QVL53L0X_SETARRAYPARAMETERFIELD(\E([A-Z\d]+)[[:punct:]](\s*)([A-Z\d_]+)[[:punct:]](\s*)([A-Z\d_]+)\Q);\E
sepp_nepp 11:c6f95a42d4d7 40 // _device->CurrParams.\1[\3] = \5;
sepp_nepp 11:c6f95a42d4d7 41 // to replace this "#define VL53L0X_SETARRAYPARAMETERFIELD(field, index, value)" by "_device->CurrParams.field[index] = value"
sepp_nepp 11:c6f95a42d4d7 42
sepp_nepp 11:c6f95a42d4d7 43 // to replace "Read_Byte(0x90,&module_id);" by "module_id = Read_Byte(0x90);" search and replace
sepp_nepp 11:c6f95a42d4d7 44 // \QRead_Byte(\E([A-Za-z_\d]+)[[:punct:]](\s*)\Q&\E([A-Za-z\d_]+)\Q);\E
sepp_nepp 11:c6f95a42d4d7 45 // \3 = Read_Byte\(\1\);
sepp_nepp 7:41cbc431e1f4 46
nikapov 0:a1a69d32f310 47 /* Includes */
nikapov 0:a1a69d32f310 48 #include <stdlib.h>
nikapov 0:a1a69d32f310 49 #include "VL53L0X.h"
sepp_nepp 10:cd251e0fc2fd 50 #include "VL53L0X_tuning.h"
sepp_nepp 7:41cbc431e1f4 51
sepp_nepp 12:aa177f0e4c10 52 /******************************************************************************/
sepp_nepp 12:aa177f0e4c10 53 /******************************************************************************/
sepp_nepp 12:aa177f0e4c10 54 /****************** All initialization functions *************************/
sepp_nepp 12:aa177f0e4c10 55 /******************************************************************************/
sepp_nepp 12:aa177f0e4c10 56 /******************************************************************************/
sepp_nepp 12:aa177f0e4c10 57
sepp_nepp 11:c6f95a42d4d7 58 // Function Data_init and Init_Sensor is united into Start_Sensor
sepp_nepp 11:c6f95a42d4d7 59 VL53L0X_Error VL53L0X::Start_Sensor(uint8_t new_addr)
sepp_nepp 11:c6f95a42d4d7 60 { ErrState = VL53L0X_OK;
sepp_nepp 11:c6f95a42d4d7 61
sepp_nepp 11:c6f95a42d4d7 62 if (_gpio0) { // Can the shutdown pin be controlled?
sepp_nepp 11:c6f95a42d4d7 63 *_gpio0 = 0; wait_ms(1); // quick shutdown
sepp_nepp 11:c6f95a42d4d7 64 *_gpio0 = 1; wait_ms(10); // and back ON again
sepp_nepp 11:c6f95a42d4d7 65 }
sepp_nepp 11:c6f95a42d4d7 66
sepp_nepp 11:c6f95a42d4d7 67 /* Setup the I2C bus. By default the I2C is running at 1V8 if you
sepp_nepp 11:c6f95a42d4d7 68 * want to change it you need to include this define at compilation level. */
sepp_nepp 11:c6f95a42d4d7 69 #ifdef USE_I2C_2V8
sepp_nepp 11:c6f95a42d4d7 70 VL53L0X_UpdateByte(REG_VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV, 0xFE, 0x01);
sepp_nepp 11:c6f95a42d4d7 71 #endif
sepp_nepp 11:c6f95a42d4d7 72 /* Set I2C standard mode */
sepp_nepp 11:c6f95a42d4d7 73 Write_Byte(0x88,0x00);
sepp_nepp 11:c6f95a42d4d7 74
sepp_nepp 11:c6f95a42d4d7 75 // read and check the device ID from the ID register
sepp_nepp 11:c6f95a42d4d7 76 Device_Info.ProductType = Read_Byte(REG_IDENTIFICATION_MODEL_ID);
sepp_nepp 11:c6f95a42d4d7 77 if ( (ErrState == VL53L0X_OK) && (Device_Info.ProductType != 0xEEAA) )
sepp_nepp 11:c6f95a42d4d7 78 {return VL53L0X_ERROR_I2C_WRONG_DEV_ID; }
sepp_nepp 11:c6f95a42d4d7 79
sepp_nepp 11:c6f95a42d4d7 80 // reconfigure the address with a new address if requested
sepp_nepp 11:c6f95a42d4d7 81 if ( (ErrState == VL53L0X_OK) && (new_addr != VL53L0X_DEFAULT_ADDRESS) )
sepp_nepp 11:c6f95a42d4d7 82 { Write_Byte(REG_I2C_SLAVE_DEVICE_ADDRESS, new_addr / 2);
sepp_nepp 11:c6f95a42d4d7 83 I2cDevAddr = new_addr;
sepp_nepp 11:c6f95a42d4d7 84 }
sepp_nepp 11:c6f95a42d4d7 85 // quite if an error was raised
sepp_nepp 11:c6f95a42d4d7 86 if (ErrState != VL53L0X_OK) {return ErrState; }
sepp_nepp 11:c6f95a42d4d7 87
sepp_nepp 11:c6f95a42d4d7 88 /* Set Default static parameters
sepp_nepp 11:c6f95a42d4d7 89 *set first temporary values 9.44MHz * 65536 = 618660 */
sepp_nepp 12:aa177f0e4c10 90 OscFrequencyMHz = 618660;
sepp_nepp 12:aa177f0e4c10 91 RefSPADSInitialised = 0;
sepp_nepp 12:aa177f0e4c10 92
sepp_nepp 12:aa177f0e4c10 93 // Read All NVM from device
sepp_nepp 12:aa177f0e4c10 94 ReadNVMDataFromDeviceDone = 0;
sepp_nepp 12:aa177f0e4c10 95 Get_all_NVM_info_from_device();
sepp_nepp 11:c6f95a42d4d7 96
sepp_nepp 11:c6f95a42d4d7 97 #ifdef USE_IQC_STATION
sepp_nepp 12:aa177f0e4c10 98 // must make sure that first your read all NVM info used by the API */
sepp_nepp 11:c6f95a42d4d7 99 VL53L0X_Apply_Offset_Cal();
sepp_nepp 11:c6f95a42d4d7 100 #endif
sepp_nepp 11:c6f95a42d4d7 101
sepp_nepp 11:c6f95a42d4d7 102 /* Default value is 1000 for Linearity Corrective Gain */
sepp_nepp 11:c6f95a42d4d7 103 LinearityCorrectiveGain = 1000;
sepp_nepp 11:c6f95a42d4d7 104
sepp_nepp 11:c6f95a42d4d7 105 /* Dmax default Parameter */
sepp_nepp 11:c6f95a42d4d7 106 DmaxCalRangeMilliMeter = 400;
sepp_nepp 11:c6f95a42d4d7 107 DmaxCalSignalRateRtnMHz = (TFP1616)((0x00016B85)); /* 1.42 No Cover Glass*/
sepp_nepp 11:c6f95a42d4d7 108
sepp_nepp 11:c6f95a42d4d7 109 /* Get default parameters */
sepp_nepp 11:c6f95a42d4d7 110 CurrParams = Get_device_parameters();
sepp_nepp 11:c6f95a42d4d7 111
sepp_nepp 11:c6f95a42d4d7 112 /* Set Default Xtalk_CompRate_MHz to 0 */
sepp_nepp 11:c6f95a42d4d7 113 CurrParams.Xtalk_CompRate_MHz = 0;
sepp_nepp 11:c6f95a42d4d7 114
sepp_nepp 11:c6f95a42d4d7 115 /* initialize CurrParams values */
sepp_nepp 11:c6f95a42d4d7 116 CurrParams.DeviceMode = VL53L0X_DEVICEMODE_SINGLE_RANGING;
sepp_nepp 11:c6f95a42d4d7 117 CurrParams.HistogramMode = VL53L0X_HISTOGRAMMODE_DISABLED;
sepp_nepp 11:c6f95a42d4d7 118
sepp_nepp 11:c6f95a42d4d7 119 /* Sigma estimator variable */
sepp_nepp 11:c6f95a42d4d7 120 SigmaEstRefArray = 100;
sepp_nepp 11:c6f95a42d4d7 121 SigmaEstEffPulseWidth = 900;
sepp_nepp 11:c6f95a42d4d7 122 SigmaEstEffAmbWidth = 500;
sepp_nepp 11:c6f95a42d4d7 123 targetRefRate = 0x0A00; /* 20 MHz in 9:7 format */
sepp_nepp 11:c6f95a42d4d7 124
sepp_nepp 11:c6f95a42d4d7 125 /* Use internal default settings */
sepp_nepp 11:c6f95a42d4d7 126 UseInternalTuningSettings = 1;
sepp_nepp 11:c6f95a42d4d7 127 Write_Byte(0x80,0x01);
sepp_nepp 11:c6f95a42d4d7 128 Write_Byte(0xFF,0x01);
sepp_nepp 11:c6f95a42d4d7 129 Write_Byte(0x00,0x00);
sepp_nepp 11:c6f95a42d4d7 130 StopVariable = Read_Byte(0x91);
sepp_nepp 11:c6f95a42d4d7 131 Write_Byte(0x00,0x01);
sepp_nepp 11:c6f95a42d4d7 132 Write_Byte(0xFF,0x00);
sepp_nepp 11:c6f95a42d4d7 133 Write_Byte(0x80,0x00);
sepp_nepp 11:c6f95a42d4d7 134
sepp_nepp 11:c6f95a42d4d7 135 // quite if an error was raised
sepp_nepp 11:c6f95a42d4d7 136 if (ErrState != VL53L0X_OK) {return ErrState; }
sepp_nepp 11:c6f95a42d4d7 137
sepp_nepp 11:c6f95a42d4d7 138 /* Disable the following SW-internal checks plaus set some values */
sepp_nepp 11:c6f95a42d4d7 139 CurrParams.Limit_Chk_En [VL53L0X_CHECKEN_SIG_REF_CLIP] = 0;
sepp_nepp 11:c6f95a42d4d7 140 CurrParams.Limit_Chk_Val[VL53L0X_CHECKEN_SIG_REF_CLIP] = (35 * 65536);
sepp_nepp 11:c6f95a42d4d7 141 CurrParams.Limit_Chk_En [VL53L0X_CHECKEN_RANGE_IGNORE_THRESHOLD] = 0;
sepp_nepp 11:c6f95a42d4d7 142 CurrParams.Limit_Chk_Val[VL53L0X_CHECKEN_RANGE_IGNORE_THRESHOLD] = 0;
sepp_nepp 11:c6f95a42d4d7 143
sepp_nepp 11:c6f95a42d4d7 144 /* Disable the following Device-Internal Checks: */
sepp_nepp 11:c6f95a42d4d7 145 CurrParams.Limit_Chk_En[VL53L0X_CHECKEN_SIG_RATE_MSRC] = 0;
sepp_nepp 11:c6f95a42d4d7 146 CurrParams.Limit_Chk_En[VL53L0X_CHECKEN_SIG_RATE_PRE_RANGE] = 0;
sepp_nepp 11:c6f95a42d4d7 147 Register_BitMask(REG_MSRC_CONFIG_CONTROL,0xEE, 0);
sepp_nepp 11:c6f95a42d4d7 148
sepp_nepp 11:c6f95a42d4d7 149 /* Only enable this internal Check : */
sepp_nepp 11:c6f95a42d4d7 150 CurrParams.Limit_Chk_En [VL53L0X_CHECKEN_SIGMA_FINAL_RANGE] = 1;
sepp_nepp 11:c6f95a42d4d7 151 CurrParams.Limit_Chk_Val[VL53L0X_CHECKEN_SIGMA_FINAL_RANGE] = (18 * 65536);
sepp_nepp 11:c6f95a42d4d7 152
sepp_nepp 11:c6f95a42d4d7 153 /* Plus Enable VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE check */
sepp_nepp 11:c6f95a42d4d7 154 Set_limit_chk_en(VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE,1);
sepp_nepp 11:c6f95a42d4d7 155
sepp_nepp 11:c6f95a42d4d7 156 if (ErrState == VL53L0X_OK) { /* 0.25 in FP1616 notation 65536 */
sepp_nepp 11:c6f95a42d4d7 157 Set_limit_chk_val(VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE,
sepp_nepp 11:c6f95a42d4d7 158 (TFP1616)(25 * 65536 / 100)); }
sepp_nepp 11:c6f95a42d4d7 159
sepp_nepp 11:c6f95a42d4d7 160 // quit if an error was raised
sepp_nepp 11:c6f95a42d4d7 161 if (ErrState != VL53L0X_OK) {return ErrState; }
sepp_nepp 11:c6f95a42d4d7 162
sepp_nepp 11:c6f95a42d4d7 163 // Preset the Config States
sepp_nepp 11:c6f95a42d4d7 164 SequenceConfig = 0xFF ;
sepp_nepp 11:c6f95a42d4d7 165
sepp_nepp 11:c6f95a42d4d7 166 /* Set Device state to tell that we are waiting for call to VL53L0X_StaticInit */
sepp_nepp 11:c6f95a42d4d7 167 Current_State = VL53L0X_STATE_WAIT_STATICINIT ;
sepp_nepp 11:c6f95a42d4d7 168
sepp_nepp 11:c6f95a42d4d7 169 Fill_device_info(); // Retrieve Silicon version, stored in Device_Info
sepp_nepp 11:c6f95a42d4d7 170
sepp_nepp 11:c6f95a42d4d7 171 uint32_t ref_SPAD_count;
sepp_nepp 11:c6f95a42d4d7 172 uint8_t is_aperture_SPADS;
sepp_nepp 11:c6f95a42d4d7 173 uint8_t vhv_settings;
sepp_nepp 11:c6f95a42d4d7 174 uint8_t phase_cal;
sepp_nepp 11:c6f95a42d4d7 175
sepp_nepp 12:aa177f0e4c10 176 // make sure the Get_all_NVM_info_from_device was called before calling static init
sepp_nepp 11:c6f95a42d4d7 177 if (ErrState == VL53L0X_OK) { Static_init(); } // Device Initialization
sepp_nepp 11:c6f95a42d4d7 178
sepp_nepp 11:c6f95a42d4d7 179 if (ErrState == VL53L0X_OK) { // Device Calibration
sepp_nepp 11:c6f95a42d4d7 180 Perf_Ref_calibration( &vhv_settings, &phase_cal, 1); }
sepp_nepp 11:c6f95a42d4d7 181
sepp_nepp 11:c6f95a42d4d7 182 if (ErrState == VL53L0X_OK) { // SPAD Configuration
sepp_nepp 11:c6f95a42d4d7 183 Perf_Ref_SPAD_management( &ref_SPAD_count, &is_aperture_SPADS); }
sepp_nepp 11:c6f95a42d4d7 184
sepp_nepp 11:c6f95a42d4d7 185 return ErrState;
sepp_nepp 11:c6f95a42d4d7 186 }
sepp_nepp 11:c6f95a42d4d7 187
sepp_nepp 11:c6f95a42d4d7 188 void VL53L0X::Fill_device_info()
sepp_nepp 11:c6f95a42d4d7 189 { uint8_t revision;
sepp_nepp 11:c6f95a42d4d7 190
sepp_nepp 11:c6f95a42d4d7 191 if (ErrState == VL53L0X_OK)
sepp_nepp 12:aa177f0e4c10 192 { if (ModuleId == 0)
sepp_nepp 11:c6f95a42d4d7 193 { revision = 0;
sepp_nepp 11:c6f95a42d4d7 194 strcpy(Device_Info.ProductId,""); }
sepp_nepp 11:c6f95a42d4d7 195 else
sepp_nepp 12:aa177f0e4c10 196 { revision = Revision;
sepp_nepp 12:aa177f0e4c10 197 strcpy(Device_Info.ProductId,ProductId);
sepp_nepp 11:c6f95a42d4d7 198 }
sepp_nepp 11:c6f95a42d4d7 199 if (revision == 0)
sepp_nepp 11:c6f95a42d4d7 200 { strcpy(Device_Info.Name,VL53L0X_STRING_DEVICE_INFO_NAME_TS0); }
sepp_nepp 11:c6f95a42d4d7 201 else if ((revision <= 34) && (revision != 32))
sepp_nepp 11:c6f95a42d4d7 202 { strcpy(Device_Info.Name,VL53L0X_STRING_DEVICE_INFO_NAME_TS1); }
sepp_nepp 11:c6f95a42d4d7 203 else if (revision < 39)
sepp_nepp 11:c6f95a42d4d7 204 { strcpy(Device_Info.Name,VL53L0X_STRING_DEVICE_INFO_NAME_TS2); }
sepp_nepp 11:c6f95a42d4d7 205 else { strcpy(Device_Info.Name,VL53L0X_STRING_DEVICE_INFO_NAME_ES1); }
sepp_nepp 11:c6f95a42d4d7 206 strcpy(Device_Info.Type,VL53L0X_STRING_DEVICE_INFO_TYPE);
sepp_nepp 11:c6f95a42d4d7 207 }
sepp_nepp 11:c6f95a42d4d7 208
sepp_nepp 11:c6f95a42d4d7 209 Device_Info.ProductRevisionMajor = 1;
sepp_nepp 11:c6f95a42d4d7 210 Device_Info.ProductRevisionMinor =
sepp_nepp 11:c6f95a42d4d7 211 (Read_Byte(REG_IDENTIFICATION_REVISION_ID) & 0xF0) >> 4;
nikapov 0:a1a69d32f310 212 }
sepp_nepp 11:c6f95a42d4d7 213
sepp_nepp 12:aa177f0e4c10 214 /* That was 'Get_info_from_device( unit8_t option)' where option was composed of
sepp_nepp 12:aa177f0e4c10 215 1, 2, 4 depending what info was requested.
sepp_nepp 12:aa177f0e4c10 216 It was decided to combine all that into a single Get ALL infos from the device
sepp_nepp 12:aa177f0e4c10 217 that is called once at Sensor Init so all the job is done.
sepp_nepp 12:aa177f0e4c10 218 In addition, originally values would first be read into local variables and then
sepp_nepp 12:aa177f0e4c10 219 only copied to class fields if no errors.
sepp_nepp 12:aa177f0e4c10 220 However, if at device initialization an error is raised the whole class instance
sepp_nepp 12:aa177f0e4c10 221 cannot be used anyway, so decided that we directly read into class fields. */
sepp_nepp 12:aa177f0e4c10 222
sepp_nepp 12:aa177f0e4c10 223 void VL53L0X::Get_all_NVM_info_from_device()
sepp_nepp 12:aa177f0e4c10 224 { uint32_t tmp_dword;
sepp_nepp 12:aa177f0e4c10 225 uint32_t offset_fixed1104_mm = 0;
sepp_nepp 12:aa177f0e4c10 226 int16_t offset_um = 0;
sepp_nepp 12:aa177f0e4c10 227 uint32_t dist_meas_tgt_fixed1104_mm = 400 << 4;
sepp_nepp 12:aa177f0e4c10 228 uint32_t dist_meas_fixed1104_400_mm = 0;
sepp_nepp 12:aa177f0e4c10 229 uint32_t signal_rate_meas_fixed1104_400_mm = 0;
sepp_nepp 12:aa177f0e4c10 230 int i;
sepp_nepp 12:aa177f0e4c10 231
sepp_nepp 12:aa177f0e4c10 232 /* This access is done only once after that a GetDeviceInfo or datainit is done*/
sepp_nepp 12:aa177f0e4c10 233 if (ReadNVMDataFromDeviceDone == 7) { return ; }
sepp_nepp 12:aa177f0e4c10 234
sepp_nepp 12:aa177f0e4c10 235 Write_Byte(0x80,0x01);
sepp_nepp 12:aa177f0e4c10 236 Write_Byte(0xFF,0x01);
sepp_nepp 12:aa177f0e4c10 237 Write_Byte(0x00,0x00);
sepp_nepp 12:aa177f0e4c10 238 Write_Byte(0xFF,0x06);
sepp_nepp 12:aa177f0e4c10 239 Register_BitMask(0x83,0xFF,0x04)
sepp_nepp 12:aa177f0e4c10 240 Write_Byte(0xFF,0x07);
sepp_nepp 12:aa177f0e4c10 241 Write_Byte(0x81,0x01);
sepp_nepp 12:aa177f0e4c10 242 Polling_delay(); // warning, does nothing!!
sepp_nepp 12:aa177f0e4c10 243 Write_Byte(0x80,0x01);
sepp_nepp 12:aa177f0e4c10 244
sepp_nepp 12:aa177f0e4c10 245 /* ************* First Block of NVM data is read: ************/
sepp_nepp 12:aa177f0e4c10 246 tmp_dword = Get_NVM_DWord(0x6b);
sepp_nepp 12:aa177f0e4c10 247 ReferenceSPADCount = (uint8_t)((tmp_dword >> 8) & 0x7f);
sepp_nepp 12:aa177f0e4c10 248 ReferenceSPADType = (uint8_t)((tmp_dword >> 15) & 0x01);
sepp_nepp 12:aa177f0e4c10 249
sepp_nepp 12:aa177f0e4c10 250 tmp_dword = Get_NVM_DWord(0x24);
sepp_nepp 12:aa177f0e4c10 251 SPADData.RefGoodSPADMap[0] = (uint8_t)((tmp_dword >> 24)& 0xff);
sepp_nepp 12:aa177f0e4c10 252 SPADData.RefGoodSPADMap[1] = (uint8_t)((tmp_dword >> 16)& 0xff);
sepp_nepp 12:aa177f0e4c10 253 SPADData.RefGoodSPADMap[2] = (uint8_t)((tmp_dword >> 8)& 0xff);
sepp_nepp 12:aa177f0e4c10 254 SPADData.RefGoodSPADMap[3] = (uint8_t)(tmp_dword & 0xff);
sepp_nepp 12:aa177f0e4c10 255
sepp_nepp 12:aa177f0e4c10 256 tmp_dword = Get_NVM_DWord(0x25);
sepp_nepp 12:aa177f0e4c10 257 SPADData.RefGoodSPADMap[4] = (uint8_t)((tmp_dword >> 24)& 0xff);
sepp_nepp 12:aa177f0e4c10 258 SPADData.RefGoodSPADMap[5] = (uint8_t)((tmp_dword >> 16)& 0xff);
sepp_nepp 12:aa177f0e4c10 259
sepp_nepp 12:aa177f0e4c10 260 /* ************* Second Block of NVM data is read: ************/
sepp_nepp 12:aa177f0e4c10 261 ModuleId = Get_NVM_Byte(0x02);
sepp_nepp 12:aa177f0e4c10 262 Revision = Get_NVM_Byte(0x02);
sepp_nepp 12:aa177f0e4c10 263
sepp_nepp 12:aa177f0e4c10 264 tmp_dword = Get_NVM_DWord(0x77);
sepp_nepp 12:aa177f0e4c10 265 ProductId[0] = (char)((tmp_dword >> 25) & 0x07f);
sepp_nepp 12:aa177f0e4c10 266 ProductId[1] = (char)((tmp_dword >> 18) & 0x07f);
sepp_nepp 12:aa177f0e4c10 267 ProductId[2] = (char)((tmp_dword >> 11) & 0x07f);
sepp_nepp 12:aa177f0e4c10 268 ProductId[3] = (char)((tmp_dword >> 4) & 0x07f);
sepp_nepp 12:aa177f0e4c10 269 ProductId[4] = (char)((tmp_dword << 3) & 0x07f);
sepp_nepp 12:aa177f0e4c10 270
sepp_nepp 12:aa177f0e4c10 271 tmp_dword = Get_NVM_DWord(0x78);
sepp_nepp 12:aa177f0e4c10 272 ProductId[4] = ProductId[4] |
sepp_nepp 12:aa177f0e4c10 273 (char)((tmp_dword >> 29) & 0x07f));
sepp_nepp 12:aa177f0e4c10 274 ProductId[5] = (char)((tmp_dword >> 22) & 0x07f);
sepp_nepp 12:aa177f0e4c10 275 ProductId[6] = (char)((tmp_dword >> 15) & 0x07f);
sepp_nepp 12:aa177f0e4c10 276 ProductId[7] = (char)((tmp_dword >> 8) & 0x07f);
sepp_nepp 12:aa177f0e4c10 277 ProductId[8] = (char)((tmp_dword >> 1) & 0x07f);
sepp_nepp 12:aa177f0e4c10 278 ProductId[9] = (char)((tmp_dword << 6) & 0x07f);
sepp_nepp 12:aa177f0e4c10 279
sepp_nepp 12:aa177f0e4c10 280 tmp_dword = Get_NVM_DWord(0x79);
sepp_nepp 12:aa177f0e4c10 281 ProductId[9] = ProductId[9] |
sepp_nepp 12:aa177f0e4c10 282 (char)((tmp_dword >> 26) & 0x07f);
sepp_nepp 12:aa177f0e4c10 283 ProductId[10] = (char)((tmp_dword >> 19) & 0x07f);
sepp_nepp 12:aa177f0e4c10 284 ProductId[11] = (char)((tmp_dword >> 12) & 0x07f);
sepp_nepp 12:aa177f0e4c10 285 ProductId[12] = (char)((tmp_dword >> 5) & 0x07f);
sepp_nepp 12:aa177f0e4c10 286 ProductId[13] = (char)((tmp_dword << 2) & 0x07f);
sepp_nepp 12:aa177f0e4c10 287
sepp_nepp 12:aa177f0e4c10 288 tmp_dword = Get_NVM_DWord(0x7A);
sepp_nepp 12:aa177f0e4c10 289 ProductId[13] = ProductId[13] |
sepp_nepp 12:aa177f0e4c10 290 (char)((tmp_dword >> 30) & 0x07f));
sepp_nepp 12:aa177f0e4c10 291 ProductId[14] = (char)((tmp_dword >> 23) & 0x07f);
sepp_nepp 12:aa177f0e4c10 292 ProductId[15] = (char)((tmp_dword >> 16) & 0x07f);
sepp_nepp 12:aa177f0e4c10 293 ProductId[16] = (char)((tmp_dword >> 9) & 0x07f);
sepp_nepp 12:aa177f0e4c10 294 ProductId[17] = (char)((tmp_dword >> 2) & 0x07f);
sepp_nepp 12:aa177f0e4c10 295 ProductId[18] = '\0';
sepp_nepp 12:aa177f0e4c10 296
sepp_nepp 12:aa177f0e4c10 297 /* ************* Third Block of NVM data is read: ************/
sepp_nepp 12:aa177f0e4c10 298 PartUIDUpper = Get_NVM_DWord(0x7B);
sepp_nepp 12:aa177f0e4c10 299 PartUIDLower = Get_NVM_DWord(0x7C);
sepp_nepp 12:aa177f0e4c10 300 SignalRateMeasFixed400mm = // convert from FP97_TO_FP1616
sepp_nepp 12:aa177f0e4c10 301 ( (( Get_NVM_DWord(0x73) << 17) & 0x1fE0000) |
sepp_nepp 12:aa177f0e4c10 302 (( Get_NVM_DWord(0x74) >> 15) & 0x001fE00) ) ;
sepp_nepp 12:aa177f0e4c10 303
sepp_nepp 12:aa177f0e4c10 304 dist_meas_fixed1104_400_mm =
sepp_nepp 12:aa177f0e4c10 305 (( Get_NVM_DWord(0x75) << 8) & 0xff00) |
sepp_nepp 12:aa177f0e4c10 306 (( Get_NVM_DWord(0x76) >> 24) & 0x00ff);
sepp_nepp 12:aa177f0e4c10 307
sepp_nepp 12:aa177f0e4c10 308 if (dist_meas_fixed1104_400_mm != 0) {
sepp_nepp 12:aa177f0e4c10 309 offset_fixed1104_mm = dist_meas_fixed1104_400_mm -
sepp_nepp 12:aa177f0e4c10 310 dist_meas_tgt_fixed1104_mm;
sepp_nepp 12:aa177f0e4c10 311 NVM_Offset_Cal_um = (offset_fixed1104_mm * 1000) >> 4;
sepp_nepp 12:aa177f0e4c10 312 NVM_Offset_Cal_um *= -1; }
sepp_nepp 12:aa177f0e4c10 313 else { NVM_Offset_Cal_um = 0; }
sepp_nepp 12:aa177f0e4c10 314
sepp_nepp 12:aa177f0e4c10 315 Write_Byte(0x81,0x00);
sepp_nepp 12:aa177f0e4c10 316 Write_Byte(0xFF,0x06);
sepp_nepp 12:aa177f0e4c10 317 Register_BitMask(0x83,0xfb,0x00)
sepp_nepp 12:aa177f0e4c10 318 Write_Byte(0xFF,0x01);
sepp_nepp 12:aa177f0e4c10 319 Write_Byte(0x00,0x01);
sepp_nepp 12:aa177f0e4c10 320 Write_Byte(0xFF,0x00);
sepp_nepp 12:aa177f0e4c10 321 Write_Byte(0x80,0x00);
sepp_nepp 12:aa177f0e4c10 322 ReadNVMDataFromDeviceDone = 7;
sepp_nepp 12:aa177f0e4c10 323 }
sepp_nepp 11:c6f95a42d4d7 324
sepp_nepp 11:c6f95a42d4d7 325 uint32_t VL53L0X::Get_distance()
sepp_nepp 11:c6f95a42d4d7 326 { ErrState = VL53L0X_OK;
sepp_nepp 11:c6f95a42d4d7 327 TRangeResults p_ranging_results;
sepp_nepp 11:c6f95a42d4d7 328
sepp_nepp 11:c6f95a42d4d7 329 Start_Measurement(op_single_shot_poll, NULL);
sepp_nepp 11:c6f95a42d4d7 330 if (ErrState==VL53L0X_OK)
sepp_nepp 11:c6f95a42d4d7 331 { p_ranging_results = Get_Measurement(op_single_shot_poll); }
sepp_nepp 11:c6f95a42d4d7 332
sepp_nepp 11:c6f95a42d4d7 333 Stop_Measurement(op_single_shot_poll);
sepp_nepp 11:c6f95a42d4d7 334
sepp_nepp 11:c6f95a42d4d7 335 if (p_ranging_results.RangeStatus == 0) // we have a valid range ?
sepp_nepp 11:c6f95a42d4d7 336 { return p_ranging_results.RangeMilliMeter; }
sepp_nepp 11:c6f95a42d4d7 337 else
sepp_nepp 11:c6f95a42d4d7 338 { ErrState = VL53L0X_ERROR_RANGE_ERROR; return 0;}
nikapov 0:a1a69d32f310 339 }
nikapov 0:a1a69d32f310 340
sepp_nepp 12:aa177f0e4c10 341 /******************************************************************************/
sepp_nepp 12:aa177f0e4c10 342 /******************************************************************************/
sepp_nepp 12:aa177f0e4c10 343 /****************** Actual Measurement functions *************************/
sepp_nepp 12:aa177f0e4c10 344 /******************************************************************************/
sepp_nepp 12:aa177f0e4c10 345 /******************************************************************************/
sepp_nepp 12:aa177f0e4c10 346
sepp_nepp 11:c6f95a42d4d7 347 TRangeResults VL53L0X::Get_Measurement(TOperatingMode operating_mode)
sepp_nepp 11:c6f95a42d4d7 348 { TRangeResults p_data;
sepp_nepp 11:c6f95a42d4d7 349
sepp_nepp 11:c6f95a42d4d7 350 switch (operating_mode) {
sepp_nepp 11:c6f95a42d4d7 351 case op_single_shot_poll:
sepp_nepp 11:c6f95a42d4d7 352 Perf_single_ranging_measurement(&p_data);
sepp_nepp 11:c6f95a42d4d7 353 break;
sepp_nepp 11:c6f95a42d4d7 354 case op_poll:
sepp_nepp 11:c6f95a42d4d7 355 Poll_Measure_Completion();
sepp_nepp 11:c6f95a42d4d7 356 Get_ranging_results(&p_data);
sepp_nepp 11:c6f95a42d4d7 357 if (ErrState == VL53L0X_OK) { // Clear the interrupt
sepp_nepp 11:c6f95a42d4d7 358 Clear_interrupt_mask(REG_SYSINT_GPIO_NEW_SAMPLE_READY);
sepp_nepp 11:c6f95a42d4d7 359 Polling_delay();
sepp_nepp 11:c6f95a42d4d7 360 }
sepp_nepp 11:c6f95a42d4d7 361 break;
sepp_nepp 11:c6f95a42d4d7 362 case op_INT:
sepp_nepp 11:c6f95a42d4d7 363 Get_ranging_results(&p_data);
sepp_nepp 11:c6f95a42d4d7 364 Clear_interrupt_mask(REG_SYSINT_CLEAR | REG_RESULT_INTERRUPT_STATUS);
sepp_nepp 11:c6f95a42d4d7 365 } // switch
sepp_nepp 11:c6f95a42d4d7 366 return p_data;
sepp_nepp 11:c6f95a42d4d7 367 }
sepp_nepp 11:c6f95a42d4d7 368
sepp_nepp 11:c6f95a42d4d7 369 /** Get part to part calibration offset; Should only be used after a
sepp_nepp 11:c6f95a42d4d7 370 successful call to @a VL53L0X_DataInit to backup device NVM value **/
sepp_nepp 11:c6f95a42d4d7 371 int32_t VL53L0X::Get_Offset_Cal_um()
sepp_nepp 11:c6f95a42d4d7 372 { uint16_t range_offset_register;
nikapov 0:a1a69d32f310 373 int16_t c_max_offset = 2047;
nikapov 0:a1a69d32f310 374 int16_t c_offset_range = 4096;
nikapov 0:a1a69d32f310 375
nikapov 0:a1a69d32f310 376 /* Note that offset has 10.2 format */
sepp_nepp 11:c6f95a42d4d7 377 range_offset_register = Read_Word(REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM);
sepp_nepp 11:c6f95a42d4d7 378
sepp_nepp 11:c6f95a42d4d7 379 if (ErrState == VL53L0X_OK) {
nikapov 0:a1a69d32f310 380 range_offset_register = (range_offset_register & 0x0fff);
nikapov 0:a1a69d32f310 381
sepp_nepp 11:c6f95a42d4d7 382 /* Apply 12 bit 2's complement conversion */
sepp_nepp 11:c6f95a42d4d7 383 if (range_offset_register > c_max_offset)
sepp_nepp 11:c6f95a42d4d7 384 { return (int16_t)(range_offset_register - c_offset_range) * 250; }
sepp_nepp 11:c6f95a42d4d7 385 else
sepp_nepp 11:c6f95a42d4d7 386 { return (int16_t)range_offset_register * 250;}
nikapov 0:a1a69d32f310 387 }
sepp_nepp 11:c6f95a42d4d7 388 else return 0;
nikapov 0:a1a69d32f310 389 }
nikapov 0:a1a69d32f310 390
sepp_nepp 11:c6f95a42d4d7 391 void VL53L0X::Set_Offset_Cal_um(int32_t Offset_Cal_um)
sepp_nepp 11:c6f95a42d4d7 392 { int32_t c_max_offset_um = 511000;
sepp_nepp 10:cd251e0fc2fd 393 int32_t c_min_offset_um = -512000;
sepp_nepp 11:c6f95a42d4d7 394 int16_t c_offset_range = 4096;
nikapov 0:a1a69d32f310 395 uint32_t encoded_offset_val;
nikapov 0:a1a69d32f310 396
sepp_nepp 11:c6f95a42d4d7 397 if (Offset_Cal_um > c_max_offset_um) { Offset_Cal_um = c_max_offset_um; }
sepp_nepp 11:c6f95a42d4d7 398 else
sepp_nepp 11:c6f95a42d4d7 399 if (Offset_Cal_um < c_min_offset_um) { Offset_Cal_um = c_min_offset_um; }
nikapov 0:a1a69d32f310 400
nikapov 0:a1a69d32f310 401 /* The offset register is 10.2 format and units are mm
sepp_nepp 10:cd251e0fc2fd 402 * therefore conversion is applied by a division of 250. */
sepp_nepp 11:c6f95a42d4d7 403 if (Offset_Cal_um >= 0) { encoded_offset_val = Offset_Cal_um / 250; }
sepp_nepp 11:c6f95a42d4d7 404 else { encoded_offset_val = c_offset_range + Offset_Cal_um / 250; }
sepp_nepp 11:c6f95a42d4d7 405
sepp_nepp 11:c6f95a42d4d7 406 Write_Word(REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM, encoded_offset_val);
nikapov 0:a1a69d32f310 407 }
nikapov 0:a1a69d32f310 408
sepp_nepp 11:c6f95a42d4d7 409 void VL53L0X::VL53L0X_Apply_Offset_Cal()
sepp_nepp 11:c6f95a42d4d7 410 { int32_t Summed_Offset_Cal_um;
sepp_nepp 11:c6f95a42d4d7 411
sepp_nepp 11:c6f95a42d4d7 412 /* Read back current device offset, and remember in case later someone wants to use it */
sepp_nepp 11:c6f95a42d4d7 413 if (ErrState == VL53L0X_OK) { Last_Offset_Cal_um = Get_Offset_Cal_um(); }
nikapov 0:a1a69d32f310 414
nikapov 0:a1a69d32f310 415 /* Apply Offset Adjustment derived from 400mm measurements */
sepp_nepp 11:c6f95a42d4d7 416 if (ErrState == VL53L0X_OK)
sepp_nepp 11:c6f95a42d4d7 417 { Summed_Offset_Cal_um = Last_Offset_Cal_um + (int32_t) NVM_Offset_Cal_um;
sepp_nepp 11:c6f95a42d4d7 418 Set_Offset_Cal_um(Summed_Offset_Cal_um);
sepp_nepp 11:c6f95a42d4d7 419 /* remember current,adjusted offset */
sepp_nepp 11:c6f95a42d4d7 420 if (ErrState == VL53L0X_OK) { CurrParams.Offset_Cal_um = Summed_Offset_Cal_um; }
sepp_nepp 11:c6f95a42d4d7 421 }
sepp_nepp 11:c6f95a42d4d7 422 }
sepp_nepp 11:c6f95a42d4d7 423
sepp_nepp 11:c6f95a42d4d7 424 void VL53L0X::Get_measure_period_ms(uint32_t *p_measure_period_ms)
sepp_nepp 11:c6f95a42d4d7 425 { uint16_t osc_calibrate_val;
sepp_nepp 11:c6f95a42d4d7 426 uint32_t im_period_ms;
sepp_nepp 11:c6f95a42d4d7 427
sepp_nepp 11:c6f95a42d4d7 428 osc_calibrate_val = Read_Word(REG_OSC_CALIBRATE_VAL);
sepp_nepp 11:c6f95a42d4d7 429
sepp_nepp 11:c6f95a42d4d7 430 if (ErrState == VL53L0X_OK) { im_period_ms = Read_DWord(REG_SYSTEM_MEASURE_PERIOD); }
sepp_nepp 11:c6f95a42d4d7 431
sepp_nepp 11:c6f95a42d4d7 432 if (ErrState == VL53L0X_OK) {
sepp_nepp 11:c6f95a42d4d7 433 if (osc_calibrate_val != 0)
sepp_nepp 11:c6f95a42d4d7 434 {*p_measure_period_ms = im_period_ms / osc_calibrate_val; }
sepp_nepp 11:c6f95a42d4d7 435 CurrParams.Measure_Period_ms = *p_measure_period_ms;
sepp_nepp 11:c6f95a42d4d7 436 }
sepp_nepp 11:c6f95a42d4d7 437 }
sepp_nepp 11:c6f95a42d4d7 438
sepp_nepp 11:c6f95a42d4d7 439 void VL53L0X::Get_Xtalk_CompRate_MHz( TFP1616 *p_Xtalk_CompRate_MHz)
sepp_nepp 11:c6f95a42d4d7 440 { uint16_t value;
sepp_nepp 11:c6f95a42d4d7 441 TFP1616 temp_fix1616;
sepp_nepp 11:c6f95a42d4d7 442
sepp_nepp 11:c6f95a42d4d7 443 value = Read_Word(REG_XTALK_COMPENS_RATE_MHz);
sepp_nepp 11:c6f95a42d4d7 444
sepp_nepp 11:c6f95a42d4d7 445 if (ErrState == VL53L0X_OK) {
sepp_nepp 11:c6f95a42d4d7 446 if (value == 0) {
sepp_nepp 11:c6f95a42d4d7 447 /* the Xtalk is disabled return value from memory */
sepp_nepp 11:c6f95a42d4d7 448 temp_fix1616 = CurrParams.Xtalk_CompRate_MHz;
sepp_nepp 11:c6f95a42d4d7 449 *p_Xtalk_CompRate_MHz = temp_fix1616;
sepp_nepp 11:c6f95a42d4d7 450 CurrParams.XTalk_Compens_En = 0;
sepp_nepp 11:c6f95a42d4d7 451 } else {
sepp_nepp 11:c6f95a42d4d7 452 temp_fix1616 = FP313_TO_FP1616(value);
sepp_nepp 11:c6f95a42d4d7 453 *p_Xtalk_CompRate_MHz = temp_fix1616;
sepp_nepp 11:c6f95a42d4d7 454 CurrParams.Xtalk_CompRate_MHz = temp_fix1616;
sepp_nepp 11:c6f95a42d4d7 455 CurrParams.XTalk_Compens_En = 1;
nikapov 0:a1a69d32f310 456 }
nikapov 0:a1a69d32f310 457 }
nikapov 0:a1a69d32f310 458 }
nikapov 0:a1a69d32f310 459
sepp_nepp 11:c6f95a42d4d7 460 TFP1616 VL53L0X::Get_limit_chk_val( uint16_t limit_check_id )
sepp_nepp 11:c6f95a42d4d7 461 { uint16_t temp16;
sepp_nepp 11:c6f95a42d4d7 462 TFP1616 temp_fix1616;
nikapov 0:a1a69d32f310 463
nikapov 0:a1a69d32f310 464 switch (limit_check_id) {
sepp_nepp 11:c6f95a42d4d7 465 case VL53L0X_CHECKEN_SIGMA_FINAL_RANGE: /* only internal computations: */
sepp_nepp 11:c6f95a42d4d7 466 case VL53L0X_CHECKEN_SIG_REF_CLIP:
sepp_nepp 11:c6f95a42d4d7 467 case VL53L0X_CHECKEN_RANGE_IGNORE_THRESHOLD:
sepp_nepp 11:c6f95a42d4d7 468 return CurrParams.Limit_Chk_Val[limit_check_id];// need no more 'break';
sepp_nepp 11:c6f95a42d4d7 469
sepp_nepp 11:c6f95a42d4d7 470 case VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE:
sepp_nepp 11:c6f95a42d4d7 471 temp16 = Read_Word(REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT);
sepp_nepp 11:c6f95a42d4d7 472 temp_fix1616 = FP97_TO_FP1616(temp16);
sepp_nepp 11:c6f95a42d4d7 473 if (temp_fix1616 == 0) /* disabled: return value from memory instead*/
sepp_nepp 11:c6f95a42d4d7 474 { temp_fix1616 = CurrParams.Limit_Chk_Val[limit_check_id];
sepp_nepp 11:c6f95a42d4d7 475 CurrParams.Limit_Chk_En[limit_check_id] = 0; }
sepp_nepp 11:c6f95a42d4d7 476 else
sepp_nepp 11:c6f95a42d4d7 477 { CurrParams.Limit_Chk_Val[limit_check_id] = temp_fix1616;
sepp_nepp 11:c6f95a42d4d7 478 CurrParams.Limit_Chk_En[limit_check_id] = 1; }
sepp_nepp 11:c6f95a42d4d7 479 return temp_fix1616; // need no more 'break';
sepp_nepp 11:c6f95a42d4d7 480
sepp_nepp 11:c6f95a42d4d7 481 case VL53L0X_CHECKEN_SIG_RATE_MSRC:
sepp_nepp 11:c6f95a42d4d7 482 case VL53L0X_CHECKEN_SIG_RATE_PRE_RANGE:
sepp_nepp 11:c6f95a42d4d7 483 temp16 = Read_Word(REG_PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT);
sepp_nepp 11:c6f95a42d4d7 484 return FP97_TO_FP1616(temp16); // need no more break;
nikapov 0:a1a69d32f310 485
nikapov 0:a1a69d32f310 486 default:
sepp_nepp 11:c6f95a42d4d7 487 ErrState = VL53L0X_ERROR_INVALID_PARAMS;
sepp_nepp 11:c6f95a42d4d7 488 return 0;
nikapov 0:a1a69d32f310 489 }
nikapov 0:a1a69d32f310 490 }
nikapov 0:a1a69d32f310 491
sepp_nepp 11:c6f95a42d4d7 492 uint8_t VL53L0X::Get_limit_chk_en(uint16_t limit_check_id )
sepp_nepp 11:c6f95a42d4d7 493 { if (limit_check_id >= VL53L0X_CHECKEN_NUMBER_OF_CHECKS)
sepp_nepp 11:c6f95a42d4d7 494 { ErrState = VL53L0X_ERROR_INVALID_PARAMS;
sepp_nepp 11:c6f95a42d4d7 495 return 0; }
sepp_nepp 11:c6f95a42d4d7 496 else { return CurrParams.Limit_Chk_En[limit_check_id]; }
nikapov 0:a1a69d32f310 497 }
nikapov 0:a1a69d32f310 498
sepp_nepp 11:c6f95a42d4d7 499 uint8_t VL53L0X::Get_Wrap_Around_Chk_En()
sepp_nepp 11:c6f95a42d4d7 500 { /* Now using the private state field SequenceConfig instead of reading from device:
sepp_nepp 11:c6f95a42d4d7 501 uint8_t SequenceConfig;
sepp_nepp 11:c6f95a42d4d7 502 SequenceConfig = Read_Byte(REG_SYSTEM_SEQUENCE_CONFIG);
sepp_nepp 11:c6f95a42d4d7 503 Set_SequenceConfig( SequenceConfig ); // checks for ErrState
sepp_nepp 5:b95f6951f7d5 504
sepp_nepp 11:c6f95a42d4d7 505 if (ErrState == VL53L0X_OK) {
sepp_nepp 11:c6f95a42d4d7 506 */
sepp_nepp 11:c6f95a42d4d7 507 CurrParams.Wrap_Around_Chk_En = (SequenceConfig >> 7) & 0x01;
sepp_nepp 11:c6f95a42d4d7 508 return CurrParams.Wrap_Around_Chk_En;
sepp_nepp 11:c6f95a42d4d7 509 // } else return 0;
nikapov 0:a1a69d32f310 510 }
nikapov 0:a1a69d32f310 511
sepp_nepp 11:c6f95a42d4d7 512 VL53L0X_Sequence_Steps_t VL53L0X::Get_sequence_step_enables()
sepp_nepp 11:c6f95a42d4d7 513 { VL53L0X_Sequence_Steps_t p_sequence_steps;
sepp_nepp 11:c6f95a42d4d7 514 /* Now using the private state field SequenceConfig instead of reading from device:
sepp_nepp 11:c6f95a42d4d7 515 uint8_t SequenceConfig;
sepp_nepp 11:c6f95a42d4d7 516
sepp_nepp 11:c6f95a42d4d7 517 SequenceConfig = Read_Byte(REG_SYSTEM_SEQUENCE_CONFIG);
sepp_nepp 11:c6f95a42d4d7 518
sepp_nepp 11:c6f95a42d4d7 519 if (ErrState == VL53L0X_OK) {
sepp_nepp 11:c6f95a42d4d7 520 */
sepp_nepp 11:c6f95a42d4d7 521 p_sequence_steps.TccOn = (SequenceConfig & 0x10) >> 4;
sepp_nepp 11:c6f95a42d4d7 522 p_sequence_steps.DssOn = (SequenceConfig & 0x08) >> 3;
sepp_nepp 11:c6f95a42d4d7 523 p_sequence_steps.MsrcOn = (SequenceConfig & 0x04) >> 2;
sepp_nepp 11:c6f95a42d4d7 524 p_sequence_steps.PreRangeOn = (SequenceConfig & 0x40) >> 6;
sepp_nepp 11:c6f95a42d4d7 525 p_sequence_steps.FinalRangeOn = (SequenceConfig & 0x80) >> 7;
sepp_nepp 11:c6f95a42d4d7 526 // }
sepp_nepp 11:c6f95a42d4d7 527 return p_sequence_steps;
nikapov 0:a1a69d32f310 528 }
nikapov 0:a1a69d32f310 529
sepp_nepp 11:c6f95a42d4d7 530 void VL53L0X::Set_vcsel_PPeriod(VL53L0X_Range_Phase Vcsel_Range_Phase, uint8_t vcsel_PPeriod_pclk)
sepp_nepp 11:c6f95a42d4d7 531 { uint8_t vcsel_period_reg;
nikapov 0:a1a69d32f310 532 uint8_t min_pre_vcsel_period_pclk = 12;
nikapov 0:a1a69d32f310 533 uint8_t max_pre_vcsel_period_pclk = 18;
nikapov 0:a1a69d32f310 534 uint8_t min_final_vcsel_period_pclk = 8;
nikapov 0:a1a69d32f310 535 uint8_t max_final_vcsel_period_pclk = 14;
sepp_nepp 10:cd251e0fc2fd 536 uint32_t final_range_timeout_us;
sepp_nepp 10:cd251e0fc2fd 537 uint32_t pre_range_timeout_us;
sepp_nepp 10:cd251e0fc2fd 538 uint32_t msrc_timeout_us;
nikapov 0:a1a69d32f310 539 uint8_t phase_cal_int = 0;
nikapov 0:a1a69d32f310 540
nikapov 0:a1a69d32f310 541 /* Check if valid clock period requested */
sepp_nepp 11:c6f95a42d4d7 542 if ( ((vcsel_PPeriod_pclk % 2) != 0 ) /* Value must be an even number */
sepp_nepp 11:c6f95a42d4d7 543 ||
sepp_nepp 11:c6f95a42d4d7 544 ( Vcsel_Range_Phase == VL53L0X_VCSEL_PRE_RANGE &&
sepp_nepp 11:c6f95a42d4d7 545 ((vcsel_PPeriod_pclk < min_pre_vcsel_period_pclk)||
sepp_nepp 11:c6f95a42d4d7 546 (vcsel_PPeriod_pclk > max_pre_vcsel_period_pclk) ) )
sepp_nepp 11:c6f95a42d4d7 547 ||
sepp_nepp 11:c6f95a42d4d7 548 ( Vcsel_Range_Phase == VL53L0X_VCSEL_FINAL_RANGE &&
sepp_nepp 11:c6f95a42d4d7 549 (vcsel_PPeriod_pclk < min_final_vcsel_period_pclk ||
sepp_nepp 11:c6f95a42d4d7 550 vcsel_PPeriod_pclk > max_final_vcsel_period_pclk) ) )
sepp_nepp 11:c6f95a42d4d7 551 { ErrState = VL53L0X_ERROR_INVALID_PARAMS;
sepp_nepp 11:c6f95a42d4d7 552 return;}
nikapov 0:a1a69d32f310 553
nikapov 0:a1a69d32f310 554 /* Apply specific settings for the requested clock period */
sepp_nepp 11:c6f95a42d4d7 555 if (Vcsel_Range_Phase == VL53L0X_VCSEL_PRE_RANGE) {
sepp_nepp 11:c6f95a42d4d7 556 /* Set phase check limits for pre-ranging*/
sepp_nepp 11:c6f95a42d4d7 557 if (vcsel_PPeriod_pclk == 12) {
sepp_nepp 11:c6f95a42d4d7 558 Write_Byte(REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,0x18);
sepp_nepp 11:c6f95a42d4d7 559 Write_Byte(REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW ,0x08);
sepp_nepp 11:c6f95a42d4d7 560 } else if (vcsel_PPeriod_pclk == 14) {
sepp_nepp 11:c6f95a42d4d7 561 Write_Byte(REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,0x30);
sepp_nepp 11:c6f95a42d4d7 562 Write_Byte(REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW ,0x08);
sepp_nepp 11:c6f95a42d4d7 563 } else if (vcsel_PPeriod_pclk == 16) {
sepp_nepp 11:c6f95a42d4d7 564 Write_Byte(REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,0x40);
sepp_nepp 11:c6f95a42d4d7 565 Write_Byte(REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW ,0x08);
sepp_nepp 11:c6f95a42d4d7 566 } else if (vcsel_PPeriod_pclk == 18) {
sepp_nepp 11:c6f95a42d4d7 567 Write_Byte(REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,0x50);
sepp_nepp 11:c6f95a42d4d7 568 Write_Byte(REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW ,0x08);
nikapov 0:a1a69d32f310 569 }
sepp_nepp 11:c6f95a42d4d7 570 } else if (Vcsel_Range_Phase == VL53L0X_VCSEL_FINAL_RANGE) {
sepp_nepp 11:c6f95a42d4d7 571 if (vcsel_PPeriod_pclk == 8) {
sepp_nepp 11:c6f95a42d4d7 572 Write_Byte(REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,0x10);
sepp_nepp 11:c6f95a42d4d7 573 Write_Byte(REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW ,0x08);
sepp_nepp 11:c6f95a42d4d7 574 Write_Byte(REG_GLOBAL_CONFIG_VCSEL_WIDTH ,0x02);
sepp_nepp 11:c6f95a42d4d7 575 Write_Byte(REG_ALGO_PHASECAL_CONFIG_TIMEOUT,0x0C);
sepp_nepp 11:c6f95a42d4d7 576 Write_Byte(0xff,0x01);
sepp_nepp 11:c6f95a42d4d7 577 Write_Byte(REG_ALGO_PHASECAL_LIM,0x30);
sepp_nepp 11:c6f95a42d4d7 578 Write_Byte(0xff,0x00);
sepp_nepp 11:c6f95a42d4d7 579 } else if (vcsel_PPeriod_pclk == 10) {
sepp_nepp 11:c6f95a42d4d7 580 Write_Byte(REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,0x28);
sepp_nepp 11:c6f95a42d4d7 581 Write_Byte(REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,0x08);
sepp_nepp 11:c6f95a42d4d7 582 Write_Byte(REG_GLOBAL_CONFIG_VCSEL_WIDTH,0x03);
sepp_nepp 11:c6f95a42d4d7 583 Write_Byte(REG_ALGO_PHASECAL_CONFIG_TIMEOUT,0x09);
sepp_nepp 11:c6f95a42d4d7 584 Write_Byte(0xff,0x01);
sepp_nepp 11:c6f95a42d4d7 585 Write_Byte(REG_ALGO_PHASECAL_LIM,0x20);
sepp_nepp 11:c6f95a42d4d7 586 Write_Byte(0xff,0x00);
sepp_nepp 11:c6f95a42d4d7 587 } else if (vcsel_PPeriod_pclk == 12) {
sepp_nepp 11:c6f95a42d4d7 588 Write_Byte(REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,0x38);
sepp_nepp 11:c6f95a42d4d7 589 Write_Byte(REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,0x08);
sepp_nepp 11:c6f95a42d4d7 590 Write_Byte(REG_GLOBAL_CONFIG_VCSEL_WIDTH,0x03);
sepp_nepp 11:c6f95a42d4d7 591 Write_Byte(REG_ALGO_PHASECAL_CONFIG_TIMEOUT,0x08);
sepp_nepp 11:c6f95a42d4d7 592 Write_Byte(0xff,0x01);
sepp_nepp 11:c6f95a42d4d7 593 Write_Byte(REG_ALGO_PHASECAL_LIM,0x20);
sepp_nepp 11:c6f95a42d4d7 594 Write_Byte(0xff,0x00);
sepp_nepp 11:c6f95a42d4d7 595 } else if (vcsel_PPeriod_pclk == 14) {
sepp_nepp 11:c6f95a42d4d7 596 Write_Byte(REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,0x048);
sepp_nepp 11:c6f95a42d4d7 597 Write_Byte(REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,0x08);
sepp_nepp 11:c6f95a42d4d7 598 Write_Byte(REG_GLOBAL_CONFIG_VCSEL_WIDTH,0x03);
sepp_nepp 11:c6f95a42d4d7 599 Write_Byte(REG_ALGO_PHASECAL_CONFIG_TIMEOUT,0x07);
sepp_nepp 11:c6f95a42d4d7 600 Write_Byte(0xff,0x01);
sepp_nepp 11:c6f95a42d4d7 601 Write_Byte(REG_ALGO_PHASECAL_LIM, 0x20);
sepp_nepp 11:c6f95a42d4d7 602 Write_Byte(0xff,0x00);
nikapov 0:a1a69d32f310 603 }
nikapov 0:a1a69d32f310 604 }
nikapov 0:a1a69d32f310 605
sepp_nepp 7:41cbc431e1f4 606 /* Re-calculate and apply timeouts,in macro periods */
sepp_nepp 11:c6f95a42d4d7 607 if (ErrState == VL53L0X_OK) {
sepp_nepp 11:c6f95a42d4d7 608 /* Converts the encoded VCSEL period register value into the real period in PLL clocks */
sepp_nepp 11:c6f95a42d4d7 609 /* Flattened from procedure called Encode_vcsel_period */
sepp_nepp 11:c6f95a42d4d7 610 vcsel_period_reg = (vcsel_PPeriod_pclk >> 1) - 1;
sepp_nepp 11:c6f95a42d4d7 611
nikapov 0:a1a69d32f310 612 /* When the VCSEL period for the pre or final range is changed,
nikapov 0:a1a69d32f310 613 * the corresponding timeout must be read from the device using
sepp_nepp 7:41cbc431e1f4 614 * the current VCSEL period,then the new VCSEL period can be
nikapov 0:a1a69d32f310 615 * applied. The timeout then must be written back to the device
nikapov 0:a1a69d32f310 616 * using the new VCSEL period.
sepp_nepp 7:41cbc431e1f4 617 * For the MSRC timeout,the same applies - this timeout being
nikapov 0:a1a69d32f310 618 * dependant on the pre-range vcsel period.
nikapov 0:a1a69d32f310 619 */
sepp_nepp 11:c6f95a42d4d7 620 switch (Vcsel_Range_Phase) {
sepp_nepp 11:c6f95a42d4d7 621 case VL53L0X_VCSEL_PRE_RANGE:
sepp_nepp 11:c6f95a42d4d7 622 Get_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_PRE_RANGE,&pre_range_timeout_us);
sepp_nepp 11:c6f95a42d4d7 623
sepp_nepp 11:c6f95a42d4d7 624 if (ErrState == VL53L0X_OK)
sepp_nepp 11:c6f95a42d4d7 625 Get_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_MSRC,&msrc_timeout_us);
sepp_nepp 11:c6f95a42d4d7 626
sepp_nepp 11:c6f95a42d4d7 627 Write_Byte(REG_PRE_RANGE_CONFIG_VCSEL_PERIOD,vcsel_period_reg);
sepp_nepp 11:c6f95a42d4d7 628
sepp_nepp 11:c6f95a42d4d7 629 if (ErrState == VL53L0X_OK)
sepp_nepp 11:c6f95a42d4d7 630 Set_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_PRE_RANGE,pre_range_timeout_us);
sepp_nepp 11:c6f95a42d4d7 631
sepp_nepp 11:c6f95a42d4d7 632 if (ErrState == VL53L0X_OK)
sepp_nepp 11:c6f95a42d4d7 633 Set_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_MSRC,msrc_timeout_us);
sepp_nepp 11:c6f95a42d4d7 634
sepp_nepp 12:aa177f0e4c10 635 PreRangeVcselPPeriod = vcsel_PPeriod_pclk;
sepp_nepp 11:c6f95a42d4d7 636 break;
sepp_nepp 11:c6f95a42d4d7 637
sepp_nepp 11:c6f95a42d4d7 638 case VL53L0X_VCSEL_FINAL_RANGE:
sepp_nepp 11:c6f95a42d4d7 639 Get_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_FINAL_RANGE,&final_range_timeout_us);
sepp_nepp 11:c6f95a42d4d7 640
sepp_nepp 11:c6f95a42d4d7 641 Write_Byte(REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD,vcsel_period_reg);
sepp_nepp 11:c6f95a42d4d7 642
sepp_nepp 11:c6f95a42d4d7 643 if (ErrState == VL53L0X_OK)
sepp_nepp 11:c6f95a42d4d7 644 Set_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_FINAL_RANGE,final_range_timeout_us);
sepp_nepp 11:c6f95a42d4d7 645
sepp_nepp 12:aa177f0e4c10 646 FinalRangeVcselPPeriod = vcsel_PPeriod_pclk;
sepp_nepp 11:c6f95a42d4d7 647 break;
sepp_nepp 11:c6f95a42d4d7 648 default: ErrState = VL53L0X_ERROR_INVALID_PARAMS;
nikapov 0:a1a69d32f310 649 }
nikapov 0:a1a69d32f310 650 }
nikapov 0:a1a69d32f310 651
sepp_nepp 11:c6f95a42d4d7 652 /* Finally,the timing budget is re-applied */
sepp_nepp 11:c6f95a42d4d7 653 if (ErrState == VL53L0X_OK)
sepp_nepp 11:c6f95a42d4d7 654 { Set_Measure_Time_Budget_us(CurrParams.Measure_Time_Budget_us); }
sepp_nepp 11:c6f95a42d4d7 655
sepp_nepp 11:c6f95a42d4d7 656 /* Perform the phase calibration. This is needed after changing on vcsel period.
sepp_nepp 7:41cbc431e1f4 657 * get_data_enable = 0,restore_config = 1 */
sepp_nepp 11:c6f95a42d4d7 658 Perf_phase_calibration(&phase_cal_int,0,1);
nikapov 0:a1a69d32f310 659 }
nikapov 0:a1a69d32f310 660
sepp_nepp 11:c6f95a42d4d7 661 #define VL53L0X_MACRO_PERIOD_NS 3813; // = ( VL53L0X_PLL_PERIOD_PS * VL53L0X_MACRO_PERIOD_VCLKS / 1000 )
nikapov 0:a1a69d32f310 662
nikapov 0:a1a69d32f310 663 /* To convert register value into us */
sepp_nepp 11:c6f95a42d4d7 664 uint32_t VL53L0X::Calc_timeout_us(uint16_t timeout_period_mclks,
nikapov 0:a1a69d32f310 665 uint8_t vcsel_period_pclks)
nikapov 0:a1a69d32f310 666 {
nikapov 0:a1a69d32f310 667 uint32_t macro_period_ns;
nikapov 0:a1a69d32f310 668 uint32_t actual_timeout_period_us = 0;
nikapov 0:a1a69d32f310 669
sepp_nepp 11:c6f95a42d4d7 670 macro_period_ns = (uint32_t) (vcsel_period_pclks ) * VL53L0X_MACRO_PERIOD_NS;
sepp_nepp 11:c6f95a42d4d7 671
sepp_nepp 11:c6f95a42d4d7 672 actual_timeout_period_us = ((timeout_period_mclks * macro_period_ns) + 500) / 1000;
nikapov 0:a1a69d32f310 673
nikapov 0:a1a69d32f310 674 return actual_timeout_period_us;
nikapov 0:a1a69d32f310 675 }
nikapov 0:a1a69d32f310 676
sepp_nepp 11:c6f95a42d4d7 677 void VL53L0X::Get_Sequence_Step_Timeout(VL53L0X_SequenceStepId sequence_step_id,
nikapov 0:a1a69d32f310 678 uint32_t *p_time_out_micro_secs)
sepp_nepp 11:c6f95a42d4d7 679 { uint8_t current_vcsel_PPeriod_p_clk;
nikapov 0:a1a69d32f310 680 uint8_t encoded_time_out_byte = 0;
sepp_nepp 10:cd251e0fc2fd 681 uint32_t timeout_us = 0;
nikapov 0:a1a69d32f310 682 uint16_t pre_range_encoded_time_out = 0;
nikapov 0:a1a69d32f310 683 uint16_t msrc_time_out_m_clks;
nikapov 0:a1a69d32f310 684 uint16_t pre_range_time_out_m_clks;
nikapov 0:a1a69d32f310 685 uint16_t final_range_time_out_m_clks = 0;
nikapov 0:a1a69d32f310 686 uint16_t final_range_encoded_time_out;
sepp_nepp 11:c6f95a42d4d7 687 VL53L0X_Sequence_Steps_t sequence_steps;
sepp_nepp 11:c6f95a42d4d7 688
sepp_nepp 11:c6f95a42d4d7 689 if ((sequence_step_id == VL53L0X_SEQUENCESTEP_TCC ) ||
sepp_nepp 11:c6f95a42d4d7 690 (sequence_step_id == VL53L0X_SEQUENCESTEP_DSS ) ||
sepp_nepp 11:c6f95a42d4d7 691 (sequence_step_id == VL53L0X_SEQUENCESTEP_MSRC) ) {
sepp_nepp 11:c6f95a42d4d7 692
sepp_nepp 11:c6f95a42d4d7 693 current_vcsel_PPeriod_p_clk = /* Gets and converts the VCSEL period register into actual clock periods */
sepp_nepp 11:c6f95a42d4d7 694 ( Read_Byte(REG_PRE_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1;
sepp_nepp 11:c6f95a42d4d7 695
sepp_nepp 11:c6f95a42d4d7 696 if (ErrState == VL53L0X_OK) {
sepp_nepp 11:c6f95a42d4d7 697 encoded_time_out_byte = Read_Byte(REG_MSRC_CONFIG_TIMEOUT_MACROP);
nikapov 0:a1a69d32f310 698 }
sepp_nepp 11:c6f95a42d4d7 699 msrc_time_out_m_clks = Decode_timeout(encoded_time_out_byte);
sepp_nepp 11:c6f95a42d4d7 700
sepp_nepp 11:c6f95a42d4d7 701 timeout_us = Calc_timeout_us(msrc_time_out_m_clks,
sepp_nepp 11:c6f95a42d4d7 702 current_vcsel_PPeriod_p_clk);
nikapov 0:a1a69d32f310 703 } else if (sequence_step_id == VL53L0X_SEQUENCESTEP_PRE_RANGE) {
sepp_nepp 11:c6f95a42d4d7 704
sepp_nepp 11:c6f95a42d4d7 705 current_vcsel_PPeriod_p_clk = /* Gets and converts the VCSEL period register into actual clock periods */
sepp_nepp 11:c6f95a42d4d7 706 ( Read_Byte(REG_PRE_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1;
nikapov 0:a1a69d32f310 707
nikapov 0:a1a69d32f310 708 /* Retrieve PRE-RANGE Timeout in Macro periods (MCLKS) */
sepp_nepp 11:c6f95a42d4d7 709 if (ErrState == VL53L0X_OK) {
sepp_nepp 11:c6f95a42d4d7 710
sepp_nepp 11:c6f95a42d4d7 711 pre_range_encoded_time_out = Read_Word(REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI);
sepp_nepp 11:c6f95a42d4d7 712
sepp_nepp 11:c6f95a42d4d7 713 pre_range_time_out_m_clks = Decode_timeout(pre_range_encoded_time_out);
sepp_nepp 11:c6f95a42d4d7 714
sepp_nepp 11:c6f95a42d4d7 715 timeout_us = Calc_timeout_us(pre_range_time_out_m_clks,
sepp_nepp 11:c6f95a42d4d7 716 current_vcsel_PPeriod_p_clk);
nikapov 0:a1a69d32f310 717 }
nikapov 0:a1a69d32f310 718 } else if (sequence_step_id == VL53L0X_SEQUENCESTEP_FINAL_RANGE) {
nikapov 0:a1a69d32f310 719
sepp_nepp 11:c6f95a42d4d7 720 sequence_steps = Get_sequence_step_enables();
nikapov 0:a1a69d32f310 721 pre_range_time_out_m_clks = 0;
nikapov 0:a1a69d32f310 722
sepp_nepp 11:c6f95a42d4d7 723 if (sequence_steps.PreRangeOn) {
sepp_nepp 11:c6f95a42d4d7 724 current_vcsel_PPeriod_p_clk = /* Gets and converts the VCSEL period register into actual clock periods */
sepp_nepp 11:c6f95a42d4d7 725 ( Read_Byte(REG_PRE_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1;
sepp_nepp 11:c6f95a42d4d7 726
sepp_nepp 11:c6f95a42d4d7 727 /* Retrieve PRE-RANGE Timeout in Macro periods (MCLKS) */
sepp_nepp 11:c6f95a42d4d7 728 if (ErrState == VL53L0X_OK) {
sepp_nepp 11:c6f95a42d4d7 729 pre_range_encoded_time_out = Read_Word(REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI);
sepp_nepp 11:c6f95a42d4d7 730 pre_range_time_out_m_clks = Decode_timeout(pre_range_encoded_time_out);
nikapov 0:a1a69d32f310 731 }
nikapov 0:a1a69d32f310 732 }
nikapov 0:a1a69d32f310 733
sepp_nepp 11:c6f95a42d4d7 734 if (ErrState == VL53L0X_OK) {
sepp_nepp 11:c6f95a42d4d7 735 current_vcsel_PPeriod_p_clk = /* Get and converts the VCSEL period register into actual clock periods */
sepp_nepp 11:c6f95a42d4d7 736 ( Read_Byte(REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1;
sepp_nepp 11:c6f95a42d4d7 737
nikapov 0:a1a69d32f310 738 }
nikapov 0:a1a69d32f310 739
nikapov 0:a1a69d32f310 740 /* Retrieve FINAL-RANGE Timeout in Macro periods (MCLKS) */
sepp_nepp 11:c6f95a42d4d7 741 if (ErrState == VL53L0X_OK) {
sepp_nepp 11:c6f95a42d4d7 742 final_range_encoded_time_out = Read_Word(REG_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI);
sepp_nepp 11:c6f95a42d4d7 743 final_range_time_out_m_clks = Decode_timeout(final_range_encoded_time_out);
nikapov 0:a1a69d32f310 744 }
nikapov 0:a1a69d32f310 745
nikapov 0:a1a69d32f310 746 final_range_time_out_m_clks -= pre_range_time_out_m_clks;
sepp_nepp 11:c6f95a42d4d7 747 timeout_us = Calc_timeout_us(final_range_time_out_m_clks,current_vcsel_PPeriod_p_clk);
nikapov 0:a1a69d32f310 748 }
nikapov 0:a1a69d32f310 749
sepp_nepp 10:cd251e0fc2fd 750 *p_time_out_micro_secs = timeout_us;
nikapov 0:a1a69d32f310 751 }
nikapov 0:a1a69d32f310 752
sepp_nepp 11:c6f95a42d4d7 753 uint32_t VL53L0X::Get_Measure_Time_Budget_us()
sepp_nepp 11:c6f95a42d4d7 754 { VL53L0X_Sequence_Steps_t sequence_steps;
sepp_nepp 11:c6f95a42d4d7 755 uint32_t p_Measure_Time_Budget_us;
sepp_nepp 10:cd251e0fc2fd 756 uint32_t final_range_timeout_us;
sepp_nepp 11:c6f95a42d4d7 757 uint32_t msrc_dcc_tcc_timeout_us= 2000;
sepp_nepp 10:cd251e0fc2fd 758 uint32_t start_overhead_us = 1910;
sepp_nepp 11:c6f95a42d4d7 759 uint32_t end_overhead_us = 960;
sepp_nepp 10:cd251e0fc2fd 760 uint32_t msrc_overhead_us = 660;
sepp_nepp 11:c6f95a42d4d7 761 uint32_t tcc_overhead_us = 590;
sepp_nepp 11:c6f95a42d4d7 762 uint32_t dss_overhead_us = 690;
sepp_nepp 10:cd251e0fc2fd 763 uint32_t pre_range_overhead_us = 660;
sepp_nepp 11:c6f95a42d4d7 764 uint32_t final_range_overhead_us= 550;
sepp_nepp 10:cd251e0fc2fd 765 uint32_t pre_range_timeout_us = 0;
nikapov 0:a1a69d32f310 766
sepp_nepp 11:c6f95a42d4d7 767 if (ErrState != VL53L0X_OK) {return 0; } // do nothing while in Error State!!!!
sepp_nepp 11:c6f95a42d4d7 768
nikapov 0:a1a69d32f310 769 /* Start and end overhead times always present */
sepp_nepp 11:c6f95a42d4d7 770 p_Measure_Time_Budget_us = start_overhead_us + end_overhead_us;
sepp_nepp 11:c6f95a42d4d7 771
sepp_nepp 11:c6f95a42d4d7 772 sequence_steps = Get_sequence_step_enables();
sepp_nepp 11:c6f95a42d4d7 773
sepp_nepp 11:c6f95a42d4d7 774 if (sequence_steps.TccOn || sequence_steps.MsrcOn || sequence_steps.DssOn)
sepp_nepp 11:c6f95a42d4d7 775 { Get_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_MSRC, &msrc_dcc_tcc_timeout_us);
sepp_nepp 11:c6f95a42d4d7 776
sepp_nepp 11:c6f95a42d4d7 777 if (ErrState == VL53L0X_OK) {
sepp_nepp 11:c6f95a42d4d7 778 if (sequence_steps.TccOn)
sepp_nepp 11:c6f95a42d4d7 779 { p_Measure_Time_Budget_us += msrc_dcc_tcc_timeout_us + tcc_overhead_us; }
sepp_nepp 11:c6f95a42d4d7 780
sepp_nepp 11:c6f95a42d4d7 781 if (sequence_steps.DssOn) {
sepp_nepp 11:c6f95a42d4d7 782 p_Measure_Time_Budget_us += 2 * (msrc_dcc_tcc_timeout_us + dss_overhead_us);
sepp_nepp 11:c6f95a42d4d7 783 } else if (sequence_steps.MsrcOn) {
sepp_nepp 11:c6f95a42d4d7 784 p_Measure_Time_Budget_us += msrc_dcc_tcc_timeout_us + msrc_overhead_us;
nikapov 0:a1a69d32f310 785 }
nikapov 0:a1a69d32f310 786 }
nikapov 0:a1a69d32f310 787 }
nikapov 0:a1a69d32f310 788
sepp_nepp 11:c6f95a42d4d7 789 if ( (ErrState == VL53L0X_OK) && sequence_steps.PreRangeOn) {
sepp_nepp 11:c6f95a42d4d7 790 Get_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_PRE_RANGE, &pre_range_timeout_us);
sepp_nepp 11:c6f95a42d4d7 791 p_Measure_Time_Budget_us += pre_range_timeout_us + pre_range_overhead_us;
sepp_nepp 11:c6f95a42d4d7 792 }
sepp_nepp 11:c6f95a42d4d7 793
sepp_nepp 11:c6f95a42d4d7 794 if (ErrState == VL53L0X_OK) {
sepp_nepp 11:c6f95a42d4d7 795 if (sequence_steps.FinalRangeOn) {
sepp_nepp 11:c6f95a42d4d7 796 Get_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_FINAL_RANGE, &final_range_timeout_us);
sepp_nepp 11:c6f95a42d4d7 797 p_Measure_Time_Budget_us += (final_range_timeout_us + final_range_overhead_us);
nikapov 0:a1a69d32f310 798 }
nikapov 0:a1a69d32f310 799 }
nikapov 0:a1a69d32f310 800
sepp_nepp 11:c6f95a42d4d7 801 if (ErrState == VL53L0X_OK)
sepp_nepp 11:c6f95a42d4d7 802 { CurrParams.Measure_Time_Budget_us = p_Measure_Time_Budget_us; }
sepp_nepp 11:c6f95a42d4d7 803
sepp_nepp 11:c6f95a42d4d7 804 return p_Measure_Time_Budget_us;
nikapov 0:a1a69d32f310 805 }
nikapov 0:a1a69d32f310 806
sepp_nepp 11:c6f95a42d4d7 807 VL53L0X_DeviceParams_t VL53L0X::Get_device_parameters()
sepp_nepp 11:c6f95a42d4d7 808 { VL53L0X_DeviceParams_t device_params = {0};
sepp_nepp 11:c6f95a42d4d7 809 int i;
sepp_nepp 11:c6f95a42d4d7 810
sepp_nepp 11:c6f95a42d4d7 811 if (ErrState != VL53L0X_OK) {return device_params; } // do nothing while in Error State!!!!
sepp_nepp 11:c6f95a42d4d7 812
sepp_nepp 11:c6f95a42d4d7 813 device_params.DeviceMode = CurrParams.DeviceMode;
sepp_nepp 11:c6f95a42d4d7 814 device_params.XTalk_Compens_En = 0;
sepp_nepp 11:c6f95a42d4d7 815 device_params.Offset_Cal_um = Get_Offset_Cal_um();
sepp_nepp 11:c6f95a42d4d7 816
sepp_nepp 11:c6f95a42d4d7 817 Get_measure_period_ms(&(device_params.Measure_Period_ms));
sepp_nepp 11:c6f95a42d4d7 818
sepp_nepp 11:c6f95a42d4d7 819 if (ErrState == VL53L0X_OK)
sepp_nepp 11:c6f95a42d4d7 820 Get_Xtalk_CompRate_MHz(&(device_params.Xtalk_CompRate_MHz));
sepp_nepp 11:c6f95a42d4d7 821
sepp_nepp 11:c6f95a42d4d7 822 if (ErrState == VL53L0X_OK) {
sepp_nepp 11:c6f95a42d4d7 823 for (i = 0; i < VL53L0X_CHECKEN_NUMBER_OF_CHECKS; i++)
sepp_nepp 11:c6f95a42d4d7 824 {/* get first the values,then the enables. GetLimitCheckValue will
sepp_nepp 11:c6f95a42d4d7 825 modify the enable flags */
sepp_nepp 11:c6f95a42d4d7 826 if (ErrState == VL53L0X_OK)
sepp_nepp 11:c6f95a42d4d7 827 { device_params.Limit_Chk_Val[i] = Get_limit_chk_val(i); }
sepp_nepp 11:c6f95a42d4d7 828 else { break; }
sepp_nepp 11:c6f95a42d4d7 829 if (ErrState == VL53L0X_OK)
sepp_nepp 11:c6f95a42d4d7 830 { device_params.Limit_Chk_En[i]= Get_limit_chk_en(i);}
sepp_nepp 10:cd251e0fc2fd 831 else { break; }
nikapov 0:a1a69d32f310 832 }
nikapov 0:a1a69d32f310 833 }
nikapov 0:a1a69d32f310 834
sepp_nepp 11:c6f95a42d4d7 835 if (ErrState == VL53L0X_OK) {
sepp_nepp 11:c6f95a42d4d7 836 device_params.Wrap_Around_Chk_En = Get_Wrap_Around_Chk_En();}
sepp_nepp 11:c6f95a42d4d7 837
sepp_nepp 11:c6f95a42d4d7 838 /* Need to be done at the end as it uses VCSELPPeriod */
sepp_nepp 11:c6f95a42d4d7 839 if (ErrState == VL53L0X_OK) {
sepp_nepp 11:c6f95a42d4d7 840 device_params.Measure_Time_Budget_us = Get_Measure_Time_Budget_us(); }
sepp_nepp 11:c6f95a42d4d7 841
sepp_nepp 11:c6f95a42d4d7 842 return device_params;
nikapov 0:a1a69d32f310 843 }
nikapov 0:a1a69d32f310 844
sepp_nepp 11:c6f95a42d4d7 845 void VL53L0X::Set_limit_chk_val(uint16_t limit_check_id, TFP1616 limit_chk_val)
sepp_nepp 11:c6f95a42d4d7 846 { /* first verify that the ID is within bounds .. */
sepp_nepp 11:c6f95a42d4d7 847 if (limit_check_id>=VL53L0X_CHECKEN_NUMBER_OF_CHECKS)
sepp_nepp 11:c6f95a42d4d7 848 { ErrState = VL53L0X_ERROR_INVALID_PARAMS; return; }
sepp_nepp 11:c6f95a42d4d7 849
sepp_nepp 11:c6f95a42d4d7 850 /* Under all other circumstances store value in local array: */
sepp_nepp 11:c6f95a42d4d7 851 CurrParams.Limit_Chk_Val[limit_check_id] = limit_chk_val;
sepp_nepp 11:c6f95a42d4d7 852
sepp_nepp 11:c6f95a42d4d7 853 /* in addition, if enabled, then write the external ones also to the Registers */
sepp_nepp 11:c6f95a42d4d7 854 if (CurrParams.Limit_Chk_En[ limit_check_id ])
nikapov 0:a1a69d32f310 855 switch (limit_check_id) {
sepp_nepp 11:c6f95a42d4d7 856 case VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE:
sepp_nepp 11:c6f95a42d4d7 857 Write_Word(REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT,
sepp_nepp 11:c6f95a42d4d7 858 FP1616_TO_FP97(limit_chk_val));
nikapov 0:a1a69d32f310 859 break;
sepp_nepp 11:c6f95a42d4d7 860 case VL53L0X_CHECKEN_SIG_RATE_MSRC:
sepp_nepp 11:c6f95a42d4d7 861 case VL53L0X_CHECKEN_SIG_RATE_PRE_RANGE:
sepp_nepp 11:c6f95a42d4d7 862 Write_Word(REG_PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT,
sepp_nepp 11:c6f95a42d4d7 863 FP1616_TO_FP97(limit_chk_val));
sepp_nepp 11:c6f95a42d4d7 864 break;
sepp_nepp 11:c6f95a42d4d7 865 } // switch
nikapov 0:a1a69d32f310 866 }
nikapov 0:a1a69d32f310 867
sepp_nepp 11:c6f95a42d4d7 868
sepp_nepp 11:c6f95a42d4d7 869
sepp_nepp 11:c6f95a42d4d7 870
sepp_nepp 11:c6f95a42d4d7 871 void VL53L0X::Get_interrupt_mask_status(uint32_t *p_interrupt_mask_status)
sepp_nepp 11:c6f95a42d4d7 872 { uint8_t intStat;
sepp_nepp 11:c6f95a42d4d7 873
sepp_nepp 11:c6f95a42d4d7 874 intStat = Read_Byte(REG_RESULT_INTERRUPT_STATUS);
sepp_nepp 11:c6f95a42d4d7 875 *p_interrupt_mask_status = intStat & 0x07;
sepp_nepp 11:c6f95a42d4d7 876 if (intStat & 0x18) { ErrState = VL53L0X_ERROR_RANGE_ERROR; }
sepp_nepp 11:c6f95a42d4d7 877 }
sepp_nepp 11:c6f95a42d4d7 878
sepp_nepp 11:c6f95a42d4d7 879 uint8_t VL53L0X::Get_Measurement_Ready()
sepp_nepp 11:c6f95a42d4d7 880 { uint8_t sys_range_status_register;
sepp_nepp 11:c6f95a42d4d7 881 uint32_t interrupt_mask;
sepp_nepp 11:c6f95a42d4d7 882
sepp_nepp 12:aa177f0e4c10 883 if (GpioFunctionality == REG_SYSINT_GPIO_NEW_SAMPLE_READY)
sepp_nepp 11:c6f95a42d4d7 884 { Get_interrupt_mask_status(&interrupt_mask);
sepp_nepp 11:c6f95a42d4d7 885 if (interrupt_mask == REG_SYSINT_GPIO_NEW_SAMPLE_READY)
sepp_nepp 11:c6f95a42d4d7 886 { return 1; } else { return 0; }
sepp_nepp 11:c6f95a42d4d7 887 }
sepp_nepp 11:c6f95a42d4d7 888 else
sepp_nepp 11:c6f95a42d4d7 889 { sys_range_status_register = Read_Byte(REG_RESULT_RANGE_STATUS);
sepp_nepp 11:c6f95a42d4d7 890 if ( ( ErrState == VL53L0X_OK ) & (sys_range_status_register & 0x01) )
sepp_nepp 11:c6f95a42d4d7 891 { return 1; } else { return 0; }
sepp_nepp 10:cd251e0fc2fd 892 }
nikapov 0:a1a69d32f310 893 }
nikapov 0:a1a69d32f310 894
sepp_nepp 11:c6f95a42d4d7 895 void VL53L0X::Polling_delay()
nikapov 0:a1a69d32f310 896 {
sepp_nepp 11:c6f95a42d4d7 897 // do nothing VL53L0X_OsDelay();
nikapov 0:a1a69d32f310 898 }
nikapov 0:a1a69d32f310 899
sepp_nepp 11:c6f95a42d4d7 900 void VL53L0X::Poll_Measure_Completion()
sepp_nepp 11:c6f95a42d4d7 901 { uint8_t new_data_ready;
sepp_nepp 11:c6f95a42d4d7 902 uint32_t loop_nb = 0;
sepp_nepp 11:c6f95a42d4d7 903
sepp_nepp 11:c6f95a42d4d7 904 if (ErrState != VL53L0X_OK) { return; } // Do nothing if not Cleared error
sepp_nepp 11:c6f95a42d4d7 905
sepp_nepp 11:c6f95a42d4d7 906 new_data_ready = Get_Measurement_Ready();
sepp_nepp 11:c6f95a42d4d7 907
sepp_nepp 11:c6f95a42d4d7 908 while ((ErrState==0) && (new_data_ready != 1) )
sepp_nepp 11:c6f95a42d4d7 909 { Polling_delay();
sepp_nepp 11:c6f95a42d4d7 910 new_data_ready = Get_Measurement_Ready();
sepp_nepp 11:c6f95a42d4d7 911 if (loop_nb++ >= VL53L0X_DEFAULT_MAX_LOOP) ErrState=VL53L0X_ERROR_TIME_OUT;
sepp_nepp 11:c6f95a42d4d7 912 } // while ;
nikapov 0:a1a69d32f310 913 }
nikapov 0:a1a69d32f310 914
sepp_nepp 11:c6f95a42d4d7 915 /* Group Device Interrupt Functions */
sepp_nepp 11:c6f95a42d4d7 916 void VL53L0X::Clear_interrupt_mask(uint32_t interrupt_mask)
sepp_nepp 11:c6f95a42d4d7 917 { uint8_t loop_count = 0;
nikapov 0:a1a69d32f310 918 uint8_t byte;
nikapov 0:a1a69d32f310 919
sepp_nepp 11:c6f95a42d4d7 920 if (ErrState != VL53L0X_OK) { return; } // Do nothing if not Cleared error
sepp_nepp 11:c6f95a42d4d7 921
sepp_nepp 7:41cbc431e1f4 922 /* clear bit 0 range interrupt,bit 1 error interrupt */
nikapov 0:a1a69d32f310 923 do {
sepp_nepp 11:c6f95a42d4d7 924 Write_Byte(REG_SYSINT_CLEAR,0x01);
sepp_nepp 11:c6f95a42d4d7 925 Write_Byte(REG_SYSINT_CLEAR,0x00);
sepp_nepp 11:c6f95a42d4d7 926 byte = Read_Byte(REG_RESULT_INTERRUPT_STATUS);
sepp_nepp 11:c6f95a42d4d7 927 if (loop_count++ > 3) {ErrState =VL53L0X_ERROR_INTERRUPT_NOT_CLEARED;}
sepp_nepp 11:c6f95a42d4d7 928 } while (((byte & 0x07) != 0x00) && (ErrState == VL53L0X_OK));
sepp_nepp 11:c6f95a42d4d7 929
nikapov 0:a1a69d32f310 930 }
nikapov 0:a1a69d32f310 931
sepp_nepp 11:c6f95a42d4d7 932 void VL53L0X::Perf_single_Ref_calibration(uint8_t vhv_init_byte)
sepp_nepp 11:c6f95a42d4d7 933 { if (ErrState != VL53L0X_OK) {return; } // no activity while in Error State!!!!
sepp_nepp 11:c6f95a42d4d7 934 Write_Byte(REG_SYSRANGE_START, REG_SYSRANGE_MODE_START_STOP | vhv_init_byte);
sepp_nepp 11:c6f95a42d4d7 935 Poll_Measure_Completion();
sepp_nepp 11:c6f95a42d4d7 936 Clear_interrupt_mask(0);
sepp_nepp 11:c6f95a42d4d7 937 Write_Byte(REG_SYSRANGE_START,0x00);
nikapov 0:a1a69d32f310 938 }
nikapov 0:a1a69d32f310 939
sepp_nepp 11:c6f95a42d4d7 940 void VL53L0X::Ref_calibration_io(uint8_t read_not_write,
sepp_nepp 7:41cbc431e1f4 941 uint8_t vhv_settings,uint8_t phase_cal,
sepp_nepp 7:41cbc431e1f4 942 uint8_t *p_vhv_settings,uint8_t *p_phase_cal,
sepp_nepp 7:41cbc431e1f4 943 const uint8_t vhv_enable,const uint8_t phase_enable)
sepp_nepp 11:c6f95a42d4d7 944 { uint8_t phase_calint = 0;
nikapov 0:a1a69d32f310 945
nikapov 0:a1a69d32f310 946 /* Read VHV from device */
sepp_nepp 11:c6f95a42d4d7 947 Write_Byte(0xFF,0x01);
sepp_nepp 11:c6f95a42d4d7 948 Write_Byte(0x00,0x00);
sepp_nepp 11:c6f95a42d4d7 949 Write_Byte(0xFF,0x00);
nikapov 0:a1a69d32f310 950
nikapov 0:a1a69d32f310 951 if (read_not_write) {
sepp_nepp 11:c6f95a42d4d7 952 if (vhv_enable ) { *p_vhv_settings = Read_Byte(0xCB); }
sepp_nepp 11:c6f95a42d4d7 953 if (phase_enable) { phase_calint = Read_Byte(0xEE); }
sepp_nepp 11:c6f95a42d4d7 954 }
sepp_nepp 11:c6f95a42d4d7 955 else {
sepp_nepp 11:c6f95a42d4d7 956 if (vhv_enable ) { Write_Byte(0xCB,vhv_settings); }
sepp_nepp 11:c6f95a42d4d7 957 if (phase_enable) { Register_BitMask(0xEE,0x80,phase_cal); }
sepp_nepp 11:c6f95a42d4d7 958 }
sepp_nepp 11:c6f95a42d4d7 959
sepp_nepp 11:c6f95a42d4d7 960 Write_Byte(0xFF,0x01);
sepp_nepp 11:c6f95a42d4d7 961 Write_Byte(0x00,0x01);
sepp_nepp 11:c6f95a42d4d7 962 Write_Byte(0xFF,0x00);
nikapov 0:a1a69d32f310 963
nikapov 0:a1a69d32f310 964 *p_phase_cal = (uint8_t)(phase_calint & 0xEF);
nikapov 0:a1a69d32f310 965 }
nikapov 0:a1a69d32f310 966
sepp_nepp 11:c6f95a42d4d7 967 void VL53L0X::Perf_vhv_calibration(uint8_t *p_vhv_settings,
sepp_nepp 11:c6f95a42d4d7 968 const uint8_t get_data_enable, const uint8_t restore_config)
sepp_nepp 11:c6f95a42d4d7 969 { uint8_t orig_sequence_config = 0;
nikapov 0:a1a69d32f310 970 uint8_t vhv_settings = 0;
nikapov 0:a1a69d32f310 971 uint8_t phase_cal = 0;
nikapov 0:a1a69d32f310 972 uint8_t phase_cal_int = 0;
nikapov 0:a1a69d32f310 973
nikapov 0:a1a69d32f310 974 /* store the value of the sequence config,
sepp_nepp 11:c6f95a42d4d7 975 * this will be reset before the end of the function */
sepp_nepp 11:c6f95a42d4d7 976 orig_sequence_config = SequenceConfig;
nikapov 0:a1a69d32f310 977
nikapov 0:a1a69d32f310 978 /* Run VHV */
sepp_nepp 11:c6f95a42d4d7 979 Set_SequenceConfig( 0x01 );
sepp_nepp 11:c6f95a42d4d7 980 Perf_single_Ref_calibration(0x40);
nikapov 0:a1a69d32f310 981
nikapov 0:a1a69d32f310 982 /* Read VHV from device */
sepp_nepp 11:c6f95a42d4d7 983 if ((ErrState == VL53L0X_OK) && (get_data_enable == 1))
sepp_nepp 11:c6f95a42d4d7 984 { Ref_calibration_io(1,vhv_settings,phase_cal,/* Not used here */
sepp_nepp 11:c6f95a42d4d7 985 p_vhv_settings,&phase_cal_int, 1,0); }
sepp_nepp 11:c6f95a42d4d7 986 else { *p_vhv_settings = 0; }
sepp_nepp 11:c6f95a42d4d7 987
sepp_nepp 11:c6f95a42d4d7 988 if (restore_config) { /* restore the previous Sequence Config */
sepp_nepp 11:c6f95a42d4d7 989 Set_SequenceConfig( orig_sequence_config ); } // checks for ErrState
nikapov 0:a1a69d32f310 990 }
nikapov 0:a1a69d32f310 991
sepp_nepp 11:c6f95a42d4d7 992 void VL53L0X::Perf_phase_calibration(uint8_t *p_phase_cal,const uint8_t get_data_enable,
nikapov 0:a1a69d32f310 993 const uint8_t restore_config)
sepp_nepp 11:c6f95a42d4d7 994 { uint8_t orig_sequence_config;
nikapov 0:a1a69d32f310 995 uint8_t vhv_settings = 0;
nikapov 0:a1a69d32f310 996 uint8_t phase_cal = 0;
nikapov 0:a1a69d32f310 997 uint8_t vhv_settingsint;
nikapov 0:a1a69d32f310 998
sepp_nepp 11:c6f95a42d4d7 999 if (ErrState != VL53L0X_OK) { return; } // Do nothing if not Cleared error
sepp_nepp 11:c6f95a42d4d7 1000
sepp_nepp 11:c6f95a42d4d7 1001 /* store the value of the sequence config, this will be reset before the end of the function */
sepp_nepp 11:c6f95a42d4d7 1002 orig_sequence_config = SequenceConfig;
sepp_nepp 11:c6f95a42d4d7 1003
sepp_nepp 11:c6f95a42d4d7 1004 /* Run PhaseCal: */
sepp_nepp 11:c6f95a42d4d7 1005 Set_SequenceConfig( 0x02 ); // sets REG_SYSTEM_SEQUENCE_CONFIG
sepp_nepp 11:c6f95a42d4d7 1006 Perf_single_Ref_calibration(0x0);
nikapov 0:a1a69d32f310 1007
nikapov 0:a1a69d32f310 1008 /* Read PhaseCal from device */
sepp_nepp 11:c6f95a42d4d7 1009 if ((ErrState == VL53L0X_OK) && (get_data_enable == 1))
sepp_nepp 11:c6f95a42d4d7 1010 { Ref_calibration_io(1,vhv_settings,phase_cal,/* Not used here */
sepp_nepp 11:c6f95a42d4d7 1011 &vhv_settingsint,p_phase_cal, 0,1); }
sepp_nepp 11:c6f95a42d4d7 1012 else { *p_phase_cal = 0; }
sepp_nepp 11:c6f95a42d4d7 1013
sepp_nepp 11:c6f95a42d4d7 1014 if (restore_config) { /* restore the previous Sequence Config */
sepp_nepp 11:c6f95a42d4d7 1015 Set_SequenceConfig( orig_sequence_config ); }
nikapov 0:a1a69d32f310 1016 }
nikapov 0:a1a69d32f310 1017
sepp_nepp 11:c6f95a42d4d7 1018 void VL53L0X::Perf_Ref_calibration(uint8_t *p_vhv_settings,
sepp_nepp 10:cd251e0fc2fd 1019 uint8_t *p_phase_cal, uint8_t get_data_enable)
sepp_nepp 11:c6f95a42d4d7 1020 { uint8_t orig_sequence_config;
nikapov 0:a1a69d32f310 1021
nikapov 0:a1a69d32f310 1022 /* store the value of the sequence config,
sepp_nepp 10:cd251e0fc2fd 1023 * this will be reset before the end of the function */
sepp_nepp 11:c6f95a42d4d7 1024 orig_sequence_config = SequenceConfig;
nikapov 0:a1a69d32f310 1025
nikapov 0:a1a69d32f310 1026 /* In the following function we don't save the config to optimize
nikapov 0:a1a69d32f310 1027 * writes on device. Config is saved and restored only once. */
sepp_nepp 11:c6f95a42d4d7 1028 Perf_vhv_calibration(p_vhv_settings,get_data_enable,0);
sepp_nepp 11:c6f95a42d4d7 1029 Perf_phase_calibration(p_phase_cal,get_data_enable,0);
sepp_nepp 11:c6f95a42d4d7 1030
sepp_nepp 11:c6f95a42d4d7 1031 /* restore the previous Sequence Config */
sepp_nepp 11:c6f95a42d4d7 1032 Set_SequenceConfig( orig_sequence_config ); // sets REG_SYSTEM_SEQUENCE_CONFIG
nikapov 0:a1a69d32f310 1033 }
nikapov 0:a1a69d32f310 1034
sepp_nepp 11:c6f95a42d4d7 1035 void VL53L0X::Get_Next_Good_SPAD(uint8_t good_SPAD_array[],uint32_t size,
sepp_nepp 7:41cbc431e1f4 1036 uint32_t curr,int32_t *p_next)
sepp_nepp 10:cd251e0fc2fd 1037 { uint32_t start_index;
nikapov 0:a1a69d32f310 1038 uint32_t fine_offset;
sepp_nepp 11:c6f95a42d4d7 1039 uint32_t c_SPADS_per_byte = 8;
nikapov 0:a1a69d32f310 1040 uint32_t coarse_index;
nikapov 0:a1a69d32f310 1041 uint32_t fine_index;
nikapov 0:a1a69d32f310 1042 uint8_t data_byte;
nikapov 0:a1a69d32f310 1043 uint8_t success = 0;
nikapov 0:a1a69d32f310 1044
sepp_nepp 11:c6f95a42d4d7 1045 /* Starting with the current good SPAD,loop through the array to find
nikapov 0:a1a69d32f310 1046 * the next. i.e. the next bit set in the sequence.
nikapov 0:a1a69d32f310 1047 * The coarse index is the byte index of the array and the fine index is
sepp_nepp 11:c6f95a42d4d7 1048 * the index of the bit within each byte. */
nikapov 0:a1a69d32f310 1049 *p_next = -1;
nikapov 0:a1a69d32f310 1050
sepp_nepp 11:c6f95a42d4d7 1051 start_index = curr / c_SPADS_per_byte;
sepp_nepp 11:c6f95a42d4d7 1052 fine_offset = curr % c_SPADS_per_byte;
nikapov 0:a1a69d32f310 1053
nikapov 0:a1a69d32f310 1054 for (coarse_index = start_index; ((coarse_index < size) && !success);
nikapov 0:a1a69d32f310 1055 coarse_index++) {
nikapov 0:a1a69d32f310 1056 fine_index = 0;
sepp_nepp 11:c6f95a42d4d7 1057 data_byte = good_SPAD_array[coarse_index];
nikapov 0:a1a69d32f310 1058
nikapov 0:a1a69d32f310 1059 if (coarse_index == start_index) {
nikapov 0:a1a69d32f310 1060 /* locate the bit position of the provided current
sepp_nepp 11:c6f95a42d4d7 1061 * SPAD bit before iterating */
nikapov 0:a1a69d32f310 1062 data_byte >>= fine_offset;
nikapov 0:a1a69d32f310 1063 fine_index = fine_offset;
nikapov 0:a1a69d32f310 1064 }
nikapov 0:a1a69d32f310 1065
sepp_nepp 11:c6f95a42d4d7 1066 while (fine_index < c_SPADS_per_byte) {
nikapov 0:a1a69d32f310 1067 if ((data_byte & 0x1) == 1) {
nikapov 0:a1a69d32f310 1068 success = 1;
sepp_nepp 11:c6f95a42d4d7 1069 *p_next = coarse_index * c_SPADS_per_byte + fine_index;
nikapov 0:a1a69d32f310 1070 break;
nikapov 0:a1a69d32f310 1071 }
nikapov 0:a1a69d32f310 1072 data_byte >>= 1;
nikapov 0:a1a69d32f310 1073 fine_index++;
nikapov 0:a1a69d32f310 1074 }
nikapov 0:a1a69d32f310 1075 }
nikapov 0:a1a69d32f310 1076 }
nikapov 0:a1a69d32f310 1077
sepp_nepp 11:c6f95a42d4d7 1078 void VL53L0X::Enable_SPAD_bit(uint8_t SPAD_array[],uint32_t size,uint32_t SPAD_index)
sepp_nepp 11:c6f95a42d4d7 1079 { uint32_t c_SPADS_per_byte = 8;
nikapov 0:a1a69d32f310 1080 uint32_t coarse_index;
nikapov 0:a1a69d32f310 1081 uint32_t fine_index;
nikapov 0:a1a69d32f310 1082
sepp_nepp 11:c6f95a42d4d7 1083 coarse_index = SPAD_index / c_SPADS_per_byte;
sepp_nepp 11:c6f95a42d4d7 1084 fine_index = SPAD_index % c_SPADS_per_byte;
sepp_nepp 11:c6f95a42d4d7 1085 if (coarse_index >= size) { ErrState = VL53L0X_ERROR_REF_SPAD_INIT; }
sepp_nepp 11:c6f95a42d4d7 1086 else { SPAD_array[coarse_index] |= (1 << fine_index); }
nikapov 0:a1a69d32f310 1087 }
nikapov 0:a1a69d32f310 1088
sepp_nepp 11:c6f95a42d4d7 1089 void VL53L0X::Enable_Ref_SPADS( uint8_t aperture_SPADS, uint8_t good_SPAD_array[],
sepp_nepp 11:c6f95a42d4d7 1090 uint8_t SPAD_array[], uint32_t size, uint32_t start, uint32_t offset,
sepp_nepp 11:c6f95a42d4d7 1091 uint32_t SPAD_count, uint32_t *p_last_SPAD )
sepp_nepp 11:c6f95a42d4d7 1092 { uint32_t index;
nikapov 0:a1a69d32f310 1093 uint32_t i;
sepp_nepp 11:c6f95a42d4d7 1094 int32_t next_good_SPAD = offset;
sepp_nepp 11:c6f95a42d4d7 1095 uint32_t current_SPAD;
sepp_nepp 11:c6f95a42d4d7 1096 uint8_t check_SPAD_array[6];
sepp_nepp 11:c6f95a42d4d7 1097
sepp_nepp 11:c6f95a42d4d7 1098 /* This function takes in a SPAD array which may or may not have SPADS
nikapov 0:a1a69d32f310 1099 * already enabled and appends from a given offset a requested number
sepp_nepp 11:c6f95a42d4d7 1100 * of new SPAD enables. The 'good SPAD map' is applied to
sepp_nepp 11:c6f95a42d4d7 1101 * determine the next SPADS to enable.
nikapov 0:a1a69d32f310 1102 *
sepp_nepp 11:c6f95a42d4d7 1103 * This function applies to only aperture or only non-aperture SPADS.
nikapov 0:a1a69d32f310 1104 * Checks are performed to ensure this.
nikapov 0:a1a69d32f310 1105 */
nikapov 0:a1a69d32f310 1106
sepp_nepp 11:c6f95a42d4d7 1107 current_SPAD = offset;
sepp_nepp 11:c6f95a42d4d7 1108 for (index = 0; index < SPAD_count; index++) {
sepp_nepp 11:c6f95a42d4d7 1109 Get_Next_Good_SPAD(good_SPAD_array,size,current_SPAD, &next_good_SPAD);
sepp_nepp 11:c6f95a42d4d7 1110
sepp_nepp 11:c6f95a42d4d7 1111 if (next_good_SPAD == -1)
sepp_nepp 11:c6f95a42d4d7 1112 { ErrState = VL53L0X_ERROR_REF_SPAD_INIT;
sepp_nepp 11:c6f95a42d4d7 1113 break; }
sepp_nepp 11:c6f95a42d4d7 1114
nikapov 0:a1a69d32f310 1115
nikapov 0:a1a69d32f310 1116 /* Confirm that the next good SPAD is non-aperture */
sepp_nepp 11:c6f95a42d4d7 1117 if (Is_ApertureSPAD(start + next_good_SPAD) != aperture_SPADS) {
nikapov 0:a1a69d32f310 1118 /* if we can't get the required number of good aperture
sepp_nepp 11:c6f95a42d4d7 1119 * SPADS from the current quadrant then this is an error */
sepp_nepp 11:c6f95a42d4d7 1120 ErrState = VL53L0X_ERROR_REF_SPAD_INIT;
sepp_nepp 11:c6f95a42d4d7 1121 break;}
sepp_nepp 11:c6f95a42d4d7 1122
sepp_nepp 11:c6f95a42d4d7 1123 current_SPAD = (uint32_t)next_good_SPAD;
sepp_nepp 11:c6f95a42d4d7 1124 Enable_SPAD_bit(SPAD_array,size,current_SPAD);
sepp_nepp 11:c6f95a42d4d7 1125 current_SPAD++;
nikapov 0:a1a69d32f310 1126 }
sepp_nepp 11:c6f95a42d4d7 1127 *p_last_SPAD = current_SPAD;
sepp_nepp 11:c6f95a42d4d7 1128
sepp_nepp 11:c6f95a42d4d7 1129 if (ErrState == VL53L0X_OK)
sepp_nepp 11:c6f95a42d4d7 1130 { I2c_Write(REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0, SPAD_array,6); } // set_Ref_SPAD_map()
sepp_nepp 11:c6f95a42d4d7 1131
sepp_nepp 11:c6f95a42d4d7 1132 if (ErrState == VL53L0X_OK) {
sepp_nepp 11:c6f95a42d4d7 1133 // Get the ref_SPAD_map from the device
sepp_nepp 11:c6f95a42d4d7 1134 I2c_Read(REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0,check_SPAD_array,6);
sepp_nepp 11:c6f95a42d4d7 1135
sepp_nepp 11:c6f95a42d4d7 1136 /* Compare SPAD maps. If not equal report error. */
nikapov 0:a1a69d32f310 1137 i = 0;
nikapov 0:a1a69d32f310 1138 while (i < size) {
sepp_nepp 11:c6f95a42d4d7 1139 if (SPAD_array[i] != check_SPAD_array[i]) {
sepp_nepp 11:c6f95a42d4d7 1140 ErrState = VL53L0X_ERROR_REF_SPAD_INIT;
nikapov 0:a1a69d32f310 1141 break;
nikapov 0:a1a69d32f310 1142 }
nikapov 0:a1a69d32f310 1143 i++;
nikapov 0:a1a69d32f310 1144 }
nikapov 0:a1a69d32f310 1145 }
nikapov 0:a1a69d32f310 1146 }
nikapov 0:a1a69d32f310 1147
sepp_nepp 11:c6f95a42d4d7 1148 void VL53L0X::Set_device_mode(VL53L0X_DeviceModes device_mode)
sepp_nepp 11:c6f95a42d4d7 1149 { if (ErrState != VL53L0X_OK) {return; } // no reaction while in Error State!!!!
nikapov 0:a1a69d32f310 1150
nikapov 0:a1a69d32f310 1151 switch (device_mode) {
nikapov 0:a1a69d32f310 1152 case VL53L0X_DEVICEMODE_SINGLE_RANGING:
nikapov 0:a1a69d32f310 1153 case VL53L0X_DEVICEMODE_CONTINUOUS_RANGING:
nikapov 0:a1a69d32f310 1154 case VL53L0X_DEVICEMODE_CONTINUOUS_TIMED_RANGING:
nikapov 0:a1a69d32f310 1155 case VL53L0X_DEVICEMODE_GPIO_DRIVE:
sepp_nepp 11:c6f95a42d4d7 1156 case VL53L0X_DEVICEMODE_GPIO_OSC: /* Supported modes */
sepp_nepp 11:c6f95a42d4d7 1157 CurrParams.DeviceMode = device_mode;
nikapov 0:a1a69d32f310 1158 break;
sepp_nepp 11:c6f95a42d4d7 1159 default: /* Unsupported mode */
sepp_nepp 11:c6f95a42d4d7 1160 ErrState = VL53L0X_ERROR_MODE_NOT_SUPPORTED;
nikapov 0:a1a69d32f310 1161 }
nikapov 0:a1a69d32f310 1162 }
nikapov 0:a1a69d32f310 1163
sepp_nepp 11:c6f95a42d4d7 1164 void VL53L0X::Set_interrupt_thresholds(VL53L0X_DeviceModes device_mode,TFP1616 threshold_low,
sepp_nepp 11:c6f95a42d4d7 1165 TFP1616 threshold_high)
sepp_nepp 11:c6f95a42d4d7 1166 { uint16_t threshold16;
sepp_nepp 5:b95f6951f7d5 1167
sepp_nepp 5:b95f6951f7d5 1168 /* no dependency on DeviceMode for FlightSense */
nikapov 0:a1a69d32f310 1169 /* Need to divide by 2 because the FW will apply a x2 */
nikapov 0:a1a69d32f310 1170 threshold16 = (uint16_t)((threshold_low >> 17) & 0x00fff);
sepp_nepp 11:c6f95a42d4d7 1171 Write_Word(REG_SYSTEM_THRESH_LOW,threshold16);
sepp_nepp 11:c6f95a42d4d7 1172
sepp_nepp 11:c6f95a42d4d7 1173 /* Need to divide by 2 because the FW will apply a x2 */
sepp_nepp 11:c6f95a42d4d7 1174 threshold16 = (uint16_t)((threshold_high >> 17) & 0x00fff);
sepp_nepp 11:c6f95a42d4d7 1175 Write_Word(REG_SYSTEM_THRESH_HIGH,threshold16);
nikapov 0:a1a69d32f310 1176 }
nikapov 0:a1a69d32f310 1177
sepp_nepp 11:c6f95a42d4d7 1178 void VL53L0X::Get_interrupt_thresholds(VL53L0X_DeviceModes device_mode,TFP1616 *p_threshold_low,
sepp_nepp 11:c6f95a42d4d7 1179 TFP1616 *p_threshold_high)
sepp_nepp 11:c6f95a42d4d7 1180 { uint16_t threshold16;
sepp_nepp 5:b95f6951f7d5 1181
sepp_nepp 5:b95f6951f7d5 1182 /* no dependency on DeviceMode for FlightSense */
sepp_nepp 11:c6f95a42d4d7 1183 threshold16 = Read_Word(REG_SYSTEM_THRESH_LOW);
nikapov 0:a1a69d32f310 1184 /* Need to multiply by 2 because the FW will apply a x2 */
sepp_nepp 11:c6f95a42d4d7 1185 *p_threshold_low = (TFP1616)((0x00fff & threshold16) << 17);
sepp_nepp 11:c6f95a42d4d7 1186
sepp_nepp 11:c6f95a42d4d7 1187 if (ErrState == VL53L0X_OK) {
sepp_nepp 11:c6f95a42d4d7 1188 threshold16 = Read_Word(REG_SYSTEM_THRESH_HIGH);
nikapov 0:a1a69d32f310 1189 /* Need to multiply by 2 because the FW will apply a x2 */
sepp_nepp 11:c6f95a42d4d7 1190 *p_threshold_high = (TFP1616)((0x00fff & threshold16) << 17);
nikapov 0:a1a69d32f310 1191 }
nikapov 0:a1a69d32f310 1192 }
nikapov 0:a1a69d32f310 1193
sepp_nepp 11:c6f95a42d4d7 1194 void VL53L0X::Load_tuning_settings(uint8_t *p_tuning_setting_buffer)
sepp_nepp 11:c6f95a42d4d7 1195 { int i;
nikapov 0:a1a69d32f310 1196 int index;
nikapov 0:a1a69d32f310 1197 uint8_t msb;
nikapov 0:a1a69d32f310 1198 uint8_t lsb;
nikapov 0:a1a69d32f310 1199 uint8_t select_param;
nikapov 0:a1a69d32f310 1200 uint8_t number_of_writes;
nikapov 0:a1a69d32f310 1201 uint8_t address;
nikapov 0:a1a69d32f310 1202 uint8_t local_buffer[4]; /* max */
nikapov 0:a1a69d32f310 1203 uint16_t temp16;
nikapov 0:a1a69d32f310 1204
nikapov 0:a1a69d32f310 1205 index = 0;
nikapov 0:a1a69d32f310 1206
nikapov 0:a1a69d32f310 1207 while ((*(p_tuning_setting_buffer + index) != 0) &&
sepp_nepp 11:c6f95a42d4d7 1208 (ErrState == VL53L0X_OK)) {
nikapov 0:a1a69d32f310 1209 number_of_writes = *(p_tuning_setting_buffer + index);
nikapov 0:a1a69d32f310 1210 index++;
nikapov 0:a1a69d32f310 1211 if (number_of_writes == 0xFF) {
nikapov 0:a1a69d32f310 1212 /* internal parameters */
nikapov 0:a1a69d32f310 1213 select_param = *(p_tuning_setting_buffer + index);
nikapov 0:a1a69d32f310 1214 index++;
nikapov 0:a1a69d32f310 1215 switch (select_param) {
nikapov 0:a1a69d32f310 1216 case 0: /* uint16_t SigmaEstRefArray -> 2 bytes */
nikapov 0:a1a69d32f310 1217 msb = *(p_tuning_setting_buffer + index);
nikapov 0:a1a69d32f310 1218 index++;
nikapov 0:a1a69d32f310 1219 lsb = *(p_tuning_setting_buffer + index);
nikapov 0:a1a69d32f310 1220 index++;
sepp_nepp 7:41cbc431e1f4 1221 temp16 = VL53L0X_MAKEUINT16(lsb,msb);
sepp_nepp 10:cd251e0fc2fd 1222 SigmaEstRefArray = temp16;
nikapov 0:a1a69d32f310 1223 break;
nikapov 0:a1a69d32f310 1224 case 1: /* uint16_t SigmaEstEffPulseWidth -> 2 bytes */
nikapov 0:a1a69d32f310 1225 msb = *(p_tuning_setting_buffer + index);
nikapov 0:a1a69d32f310 1226 index++;
nikapov 0:a1a69d32f310 1227 lsb = *(p_tuning_setting_buffer + index);
nikapov 0:a1a69d32f310 1228 index++;
sepp_nepp 7:41cbc431e1f4 1229 temp16 = VL53L0X_MAKEUINT16(lsb,msb);
sepp_nepp 10:cd251e0fc2fd 1230 SigmaEstEffPulseWidth = temp16;
nikapov 0:a1a69d32f310 1231 break;
nikapov 0:a1a69d32f310 1232 case 2: /* uint16_t SigmaEstEffAmbWidth -> 2 bytes */
nikapov 0:a1a69d32f310 1233 msb = *(p_tuning_setting_buffer + index);
nikapov 0:a1a69d32f310 1234 index++;
nikapov 0:a1a69d32f310 1235 lsb = *(p_tuning_setting_buffer + index);
nikapov 0:a1a69d32f310 1236 index++;
sepp_nepp 7:41cbc431e1f4 1237 temp16 = VL53L0X_MAKEUINT16(lsb,msb);
sepp_nepp 10:cd251e0fc2fd 1238 SigmaEstEffAmbWidth = temp16;
nikapov 0:a1a69d32f310 1239 break;
nikapov 0:a1a69d32f310 1240 case 3: /* uint16_t targetRefRate -> 2 bytes */
nikapov 0:a1a69d32f310 1241 msb = *(p_tuning_setting_buffer + index);
nikapov 0:a1a69d32f310 1242 index++;
nikapov 0:a1a69d32f310 1243 lsb = *(p_tuning_setting_buffer + index);
nikapov 0:a1a69d32f310 1244 index++;
sepp_nepp 7:41cbc431e1f4 1245 temp16 = VL53L0X_MAKEUINT16(lsb,msb);
sepp_nepp 10:cd251e0fc2fd 1246 targetRefRate = temp16;
nikapov 0:a1a69d32f310 1247 break;
nikapov 0:a1a69d32f310 1248 default: /* invalid parameter */
sepp_nepp 11:c6f95a42d4d7 1249 ErrState = VL53L0X_ERROR_INVALID_PARAMS;
nikapov 0:a1a69d32f310 1250 }
nikapov 0:a1a69d32f310 1251 } else if (number_of_writes <= 4) {
nikapov 0:a1a69d32f310 1252 address = *(p_tuning_setting_buffer + index);
nikapov 0:a1a69d32f310 1253 index++;
nikapov 0:a1a69d32f310 1254 for (i = 0; i < number_of_writes; i++) {
sepp_nepp 11:c6f95a42d4d7 1255 local_buffer[i] = *(p_tuning_setting_buffer + index);
nikapov 0:a1a69d32f310 1256 index++;
nikapov 0:a1a69d32f310 1257 }
sepp_nepp 11:c6f95a42d4d7 1258 I2c_Write(address,local_buffer,number_of_writes);
nikapov 0:a1a69d32f310 1259 } else {
sepp_nepp 11:c6f95a42d4d7 1260 ErrState = VL53L0X_ERROR_INVALID_PARAMS;
nikapov 0:a1a69d32f310 1261 }
nikapov 0:a1a69d32f310 1262 }
sepp_nepp 11:c6f95a42d4d7 1263 }
sepp_nepp 11:c6f95a42d4d7 1264
sepp_nepp 11:c6f95a42d4d7 1265 void VL53L0X::Check_and_load_interrupt_settings(uint8_t start_not_stopflag)
sepp_nepp 11:c6f95a42d4d7 1266 { uint8_t interrupt_config;
sepp_nepp 11:c6f95a42d4d7 1267 TFP1616 threshold_low;
sepp_nepp 11:c6f95a42d4d7 1268 TFP1616 threshold_high;
sepp_nepp 5:b95f6951f7d5 1269
sepp_nepp 11:c6f95a42d4d7 1270 if (ErrState != VL53L0X_OK) { return; } // Do nothing if not Cleared error
sepp_nepp 11:c6f95a42d4d7 1271
sepp_nepp 12:aa177f0e4c10 1272 interrupt_config = GpioFunctionality;
sepp_nepp 11:c6f95a42d4d7 1273
sepp_nepp 11:c6f95a42d4d7 1274 if ((interrupt_config == GPIO_FUNC_THRESHOLD_CROSSED_LOW ) ||
sepp_nepp 11:c6f95a42d4d7 1275 (interrupt_config == GPIO_FUNC_THRESHOLD_CROSSED_HIGH) ||
sepp_nepp 11:c6f95a42d4d7 1276 (interrupt_config == GPIO_FUNC_THRESHOLD_CROSSED_OUT )) {
sepp_nepp 11:c6f95a42d4d7 1277 Get_interrupt_thresholds(VL53L0X_DEVICEMODE_CONTINUOUS_RANGING,
sepp_nepp 7:41cbc431e1f4 1278 &threshold_low,&threshold_high);
nikapov 0:a1a69d32f310 1279
sepp_nepp 11:c6f95a42d4d7 1280 if (((threshold_low > 255 * 65536) || (threshold_high > 255 * 65536)) &&
sepp_nepp 11:c6f95a42d4d7 1281 (ErrState == VL53L0X_OK))
sepp_nepp 11:c6f95a42d4d7 1282 { if (start_not_stopflag != 0)
sepp_nepp 11:c6f95a42d4d7 1283 {Load_tuning_settings(InterruptThresholdSettings); }
sepp_nepp 11:c6f95a42d4d7 1284 else
sepp_nepp 11:c6f95a42d4d7 1285 {Write_Byte(0xFF,0x04);
sepp_nepp 11:c6f95a42d4d7 1286 Write_Byte(0x70,0x00);
sepp_nepp 11:c6f95a42d4d7 1287 Write_Byte(0xFF,0x00);
sepp_nepp 11:c6f95a42d4d7 1288 Write_Byte(0x80,0x00);
sepp_nepp 11:c6f95a42d4d7 1289 }
nikapov 0:a1a69d32f310 1290 }
nikapov 0:a1a69d32f310 1291 }
nikapov 0:a1a69d32f310 1292 }
nikapov 0:a1a69d32f310 1293
sepp_nepp 11:c6f95a42d4d7 1294 void VL53L0X::Start_Measurement()
sepp_nepp 11:c6f95a42d4d7 1295 { VL53L0X_DeviceModes device_mode;
nikapov 0:a1a69d32f310 1296 uint8_t byte;
sepp_nepp 11:c6f95a42d4d7 1297 uint8_t start_stop_byte = REG_SYSRANGE_MODE_START_STOP;
nikapov 0:a1a69d32f310 1298 uint32_t loop_nb;
nikapov 0:a1a69d32f310 1299
sepp_nepp 11:c6f95a42d4d7 1300 if (ErrState != VL53L0X_OK) {return; } // no activity while in Error State!!!!
sepp_nepp 11:c6f95a42d4d7 1301
nikapov 0:a1a69d32f310 1302 /* Get Current DeviceMode */
sepp_nepp 11:c6f95a42d4d7 1303 device_mode = CurrParams.DeviceMode;
sepp_nepp 11:c6f95a42d4d7 1304
sepp_nepp 11:c6f95a42d4d7 1305 Write_Byte(0x80,0x01);
sepp_nepp 11:c6f95a42d4d7 1306 Write_Byte(0xFF,0x01);
sepp_nepp 11:c6f95a42d4d7 1307 Write_Byte(0x00,0x00);
sepp_nepp 11:c6f95a42d4d7 1308 Write_Byte(0x91,StopVariable);
sepp_nepp 11:c6f95a42d4d7 1309 Write_Byte(0x00,0x01);
sepp_nepp 11:c6f95a42d4d7 1310 Write_Byte(0xFF,0x00);
sepp_nepp 11:c6f95a42d4d7 1311 Write_Byte(0x80,0x00);
nikapov 0:a1a69d32f310 1312
nikapov 0:a1a69d32f310 1313 switch (device_mode) {
nikapov 0:a1a69d32f310 1314 case VL53L0X_DEVICEMODE_SINGLE_RANGING:
sepp_nepp 11:c6f95a42d4d7 1315 Write_Byte(REG_SYSRANGE_START,0x01);
nikapov 0:a1a69d32f310 1316
nikapov 0:a1a69d32f310 1317 byte = start_stop_byte;
sepp_nepp 11:c6f95a42d4d7 1318 if (ErrState == VL53L0X_OK) {
nikapov 0:a1a69d32f310 1319 /* Wait until start bit has been cleared */
nikapov 0:a1a69d32f310 1320 loop_nb = 0;
nikapov 0:a1a69d32f310 1321 do {
nikapov 0:a1a69d32f310 1322 if (loop_nb > 0)
sepp_nepp 11:c6f95a42d4d7 1323 byte = Read_Byte(REG_SYSRANGE_START);
nikapov 0:a1a69d32f310 1324 loop_nb = loop_nb + 1;
nikapov 0:a1a69d32f310 1325 } while (((byte & start_stop_byte) == start_stop_byte)
sepp_nepp 11:c6f95a42d4d7 1326 && (ErrState == VL53L0X_OK)
nikapov 0:a1a69d32f310 1327 && (loop_nb < VL53L0X_DEFAULT_MAX_LOOP));
nikapov 0:a1a69d32f310 1328
Davidroid 3:e9269ff624ed 1329 if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP) {
sepp_nepp 11:c6f95a42d4d7 1330 ErrState = VL53L0X_ERROR_TIME_OUT;
Davidroid 3:e9269ff624ed 1331 }
nikapov 0:a1a69d32f310 1332 }
nikapov 0:a1a69d32f310 1333 break;
sepp_nepp 11:c6f95a42d4d7 1334
sepp_nepp 11:c6f95a42d4d7 1335 case VL53L0X_DEVICEMODE_CONTINUOUS_RANGING:
sepp_nepp 11:c6f95a42d4d7 1336 /* Back-to-back mode, Check if need to apply interrupt settings */
sepp_nepp 11:c6f95a42d4d7 1337 Check_and_load_interrupt_settings(1);
sepp_nepp 11:c6f95a42d4d7 1338 Write_Byte(REG_SYSRANGE_START,REG_SYSRANGE_MODE_BACKTOBACK);
sepp_nepp 11:c6f95a42d4d7 1339 Set_Current_State( VL53L0X_STATE_RUNNING );
sepp_nepp 11:c6f95a42d4d7 1340 break;
nikapov 0:a1a69d32f310 1341 case VL53L0X_DEVICEMODE_CONTINUOUS_TIMED_RANGING:
sepp_nepp 11:c6f95a42d4d7 1342 /* Continuous mode; Check if need to apply interrupt settings */
sepp_nepp 11:c6f95a42d4d7 1343 Check_and_load_interrupt_settings(1);
sepp_nepp 11:c6f95a42d4d7 1344 Write_Byte(REG_SYSRANGE_START, REG_SYSRANGE_MODE_TIMED);
sepp_nepp 11:c6f95a42d4d7 1345 Set_Current_State( VL53L0X_STATE_RUNNING );
nikapov 0:a1a69d32f310 1346 break;
nikapov 0:a1a69d32f310 1347 default:
nikapov 0:a1a69d32f310 1348 /* Selected mode not supported */
sepp_nepp 11:c6f95a42d4d7 1349 ErrState = VL53L0X_ERROR_MODE_NOT_SUPPORTED;
nikapov 0:a1a69d32f310 1350 }
nikapov 0:a1a69d32f310 1351 }
nikapov 0:a1a69d32f310 1352
sepp_nepp 11:c6f95a42d4d7 1353 /* Group Device Measurement Functions */
sepp_nepp 11:c6f95a42d4d7 1354 void VL53L0X::Perf_single_measurement()
sepp_nepp 11:c6f95a42d4d7 1355 { VL53L0X_DeviceModes device_mode;
sepp_nepp 11:c6f95a42d4d7 1356
sepp_nepp 11:c6f95a42d4d7 1357 if (ErrState != VL53L0X_OK) {return; } // no activity while in Error State!!!!
nikapov 0:a1a69d32f310 1358
nikapov 0:a1a69d32f310 1359 /* Get Current DeviceMode */
sepp_nepp 11:c6f95a42d4d7 1360 device_mode = CurrParams.DeviceMode;
nikapov 0:a1a69d32f310 1361
nikapov 0:a1a69d32f310 1362 /* Start immediately to run a single ranging measurement in case of
nikapov 0:a1a69d32f310 1363 * single ranging or single histogram */
sepp_nepp 11:c6f95a42d4d7 1364 if (device_mode == VL53L0X_DEVICEMODE_SINGLE_RANGING) {Start_Measurement();}
sepp_nepp 11:c6f95a42d4d7 1365
sepp_nepp 11:c6f95a42d4d7 1366 Poll_Measure_Completion();
sepp_nepp 11:c6f95a42d4d7 1367
sepp_nepp 11:c6f95a42d4d7 1368 /* Change Device State in case of single ranging or single histogram */
sepp_nepp 11:c6f95a42d4d7 1369 if (device_mode == VL53L0X_DEVICEMODE_SINGLE_RANGING)
sepp_nepp 11:c6f95a42d4d7 1370 { Set_Current_State( VL53L0X_STATE_IDLE ); }
nikapov 0:a1a69d32f310 1371 }
nikapov 0:a1a69d32f310 1372
sepp_nepp 11:c6f95a42d4d7 1373 TFP1616 VL53L0X::Get_total_xtalk_rate(TRangeResults *p_ranging_results)
sepp_nepp 11:c6f95a42d4d7 1374 { TFP1616 total_xtalk_MHz;
sepp_nepp 11:c6f95a42d4d7 1375
sepp_nepp 11:c6f95a42d4d7 1376 // CurrParams.XTalk_Compens_En was Get_xtalk_compensation_enable
sepp_nepp 11:c6f95a42d4d7 1377 if ( (ErrState == VL53L0X_OK) & (CurrParams.XTalk_Compens_En ) )
sepp_nepp 11:c6f95a42d4d7 1378 { /* FixPoint1616 * FixPoint 8:8 = FixPoint0824 */
sepp_nepp 11:c6f95a42d4d7 1379 total_xtalk_MHz = p_ranging_results->EffectiveSPADRtnCount *
sepp_nepp 11:c6f95a42d4d7 1380 CurrParams.Xtalk_CompRate_MHz;
sepp_nepp 11:c6f95a42d4d7 1381
sepp_nepp 11:c6f95a42d4d7 1382 /* FixPoint0824 >> 8 = FixPoint1616 */
sepp_nepp 11:c6f95a42d4d7 1383 return (total_xtalk_MHz + 0x80) >> 8;
sepp_nepp 11:c6f95a42d4d7 1384 }
sepp_nepp 11:c6f95a42d4d7 1385 else { return 0; }
nikapov 0:a1a69d32f310 1386 }
nikapov 0:a1a69d32f310 1387
sepp_nepp 11:c6f95a42d4d7 1388 void VL53L0X::Get_total_SIG_rate(TRangeResults *p_ranging_results,
sepp_nepp 11:c6f95a42d4d7 1389 TFP1616 *p_total_SIG_rate_mcps)
sepp_nepp 11:c6f95a42d4d7 1390 { TFP1616 total_xtalk_MHz;
sepp_nepp 11:c6f95a42d4d7 1391
sepp_nepp 11:c6f95a42d4d7 1392 *p_total_SIG_rate_mcps = p_ranging_results->SignalRateRtnMHz;
sepp_nepp 11:c6f95a42d4d7 1393 total_xtalk_MHz = Get_total_xtalk_rate(p_ranging_results);
sepp_nepp 5:b95f6951f7d5 1394
sepp_nepp 11:c6f95a42d4d7 1395 if (ErrState == VL53L0X_OK) { *p_total_SIG_rate_mcps += total_xtalk_MHz;}
nikapov 0:a1a69d32f310 1396 }
nikapov 0:a1a69d32f310 1397
nikapov 0:a1a69d32f310 1398 /* To convert ms into register value */
sepp_nepp 11:c6f95a42d4d7 1399 uint32_t VL53L0X::Calc_timeout_mclks(uint32_t timeout_period_us,
nikapov 0:a1a69d32f310 1400 uint8_t vcsel_period_pclks)
sepp_nepp 11:c6f95a42d4d7 1401 { uint32_t macro_period_ns;
sepp_nepp 11:c6f95a42d4d7 1402
sepp_nepp 11:c6f95a42d4d7 1403 macro_period_ns = (uint32_t)(vcsel_period_pclks) * VL53L0X_MACRO_PERIOD_NS;
sepp_nepp 11:c6f95a42d4d7 1404
sepp_nepp 11:c6f95a42d4d7 1405 return (uint32_t)(((timeout_period_us * 1000)
sepp_nepp 11:c6f95a42d4d7 1406 + (macro_period_ns / 2)) / macro_period_ns);
nikapov 0:a1a69d32f310 1407 }
nikapov 0:a1a69d32f310 1408
sepp_nepp 11:c6f95a42d4d7 1409 uint32_t VL53L0X::ISQRT(uint32_t num)
sepp_nepp 11:c6f95a42d4d7 1410 { /* Implements an integer square root
sepp_nepp 11:c6f95a42d4d7 1411 * From: http://en.wikipedia.org/wiki/Methods_of_computing_square_roots */
nikapov 0:a1a69d32f310 1412 uint32_t res = 0;
nikapov 0:a1a69d32f310 1413 uint32_t bit = 1 << 30;
sepp_nepp 11:c6f95a42d4d7 1414 /* The second-to-top bit is set: 1 << 14 for 16-bits,1 << 30 for 32 bits */
nikapov 0:a1a69d32f310 1415
nikapov 0:a1a69d32f310 1416 /* "bit" starts at the highest power of four <= the argument. */
sepp_nepp 10:cd251e0fc2fd 1417 while (bit > num) { bit >>= 2; }
nikapov 0:a1a69d32f310 1418
nikapov 0:a1a69d32f310 1419 while (bit != 0) {
nikapov 0:a1a69d32f310 1420 if (num >= res + bit) {
nikapov 0:a1a69d32f310 1421 num -= res + bit;
nikapov 0:a1a69d32f310 1422 res = (res >> 1) + bit;
sepp_nepp 11:c6f95a42d4d7 1423 } else { res >>= 1; }
nikapov 0:a1a69d32f310 1424 bit >>= 2;
nikapov 0:a1a69d32f310 1425 }
nikapov 0:a1a69d32f310 1426 return res;
nikapov 0:a1a69d32f310 1427 }
nikapov 0:a1a69d32f310 1428
sepp_nepp 11:c6f95a42d4d7 1429 void VL53L0X::Calc_dmax(TFP1616 total_SIG_rate_mcps,
sepp_nepp 11:c6f95a42d4d7 1430 TFP1616 total_corr_SIG_rate_mcps,
sepp_nepp 11:c6f95a42d4d7 1431 TFP1616 pw_mult,
nikapov 0:a1a69d32f310 1432 uint32_t sigma_estimate_p1,
sepp_nepp 11:c6f95a42d4d7 1433 TFP1616 sigma_estimate_p2,
nikapov 0:a1a69d32f310 1434 uint32_t peak_vcsel_duration_us,
nikapov 0:a1a69d32f310 1435 uint32_t *pd_max_mm)
sepp_nepp 11:c6f95a42d4d7 1436 { const uint32_t c_sigma_limit = 18;
sepp_nepp 11:c6f95a42d4d7 1437 const TFP1616 c_SIG_limit = 0x4000; /* 0.25 */
sepp_nepp 11:c6f95a42d4d7 1438 const TFP1616 c_sigma_est_Ref = 0x00000042; /* 0.001 */
nikapov 0:a1a69d32f310 1439 const uint32_t c_amb_eff_width_sigma_est_ns = 6;
nikapov 0:a1a69d32f310 1440 const uint32_t c_amb_eff_width_d_max_ns = 7;
nikapov 0:a1a69d32f310 1441 uint32_t dmax_cal_range_mm;
sepp_nepp 11:c6f95a42d4d7 1442 TFP1616 dmax_cal_SIG_rate_rtn_mcps;
sepp_nepp 11:c6f95a42d4d7 1443 TFP1616 min_SIG_needed;
sepp_nepp 11:c6f95a42d4d7 1444 TFP1616 min_SIG_needed_p1;
sepp_nepp 11:c6f95a42d4d7 1445 TFP1616 min_SIG_needed_p2;
sepp_nepp 11:c6f95a42d4d7 1446 TFP1616 min_SIG_needed_p3;
sepp_nepp 11:c6f95a42d4d7 1447 TFP1616 min_SIG_needed_p4;
sepp_nepp 11:c6f95a42d4d7 1448 TFP1616 sigma_limit_tmp;
sepp_nepp 11:c6f95a42d4d7 1449 TFP1616 sigma_est_sq_tmp;
sepp_nepp 11:c6f95a42d4d7 1450 TFP1616 signal_limit_tmp;
sepp_nepp 11:c6f95a42d4d7 1451 TFP1616 signal_at0_mm;
sepp_nepp 11:c6f95a42d4d7 1452 TFP1616 dmax_dark;
sepp_nepp 11:c6f95a42d4d7 1453 TFP1616 dmax_ambient;
sepp_nepp 11:c6f95a42d4d7 1454 TFP1616 dmax_dark_tmp;
sepp_nepp 11:c6f95a42d4d7 1455 TFP1616 sigma_est_p2_tmp;
nikapov 0:a1a69d32f310 1456 uint32_t signal_rate_temp_mcps;
nikapov 0:a1a69d32f310 1457
sepp_nepp 10:cd251e0fc2fd 1458 dmax_cal_range_mm = DmaxCalRangeMilliMeter;
sepp_nepp 10:cd251e0fc2fd 1459
sepp_nepp 11:c6f95a42d4d7 1460 dmax_cal_SIG_rate_rtn_mcps = DmaxCalSignalRateRtnMHz;
nikapov 0:a1a69d32f310 1461
nikapov 0:a1a69d32f310 1462 /* uint32 * FixPoint1616 = FixPoint1616 */
sepp_nepp 11:c6f95a42d4d7 1463 signal_at0_mm = dmax_cal_range_mm * dmax_cal_SIG_rate_rtn_mcps;
nikapov 0:a1a69d32f310 1464
nikapov 0:a1a69d32f310 1465 /* FixPoint1616 >> 8 = FixPoint2408 */
nikapov 0:a1a69d32f310 1466 signal_at0_mm = (signal_at0_mm + 0x80) >> 8;
nikapov 0:a1a69d32f310 1467 signal_at0_mm *= dmax_cal_range_mm;
nikapov 0:a1a69d32f310 1468
sepp_nepp 11:c6f95a42d4d7 1469 min_SIG_needed_p1 = 0;
sepp_nepp 11:c6f95a42d4d7 1470 if (total_corr_SIG_rate_mcps > 0) {
sepp_nepp 11:c6f95a42d4d7 1471 /* Shift by 10 bits to increase resolution prior to the division */
sepp_nepp 11:c6f95a42d4d7 1472 signal_rate_temp_mcps = total_SIG_rate_mcps << 10;
nikapov 0:a1a69d32f310 1473
nikapov 0:a1a69d32f310 1474 /* Add rounding value prior to division */
sepp_nepp 11:c6f95a42d4d7 1475 min_SIG_needed_p1 = signal_rate_temp_mcps + (total_corr_SIG_rate_mcps / 2);
nikapov 0:a1a69d32f310 1476
nikapov 0:a1a69d32f310 1477 /* FixPoint0626/FixPoint1616 = FixPoint2210 */
sepp_nepp 11:c6f95a42d4d7 1478 min_SIG_needed_p1 /= total_corr_SIG_rate_mcps;
nikapov 0:a1a69d32f310 1479
nikapov 0:a1a69d32f310 1480 /* Apply a factored version of the speed of light.
nikapov 0:a1a69d32f310 1481 Correction to be applied at the end */
sepp_nepp 11:c6f95a42d4d7 1482 min_SIG_needed_p1 *= 3;
nikapov 0:a1a69d32f310 1483
nikapov 0:a1a69d32f310 1484 /* FixPoint2210 * FixPoint2210 = FixPoint1220 */
sepp_nepp 11:c6f95a42d4d7 1485 min_SIG_needed_p1 *= min_SIG_needed_p1;
nikapov 0:a1a69d32f310 1486
nikapov 0:a1a69d32f310 1487 /* FixPoint1220 >> 16 = FixPoint2804 */
sepp_nepp 11:c6f95a42d4d7 1488 min_SIG_needed_p1 = (min_SIG_needed_p1 + 0x8000) >> 16;
nikapov 0:a1a69d32f310 1489 }
nikapov 0:a1a69d32f310 1490
sepp_nepp 11:c6f95a42d4d7 1491 min_SIG_needed_p2 = pw_mult * sigma_estimate_p1;
nikapov 0:a1a69d32f310 1492
nikapov 0:a1a69d32f310 1493 /* FixPoint1616 >> 16 = uint32 */
sepp_nepp 11:c6f95a42d4d7 1494 min_SIG_needed_p2 = (min_SIG_needed_p2 + 0x8000) >> 16;
nikapov 0:a1a69d32f310 1495
nikapov 0:a1a69d32f310 1496 /* uint32 * uint32 = uint32 */
sepp_nepp 11:c6f95a42d4d7 1497 min_SIG_needed_p2 *= min_SIG_needed_p2;
sepp_nepp 11:c6f95a42d4d7 1498
sepp_nepp 11:c6f95a42d4d7 1499 /* Check sigmaEstimateP2; If this value is too high, there is not enough
sepp_nepp 11:c6f95a42d4d7 1500 * signal rate to calculate dmax value so set a suitable value to ensure
sepp_nepp 11:c6f95a42d4d7 1501 * a very small dmax. */
nikapov 0:a1a69d32f310 1502 sigma_est_p2_tmp = (sigma_estimate_p2 + 0x8000) >> 16;
nikapov 0:a1a69d32f310 1503 sigma_est_p2_tmp = (sigma_est_p2_tmp + c_amb_eff_width_sigma_est_ns / 2) /
nikapov 0:a1a69d32f310 1504 c_amb_eff_width_sigma_est_ns;
nikapov 0:a1a69d32f310 1505 sigma_est_p2_tmp *= c_amb_eff_width_d_max_ns;
nikapov 0:a1a69d32f310 1506
nikapov 0:a1a69d32f310 1507 if (sigma_est_p2_tmp > 0xffff) {
sepp_nepp 11:c6f95a42d4d7 1508 min_SIG_needed_p3 = 0xfff00000;
nikapov 0:a1a69d32f310 1509 } else {
sepp_nepp 11:c6f95a42d4d7 1510 /* DMAX uses a different ambient width from sigma,so apply correction.
sepp_nepp 11:c6f95a42d4d7 1511 * Perform division before multiplication to prevent overflow. */
nikapov 0:a1a69d32f310 1512 sigma_estimate_p2 = (sigma_estimate_p2 + c_amb_eff_width_sigma_est_ns / 2) /
nikapov 0:a1a69d32f310 1513 c_amb_eff_width_sigma_est_ns;
nikapov 0:a1a69d32f310 1514 sigma_estimate_p2 *= c_amb_eff_width_d_max_ns;
nikapov 0:a1a69d32f310 1515
nikapov 0:a1a69d32f310 1516 /* FixPoint1616 >> 16 = uint32 */
sepp_nepp 11:c6f95a42d4d7 1517 min_SIG_needed_p3 = (sigma_estimate_p2 + 0x8000) >> 16;
sepp_nepp 11:c6f95a42d4d7 1518 min_SIG_needed_p3 *= min_SIG_needed_p3;
nikapov 0:a1a69d32f310 1519 }
nikapov 0:a1a69d32f310 1520
nikapov 0:a1a69d32f310 1521 /* FixPoint1814 / uint32 = FixPoint1814 */
nikapov 0:a1a69d32f310 1522 sigma_limit_tmp = ((c_sigma_limit << 14) + 500) / 1000;
nikapov 0:a1a69d32f310 1523
nikapov 0:a1a69d32f310 1524 /* FixPoint1814 * FixPoint1814 = FixPoint3628 := FixPoint0428 */
nikapov 0:a1a69d32f310 1525 sigma_limit_tmp *= sigma_limit_tmp;
nikapov 0:a1a69d32f310 1526
nikapov 0:a1a69d32f310 1527 /* FixPoint1616 * FixPoint1616 = FixPoint3232 */
sepp_nepp 11:c6f95a42d4d7 1528 sigma_est_sq_tmp = c_sigma_est_Ref * c_sigma_est_Ref;
nikapov 0:a1a69d32f310 1529
nikapov 0:a1a69d32f310 1530 /* FixPoint3232 >> 4 = FixPoint0428 */
nikapov 0:a1a69d32f310 1531 sigma_est_sq_tmp = (sigma_est_sq_tmp + 0x08) >> 4;
nikapov 0:a1a69d32f310 1532
nikapov 0:a1a69d32f310 1533 /* FixPoint0428 - FixPoint0428 = FixPoint0428 */
nikapov 0:a1a69d32f310 1534 sigma_limit_tmp -= sigma_est_sq_tmp;
nikapov 0:a1a69d32f310 1535
nikapov 0:a1a69d32f310 1536 /* uint32_t * FixPoint0428 = FixPoint0428 */
sepp_nepp 11:c6f95a42d4d7 1537 min_SIG_needed_p4 = 4 * 12 * sigma_limit_tmp;
nikapov 0:a1a69d32f310 1538
nikapov 0:a1a69d32f310 1539 /* FixPoint0428 >> 14 = FixPoint1814 */
sepp_nepp 11:c6f95a42d4d7 1540 min_SIG_needed_p4 = (min_SIG_needed_p4 + 0x2000) >> 14;
nikapov 0:a1a69d32f310 1541
nikapov 0:a1a69d32f310 1542 /* uint32 + uint32 = uint32 */
sepp_nepp 11:c6f95a42d4d7 1543 min_SIG_needed = (min_SIG_needed_p2 + min_SIG_needed_p3);
nikapov 0:a1a69d32f310 1544
nikapov 0:a1a69d32f310 1545 /* uint32 / uint32 = uint32 */
sepp_nepp 11:c6f95a42d4d7 1546 min_SIG_needed += (peak_vcsel_duration_us / 2);
sepp_nepp 11:c6f95a42d4d7 1547 min_SIG_needed /= peak_vcsel_duration_us;
nikapov 0:a1a69d32f310 1548
nikapov 0:a1a69d32f310 1549 /* uint32 << 14 = FixPoint1814 */
sepp_nepp 11:c6f95a42d4d7 1550 min_SIG_needed <<= 14;
nikapov 0:a1a69d32f310 1551
nikapov 0:a1a69d32f310 1552 /* FixPoint1814 / FixPoint1814 = uint32 */
sepp_nepp 11:c6f95a42d4d7 1553 min_SIG_needed += (min_SIG_needed_p4 / 2);
sepp_nepp 11:c6f95a42d4d7 1554 min_SIG_needed /= min_SIG_needed_p4;
nikapov 0:a1a69d32f310 1555
nikapov 0:a1a69d32f310 1556 /* FixPoint3200 * FixPoint2804 := FixPoint2804*/
sepp_nepp 11:c6f95a42d4d7 1557 min_SIG_needed *= min_SIG_needed_p1;
nikapov 0:a1a69d32f310 1558
nikapov 0:a1a69d32f310 1559 /* Apply correction by dividing by 1000000.
sepp_nepp 11:c6f95a42d4d7 1560 * This assumes 10E16 on the numerator of the equation and 10E-22 on the denominator.
nikapov 0:a1a69d32f310 1561 * We do this because 32bit fix point calculation can't
nikapov 0:a1a69d32f310 1562 * handle the larger and smaller elements of this equation,
nikapov 0:a1a69d32f310 1563 * i.e. speed of light and pulse widths.
nikapov 0:a1a69d32f310 1564 */
sepp_nepp 11:c6f95a42d4d7 1565 min_SIG_needed = (min_SIG_needed + 500) / 1000;
sepp_nepp 11:c6f95a42d4d7 1566 min_SIG_needed <<= 4;
sepp_nepp 11:c6f95a42d4d7 1567
sepp_nepp 11:c6f95a42d4d7 1568 min_SIG_needed = (min_SIG_needed + 500) / 1000;
nikapov 0:a1a69d32f310 1569
nikapov 0:a1a69d32f310 1570 /* FixPoint1616 >> 8 = FixPoint2408 */
sepp_nepp 11:c6f95a42d4d7 1571 signal_limit_tmp = (c_SIG_limit + 0x80) >> 8;
nikapov 0:a1a69d32f310 1572
nikapov 0:a1a69d32f310 1573 /* FixPoint2408/FixPoint2408 = uint32 */
nikapov 0:a1a69d32f310 1574 if (signal_limit_tmp != 0) {
nikapov 0:a1a69d32f310 1575 dmax_dark_tmp = (signal_at0_mm + (signal_limit_tmp / 2))
nikapov 0:a1a69d32f310 1576 / signal_limit_tmp;
sepp_nepp 11:c6f95a42d4d7 1577 } else { dmax_dark_tmp = 0; }
sepp_nepp 11:c6f95a42d4d7 1578
sepp_nepp 11:c6f95a42d4d7 1579 dmax_dark = ISQRT(dmax_dark_tmp);
nikapov 0:a1a69d32f310 1580
nikapov 0:a1a69d32f310 1581 /* FixPoint2408/FixPoint2408 = uint32 */
sepp_nepp 11:c6f95a42d4d7 1582 if (min_SIG_needed != 0)
sepp_nepp 11:c6f95a42d4d7 1583 { dmax_ambient = (signal_at0_mm + min_SIG_needed / 2) / min_SIG_needed; }
sepp_nepp 11:c6f95a42d4d7 1584 else { dmax_ambient = 0; }
sepp_nepp 11:c6f95a42d4d7 1585
sepp_nepp 11:c6f95a42d4d7 1586 dmax_ambient = ISQRT(dmax_ambient);
nikapov 0:a1a69d32f310 1587
nikapov 0:a1a69d32f310 1588 *pd_max_mm = dmax_dark;
sepp_nepp 11:c6f95a42d4d7 1589 if (dmax_dark > dmax_ambient) { *pd_max_mm = dmax_ambient; }
nikapov 0:a1a69d32f310 1590 }
nikapov 0:a1a69d32f310 1591
sepp_nepp 11:c6f95a42d4d7 1592 void VL53L0X::Calc_sigma_estimate(TRangeResults *p_ranging_results,
sepp_nepp 11:c6f95a42d4d7 1593 TFP1616 *p_sigma_estimate, uint32_t *p_dmax_mm)
sepp_nepp 11:c6f95a42d4d7 1594 { /* Expressed in 100ths of a ns,i.e. centi-ns */
nikapov 0:a1a69d32f310 1595 const uint32_t c_pulse_effective_width_centi_ns = 800;
sepp_nepp 7:41cbc431e1f4 1596 /* Expressed in 100ths of a ns,i.e. centi-ns */
nikapov 0:a1a69d32f310 1597 const uint32_t c_ambient_effective_width_centi_ns = 600;
sepp_nepp 11:c6f95a42d4d7 1598 const TFP1616 c_dflt_final_range_integration_time_milli_secs = 0x00190000; /* 25ms */
nikapov 0:a1a69d32f310 1599 const uint32_t c_vcsel_pulse_width_ps = 4700; /* pico secs */
sepp_nepp 11:c6f95a42d4d7 1600 const TFP1616 c_sigma_est_max = 0x028F87AE;
sepp_nepp 11:c6f95a42d4d7 1601 const TFP1616 c_sigma_est_rtn_max = 0xF000;
sepp_nepp 11:c6f95a42d4d7 1602 const TFP1616 c_amb_to_SIG_ratio_max = 0xF0000000 /
nikapov 0:a1a69d32f310 1603 c_ambient_effective_width_centi_ns;
nikapov 0:a1a69d32f310 1604 /* Time Of Flight per mm (6.6 pico secs) */
sepp_nepp 11:c6f95a42d4d7 1605 const TFP1616 c_tof_per_mm_ps = 0x0006999A;
nikapov 0:a1a69d32f310 1606 const uint32_t c_16bit_rounding_param = 0x00008000;
sepp_nepp 11:c6f95a42d4d7 1607 const TFP1616 c_max_xtalk_kcps = 0x00320000;
nikapov 0:a1a69d32f310 1608 const uint32_t c_pll_period_ps = 1655;
nikapov 0:a1a69d32f310 1609
nikapov 0:a1a69d32f310 1610 uint32_t vcsel_total_events_rtn;
nikapov 0:a1a69d32f310 1611 uint32_t final_range_timeout_micro_secs;
nikapov 0:a1a69d32f310 1612 uint32_t pre_range_timeout_micro_secs;
nikapov 0:a1a69d32f310 1613 uint32_t final_range_integration_time_milli_secs;
sepp_nepp 11:c6f95a42d4d7 1614 TFP1616 sigma_estimate_p1;
sepp_nepp 11:c6f95a42d4d7 1615 TFP1616 sigma_estimate_p2;
sepp_nepp 11:c6f95a42d4d7 1616 TFP1616 sigma_estimate_p3;
sepp_nepp 11:c6f95a42d4d7 1617 TFP1616 delta_t_ps;
sepp_nepp 11:c6f95a42d4d7 1618 TFP1616 pw_mult;
sepp_nepp 11:c6f95a42d4d7 1619 TFP1616 sigma_est_rtn;
sepp_nepp 11:c6f95a42d4d7 1620 TFP1616 sigma_estimate;
sepp_nepp 11:c6f95a42d4d7 1621 TFP1616 xtalk_correction;
sepp_nepp 11:c6f95a42d4d7 1622 TFP1616 ambient_rate_kcps;
sepp_nepp 11:c6f95a42d4d7 1623 TFP1616 peak_SIG_rate_kcps;
sepp_nepp 11:c6f95a42d4d7 1624 TFP1616 xtalk_comp_rate_mcps;
sepp_nepp 11:c6f95a42d4d7 1625 uint32_t xtalk_comp_rate_kcps;
sepp_nepp 11:c6f95a42d4d7 1626
sepp_nepp 11:c6f95a42d4d7 1627 TFP1616 diff1_mcps;
sepp_nepp 11:c6f95a42d4d7 1628 TFP1616 diff2_mcps;
sepp_nepp 11:c6f95a42d4d7 1629 TFP1616 sqr1;
sepp_nepp 11:c6f95a42d4d7 1630 TFP1616 sqr2;
sepp_nepp 11:c6f95a42d4d7 1631 TFP1616 sqr_sum;
sepp_nepp 11:c6f95a42d4d7 1632 TFP1616 sqrt_result_centi_ns;
sepp_nepp 11:c6f95a42d4d7 1633 TFP1616 sqrt_result;
sepp_nepp 11:c6f95a42d4d7 1634 TFP1616 total_SIG_rate_mcps;
sepp_nepp 11:c6f95a42d4d7 1635 TFP1616 corrected_SIG_rate_mcps;
sepp_nepp 11:c6f95a42d4d7 1636 TFP1616 sigma_est_Ref;
nikapov 0:a1a69d32f310 1637 uint32_t vcsel_width;
nikapov 0:a1a69d32f310 1638 uint32_t final_range_macro_pclks;
nikapov 0:a1a69d32f310 1639 uint32_t pre_range_macro_pclks;
nikapov 0:a1a69d32f310 1640 uint32_t peak_vcsel_duration_us;
nikapov 0:a1a69d32f310 1641 uint8_t final_range_vcsel_pclks;
nikapov 0:a1a69d32f310 1642 uint8_t pre_range_vcsel_pclks;
nikapov 0:a1a69d32f310 1643 /*! \addtogroup calc_sigma_estimate
nikapov 0:a1a69d32f310 1644 * @{
sepp_nepp 11:c6f95a42d4d7 1645 * Estimates the range sigma */
sepp_nepp 11:c6f95a42d4d7 1646
sepp_nepp 11:c6f95a42d4d7 1647 xtalk_comp_rate_mcps = CurrParams.Xtalk_CompRate_MHz;
sepp_nepp 10:cd251e0fc2fd 1648
sepp_nepp 10:cd251e0fc2fd 1649 /* We work in kcps rather than mcps as this helps keep within the
sepp_nepp 11:c6f95a42d4d7 1650 * confines of the 32 Fix1616 type. */
sepp_nepp 11:c6f95a42d4d7 1651 ambient_rate_kcps = (p_ranging_results->AmbientRateRtnMHz * 1000) >> 16;
sepp_nepp 11:c6f95a42d4d7 1652
sepp_nepp 11:c6f95a42d4d7 1653 corrected_SIG_rate_mcps = p_ranging_results->SignalRateRtnMHz;
sepp_nepp 11:c6f95a42d4d7 1654
sepp_nepp 11:c6f95a42d4d7 1655 Get_total_SIG_rate(p_ranging_results,&total_SIG_rate_mcps);
sepp_nepp 11:c6f95a42d4d7 1656 xtalk_comp_rate_mcps = Get_total_xtalk_rate(p_ranging_results);
nikapov 0:a1a69d32f310 1657
nikapov 0:a1a69d32f310 1658 /* Signal rate measurement provided by device is the
sepp_nepp 11:c6f95a42d4d7 1659 * peak signal rate,not average. */
sepp_nepp 11:c6f95a42d4d7 1660 peak_SIG_rate_kcps = (total_SIG_rate_mcps * 1000);
sepp_nepp 11:c6f95a42d4d7 1661 peak_SIG_rate_kcps = (peak_SIG_rate_kcps + 0x8000) >> 16;
sepp_nepp 11:c6f95a42d4d7 1662
sepp_nepp 11:c6f95a42d4d7 1663 xtalk_comp_rate_kcps = xtalk_comp_rate_mcps * 1000;
sepp_nepp 11:c6f95a42d4d7 1664
sepp_nepp 11:c6f95a42d4d7 1665 if (xtalk_comp_rate_kcps > c_max_xtalk_kcps)
sepp_nepp 11:c6f95a42d4d7 1666 { xtalk_comp_rate_kcps = c_max_xtalk_kcps; }
sepp_nepp 11:c6f95a42d4d7 1667
sepp_nepp 11:c6f95a42d4d7 1668 if (ErrState == VL53L0X_OK) {
nikapov 0:a1a69d32f310 1669 /* Calculate final range macro periods */
sepp_nepp 12:aa177f0e4c10 1670 final_range_timeout_micro_secs = FinalRangeTimeoutMicroSecs;
sepp_nepp 12:aa177f0e4c10 1671 final_range_vcsel_pclks = FinalRangeVcselPPeriod;
sepp_nepp 11:c6f95a42d4d7 1672 final_range_macro_pclks = Calc_timeout_mclks(final_range_timeout_micro_secs,final_range_vcsel_pclks);
nikapov 0:a1a69d32f310 1673
nikapov 0:a1a69d32f310 1674 /* Calculate pre-range macro periods */
sepp_nepp 12:aa177f0e4c10 1675 pre_range_timeout_micro_secs = PreRangeTimeoutMicroSecs;
sepp_nepp 12:aa177f0e4c10 1676 pre_range_vcsel_pclks = PreRangeVcselPPeriod;
sepp_nepp 11:c6f95a42d4d7 1677 pre_range_macro_pclks = Calc_timeout_mclks(pre_range_timeout_micro_secs,pre_range_vcsel_pclks);
nikapov 0:a1a69d32f310 1678 vcsel_width = 3;
sepp_nepp 11:c6f95a42d4d7 1679 if (final_range_vcsel_pclks == 8) { vcsel_width = 2; }
nikapov 0:a1a69d32f310 1680
nikapov 0:a1a69d32f310 1681 peak_vcsel_duration_us = vcsel_width * 2048 *
nikapov 0:a1a69d32f310 1682 (pre_range_macro_pclks + final_range_macro_pclks);
nikapov 0:a1a69d32f310 1683 peak_vcsel_duration_us = (peak_vcsel_duration_us + 500) / 1000;
nikapov 0:a1a69d32f310 1684 peak_vcsel_duration_us *= c_pll_period_ps;
nikapov 0:a1a69d32f310 1685 peak_vcsel_duration_us = (peak_vcsel_duration_us + 500) / 1000;
nikapov 0:a1a69d32f310 1686
nikapov 0:a1a69d32f310 1687 /* Fix1616 >> 8 = Fix2408 */
sepp_nepp 11:c6f95a42d4d7 1688 total_SIG_rate_mcps = (total_SIG_rate_mcps + 0x80) >> 8;
nikapov 0:a1a69d32f310 1689
nikapov 0:a1a69d32f310 1690 /* Fix2408 * uint32 = Fix2408 */
sepp_nepp 11:c6f95a42d4d7 1691 vcsel_total_events_rtn = total_SIG_rate_mcps * peak_vcsel_duration_us;
nikapov 0:a1a69d32f310 1692
nikapov 0:a1a69d32f310 1693 /* Fix2408 >> 8 = uint32 */
nikapov 0:a1a69d32f310 1694 vcsel_total_events_rtn = (vcsel_total_events_rtn + 0x80) >> 8;
nikapov 0:a1a69d32f310 1695
nikapov 0:a1a69d32f310 1696 /* Fix2408 << 8 = Fix1616 = */
sepp_nepp 11:c6f95a42d4d7 1697 total_SIG_rate_mcps <<= 8;
nikapov 0:a1a69d32f310 1698 }
nikapov 0:a1a69d32f310 1699
sepp_nepp 11:c6f95a42d4d7 1700 if (ErrState != VL53L0X_OK) { return ; }
sepp_nepp 11:c6f95a42d4d7 1701
sepp_nepp 11:c6f95a42d4d7 1702 if (peak_SIG_rate_kcps == 0) {
nikapov 0:a1a69d32f310 1703 *p_sigma_estimate = c_sigma_est_max;
sepp_nepp 10:cd251e0fc2fd 1704 SigmaEstimate = c_sigma_est_max;
nikapov 0:a1a69d32f310 1705 *p_dmax_mm = 0;
nikapov 0:a1a69d32f310 1706 } else {
sepp_nepp 11:c6f95a42d4d7 1707 if (vcsel_total_events_rtn < 1) {vcsel_total_events_rtn = 1; }
nikapov 0:a1a69d32f310 1708
nikapov 0:a1a69d32f310 1709 sigma_estimate_p1 = c_pulse_effective_width_centi_ns;
nikapov 0:a1a69d32f310 1710
nikapov 0:a1a69d32f310 1711 /* ((FixPoint1616 << 16)* uint32)/uint32 = FixPoint1616 */
sepp_nepp 11:c6f95a42d4d7 1712 sigma_estimate_p2 = (ambient_rate_kcps << 16) / peak_SIG_rate_kcps;
sepp_nepp 11:c6f95a42d4d7 1713 if (sigma_estimate_p2 > c_amb_to_SIG_ratio_max)
sepp_nepp 11:c6f95a42d4d7 1714 /* Clip to prevent overflow. Will ensure safe max result. */
sepp_nepp 11:c6f95a42d4d7 1715 { sigma_estimate_p2 = c_amb_to_SIG_ratio_max; }
nikapov 0:a1a69d32f310 1716 sigma_estimate_p2 *= c_ambient_effective_width_centi_ns;
sepp_nepp 11:c6f95a42d4d7 1717 sigma_estimate_p3 = 2 * ISQRT(vcsel_total_events_rtn * 12);
nikapov 0:a1a69d32f310 1718
nikapov 0:a1a69d32f310 1719 /* uint32 * FixPoint1616 = FixPoint1616 */
sepp_nepp 11:c6f95a42d4d7 1720 delta_t_ps = p_ranging_results->RangeMilliMeter * c_tof_per_mm_ps;
sepp_nepp 11:c6f95a42d4d7 1721
sepp_nepp 11:c6f95a42d4d7 1722 /* vcselRate - xtalkCompRate
nikapov 0:a1a69d32f310 1723 * (uint32 << 16) - FixPoint1616 = FixPoint1616.
nikapov 0:a1a69d32f310 1724 * Divide result by 1000 to convert to mcps.
sepp_nepp 11:c6f95a42d4d7 1725 * 500 is added to ensure rounding when integer division truncates. */
sepp_nepp 11:c6f95a42d4d7 1726 diff1_mcps = (((peak_SIG_rate_kcps << 16) - 2 * xtalk_comp_rate_kcps) + 500) / 1000;
nikapov 0:a1a69d32f310 1727
nikapov 0:a1a69d32f310 1728 /* vcselRate + xtalkCompRate */
sepp_nepp 11:c6f95a42d4d7 1729 diff2_mcps = ((peak_SIG_rate_kcps << 16) + 500) / 1000;
sepp_nepp 11:c6f95a42d4d7 1730
sepp_nepp 11:c6f95a42d4d7 1731 /* Shift by 8 bits to increase resolution prior to the division */
nikapov 0:a1a69d32f310 1732 diff1_mcps <<= 8;
nikapov 0:a1a69d32f310 1733
nikapov 0:a1a69d32f310 1734 /* FixPoint0824/FixPoint1616 = FixPoint2408 */
nikapov 0:a1a69d32f310 1735 // xTalkCorrection = abs(diff1_mcps/diff2_mcps);
sepp_nepp 11:c6f95a42d4d7 1736 // abs is causing compiler overloading isue in C++, but unsigned types. So,redundant call anyway!
sepp_nepp 11:c6f95a42d4d7 1737 xtalk_correction = diff1_mcps / diff2_mcps;
nikapov 0:a1a69d32f310 1738
nikapov 0:a1a69d32f310 1739 /* FixPoint2408 << 8 = FixPoint1616 */
sepp_nepp 11:c6f95a42d4d7 1740 xtalk_correction <<= 8;
sepp_nepp 11:c6f95a42d4d7 1741
sepp_nepp 11:c6f95a42d4d7 1742 if (p_ranging_results->RangeStatus != 0)
sepp_nepp 11:c6f95a42d4d7 1743 { pw_mult = 1 << 16; }
sepp_nepp 11:c6f95a42d4d7 1744 else {
nikapov 0:a1a69d32f310 1745 /* FixPoint1616/uint32 = FixPoint1616 */
nikapov 0:a1a69d32f310 1746 pw_mult = delta_t_ps / c_vcsel_pulse_width_ps; /* smaller than 1.0f */
nikapov 0:a1a69d32f310 1747
sepp_nepp 10:cd251e0fc2fd 1748 /* FixPoint1616 * FixPoint1616 = FixPoint3232,however both
nikapov 0:a1a69d32f310 1749 * values are small enough such that32 bits will not be
sepp_nepp 11:c6f95a42d4d7 1750 * exceeded. */
sepp_nepp 11:c6f95a42d4d7 1751 pw_mult *= ((1 << 16) - xtalk_correction);
nikapov 0:a1a69d32f310 1752
nikapov 0:a1a69d32f310 1753 /* (FixPoint3232 >> 16) = FixPoint1616 */
nikapov 0:a1a69d32f310 1754 pw_mult = (pw_mult + c_16bit_rounding_param) >> 16;
nikapov 0:a1a69d32f310 1755
nikapov 0:a1a69d32f310 1756 /* FixPoint1616 + FixPoint1616 = FixPoint1616 */
nikapov 0:a1a69d32f310 1757 pw_mult += (1 << 16);
nikapov 0:a1a69d32f310 1758
sepp_nepp 10:cd251e0fc2fd 1759 /* At this point the value will be 1.xx,therefore if we square
nikapov 0:a1a69d32f310 1760 * the value this will exceed 32 bits. To address this perform
sepp_nepp 11:c6f95a42d4d7 1761 * a single shift to the right before the multiplication. */
nikapov 0:a1a69d32f310 1762 pw_mult >>= 1;
nikapov 0:a1a69d32f310 1763 /* FixPoint1715 * FixPoint1715 = FixPoint3430 */
nikapov 0:a1a69d32f310 1764 pw_mult = pw_mult * pw_mult;
nikapov 0:a1a69d32f310 1765
nikapov 0:a1a69d32f310 1766 /* (FixPoint3430 >> 14) = Fix1616 */
nikapov 0:a1a69d32f310 1767 pw_mult >>= 14;
nikapov 0:a1a69d32f310 1768 }
nikapov 0:a1a69d32f310 1769
nikapov 0:a1a69d32f310 1770 /* FixPoint1616 * uint32 = FixPoint1616 */
nikapov 0:a1a69d32f310 1771 sqr1 = pw_mult * sigma_estimate_p1;
nikapov 0:a1a69d32f310 1772
nikapov 0:a1a69d32f310 1773 /* (FixPoint1616 >> 16) = FixPoint3200 */
nikapov 0:a1a69d32f310 1774 sqr1 = (sqr1 + 0x8000) >> 16;
nikapov 0:a1a69d32f310 1775
nikapov 0:a1a69d32f310 1776 /* FixPoint3200 * FixPoint3200 = FixPoint6400 */
nikapov 0:a1a69d32f310 1777 sqr1 *= sqr1;
nikapov 0:a1a69d32f310 1778 sqr2 = sigma_estimate_p2;
nikapov 0:a1a69d32f310 1779
nikapov 0:a1a69d32f310 1780 /* (FixPoint1616 >> 16) = FixPoint3200 */
nikapov 0:a1a69d32f310 1781 sqr2 = (sqr2 + 0x8000) >> 16;
nikapov 0:a1a69d32f310 1782
nikapov 0:a1a69d32f310 1783 /* FixPoint3200 * FixPoint3200 = FixPoint6400 */
nikapov 0:a1a69d32f310 1784 sqr2 *= sqr2;
nikapov 0:a1a69d32f310 1785
nikapov 0:a1a69d32f310 1786 /* FixPoint64000 + FixPoint6400 = FixPoint6400 */
nikapov 0:a1a69d32f310 1787 sqr_sum = sqr1 + sqr2;
nikapov 0:a1a69d32f310 1788
nikapov 0:a1a69d32f310 1789 /* SQRT(FixPoin6400) = FixPoint3200 */
sepp_nepp 11:c6f95a42d4d7 1790 sqrt_result_centi_ns = ISQRT(sqr_sum);
nikapov 0:a1a69d32f310 1791
nikapov 0:a1a69d32f310 1792 /* (FixPoint3200 << 16) = FixPoint1616 */
nikapov 0:a1a69d32f310 1793 sqrt_result_centi_ns <<= 16;
nikapov 0:a1a69d32f310 1794
sepp_nepp 11:c6f95a42d4d7 1795 /* Note that the Speed Of Light is expressed in um per 1E-10
sepp_nepp 11:c6f95a42d4d7 1796 * seconds (2997) Therefore to get mm/ns we have to divide by 10000 */
nikapov 0:a1a69d32f310 1797 sigma_est_rtn = (((sqrt_result_centi_ns + 50) / 100) /
nikapov 0:a1a69d32f310 1798 sigma_estimate_p3);
nikapov 0:a1a69d32f310 1799 sigma_est_rtn *= VL53L0X_SPEED_OF_LIGHT_IN_AIR;
nikapov 0:a1a69d32f310 1800
nikapov 0:a1a69d32f310 1801 /* Add 5000 before dividing by 10000 to ensure rounding. */
sepp_nepp 11:c6f95a42d4d7 1802 sigma_est_rtn = (sigma_est_rtn + 5000) / 10000;
sepp_nepp 11:c6f95a42d4d7 1803
sepp_nepp 11:c6f95a42d4d7 1804 if (sigma_est_rtn > c_sigma_est_rtn_max)
sepp_nepp 11:c6f95a42d4d7 1805 /* Clip to prevent overflow. Will ensure safe max result. */
sepp_nepp 11:c6f95a42d4d7 1806 { sigma_est_rtn = c_sigma_est_rtn_max; }
sepp_nepp 11:c6f95a42d4d7 1807
nikapov 0:a1a69d32f310 1808 final_range_integration_time_milli_secs =
nikapov 0:a1a69d32f310 1809 (final_range_timeout_micro_secs + pre_range_timeout_micro_secs + 500) / 1000;
nikapov 0:a1a69d32f310 1810
nikapov 0:a1a69d32f310 1811 /* sigmaEstRef = 1mm * 25ms/final range integration time (inc pre-range)
sepp_nepp 11:c6f95a42d4d7 1812 * sqrt(FixPoint1616/int) = FixPoint2408) */
sepp_nepp 11:c6f95a42d4d7 1813 sigma_est_Ref =
sepp_nepp 11:c6f95a42d4d7 1814 ISQRT((c_dflt_final_range_integration_time_milli_secs +
nikapov 0:a1a69d32f310 1815 final_range_integration_time_milli_secs / 2) /
nikapov 0:a1a69d32f310 1816 final_range_integration_time_milli_secs);
nikapov 0:a1a69d32f310 1817
nikapov 0:a1a69d32f310 1818 /* FixPoint2408 << 8 = FixPoint1616 */
sepp_nepp 11:c6f95a42d4d7 1819 sigma_est_Ref <<= 8;
sepp_nepp 11:c6f95a42d4d7 1820 sigma_est_Ref = (sigma_est_Ref + 500) / 1000;
nikapov 0:a1a69d32f310 1821
nikapov 0:a1a69d32f310 1822 /* FixPoint1616 * FixPoint1616 = FixPoint3232 */
nikapov 0:a1a69d32f310 1823 sqr1 = sigma_est_rtn * sigma_est_rtn;
nikapov 0:a1a69d32f310 1824 /* FixPoint1616 * FixPoint1616 = FixPoint3232 */
sepp_nepp 11:c6f95a42d4d7 1825 sqr2 = sigma_est_Ref * sigma_est_Ref;
nikapov 0:a1a69d32f310 1826
nikapov 0:a1a69d32f310 1827 /* sqrt(FixPoint3232) = FixPoint1616 */
sepp_nepp 11:c6f95a42d4d7 1828 sqrt_result = ISQRT((sqr1 + sqr2));
sepp_nepp 11:c6f95a42d4d7 1829 /* Note that the Shift by 4 bits increases resolution prior to
sepp_nepp 7:41cbc431e1f4 1830 * the sqrt,therefore the result must be shifted by 2 bits to
sepp_nepp 11:c6f95a42d4d7 1831 * the right to revert back to the FixPoint1616 format. */
nikapov 0:a1a69d32f310 1832 sigma_estimate = 1000 * sqrt_result;
nikapov 0:a1a69d32f310 1833
sepp_nepp 11:c6f95a42d4d7 1834 if ((peak_SIG_rate_kcps < 1) || (vcsel_total_events_rtn < 1) ||
nikapov 0:a1a69d32f310 1835 (sigma_estimate > c_sigma_est_max)) {
sepp_nepp 11:c6f95a42d4d7 1836 sigma_estimate = c_sigma_est_max; }
nikapov 0:a1a69d32f310 1837
nikapov 0:a1a69d32f310 1838 *p_sigma_estimate = (uint32_t)(sigma_estimate);
sepp_nepp 10:cd251e0fc2fd 1839 SigmaEstimate = *p_sigma_estimate;
sepp_nepp 11:c6f95a42d4d7 1840 Calc_dmax(total_SIG_rate_mcps,
sepp_nepp 11:c6f95a42d4d7 1841 corrected_SIG_rate_mcps,
nikapov 0:a1a69d32f310 1842 pw_mult,
nikapov 0:a1a69d32f310 1843 sigma_estimate_p1,
nikapov 0:a1a69d32f310 1844 sigma_estimate_p2,
nikapov 0:a1a69d32f310 1845 peak_vcsel_duration_us,
nikapov 0:a1a69d32f310 1846 p_dmax_mm);
nikapov 0:a1a69d32f310 1847 }
nikapov 0:a1a69d32f310 1848 }
nikapov 0:a1a69d32f310 1849
sepp_nepp 11:c6f95a42d4d7 1850 void VL53L0X::Get_Device_range_status(uint8_t device_range_status,
sepp_nepp 11:c6f95a42d4d7 1851 TFP1616 signal_rate,
sepp_nepp 11:c6f95a42d4d7 1852 uint16_t effective_SPAD_rtn_count,
sepp_nepp 11:c6f95a42d4d7 1853 TRangeResults *p_ranging_results,
sepp_nepp 11:c6f95a42d4d7 1854 uint8_t *p_Device_range_status)
sepp_nepp 11:c6f95a42d4d7 1855 { uint8_t none_flag;
nikapov 0:a1a69d32f310 1856 uint8_t sigma_limitflag = 0;
sepp_nepp 11:c6f95a42d4d7 1857 uint8_t signal_Ref_clipflag = 0;
nikapov 0:a1a69d32f310 1858 uint8_t range_ignore_thresholdflag = 0;
sepp_nepp 11:c6f95a42d4d7 1859 uint8_t sigma_limit_chk_en = 0;
sepp_nepp 11:c6f95a42d4d7 1860 uint8_t signal_rate_final_range_limit_chk_en = 0;
sepp_nepp 11:c6f95a42d4d7 1861 uint8_t signal_Ref_clip_limit_chk_en = 0;
sepp_nepp 11:c6f95a42d4d7 1862 uint8_t range_ignore_threshold_chk_en = 0;
sepp_nepp 11:c6f95a42d4d7 1863 TFP1616 sigma_estimate;
sepp_nepp 11:c6f95a42d4d7 1864 TFP1616 sigma_limit_value;
sepp_nepp 11:c6f95a42d4d7 1865 TFP1616 signal_Ref_clip_value;
sepp_nepp 11:c6f95a42d4d7 1866 TFP1616 range_ignore_threshold;
sepp_nepp 11:c6f95a42d4d7 1867 TFP1616 signal_rate_per_SPAD;
nikapov 0:a1a69d32f310 1868 uint8_t device_range_status_internal = 0;
nikapov 0:a1a69d32f310 1869 uint8_t temp8;
nikapov 0:a1a69d32f310 1870 uint32_t dmax_mm = 0;
sepp_nepp 11:c6f95a42d4d7 1871
sepp_nepp 11:c6f95a42d4d7 1872 /* VL53L0X has a good ranging when the value of the
nikapov 0:a1a69d32f310 1873 * DeviceRangeStatus = 11. This function will replace the value 0 with
nikapov 0:a1a69d32f310 1874 * the value 11 in the DeviceRangeStatus.
sepp_nepp 7:41cbc431e1f4 1875 * In addition,the SigmaEstimator is not included in the VL53L0X
sepp_nepp 11:c6f95a42d4d7 1876 * DeviceRangeStatus,this will be added in the DeviceRangeStatus. */
nikapov 0:a1a69d32f310 1877
nikapov 0:a1a69d32f310 1878 device_range_status_internal = ((device_range_status & 0x78) >> 3);
nikapov 0:a1a69d32f310 1879
sepp_nepp 11:c6f95a42d4d7 1880 if ( device_range_status_internal == 0 ||
sepp_nepp 11:c6f95a42d4d7 1881 device_range_status_internal == 5 ||
sepp_nepp 11:c6f95a42d4d7 1882 device_range_status_internal == 7 ||
sepp_nepp 11:c6f95a42d4d7 1883 device_range_status_internal == 12 ||
sepp_nepp 11:c6f95a42d4d7 1884 device_range_status_internal == 13 ||
sepp_nepp 11:c6f95a42d4d7 1885 device_range_status_internal == 14 ||
sepp_nepp 11:c6f95a42d4d7 1886 device_range_status_internal == 15 )
sepp_nepp 11:c6f95a42d4d7 1887 { none_flag = 1; }
sepp_nepp 11:c6f95a42d4d7 1888 else { none_flag = 0; }
sepp_nepp 11:c6f95a42d4d7 1889
sepp_nepp 11:c6f95a42d4d7 1890 /* Check if Sigma limit is enabled,if yes then do comparison with limit
sepp_nepp 11:c6f95a42d4d7 1891 * value and put the result back into pDeviceRangeStatus. */
sepp_nepp 11:c6f95a42d4d7 1892 if (ErrState == VL53L0X_OK)
sepp_nepp 11:c6f95a42d4d7 1893 { sigma_limit_chk_en = Get_limit_chk_en(VL53L0X_CHECKEN_SIGMA_FINAL_RANGE); }
sepp_nepp 11:c6f95a42d4d7 1894
sepp_nepp 11:c6f95a42d4d7 1895 if ((sigma_limit_chk_en != 0) && (ErrState == VL53L0X_OK)) {
sepp_nepp 11:c6f95a42d4d7 1896 /* compute the Sigma and check with limit */
sepp_nepp 11:c6f95a42d4d7 1897 Calc_sigma_estimate(p_ranging_results, &sigma_estimate, &dmax_mm);
sepp_nepp 11:c6f95a42d4d7 1898 if (ErrState == VL53L0X_OK)
sepp_nepp 11:c6f95a42d4d7 1899 { p_ranging_results->RangeDMaxMilliMeter = dmax_mm; }
sepp_nepp 11:c6f95a42d4d7 1900
sepp_nepp 11:c6f95a42d4d7 1901 if (ErrState == VL53L0X_OK)
sepp_nepp 11:c6f95a42d4d7 1902 { sigma_limit_value = Get_limit_chk_val(VL53L0X_CHECKEN_SIGMA_FINAL_RANGE);
sepp_nepp 11:c6f95a42d4d7 1903
sepp_nepp 11:c6f95a42d4d7 1904 if ((sigma_limit_value > 0) && (sigma_estimate > sigma_limit_value))
sepp_nepp 11:c6f95a42d4d7 1905 { sigma_limitflag = 1; }/* Limit Fail */
sepp_nepp 11:c6f95a42d4d7 1906 }
sepp_nepp 11:c6f95a42d4d7 1907 }
sepp_nepp 11:c6f95a42d4d7 1908
sepp_nepp 11:c6f95a42d4d7 1909 /* Check if Signal ref clip limit is enabled,if yes then do comparison
sepp_nepp 11:c6f95a42d4d7 1910 * with limit value and put the result back into pDeviceRangeStatus. */
sepp_nepp 11:c6f95a42d4d7 1911 if (ErrState == VL53L0X_OK)
sepp_nepp 11:c6f95a42d4d7 1912 {signal_Ref_clip_limit_chk_en = Get_limit_chk_en(VL53L0X_CHECKEN_SIG_REF_CLIP);}
sepp_nepp 11:c6f95a42d4d7 1913
sepp_nepp 11:c6f95a42d4d7 1914 if ((signal_Ref_clip_limit_chk_en != 0) && (ErrState == VL53L0X_OK))
sepp_nepp 11:c6f95a42d4d7 1915 { signal_Ref_clip_value = Get_limit_chk_val(VL53L0X_CHECKEN_SIG_REF_CLIP);
nikapov 0:a1a69d32f310 1916
nikapov 0:a1a69d32f310 1917 /* Read LastSignalRefMcps from device */
sepp_nepp 11:c6f95a42d4d7 1918 Write_Byte(0xFF,0x01);
sepp_nepp 11:c6f95a42d4d7 1919 LastSignalRefMcps = FP97_TO_FP1616( Read_Word(REG_RESULT_PEAK_SIG_RATE_REF));
sepp_nepp 11:c6f95a42d4d7 1920 Write_Byte(0xFF,0x00);
sepp_nepp 11:c6f95a42d4d7 1921
sepp_nepp 11:c6f95a42d4d7 1922 if ((signal_Ref_clip_value > 0) && (LastSignalRefMcps > signal_Ref_clip_value)) \
sepp_nepp 11:c6f95a42d4d7 1923 { signal_Ref_clipflag = 1; /* Limit Fail */ }
sepp_nepp 11:c6f95a42d4d7 1924 }
sepp_nepp 11:c6f95a42d4d7 1925
sepp_nepp 11:c6f95a42d4d7 1926 /* Check if Signal ref clip limit is enabled,if yes then do comparison
sepp_nepp 11:c6f95a42d4d7 1927 * with limit value and put the result back into pDeviceRangeStatus.
sepp_nepp 11:c6f95a42d4d7 1928 * EffectiveSPADRtnCount has a format 8.8
sepp_nepp 11:c6f95a42d4d7 1929 * If (Return signal rate < (1.5 x Xtalk x number of SPADS)) : FAIL */
sepp_nepp 11:c6f95a42d4d7 1930 if (ErrState == VL53L0X_OK)
sepp_nepp 11:c6f95a42d4d7 1931 { range_ignore_threshold_chk_en = Get_limit_chk_en(VL53L0X_CHECKEN_RANGE_IGNORE_THRESHOLD); }
sepp_nepp 11:c6f95a42d4d7 1932
sepp_nepp 11:c6f95a42d4d7 1933 if ((range_ignore_threshold_chk_en != 0) && (ErrState == VL53L0X_OK))
sepp_nepp 11:c6f95a42d4d7 1934 {/* Compute the signal rate per SPAD */
sepp_nepp 11:c6f95a42d4d7 1935 if (effective_SPAD_rtn_count == 0) { signal_rate_per_SPAD = 0; }
sepp_nepp 11:c6f95a42d4d7 1936 else { signal_rate_per_SPAD =
sepp_nepp 11:c6f95a42d4d7 1937 (TFP1616)((256 * signal_rate) / effective_SPAD_rtn_count); }
sepp_nepp 11:c6f95a42d4d7 1938
sepp_nepp 11:c6f95a42d4d7 1939 range_ignore_threshold=Get_limit_chk_val(VL53L0X_CHECKEN_RANGE_IGNORE_THRESHOLD);
sepp_nepp 11:c6f95a42d4d7 1940
sepp_nepp 11:c6f95a42d4d7 1941 if ((range_ignore_threshold > 0) && (signal_rate_per_SPAD < range_ignore_threshold)) {
sepp_nepp 11:c6f95a42d4d7 1942 /* Limit Fail add 2^6 to range ErrState */
nikapov 0:a1a69d32f310 1943 range_ignore_thresholdflag = 1;
nikapov 0:a1a69d32f310 1944 }
nikapov 0:a1a69d32f310 1945 }
nikapov 0:a1a69d32f310 1946
sepp_nepp 11:c6f95a42d4d7 1947 if (ErrState == VL53L0X_OK) {
nikapov 0:a1a69d32f310 1948 if (none_flag == 1) {
sepp_nepp 11:c6f95a42d4d7 1949 *p_Device_range_status = 255; /* NONE */
nikapov 0:a1a69d32f310 1950 } else if (device_range_status_internal == 1 ||
nikapov 0:a1a69d32f310 1951 device_range_status_internal == 2 ||
nikapov 0:a1a69d32f310 1952 device_range_status_internal == 3) {
sepp_nepp 11:c6f95a42d4d7 1953 *p_Device_range_status = 5; /* HW fail */
nikapov 0:a1a69d32f310 1954 } else if (device_range_status_internal == 6 ||
nikapov 0:a1a69d32f310 1955 device_range_status_internal == 9) {
sepp_nepp 11:c6f95a42d4d7 1956 *p_Device_range_status = 4; /* Phase fail */
nikapov 0:a1a69d32f310 1957 } else if (device_range_status_internal == 8 ||
nikapov 0:a1a69d32f310 1958 device_range_status_internal == 10 ||
sepp_nepp 11:c6f95a42d4d7 1959 signal_Ref_clipflag == 1) {
sepp_nepp 11:c6f95a42d4d7 1960 *p_Device_range_status = 3; /* Min range */
nikapov 0:a1a69d32f310 1961 } else if (device_range_status_internal == 4 ||
nikapov 0:a1a69d32f310 1962 range_ignore_thresholdflag == 1) {
sepp_nepp 11:c6f95a42d4d7 1963 *p_Device_range_status = 2; /* Signal Fail */
nikapov 0:a1a69d32f310 1964 } else if (sigma_limitflag == 1) {
sepp_nepp 11:c6f95a42d4d7 1965 *p_Device_range_status = 1; /* Sigma Fail */
nikapov 0:a1a69d32f310 1966 } else {
sepp_nepp 11:c6f95a42d4d7 1967 *p_Device_range_status = 0; /* Range Valid */
nikapov 0:a1a69d32f310 1968 }
nikapov 0:a1a69d32f310 1969 }
nikapov 0:a1a69d32f310 1970
nikapov 0:a1a69d32f310 1971 /* DMAX only relevant during range error */
sepp_nepp 11:c6f95a42d4d7 1972 if (*p_Device_range_status == 0) { p_ranging_results->RangeDMaxMilliMeter = 0; }
sepp_nepp 11:c6f95a42d4d7 1973
sepp_nepp 11:c6f95a42d4d7 1974 /* fill the Limit Check ErrState */
sepp_nepp 11:c6f95a42d4d7 1975 signal_rate_final_range_limit_chk_en = Get_limit_chk_en(VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE);
sepp_nepp 11:c6f95a42d4d7 1976
sepp_nepp 11:c6f95a42d4d7 1977 if (ErrState == VL53L0X_OK) {
sepp_nepp 11:c6f95a42d4d7 1978 if ((sigma_limit_chk_en == 0) || (sigma_limitflag == 1))
sepp_nepp 11:c6f95a42d4d7 1979 { temp8 = 1; } else { temp8 = 0; }
sepp_nepp 11:c6f95a42d4d7 1980 CurrParams.LimitChecksStatus[VL53L0X_CHECKEN_SIGMA_FINAL_RANGE] = temp8;
sepp_nepp 11:c6f95a42d4d7 1981
sepp_nepp 11:c6f95a42d4d7 1982 if ((device_range_status_internal == 4) || (signal_rate_final_range_limit_chk_en == 0))
sepp_nepp 11:c6f95a42d4d7 1983 { temp8 = 1; } else { temp8 = 0; }
sepp_nepp 11:c6f95a42d4d7 1984 CurrParams.LimitChecksStatus[VL53L0X_CHECKEN_SIG_RATE_FINAL_RANGE] = temp8;
sepp_nepp 11:c6f95a42d4d7 1985
sepp_nepp 11:c6f95a42d4d7 1986 if ((signal_Ref_clip_limit_chk_en == 0) || (signal_Ref_clipflag == 1))
sepp_nepp 11:c6f95a42d4d7 1987 { temp8 = 1; } else { temp8 = 0; }
sepp_nepp 11:c6f95a42d4d7 1988 CurrParams.LimitChecksStatus[VL53L0X_CHECKEN_SIG_REF_CLIP] = temp8;
sepp_nepp 11:c6f95a42d4d7 1989
sepp_nepp 11:c6f95a42d4d7 1990 if ((range_ignore_threshold_chk_en == 0) || (range_ignore_thresholdflag == 1))
sepp_nepp 11:c6f95a42d4d7 1991 { temp8 = 1; } else { temp8 = 0;}
sepp_nepp 11:c6f95a42d4d7 1992 CurrParams.LimitChecksStatus[VL53L0X_CHECKEN_RANGE_IGNORE_THRESHOLD] = temp8;
Davidroid 3:e9269ff624ed 1993 }
nikapov 0:a1a69d32f310 1994 }
nikapov 0:a1a69d32f310 1995
sepp_nepp 11:c6f95a42d4d7 1996 void VL53L0X::Get_ranging_results(TRangeResults *p_ranging_results)
sepp_nepp 11:c6f95a42d4d7 1997 { uint8_t device_range_status;
nikapov 0:a1a69d32f310 1998 uint8_t range_fractional_enable;
sepp_nepp 11:c6f95a42d4d7 1999 uint8_t Device_range_status;
sepp_nepp 11:c6f95a42d4d7 2000 uint8_t xtalk_compensation_enable;
nikapov 0:a1a69d32f310 2001 uint16_t ambient_rate;
sepp_nepp 11:c6f95a42d4d7 2002 TFP1616 signal_rate;
sepp_nepp 11:c6f95a42d4d7 2003 uint16_t Xtalk_CompRate_MHz;
sepp_nepp 11:c6f95a42d4d7 2004 uint16_t effective_SPAD_rtn_count;
nikapov 0:a1a69d32f310 2005 uint16_t tmpuint16;
nikapov 0:a1a69d32f310 2006 uint16_t xtalk_range_milli_meter;
nikapov 0:a1a69d32f310 2007 uint16_t linearity_corrective_gain;
nikapov 0:a1a69d32f310 2008 uint8_t localBuffer[12];
sepp_nepp 11:c6f95a42d4d7 2009 TRangeResults last_range_data_buffer;
sepp_nepp 11:c6f95a42d4d7 2010
sepp_nepp 11:c6f95a42d4d7 2011 if (ErrState != VL53L0X_OK) { return; } // Do nothing while in error state
sepp_nepp 11:c6f95a42d4d7 2012
sepp_nepp 11:c6f95a42d4d7 2013 /* use multi read even if some registers are not useful,result will
sepp_nepp 11:c6f95a42d4d7 2014 * be more efficient start reading at REG_RESULT_RANGE_STATUS = 0x14
sepp_nepp 11:c6f95a42d4d7 2015 * end reading at 0x21 dec33 total 14 bytes to read */
sepp_nepp 11:c6f95a42d4d7 2016 I2c_Read(REG_RESULT_RANGE_STATUS, localBuffer,12);
sepp_nepp 11:c6f95a42d4d7 2017
sepp_nepp 11:c6f95a42d4d7 2018 if (ErrState == VL53L0X_OK) {
sepp_nepp 11:c6f95a42d4d7 2019 p_ranging_results->ZoneId = 0; /* Only one zone */
sepp_nepp 11:c6f95a42d4d7 2020 p_ranging_results->TimeStamp = 0; /* Not Implemented */
nikapov 0:a1a69d32f310 2021
sepp_nepp 7:41cbc431e1f4 2022 tmpuint16 = VL53L0X_MAKEUINT16(localBuffer[11],localBuffer[10]);
nikapov 0:a1a69d32f310 2023 /* cut1.1 if SYSTEM__RANGE_CONFIG if 1 range is 2bits fractional
sepp_nepp 11:c6f95a42d4d7 2024 *(format 11.2) else no fractional */
sepp_nepp 11:c6f95a42d4d7 2025
sepp_nepp 11:c6f95a42d4d7 2026 p_ranging_results->MeasurementTimeUsec = 0;
sepp_nepp 11:c6f95a42d4d7 2027
sepp_nepp 11:c6f95a42d4d7 2028 signal_rate = FP97_TO_FP1616(VL53L0X_MAKEUINT16(localBuffer[7],localBuffer[6]));
sepp_nepp 11:c6f95a42d4d7 2029 /* peak_SIG_count_rate_rtn_mcps */
sepp_nepp 11:c6f95a42d4d7 2030 p_ranging_results->SignalRateRtnMHz = signal_rate;
nikapov 0:a1a69d32f310 2031
sepp_nepp 7:41cbc431e1f4 2032 ambient_rate = VL53L0X_MAKEUINT16(localBuffer[9],localBuffer[8]);
sepp_nepp 11:c6f95a42d4d7 2033 p_ranging_results->AmbientRateRtnMHz = FP97_TO_FP1616(ambient_rate);
sepp_nepp 11:c6f95a42d4d7 2034
sepp_nepp 11:c6f95a42d4d7 2035 effective_SPAD_rtn_count = VL53L0X_MAKEUINT16(localBuffer[3], localBuffer[2]);
sepp_nepp 11:c6f95a42d4d7 2036 /* EffectiveSPADRtnCount is 8.8 format */
sepp_nepp 11:c6f95a42d4d7 2037 p_ranging_results->EffectiveSPADRtnCount = effective_SPAD_rtn_count;
nikapov 0:a1a69d32f310 2038
nikapov 0:a1a69d32f310 2039 device_range_status = localBuffer[0];
nikapov 0:a1a69d32f310 2040
nikapov 0:a1a69d32f310 2041 /* Get Linearity Corrective Gain */
sepp_nepp 10:cd251e0fc2fd 2042 linearity_corrective_gain = LinearityCorrectiveGain;
nikapov 0:a1a69d32f310 2043
nikapov 0:a1a69d32f310 2044 /* Get ranging configuration */
sepp_nepp 10:cd251e0fc2fd 2045 range_fractional_enable = RangeFractionalEnable;
nikapov 0:a1a69d32f310 2046
nikapov 0:a1a69d32f310 2047 if (linearity_corrective_gain != 1000) {
nikapov 0:a1a69d32f310 2048 tmpuint16 = (uint16_t)((linearity_corrective_gain
nikapov 0:a1a69d32f310 2049 * tmpuint16 + 500) / 1000);
nikapov 0:a1a69d32f310 2050
nikapov 0:a1a69d32f310 2051 /* Implement Xtalk */
sepp_nepp 11:c6f95a42d4d7 2052 Xtalk_CompRate_MHz = CurrParams.Xtalk_CompRate_MHz;
sepp_nepp 11:c6f95a42d4d7 2053 xtalk_compensation_enable = CurrParams.XTalk_Compens_En;
sepp_nepp 11:c6f95a42d4d7 2054
sepp_nepp 11:c6f95a42d4d7 2055 if (xtalk_compensation_enable) {
sepp_nepp 11:c6f95a42d4d7 2056 if ((signal_rate - ((Xtalk_CompRate_MHz
sepp_nepp 11:c6f95a42d4d7 2057 * effective_SPAD_rtn_count) >> 8)) <= 0) {
sepp_nepp 11:c6f95a42d4d7 2058 if (range_fractional_enable) { xtalk_range_milli_meter = 8888;
sepp_nepp 11:c6f95a42d4d7 2059 } else { xtalk_range_milli_meter = 8888 << 2; }
nikapov 0:a1a69d32f310 2060 } else {
sepp_nepp 11:c6f95a42d4d7 2061 xtalk_range_milli_meter = (tmpuint16 * signal_rate)
sepp_nepp 11:c6f95a42d4d7 2062 / (signal_rate - ((Xtalk_CompRate_MHz * effective_SPAD_rtn_count) >> 8));
nikapov 0:a1a69d32f310 2063 }
nikapov 0:a1a69d32f310 2064 tmpuint16 = xtalk_range_milli_meter;
nikapov 0:a1a69d32f310 2065 }
nikapov 0:a1a69d32f310 2066 }
nikapov 0:a1a69d32f310 2067
nikapov 0:a1a69d32f310 2068 if (range_fractional_enable) {
sepp_nepp 11:c6f95a42d4d7 2069 p_ranging_results->RangeMilliMeter = (uint16_t)((tmpuint16) >> 2);
sepp_nepp 11:c6f95a42d4d7 2070 p_ranging_results->RangeFractionalPart =
nikapov 0:a1a69d32f310 2071 (uint8_t)((tmpuint16 & 0x03) << 6);
nikapov 0:a1a69d32f310 2072 } else {
sepp_nepp 11:c6f95a42d4d7 2073 p_ranging_results->RangeMilliMeter = tmpuint16;
sepp_nepp 11:c6f95a42d4d7 2074 p_ranging_results->RangeFractionalPart = 0;
nikapov 0:a1a69d32f310 2075 }
nikapov 0:a1a69d32f310 2076
sepp_nepp 11:c6f95a42d4d7 2077 /* For a standard definition of RangeStatus,this should
nikapov 0:a1a69d32f310 2078 * return 0 in case of good result after a ranging
sepp_nepp 11:c6f95a42d4d7 2079 * The range ErrState depends on the device so call a device
sepp_nepp 11:c6f95a42d4d7 2080 * specific function to obtain the right ErrState. */
sepp_nepp 11:c6f95a42d4d7 2081 Get_Device_range_status(device_range_status,signal_rate,effective_SPAD_rtn_count,
sepp_nepp 11:c6f95a42d4d7 2082 p_ranging_results,&Device_range_status);
sepp_nepp 11:c6f95a42d4d7 2083 if (ErrState == VL53L0X_OK) {
sepp_nepp 11:c6f95a42d4d7 2084 p_ranging_results->RangeStatus = Device_range_status;
nikapov 0:a1a69d32f310 2085 }
nikapov 0:a1a69d32f310 2086 }
nikapov 0:a1a69d32f310 2087
sepp_nepp 11:c6f95a42d4d7 2088 if (ErrState == VL53L0X_OK) {
sepp_nepp 11:c6f95a42d4d7 2089 /* Copy last read data into device+ buffer */
sepp_nepp 10:cd251e0fc2fd 2090 last_range_data_buffer = LastRangeMeasure;
nikapov 0:a1a69d32f310 2091 last_range_data_buffer.RangeMilliMeter =
sepp_nepp 11:c6f95a42d4d7 2092 p_ranging_results->RangeMilliMeter;
nikapov 0:a1a69d32f310 2093 last_range_data_buffer.RangeFractionalPart =
sepp_nepp 11:c6f95a42d4d7 2094 p_ranging_results->RangeFractionalPart;
nikapov 0:a1a69d32f310 2095 last_range_data_buffer.RangeDMaxMilliMeter =
sepp_nepp 11:c6f95a42d4d7 2096 p_ranging_results->RangeDMaxMilliMeter;
nikapov 0:a1a69d32f310 2097 last_range_data_buffer.MeasurementTimeUsec =
sepp_nepp 11:c6f95a42d4d7 2098 p_ranging_results->MeasurementTimeUsec;
sepp_nepp 11:c6f95a42d4d7 2099 last_range_data_buffer.SignalRateRtnMHz =
sepp_nepp 11:c6f95a42d4d7 2100 p_ranging_results->SignalRateRtnMHz;
sepp_nepp 11:c6f95a42d4d7 2101 last_range_data_buffer.AmbientRateRtnMHz =
sepp_nepp 11:c6f95a42d4d7 2102 p_ranging_results->AmbientRateRtnMHz;
sepp_nepp 11:c6f95a42d4d7 2103 last_range_data_buffer.EffectiveSPADRtnCount =
sepp_nepp 11:c6f95a42d4d7 2104 p_ranging_results->EffectiveSPADRtnCount;
nikapov 0:a1a69d32f310 2105 last_range_data_buffer.RangeStatus =
sepp_nepp 11:c6f95a42d4d7 2106 p_ranging_results->RangeStatus;
sepp_nepp 10:cd251e0fc2fd 2107 LastRangeMeasure = last_range_data_buffer;
nikapov 0:a1a69d32f310 2108 }
nikapov 0:a1a69d32f310 2109 }
nikapov 0:a1a69d32f310 2110
sepp_nepp 11:c6f95a42d4d7 2111 void VL53L0X::Perf_single_ranging_measurement(
sepp_nepp 11:c6f95a42d4d7 2112 TRangeResults *p_ranging_results)
sepp_nepp 11:c6f95a42d4d7 2113 { if (ErrState != VL53L0X_OK) {return; } // no activity while in Error State!!!!
sepp_nepp 11:c6f95a42d4d7 2114
sepp_nepp 11:c6f95a42d4d7 2115 /* This function will do a complete single ranging Here we fix the mode! */
sepp_nepp 11:c6f95a42d4d7 2116 Set_device_mode(VL53L0X_DEVICEMODE_SINGLE_RANGING);
sepp_nepp 11:c6f95a42d4d7 2117
sepp_nepp 11:c6f95a42d4d7 2118 Perf_single_measurement();
sepp_nepp 11:c6f95a42d4d7 2119
sepp_nepp 11:c6f95a42d4d7 2120 Get_ranging_results(p_ranging_results);
sepp_nepp 11:c6f95a42d4d7 2121
sepp_nepp 11:c6f95a42d4d7 2122 Clear_interrupt_mask(0);
nikapov 0:a1a69d32f310 2123 }
nikapov 0:a1a69d32f310 2124
sepp_nepp 11:c6f95a42d4d7 2125 uint16_t VL53L0X::Get_Perf_Ref_SIG_measurement()
sepp_nepp 11:c6f95a42d4d7 2126 { TRangeResults ranging_results;
sepp_nepp 11:c6f95a42d4d7 2127 uint8_t orig_sequence_config;
sepp_nepp 11:c6f95a42d4d7 2128 uint16_t Ref_SIG_rate ;
nikapov 0:a1a69d32f310 2129
nikapov 0:a1a69d32f310 2130 /* store the value of the sequence config,
sepp_nepp 10:cd251e0fc2fd 2131 * this will be reset before the end of the function*/
sepp_nepp 11:c6f95a42d4d7 2132 orig_sequence_config = SequenceConfig;
sepp_nepp 10:cd251e0fc2fd 2133
sepp_nepp 10:cd251e0fc2fd 2134 /* This function performs a reference signal rate measurement.*/
sepp_nepp 11:c6f95a42d4d7 2135 Set_SequenceConfig( 0xC0 ); // sets REG_SYSTEM_SEQUENCE_CONFIG
sepp_nepp 11:c6f95a42d4d7 2136
sepp_nepp 11:c6f95a42d4d7 2137 Perf_single_ranging_measurement(&ranging_results);
sepp_nepp 11:c6f95a42d4d7 2138
sepp_nepp 11:c6f95a42d4d7 2139 Write_Byte(0xFF,0x01);
sepp_nepp 11:c6f95a42d4d7 2140 Ref_SIG_rate = Read_Word(REG_RESULT_PEAK_SIG_RATE_REF);
sepp_nepp 11:c6f95a42d4d7 2141 Write_Byte(0xFF,0x00);
sepp_nepp 11:c6f95a42d4d7 2142
sepp_nepp 11:c6f95a42d4d7 2143 /* restore the previous Sequence Config */
sepp_nepp 11:c6f95a42d4d7 2144 Set_SequenceConfig( orig_sequence_config ); // resets REG_SYSTEM_SEQUENCE_CONFIG
sepp_nepp 11:c6f95a42d4d7 2145
sepp_nepp 11:c6f95a42d4d7 2146 return Ref_SIG_rate;
nikapov 0:a1a69d32f310 2147 }
nikapov 0:a1a69d32f310 2148
sepp_nepp 11:c6f95a42d4d7 2149 void VL53L0X::Perf_Ref_SPAD_management(uint32_t *ref_SPAD_count,
sepp_nepp 11:c6f95a42d4d7 2150 uint8_t *is_aperture_SPADS)
sepp_nepp 11:c6f95a42d4d7 2151 { uint8_t last_SPAD_array[6];
nikapov 0:a1a69d32f310 2152 uint8_t start_select = 0xB4;
sepp_nepp 11:c6f95a42d4d7 2153 uint32_t minimum_SPAD_count = 3;
sepp_nepp 11:c6f95a42d4d7 2154 uint32_t max_SPAD_count = 44;
sepp_nepp 11:c6f95a42d4d7 2155 uint32_t current_SPAD_index = 0;
sepp_nepp 11:c6f95a42d4d7 2156 uint32_t last_SPAD_index = 0;
sepp_nepp 11:c6f95a42d4d7 2157 int32_t next_good_SPAD = 0;
sepp_nepp 11:c6f95a42d4d7 2158 uint16_t target_Ref_rate = 0x0A00; /* 20 MHz in 9:7 format */
sepp_nepp 11:c6f95a42d4d7 2159 uint16_t peak_SIG_rate_Ref;
sepp_nepp 11:c6f95a42d4d7 2160 uint32_t need_apt_SPADS = 0;
nikapov 0:a1a69d32f310 2161 uint32_t index = 0;
sepp_nepp 12:aa177f0e4c10 2162 uint32_t REF_SPAD_ARRAY_SIZE = 6;
nikapov 0:a1a69d32f310 2163 uint32_t signal_rate_diff = 0;
sepp_nepp 11:c6f95a42d4d7 2164 uint32_t last_SIG_rate_diff = 0;
nikapov 0:a1a69d32f310 2165 uint8_t complete = 0;
nikapov 0:a1a69d32f310 2166 uint8_t vhv_settings = 0;
nikapov 0:a1a69d32f310 2167 uint8_t phase_cal = 0;
sepp_nepp 11:c6f95a42d4d7 2168 uint32_t ref_SPAD_count_int = 0;
sepp_nepp 11:c6f95a42d4d7 2169 uint8_t is_aperture_SPADS_int = 0;
nikapov 0:a1a69d32f310 2170
nikapov 0:a1a69d32f310 2171 /*
nikapov 0:a1a69d32f310 2172 * The reference SPAD initialization procedure determines the minimum
sepp_nepp 11:c6f95a42d4d7 2173 * amount of reference SPADS to be enables to achieve a target reference
nikapov 0:a1a69d32f310 2174 * signal rate and should be performed once during initialization.
nikapov 0:a1a69d32f310 2175 *
sepp_nepp 11:c6f95a42d4d7 2176 * Either aperture or non-aperture SPADS are applied but never both.
sepp_nepp 11:c6f95a42d4d7 2177 * Firstly non-aperture SPADS are set,begining with 5 SPADS,and
sepp_nepp 11:c6f95a42d4d7 2178 * increased one SPAD at a time until the closest measurement to the
nikapov 0:a1a69d32f310 2179 * target rate is achieved.
nikapov 0:a1a69d32f310 2180 *
sepp_nepp 11:c6f95a42d4d7 2181 * If the target rate is exceeded when 5 non-aperture SPADS are enabled,
sepp_nepp 11:c6f95a42d4d7 2182 * initialization is performed instead with aperture SPADS.
nikapov 0:a1a69d32f310 2183 *
sepp_nepp 11:c6f95a42d4d7 2184 * When setting SPADS,a 'Good SPAD Map' is applied.
nikapov 0:a1a69d32f310 2185 *
nikapov 0:a1a69d32f310 2186 * This procedure operates within a SPAD window of interest of a maximum
sepp_nepp 11:c6f95a42d4d7 2187 * 44 SPADS.
sepp_nepp 7:41cbc431e1f4 2188 * The start point is currently fixed to 180,which lies towards the end
nikapov 0:a1a69d32f310 2189 * of the non-aperture quadrant and runs in to the adjacent aperture
sepp_nepp 11:c6f95a42d4d7 2190 * quadrant. */
sepp_nepp 11:c6f95a42d4d7 2191 target_Ref_rate = targetRefRate;
sepp_nepp 11:c6f95a42d4d7 2192
sepp_nepp 11:c6f95a42d4d7 2193 /* Initialize SPAD arrays.
sepp_nepp 11:c6f95a42d4d7 2194 * Currently the good SPAD map is initialised to 'All good'.
sepp_nepp 11:c6f95a42d4d7 2195 * This is a short term implementation. The good SPAD map will be
nikapov 0:a1a69d32f310 2196 * provided as an input.
nikapov 0:a1a69d32f310 2197 * Note that there are 6 bytes. Only the first 44 bits will be used to
sepp_nepp 11:c6f95a42d4d7 2198 * represent SPADS. */
sepp_nepp 12:aa177f0e4c10 2199 for (index = 0; index < REF_SPAD_ARRAY_SIZE; index++) {
sepp_nepp 11:c6f95a42d4d7 2200 SPADData.RefSPADEnables[index] = 0; }
sepp_nepp 11:c6f95a42d4d7 2201
sepp_nepp 11:c6f95a42d4d7 2202 Write_Byte(0xFF,0x01);
sepp_nepp 11:c6f95a42d4d7 2203 Write_Byte(REG_DYNAMIC_SPAD_REF_EN_START_OFFSET,0x00);
sepp_nepp 11:c6f95a42d4d7 2204 Write_Byte(REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD,0x2C);
sepp_nepp 11:c6f95a42d4d7 2205 Write_Byte(0xFF,0x00);
sepp_nepp 11:c6f95a42d4d7 2206 Write_Byte(REG_GLOBAL_CONFIG_REF_EN_START_SELECT,start_select);
sepp_nepp 11:c6f95a42d4d7 2207 Write_Byte(REG_POWER_MANAGEMENT_GO1_POWER_FORCE,0);
nikapov 0:a1a69d32f310 2208
nikapov 0:a1a69d32f310 2209 /* Perform ref calibration */
sepp_nepp 11:c6f95a42d4d7 2210 if (ErrState == VL53L0X_OK)
sepp_nepp 11:c6f95a42d4d7 2211 {Perf_Ref_calibration(&vhv_settings, &phase_cal, 0);}
sepp_nepp 11:c6f95a42d4d7 2212
sepp_nepp 11:c6f95a42d4d7 2213 if (ErrState == VL53L0X_OK) {
sepp_nepp 11:c6f95a42d4d7 2214 /* Enable Minimum NON-APERTURE SPADS */
sepp_nepp 11:c6f95a42d4d7 2215 current_SPAD_index = 0;
sepp_nepp 11:c6f95a42d4d7 2216 last_SPAD_index = current_SPAD_index;
sepp_nepp 11:c6f95a42d4d7 2217 need_apt_SPADS = 0;
sepp_nepp 11:c6f95a42d4d7 2218 Enable_Ref_SPADS(need_apt_SPADS,
sepp_nepp 11:c6f95a42d4d7 2219 SPADData.RefGoodSPADMap,
sepp_nepp 11:c6f95a42d4d7 2220 SPADData.RefSPADEnables,
sepp_nepp 12:aa177f0e4c10 2221 REF_SPAD_ARRAY_SIZE,
nikapov 0:a1a69d32f310 2222 start_select,
sepp_nepp 11:c6f95a42d4d7 2223 current_SPAD_index,
sepp_nepp 11:c6f95a42d4d7 2224 minimum_SPAD_count,
sepp_nepp 11:c6f95a42d4d7 2225 &last_SPAD_index);
nikapov 0:a1a69d32f310 2226 }
nikapov 0:a1a69d32f310 2227
sepp_nepp 11:c6f95a42d4d7 2228 if (ErrState == VL53L0X_OK) {
sepp_nepp 11:c6f95a42d4d7 2229 current_SPAD_index = last_SPAD_index;
sepp_nepp 11:c6f95a42d4d7 2230
sepp_nepp 11:c6f95a42d4d7 2231 peak_SIG_rate_Ref = Get_Perf_Ref_SIG_measurement();
sepp_nepp 11:c6f95a42d4d7 2232 if ((ErrState == VL53L0X_OK) && (peak_SIG_rate_Ref > target_Ref_rate))
sepp_nepp 11:c6f95a42d4d7 2233 { /* Signal rate measurement too high, switch to APERTURE SPADS */
sepp_nepp 12:aa177f0e4c10 2234 for (index = 0; index < REF_SPAD_ARRAY_SIZE; index++)
sepp_nepp 11:c6f95a42d4d7 2235 { SPADData.RefSPADEnables[index] = 0; }
sepp_nepp 11:c6f95a42d4d7 2236
sepp_nepp 11:c6f95a42d4d7 2237 /* Increment to the first APERTURE SPAD */
sepp_nepp 11:c6f95a42d4d7 2238 while ((Is_ApertureSPAD(start_select + current_SPAD_index)
sepp_nepp 11:c6f95a42d4d7 2239 == 0) && (current_SPAD_index < max_SPAD_count))
sepp_nepp 11:c6f95a42d4d7 2240 { current_SPAD_index++; }
sepp_nepp 11:c6f95a42d4d7 2241
sepp_nepp 11:c6f95a42d4d7 2242 need_apt_SPADS = 1;
sepp_nepp 11:c6f95a42d4d7 2243
sepp_nepp 11:c6f95a42d4d7 2244 Enable_Ref_SPADS(need_apt_SPADS,
sepp_nepp 11:c6f95a42d4d7 2245 SPADData.RefGoodSPADMap,
sepp_nepp 11:c6f95a42d4d7 2246 SPADData.RefSPADEnables,
sepp_nepp 12:aa177f0e4c10 2247 REF_SPAD_ARRAY_SIZE,
nikapov 0:a1a69d32f310 2248 start_select,
sepp_nepp 11:c6f95a42d4d7 2249 current_SPAD_index,
sepp_nepp 11:c6f95a42d4d7 2250 minimum_SPAD_count,
sepp_nepp 11:c6f95a42d4d7 2251 &last_SPAD_index);
sepp_nepp 11:c6f95a42d4d7 2252
sepp_nepp 11:c6f95a42d4d7 2253 if (ErrState == VL53L0X_OK) {
sepp_nepp 11:c6f95a42d4d7 2254 current_SPAD_index = last_SPAD_index;
sepp_nepp 11:c6f95a42d4d7 2255 peak_SIG_rate_Ref = Get_Perf_Ref_SIG_measurement();
sepp_nepp 11:c6f95a42d4d7 2256
sepp_nepp 11:c6f95a42d4d7 2257 if ((ErrState == VL53L0X_OK) && (peak_SIG_rate_Ref > target_Ref_rate))
sepp_nepp 11:c6f95a42d4d7 2258 { /* Signal rate still too high after setting the minimum number of
sepp_nepp 11:c6f95a42d4d7 2259 * APERTURE SPADS. Can do no more therefore set the min number of
sepp_nepp 11:c6f95a42d4d7 2260 * aperture SPADS as the result. */
sepp_nepp 11:c6f95a42d4d7 2261 is_aperture_SPADS_int = 1;
sepp_nepp 11:c6f95a42d4d7 2262 ref_SPAD_count_int = minimum_SPAD_count;
nikapov 0:a1a69d32f310 2263 }
nikapov 0:a1a69d32f310 2264 }
sepp_nepp 11:c6f95a42d4d7 2265 } else { need_apt_SPADS = 0;}
nikapov 0:a1a69d32f310 2266 }
nikapov 0:a1a69d32f310 2267
sepp_nepp 11:c6f95a42d4d7 2268 if ((ErrState == VL53L0X_OK) && (peak_SIG_rate_Ref < target_Ref_rate))
sepp_nepp 11:c6f95a42d4d7 2269 { /* At this point,the minimum number of either aperture
sepp_nepp 11:c6f95a42d4d7 2270 * or non-aperture SPADS have been set. Proceed to add
sepp_nepp 11:c6f95a42d4d7 2271 * SPADS and perform measurements until the target reference is reached.*/
sepp_nepp 11:c6f95a42d4d7 2272 is_aperture_SPADS_int = need_apt_SPADS;
sepp_nepp 11:c6f95a42d4d7 2273 ref_SPAD_count_int = minimum_SPAD_count;
sepp_nepp 11:c6f95a42d4d7 2274
sepp_nepp 12:aa177f0e4c10 2275 memcpy(last_SPAD_array,SPADData.RefSPADEnables, REF_SPAD_ARRAY_SIZE);
sepp_nepp 11:c6f95a42d4d7 2276 last_SIG_rate_diff = abs(peak_SIG_rate_Ref - target_Ref_rate);
nikapov 0:a1a69d32f310 2277 complete = 0;
nikapov 0:a1a69d32f310 2278
nikapov 0:a1a69d32f310 2279 while (!complete) {
sepp_nepp 11:c6f95a42d4d7 2280 Get_Next_Good_SPAD(SPADData.RefGoodSPADMap,
sepp_nepp 12:aa177f0e4c10 2281 REF_SPAD_ARRAY_SIZE,current_SPAD_index, &next_good_SPAD);
sepp_nepp 11:c6f95a42d4d7 2282
sepp_nepp 11:c6f95a42d4d7 2283 if (next_good_SPAD == -1) {
sepp_nepp 11:c6f95a42d4d7 2284 ErrState = VL53L0X_ERROR_REF_SPAD_INIT;
nikapov 0:a1a69d32f310 2285 break;
nikapov 0:a1a69d32f310 2286 }
nikapov 0:a1a69d32f310 2287
sepp_nepp 11:c6f95a42d4d7 2288 /* Cannot combine Aperture and Non-Aperture SPADS,so
sepp_nepp 11:c6f95a42d4d7 2289 * ensure the current SPAD is of the correct type. */
sepp_nepp 11:c6f95a42d4d7 2290 if (Is_ApertureSPAD((uint32_t)start_select + next_good_SPAD) !=
sepp_nepp 11:c6f95a42d4d7 2291 need_apt_SPADS) {
sepp_nepp 11:c6f95a42d4d7 2292 /* At this point we have enabled the maximum number of Aperture SPADS. */
nikapov 0:a1a69d32f310 2293 complete = 1;
nikapov 0:a1a69d32f310 2294 break;
nikapov 0:a1a69d32f310 2295 }
nikapov 0:a1a69d32f310 2296
sepp_nepp 11:c6f95a42d4d7 2297 (ref_SPAD_count_int)++;
sepp_nepp 11:c6f95a42d4d7 2298
sepp_nepp 11:c6f95a42d4d7 2299 current_SPAD_index = next_good_SPAD;
sepp_nepp 11:c6f95a42d4d7 2300 Enable_SPAD_bit(SPADData.RefSPADEnables,
sepp_nepp 12:aa177f0e4c10 2301 REF_SPAD_ARRAY_SIZE,current_SPAD_index);
sepp_nepp 11:c6f95a42d4d7 2302
sepp_nepp 11:c6f95a42d4d7 2303 if (ErrState == VL53L0X_OK) {
sepp_nepp 11:c6f95a42d4d7 2304 current_SPAD_index++;
sepp_nepp 11:c6f95a42d4d7 2305 /* Proceed to apply the additional SPAD and perform measurement. */
sepp_nepp 11:c6f95a42d4d7 2306 I2c_Write(REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0, SPADData.RefSPADEnables,6); //Set_Ref_SPAD_map
nikapov 0:a1a69d32f310 2307 }
nikapov 0:a1a69d32f310 2308
sepp_nepp 11:c6f95a42d4d7 2309 if (ErrState != VL53L0X_OK) { break; }
sepp_nepp 11:c6f95a42d4d7 2310
sepp_nepp 11:c6f95a42d4d7 2311 peak_SIG_rate_Ref = Get_Perf_Ref_SIG_measurement();
sepp_nepp 11:c6f95a42d4d7 2312
sepp_nepp 11:c6f95a42d4d7 2313 if (ErrState != VL53L0X_OK) { break; }
sepp_nepp 11:c6f95a42d4d7 2314
sepp_nepp 11:c6f95a42d4d7 2315 signal_rate_diff = abs(peak_SIG_rate_Ref - target_Ref_rate);
sepp_nepp 11:c6f95a42d4d7 2316
sepp_nepp 11:c6f95a42d4d7 2317 if (peak_SIG_rate_Ref > target_Ref_rate) {
sepp_nepp 11:c6f95a42d4d7 2318 /* Select the SPAD map that provides the
nikapov 0:a1a69d32f310 2319 * measurement closest to the target rate,
sepp_nepp 11:c6f95a42d4d7 2320 * either above or below it. */
sepp_nepp 11:c6f95a42d4d7 2321 if (signal_rate_diff > last_SIG_rate_diff) {
sepp_nepp 11:c6f95a42d4d7 2322 /* Previous SPAD map produced a closer measurement,so choose this. */
sepp_nepp 11:c6f95a42d4d7 2323 I2c_Write(REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0, last_SPAD_array,6); // Set_Ref_SPAD_map();
sepp_nepp 12:aa177f0e4c10 2324 memcpy(SPADData.RefSPADEnables,last_SPAD_array,REF_SPAD_ARRAY_SIZE);
sepp_nepp 11:c6f95a42d4d7 2325 (ref_SPAD_count_int)--;
nikapov 0:a1a69d32f310 2326 }
nikapov 0:a1a69d32f310 2327 complete = 1;
nikapov 0:a1a69d32f310 2328 } else {
sepp_nepp 11:c6f95a42d4d7 2329 /* Continue to add SPADS */
sepp_nepp 11:c6f95a42d4d7 2330 last_SIG_rate_diff = signal_rate_diff;
sepp_nepp 12:aa177f0e4c10 2331 memcpy(last_SPAD_array, SPADData.RefSPADEnables,REF_SPAD_ARRAY_SIZE);
nikapov 0:a1a69d32f310 2332 }
nikapov 0:a1a69d32f310 2333 } /* while */
nikapov 0:a1a69d32f310 2334 }
nikapov 0:a1a69d32f310 2335
sepp_nepp 11:c6f95a42d4d7 2336 if (ErrState == VL53L0X_OK) {
sepp_nepp 11:c6f95a42d4d7 2337 *ref_SPAD_count = ref_SPAD_count_int;
sepp_nepp 11:c6f95a42d4d7 2338 *is_aperture_SPADS = is_aperture_SPADS_int;
sepp_nepp 12:aa177f0e4c10 2339 RefSPADSInitialised = 1;
sepp_nepp 12:aa177f0e4c10 2340 ReferenceSPADCount = (uint8_t)(*ref_SPAD_count);
sepp_nepp 12:aa177f0e4c10 2341 ReferenceSPADType = *is_aperture_SPADS;
nikapov 0:a1a69d32f310 2342 }
nikapov 0:a1a69d32f310 2343 }
nikapov 0:a1a69d32f310 2344
sepp_nepp 11:c6f95a42d4d7 2345 void VL53L0X::Set_Reference_SPADS(uint32_t count,uint8_t is_aperture_SPADS)
sepp_nepp 11:c6f95a42d4d7 2346 { uint32_t current_SPAD_index = 0;
sepp_nepp 11:c6f95a42d4d7 2347 uint8_t start_select = 0xB4;
sepp_nepp 11:c6f95a42d4d7 2348 uint32_t max_SPAD_count = 44;
sepp_nepp 11:c6f95a42d4d7 2349 uint32_t last_SPAD_index;
nikapov 0:a1a69d32f310 2350 uint32_t index;
nikapov 0:a1a69d32f310 2351
sepp_nepp 11:c6f95a42d4d7 2352 /* This function applies a requested number of reference SPADS,either
sepp_nepp 11:c6f95a42d4d7 2353 * aperture or non-aperture,as requested. The good SPAD map will be applied.*/
sepp_nepp 11:c6f95a42d4d7 2354 Write_Byte(0xFF,0x01);
sepp_nepp 11:c6f95a42d4d7 2355 Write_Byte(REG_DYNAMIC_SPAD_REF_EN_START_OFFSET,0x00);
sepp_nepp 11:c6f95a42d4d7 2356 Write_Byte(REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD,0x2C);
sepp_nepp 11:c6f95a42d4d7 2357 Write_Byte(0xFF,0x00);
sepp_nepp 11:c6f95a42d4d7 2358 Write_Byte(REG_GLOBAL_CONFIG_REF_EN_START_SELECT, start_select);
sepp_nepp 11:c6f95a42d4d7 2359
sepp_nepp 12:aa177f0e4c10 2360 for (index = 0; index < REF_SPAD_ARRAY_SIZE; index++) {
sepp_nepp 11:c6f95a42d4d7 2361 SPADData.RefSPADEnables[index] = 0; }
sepp_nepp 11:c6f95a42d4d7 2362
sepp_nepp 11:c6f95a42d4d7 2363 if (is_aperture_SPADS) {
sepp_nepp 11:c6f95a42d4d7 2364 /* Increment to the first APERTURE SPAD */
sepp_nepp 11:c6f95a42d4d7 2365 while ((Is_ApertureSPAD(start_select + current_SPAD_index) == 0) &&
sepp_nepp 11:c6f95a42d4d7 2366 (current_SPAD_index < max_SPAD_count)) {
sepp_nepp 11:c6f95a42d4d7 2367 current_SPAD_index++;
nikapov 0:a1a69d32f310 2368 }
nikapov 0:a1a69d32f310 2369 }
sepp_nepp 12:aa177f0e4c10 2370 Enable_Ref_SPADS(is_aperture_SPADS, SPADData.RefGoodSPADMap,
sepp_nepp 11:c6f95a42d4d7 2371 SPADData.RefSPADEnables,
sepp_nepp 12:aa177f0e4c10 2372 REF_SPAD_ARRAY_SIZE,
nikapov 0:a1a69d32f310 2373 start_select,
sepp_nepp 11:c6f95a42d4d7 2374 current_SPAD_index,
nikapov 0:a1a69d32f310 2375 count,
sepp_nepp 11:c6f95a42d4d7 2376 &last_SPAD_index);
sepp_nepp 11:c6f95a42d4d7 2377
sepp_nepp 11:c6f95a42d4d7 2378 if (ErrState == VL53L0X_OK) {
sepp_nepp 12:aa177f0e4c10 2379 RefSPADSInitialised = 1;
sepp_nepp 12:aa177f0e4c10 2380 ReferenceSPADCount = (uint8_t)(count);
sepp_nepp 12:aa177f0e4c10 2381 ReferenceSPADType = is_aperture_SPADS;
nikapov 0:a1a69d32f310 2382 }
nikapov 0:a1a69d32f310 2383 }
nikapov 0:a1a69d32f310 2384
sepp_nepp 11:c6f95a42d4d7 2385 void VL53L0X::Set_GPIO_config(VL53L0X_DeviceModes device_mode,
sepp_nepp 11:c6f95a42d4d7 2386 TGPIO_Func functionality, VL53L0X_InterruptPolarity polarity)
sepp_nepp 11:c6f95a42d4d7 2387 { uint8_t pol_data;
sepp_nepp 11:c6f95a42d4d7 2388
sepp_nepp 11:c6f95a42d4d7 2389 if (polarity == VL53L0X_INTERRUPTPOLARITY_LOW)
sepp_nepp 11:c6f95a42d4d7 2390 { pol_data = 0x00;} else { pol_data = 0x10;}
sepp_nepp 11:c6f95a42d4d7 2391
sepp_nepp 11:c6f95a42d4d7 2392 switch ( device_mode ) {
sepp_nepp 11:c6f95a42d4d7 2393 case VL53L0X_DEVICEMODE_GPIO_DRIVE:
sepp_nepp 11:c6f95a42d4d7 2394 Write_Byte(REG_GPIO_HV_MUX_ACTIVE_HIGH,pol_data);
sepp_nepp 11:c6f95a42d4d7 2395 break;
sepp_nepp 11:c6f95a42d4d7 2396 case VL53L0X_DEVICEMODE_GPIO_OSC:
sepp_nepp 11:c6f95a42d4d7 2397 Write_Byte(0xff,0x01);
sepp_nepp 11:c6f95a42d4d7 2398 Write_Byte(0x00,0x00);
sepp_nepp 11:c6f95a42d4d7 2399 Write_Byte(0xff,0x00);
sepp_nepp 11:c6f95a42d4d7 2400 Write_Byte(0x80,0x01);
sepp_nepp 11:c6f95a42d4d7 2401 Write_Byte(0x85,0x02);
sepp_nepp 11:c6f95a42d4d7 2402 Write_Byte(0xff,0x04);
sepp_nepp 11:c6f95a42d4d7 2403 Write_Byte(0xcd,0x00);
sepp_nepp 11:c6f95a42d4d7 2404 Write_Byte(0xcc,0x11);
sepp_nepp 11:c6f95a42d4d7 2405 Write_Byte(0xff,0x07);
sepp_nepp 11:c6f95a42d4d7 2406 Write_Byte(0xbe,0x00);
sepp_nepp 11:c6f95a42d4d7 2407 Write_Byte(0xff,0x06);
sepp_nepp 11:c6f95a42d4d7 2408 Write_Byte(0xcc,0x09);
sepp_nepp 11:c6f95a42d4d7 2409 Write_Byte(0xff,0x00);
sepp_nepp 11:c6f95a42d4d7 2410 Write_Byte(0xff,0x01);
sepp_nepp 11:c6f95a42d4d7 2411 Write_Byte(0x00,0x00);
sepp_nepp 11:c6f95a42d4d7 2412 break;
sepp_nepp 11:c6f95a42d4d7 2413 default:
sepp_nepp 11:c6f95a42d4d7 2414 if (functionality>GPIO_FUNC_NEW_MEASURE_READY)
sepp_nepp 11:c6f95a42d4d7 2415 { ErrState = VL53L0X_ERROR_GPIO_FUNC_NOT_SUPPORTED; }
sepp_nepp 11:c6f95a42d4d7 2416 else { Write_Byte(REG_SYSINT_CONFIG_GPIO,functionality); }
sepp_nepp 11:c6f95a42d4d7 2417
sepp_nepp 11:c6f95a42d4d7 2418 if (ErrState == VL53L0X_OK)
sepp_nepp 11:c6f95a42d4d7 2419 { Register_BitMask(REG_GPIO_HV_MUX_ACTIVE_HIGH,0xEF,pol_data); }
sepp_nepp 11:c6f95a42d4d7 2420
sepp_nepp 11:c6f95a42d4d7 2421 if (ErrState == VL53L0X_OK)
sepp_nepp 12:aa177f0e4c10 2422 {GpioFunctionality = functionality; }
sepp_nepp 11:c6f95a42d4d7 2423
sepp_nepp 11:c6f95a42d4d7 2424 Clear_interrupt_mask(0);
sepp_nepp 11:c6f95a42d4d7 2425 } // switch
sepp_nepp 11:c6f95a42d4d7 2426 } // Set_GPIO_config
sepp_nepp 11:c6f95a42d4d7 2427
sepp_nepp 11:c6f95a42d4d7 2428 /* Encode timeout in macro periods in (LSByte * 2^MSByte) + 1 format */
sepp_nepp 11:c6f95a42d4d7 2429 uint16_t VL53L0X::Encode_timeout(uint32_t timeout_macro_clks)
sepp_nepp 11:c6f95a42d4d7 2430 { uint16_t encoded_timeout = 0;
nikapov 0:a1a69d32f310 2431 uint16_t ms_byte = 0;
nikapov 0:a1a69d32f310 2432
nikapov 0:a1a69d32f310 2433 if (timeout_macro_clks > 0) {
sepp_nepp 11:c6f95a42d4d7 2434 timeout_macro_clks = timeout_macro_clks - 1;
sepp_nepp 11:c6f95a42d4d7 2435 while ((timeout_macro_clks & 0xFFFFFF00) > 0) {
sepp_nepp 11:c6f95a42d4d7 2436 timeout_macro_clks = timeout_macro_clks >> 1;
nikapov 0:a1a69d32f310 2437 ms_byte++;
sepp_nepp 11:c6f95a42d4d7 2438 } // while
sepp_nepp 11:c6f95a42d4d7 2439 encoded_timeout = (ms_byte << 8) + (uint16_t)(timeout_macro_clks & 0x000000FF);
sepp_nepp 11:c6f95a42d4d7 2440 }
nikapov 0:a1a69d32f310 2441 return encoded_timeout;
nikapov 0:a1a69d32f310 2442 }
nikapov 0:a1a69d32f310 2443
sepp_nepp 11:c6f95a42d4d7 2444 void VL53L0X::Set_Sequence_Step_Timeout(VL53L0X_SequenceStepId sequence_step_id,
nikapov 0:a1a69d32f310 2445 uint32_t timeout_micro_secs)
sepp_nepp 11:c6f95a42d4d7 2446 { uint8_t current_vcsel_PPeriod_p_clk;
nikapov 0:a1a69d32f310 2447 uint8_t msrc_encoded_time_out;
nikapov 0:a1a69d32f310 2448 uint16_t pre_range_encoded_time_out;
nikapov 0:a1a69d32f310 2449 uint16_t pre_range_time_out_m_clks;
nikapov 0:a1a69d32f310 2450 uint16_t msrc_range_time_out_m_clks;
nikapov 0:a1a69d32f310 2451 uint32_t final_range_time_out_m_clks;
nikapov 0:a1a69d32f310 2452 uint16_t final_range_encoded_time_out;
sepp_nepp 11:c6f95a42d4d7 2453 VL53L0X_Sequence_Steps_t sequence_steps;
sepp_nepp 11:c6f95a42d4d7 2454
sepp_nepp 11:c6f95a42d4d7 2455 switch (sequence_step_id) {
sepp_nepp 11:c6f95a42d4d7 2456 case VL53L0X_SEQUENCESTEP_TCC:
sepp_nepp 11:c6f95a42d4d7 2457 case VL53L0X_SEQUENCESTEP_DSS:
sepp_nepp 11:c6f95a42d4d7 2458 case VL53L0X_SEQUENCESTEP_MSRC:
sepp_nepp 11:c6f95a42d4d7 2459 current_vcsel_PPeriod_p_clk = /* Gets and converts the VCSEL period register into actual clock periods */
sepp_nepp 11:c6f95a42d4d7 2460 ( Read_Byte(REG_PRE_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1;
sepp_nepp 11:c6f95a42d4d7 2461
sepp_nepp 11:c6f95a42d4d7 2462 if (ErrState == VL53L0X_OK) {
sepp_nepp 11:c6f95a42d4d7 2463 msrc_range_time_out_m_clks = Calc_timeout_mclks(timeout_micro_secs,
sepp_nepp 11:c6f95a42d4d7 2464 (uint8_t)current_vcsel_PPeriod_p_clk);
sepp_nepp 11:c6f95a42d4d7 2465
sepp_nepp 11:c6f95a42d4d7 2466 if (msrc_range_time_out_m_clks > 256) { msrc_encoded_time_out = 255;}
sepp_nepp 11:c6f95a42d4d7 2467 else {msrc_encoded_time_out = (uint8_t)msrc_range_time_out_m_clks - 1; }
nikapov 0:a1a69d32f310 2468
sepp_nepp 12:aa177f0e4c10 2469 LastEncodedTimeout = msrc_encoded_time_out;
sepp_nepp 11:c6f95a42d4d7 2470 }
sepp_nepp 11:c6f95a42d4d7 2471 Write_Byte(REG_MSRC_CONFIG_TIMEOUT_MACROP,msrc_encoded_time_out);
sepp_nepp 11:c6f95a42d4d7 2472 break;
sepp_nepp 11:c6f95a42d4d7 2473
sepp_nepp 11:c6f95a42d4d7 2474 case VL53L0X_SEQUENCESTEP_PRE_RANGE:
sepp_nepp 11:c6f95a42d4d7 2475 current_vcsel_PPeriod_p_clk = /* Gets and converts the VCSEL period register into actual clock periods */
sepp_nepp 11:c6f95a42d4d7 2476 ( Read_Byte(REG_PRE_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1;
sepp_nepp 11:c6f95a42d4d7 2477
sepp_nepp 11:c6f95a42d4d7 2478 pre_range_time_out_m_clks = Calc_timeout_mclks(timeout_micro_secs,
sepp_nepp 11:c6f95a42d4d7 2479 (uint8_t)current_vcsel_PPeriod_p_clk);
sepp_nepp 11:c6f95a42d4d7 2480 pre_range_encoded_time_out = Encode_timeout(pre_range_time_out_m_clks);
sepp_nepp 11:c6f95a42d4d7 2481
sepp_nepp 12:aa177f0e4c10 2482 LastEncodedTimeout = pre_range_encoded_time_out;
sepp_nepp 11:c6f95a42d4d7 2483
sepp_nepp 11:c6f95a42d4d7 2484 Write_Word(REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI,pre_range_encoded_time_out);
sepp_nepp 11:c6f95a42d4d7 2485
sepp_nepp 11:c6f95a42d4d7 2486 if (ErrState == VL53L0X_OK)
sepp_nepp 12:aa177f0e4c10 2487 {PreRangeTimeoutMicroSecs=timeout_micro_secs; }
sepp_nepp 11:c6f95a42d4d7 2488 break;
sepp_nepp 11:c6f95a42d4d7 2489
sepp_nepp 11:c6f95a42d4d7 2490 case VL53L0X_SEQUENCESTEP_FINAL_RANGE:
sepp_nepp 11:c6f95a42d4d7 2491 /* For the final range timeout,the pre-range timeout must be added.
sepp_nepp 11:c6f95a42d4d7 2492 * To do this both final and pre-range timeouts must be expressed in
sepp_nepp 11:c6f95a42d4d7 2493 * macro periods MClks because they have different vcsel periods.*/
sepp_nepp 11:c6f95a42d4d7 2494 sequence_steps = Get_sequence_step_enables();
nikapov 0:a1a69d32f310 2495 pre_range_time_out_m_clks = 0;
sepp_nepp 11:c6f95a42d4d7 2496
sepp_nepp 11:c6f95a42d4d7 2497 if (sequence_steps.PreRangeOn)
sepp_nepp 11:c6f95a42d4d7 2498 { current_vcsel_PPeriod_p_clk = /* Gets and converts the VCSEL period register into actual clock periods */
sepp_nepp 11:c6f95a42d4d7 2499 ( Read_Byte(REG_PRE_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1;
sepp_nepp 11:c6f95a42d4d7 2500
sepp_nepp 11:c6f95a42d4d7 2501 /* Retrieve PRE-RANGE Timeout in Macro periods (MCLKS) */
sepp_nepp 11:c6f95a42d4d7 2502 if (ErrState == VL53L0X_OK)
sepp_nepp 11:c6f95a42d4d7 2503 { pre_range_encoded_time_out = Read_Word(0x51);
sepp_nepp 11:c6f95a42d4d7 2504 pre_range_time_out_m_clks = Decode_timeout(pre_range_encoded_time_out);
sepp_nepp 11:c6f95a42d4d7 2505 }
sepp_nepp 11:c6f95a42d4d7 2506 }
sepp_nepp 11:c6f95a42d4d7 2507
sepp_nepp 11:c6f95a42d4d7 2508 /* Calculate FINAL RANGE Timeout in Macro Periode (MCLKS) and add PRE-RANGE value */
sepp_nepp 11:c6f95a42d4d7 2509 if (ErrState == VL53L0X_OK)
sepp_nepp 11:c6f95a42d4d7 2510 { current_vcsel_PPeriod_p_clk /* Get and converts the VCSEL period register into actual clock periods */
sepp_nepp 11:c6f95a42d4d7 2511 = ( Read_Byte(REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD) + 1) << 1; }
sepp_nepp 11:c6f95a42d4d7 2512
sepp_nepp 11:c6f95a42d4d7 2513 if (ErrState == VL53L0X_OK)
sepp_nepp 11:c6f95a42d4d7 2514 { final_range_time_out_m_clks = Calc_timeout_mclks(timeout_micro_secs,
sepp_nepp 11:c6f95a42d4d7 2515 (uint8_t) current_vcsel_PPeriod_p_clk);
nikapov 0:a1a69d32f310 2516
nikapov 0:a1a69d32f310 2517 final_range_time_out_m_clks += pre_range_time_out_m_clks;
nikapov 0:a1a69d32f310 2518
sepp_nepp 11:c6f95a42d4d7 2519 final_range_encoded_time_out = Encode_timeout(final_range_time_out_m_clks);
sepp_nepp 11:c6f95a42d4d7 2520
sepp_nepp 11:c6f95a42d4d7 2521 Write_Word(0x71,final_range_encoded_time_out);
sepp_nepp 11:c6f95a42d4d7 2522 }
sepp_nepp 11:c6f95a42d4d7 2523
sepp_nepp 11:c6f95a42d4d7 2524 if (ErrState == VL53L0X_OK)
sepp_nepp 12:aa177f0e4c10 2525 { FinalRangeTimeoutMicroSecs = timeout_micro_secs; }
sepp_nepp 11:c6f95a42d4d7 2526 break;
sepp_nepp 11:c6f95a42d4d7 2527
sepp_nepp 11:c6f95a42d4d7 2528 default: ErrState = VL53L0X_ERROR_INVALID_PARAMS;
sepp_nepp 11:c6f95a42d4d7 2529 } // switch (sequence_step_id)
sepp_nepp 11:c6f95a42d4d7 2530 }
sepp_nepp 11:c6f95a42d4d7 2531
sepp_nepp 11:c6f95a42d4d7 2532 void VL53L0X::Set_Measure_Time_Budget_us (uint32_t Measure_Time_Budget_us)
sepp_nepp 11:c6f95a42d4d7 2533 { uint32_t final_range_timing_budget_us;
sepp_nepp 11:c6f95a42d4d7 2534 VL53L0X_Sequence_Steps_t sequence_steps;
sepp_nepp 11:c6f95a42d4d7 2535 uint32_t msrc_dcc_tcc_timeout_us= 2000;
sepp_nepp 10:cd251e0fc2fd 2536 uint32_t start_overhead_us = 1910;
sepp_nepp 10:cd251e0fc2fd 2537 uint32_t end_overhead_us = 960;
sepp_nepp 10:cd251e0fc2fd 2538 uint32_t msrc_overhead_us = 660;
sepp_nepp 10:cd251e0fc2fd 2539 uint32_t tcc_overhead_us = 590;
sepp_nepp 10:cd251e0fc2fd 2540 uint32_t dss_overhead_us = 690;
sepp_nepp 10:cd251e0fc2fd 2541 uint32_t pre_range_overhead_us = 660;
sepp_nepp 11:c6f95a42d4d7 2542 uint32_t final_range_overhead_us= 550;
sepp_nepp 10:cd251e0fc2fd 2543 uint32_t pre_range_timeout_us = 0;
sepp_nepp 10:cd251e0fc2fd 2544 uint32_t c_min_timing_budget_us = 20000;
nikapov 0:a1a69d32f310 2545 uint32_t sub_timeout = 0;
nikapov 0:a1a69d32f310 2546
sepp_nepp 11:c6f95a42d4d7 2547 if (Measure_Time_Budget_us < c_min_timing_budget_us)
sepp_nepp 11:c6f95a42d4d7 2548 { ErrState = VL53L0X_ERROR_INVALID_PARAMS;
sepp_nepp 11:c6f95a42d4d7 2549 return ; }
sepp_nepp 11:c6f95a42d4d7 2550
sepp_nepp 11:c6f95a42d4d7 2551 final_range_timing_budget_us = Measure_Time_Budget_us -
sepp_nepp 11:c6f95a42d4d7 2552 (start_overhead_us + end_overhead_us);
sepp_nepp 11:c6f95a42d4d7 2553
sepp_nepp 11:c6f95a42d4d7 2554 sequence_steps = Get_sequence_step_enables();
sepp_nepp 11:c6f95a42d4d7 2555
sepp_nepp 11:c6f95a42d4d7 2556 if (ErrState == VL53L0X_OK &&
sepp_nepp 11:c6f95a42d4d7 2557 (sequence_steps.TccOn ||
sepp_nepp 11:c6f95a42d4d7 2558 sequence_steps.MsrcOn ||
sepp_nepp 11:c6f95a42d4d7 2559 sequence_steps.DssOn)) {
nikapov 0:a1a69d32f310 2560
sepp_nepp 7:41cbc431e1f4 2561 /* TCC,MSRC and DSS all share the same timeout */
sepp_nepp 11:c6f95a42d4d7 2562 Get_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_MSRC,
sepp_nepp 10:cd251e0fc2fd 2563 &msrc_dcc_tcc_timeout_us);
nikapov 0:a1a69d32f310 2564
sepp_nepp 11:c6f95a42d4d7 2565 /* Subtract the TCC,MSRC and DSS timeouts if they are enabled. */
sepp_nepp 11:c6f95a42d4d7 2566 if (ErrState != VL53L0X_OK) {return ; }
nikapov 0:a1a69d32f310 2567
nikapov 0:a1a69d32f310 2568 /* TCC */
sepp_nepp 11:c6f95a42d4d7 2569 if (sequence_steps.TccOn) {
sepp_nepp 11:c6f95a42d4d7 2570
sepp_nepp 11:c6f95a42d4d7 2571 sub_timeout = msrc_dcc_tcc_timeout_us + tcc_overhead_us;
sepp_nepp 11:c6f95a42d4d7 2572
sepp_nepp 11:c6f95a42d4d7 2573 if (sub_timeout < final_range_timing_budget_us) {
sepp_nepp 11:c6f95a42d4d7 2574 final_range_timing_budget_us -= sub_timeout;
sepp_nepp 11:c6f95a42d4d7 2575 } else { /* Requested timeout too big. */
sepp_nepp 11:c6f95a42d4d7 2576 ErrState = VL53L0X_ERROR_INVALID_PARAMS;
nikapov 0:a1a69d32f310 2577 }
nikapov 0:a1a69d32f310 2578 }
nikapov 0:a1a69d32f310 2579
sepp_nepp 11:c6f95a42d4d7 2580 if (ErrState != VL53L0X_OK) {return; }
nikapov 0:a1a69d32f310 2581
nikapov 0:a1a69d32f310 2582 /* DSS */
sepp_nepp 11:c6f95a42d4d7 2583 if (sequence_steps.DssOn)
sepp_nepp 11:c6f95a42d4d7 2584 { sub_timeout = 2 * (msrc_dcc_tcc_timeout_us + dss_overhead_us);
sepp_nepp 11:c6f95a42d4d7 2585
sepp_nepp 11:c6f95a42d4d7 2586 if (sub_timeout < final_range_timing_budget_us)
sepp_nepp 11:c6f95a42d4d7 2587 { final_range_timing_budget_us -= sub_timeout; }
sepp_nepp 11:c6f95a42d4d7 2588 else { /* Requested timeout too big. */
sepp_nepp 11:c6f95a42d4d7 2589 ErrState = VL53L0X_ERROR_INVALID_PARAMS; }
sepp_nepp 11:c6f95a42d4d7 2590 }
sepp_nepp 11:c6f95a42d4d7 2591 else if (sequence_steps.MsrcOn) /* MSRC */
sepp_nepp 11:c6f95a42d4d7 2592 { sub_timeout = msrc_dcc_tcc_timeout_us + msrc_overhead_us;
sepp_nepp 11:c6f95a42d4d7 2593
sepp_nepp 11:c6f95a42d4d7 2594 if (sub_timeout < final_range_timing_budget_us)
sepp_nepp 11:c6f95a42d4d7 2595 { final_range_timing_budget_us -= sub_timeout; }
sepp_nepp 11:c6f95a42d4d7 2596 else /* Requested timeout too big. */
sepp_nepp 11:c6f95a42d4d7 2597 { ErrState = VL53L0X_ERROR_INVALID_PARAMS; }
sepp_nepp 11:c6f95a42d4d7 2598 }
nikapov 0:a1a69d32f310 2599 }
nikapov 0:a1a69d32f310 2600
sepp_nepp 11:c6f95a42d4d7 2601 if (ErrState != VL53L0X_OK) {return; }
sepp_nepp 11:c6f95a42d4d7 2602
sepp_nepp 11:c6f95a42d4d7 2603 if (sequence_steps.PreRangeOn) {
nikapov 0:a1a69d32f310 2604 /* Subtract the Pre-range timeout if enabled. */
sepp_nepp 11:c6f95a42d4d7 2605 Get_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_PRE_RANGE,
sepp_nepp 10:cd251e0fc2fd 2606 &pre_range_timeout_us);
sepp_nepp 11:c6f95a42d4d7 2607 sub_timeout = pre_range_timeout_us + pre_range_overhead_us;
sepp_nepp 10:cd251e0fc2fd 2608
sepp_nepp 10:cd251e0fc2fd 2609 if (sub_timeout < final_range_timing_budget_us) {
sepp_nepp 10:cd251e0fc2fd 2610 final_range_timing_budget_us -= sub_timeout;
nikapov 0:a1a69d32f310 2611 } else {
nikapov 0:a1a69d32f310 2612 /* Requested timeout too big. */
sepp_nepp 11:c6f95a42d4d7 2613 ErrState = VL53L0X_ERROR_INVALID_PARAMS;
nikapov 0:a1a69d32f310 2614 }
nikapov 0:a1a69d32f310 2615 }
nikapov 0:a1a69d32f310 2616
sepp_nepp 11:c6f95a42d4d7 2617 if (ErrState == VL53L0X_OK && sequence_steps.FinalRangeOn)
sepp_nepp 11:c6f95a42d4d7 2618 { final_range_timing_budget_us -= final_range_overhead_us;
nikapov 0:a1a69d32f310 2619
nikapov 0:a1a69d32f310 2620 /* Final Range Timeout
nikapov 0:a1a69d32f310 2621 * Note that the final range timeout is determined by the timing
nikapov 0:a1a69d32f310 2622 * budget and the sum of all other timeouts within the sequence.
sepp_nepp 7:41cbc431e1f4 2623 * If there is no room for the final range timeout,then an error
nikapov 0:a1a69d32f310 2624 * will be set. Otherwise the remaining time will be applied to
nikapov 0:a1a69d32f310 2625 * the final range.
nikapov 0:a1a69d32f310 2626 */
sepp_nepp 11:c6f95a42d4d7 2627 Set_Sequence_Step_Timeout(VL53L0X_SEQUENCESTEP_FINAL_RANGE,
sepp_nepp 10:cd251e0fc2fd 2628 final_range_timing_budget_us);
sepp_nepp 10:cd251e0fc2fd 2629
sepp_nepp 11:c6f95a42d4d7 2630 CurrParams.Measure_Time_Budget_us = Measure_Time_Budget_us;
nikapov 0:a1a69d32f310 2631 }
nikapov 0:a1a69d32f310 2632 }
nikapov 0:a1a69d32f310 2633
sepp_nepp 11:c6f95a42d4d7 2634 const uint8_t SEQUENCESTEP_MASK[] =
sepp_nepp 11:c6f95a42d4d7 2635 { 0x10, //VL53L0X_SEQUENCESTEP_TCC = 0
sepp_nepp 11:c6f95a42d4d7 2636 0x28, //VL53L0X_SEQUENCESTEP_DSS = 1
sepp_nepp 11:c6f95a42d4d7</