Peng Jing Xuan / Mbed OS mbed-os-example-wifi-DISCO_IOTBOARD_MQTT_copy

0527

Dependencies:   MQTT

VL53L1X.cpp@60:c0c04325453c, 2019-05-27 (annotated)

Committer:
peng103617
Date:
Mon May 27 05:38:15 2019 +0000
Revision:
60:c0c04325453c
20190527

Who changed what in which revision?

UserRevisionLine numberNew contents of line
peng103617 60:c0c04325453c 1 // Most of the functionality of this library is based on the VL53L1X API
peng103617 60:c0c04325453c 2 // provided by ST (STSW-IMG007), and some of the explanatory comments are quoted
peng103617 60:c0c04325453c 3 // or paraphrased from the API source code, API user manual (UM2356), and
peng103617 60:c0c04325453c 4 // VL53L1X datasheet.
peng103617 60:c0c04325453c 5
peng103617 60:c0c04325453c 6 #include "VL53L1X.h"
peng103617 60:c0c04325453c 7 #include "mbed.h"
peng103617 60:c0c04325453c 8
peng103617 60:c0c04325453c 9 // Constructors ////////////////////////////////////////////////////////////////
peng103617 60:c0c04325453c 10 VL53L1X::VL53L1X(PinName SDA, PinName SCL, PinName shutDown) :
peng103617 60:c0c04325453c 11 _i2c(SDA,SCL), _shutDown(shutDown)
peng103617 60:c0c04325453c 12 , io_timeout(0) // no timeout
peng103617 60:c0c04325453c 13 , did_timeout(false)
peng103617 60:c0c04325453c 14 , calibrated(false)
peng103617 60:c0c04325453c 15 , saved_vhv_init(0)
peng103617 60:c0c04325453c 16 , saved_vhv_timeout(0)
peng103617 60:c0c04325453c 17 , distance_mode(Unknown){
peng103617 60:c0c04325453c 18 //Set I2C fast and bring reset line high
peng103617 60:c0c04325453c 19 _i2c.frequency(400000);
peng103617 60:c0c04325453c 20 address = AddressDefault << 1;
peng103617 60:c0c04325453c 21 turnOff();
peng103617 60:c0c04325453c 22 }
peng103617 60:c0c04325453c 23
peng103617 60:c0c04325453c 24 /*VL53L1X::VL53L1X()
peng103617 60:c0c04325453c 25 : address(AddressDefault)
peng103617 60:c0c04325453c 26 {
peng103617 60:c0c04325453c 27 }*/
peng103617 60:c0c04325453c 28
peng103617 60:c0c04325453c 29 // Public Methods //////////////////////////////////////////////////////////////
peng103617 60:c0c04325453c 30
peng103617 60:c0c04325453c 31 void VL53L1X::setAddress(uint8_t new_addr)
peng103617 60:c0c04325453c 32 {
peng103617 60:c0c04325453c 33 writeReg(I2C_SLAVE__DEVICE_ADDRESS, new_addr & 0x7F);
peng103617 60:c0c04325453c 34 wait(.01);
peng103617 60:c0c04325453c 35 printf("%x\r\n", readReg(I2C_SLAVE__DEVICE_ADDRESS));
peng103617 60:c0c04325453c 36 address = new_addr << 1;
peng103617 60:c0c04325453c 37 }
peng103617 60:c0c04325453c 38
peng103617 60:c0c04325453c 39 // Initialize sensor using settings taken mostly from VL53L1_DataInit() and
peng103617 60:c0c04325453c 40 // VL53L1_StaticInit().
peng103617 60:c0c04325453c 41 // If io_2v8 (optional) is true or not given, the sensor is configured for 2V8
peng103617 60:c0c04325453c 42 // mode.
peng103617 60:c0c04325453c 43 bool VL53L1X::init(bool io_2v8)
peng103617 60:c0c04325453c 44 {
peng103617 60:c0c04325453c 45 // check model ID and module type registers (values specified in datasheet)
peng103617 60:c0c04325453c 46 int tempRegister = readReg16Bit(IDENTIFICATION__MODEL_ID);
peng103617 60:c0c04325453c 47 printf("temporary %x\r\n", tempRegister);
peng103617 60:c0c04325453c 48 if (tempRegister != 0xEACC) {
peng103617 60:c0c04325453c 49 return false;
peng103617 60:c0c04325453c 50 }
peng103617 60:c0c04325453c 51
peng103617 60:c0c04325453c 52 // VL53L1_software_reset() begin
peng103617 60:c0c04325453c 53
peng103617 60:c0c04325453c 54 writeReg(SOFT_RESET, 0x00);
peng103617 60:c0c04325453c 55 wait(.001);
peng103617 60:c0c04325453c 56 writeReg(SOFT_RESET, 0x01);
peng103617 60:c0c04325453c 57
peng103617 60:c0c04325453c 58 // VL53L1_poll_for_boot_completion() begin
peng103617 60:c0c04325453c 59
peng103617 60:c0c04325453c 60 startTimeout();
peng103617 60:c0c04325453c 61 int firmware = (readReg16Bit(FIRMWARE__SYSTEM_STATUS));
peng103617 60:c0c04325453c 62 printf("firmware : %x\r\n", firmware);
peng103617 60:c0c04325453c 63 while ((readReg(FIRMWARE__SYSTEM_STATUS) & 0x01) == 0)
peng103617 60:c0c04325453c 64 {
peng103617 60:c0c04325453c 65 printf("stuck\r\n");
peng103617 60:c0c04325453c 66 if (checkTimeoutExpired())
peng103617 60:c0c04325453c 67 {
peng103617 60:c0c04325453c 68 did_timeout = true;
peng103617 60:c0c04325453c 69 return false;
peng103617 60:c0c04325453c 70 }
peng103617 60:c0c04325453c 71 }
peng103617 60:c0c04325453c 72 // VL53L1_poll_for_boot_completion() end
peng103617 60:c0c04325453c 73
peng103617 60:c0c04325453c 74 // VL53L1_software_reset() end
peng103617 60:c0c04325453c 75
peng103617 60:c0c04325453c 76 // VL53L1_DataInit() begin
peng103617 60:c0c04325453c 77
peng103617 60:c0c04325453c 78 // sensor uses 1V8 mode for I/O by default; switch to 2V8 mode if necessary
peng103617 60:c0c04325453c 79 if (io_2v8)
peng103617 60:c0c04325453c 80 {
peng103617 60:c0c04325453c 81 writeReg(PAD_I2C_HV__EXTSUP_CONFIG,
peng103617 60:c0c04325453c 82 readReg(PAD_I2C_HV__EXTSUP_CONFIG) | 0x01);
peng103617 60:c0c04325453c 83 }
peng103617 60:c0c04325453c 84
peng103617 60:c0c04325453c 85 // store oscillator info for later use
peng103617 60:c0c04325453c 86 fast_osc_frequency = readReg16Bit(OSC_MEASURED__FAST_OSC__FREQUENCY);
peng103617 60:c0c04325453c 87 osc_calibrate_val = readReg16Bit(RESULT__OSC_CALIBRATE_VAL);
peng103617 60:c0c04325453c 88
peng103617 60:c0c04325453c 89 // VL53L1_DataInit() end
peng103617 60:c0c04325453c 90
peng103617 60:c0c04325453c 91 // VL53L1_StaticInit() begin
peng103617 60:c0c04325453c 92
peng103617 60:c0c04325453c 93 // Note that the API does not actually apply the configuration settings below
peng103617 60:c0c04325453c 94 // when VL53L1_StaticInit() is called: it keeps a copy of the sensor's
peng103617 60:c0c04325453c 95 // register contents in memory and doesn't actually write them until a
peng103617 60:c0c04325453c 96 // measurement is started. Writing the configuration here means we don't have
peng103617 60:c0c04325453c 97 // to keep it all in memory and avoids a lot of redundant writes later.
peng103617 60:c0c04325453c 98
peng103617 60:c0c04325453c 99 // the API sets the preset mode to LOWPOWER_AUTONOMOUS here:
peng103617 60:c0c04325453c 100 // VL53L1_set_preset_mode() begin
peng103617 60:c0c04325453c 101
peng103617 60:c0c04325453c 102 // VL53L1_preset_mode_standard_ranging() begin
peng103617 60:c0c04325453c 103
peng103617 60:c0c04325453c 104 // values labeled "tuning parm default" are from vl53l1_tuning_parm_defaults.h
peng103617 60:c0c04325453c 105 // (API uses these in VL53L1_init_tuning_parm_storage_struct())
peng103617 60:c0c04325453c 106
peng103617 60:c0c04325453c 107 // static config
peng103617 60:c0c04325453c 108 // API resets PAD_I2C_HV__EXTSUP_CONFIG here, but maybe we don't want to do
peng103617 60:c0c04325453c 109 // that? (seems like it would disable 2V8 mode)
peng103617 60:c0c04325453c 110 writeReg16Bit(DSS_CONFIG__TARGET_TOTAL_RATE_MCPS, TargetRate); // should already be this value after reset
peng103617 60:c0c04325453c 111 writeReg(GPIO__TIO_HV_STATUS, 0x02);
peng103617 60:c0c04325453c 112 writeReg(SIGMA_ESTIMATOR__EFFECTIVE_PULSE_WIDTH_NS, 8); // tuning parm default
peng103617 60:c0c04325453c 113 writeReg(SIGMA_ESTIMATOR__EFFECTIVE_AMBIENT_WIDTH_NS, 16); // tuning parm default
peng103617 60:c0c04325453c 114 writeReg(ALGO__CROSSTALK_COMPENSATION_VALID_HEIGHT_MM, 0x01);
peng103617 60:c0c04325453c 115 writeReg(ALGO__RANGE_IGNORE_VALID_HEIGHT_MM, 0xFF);
peng103617 60:c0c04325453c 116 writeReg(ALGO__RANGE_MIN_CLIP, 0); // tuning parm default
peng103617 60:c0c04325453c 117 writeReg(ALGO__CONSISTENCY_CHECK__TOLERANCE, 2); // tuning parm default
peng103617 60:c0c04325453c 118
peng103617 60:c0c04325453c 119 // general config
peng103617 60:c0c04325453c 120 writeReg16Bit(SYSTEM__THRESH_RATE_HIGH, 0x0000);
peng103617 60:c0c04325453c 121 writeReg16Bit(SYSTEM__THRESH_RATE_LOW, 0x0000);
peng103617 60:c0c04325453c 122 writeReg(DSS_CONFIG__APERTURE_ATTENUATION, 0x38);
peng103617 60:c0c04325453c 123
peng103617 60:c0c04325453c 124 // timing config
peng103617 60:c0c04325453c 125 // most of these settings will be determined later by distance and timing
peng103617 60:c0c04325453c 126 // budget configuration
peng103617 60:c0c04325453c 127 writeReg16Bit(RANGE_CONFIG__SIGMA_THRESH, 360); // tuning parm default
peng103617 60:c0c04325453c 128 writeReg16Bit(RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS, 192); // tuning parm default
peng103617 60:c0c04325453c 129
peng103617 60:c0c04325453c 130 // dynamic config
peng103617 60:c0c04325453c 131
peng103617 60:c0c04325453c 132 writeReg(SYSTEM__GROUPED_PARAMETER_HOLD_0, 0x01);
peng103617 60:c0c04325453c 133 writeReg(SYSTEM__GROUPED_PARAMETER_HOLD_1, 0x01);
peng103617 60:c0c04325453c 134 writeReg(SD_CONFIG__QUANTIFIER, 2); // tuning parm default
peng103617 60:c0c04325453c 135
peng103617 60:c0c04325453c 136 // VL53L1_preset_mode_standard_ranging() end
peng103617 60:c0c04325453c 137
peng103617 60:c0c04325453c 138 // from VL53L1_preset_mode_timed_ranging_*
peng103617 60:c0c04325453c 139 // GPH is 0 after reset, but writing GPH0 and GPH1 above seem to set GPH to 1,
peng103617 60:c0c04325453c 140 // and things don't seem to work if we don't set GPH back to 0 (which the API
peng103617 60:c0c04325453c 141 // does here).
peng103617 60:c0c04325453c 142 writeReg(SYSTEM__GROUPED_PARAMETER_HOLD, 0x00);
peng103617 60:c0c04325453c 143 writeReg(SYSTEM__SEED_CONFIG, 1); // tuning parm default
peng103617 60:c0c04325453c 144
peng103617 60:c0c04325453c 145 // from VL53L1_config_low_power_auto_mode
peng103617 60:c0c04325453c 146 writeReg(SYSTEM__SEQUENCE_CONFIG, 0x8B); // VHV, PHASECAL, DSS1, RANGE
peng103617 60:c0c04325453c 147 writeReg16Bit(DSS_CONFIG__MANUAL_EFFECTIVE_SPADS_SELECT, 200 << 8);
peng103617 60:c0c04325453c 148 writeReg(DSS_CONFIG__ROI_MODE_CONTROL, 2); // REQUESTED_EFFFECTIVE_SPADS
peng103617 60:c0c04325453c 149
peng103617 60:c0c04325453c 150 // VL53L1_set_preset_mode() end
peng103617 60:c0c04325453c 151
peng103617 60:c0c04325453c 152 // default to long range, 50 ms timing budget
peng103617 60:c0c04325453c 153 // note that this is different than what the API defaults to
peng103617 60:c0c04325453c 154 setDistanceMode(Short);
peng103617 60:c0c04325453c 155 setMeasurementTimingBudget(50000);
peng103617 60:c0c04325453c 156
peng103617 60:c0c04325453c 157 // VL53L1_StaticInit() end
peng103617 60:c0c04325453c 158
peng103617 60:c0c04325453c 159 // the API triggers this change in VL53L1_init_and_start_range() once a
peng103617 60:c0c04325453c 160 // measurement is started; assumes MM1 and MM2 are disabled
peng103617 60:c0c04325453c 161 writeReg16Bit(ALGO__PART_TO_PART_RANGE_OFFSET_MM,
peng103617 60:c0c04325453c 162 readReg16Bit(MM_CONFIG__OUTER_OFFSET_MM) * 4);
peng103617 60:c0c04325453c 163 t.start();
peng103617 60:c0c04325453c 164 return true;
peng103617 60:c0c04325453c 165 }
peng103617 60:c0c04325453c 166
peng103617 60:c0c04325453c 167 // Write an 8-bit register
peng103617 60:c0c04325453c 168 void VL53L1X::writeReg(uint16_t registerAddr, uint8_t data)
peng103617 60:c0c04325453c 169 {
peng103617 60:c0c04325453c 170 char data_write[3];
peng103617 60:c0c04325453c 171 data_write[0] = (registerAddr >> 8) & 0xFF; //MSB of register address
peng103617 60:c0c04325453c 172 data_write[1] = registerAddr & 0xFF; //LSB of register address
peng103617 60:c0c04325453c 173 data_write[2] = data & 0xFF;
peng103617 60:c0c04325453c 174 _i2c.write(address, data_write, 3);
peng103617 60:c0c04325453c 175 }
peng103617 60:c0c04325453c 176
peng103617 60:c0c04325453c 177 void VL53L1X::writeReg16Bit(uint16_t registerAddr, uint16_t data)
peng103617 60:c0c04325453c 178 {
peng103617 60:c0c04325453c 179 char data_write[4];
peng103617 60:c0c04325453c 180 data_write[0] = (registerAddr >> 8) & 0xFF; //MSB of register address
peng103617 60:c0c04325453c 181 data_write[1] = registerAddr & 0xFF; //LSB of register address
peng103617 60:c0c04325453c 182 data_write[2] = (data >> 8) & 0xFF;
peng103617 60:c0c04325453c 183 data_write[3] = data & 0xFF;
peng103617 60:c0c04325453c 184 _i2c.write(address, data_write, 4);
peng103617 60:c0c04325453c 185 }
peng103617 60:c0c04325453c 186
peng103617 60:c0c04325453c 187
peng103617 60:c0c04325453c 188 // Write a 32-bit register
peng103617 60:c0c04325453c 189 /*
peng103617 60:c0c04325453c 190 void VL53L1X::writeReg32Bit(uint16_t registerAddr, uint32_t data)
peng103617 60:c0c04325453c 191 {
peng103617 60:c0c04325453c 192 char data_write[5];
peng103617 60:c0c04325453c 193 data_write[0] = (registerAddr >> 8) & 0xFF; //MSB of register address
peng103617 60:c0c04325453c 194 data_write[1] = registerAddr & 0xFF; //LSB of register address
peng103617 60:c0c04325453c 195 data_write[2] = (data >> 16) & 0xFF;
peng103617 60:c0c04325453c 196 data_write[3] = (data >> 8) & 0xFF;
peng103617 60:c0c04325453c 197 data_write[4] = data & 0xFF;
peng103617 60:c0c04325453c 198 _i2c.write(address, data_write, 5);
peng103617 60:c0c04325453c 199 }
peng103617 60:c0c04325453c 200 */
peng103617 60:c0c04325453c 201 void VL53L1X::writeReg32Bit(uint16_t registerAddr, uint32_t data)
peng103617 60:c0c04325453c 202 {
peng103617 60:c0c04325453c 203 char data_write[6];
peng103617 60:c0c04325453c 204 data_write[0] = (registerAddr >> 8) & 0xFF; //MSB of register address
peng103617 60:c0c04325453c 205 data_write[1] = registerAddr & 0xFF; //LSB of register address
peng103617 60:c0c04325453c 206 data_write[2] = (data >> 24) & 0xFF;
peng103617 60:c0c04325453c 207 data_write[3] = (data >> 16) & 0xFF;
peng103617 60:c0c04325453c 208 data_write[4] = (data >> 8) & 0xFF;;
peng103617 60:c0c04325453c 209 data_write[5] = data & 0xFF;
peng103617 60:c0c04325453c 210 _i2c.write(address, data_write, 6);
peng103617 60:c0c04325453c 211 }
peng103617 60:c0c04325453c 212
peng103617 60:c0c04325453c 213
peng103617 60:c0c04325453c 214 // Read an 8-bit register
peng103617 60:c0c04325453c 215 uint8_t VL53L1X::readReg(uint16_t registerAddr)
peng103617 60:c0c04325453c 216 {
peng103617 60:c0c04325453c 217 uint8_t data;
peng103617 60:c0c04325453c 218 char data_write[2];
peng103617 60:c0c04325453c 219 char data_read[1];
peng103617 60:c0c04325453c 220 data_write[0] = (registerAddr >> 8) & 0xFF; //MSB of register address
peng103617 60:c0c04325453c 221 data_write[1] = registerAddr & 0xFF; //LSB of register address
peng103617 60:c0c04325453c 222 _i2c.write(address, data_write, 2,0);
peng103617 60:c0c04325453c 223 _i2c.read(address,data_read,1,1);
peng103617 60:c0c04325453c 224 //Read Data from selected register
peng103617 60:c0c04325453c 225 data=data_read[0];
peng103617 60:c0c04325453c 226 return data;
peng103617 60:c0c04325453c 227 }
peng103617 60:c0c04325453c 228
peng103617 60:c0c04325453c 229 uint16_t VL53L1X::readReg16Bit(uint16_t registerAddr)
peng103617 60:c0c04325453c 230 {
peng103617 60:c0c04325453c 231 uint8_t data_low;
peng103617 60:c0c04325453c 232 uint8_t data_high;
peng103617 60:c0c04325453c 233 uint16_t data;
peng103617 60:c0c04325453c 234
peng103617 60:c0c04325453c 235 char data_write[2];
peng103617 60:c0c04325453c 236 char data_read[2];
peng103617 60:c0c04325453c 237 data_write[0] = (registerAddr >> 8) & 0xFF; //MSB of register address
peng103617 60:c0c04325453c 238 data_write[1] = registerAddr & 0xFF; //LSB of register address
peng103617 60:c0c04325453c 239 _i2c.write(address, data_write, 2,0);
peng103617 60:c0c04325453c 240 _i2c.read(address,data_read,2,1);
peng103617 60:c0c04325453c 241 data_high = data_read[0]; //Read Data from selected register
peng103617 60:c0c04325453c 242 data_low = data_read[1]; //Read Data from selected register
peng103617 60:c0c04325453c 243 data = (data_high << 8)|data_low;
peng103617 60:c0c04325453c 244
peng103617 60:c0c04325453c 245 return data;
peng103617 60:c0c04325453c 246 }
peng103617 60:c0c04325453c 247 // Read a 32-bit register
peng103617 60:c0c04325453c 248 uint32_t VL53L1X::readReg32Bit(uint16_t reg)
peng103617 60:c0c04325453c 249 {
peng103617 60:c0c04325453c 250 uint32_t value;
peng103617 60:c0c04325453c 251 /*
peng103617 60:c0c04325453c 252 _i2c.beginTransmission(address);
peng103617 60:c0c04325453c 253 _i2c.write((reg >> 8) & 0xFF); // reg high byte
peng103617 60:c0c04325453c 254 _i2c.write( reg & 0xFF); // reg low byte
peng103617 60:c0c04325453c 255 last_status = _i2c.endTransmission();
peng103617 60:c0c04325453c 256
peng103617 60:c0c04325453c 257 _i2c.requestFrom(address, (uint8_t)4);
peng103617 60:c0c04325453c 258 value = (uint32_t)_i2c.read() << 24; // value highest byte
peng103617 60:c0c04325453c 259 value |= (uint32_t)_i2c.read() << 16;
peng103617 60:c0c04325453c 260 value |= (uint16_t)_i2c.read() << 8;
peng103617 60:c0c04325453c 261 value |= _i2c.read(); // value lowest byte
peng103617 60:c0c04325453c 262 */
peng103617 60:c0c04325453c 263 return value;
peng103617 60:c0c04325453c 264 }
peng103617 60:c0c04325453c 265
peng103617 60:c0c04325453c 266 // set distance mode to Short, Medium, or Long
peng103617 60:c0c04325453c 267 // based on VL53L1_SetDistanceMode()
peng103617 60:c0c04325453c 268 bool VL53L1X::setDistanceMode(DistanceMode mode)
peng103617 60:c0c04325453c 269 {
peng103617 60:c0c04325453c 270 // save existing timing budget
peng103617 60:c0c04325453c 271 uint32_t budget_us = getMeasurementTimingBudget();
peng103617 60:c0c04325453c 272 switch (mode)
peng103617 60:c0c04325453c 273 {
peng103617 60:c0c04325453c 274 case Short:
peng103617 60:c0c04325453c 275 // from VL53L1_preset_mode_standard_ranging_short_range()
peng103617 60:c0c04325453c 276
peng103617 60:c0c04325453c 277 // timing config
peng103617 60:c0c04325453c 278 writeReg(RANGE_CONFIG__VCSEL_PERIOD_A, 0x07);
peng103617 60:c0c04325453c 279 writeReg(RANGE_CONFIG__VCSEL_PERIOD_B, 0x05);
peng103617 60:c0c04325453c 280 writeReg(RANGE_CONFIG__VALID_PHASE_HIGH, 0x38);
peng103617 60:c0c04325453c 281
peng103617 60:c0c04325453c 282 // dynamic config
peng103617 60:c0c04325453c 283 writeReg(SD_CONFIG__WOI_SD0, 0x07);
peng103617 60:c0c04325453c 284 writeReg(SD_CONFIG__WOI_SD1, 0x05);
peng103617 60:c0c04325453c 285 writeReg(SD_CONFIG__INITIAL_PHASE_SD0, 6); // tuning parm default
peng103617 60:c0c04325453c 286 writeReg(SD_CONFIG__INITIAL_PHASE_SD1, 6); // tuning parm default
peng103617 60:c0c04325453c 287
peng103617 60:c0c04325453c 288 break;
peng103617 60:c0c04325453c 289
peng103617 60:c0c04325453c 290 case Medium:
peng103617 60:c0c04325453c 291 // from VL53L1_preset_mode_standard_ranging()
peng103617 60:c0c04325453c 292
peng103617 60:c0c04325453c 293 // timing config
peng103617 60:c0c04325453c 294 writeReg(RANGE_CONFIG__VCSEL_PERIOD_A, 0x0B);
peng103617 60:c0c04325453c 295 writeReg(RANGE_CONFIG__VCSEL_PERIOD_B, 0x09);
peng103617 60:c0c04325453c 296 writeReg(RANGE_CONFIG__VALID_PHASE_HIGH, 0x78);
peng103617 60:c0c04325453c 297
peng103617 60:c0c04325453c 298 // dynamic config
peng103617 60:c0c04325453c 299 writeReg(SD_CONFIG__WOI_SD0, 0x0B);
peng103617 60:c0c04325453c 300 writeReg(SD_CONFIG__WOI_SD1, 0x09);
peng103617 60:c0c04325453c 301 writeReg(SD_CONFIG__INITIAL_PHASE_SD0, 10); // tuning parm default
peng103617 60:c0c04325453c 302 writeReg(SD_CONFIG__INITIAL_PHASE_SD1, 10); // tuning parm default
peng103617 60:c0c04325453c 303
peng103617 60:c0c04325453c 304 break;
peng103617 60:c0c04325453c 305
peng103617 60:c0c04325453c 306 case Long: // long
peng103617 60:c0c04325453c 307 // from VL53L1_preset_mode_standard_ranging_long_range()
peng103617 60:c0c04325453c 308
peng103617 60:c0c04325453c 309 // timing config
peng103617 60:c0c04325453c 310 writeReg(RANGE_CONFIG__VCSEL_PERIOD_A, 0x0F);
peng103617 60:c0c04325453c 311 writeReg(RANGE_CONFIG__VCSEL_PERIOD_B, 0x0D);
peng103617 60:c0c04325453c 312 writeReg(RANGE_CONFIG__VALID_PHASE_HIGH, 0xB8);
peng103617 60:c0c04325453c 313
peng103617 60:c0c04325453c 314 // dynamic config
peng103617 60:c0c04325453c 315 writeReg(SD_CONFIG__WOI_SD0, 0x0F);
peng103617 60:c0c04325453c 316 writeReg(SD_CONFIG__WOI_SD1, 0x0D);
peng103617 60:c0c04325453c 317 writeReg(SD_CONFIG__INITIAL_PHASE_SD0, 14); // tuning parm default
peng103617 60:c0c04325453c 318 writeReg(SD_CONFIG__INITIAL_PHASE_SD1, 14); // tuning parm default
peng103617 60:c0c04325453c 319
peng103617 60:c0c04325453c 320 break;
peng103617 60:c0c04325453c 321
peng103617 60:c0c04325453c 322 default:
peng103617 60:c0c04325453c 323 // unrecognized mode - do nothing
peng103617 60:c0c04325453c 324 return false;
peng103617 60:c0c04325453c 325 }
peng103617 60:c0c04325453c 326
peng103617 60:c0c04325453c 327 // reapply timing budget
peng103617 60:c0c04325453c 328 setMeasurementTimingBudget(budget_us);
peng103617 60:c0c04325453c 329
peng103617 60:c0c04325453c 330 // save mode so it can be returned by getDistanceMode()
peng103617 60:c0c04325453c 331 distance_mode = mode;
peng103617 60:c0c04325453c 332
peng103617 60:c0c04325453c 333 return true;
peng103617 60:c0c04325453c 334 }
peng103617 60:c0c04325453c 335
peng103617 60:c0c04325453c 336 // Set the measurement timing budget in microseconds, which is the time allowed
peng103617 60:c0c04325453c 337 // for one measurement. A longer timing budget allows for more accurate
peng103617 60:c0c04325453c 338 // measurements.
peng103617 60:c0c04325453c 339 // based on VL53L1_SetMeasurementTimingBudgetMicroSeconds()
peng103617 60:c0c04325453c 340 bool VL53L1X::setMeasurementTimingBudget(uint32_t budget_us)
peng103617 60:c0c04325453c 341 {
peng103617 60:c0c04325453c 342 // assumes PresetMode is LOWPOWER_AUTONOMOUS
peng103617 60:c0c04325453c 343
peng103617 60:c0c04325453c 344 if (budget_us <= TimingGuard) { return false; }
peng103617 60:c0c04325453c 345
peng103617 60:c0c04325453c 346 uint32_t range_config_timeout_us = budget_us -= TimingGuard;
peng103617 60:c0c04325453c 347 if (range_config_timeout_us > 1100000) { return false; } // FDA_MAX_TIMING_BUDGET_US * 2
peng103617 60:c0c04325453c 348
peng103617 60:c0c04325453c 349 range_config_timeout_us /= 2;
peng103617 60:c0c04325453c 350
peng103617 60:c0c04325453c 351 // VL53L1_calc_timeout_register_values() begin
peng103617 60:c0c04325453c 352
peng103617 60:c0c04325453c 353 uint32_t macro_period_us;
peng103617 60:c0c04325453c 354
peng103617 60:c0c04325453c 355 // "Update Macro Period for Range A VCSEL Period"
peng103617 60:c0c04325453c 356 macro_period_us = calcMacroPeriod(readReg(RANGE_CONFIG__VCSEL_PERIOD_A));
peng103617 60:c0c04325453c 357
peng103617 60:c0c04325453c 358 // "Update Phase timeout - uses Timing A"
peng103617 60:c0c04325453c 359 // Timeout of 1000 is tuning parm default (TIMED_PHASECAL_CONFIG_TIMEOUT_US_DEFAULT)
peng103617 60:c0c04325453c 360 // via VL53L1_get_preset_mode_timing_cfg().
peng103617 60:c0c04325453c 361 uint32_t phasecal_timeout_mclks = timeoutMicrosecondsToMclks(1000, macro_period_us);
peng103617 60:c0c04325453c 362 if (phasecal_timeout_mclks > 0xFF) { phasecal_timeout_mclks = 0xFF; }
peng103617 60:c0c04325453c 363 writeReg(PHASECAL_CONFIG__TIMEOUT_MACROP, phasecal_timeout_mclks);
peng103617 60:c0c04325453c 364
peng103617 60:c0c04325453c 365 // "Update MM Timing A timeout"
peng103617 60:c0c04325453c 366 // Timeout of 1 is tuning parm default (LOWPOWERAUTO_MM_CONFIG_TIMEOUT_US_DEFAULT)
peng103617 60:c0c04325453c 367 // via VL53L1_get_preset_mode_timing_cfg(). With the API, the register
peng103617 60:c0c04325453c 368 // actually ends up with a slightly different value because it gets assigned,
peng103617 60:c0c04325453c 369 // retrieved, recalculated with a different macro period, and reassigned,
peng103617 60:c0c04325453c 370 // but it probably doesn't matter because it seems like the MM ("mode
peng103617 60:c0c04325453c 371 // mitigation"?) sequence steps are disabled in low power auto mode anyway.
peng103617 60:c0c04325453c 372 writeReg16Bit(MM_CONFIG__TIMEOUT_MACROP_A, encodeTimeout(
peng103617 60:c0c04325453c 373 timeoutMicrosecondsToMclks(1, macro_period_us)));
peng103617 60:c0c04325453c 374
peng103617 60:c0c04325453c 375 // "Update Range Timing A timeout"
peng103617 60:c0c04325453c 376 writeReg16Bit(RANGE_CONFIG__TIMEOUT_MACROP_A, encodeTimeout(
peng103617 60:c0c04325453c 377 timeoutMicrosecondsToMclks(range_config_timeout_us, macro_period_us)));
peng103617 60:c0c04325453c 378
peng103617 60:c0c04325453c 379 // "Update Macro Period for Range B VCSEL Period"
peng103617 60:c0c04325453c 380 macro_period_us = calcMacroPeriod(readReg(RANGE_CONFIG__VCSEL_PERIOD_B));
peng103617 60:c0c04325453c 381
peng103617 60:c0c04325453c 382 // "Update MM Timing B timeout"
peng103617 60:c0c04325453c 383 // (See earlier comment about MM Timing A timeout.)
peng103617 60:c0c04325453c 384 writeReg16Bit(MM_CONFIG__TIMEOUT_MACROP_B, encodeTimeout(
peng103617 60:c0c04325453c 385 timeoutMicrosecondsToMclks(1, macro_period_us)));
peng103617 60:c0c04325453c 386
peng103617 60:c0c04325453c 387 // "Update Range Timing B timeout"
peng103617 60:c0c04325453c 388 writeReg16Bit(RANGE_CONFIG__TIMEOUT_MACROP_B, encodeTimeout(
peng103617 60:c0c04325453c 389 timeoutMicrosecondsToMclks(range_config_timeout_us, macro_period_us)));
peng103617 60:c0c04325453c 390 // VL53L1_calc_timeout_register_values() end
peng103617 60:c0c04325453c 391
peng103617 60:c0c04325453c 392 return true;
peng103617 60:c0c04325453c 393 }
peng103617 60:c0c04325453c 394
peng103617 60:c0c04325453c 395 // Get the measurement timing budget in microseconds
peng103617 60:c0c04325453c 396 // based on VL53L1_SetMeasurementTimingBudgetMicroSeconds()
peng103617 60:c0c04325453c 397 uint32_t VL53L1X::getMeasurementTimingBudget()
peng103617 60:c0c04325453c 398 {
peng103617 60:c0c04325453c 399 // assumes PresetMode is LOWPOWER_AUTONOMOUS and these sequence steps are
peng103617 60:c0c04325453c 400 // enabled: VHV, PHASECAL, DSS1, RANGE
peng103617 60:c0c04325453c 401
peng103617 60:c0c04325453c 402 // VL53L1_get_timeouts_us() begin
peng103617 60:c0c04325453c 403
peng103617 60:c0c04325453c 404 // "Update Macro Period for Range A VCSEL Period"
peng103617 60:c0c04325453c 405 uint32_t macro_period_us = calcMacroPeriod(readReg(RANGE_CONFIG__VCSEL_PERIOD_A));
peng103617 60:c0c04325453c 406
peng103617 60:c0c04325453c 407 // "Get Range Timing A timeout"
peng103617 60:c0c04325453c 408
peng103617 60:c0c04325453c 409 uint32_t range_config_timeout_us = timeoutMclksToMicroseconds(decodeTimeout(
peng103617 60:c0c04325453c 410 readReg16Bit(RANGE_CONFIG__TIMEOUT_MACROP_A)), macro_period_us);
peng103617 60:c0c04325453c 411
peng103617 60:c0c04325453c 412 // VL53L1_get_timeouts_us() end
peng103617 60:c0c04325453c 413
peng103617 60:c0c04325453c 414 return 2 * range_config_timeout_us + TimingGuard;
peng103617 60:c0c04325453c 415 }
peng103617 60:c0c04325453c 416
peng103617 60:c0c04325453c 417 // Start continuous ranging measurements, with the given inter-measurement
peng103617 60:c0c04325453c 418 // period in milliseconds determining how often the sensor takes a measurement.
peng103617 60:c0c04325453c 419 void VL53L1X::startContinuous(uint32_t period_ms)
peng103617 60:c0c04325453c 420 {
peng103617 60:c0c04325453c 421 // from VL53L1_set_inter_measurement_period_ms()
peng103617 60:c0c04325453c 422 writeReg32Bit(SYSTEM__INTERMEASUREMENT_PERIOD, period_ms * osc_calibrate_val);
peng103617 60:c0c04325453c 423 writeReg(SYSTEM__INTERRUPT_CLEAR, 0x01); // sys_interrupt_clear_range
peng103617 60:c0c04325453c 424 writeReg(SYSTEM__MODE_START, 0x40); // mode_range__timed
peng103617 60:c0c04325453c 425 }
peng103617 60:c0c04325453c 426
peng103617 60:c0c04325453c 427 // Stop continuous measurements
peng103617 60:c0c04325453c 428 // based on VL53L1_stop_range()
peng103617 60:c0c04325453c 429 void VL53L1X::stopContinuous()
peng103617 60:c0c04325453c 430 {
peng103617 60:c0c04325453c 431 writeReg(SYSTEM__MODE_START, 0x80); // mode_range__abort
peng103617 60:c0c04325453c 432
peng103617 60:c0c04325453c 433 // VL53L1_low_power_auto_data_stop_range() begin
peng103617 60:c0c04325453c 434
peng103617 60:c0c04325453c 435 calibrated = false;
peng103617 60:c0c04325453c 436
peng103617 60:c0c04325453c 437 // "restore vhv configs"
peng103617 60:c0c04325453c 438 if (saved_vhv_init != 0)
peng103617 60:c0c04325453c 439 {
peng103617 60:c0c04325453c 440 writeReg(VHV_CONFIG__INIT, saved_vhv_init);
peng103617 60:c0c04325453c 441 }
peng103617 60:c0c04325453c 442 if (saved_vhv_timeout != 0)
peng103617 60:c0c04325453c 443 {
peng103617 60:c0c04325453c 444 writeReg(VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND, saved_vhv_timeout);
peng103617 60:c0c04325453c 445 }
peng103617 60:c0c04325453c 446
peng103617 60:c0c04325453c 447 // "remove phasecal override"
peng103617 60:c0c04325453c 448 writeReg(PHASECAL_CONFIG__OVERRIDE, 0x00);
peng103617 60:c0c04325453c 449
peng103617 60:c0c04325453c 450 // VL53L1_low_power_auto_data_stop_range() end
peng103617 60:c0c04325453c 451 }
peng103617 60:c0c04325453c 452
peng103617 60:c0c04325453c 453 // Returns a range reading in millimeters when continuous mode is active
peng103617 60:c0c04325453c 454 // (readRangeSingleMillimetersx () also calls this function after starting a
peng103617 60:c0c04325453c 455 // single-shot range measurement)
peng103617 60:c0c04325453c 456 uint16_t VL53L1X::read(bool blocking)
peng103617 60:c0c04325453c 457 {
peng103617 60:c0c04325453c 458 if (blocking)
peng103617 60:c0c04325453c 459 {
peng103617 60:c0c04325453c 460 // startTimeout();
peng103617 60:c0c04325453c 461
peng103617 60:c0c04325453c 462 /* dataReady returns 0. Loop is never entered. */
peng103617 60:c0c04325453c 463 /*
peng103617 60:c0c04325453c 464 while (dataReady())
peng103617 60:c0c04325453c 465 {
peng103617 60:c0c04325453c 466 if (checkTimeoutExpired())
peng103617 60:c0c04325453c 467 {
peng103617 60:c0c04325453c 468 did_timeout = true;
peng103617 60:c0c04325453c 469 ranging_data.range_status = None;
peng103617 60:c0c04325453c 470 ranging_data.range_mm = 0;
peng103617 60:c0c04325453c 471 ranging_data.peak_signal_count_rate_MCPS = 0;
peng103617 60:c0c04325453c 472 ranging_data.ambient_count_rate_MCPS = 0;
peng103617 60:c0c04325453c 473 return ranging_data.range_mm;
peng103617 60:c0c04325453c 474 }
peng103617 60:c0c04325453c 475 }*/
peng103617 60:c0c04325453c 476 }
peng103617 60:c0c04325453c 477
peng103617 60:c0c04325453c 478 readResults();
peng103617 60:c0c04325453c 479
peng103617 60:c0c04325453c 480 if (!calibrated)
peng103617 60:c0c04325453c 481 {
peng103617 60:c0c04325453c 482 setupManualCalibration();
peng103617 60:c0c04325453c 483 calibrated = true;
peng103617 60:c0c04325453c 484 }
peng103617 60:c0c04325453c 485
peng103617 60:c0c04325453c 486 updateDSS();
peng103617 60:c0c04325453c 487
peng103617 60:c0c04325453c 488 getRangingData();
peng103617 60:c0c04325453c 489
peng103617 60:c0c04325453c 490 writeReg(SYSTEM__INTERRUPT_CLEAR, 0x01); // sys_interrupt_clear_range
peng103617 60:c0c04325453c 491
peng103617 60:c0c04325453c 492 return ranging_data.range_mm;
peng103617 60:c0c04325453c 493 }
peng103617 60:c0c04325453c 494
peng103617 60:c0c04325453c 495 // convert a RangeStatus to a readable string
peng103617 60:c0c04325453c 496 // Note that on an AVR, these strings are stored in RAM (dynamic memory), which
peng103617 60:c0c04325453c 497 // makes working with them easier but uses up 200+ bytes of RAM (many AVR-based
peng103617 60:c0c04325453c 498 // Arduinos only have about 2000 bytes of RAM). You can avoid this memory usage
peng103617 60:c0c04325453c 499 // if you do not call this function in your sketch.
peng103617 60:c0c04325453c 500 const char * VL53L1X::rangeStatusToString(RangeStatus status)
peng103617 60:c0c04325453c 501 {
peng103617 60:c0c04325453c 502 switch (status)
peng103617 60:c0c04325453c 503 {
peng103617 60:c0c04325453c 504 case RangeValid:
peng103617 60:c0c04325453c 505 return "range valid";
peng103617 60:c0c04325453c 506
peng103617 60:c0c04325453c 507 case SigmaFail:
peng103617 60:c0c04325453c 508 return "sigma fail";
peng103617 60:c0c04325453c 509
peng103617 60:c0c04325453c 510 case SignalFail:
peng103617 60:c0c04325453c 511 return "signal fail";
peng103617 60:c0c04325453c 512
peng103617 60:c0c04325453c 513 case RangeValidMinRangeClipped:
peng103617 60:c0c04325453c 514 return "range valid, min range clipped";
peng103617 60:c0c04325453c 515
peng103617 60:c0c04325453c 516 case OutOfBoundsFail:
peng103617 60:c0c04325453c 517 return "out of bounds fail";
peng103617 60:c0c04325453c 518
peng103617 60:c0c04325453c 519 case HardwareFail:
peng103617 60:c0c04325453c 520 return "hardware fail";
peng103617 60:c0c04325453c 521
peng103617 60:c0c04325453c 522 case RangeValidNoWrapCheckFail:
peng103617 60:c0c04325453c 523 return "range valid, no wrap check fail";
peng103617 60:c0c04325453c 524
peng103617 60:c0c04325453c 525 case WrapTargetFail:
peng103617 60:c0c04325453c 526 return "wrap target fail";
peng103617 60:c0c04325453c 527
peng103617 60:c0c04325453c 528 case XtalkSignalFail:
peng103617 60:c0c04325453c 529 return "xtalk signal fail";
peng103617 60:c0c04325453c 530
peng103617 60:c0c04325453c 531 case SynchronizationInt:
peng103617 60:c0c04325453c 532 return "synchronization int";
peng103617 60:c0c04325453c 533
peng103617 60:c0c04325453c 534 case MinRangeFail:
peng103617 60:c0c04325453c 535 return "min range fail";
peng103617 60:c0c04325453c 536
peng103617 60:c0c04325453c 537 case None:
peng103617 60:c0c04325453c 538 return "no update";
peng103617 60:c0c04325453c 539
peng103617 60:c0c04325453c 540 default:
peng103617 60:c0c04325453c 541 return "unknown status";
peng103617 60:c0c04325453c 542 }
peng103617 60:c0c04325453c 543 }
peng103617 60:c0c04325453c 544
peng103617 60:c0c04325453c 545 // Did a timeout occur in one of the read functions since the last call to
peng103617 60:c0c04325453c 546 // timeoutOccurred()?
peng103617 60:c0c04325453c 547 bool VL53L1X::timeoutOccurred()
peng103617 60:c0c04325453c 548 {
peng103617 60:c0c04325453c 549 bool tmp = did_timeout;
peng103617 60:c0c04325453c 550 did_timeout = false;
peng103617 60:c0c04325453c 551 return tmp;
peng103617 60:c0c04325453c 552 }
peng103617 60:c0c04325453c 553
peng103617 60:c0c04325453c 554 // Private Methods /////////////////////////////////////////////////////////////
peng103617 60:c0c04325453c 555
peng103617 60:c0c04325453c 556 // "Setup ranges after the first one in low power auto mode by turning off
peng103617 60:c0c04325453c 557 // FW calibration steps and programming static values"
peng103617 60:c0c04325453c 558 // based on VL53L1_low_power_auto_setup_manual_calibration()
peng103617 60:c0c04325453c 559 void VL53L1X::setupManualCalibration()
peng103617 60:c0c04325453c 560 {
peng103617 60:c0c04325453c 561 // "save original vhv configs"
peng103617 60:c0c04325453c 562 saved_vhv_init = readReg(VHV_CONFIG__INIT);
peng103617 60:c0c04325453c 563 saved_vhv_timeout = readReg(VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND);
peng103617 60:c0c04325453c 564
peng103617 60:c0c04325453c 565 // "disable VHV init"
peng103617 60:c0c04325453c 566 writeReg(VHV_CONFIG__INIT, saved_vhv_init & 0x7F);
peng103617 60:c0c04325453c 567
peng103617 60:c0c04325453c 568 // "set loop bound to tuning param"
peng103617 60:c0c04325453c 569 writeReg(VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND,
peng103617 60:c0c04325453c 570 (saved_vhv_timeout & 0x03) + (3 << 2)); // tuning parm default (LOWPOWERAUTO_VHV_LOOP_BOUND_DEFAULT)
peng103617 60:c0c04325453c 571
peng103617 60:c0c04325453c 572 // "override phasecal"
peng103617 60:c0c04325453c 573 writeReg(PHASECAL_CONFIG__OVERRIDE, 0x01);
peng103617 60:c0c04325453c 574 writeReg(CAL_CONFIG__VCSEL_START, readReg(PHASECAL_RESULT__VCSEL_START));
peng103617 60:c0c04325453c 575 }
peng103617 60:c0c04325453c 576
peng103617 60:c0c04325453c 577 // read measurement results into buffer
peng103617 60:c0c04325453c 578 void VL53L1X::readResults()
peng103617 60:c0c04325453c 579 {
peng103617 60:c0c04325453c 580 char infoToWrite[2];
peng103617 60:c0c04325453c 581 char infoToRead[18];
peng103617 60:c0c04325453c 582 //_i2c.beginTransmission(address);
peng103617 60:c0c04325453c 583 //_i2c.write(address);
peng103617 60:c0c04325453c 584 //_i2c.write((RESULT__RANGE_STATUS >> 8) & 0xFF); // reg high byte
peng103617 60:c0c04325453c 585 //_i2c.write( RESULT__RANGE_STATUS & 0xFF); // reg low byte
peng103617 60:c0c04325453c 586 // last_status = _i2c.endTransmission();
peng103617 60:c0c04325453c 587 infoToWrite[0] = ((RESULT__RANGE_STATUS >> 8) & 0xFF);
peng103617 60:c0c04325453c 588 infoToWrite[1] = ( RESULT__RANGE_STATUS & 0xFF);
peng103617 60:c0c04325453c 589 _i2c.write(address, infoToWrite, 2, 1);
peng103617 60:c0c04325453c 590
peng103617 60:c0c04325453c 591 // _i2c.requestFrom(address, (uint8_t)17);
peng103617 60:c0c04325453c 592 _i2c.read(address, infoToRead, 17, 0);
peng103617 60:c0c04325453c 593
peng103617 60:c0c04325453c 594 wait(.005);
peng103617 60:c0c04325453c 595 results.range_status = infoToRead[0];
peng103617 60:c0c04325453c 596
peng103617 60:c0c04325453c 597 // infoToRead[1]; // report_status: not used
peng103617 60:c0c04325453c 598
peng103617 60:c0c04325453c 599 results.stream_count = infoToRead[2];
peng103617 60:c0c04325453c 600
peng103617 60:c0c04325453c 601 results.dss_actual_effective_spads_sd0 = (uint16_t)infoToRead[3] << 8; // high byte
peng103617 60:c0c04325453c 602 results.dss_actual_effective_spads_sd0 |= infoToRead[4]; // low byte
peng103617 60:c0c04325453c 603
peng103617 60:c0c04325453c 604 // infoToRead[5]; // peak_signal_count_rate_mcps_sd0: not used
peng103617 60:c0c04325453c 605 // infoToRead[6];
peng103617 60:c0c04325453c 606
peng103617 60:c0c04325453c 607 results.ambient_count_rate_mcps_sd0 = (uint16_t)infoToRead[7] << 8; // high byte
peng103617 60:c0c04325453c 608 results.ambient_count_rate_mcps_sd0 |= infoToRead[8]; // low byte
peng103617 60:c0c04325453c 609
peng103617 60:c0c04325453c 610 // infoToRead[9]; // sigma_sd0: not used
peng103617 60:c0c04325453c 611 // infoToRead[10];
peng103617 60:c0c04325453c 612
peng103617 60:c0c04325453c 613 // infoToRead[11]; // phase_sd0: not used
peng103617 60:c0c04325453c 614 // infoToRead[12];
peng103617 60:c0c04325453c 615
peng103617 60:c0c04325453c 616 results.final_crosstalk_corrected_range_mm_sd0 = (uint16_t)infoToRead[13] << 8; // high byte
peng103617 60:c0c04325453c 617 results.final_crosstalk_corrected_range_mm_sd0 |= infoToRead[14]; // low byte
peng103617 60:c0c04325453c 618
peng103617 60:c0c04325453c 619 results.peak_signal_count_rate_crosstalk_corrected_mcps_sd0 = (uint16_t)infoToRead[15] << 8; // high byte
peng103617 60:c0c04325453c 620 results.peak_signal_count_rate_crosstalk_corrected_mcps_sd0 |= infoToRead[16]; // low byte
peng103617 60:c0c04325453c 621 }
peng103617 60:c0c04325453c 622
peng103617 60:c0c04325453c 623 // perform Dynamic SPAD Selection calculation/update
peng103617 60:c0c04325453c 624 // based on VL53L1_low_power_auto_update_DSS()
peng103617 60:c0c04325453c 625 void VL53L1X::updateDSS()
peng103617 60:c0c04325453c 626 {
peng103617 60:c0c04325453c 627 uint16_t spadCount = results.dss_actual_effective_spads_sd0;
peng103617 60:c0c04325453c 628
peng103617 60:c0c04325453c 629 if (spadCount != 0)
peng103617 60:c0c04325453c 630 {
peng103617 60:c0c04325453c 631 // "Calc total rate per spad"
peng103617 60:c0c04325453c 632
peng103617 60:c0c04325453c 633 uint32_t totalRatePerSpad =
peng103617 60:c0c04325453c 634 (uint32_t)results.peak_signal_count_rate_crosstalk_corrected_mcps_sd0 +
peng103617 60:c0c04325453c 635 results.ambient_count_rate_mcps_sd0;
peng103617 60:c0c04325453c 636
peng103617 60:c0c04325453c 637 // "clip to 16 bits"
peng103617 60:c0c04325453c 638 if (totalRatePerSpad > 0xFFFF) { totalRatePerSpad = 0xFFFF; }
peng103617 60:c0c04325453c 639
peng103617 60:c0c04325453c 640 // "shift up to take advantage of 32 bits"
peng103617 60:c0c04325453c 641 totalRatePerSpad <<= 16;
peng103617 60:c0c04325453c 642
peng103617 60:c0c04325453c 643 totalRatePerSpad /= spadCount;
peng103617 60:c0c04325453c 644
peng103617 60:c0c04325453c 645 if (totalRatePerSpad != 0)
peng103617 60:c0c04325453c 646 {
peng103617 60:c0c04325453c 647 // "get the target rate and shift up by 16"
peng103617 60:c0c04325453c 648 uint32_t requiredSpads = ((uint32_t)TargetRate << 16) / totalRatePerSpad;
peng103617 60:c0c04325453c 649
peng103617 60:c0c04325453c 650 // "clip to 16 bit"
peng103617 60:c0c04325453c 651 if (requiredSpads > 0xFFFF) { requiredSpads = 0xFFFF; }
peng103617 60:c0c04325453c 652
peng103617 60:c0c04325453c 653 // "override DSS config"
peng103617 60:c0c04325453c 654 writeReg16Bit(DSS_CONFIG__MANUAL_EFFECTIVE_SPADS_SELECT, requiredSpads);
peng103617 60:c0c04325453c 655 // DSS_CONFIG__ROI_MODE_CONTROL should already be set to REQUESTED_EFFFECTIVE_SPADS
peng103617 60:c0c04325453c 656
peng103617 60:c0c04325453c 657 return;
peng103617 60:c0c04325453c 658 }
peng103617 60:c0c04325453c 659 }
peng103617 60:c0c04325453c 660
peng103617 60:c0c04325453c 661 // If we reached this point, it means something above would have resulted in a
peng103617 60:c0c04325453c 662 // divide by zero.
peng103617 60:c0c04325453c 663 // "We want to gracefully set a spad target, not just exit with an error"
peng103617 60:c0c04325453c 664
peng103617 60:c0c04325453c 665 // "set target to mid point"
peng103617 60:c0c04325453c 666 writeReg16Bit(DSS_CONFIG__MANUAL_EFFECTIVE_SPADS_SELECT, 0x8000);
peng103617 60:c0c04325453c 667 }
peng103617 60:c0c04325453c 668
peng103617 60:c0c04325453c 669 // get range, status, rates from results buffer
peng103617 60:c0c04325453c 670 // based on VL53L1_GetRangingMeasurementData()
peng103617 60:c0c04325453c 671 void VL53L1X::getRangingData()
peng103617 60:c0c04325453c 672 {
peng103617 60:c0c04325453c 673 // VL53L1_copy_sys_and_core_results_to_range_results() begin
peng103617 60:c0c04325453c 674
peng103617 60:c0c04325453c 675 uint16_t range = results.final_crosstalk_corrected_range_mm_sd0;
peng103617 60:c0c04325453c 676
peng103617 60:c0c04325453c 677 // "apply correction gain"
peng103617 60:c0c04325453c 678 // gain factor of 2011 is tuning parm default (VL53L1_TUNINGPARM_LITE_RANGING_GAIN_FACTOR_DEFAULT)
peng103617 60:c0c04325453c 679 // Basically, this appears to scale the result by 2011/2048, or about 98%
peng103617 60:c0c04325453c 680 // (with the 1024 added for proper rounding).
peng103617 60:c0c04325453c 681 ranging_data.range_mm = ((uint32_t)range * 2011 + 0x0400) / 0x0800;
peng103617 60:c0c04325453c 682 wait(.005);
peng103617 60:c0c04325453c 683 // VL53L1_copy_sys_and_core_results_to_range_results() end
peng103617 60:c0c04325453c 684
peng103617 60:c0c04325453c 685 // set range_status in ranging_data based on value of RESULT__RANGE_STATUS register
peng103617 60:c0c04325453c 686 // mostly based on ConvertStatusLite()
peng103617 60:c0c04325453c 687 switch(results.range_status)
peng103617 60:c0c04325453c 688 {
peng103617 60:c0c04325453c 689 case 17: // MULTCLIPFAIL
peng103617 60:c0c04325453c 690 case 2: // VCSELWATCHDOGTESTFAILURE
peng103617 60:c0c04325453c 691 case 1: // VCSELCONTINUITYTESTFAILURE
peng103617 60:c0c04325453c 692 case 3: // NOVHVVALUEFOUND
peng103617 60:c0c04325453c 693 // from SetSimpleData()
peng103617 60:c0c04325453c 694 ranging_data.range_status = HardwareFail;
peng103617 60:c0c04325453c 695 break;
peng103617 60:c0c04325453c 696
peng103617 60:c0c04325453c 697 case 13: // USERROICLIP
peng103617 60:c0c04325453c 698 // from SetSimpleData()
peng103617 60:c0c04325453c 699 ranging_data.range_status = MinRangeFail;
peng103617 60:c0c04325453c 700 break;
peng103617 60:c0c04325453c 701
peng103617 60:c0c04325453c 702 case 18: // GPHSTREAMCOUNT0READY
peng103617 60:c0c04325453c 703 ranging_data.range_status = SynchronizationInt;
peng103617 60:c0c04325453c 704 break;
peng103617 60:c0c04325453c 705
peng103617 60:c0c04325453c 706 case 5: // RANGEPHASECHECK
peng103617 60:c0c04325453c 707 ranging_data.range_status = OutOfBoundsFail;
peng103617 60:c0c04325453c 708 break;
peng103617 60:c0c04325453c 709
peng103617 60:c0c04325453c 710 case 4: // MSRCNOTARGET
peng103617 60:c0c04325453c 711 ranging_data.range_status = SignalFail;
peng103617 60:c0c04325453c 712 break;
peng103617 60:c0c04325453c 713
peng103617 60:c0c04325453c 714 case 6: // SIGMATHRESHOLDCHECK
peng103617 60:c0c04325453c 715 ranging_data.range_status = SignalFail;
peng103617 60:c0c04325453c 716 break;
peng103617 60:c0c04325453c 717
peng103617 60:c0c04325453c 718 case 7: // PHASECONSISTENCY
peng103617 60:c0c04325453c 719 ranging_data.range_status = WrapTargetFail;
peng103617 60:c0c04325453c 720 break;
peng103617 60:c0c04325453c 721
peng103617 60:c0c04325453c 722 case 12: // RANGEIGNORETHRESHOLD
peng103617 60:c0c04325453c 723 ranging_data.range_status = XtalkSignalFail;
peng103617 60:c0c04325453c 724 break;
peng103617 60:c0c04325453c 725
peng103617 60:c0c04325453c 726 case 8: // MINCLIP
peng103617 60:c0c04325453c 727 ranging_data.range_status = RangeValidMinRangeClipped;
peng103617 60:c0c04325453c 728 break;
peng103617 60:c0c04325453c 729
peng103617 60:c0c04325453c 730 case 9: // RANGECOMPLETE
peng103617 60:c0c04325453c 731 // from VL53L1_copy_sys_and_core_results_to_range_results()
peng103617 60:c0c04325453c 732 if (results.stream_count == 0)
peng103617 60:c0c04325453c 733 {
peng103617 60:c0c04325453c 734 ranging_data.range_status = RangeValidNoWrapCheckFail;
peng103617 60:c0c04325453c 735 }
peng103617 60:c0c04325453c 736 else
peng103617 60:c0c04325453c 737 {
peng103617 60:c0c04325453c 738 ranging_data.range_status = RangeValid;
peng103617 60:c0c04325453c 739 }
peng103617 60:c0c04325453c 740 break;
peng103617 60:c0c04325453c 741
peng103617 60:c0c04325453c 742 default:
peng103617 60:c0c04325453c 743 ranging_data.range_status = None;
peng103617 60:c0c04325453c 744 }
peng103617 60:c0c04325453c 745
peng103617 60:c0c04325453c 746 // from SetSimpleData()
peng103617 60:c0c04325453c 747 ranging_data.peak_signal_count_rate_MCPS =
peng103617 60:c0c04325453c 748 countRateFixedToFloat(results.peak_signal_count_rate_crosstalk_corrected_mcps_sd0);
peng103617 60:c0c04325453c 749 ranging_data.ambient_count_rate_MCPS =
peng103617 60:c0c04325453c 750 countRateFixedToFloat(results.ambient_count_rate_mcps_sd0);
peng103617 60:c0c04325453c 751 }
peng103617 60:c0c04325453c 752
peng103617 60:c0c04325453c 753 // Decode sequence step timeout in MCLKs from register value
peng103617 60:c0c04325453c 754 // based on VL53L1_decode_timeout()
peng103617 60:c0c04325453c 755 uint32_t VL53L1X::decodeTimeout(uint16_t reg_val)
peng103617 60:c0c04325453c 756 {
peng103617 60:c0c04325453c 757 return ((uint32_t)(reg_val & 0xFF) << (reg_val >> 8)) + 1;
peng103617 60:c0c04325453c 758 }
peng103617 60:c0c04325453c 759
peng103617 60:c0c04325453c 760 // Encode sequence step timeout register value from timeout in MCLKs
peng103617 60:c0c04325453c 761 // based on VL53L1_encode_timeout()
peng103617 60:c0c04325453c 762 uint16_t VL53L1X::encodeTimeout(uint32_t timeout_mclks)
peng103617 60:c0c04325453c 763 {
peng103617 60:c0c04325453c 764 // encoded format: "(LSByte * 2^MSByte) + 1"
peng103617 60:c0c04325453c 765
peng103617 60:c0c04325453c 766 uint32_t ls_byte = 0;
peng103617 60:c0c04325453c 767 uint16_t ms_byte = 0;
peng103617 60:c0c04325453c 768
peng103617 60:c0c04325453c 769 if (timeout_mclks > 0)
peng103617 60:c0c04325453c 770 {
peng103617 60:c0c04325453c 771 ls_byte = timeout_mclks - 1;
peng103617 60:c0c04325453c 772
peng103617 60:c0c04325453c 773 while ((ls_byte & 0xFFFFFF00) > 0)
peng103617 60:c0c04325453c 774 {
peng103617 60:c0c04325453c 775 ls_byte >>= 1;
peng103617 60:c0c04325453c 776 ms_byte++;
peng103617 60:c0c04325453c 777 }
peng103617 60:c0c04325453c 778
peng103617 60:c0c04325453c 779 return (ms_byte << 8) | (ls_byte & 0xFF);
peng103617 60:c0c04325453c 780 }
peng103617 60:c0c04325453c 781 else { return 0; }
peng103617 60:c0c04325453c 782 }
peng103617 60:c0c04325453c 783
peng103617 60:c0c04325453c 784 // Convert sequence step timeout from macro periods to microseconds with given
peng103617 60:c0c04325453c 785 // macro period in microseconds (12.12 format)
peng103617 60:c0c04325453c 786 // based on VL53L1_calc_timeout_us()
peng103617 60:c0c04325453c 787 uint32_t VL53L1X::timeoutMclksToMicroseconds(uint32_t timeout_mclks, uint32_t macro_period_us)
peng103617 60:c0c04325453c 788 {
peng103617 60:c0c04325453c 789 return ((uint64_t)timeout_mclks * macro_period_us + 0x800) >> 12;
peng103617 60:c0c04325453c 790 }
peng103617 60:c0c04325453c 791
peng103617 60:c0c04325453c 792 // Convert sequence step timeout from microseconds to macro periods with given
peng103617 60:c0c04325453c 793 // macro period in microseconds (12.12 format)
peng103617 60:c0c04325453c 794 // based on VL53L1_calc_timeout_mclks()
peng103617 60:c0c04325453c 795 uint32_t VL53L1X::timeoutMicrosecondsToMclks(uint32_t timeout_us, uint32_t macro_period_us)
peng103617 60:c0c04325453c 796 {
peng103617 60:c0c04325453c 797 return (((uint32_t)timeout_us << 12) + (macro_period_us >> 1)) / macro_period_us;
peng103617 60:c0c04325453c 798 }
peng103617 60:c0c04325453c 799
peng103617 60:c0c04325453c 800 // Calculate macro period in microseconds (12.12 format) with given VCSEL period
peng103617 60:c0c04325453c 801 // assumes fast_osc_frequency has been read and stored
peng103617 60:c0c04325453c 802 // based on VL53L1_calc_macro_period_us()
peng103617 60:c0c04325453c 803 uint32_t VL53L1X::calcMacroPeriod(uint8_t vcsel_period)
peng103617 60:c0c04325453c 804 {
peng103617 60:c0c04325453c 805 // from VL53L1_calc_pll_period_us()
peng103617 60:c0c04325453c 806 // fast osc frequency in 4.12 format; PLL period in 0.24 format
peng103617 60:c0c04325453c 807 uint32_t pll_period_us = ((uint32_t)0x01 << 30) / fast_osc_frequency;
peng103617 60:c0c04325453c 808
peng103617 60:c0c04325453c 809 // from VL53L1_decode_vcsel_period()
peng103617 60:c0c04325453c 810 uint8_t vcsel_period_pclks = (vcsel_period + 1) << 1;
peng103617 60:c0c04325453c 811
peng103617 60:c0c04325453c 812 // VL53L1_MACRO_PERIOD_VCSEL_PERIODS = 2304
peng103617 60:c0c04325453c 813 uint32_t macro_period_us = (uint32_t)2304 * pll_period_us;
peng103617 60:c0c04325453c 814 macro_period_us >>= 6;
peng103617 60:c0c04325453c 815 macro_period_us *= vcsel_period_pclks;
peng103617 60:c0c04325453c 816 macro_period_us >>= 6;
peng103617 60:c0c04325453c 817
peng103617 60:c0c04325453c 818 return macro_period_us;
peng103617 60:c0c04325453c 819 }
peng103617 60:c0c04325453c 820
peng103617 60:c0c04325453c 821
peng103617 60:c0c04325453c 822
peng103617 60:c0c04325453c 823
peng103617 60:c0c04325453c 824
peng103617 60:c0c04325453c 825
peng103617 60:c0c04325453c 826
peng103617 60:c0c04325453c 827
peng103617 60:c0c04325453c 828
peng103617 60:c0c04325453c 829
peng103617 60:c0c04325453c 830 bool VL53L1X::initReading(int addr, int timing_budget)
peng103617 60:c0c04325453c 831 {
peng103617 60:c0c04325453c 832 turnOn();
peng103617 60:c0c04325453c 833 wait_ms(100);
peng103617 60:c0c04325453c 834 setTimeout(500);
peng103617 60:c0c04325453c 835 if (!init()) {
peng103617 60:c0c04325453c 836 didInitialize = false;
peng103617 60:c0c04325453c 837 return false;
peng103617 60:c0c04325453c 838 }
peng103617 60:c0c04325453c 839 // setDistanceMode(VL53L1X::Short);//Short Medium Long
peng103617 60:c0c04325453c 840 setAddress(addr);//change I2C address for next sensor
peng103617 60:c0c04325453c 841 setMeasurementTimingBudget(timing_budget);//min 20ms for Short, 33ms for Medium and Long
peng103617 60:c0c04325453c 842 startContinuous(50);
peng103617 60:c0c04325453c 843 wait_ms(100);
peng103617 60:c0c04325453c 844 didInitialize = true;
peng103617 60:c0c04325453c 845 return true;
peng103617 60:c0c04325453c 846 }
peng103617 60:c0c04325453c 847 //*************************************
peng103617 60:c0c04325453c 848
peng103617 60:c0c04325453c 849 //*********GPIO***********
peng103617 60:c0c04325453c 850 void VL53L1X::turnOff(void)
peng103617 60:c0c04325453c 851 {
peng103617 60:c0c04325453c 852 //turn pin LOW
peng103617 60:c0c04325453c 853 _shutDown = false;
peng103617 60:c0c04325453c 854 }
peng103617 60:c0c04325453c 855 void VL53L1X::resetPin(void)
peng103617 60:c0c04325453c 856 {
peng103617 60:c0c04325453c 857 //reset pin and set it to LOW
peng103617 60:c0c04325453c 858 _shutDown = false;
peng103617 60:c0c04325453c 859 wait(.05);
peng103617 60:c0c04325453c 860 _shutDown = true;
peng103617 60:c0c04325453c 861 wait(.05);
peng103617 60:c0c04325453c 862 _shutDown = false;
peng103617 60:c0c04325453c 863 wait(.05);
peng103617 60:c0c04325453c 864
peng103617 60:c0c04325453c 865 }
peng103617 60:c0c04325453c 866 void VL53L1X::turnOn(void)
peng103617 60:c0c04325453c 867 {
peng103617 60:c0c04325453c 868 //turn pin HIGH
peng103617 60:c0c04325453c 869 _shutDown = true;
peng103617 60:c0c04325453c 870 }
peng103617 60:c0c04325453c 871 int VL53L1X::readFromOneSensor(void)
peng103617 60:c0c04325453c 872 {
peng103617 60:c0c04325453c 873 if (didInitialize) //create bool
peng103617 60:c0c04325453c 874 return read();
peng103617 60:c0c04325453c 875 else
peng103617 60:c0c04325453c 876 return -1;
peng103617 60:c0c04325453c 877 }