Jesus Fausto / Mbed 2 deprecated VL53L1X_Filter

Median of 3 filter for multiple time of flight sensors translated from arduino by pololu

Dependencies:   mbed

Dependents:   wheelchaircontrol wheelchaircontrol2 wheelchaircontrol3 wheelchaircontrol4 ... more

VL53L1X.cpp@2:ee9bc2e6dffd, 2019-04-19 (annotated)

Committer:
jvfausto
Date:
Fri Apr 19 23:03:55 2019 +0000
Revision:
2:ee9bc2e6dffd
Parent:
1:0038ad0a63af 
a

Who changed what in which revision?

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