Team Virgo v3 / Orion_newPCB_test_LV

Implement new controller

Dependencies:   mbed-rtos mbed QEI BNO055 MPU6050_DMP_Nucleo-I2Cdev virgo3_imuHandler_Orion_PCB MAX17048 Servo

Fork of Orion_newPCB_test by Team Virgo v3

07_Ranging/VL53L0X.cpp@30:44676e1b38f8, 2018-03-20 (annotated)

Committer:
ahmed_lv
Date:
Tue Mar 20 05:56:22 2018 +0000
Revision:
30:44676e1b38f8
Parent:
27:2db168d9fb18 
Editted Input Variables to PID

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ahmed_lv 27:2db168d9fb18 1 // Most of the functionality of this library is based on the VL53L0X API
ahmed_lv 27:2db168d9fb18 2 // provided by ST (STSW-IMG005), and some of the explanatory comments are quoted
ahmed_lv 27:2db168d9fb18 3 // or paraphrased from the API source code, API user manual (UM2039), and the
ahmed_lv 27:2db168d9fb18 4 // VL53L0X datasheet.
ahmed_lv 27:2db168d9fb18 5
ahmed_lv 27:2db168d9fb18 6 #include <memory>
ahmed_lv 27:2db168d9fb18 7 #include <VL53L0X.h>
ahmed_lv 27:2db168d9fb18 8
ahmed_lv 27:2db168d9fb18 9 // Defines /////////////////////////////////////////////////////////////////////
ahmed_lv 27:2db168d9fb18 10
ahmed_lv 27:2db168d9fb18 11 // The Arduino two-wire interface uses a 7-bit number for the address,
ahmed_lv 27:2db168d9fb18 12 // and sets the last bit correctly based on reads and writes
ahmed_lv 27:2db168d9fb18 13 #define ADDRESS_DEFAULT (0b0101001 << 1)
ahmed_lv 27:2db168d9fb18 14
ahmed_lv 27:2db168d9fb18 15 // Record the current time to check an upcoming timeout against
ahmed_lv 27:2db168d9fb18 16 #define startTimeout() (timeout_start_ms = millis())
ahmed_lv 27:2db168d9fb18 17
ahmed_lv 27:2db168d9fb18 18 // Check if timeout is enabled (set to nonzero value) and has expired
ahmed_lv 27:2db168d9fb18 19 #define checkTimeoutExpired() (io_timeout > 0 && ((uint16_t)millis() - timeout_start_ms) > io_timeout)
ahmed_lv 27:2db168d9fb18 20
ahmed_lv 27:2db168d9fb18 21 // Decode VCSEL (vertical cavity surface emitting laser) pulse period in PCLKs
ahmed_lv 27:2db168d9fb18 22 // from register value
ahmed_lv 27:2db168d9fb18 23 // based on VL53L0X_decode_vcsel_period()
ahmed_lv 27:2db168d9fb18 24 #define decodeVcselPeriod(reg_val) (((reg_val) + 1) << 1)
ahmed_lv 27:2db168d9fb18 25
ahmed_lv 27:2db168d9fb18 26 // Encode VCSEL pulse period register value from period in PCLKs
ahmed_lv 27:2db168d9fb18 27 // based on VL53L0X_encode_vcsel_period()
ahmed_lv 27:2db168d9fb18 28 #define encodeVcselPeriod(period_pclks) (((period_pclks) >> 1) - 1)
ahmed_lv 27:2db168d9fb18 29
ahmed_lv 27:2db168d9fb18 30 // Calculate macro period in *nanoseconds* from VCSEL period in PCLKs
ahmed_lv 27:2db168d9fb18 31 // based on VL53L0X_calc_macro_period_ps()
ahmed_lv 27:2db168d9fb18 32 // PLL_period_ps = 1655; macro_period_vclks = 2304
ahmed_lv 27:2db168d9fb18 33 #define calcMacroPeriod(vcsel_period_pclks) ((((uint32_t)2304 * (vcsel_period_pclks) * 1655) + 500) / 1000)
ahmed_lv 27:2db168d9fb18 34
ahmed_lv 27:2db168d9fb18 35 // Transmission status (https://www.arduino.cc/en/Reference/WireEndTransmission)
ahmed_lv 27:2db168d9fb18 36 #define ERR_OK 0
ahmed_lv 27:2db168d9fb18 37 #define ERR_NACK_ADDR 2 // received NACK on transmit of address
ahmed_lv 27:2db168d9fb18 38 #define ERR_NACK_DATA 3 // received NACK on transmit of data
ahmed_lv 27:2db168d9fb18 39 #define ERR_OTHER 4
ahmed_lv 27:2db168d9fb18 40
ahmed_lv 27:2db168d9fb18 41 #define millis() timer->read_ms()
ahmed_lv 27:2db168d9fb18 42
ahmed_lv 27:2db168d9fb18 43 // Constructors ////////////////////////////////////////////////////////////////
ahmed_lv 27:2db168d9fb18 44
ahmed_lv 27:2db168d9fb18 45 VL53L0X::VL53L0X(I2C* i2c, Timer* timer)
ahmed_lv 27:2db168d9fb18 46 : address(ADDRESS_DEFAULT)
ahmed_lv 27:2db168d9fb18 47 , io_timeout(0) // no timeout
ahmed_lv 27:2db168d9fb18 48 , did_timeout(false)
ahmed_lv 27:2db168d9fb18 49 , i2c(i2c)
ahmed_lv 27:2db168d9fb18 50 , timer(timer)
ahmed_lv 27:2db168d9fb18 51 {
ahmed_lv 27:2db168d9fb18 52 }
ahmed_lv 27:2db168d9fb18 53
ahmed_lv 27:2db168d9fb18 54 // Public Methods //////////////////////////////////////////////////////////////
ahmed_lv 27:2db168d9fb18 55
ahmed_lv 27:2db168d9fb18 56 void VL53L0X::setAddress(uint8_t new_addr)
ahmed_lv 27:2db168d9fb18 57 {
ahmed_lv 27:2db168d9fb18 58 writeReg(I2C_SLAVE_DEVICE_ADDRESS, new_addr & 0x7F);
ahmed_lv 27:2db168d9fb18 59 address = new_addr << 1;
ahmed_lv 27:2db168d9fb18 60 }
ahmed_lv 27:2db168d9fb18 61
ahmed_lv 27:2db168d9fb18 62 // Initialize sensor using sequence based on VL53L0X_DataInit(),
ahmed_lv 27:2db168d9fb18 63 // VL53L0X_StaticInit(), and VL53L0X_PerformRefCalibration().
ahmed_lv 27:2db168d9fb18 64 // This function does not perform reference SPAD calibration
ahmed_lv 27:2db168d9fb18 65 // (VL53L0X_PerformRefSpadManagement()), since the API user manual says that it
ahmed_lv 27:2db168d9fb18 66 // is performed by ST on the bare modules; it seems like that should work well
ahmed_lv 27:2db168d9fb18 67 // enough unless a cover glass is added.
ahmed_lv 27:2db168d9fb18 68 // If io_2v8 (optional) is true or not given, the sensor is configured for 2V8
ahmed_lv 27:2db168d9fb18 69 // mode.
ahmed_lv 27:2db168d9fb18 70 bool VL53L0X::init(bool io_2v8)
ahmed_lv 27:2db168d9fb18 71 {
ahmed_lv 27:2db168d9fb18 72 // VL53L0X_DataInit() begin
ahmed_lv 27:2db168d9fb18 73
ahmed_lv 27:2db168d9fb18 74 // sensor uses 1V8 mode for I/O by default; switch to 2V8 mode if necessary
ahmed_lv 27:2db168d9fb18 75 if (io_2v8)
ahmed_lv 27:2db168d9fb18 76 {
ahmed_lv 27:2db168d9fb18 77 writeReg(VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV,
ahmed_lv 27:2db168d9fb18 78 readReg(VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV) | 0x01); // set bit 0
ahmed_lv 27:2db168d9fb18 79 }
ahmed_lv 27:2db168d9fb18 80
ahmed_lv 27:2db168d9fb18 81 // "Set I2C standard mode"
ahmed_lv 27:2db168d9fb18 82 writeReg(0x88, 0x00);
ahmed_lv 27:2db168d9fb18 83
ahmed_lv 27:2db168d9fb18 84 writeReg(0x80, 0x01);
ahmed_lv 27:2db168d9fb18 85 writeReg(0xFF, 0x01);
ahmed_lv 27:2db168d9fb18 86 writeReg(0x00, 0x00);
ahmed_lv 27:2db168d9fb18 87 stop_variable = readReg(0x91);
ahmed_lv 27:2db168d9fb18 88 writeReg(0x00, 0x01);
ahmed_lv 27:2db168d9fb18 89 writeReg(0xFF, 0x00);
ahmed_lv 27:2db168d9fb18 90 writeReg(0x80, 0x00);
ahmed_lv 27:2db168d9fb18 91
ahmed_lv 27:2db168d9fb18 92 // disable SIGNAL_RATE_MSRC (bit 1) and SIGNAL_RATE_PRE_RANGE (bit 4) limit checks
ahmed_lv 27:2db168d9fb18 93 writeReg(MSRC_CONFIG_CONTROL, readReg(MSRC_CONFIG_CONTROL) | 0x12);
ahmed_lv 27:2db168d9fb18 94
ahmed_lv 27:2db168d9fb18 95 // set final range signal rate limit to 0.25 MCPS (million counts per second)
ahmed_lv 27:2db168d9fb18 96 setSignalRateLimit(0.25);
ahmed_lv 27:2db168d9fb18 97
ahmed_lv 27:2db168d9fb18 98 writeReg(SYSTEM_SEQUENCE_CONFIG, 0xFF);
ahmed_lv 27:2db168d9fb18 99
ahmed_lv 27:2db168d9fb18 100 // VL53L0X_DataInit() end
ahmed_lv 27:2db168d9fb18 101
ahmed_lv 27:2db168d9fb18 102 // VL53L0X_StaticInit() begin
ahmed_lv 27:2db168d9fb18 103
ahmed_lv 27:2db168d9fb18 104 uint8_t spad_count;
ahmed_lv 27:2db168d9fb18 105 bool spad_type_is_aperture;
ahmed_lv 27:2db168d9fb18 106 if (!getSpadInfo(&spad_count, &spad_type_is_aperture)) { return false; }
ahmed_lv 27:2db168d9fb18 107
ahmed_lv 27:2db168d9fb18 108 // The SPAD map (RefGoodSpadMap) is read by VL53L0X_get_info_from_device() in
ahmed_lv 27:2db168d9fb18 109 // the API, but the same data seems to be more easily readable from
ahmed_lv 27:2db168d9fb18 110 // GLOBAL_CONFIG_SPAD_ENABLES_REF_0 through _6, so read it from there
ahmed_lv 27:2db168d9fb18 111 uint8_t ref_spad_map[6];
ahmed_lv 27:2db168d9fb18 112 readMulti(GLOBAL_CONFIG_SPAD_ENABLES_REF_0, ref_spad_map, 6);
ahmed_lv 27:2db168d9fb18 113
ahmed_lv 27:2db168d9fb18 114 // -- VL53L0X_set_reference_spads() begin (assume NVM values are valid)
ahmed_lv 27:2db168d9fb18 115
ahmed_lv 27:2db168d9fb18 116 writeReg(0xFF, 0x01);
ahmed_lv 27:2db168d9fb18 117 writeReg(DYNAMIC_SPAD_REF_EN_START_OFFSET, 0x00);
ahmed_lv 27:2db168d9fb18 118 writeReg(DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD, 0x2C);
ahmed_lv 27:2db168d9fb18 119 writeReg(0xFF, 0x00);
ahmed_lv 27:2db168d9fb18 120 writeReg(GLOBAL_CONFIG_REF_EN_START_SELECT, 0xB4);
ahmed_lv 27:2db168d9fb18 121
ahmed_lv 27:2db168d9fb18 122 uint8_t first_spad_to_enable = spad_type_is_aperture ? 12 : 0; // 12 is the first aperture spad
ahmed_lv 27:2db168d9fb18 123 uint8_t spads_enabled = 0;
ahmed_lv 27:2db168d9fb18 124
ahmed_lv 27:2db168d9fb18 125 for (uint8_t i = 0; i < 48; i++)
ahmed_lv 27:2db168d9fb18 126 {
ahmed_lv 27:2db168d9fb18 127 if (i < first_spad_to_enable || spads_enabled == spad_count)
ahmed_lv 27:2db168d9fb18 128 {
ahmed_lv 27:2db168d9fb18 129 // This bit is lower than the first one that should be enabled, or
ahmed_lv 27:2db168d9fb18 130 // (reference_spad_count) bits have already been enabled, so zero this bit
ahmed_lv 27:2db168d9fb18 131 ref_spad_map[i / 8] &= ~(1 << (i % 8));
ahmed_lv 27:2db168d9fb18 132 }
ahmed_lv 27:2db168d9fb18 133 else if ((ref_spad_map[i / 8] >> (i % 8)) & 0x1)
ahmed_lv 27:2db168d9fb18 134 {
ahmed_lv 27:2db168d9fb18 135 spads_enabled++;
ahmed_lv 27:2db168d9fb18 136 }
ahmed_lv 27:2db168d9fb18 137 }
ahmed_lv 27:2db168d9fb18 138
ahmed_lv 27:2db168d9fb18 139 writeMulti(GLOBAL_CONFIG_SPAD_ENABLES_REF_0, ref_spad_map, 6);
ahmed_lv 27:2db168d9fb18 140
ahmed_lv 27:2db168d9fb18 141 // -- VL53L0X_set_reference_spads() end
ahmed_lv 27:2db168d9fb18 142
ahmed_lv 27:2db168d9fb18 143 // -- VL53L0X_load_tuning_settings() begin
ahmed_lv 27:2db168d9fb18 144 // DefaultTuningSettings from vl53l0x_tuning.h
ahmed_lv 27:2db168d9fb18 145
ahmed_lv 27:2db168d9fb18 146 writeReg(0xFF, 0x01);
ahmed_lv 27:2db168d9fb18 147 writeReg(0x00, 0x00);
ahmed_lv 27:2db168d9fb18 148
ahmed_lv 27:2db168d9fb18 149 writeReg(0xFF, 0x00);
ahmed_lv 27:2db168d9fb18 150 writeReg(0x09, 0x00);
ahmed_lv 27:2db168d9fb18 151 writeReg(0x10, 0x00);
ahmed_lv 27:2db168d9fb18 152 writeReg(0x11, 0x00);
ahmed_lv 27:2db168d9fb18 153
ahmed_lv 27:2db168d9fb18 154 writeReg(0x24, 0x01);
ahmed_lv 27:2db168d9fb18 155 writeReg(0x25, 0xFF);
ahmed_lv 27:2db168d9fb18 156 writeReg(0x75, 0x00);
ahmed_lv 27:2db168d9fb18 157
ahmed_lv 27:2db168d9fb18 158 writeReg(0xFF, 0x01);
ahmed_lv 27:2db168d9fb18 159 writeReg(0x4E, 0x2C);
ahmed_lv 27:2db168d9fb18 160 writeReg(0x48, 0x00);
ahmed_lv 27:2db168d9fb18 161 writeReg(0x30, 0x20);
ahmed_lv 27:2db168d9fb18 162
ahmed_lv 27:2db168d9fb18 163 writeReg(0xFF, 0x00);
ahmed_lv 27:2db168d9fb18 164 writeReg(0x30, 0x09);
ahmed_lv 27:2db168d9fb18 165 writeReg(0x54, 0x00);
ahmed_lv 27:2db168d9fb18 166 writeReg(0x31, 0x04);
ahmed_lv 27:2db168d9fb18 167 writeReg(0x32, 0x03);
ahmed_lv 27:2db168d9fb18 168 writeReg(0x40, 0x83);
ahmed_lv 27:2db168d9fb18 169 writeReg(0x46, 0x25);
ahmed_lv 27:2db168d9fb18 170 writeReg(0x60, 0x00);
ahmed_lv 27:2db168d9fb18 171 writeReg(0x27, 0x00);
ahmed_lv 27:2db168d9fb18 172 writeReg(0x50, 0x06);
ahmed_lv 27:2db168d9fb18 173 writeReg(0x51, 0x00);
ahmed_lv 27:2db168d9fb18 174 writeReg(0x52, 0x96);
ahmed_lv 27:2db168d9fb18 175 writeReg(0x56, 0x08);
ahmed_lv 27:2db168d9fb18 176 writeReg(0x57, 0x30);
ahmed_lv 27:2db168d9fb18 177 writeReg(0x61, 0x00);
ahmed_lv 27:2db168d9fb18 178 writeReg(0x62, 0x00);
ahmed_lv 27:2db168d9fb18 179 writeReg(0x64, 0x00);
ahmed_lv 27:2db168d9fb18 180 writeReg(0x65, 0x00);
ahmed_lv 27:2db168d9fb18 181 writeReg(0x66, 0xA0);
ahmed_lv 27:2db168d9fb18 182
ahmed_lv 27:2db168d9fb18 183 writeReg(0xFF, 0x01);
ahmed_lv 27:2db168d9fb18 184 writeReg(0x22, 0x32);
ahmed_lv 27:2db168d9fb18 185 writeReg(0x47, 0x14);
ahmed_lv 27:2db168d9fb18 186 writeReg(0x49, 0xFF);
ahmed_lv 27:2db168d9fb18 187 writeReg(0x4A, 0x00);
ahmed_lv 27:2db168d9fb18 188
ahmed_lv 27:2db168d9fb18 189 writeReg(0xFF, 0x00);
ahmed_lv 27:2db168d9fb18 190 writeReg(0x7A, 0x0A);
ahmed_lv 27:2db168d9fb18 191 writeReg(0x7B, 0x00);
ahmed_lv 27:2db168d9fb18 192 writeReg(0x78, 0x21);
ahmed_lv 27:2db168d9fb18 193
ahmed_lv 27:2db168d9fb18 194 writeReg(0xFF, 0x01);
ahmed_lv 27:2db168d9fb18 195 writeReg(0x23, 0x34);
ahmed_lv 27:2db168d9fb18 196 writeReg(0x42, 0x00);
ahmed_lv 27:2db168d9fb18 197 writeReg(0x44, 0xFF);
ahmed_lv 27:2db168d9fb18 198 writeReg(0x45, 0x26);
ahmed_lv 27:2db168d9fb18 199 writeReg(0x46, 0x05);
ahmed_lv 27:2db168d9fb18 200 writeReg(0x40, 0x40);
ahmed_lv 27:2db168d9fb18 201 writeReg(0x0E, 0x06);
ahmed_lv 27:2db168d9fb18 202 writeReg(0x20, 0x1A);
ahmed_lv 27:2db168d9fb18 203 writeReg(0x43, 0x40);
ahmed_lv 27:2db168d9fb18 204
ahmed_lv 27:2db168d9fb18 205 writeReg(0xFF, 0x00);
ahmed_lv 27:2db168d9fb18 206 writeReg(0x34, 0x03);
ahmed_lv 27:2db168d9fb18 207 writeReg(0x35, 0x44);
ahmed_lv 27:2db168d9fb18 208
ahmed_lv 27:2db168d9fb18 209 writeReg(0xFF, 0x01);
ahmed_lv 27:2db168d9fb18 210 writeReg(0x31, 0x04);
ahmed_lv 27:2db168d9fb18 211 writeReg(0x4B, 0x09);
ahmed_lv 27:2db168d9fb18 212 writeReg(0x4C, 0x05);
ahmed_lv 27:2db168d9fb18 213 writeReg(0x4D, 0x04);
ahmed_lv 27:2db168d9fb18 214
ahmed_lv 27:2db168d9fb18 215 writeReg(0xFF, 0x00);
ahmed_lv 27:2db168d9fb18 216 writeReg(0x44, 0x00);
ahmed_lv 27:2db168d9fb18 217 writeReg(0x45, 0x20);
ahmed_lv 27:2db168d9fb18 218 writeReg(0x47, 0x08);
ahmed_lv 27:2db168d9fb18 219 writeReg(0x48, 0x28);
ahmed_lv 27:2db168d9fb18 220 writeReg(0x67, 0x00);
ahmed_lv 27:2db168d9fb18 221 writeReg(0x70, 0x04);
ahmed_lv 27:2db168d9fb18 222 writeReg(0x71, 0x01);
ahmed_lv 27:2db168d9fb18 223 writeReg(0x72, 0xFE);
ahmed_lv 27:2db168d9fb18 224 writeReg(0x76, 0x00);
ahmed_lv 27:2db168d9fb18 225 writeReg(0x77, 0x00);
ahmed_lv 27:2db168d9fb18 226
ahmed_lv 27:2db168d9fb18 227 writeReg(0xFF, 0x01);
ahmed_lv 27:2db168d9fb18 228 writeReg(0x0D, 0x01);
ahmed_lv 27:2db168d9fb18 229
ahmed_lv 27:2db168d9fb18 230 writeReg(0xFF, 0x00);
ahmed_lv 27:2db168d9fb18 231 writeReg(0x80, 0x01);
ahmed_lv 27:2db168d9fb18 232 writeReg(0x01, 0xF8);
ahmed_lv 27:2db168d9fb18 233
ahmed_lv 27:2db168d9fb18 234 writeReg(0xFF, 0x01);
ahmed_lv 27:2db168d9fb18 235 writeReg(0x8E, 0x01);
ahmed_lv 27:2db168d9fb18 236 writeReg(0x00, 0x01);
ahmed_lv 27:2db168d9fb18 237 writeReg(0xFF, 0x00);
ahmed_lv 27:2db168d9fb18 238 writeReg(0x80, 0x00);
ahmed_lv 27:2db168d9fb18 239
ahmed_lv 27:2db168d9fb18 240 // -- VL53L0X_load_tuning_settings() end
ahmed_lv 27:2db168d9fb18 241
ahmed_lv 27:2db168d9fb18 242 // "Set interrupt config to new sample ready"
ahmed_lv 27:2db168d9fb18 243 // -- VL53L0X_SetGpioConfig() begin
ahmed_lv 27:2db168d9fb18 244
ahmed_lv 27:2db168d9fb18 245 writeReg(SYSTEM_INTERRUPT_CONFIG_GPIO, 0x04);
ahmed_lv 27:2db168d9fb18 246 writeReg(GPIO_HV_MUX_ACTIVE_HIGH, readReg(GPIO_HV_MUX_ACTIVE_HIGH) & ~0x10); // active low
ahmed_lv 27:2db168d9fb18 247 writeReg(SYSTEM_INTERRUPT_CLEAR, 0x01);
ahmed_lv 27:2db168d9fb18 248
ahmed_lv 27:2db168d9fb18 249 // -- VL53L0X_SetGpioConfig() end
ahmed_lv 27:2db168d9fb18 250
ahmed_lv 27:2db168d9fb18 251 measurement_timing_budget_us = getMeasurementTimingBudget();
ahmed_lv 27:2db168d9fb18 252
ahmed_lv 27:2db168d9fb18 253 // "Disable MSRC and TCC by default"
ahmed_lv 27:2db168d9fb18 254 // MSRC = Minimum Signal Rate Check
ahmed_lv 27:2db168d9fb18 255 // TCC = Target CentreCheck
ahmed_lv 27:2db168d9fb18 256 // -- VL53L0X_SetSequenceStepEnable() begin
ahmed_lv 27:2db168d9fb18 257
ahmed_lv 27:2db168d9fb18 258 writeReg(SYSTEM_SEQUENCE_CONFIG, 0xE8);
ahmed_lv 27:2db168d9fb18 259
ahmed_lv 27:2db168d9fb18 260 // -- VL53L0X_SetSequenceStepEnable() end
ahmed_lv 27:2db168d9fb18 261
ahmed_lv 27:2db168d9fb18 262 // "Recalculate timing budget"
ahmed_lv 27:2db168d9fb18 263 setMeasurementTimingBudget(measurement_timing_budget_us);
ahmed_lv 27:2db168d9fb18 264
ahmed_lv 27:2db168d9fb18 265 // VL53L0X_StaticInit() end
ahmed_lv 27:2db168d9fb18 266
ahmed_lv 27:2db168d9fb18 267 // VL53L0X_PerformRefCalibration() begin (VL53L0X_perform_ref_calibration())
ahmed_lv 27:2db168d9fb18 268
ahmed_lv 27:2db168d9fb18 269 // -- VL53L0X_perform_vhv_calibration() begin
ahmed_lv 27:2db168d9fb18 270
ahmed_lv 27:2db168d9fb18 271 writeReg(SYSTEM_SEQUENCE_CONFIG, 0x01);
ahmed_lv 27:2db168d9fb18 272 if (!performSingleRefCalibration(0x40)) { return false; }
ahmed_lv 27:2db168d9fb18 273
ahmed_lv 27:2db168d9fb18 274 // -- VL53L0X_perform_vhv_calibration() end
ahmed_lv 27:2db168d9fb18 275
ahmed_lv 27:2db168d9fb18 276 // -- VL53L0X_perform_phase_calibration() begin
ahmed_lv 27:2db168d9fb18 277
ahmed_lv 27:2db168d9fb18 278 writeReg(SYSTEM_SEQUENCE_CONFIG, 0x02);
ahmed_lv 27:2db168d9fb18 279 if (!performSingleRefCalibration(0x00)) { return false; }
ahmed_lv 27:2db168d9fb18 280
ahmed_lv 27:2db168d9fb18 281 // -- VL53L0X_perform_phase_calibration() end
ahmed_lv 27:2db168d9fb18 282
ahmed_lv 27:2db168d9fb18 283 // "restore the previous Sequence Config"
ahmed_lv 27:2db168d9fb18 284 writeReg(SYSTEM_SEQUENCE_CONFIG, 0xE8);
ahmed_lv 27:2db168d9fb18 285
ahmed_lv 27:2db168d9fb18 286 // VL53L0X_PerformRefCalibration() end
ahmed_lv 27:2db168d9fb18 287
ahmed_lv 27:2db168d9fb18 288 return true;
ahmed_lv 27:2db168d9fb18 289 }
ahmed_lv 27:2db168d9fb18 290
ahmed_lv 27:2db168d9fb18 291 // Write an 8-bit register
ahmed_lv 27:2db168d9fb18 292 void VL53L0X::writeReg(uint8_t reg, uint8_t value)
ahmed_lv 27:2db168d9fb18 293 {
ahmed_lv 27:2db168d9fb18 294 char data[] = {
ahmed_lv 27:2db168d9fb18 295 reg,
ahmed_lv 27:2db168d9fb18 296 value
ahmed_lv 27:2db168d9fb18 297 };
ahmed_lv 27:2db168d9fb18 298 if (i2c->write(address, data, 2)) {
ahmed_lv 27:2db168d9fb18 299 last_status = ERR_OTHER;
ahmed_lv 27:2db168d9fb18 300 } else {
ahmed_lv 27:2db168d9fb18 301 last_status = ERR_OK;
ahmed_lv 27:2db168d9fb18 302 }
ahmed_lv 27:2db168d9fb18 303 }
ahmed_lv 27:2db168d9fb18 304
ahmed_lv 27:2db168d9fb18 305 // Write a 16-bit register
ahmed_lv 27:2db168d9fb18 306 void VL53L0X::writeReg16Bit(uint8_t reg, uint16_t value)
ahmed_lv 27:2db168d9fb18 307 {
ahmed_lv 27:2db168d9fb18 308 char data[] = {
ahmed_lv 27:2db168d9fb18 309 reg,
ahmed_lv 27:2db168d9fb18 310 static_cast<char>((value >> 8) & 0xFF), // value high byte
ahmed_lv 27:2db168d9fb18 311 static_cast<char>(value & 0xFF) // value low byte
ahmed_lv 27:2db168d9fb18 312 };
ahmed_lv 27:2db168d9fb18 313 if (i2c->write(address, data, 3)) {
ahmed_lv 27:2db168d9fb18 314 last_status = ERR_OTHER;
ahmed_lv 27:2db168d9fb18 315 } else {
ahmed_lv 27:2db168d9fb18 316 last_status = ERR_OK;
ahmed_lv 27:2db168d9fb18 317 }
ahmed_lv 27:2db168d9fb18 318 }
ahmed_lv 27:2db168d9fb18 319
ahmed_lv 27:2db168d9fb18 320 // Write a 32-bit register
ahmed_lv 27:2db168d9fb18 321 void VL53L0X::writeReg32Bit(uint8_t reg, uint32_t value)
ahmed_lv 27:2db168d9fb18 322 {
ahmed_lv 27:2db168d9fb18 323 char data[] = {
ahmed_lv 27:2db168d9fb18 324 reg,
ahmed_lv 27:2db168d9fb18 325 static_cast<char>((value >> 24) & 0xFF), // value highest byte
ahmed_lv 27:2db168d9fb18 326 static_cast<char>((value >> 16) & 0xFF),
ahmed_lv 27:2db168d9fb18 327 static_cast<char>((value >> 8) & 0xFF),
ahmed_lv 27:2db168d9fb18 328 static_cast<char>(value & 0xFF) // value lowest byte
ahmed_lv 27:2db168d9fb18 329 };
ahmed_lv 27:2db168d9fb18 330 if (i2c->write(address, data, 5)) {
ahmed_lv 27:2db168d9fb18 331 last_status = ERR_OTHER;
ahmed_lv 27:2db168d9fb18 332 } else {
ahmed_lv 27:2db168d9fb18 333 last_status = ERR_OK;
ahmed_lv 27:2db168d9fb18 334 }
ahmed_lv 27:2db168d9fb18 335 }
ahmed_lv 27:2db168d9fb18 336
ahmed_lv 27:2db168d9fb18 337 // Read an 8-bit register
ahmed_lv 27:2db168d9fb18 338 uint8_t VL53L0X::readReg(uint8_t reg)
ahmed_lv 27:2db168d9fb18 339 {
ahmed_lv 27:2db168d9fb18 340 uint8_t value;
ahmed_lv 27:2db168d9fb18 341
ahmed_lv 27:2db168d9fb18 342 if (i2c->write(address, reinterpret_cast<char *>(&reg), 1)) {
ahmed_lv 27:2db168d9fb18 343 last_status = ERR_NACK_ADDR;
ahmed_lv 27:2db168d9fb18 344 return 0;
ahmed_lv 27:2db168d9fb18 345 }
ahmed_lv 27:2db168d9fb18 346 if (i2c->read(address, reinterpret_cast<char *>(&value), 1)) {
ahmed_lv 27:2db168d9fb18 347 last_status = ERR_NACK_DATA;
ahmed_lv 27:2db168d9fb18 348 return 0;
ahmed_lv 27:2db168d9fb18 349 }
ahmed_lv 27:2db168d9fb18 350 last_status = ERR_OK;
ahmed_lv 27:2db168d9fb18 351
ahmed_lv 27:2db168d9fb18 352 return value;
ahmed_lv 27:2db168d9fb18 353 }
ahmed_lv 27:2db168d9fb18 354
ahmed_lv 27:2db168d9fb18 355 // Read a 16-bit register
ahmed_lv 27:2db168d9fb18 356 uint16_t VL53L0X::readReg16Bit(uint8_t reg)
ahmed_lv 27:2db168d9fb18 357 {
ahmed_lv 27:2db168d9fb18 358 uint16_t value;
ahmed_lv 27:2db168d9fb18 359 uint8_t data[2];
ahmed_lv 27:2db168d9fb18 360
ahmed_lv 27:2db168d9fb18 361 if (i2c->write(address, reinterpret_cast<char *>(&reg), 1)) {
ahmed_lv 27:2db168d9fb18 362 last_status = ERR_NACK_ADDR;
ahmed_lv 27:2db168d9fb18 363 return 0;
ahmed_lv 27:2db168d9fb18 364 }
ahmed_lv 27:2db168d9fb18 365 if (i2c->read(address, reinterpret_cast<char *>(data), 2)) {
ahmed_lv 27:2db168d9fb18 366 last_status = ERR_NACK_DATA;
ahmed_lv 27:2db168d9fb18 367 return 0;
ahmed_lv 27:2db168d9fb18 368 }
ahmed_lv 27:2db168d9fb18 369 last_status = ERR_OK;
ahmed_lv 27:2db168d9fb18 370
ahmed_lv 27:2db168d9fb18 371 value = static_cast<uint16_t>(data[0] << 8); // value high byte
ahmed_lv 27:2db168d9fb18 372 value |= data[1]; // value low byte
ahmed_lv 27:2db168d9fb18 373
ahmed_lv 27:2db168d9fb18 374 return value;
ahmed_lv 27:2db168d9fb18 375 }
ahmed_lv 27:2db168d9fb18 376
ahmed_lv 27:2db168d9fb18 377 // Read a 32-bit register
ahmed_lv 27:2db168d9fb18 378 uint32_t VL53L0X::readReg32Bit(uint8_t reg)
ahmed_lv 27:2db168d9fb18 379 {
ahmed_lv 27:2db168d9fb18 380 uint32_t value;
ahmed_lv 27:2db168d9fb18 381 uint8_t data[4];
ahmed_lv 27:2db168d9fb18 382
ahmed_lv 27:2db168d9fb18 383 if (i2c->write(address, reinterpret_cast<char *>(&reg), 1)) {
ahmed_lv 27:2db168d9fb18 384 last_status = ERR_NACK_ADDR;
ahmed_lv 27:2db168d9fb18 385 return 0;
ahmed_lv 27:2db168d9fb18 386 }
ahmed_lv 27:2db168d9fb18 387 if (i2c->read(address, reinterpret_cast<char *>(data), 4)) {
ahmed_lv 27:2db168d9fb18 388 last_status = ERR_NACK_DATA;
ahmed_lv 27:2db168d9fb18 389 return 0;
ahmed_lv 27:2db168d9fb18 390 }
ahmed_lv 27:2db168d9fb18 391 last_status = ERR_OK;
ahmed_lv 27:2db168d9fb18 392
ahmed_lv 27:2db168d9fb18 393 value = static_cast<uint32_t>(data[0] << 24); // value highest byte
ahmed_lv 27:2db168d9fb18 394 value |= static_cast<uint32_t>(data[1] << 16);
ahmed_lv 27:2db168d9fb18 395 value |= static_cast<uint32_t>(data[2] << 8);
ahmed_lv 27:2db168d9fb18 396 value |= data[3]; // value lowest byte
ahmed_lv 27:2db168d9fb18 397
ahmed_lv 27:2db168d9fb18 398 return value;
ahmed_lv 27:2db168d9fb18 399 }
ahmed_lv 27:2db168d9fb18 400
ahmed_lv 27:2db168d9fb18 401 // Write an arbitrary number of bytes from the given array to the sensor,
ahmed_lv 27:2db168d9fb18 402 // starting at the given register
ahmed_lv 27:2db168d9fb18 403 void VL53L0X::writeMulti(uint8_t reg, uint8_t const * src, uint8_t count)
ahmed_lv 27:2db168d9fb18 404 {
ahmed_lv 27:2db168d9fb18 405 if (i2c->write(address, reinterpret_cast<char *>(&reg), 1, true)) {
ahmed_lv 27:2db168d9fb18 406 last_status = ERR_NACK_ADDR;
ahmed_lv 27:2db168d9fb18 407 return;
ahmed_lv 27:2db168d9fb18 408 }
ahmed_lv 27:2db168d9fb18 409
ahmed_lv 27:2db168d9fb18 410 if (i2c->write(address, const_cast<char *>(
ahmed_lv 27:2db168d9fb18 411 reinterpret_cast<const char *>(src)), count)) {
ahmed_lv 27:2db168d9fb18 412 last_status = ERR_NACK_DATA;
ahmed_lv 27:2db168d9fb18 413 return;
ahmed_lv 27:2db168d9fb18 414 }
ahmed_lv 27:2db168d9fb18 415 last_status = ERR_OK;
ahmed_lv 27:2db168d9fb18 416 }
ahmed_lv 27:2db168d9fb18 417
ahmed_lv 27:2db168d9fb18 418 // Read an arbitrary number of bytes from the sensor, starting at the given
ahmed_lv 27:2db168d9fb18 419 // register, into the given array
ahmed_lv 27:2db168d9fb18 420 void VL53L0X::readMulti(uint8_t reg, uint8_t * dst, uint8_t count)
ahmed_lv 27:2db168d9fb18 421 {
ahmed_lv 27:2db168d9fb18 422 if (i2c->write(address, reinterpret_cast<char *>(&reg), 1)) {
ahmed_lv 27:2db168d9fb18 423 last_status = ERR_NACK_ADDR;
ahmed_lv 27:2db168d9fb18 424 return;
ahmed_lv 27:2db168d9fb18 425 }
ahmed_lv 27:2db168d9fb18 426 if (i2c->read(address, reinterpret_cast<char *>(dst), count)) {
ahmed_lv 27:2db168d9fb18 427 last_status = ERR_NACK_DATA;
ahmed_lv 27:2db168d9fb18 428 return;
ahmed_lv 27:2db168d9fb18 429 }
ahmed_lv 27:2db168d9fb18 430 last_status = ERR_OK;
ahmed_lv 27:2db168d9fb18 431 }
ahmed_lv 27:2db168d9fb18 432
ahmed_lv 27:2db168d9fb18 433 // Set the return signal rate limit check value in units of MCPS (mega counts
ahmed_lv 27:2db168d9fb18 434 // per second). "This represents the amplitude of the signal reflected from the
ahmed_lv 27:2db168d9fb18 435 // target and detected by the device"; setting this limit presumably determines
ahmed_lv 27:2db168d9fb18 436 // the minimum measurement necessary for the sensor to report a valid reading.
ahmed_lv 27:2db168d9fb18 437 // Setting a lower limit increases the potential range of the sensor but also
ahmed_lv 27:2db168d9fb18 438 // seems to increase the likelihood of getting an inaccurate reading because of
ahmed_lv 27:2db168d9fb18 439 // unwanted reflections from objects other than the intended target.
ahmed_lv 27:2db168d9fb18 440 // Defaults to 0.25 MCPS as initialized by the ST API and this library.
ahmed_lv 27:2db168d9fb18 441 bool VL53L0X::setSignalRateLimit(float limit_Mcps)
ahmed_lv 27:2db168d9fb18 442 {
ahmed_lv 27:2db168d9fb18 443 if (limit_Mcps < 0 || limit_Mcps > 511.99) { return false; }
ahmed_lv 27:2db168d9fb18 444
ahmed_lv 27:2db168d9fb18 445 // Q9.7 fixed point format (9 integer bits, 7 fractional bits)
ahmed_lv 27:2db168d9fb18 446 writeReg16Bit(FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT, limit_Mcps * (1 << 7));
ahmed_lv 27:2db168d9fb18 447 return true;
ahmed_lv 27:2db168d9fb18 448 }
ahmed_lv 27:2db168d9fb18 449
ahmed_lv 27:2db168d9fb18 450 // Get the return signal rate limit check value in MCPS
ahmed_lv 27:2db168d9fb18 451 float VL53L0X::getSignalRateLimit(void)
ahmed_lv 27:2db168d9fb18 452 {
ahmed_lv 27:2db168d9fb18 453 return (float)readReg16Bit(FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT) / (1 << 7);
ahmed_lv 27:2db168d9fb18 454 }
ahmed_lv 27:2db168d9fb18 455
ahmed_lv 27:2db168d9fb18 456 // Set the measurement timing budget in microseconds, which is the time allowed
ahmed_lv 27:2db168d9fb18 457 // for one measurement; the ST API and this library take care of splitting the
ahmed_lv 27:2db168d9fb18 458 // timing budget among the sub-steps in the ranging sequence. A longer timing
ahmed_lv 27:2db168d9fb18 459 // budget allows for more accurate measurements. Increasing the budget by a
ahmed_lv 27:2db168d9fb18 460 // factor of N decreases the range measurement standard deviation by a factor of
ahmed_lv 27:2db168d9fb18 461 // sqrt(N). Defaults to about 33 milliseconds; the minimum is 20 ms.
ahmed_lv 27:2db168d9fb18 462 // based on VL53L0X_set_measurement_timing_budget_micro_seconds()
ahmed_lv 27:2db168d9fb18 463 bool VL53L0X::setMeasurementTimingBudget(uint32_t budget_us)
ahmed_lv 27:2db168d9fb18 464 {
ahmed_lv 27:2db168d9fb18 465 SequenceStepEnables enables;
ahmed_lv 27:2db168d9fb18 466 SequenceStepTimeouts timeouts;
ahmed_lv 27:2db168d9fb18 467
ahmed_lv 27:2db168d9fb18 468 uint16_t const StartOverhead = 1320; // note that this is different than the value in get_
ahmed_lv 27:2db168d9fb18 469 uint16_t const EndOverhead = 960;
ahmed_lv 27:2db168d9fb18 470 uint16_t const MsrcOverhead = 660;
ahmed_lv 27:2db168d9fb18 471 uint16_t const TccOverhead = 590;
ahmed_lv 27:2db168d9fb18 472 uint16_t const DssOverhead = 690;
ahmed_lv 27:2db168d9fb18 473 uint16_t const PreRangeOverhead = 660;
ahmed_lv 27:2db168d9fb18 474 uint16_t const FinalRangeOverhead = 550;
ahmed_lv 27:2db168d9fb18 475
ahmed_lv 27:2db168d9fb18 476 uint32_t const MinTimingBudget = 20000;
ahmed_lv 27:2db168d9fb18 477
ahmed_lv 27:2db168d9fb18 478 if (budget_us < MinTimingBudget) { return false; }
ahmed_lv 27:2db168d9fb18 479
ahmed_lv 27:2db168d9fb18 480 uint32_t used_budget_us = StartOverhead + EndOverhead;
ahmed_lv 27:2db168d9fb18 481
ahmed_lv 27:2db168d9fb18 482 getSequenceStepEnables(&enables);
ahmed_lv 27:2db168d9fb18 483 getSequenceStepTimeouts(&enables, &timeouts);
ahmed_lv 27:2db168d9fb18 484
ahmed_lv 27:2db168d9fb18 485 if (enables.tcc)
ahmed_lv 27:2db168d9fb18 486 {
ahmed_lv 27:2db168d9fb18 487 used_budget_us += (timeouts.msrc_dss_tcc_us + TccOverhead);
ahmed_lv 27:2db168d9fb18 488 }
ahmed_lv 27:2db168d9fb18 489
ahmed_lv 27:2db168d9fb18 490 if (enables.dss)
ahmed_lv 27:2db168d9fb18 491 {
ahmed_lv 27:2db168d9fb18 492 used_budget_us += 2 * (timeouts.msrc_dss_tcc_us + DssOverhead);
ahmed_lv 27:2db168d9fb18 493 }
ahmed_lv 27:2db168d9fb18 494 else if (enables.msrc)
ahmed_lv 27:2db168d9fb18 495 {
ahmed_lv 27:2db168d9fb18 496 used_budget_us += (timeouts.msrc_dss_tcc_us + MsrcOverhead);
ahmed_lv 27:2db168d9fb18 497 }
ahmed_lv 27:2db168d9fb18 498
ahmed_lv 27:2db168d9fb18 499 if (enables.pre_range)
ahmed_lv 27:2db168d9fb18 500 {
ahmed_lv 27:2db168d9fb18 501 used_budget_us += (timeouts.pre_range_us + PreRangeOverhead);
ahmed_lv 27:2db168d9fb18 502 }
ahmed_lv 27:2db168d9fb18 503
ahmed_lv 27:2db168d9fb18 504 if (enables.final_range)
ahmed_lv 27:2db168d9fb18 505 {
ahmed_lv 27:2db168d9fb18 506 used_budget_us += FinalRangeOverhead;
ahmed_lv 27:2db168d9fb18 507
ahmed_lv 27:2db168d9fb18 508 // "Note that the final range timeout is determined by the timing
ahmed_lv 27:2db168d9fb18 509 // budget and the sum of all other timeouts within the sequence.
ahmed_lv 27:2db168d9fb18 510 // If there is no room for the final range timeout, then an error
ahmed_lv 27:2db168d9fb18 511 // will be set. Otherwise the remaining time will be applied to
ahmed_lv 27:2db168d9fb18 512 // the final range."
ahmed_lv 27:2db168d9fb18 513
ahmed_lv 27:2db168d9fb18 514 if (used_budget_us > budget_us)
ahmed_lv 27:2db168d9fb18 515 {
ahmed_lv 27:2db168d9fb18 516 // "Requested timeout too big."
ahmed_lv 27:2db168d9fb18 517 return false;
ahmed_lv 27:2db168d9fb18 518 }
ahmed_lv 27:2db168d9fb18 519
ahmed_lv 27:2db168d9fb18 520 uint32_t final_range_timeout_us = budget_us - used_budget_us;
ahmed_lv 27:2db168d9fb18 521
ahmed_lv 27:2db168d9fb18 522 // set_sequence_step_timeout() begin
ahmed_lv 27:2db168d9fb18 523 // (SequenceStepId == VL53L0X_SEQUENCESTEP_FINAL_RANGE)
ahmed_lv 27:2db168d9fb18 524
ahmed_lv 27:2db168d9fb18 525 // "For the final range timeout, the pre-range timeout
ahmed_lv 27:2db168d9fb18 526 // must be added. To do this both final and pre-range
ahmed_lv 27:2db168d9fb18 527 // timeouts must be expressed in macro periods MClks
ahmed_lv 27:2db168d9fb18 528 // because they have different vcsel periods."
ahmed_lv 27:2db168d9fb18 529
ahmed_lv 27:2db168d9fb18 530 uint16_t final_range_timeout_mclks =
ahmed_lv 27:2db168d9fb18 531 timeoutMicrosecondsToMclks(final_range_timeout_us,
ahmed_lv 27:2db168d9fb18 532 timeouts.final_range_vcsel_period_pclks);
ahmed_lv 27:2db168d9fb18 533
ahmed_lv 27:2db168d9fb18 534 if (enables.pre_range)
ahmed_lv 27:2db168d9fb18 535 {
ahmed_lv 27:2db168d9fb18 536 final_range_timeout_mclks += timeouts.pre_range_mclks;
ahmed_lv 27:2db168d9fb18 537 }
ahmed_lv 27:2db168d9fb18 538
ahmed_lv 27:2db168d9fb18 539 writeReg16Bit(FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI,
ahmed_lv 27:2db168d9fb18 540 encodeTimeout(final_range_timeout_mclks));
ahmed_lv 27:2db168d9fb18 541
ahmed_lv 27:2db168d9fb18 542 // set_sequence_step_timeout() end
ahmed_lv 27:2db168d9fb18 543
ahmed_lv 27:2db168d9fb18 544 measurement_timing_budget_us = budget_us; // store for internal reuse
ahmed_lv 27:2db168d9fb18 545 }
ahmed_lv 27:2db168d9fb18 546 return true;
ahmed_lv 27:2db168d9fb18 547 }
ahmed_lv 27:2db168d9fb18 548
ahmed_lv 27:2db168d9fb18 549 // Get the measurement timing budget in microseconds
ahmed_lv 27:2db168d9fb18 550 // based on VL53L0X_get_measurement_timing_budget_micro_seconds()
ahmed_lv 27:2db168d9fb18 551 // in us
ahmed_lv 27:2db168d9fb18 552 uint32_t VL53L0X::getMeasurementTimingBudget(void)
ahmed_lv 27:2db168d9fb18 553 {
ahmed_lv 27:2db168d9fb18 554 SequenceStepEnables enables;
ahmed_lv 27:2db168d9fb18 555 SequenceStepTimeouts timeouts;
ahmed_lv 27:2db168d9fb18 556
ahmed_lv 27:2db168d9fb18 557 uint16_t const StartOverhead = 1910; // note that this is different than the value in set_
ahmed_lv 27:2db168d9fb18 558 uint16_t const EndOverhead = 960;
ahmed_lv 27:2db168d9fb18 559 uint16_t const MsrcOverhead = 660;
ahmed_lv 27:2db168d9fb18 560 uint16_t const TccOverhead = 590;
ahmed_lv 27:2db168d9fb18 561 uint16_t const DssOverhead = 690;
ahmed_lv 27:2db168d9fb18 562 uint16_t const PreRangeOverhead = 660;
ahmed_lv 27:2db168d9fb18 563 uint16_t const FinalRangeOverhead = 550;
ahmed_lv 27:2db168d9fb18 564
ahmed_lv 27:2db168d9fb18 565 // "Start and end overhead times always present"
ahmed_lv 27:2db168d9fb18 566 uint32_t budget_us = StartOverhead + EndOverhead;
ahmed_lv 27:2db168d9fb18 567
ahmed_lv 27:2db168d9fb18 568 getSequenceStepEnables(&enables);
ahmed_lv 27:2db168d9fb18 569 getSequenceStepTimeouts(&enables, &timeouts);
ahmed_lv 27:2db168d9fb18 570
ahmed_lv 27:2db168d9fb18 571 if (enables.tcc)
ahmed_lv 27:2db168d9fb18 572 {
ahmed_lv 27:2db168d9fb18 573 budget_us += (timeouts.msrc_dss_tcc_us + TccOverhead);
ahmed_lv 27:2db168d9fb18 574 }
ahmed_lv 27:2db168d9fb18 575
ahmed_lv 27:2db168d9fb18 576 if (enables.dss)
ahmed_lv 27:2db168d9fb18 577 {
ahmed_lv 27:2db168d9fb18 578 budget_us += 2 * (timeouts.msrc_dss_tcc_us + DssOverhead);
ahmed_lv 27:2db168d9fb18 579 }
ahmed_lv 27:2db168d9fb18 580 else if (enables.msrc)
ahmed_lv 27:2db168d9fb18 581 {
ahmed_lv 27:2db168d9fb18 582 budget_us += (timeouts.msrc_dss_tcc_us + MsrcOverhead);
ahmed_lv 27:2db168d9fb18 583 }
ahmed_lv 27:2db168d9fb18 584
ahmed_lv 27:2db168d9fb18 585 if (enables.pre_range)
ahmed_lv 27:2db168d9fb18 586 {
ahmed_lv 27:2db168d9fb18 587 budget_us += (timeouts.pre_range_us + PreRangeOverhead);
ahmed_lv 27:2db168d9fb18 588 }
ahmed_lv 27:2db168d9fb18 589
ahmed_lv 27:2db168d9fb18 590 if (enables.final_range)
ahmed_lv 27:2db168d9fb18 591 {
ahmed_lv 27:2db168d9fb18 592 budget_us += (timeouts.final_range_us + FinalRangeOverhead);
ahmed_lv 27:2db168d9fb18 593 }
ahmed_lv 27:2db168d9fb18 594
ahmed_lv 27:2db168d9fb18 595 measurement_timing_budget_us = budget_us; // store for internal reuse
ahmed_lv 27:2db168d9fb18 596 return budget_us;
ahmed_lv 27:2db168d9fb18 597 }
ahmed_lv 27:2db168d9fb18 598
ahmed_lv 27:2db168d9fb18 599 // Set the VCSEL (vertical cavity surface emitting laser) pulse period for the
ahmed_lv 27:2db168d9fb18 600 // given period type (pre-range or final range) to the given value in PCLKs.
ahmed_lv 27:2db168d9fb18 601 // Longer periods seem to increase the potential range of the sensor.
ahmed_lv 27:2db168d9fb18 602 // Valid values are (even numbers only):
ahmed_lv 27:2db168d9fb18 603 // pre: 12 to 18 (initialized default: 14)
ahmed_lv 27:2db168d9fb18 604 // final: 8 to 14 (initialized default: 10)
ahmed_lv 27:2db168d9fb18 605 // based on VL53L0X_set_vcsel_pulse_period()
ahmed_lv 27:2db168d9fb18 606 bool VL53L0X::setVcselPulsePeriod(vcselPeriodType type, uint8_t period_pclks)
ahmed_lv 27:2db168d9fb18 607 {
ahmed_lv 27:2db168d9fb18 608 uint8_t vcsel_period_reg = encodeVcselPeriod(period_pclks);
ahmed_lv 27:2db168d9fb18 609
ahmed_lv 27:2db168d9fb18 610 SequenceStepEnables enables;
ahmed_lv 27:2db168d9fb18 611 SequenceStepTimeouts timeouts;
ahmed_lv 27:2db168d9fb18 612
ahmed_lv 27:2db168d9fb18 613 getSequenceStepEnables(&enables);
ahmed_lv 27:2db168d9fb18 614 getSequenceStepTimeouts(&enables, &timeouts);
ahmed_lv 27:2db168d9fb18 615
ahmed_lv 27:2db168d9fb18 616 // "Apply specific settings for the requested clock period"
ahmed_lv 27:2db168d9fb18 617 // "Re-calculate and apply timeouts, in macro periods"
ahmed_lv 27:2db168d9fb18 618
ahmed_lv 27:2db168d9fb18 619 // "When the VCSEL period for the pre or final range is changed,
ahmed_lv 27:2db168d9fb18 620 // the corresponding timeout must be read from the device using
ahmed_lv 27:2db168d9fb18 621 // the current VCSEL period, then the new VCSEL period can be
ahmed_lv 27:2db168d9fb18 622 // applied. The timeout then must be written back to the device
ahmed_lv 27:2db168d9fb18 623 // using the new VCSEL period.
ahmed_lv 27:2db168d9fb18 624 //
ahmed_lv 27:2db168d9fb18 625 // For the MSRC timeout, the same applies - this timeout being
ahmed_lv 27:2db168d9fb18 626 // dependant on the pre-range vcsel period."
ahmed_lv 27:2db168d9fb18 627
ahmed_lv 27:2db168d9fb18 628
ahmed_lv 27:2db168d9fb18 629 if (type == VcselPeriodPreRange)
ahmed_lv 27:2db168d9fb18 630 {
ahmed_lv 27:2db168d9fb18 631 // "Set phase check limits"
ahmed_lv 27:2db168d9fb18 632 switch (period_pclks)
ahmed_lv 27:2db168d9fb18 633 {
ahmed_lv 27:2db168d9fb18 634 case 12:
ahmed_lv 27:2db168d9fb18 635 writeReg(PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x18);
ahmed_lv 27:2db168d9fb18 636 break;
ahmed_lv 27:2db168d9fb18 637
ahmed_lv 27:2db168d9fb18 638 case 14:
ahmed_lv 27:2db168d9fb18 639 writeReg(PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x30);
ahmed_lv 27:2db168d9fb18 640 break;
ahmed_lv 27:2db168d9fb18 641
ahmed_lv 27:2db168d9fb18 642 case 16:
ahmed_lv 27:2db168d9fb18 643 writeReg(PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x40);
ahmed_lv 27:2db168d9fb18 644 break;
ahmed_lv 27:2db168d9fb18 645
ahmed_lv 27:2db168d9fb18 646 case 18:
ahmed_lv 27:2db168d9fb18 647 writeReg(PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x50);
ahmed_lv 27:2db168d9fb18 648 break;
ahmed_lv 27:2db168d9fb18 649
ahmed_lv 27:2db168d9fb18 650 default:
ahmed_lv 27:2db168d9fb18 651 // invalid period
ahmed_lv 27:2db168d9fb18 652 return false;
ahmed_lv 27:2db168d9fb18 653 }
ahmed_lv 27:2db168d9fb18 654 writeReg(PRE_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
ahmed_lv 27:2db168d9fb18 655
ahmed_lv 27:2db168d9fb18 656 // apply new VCSEL period
ahmed_lv 27:2db168d9fb18 657 writeReg(PRE_RANGE_CONFIG_VCSEL_PERIOD, vcsel_period_reg);
ahmed_lv 27:2db168d9fb18 658
ahmed_lv 27:2db168d9fb18 659 // update timeouts
ahmed_lv 27:2db168d9fb18 660
ahmed_lv 27:2db168d9fb18 661 // set_sequence_step_timeout() begin
ahmed_lv 27:2db168d9fb18 662 // (SequenceStepId == VL53L0X_SEQUENCESTEP_PRE_RANGE)
ahmed_lv 27:2db168d9fb18 663
ahmed_lv 27:2db168d9fb18 664 uint16_t new_pre_range_timeout_mclks =
ahmed_lv 27:2db168d9fb18 665 timeoutMicrosecondsToMclks(timeouts.pre_range_us, period_pclks);
ahmed_lv 27:2db168d9fb18 666
ahmed_lv 27:2db168d9fb18 667 writeReg16Bit(PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI,
ahmed_lv 27:2db168d9fb18 668 encodeTimeout(new_pre_range_timeout_mclks));
ahmed_lv 27:2db168d9fb18 669
ahmed_lv 27:2db168d9fb18 670 // set_sequence_step_timeout() end
ahmed_lv 27:2db168d9fb18 671
ahmed_lv 27:2db168d9fb18 672 // set_sequence_step_timeout() begin
ahmed_lv 27:2db168d9fb18 673 // (SequenceStepId == VL53L0X_SEQUENCESTEP_MSRC)
ahmed_lv 27:2db168d9fb18 674
ahmed_lv 27:2db168d9fb18 675 uint16_t new_msrc_timeout_mclks =
ahmed_lv 27:2db168d9fb18 676 timeoutMicrosecondsToMclks(timeouts.msrc_dss_tcc_us, period_pclks);
ahmed_lv 27:2db168d9fb18 677
ahmed_lv 27:2db168d9fb18 678 writeReg(MSRC_CONFIG_TIMEOUT_MACROP,
ahmed_lv 27:2db168d9fb18 679 (new_msrc_timeout_mclks > 256) ? 255 : (new_msrc_timeout_mclks - 1));
ahmed_lv 27:2db168d9fb18 680
ahmed_lv 27:2db168d9fb18 681 // set_sequence_step_timeout() end
ahmed_lv 27:2db168d9fb18 682 }
ahmed_lv 27:2db168d9fb18 683 else if (type == VcselPeriodFinalRange)
ahmed_lv 27:2db168d9fb18 684 {
ahmed_lv 27:2db168d9fb18 685 switch (period_pclks)
ahmed_lv 27:2db168d9fb18 686 {
ahmed_lv 27:2db168d9fb18 687 case 8:
ahmed_lv 27:2db168d9fb18 688 writeReg(FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x10);
ahmed_lv 27:2db168d9fb18 689 writeReg(FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
ahmed_lv 27:2db168d9fb18 690 writeReg(GLOBAL_CONFIG_VCSEL_WIDTH, 0x02);
ahmed_lv 27:2db168d9fb18 691 writeReg(ALGO_PHASECAL_CONFIG_TIMEOUT, 0x0C);
ahmed_lv 27:2db168d9fb18 692 writeReg(0xFF, 0x01);
ahmed_lv 27:2db168d9fb18 693 writeReg(ALGO_PHASECAL_LIM, 0x30);
ahmed_lv 27:2db168d9fb18 694 writeReg(0xFF, 0x00);
ahmed_lv 27:2db168d9fb18 695 break;
ahmed_lv 27:2db168d9fb18 696
ahmed_lv 27:2db168d9fb18 697 case 10:
ahmed_lv 27:2db168d9fb18 698 writeReg(FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x28);
ahmed_lv 27:2db168d9fb18 699 writeReg(FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
ahmed_lv 27:2db168d9fb18 700 writeReg(GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
ahmed_lv 27:2db168d9fb18 701 writeReg(ALGO_PHASECAL_CONFIG_TIMEOUT, 0x09);
ahmed_lv 27:2db168d9fb18 702 writeReg(0xFF, 0x01);
ahmed_lv 27:2db168d9fb18 703 writeReg(ALGO_PHASECAL_LIM, 0x20);
ahmed_lv 27:2db168d9fb18 704 writeReg(0xFF, 0x00);
ahmed_lv 27:2db168d9fb18 705 break;
ahmed_lv 27:2db168d9fb18 706
ahmed_lv 27:2db168d9fb18 707 case 12:
ahmed_lv 27:2db168d9fb18 708 writeReg(FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x38);
ahmed_lv 27:2db168d9fb18 709 writeReg(FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
ahmed_lv 27:2db168d9fb18 710 writeReg(GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
ahmed_lv 27:2db168d9fb18 711 writeReg(ALGO_PHASECAL_CONFIG_TIMEOUT, 0x08);
ahmed_lv 27:2db168d9fb18 712 writeReg(0xFF, 0x01);
ahmed_lv 27:2db168d9fb18 713 writeReg(ALGO_PHASECAL_LIM, 0x20);
ahmed_lv 27:2db168d9fb18 714 writeReg(0xFF, 0x00);
ahmed_lv 27:2db168d9fb18 715 break;
ahmed_lv 27:2db168d9fb18 716
ahmed_lv 27:2db168d9fb18 717 case 14:
ahmed_lv 27:2db168d9fb18 718 writeReg(FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x48);
ahmed_lv 27:2db168d9fb18 719 writeReg(FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
ahmed_lv 27:2db168d9fb18 720 writeReg(GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
ahmed_lv 27:2db168d9fb18 721 writeReg(ALGO_PHASECAL_CONFIG_TIMEOUT, 0x07);
ahmed_lv 27:2db168d9fb18 722 writeReg(0xFF, 0x01);
ahmed_lv 27:2db168d9fb18 723 writeReg(ALGO_PHASECAL_LIM, 0x20);
ahmed_lv 27:2db168d9fb18 724 writeReg(0xFF, 0x00);
ahmed_lv 27:2db168d9fb18 725 break;
ahmed_lv 27:2db168d9fb18 726
ahmed_lv 27:2db168d9fb18 727 default:
ahmed_lv 27:2db168d9fb18 728 // invalid period
ahmed_lv 27:2db168d9fb18 729 return false;
ahmed_lv 27:2db168d9fb18 730 }
ahmed_lv 27:2db168d9fb18 731
ahmed_lv 27:2db168d9fb18 732 // apply new VCSEL period
ahmed_lv 27:2db168d9fb18 733 writeReg(FINAL_RANGE_CONFIG_VCSEL_PERIOD, vcsel_period_reg);
ahmed_lv 27:2db168d9fb18 734
ahmed_lv 27:2db168d9fb18 735 // update timeouts
ahmed_lv 27:2db168d9fb18 736
ahmed_lv 27:2db168d9fb18 737 // set_sequence_step_timeout() begin
ahmed_lv 27:2db168d9fb18 738 // (SequenceStepId == VL53L0X_SEQUENCESTEP_FINAL_RANGE)
ahmed_lv 27:2db168d9fb18 739
ahmed_lv 27:2db168d9fb18 740 // "For the final range timeout, the pre-range timeout
ahmed_lv 27:2db168d9fb18 741 // must be added. To do this both final and pre-range
ahmed_lv 27:2db168d9fb18 742 // timeouts must be expressed in macro periods MClks
ahmed_lv 27:2db168d9fb18 743 // because they have different vcsel periods."
ahmed_lv 27:2db168d9fb18 744
ahmed_lv 27:2db168d9fb18 745 uint16_t new_final_range_timeout_mclks =
ahmed_lv 27:2db168d9fb18 746 timeoutMicrosecondsToMclks(timeouts.final_range_us, period_pclks);
ahmed_lv 27:2db168d9fb18 747
ahmed_lv 27:2db168d9fb18 748 if (enables.pre_range)
ahmed_lv 27:2db168d9fb18 749 {
ahmed_lv 27:2db168d9fb18 750 new_final_range_timeout_mclks += timeouts.pre_range_mclks;
ahmed_lv 27:2db168d9fb18 751 }
ahmed_lv 27:2db168d9fb18 752
ahmed_lv 27:2db168d9fb18 753 writeReg16Bit(FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI,
ahmed_lv 27:2db168d9fb18 754 encodeTimeout(new_final_range_timeout_mclks));
ahmed_lv 27:2db168d9fb18 755
ahmed_lv 27:2db168d9fb18 756 // set_sequence_step_timeout end
ahmed_lv 27:2db168d9fb18 757 }
ahmed_lv 27:2db168d9fb18 758 else
ahmed_lv 27:2db168d9fb18 759 {
ahmed_lv 27:2db168d9fb18 760 // invalid type
ahmed_lv 27:2db168d9fb18 761 return false;
ahmed_lv 27:2db168d9fb18 762 }
ahmed_lv 27:2db168d9fb18 763
ahmed_lv 27:2db168d9fb18 764 // "Finally, the timing budget must be re-applied"
ahmed_lv 27:2db168d9fb18 765
ahmed_lv 27:2db168d9fb18 766 setMeasurementTimingBudget(measurement_timing_budget_us);
ahmed_lv 27:2db168d9fb18 767
ahmed_lv 27:2db168d9fb18 768 // "Perform the phase calibration. This is needed after changing on vcsel period."
ahmed_lv 27:2db168d9fb18 769 // VL53L0X_perform_phase_calibration() begin
ahmed_lv 27:2db168d9fb18 770
ahmed_lv 27:2db168d9fb18 771 uint8_t sequence_config = readReg(SYSTEM_SEQUENCE_CONFIG);
ahmed_lv 27:2db168d9fb18 772 writeReg(SYSTEM_SEQUENCE_CONFIG, 0x02);
ahmed_lv 27:2db168d9fb18 773 performSingleRefCalibration(0x0);
ahmed_lv 27:2db168d9fb18 774 writeReg(SYSTEM_SEQUENCE_CONFIG, sequence_config);
ahmed_lv 27:2db168d9fb18 775
ahmed_lv 27:2db168d9fb18 776 // VL53L0X_perform_phase_calibration() end
ahmed_lv 27:2db168d9fb18 777
ahmed_lv 27:2db168d9fb18 778 return true;
ahmed_lv 27:2db168d9fb18 779 }
ahmed_lv 27:2db168d9fb18 780
ahmed_lv 27:2db168d9fb18 781 // Get the VCSEL pulse period in PCLKs for the given period type.
ahmed_lv 27:2db168d9fb18 782 // based on VL53L0X_get_vcsel_pulse_period()
ahmed_lv 27:2db168d9fb18 783 uint8_t VL53L0X::getVcselPulsePeriod(vcselPeriodType type)
ahmed_lv 27:2db168d9fb18 784 {
ahmed_lv 27:2db168d9fb18 785 if (type == VcselPeriodPreRange)
ahmed_lv 27:2db168d9fb18 786 {
ahmed_lv 27:2db168d9fb18 787 return decodeVcselPeriod(readReg(PRE_RANGE_CONFIG_VCSEL_PERIOD));
ahmed_lv 27:2db168d9fb18 788 }
ahmed_lv 27:2db168d9fb18 789 else if (type == VcselPeriodFinalRange)
ahmed_lv 27:2db168d9fb18 790 {
ahmed_lv 27:2db168d9fb18 791 return decodeVcselPeriod(readReg(FINAL_RANGE_CONFIG_VCSEL_PERIOD));
ahmed_lv 27:2db168d9fb18 792 }
ahmed_lv 27:2db168d9fb18 793 else { return 255; }
ahmed_lv 27:2db168d9fb18 794 }
ahmed_lv 27:2db168d9fb18 795
ahmed_lv 27:2db168d9fb18 796 // Start continuous ranging measurements. If period_ms (optional) is 0 or not
ahmed_lv 27:2db168d9fb18 797 // given, continuous back-to-back mode is used (the sensor takes measurements as
ahmed_lv 27:2db168d9fb18 798 // often as possible); otherwise, continuous timed mode is used, with the given
ahmed_lv 27:2db168d9fb18 799 // inter-measurement period in milliseconds determining how often the sensor
ahmed_lv 27:2db168d9fb18 800 // takes a measurement.
ahmed_lv 27:2db168d9fb18 801 // based on VL53L0X_StartMeasurement()
ahmed_lv 27:2db168d9fb18 802 void VL53L0X::startContinuous(uint32_t period_ms)
ahmed_lv 27:2db168d9fb18 803 {
ahmed_lv 27:2db168d9fb18 804 writeReg(0x80, 0x01);
ahmed_lv 27:2db168d9fb18 805 writeReg(0xFF, 0x01);
ahmed_lv 27:2db168d9fb18 806 writeReg(0x00, 0x00);
ahmed_lv 27:2db168d9fb18 807 writeReg(0x91, stop_variable);
ahmed_lv 27:2db168d9fb18 808 writeReg(0x00, 0x01);
ahmed_lv 27:2db168d9fb18 809 writeReg(0xFF, 0x00);
ahmed_lv 27:2db168d9fb18 810 writeReg(0x80, 0x00);
ahmed_lv 27:2db168d9fb18 811
ahmed_lv 27:2db168d9fb18 812 if (period_ms != 0)
ahmed_lv 27:2db168d9fb18 813 {
ahmed_lv 27:2db168d9fb18 814 // continuous timed mode
ahmed_lv 27:2db168d9fb18 815
ahmed_lv 27:2db168d9fb18 816 // VL53L0X_SetInterMeasurementPeriodMilliSeconds() begin
ahmed_lv 27:2db168d9fb18 817
ahmed_lv 27:2db168d9fb18 818 uint16_t osc_calibrate_val = readReg16Bit(OSC_CALIBRATE_VAL);
ahmed_lv 27:2db168d9fb18 819
ahmed_lv 27:2db168d9fb18 820 if (osc_calibrate_val != 0)
ahmed_lv 27:2db168d9fb18 821 {
ahmed_lv 27:2db168d9fb18 822 period_ms *= osc_calibrate_val;
ahmed_lv 27:2db168d9fb18 823 }
ahmed_lv 27:2db168d9fb18 824
ahmed_lv 27:2db168d9fb18 825 writeReg32Bit(SYSTEM_INTERMEASUREMENT_PERIOD, period_ms);
ahmed_lv 27:2db168d9fb18 826
ahmed_lv 27:2db168d9fb18 827 // VL53L0X_SetInterMeasurementPeriodMilliSeconds() end
ahmed_lv 27:2db168d9fb18 828
ahmed_lv 27:2db168d9fb18 829 writeReg(SYSRANGE_START, 0x04); // VL53L0X_REG_SYSRANGE_MODE_TIMED
ahmed_lv 27:2db168d9fb18 830 }
ahmed_lv 27:2db168d9fb18 831 else
ahmed_lv 27:2db168d9fb18 832 {
ahmed_lv 27:2db168d9fb18 833 // continuous back-to-back mode
ahmed_lv 27:2db168d9fb18 834 writeReg(SYSRANGE_START, 0x02); // VL53L0X_REG_SYSRANGE_MODE_BACKTOBACK
ahmed_lv 27:2db168d9fb18 835 }
ahmed_lv 27:2db168d9fb18 836 }
ahmed_lv 27:2db168d9fb18 837
ahmed_lv 27:2db168d9fb18 838 // Stop continuous measurements
ahmed_lv 27:2db168d9fb18 839 // based on VL53L0X_StopMeasurement()
ahmed_lv 27:2db168d9fb18 840 void VL53L0X::stopContinuous(void)
ahmed_lv 27:2db168d9fb18 841 {
ahmed_lv 27:2db168d9fb18 842 writeReg(SYSRANGE_START, 0x01); // VL53L0X_REG_SYSRANGE_MODE_SINGLESHOT
ahmed_lv 27:2db168d9fb18 843
ahmed_lv 27:2db168d9fb18 844 writeReg(0xFF, 0x01);
ahmed_lv 27:2db168d9fb18 845 writeReg(0x00, 0x00);
ahmed_lv 27:2db168d9fb18 846 writeReg(0x91, 0x00);
ahmed_lv 27:2db168d9fb18 847 writeReg(0x00, 0x01);
ahmed_lv 27:2db168d9fb18 848 writeReg(0xFF, 0x00);
ahmed_lv 27:2db168d9fb18 849 }
ahmed_lv 27:2db168d9fb18 850
ahmed_lv 27:2db168d9fb18 851 // Returns a range reading in millimeters when continuous mode is active
ahmed_lv 27:2db168d9fb18 852 // (readRangeSingleMillimeters() also calls this function after starting a
ahmed_lv 27:2db168d9fb18 853 // single-shot range measurement)
ahmed_lv 27:2db168d9fb18 854 uint16_t VL53L0X::readRangeContinuousMillimeters(void)
ahmed_lv 27:2db168d9fb18 855 {
ahmed_lv 27:2db168d9fb18 856 startTimeout();
ahmed_lv 27:2db168d9fb18 857 while ((readReg(RESULT_INTERRUPT_STATUS) & 0x07) == 0)
ahmed_lv 27:2db168d9fb18 858 {
ahmed_lv 27:2db168d9fb18 859 if (checkTimeoutExpired())
ahmed_lv 27:2db168d9fb18 860 {
ahmed_lv 27:2db168d9fb18 861 did_timeout = true;
ahmed_lv 27:2db168d9fb18 862 return 65535;
ahmed_lv 27:2db168d9fb18 863 }
ahmed_lv 27:2db168d9fb18 864 }
ahmed_lv 27:2db168d9fb18 865
ahmed_lv 27:2db168d9fb18 866 // assumptions: Linearity Corrective Gain is 1000 (default);
ahmed_lv 27:2db168d9fb18 867 // fractional ranging is not enabled
ahmed_lv 27:2db168d9fb18 868 uint16_t range = readReg16Bit(RESULT_RANGE_STATUS + 10);
ahmed_lv 27:2db168d9fb18 869
ahmed_lv 27:2db168d9fb18 870 writeReg(SYSTEM_INTERRUPT_CLEAR, 0x01);
ahmed_lv 27:2db168d9fb18 871
ahmed_lv 27:2db168d9fb18 872 return range;
ahmed_lv 27:2db168d9fb18 873 }
ahmed_lv 27:2db168d9fb18 874
ahmed_lv 27:2db168d9fb18 875 // Performs a single-shot range measurement and returns the reading in
ahmed_lv 27:2db168d9fb18 876 // millimeters
ahmed_lv 27:2db168d9fb18 877 // based on VL53L0X_PerformSingleRangingMeasurement()
ahmed_lv 27:2db168d9fb18 878 uint16_t VL53L0X::readRangeSingleMillimeters(void)
ahmed_lv 27:2db168d9fb18 879 {
ahmed_lv 27:2db168d9fb18 880 writeReg(0x80, 0x01);
ahmed_lv 27:2db168d9fb18 881 writeReg(0xFF, 0x01);
ahmed_lv 27:2db168d9fb18 882 writeReg(0x00, 0x00);
ahmed_lv 27:2db168d9fb18 883 writeReg(0x91, stop_variable);
ahmed_lv 27:2db168d9fb18 884 writeReg(0x00, 0x01);
ahmed_lv 27:2db168d9fb18 885 writeReg(0xFF, 0x00);
ahmed_lv 27:2db168d9fb18 886 writeReg(0x80, 0x00);
ahmed_lv 27:2db168d9fb18 887
ahmed_lv 27:2db168d9fb18 888 writeReg(SYSRANGE_START, 0x01);
ahmed_lv 27:2db168d9fb18 889
ahmed_lv 27:2db168d9fb18 890 // "Wait until start bit has been cleared"
ahmed_lv 27:2db168d9fb18 891 startTimeout();
ahmed_lv 27:2db168d9fb18 892 while (readReg(SYSRANGE_START) & 0x01)
ahmed_lv 27:2db168d9fb18 893 {
ahmed_lv 27:2db168d9fb18 894 if (checkTimeoutExpired())
ahmed_lv 27:2db168d9fb18 895 {
ahmed_lv 27:2db168d9fb18 896 did_timeout = true;
ahmed_lv 27:2db168d9fb18 897 return 65535;
ahmed_lv 27:2db168d9fb18 898 }
ahmed_lv 27:2db168d9fb18 899 }
ahmed_lv 27:2db168d9fb18 900
ahmed_lv 27:2db168d9fb18 901 return readRangeContinuousMillimeters();
ahmed_lv 27:2db168d9fb18 902 }
ahmed_lv 27:2db168d9fb18 903
ahmed_lv 27:2db168d9fb18 904 // Did a timeout occur in one of the read functions since the last call to
ahmed_lv 27:2db168d9fb18 905 // timeoutOccurred()?
ahmed_lv 27:2db168d9fb18 906 bool VL53L0X::timeoutOccurred()
ahmed_lv 27:2db168d9fb18 907 {
ahmed_lv 27:2db168d9fb18 908 bool tmp = did_timeout;
ahmed_lv 27:2db168d9fb18 909 did_timeout = false;
ahmed_lv 27:2db168d9fb18 910 return tmp;
ahmed_lv 27:2db168d9fb18 911 }
ahmed_lv 27:2db168d9fb18 912
ahmed_lv 27:2db168d9fb18 913 // Private Methods /////////////////////////////////////////////////////////////
ahmed_lv 27:2db168d9fb18 914
ahmed_lv 27:2db168d9fb18 915 // Get reference SPAD (single photon avalanche diode) count and type
ahmed_lv 27:2db168d9fb18 916 // based on VL53L0X_get_info_from_device(),
ahmed_lv 27:2db168d9fb18 917 // but only gets reference SPAD count and type
ahmed_lv 27:2db168d9fb18 918 bool VL53L0X::getSpadInfo(uint8_t * count, bool * type_is_aperture)
ahmed_lv 27:2db168d9fb18 919 {
ahmed_lv 27:2db168d9fb18 920 uint8_t tmp;
ahmed_lv 27:2db168d9fb18 921
ahmed_lv 27:2db168d9fb18 922 writeReg(0x80, 0x01);
ahmed_lv 27:2db168d9fb18 923 writeReg(0xFF, 0x01);
ahmed_lv 27:2db168d9fb18 924 writeReg(0x00, 0x00);
ahmed_lv 27:2db168d9fb18 925
ahmed_lv 27:2db168d9fb18 926 writeReg(0xFF, 0x06);
ahmed_lv 27:2db168d9fb18 927 writeReg(0x83, readReg(0x83) | 0x04);
ahmed_lv 27:2db168d9fb18 928 writeReg(0xFF, 0x07);
ahmed_lv 27:2db168d9fb18 929 writeReg(0x81, 0x01);
ahmed_lv 27:2db168d9fb18 930
ahmed_lv 27:2db168d9fb18 931 writeReg(0x80, 0x01);
ahmed_lv 27:2db168d9fb18 932
ahmed_lv 27:2db168d9fb18 933 writeReg(0x94, 0x6b);
ahmed_lv 27:2db168d9fb18 934 writeReg(0x83, 0x00);
ahmed_lv 27:2db168d9fb18 935 startTimeout();
ahmed_lv 27:2db168d9fb18 936 while (readReg(0x83) == 0x00)
ahmed_lv 27:2db168d9fb18 937 {
ahmed_lv 27:2db168d9fb18 938 if (checkTimeoutExpired()) { return false; }
ahmed_lv 27:2db168d9fb18 939 }
ahmed_lv 27:2db168d9fb18 940 writeReg(0x83, 0x01);
ahmed_lv 27:2db168d9fb18 941 tmp = readReg(0x92);
ahmed_lv 27:2db168d9fb18 942
ahmed_lv 27:2db168d9fb18 943 *count = tmp & 0x7f;
ahmed_lv 27:2db168d9fb18 944 *type_is_aperture = (tmp >> 7) & 0x01;
ahmed_lv 27:2db168d9fb18 945
ahmed_lv 27:2db168d9fb18 946 writeReg(0x81, 0x00);
ahmed_lv 27:2db168d9fb18 947 writeReg(0xFF, 0x06);
ahmed_lv 27:2db168d9fb18 948 writeReg(0x83, readReg( 0x83 & ~0x04));
ahmed_lv 27:2db168d9fb18 949 writeReg(0xFF, 0x01);
ahmed_lv 27:2db168d9fb18 950 writeReg(0x00, 0x01);
ahmed_lv 27:2db168d9fb18 951
ahmed_lv 27:2db168d9fb18 952 writeReg(0xFF, 0x00);
ahmed_lv 27:2db168d9fb18 953 writeReg(0x80, 0x00);
ahmed_lv 27:2db168d9fb18 954
ahmed_lv 27:2db168d9fb18 955 return true;
ahmed_lv 27:2db168d9fb18 956 }
ahmed_lv 27:2db168d9fb18 957
ahmed_lv 27:2db168d9fb18 958 // Get sequence step enables
ahmed_lv 27:2db168d9fb18 959 // based on VL53L0X_GetSequenceStepEnables()
ahmed_lv 27:2db168d9fb18 960 void VL53L0X::getSequenceStepEnables(SequenceStepEnables * enables)
ahmed_lv 27:2db168d9fb18 961 {
ahmed_lv 27:2db168d9fb18 962 uint8_t sequence_config = readReg(SYSTEM_SEQUENCE_CONFIG);
ahmed_lv 27:2db168d9fb18 963
ahmed_lv 27:2db168d9fb18 964 enables->tcc = (sequence_config >> 4) & 0x1;
ahmed_lv 27:2db168d9fb18 965 enables->dss = (sequence_config >> 3) & 0x1;
ahmed_lv 27:2db168d9fb18 966 enables->msrc = (sequence_config >> 2) & 0x1;
ahmed_lv 27:2db168d9fb18 967 enables->pre_range = (sequence_config >> 6) & 0x1;
ahmed_lv 27:2db168d9fb18 968 enables->final_range = (sequence_config >> 7) & 0x1;
ahmed_lv 27:2db168d9fb18 969 }
ahmed_lv 27:2db168d9fb18 970
ahmed_lv 27:2db168d9fb18 971 // Get sequence step timeouts
ahmed_lv 27:2db168d9fb18 972 // based on get_sequence_step_timeout(),
ahmed_lv 27:2db168d9fb18 973 // but gets all timeouts instead of just the requested one, and also stores
ahmed_lv 27:2db168d9fb18 974 // intermediate values
ahmed_lv 27:2db168d9fb18 975 void VL53L0X::getSequenceStepTimeouts(SequenceStepEnables const * enables, SequenceStepTimeouts * timeouts)
ahmed_lv 27:2db168d9fb18 976 {
ahmed_lv 27:2db168d9fb18 977 timeouts->pre_range_vcsel_period_pclks = getVcselPulsePeriod(VcselPeriodPreRange);
ahmed_lv 27:2db168d9fb18 978
ahmed_lv 27:2db168d9fb18 979 timeouts->msrc_dss_tcc_mclks = readReg(MSRC_CONFIG_TIMEOUT_MACROP) + 1;
ahmed_lv 27:2db168d9fb18 980 timeouts->msrc_dss_tcc_us =
ahmed_lv 27:2db168d9fb18 981 timeoutMclksToMicroseconds(timeouts->msrc_dss_tcc_mclks,
ahmed_lv 27:2db168d9fb18 982 timeouts->pre_range_vcsel_period_pclks);
ahmed_lv 27:2db168d9fb18 983
ahmed_lv 27:2db168d9fb18 984 timeouts->pre_range_mclks =
ahmed_lv 27:2db168d9fb18 985 decodeTimeout(readReg16Bit(PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI));
ahmed_lv 27:2db168d9fb18 986 timeouts->pre_range_us =
ahmed_lv 27:2db168d9fb18 987 timeoutMclksToMicroseconds(timeouts->pre_range_mclks,
ahmed_lv 27:2db168d9fb18 988 timeouts->pre_range_vcsel_period_pclks);
ahmed_lv 27:2db168d9fb18 989
ahmed_lv 27:2db168d9fb18 990 timeouts->final_range_vcsel_period_pclks = getVcselPulsePeriod(VcselPeriodFinalRange);
ahmed_lv 27:2db168d9fb18 991
ahmed_lv 27:2db168d9fb18 992 timeouts->final_range_mclks =
ahmed_lv 27:2db168d9fb18 993 decodeTimeout(readReg16Bit(FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI));
ahmed_lv 27:2db168d9fb18 994
ahmed_lv 27:2db168d9fb18 995 if (enables->pre_range)
ahmed_lv 27:2db168d9fb18 996 {
ahmed_lv 27:2db168d9fb18 997 timeouts->final_range_mclks -= timeouts->pre_range_mclks;
ahmed_lv 27:2db168d9fb18 998 }
ahmed_lv 27:2db168d9fb18 999
ahmed_lv 27:2db168d9fb18 1000 timeouts->final_range_us =
ahmed_lv 27:2db168d9fb18 1001 timeoutMclksToMicroseconds(timeouts->final_range_mclks,
ahmed_lv 27:2db168d9fb18 1002 timeouts->final_range_vcsel_period_pclks);
ahmed_lv 27:2db168d9fb18 1003 }
ahmed_lv 27:2db168d9fb18 1004
ahmed_lv 27:2db168d9fb18 1005 // Decode sequence step timeout in MCLKs from register value
ahmed_lv 27:2db168d9fb18 1006 // based on VL53L0X_decode_timeout()
ahmed_lv 27:2db168d9fb18 1007 // Note: the original function returned a uint32_t, but the return value is
ahmed_lv 27:2db168d9fb18 1008 // always stored in a uint16_t.
ahmed_lv 27:2db168d9fb18 1009 uint16_t VL53L0X::decodeTimeout(uint16_t reg_val)
ahmed_lv 27:2db168d9fb18 1010 {
ahmed_lv 27:2db168d9fb18 1011 // format: "(LSByte * 2^MSByte) + 1"
ahmed_lv 27:2db168d9fb18 1012 return (uint16_t)((reg_val & 0x00FF) <<
ahmed_lv 27:2db168d9fb18 1013 (uint16_t)((reg_val & 0xFF00) >> 8)) + 1;
ahmed_lv 27:2db168d9fb18 1014 }
ahmed_lv 27:2db168d9fb18 1015
ahmed_lv 27:2db168d9fb18 1016 // Encode sequence step timeout register value from timeout in MCLKs
ahmed_lv 27:2db168d9fb18 1017 // based on VL53L0X_encode_timeout()
ahmed_lv 27:2db168d9fb18 1018 // Note: the original function took a uint16_t, but the argument passed to it
ahmed_lv 27:2db168d9fb18 1019 // is always a uint16_t.
ahmed_lv 27:2db168d9fb18 1020 uint16_t VL53L0X::encodeTimeout(uint16_t timeout_mclks)
ahmed_lv 27:2db168d9fb18 1021 {
ahmed_lv 27:2db168d9fb18 1022 // format: "(LSByte * 2^MSByte) + 1"
ahmed_lv 27:2db168d9fb18 1023
ahmed_lv 27:2db168d9fb18 1024 uint32_t ls_byte = 0;
ahmed_lv 27:2db168d9fb18 1025 uint16_t ms_byte = 0;
ahmed_lv 27:2db168d9fb18 1026
ahmed_lv 27:2db168d9fb18 1027 if (timeout_mclks > 0)
ahmed_lv 27:2db168d9fb18 1028 {
ahmed_lv 27:2db168d9fb18 1029 ls_byte = timeout_mclks - 1;
ahmed_lv 27:2db168d9fb18 1030
ahmed_lv 27:2db168d9fb18 1031 while ((ls_byte & 0xFFFFFF00) > 0)
ahmed_lv 27:2db168d9fb18 1032 {
ahmed_lv 27:2db168d9fb18 1033 ls_byte >>= 1;
ahmed_lv 27:2db168d9fb18 1034 ms_byte++;
ahmed_lv 27:2db168d9fb18 1035 }
ahmed_lv 27:2db168d9fb18 1036
ahmed_lv 27:2db168d9fb18 1037 return (ms_byte << 8) | (ls_byte & 0xFF);
ahmed_lv 27:2db168d9fb18 1038 }
ahmed_lv 27:2db168d9fb18 1039 else { return 0; }
ahmed_lv 27:2db168d9fb18 1040 }
ahmed_lv 27:2db168d9fb18 1041
ahmed_lv 27:2db168d9fb18 1042 // Convert sequence step timeout from MCLKs to microseconds with given VCSEL period in PCLKs
ahmed_lv 27:2db168d9fb18 1043 // based on VL53L0X_calc_timeout_us()
ahmed_lv 27:2db168d9fb18 1044 uint32_t VL53L0X::timeoutMclksToMicroseconds(uint16_t timeout_period_mclks, uint8_t vcsel_period_pclks)
ahmed_lv 27:2db168d9fb18 1045 {
ahmed_lv 27:2db168d9fb18 1046 uint32_t macro_period_ns = calcMacroPeriod(vcsel_period_pclks);
ahmed_lv 27:2db168d9fb18 1047
ahmed_lv 27:2db168d9fb18 1048 return ((timeout_period_mclks * macro_period_ns) + (macro_period_ns / 2)) / 1000;
ahmed_lv 27:2db168d9fb18 1049 }
ahmed_lv 27:2db168d9fb18 1050
ahmed_lv 27:2db168d9fb18 1051 // Convert sequence step timeout from microseconds to MCLKs with given VCSEL period in PCLKs
ahmed_lv 27:2db168d9fb18 1052 // based on VL53L0X_calc_timeout_mclks()
ahmed_lv 27:2db168d9fb18 1053 uint32_t VL53L0X::timeoutMicrosecondsToMclks(uint32_t timeout_period_us, uint8_t vcsel_period_pclks)
ahmed_lv 27:2db168d9fb18 1054 {
ahmed_lv 27:2db168d9fb18 1055 uint32_t macro_period_ns = calcMacroPeriod(vcsel_period_pclks);
ahmed_lv 27:2db168d9fb18 1056
ahmed_lv 27:2db168d9fb18 1057 return (((timeout_period_us * 1000) + (macro_period_ns / 2)) / macro_period_ns);
ahmed_lv 27:2db168d9fb18 1058 }
ahmed_lv 27:2db168d9fb18 1059
ahmed_lv 27:2db168d9fb18 1060
ahmed_lv 27:2db168d9fb18 1061 // based on VL53L0X_perform_single_ref_calibration()
ahmed_lv 27:2db168d9fb18 1062 bool VL53L0X::performSingleRefCalibration(uint8_t vhv_init_byte)
ahmed_lv 27:2db168d9fb18 1063 {
ahmed_lv 27:2db168d9fb18 1064 writeReg(SYSRANGE_START, 0x01 | vhv_init_byte); // VL53L0X_REG_SYSRANGE_MODE_START_STOP
ahmed_lv 27:2db168d9fb18 1065
ahmed_lv 27:2db168d9fb18 1066 startTimeout();
ahmed_lv 27:2db168d9fb18 1067 while ((readReg(RESULT_INTERRUPT_STATUS) & 0x07) == 0)
ahmed_lv 27:2db168d9fb18 1068 {
ahmed_lv 27:2db168d9fb18 1069 if (checkTimeoutExpired()) { return false; }
ahmed_lv 27:2db168d9fb18 1070 }
ahmed_lv 27:2db168d9fb18 1071
ahmed_lv 27:2db168d9fb18 1072 writeReg(SYSTEM_INTERRUPT_CLEAR, 0x01);
ahmed_lv 27:2db168d9fb18 1073
ahmed_lv 27:2db168d9fb18 1074 writeReg(SYSRANGE_START, 0x00);
ahmed_lv 27:2db168d9fb18 1075
ahmed_lv 27:2db168d9fb18 1076 return true;
ahmed_lv 27:2db168d9fb18 1077 }
ahmed_lv 27:2db168d9fb18 1078