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