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