This library allows you to read from multiple VL53L1X sensors.

Dependencies:   mbed

Fork of VL53L1X_Pololu by Jesus Fausto

Committer:
jvfausto
Date:
Wed Aug 08 21:44:33 2018 +0000
Revision:
1:e54ded4af43a
Parent:
0:76ea242a637f
Child:
2:f570ff03fb81
Works with 4 sensors

Who changed what in which revision?

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