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