윤 인재 / VL53L0X

dd

VL53L0X.cpp@0:71222a1e3c17, 2020-08-27 (annotated)

Committer:
injaeyoon
Date:
Thu Aug 27 03:11:34 2020 +0000
Revision:
0:71222a1e3c17
Thanks

Who changed what in which revision?

UserRevisionLine numberNew contents of line
injaeyoon 0:71222a1e3c17 1 // ================== VL53L0x library for Mbed ====================
injaeyoon 0:71222a1e3c17 2 // Modified by Alex Park 10, Aug, 2019
injaeyoon 0:71222a1e3c17 3 // Tested with NUCLEO F401RE board
injaeyoon 0:71222a1e3c17 4 // This VL53L0x library was modified from that of the Arduino made by Polou
injaeyoon 0:71222a1e3c17 5 // Firstly, constructor of the class was updated to cope with interrupt handling.
injaeyoon 0:71222a1e3c17 6 // Secondly, API of Timer,InterruptIn and I2C class are accomodated to use mbed OS5.
injaeyoon 0:71222a1e3c17 7 // Most of the functionality of this library is based on the VL53L0X API
injaeyoon 0:71222a1e3c17 8 // provided by ST (STSW-IMG005), and some of the explanatory comments are quoted
injaeyoon 0:71222a1e3c17 9 // or paraphrased from the API source code, API user manual (UM2039), and the
injaeyoon 0:71222a1e3c17 10 // VL53L0X datasheet.
injaeyoon 0:71222a1e3c17 11
injaeyoon 0:71222a1e3c17 12 #include <VL53L0X.h>
injaeyoon 0:71222a1e3c17 13
injaeyoon 0:71222a1e3c17 14 // Defines /////////////////////////////////////////////////////////////////////
injaeyoon 0:71222a1e3c17 15
injaeyoon 0:71222a1e3c17 16 // The Arduino two-wire interface uses a 7-bit number for the address,
injaeyoon 0:71222a1e3c17 17 // and sets the last bit correctly based on reads and writes
injaeyoon 0:71222a1e3c17 18 #define ADDRESS_DEFAULT 0b0101001
injaeyoon 0:71222a1e3c17 19
injaeyoon 0:71222a1e3c17 20 #if VL53L0X_RTOS_KERNEL_MS_TICK
injaeyoon 0:71222a1e3c17 21
injaeyoon 0:71222a1e3c17 22 // Record the current time to check an upcoming timeout against
injaeyoon 0:71222a1e3c17 23 #define startTimeout() (timeout_start_ms = rtos::Kernel::get_ms_count())
injaeyoon 0:71222a1e3c17 24
injaeyoon 0:71222a1e3c17 25 // Check if timeout is enabled (set to nonzero value) and has expired
injaeyoon 0:71222a1e3c17 26 #define checkTimeoutExpired() (io_timeout > 0 && (rtos::Kernel::get_ms_count() - timeout_start_ms) > io_timeout)
injaeyoon 0:71222a1e3c17 27
injaeyoon 0:71222a1e3c17 28 #else
injaeyoon 0:71222a1e3c17 29
injaeyoon 0:71222a1e3c17 30 // Record the current time to check an upcoming timeout against
injaeyoon 0:71222a1e3c17 31 #define startTimeout() (timeout_start_ms = _tmr.read_ms())
injaeyoon 0:71222a1e3c17 32
injaeyoon 0:71222a1e3c17 33 // Check if timeout is enabled (set to nonzero value) and has expired
injaeyoon 0:71222a1e3c17 34 #define checkTimeoutExpired() (io_timeout > 0 && (_tmr.read_ms() - timeout_start_ms) > io_timeout)
injaeyoon 0:71222a1e3c17 35
injaeyoon 0:71222a1e3c17 36 #endif
injaeyoon 0:71222a1e3c17 37
injaeyoon 0:71222a1e3c17 38
injaeyoon 0:71222a1e3c17 39 // Decode VCSEL (vertical cavity surface emitting laser) pulse period in PCLKs
injaeyoon 0:71222a1e3c17 40 // from register value
injaeyoon 0:71222a1e3c17 41 // based on VL53L0X_decode_vcsel_period()
injaeyoon 0:71222a1e3c17 42 #define decodeVcselPeriod(reg_val) (((reg_val) + 1) << 1)
injaeyoon 0:71222a1e3c17 43
injaeyoon 0:71222a1e3c17 44 // Encode VCSEL pulse period register value from period in PCLKs
injaeyoon 0:71222a1e3c17 45 // based on VL53L0X_encode_vcsel_period()
injaeyoon 0:71222a1e3c17 46 #define encodeVcselPeriod(period_pclks) (((period_pclks) >> 1) - 1)
injaeyoon 0:71222a1e3c17 47
injaeyoon 0:71222a1e3c17 48 // Calculate macro period in *nanoseconds* from VCSEL period in PCLKs
injaeyoon 0:71222a1e3c17 49 // based on VL53L0X_calc_macro_period_ps()
injaeyoon 0:71222a1e3c17 50 // PLL_period_ps = 1655; macro_period_vclks = 2304
injaeyoon 0:71222a1e3c17 51 #define calcMacroPeriod(vcsel_period_pclks) ((((uint32_t)2304 * (vcsel_period_pclks) * 1655) + 500) / 1000)
injaeyoon 0:71222a1e3c17 52
injaeyoon 0:71222a1e3c17 53 // Constructors ////////////////////////////////////////////////////////////////
injaeyoon 0:71222a1e3c17 54
injaeyoon 0:71222a1e3c17 55 //VL53L0X::VL53L0X(I2C & i2c_instance)
injaeyoon 0:71222a1e3c17 56 // : _i2c(i2c_instance)
injaeyoon 0:71222a1e3c17 57 // , address(ADDRESS_DEFAULT<<1)
injaeyoon 0:71222a1e3c17 58 // , io_timeout(0) // no timeout
injaeyoon 0:71222a1e3c17 59 // , _wasTimeout(false)
injaeyoon 0:71222a1e3c17 60 //{}
injaeyoon 0:71222a1e3c17 61 VL53L0X::VL53L0X(PinName SDA,PinName SCL, PinName INTRT)
injaeyoon 0:71222a1e3c17 62 : _i2c(SDA,SCL)
injaeyoon 0:71222a1e3c17 63 , _int(INTRT)
injaeyoon 0:71222a1e3c17 64 , address(ADDRESS_DEFAULT<<1)
injaeyoon 0:71222a1e3c17 65 , io_timeout(0) // no timeout
injaeyoon 0:71222a1e3c17 66 , _range(0)
injaeyoon 0:71222a1e3c17 67 , _wasTimeout(false)
injaeyoon 0:71222a1e3c17 68 , _dataRdy(false)
injaeyoon 0:71222a1e3c17 69 {}
injaeyoon 0:71222a1e3c17 70
injaeyoon 0:71222a1e3c17 71 // Public Methods //////////////////////////////////////////////////////////////
injaeyoon 0:71222a1e3c17 72 void VL53L0X::setApplication(applicationType mode){
injaeyoon 0:71222a1e3c17 73 if ( LONG_FAST || LONG_ACCURATE){
injaeyoon 0:71222a1e3c17 74 // lower the return signal rate limit (default is 0.25 MCPS)
injaeyoon 0:71222a1e3c17 75 setSignalRateLimit(0.1);
injaeyoon 0:71222a1e3c17 76 // increase laser pulse periods (defaults are 14 and 10 PCLKs)
injaeyoon 0:71222a1e3c17 77 setVcselPulsePeriod(VL53L0X::VcselPeriodPreRange, 18);
injaeyoon 0:71222a1e3c17 78 setVcselPulsePeriod(VL53L0X::VcselPeriodFinalRange, 14);
injaeyoon 0:71222a1e3c17 79 }
injaeyoon 0:71222a1e3c17 80 if( LONG_FAST || SHORT_FAST) // reduce timing budget to 20 ms (default is about 33 ms)
injaeyoon 0:71222a1e3c17 81 setMeasurementTimingBudget(20000); // minimum timing budget 20 ms
injaeyoon 0:71222a1e3c17 82 else // increase timing budget to 200 ms
injaeyoon 0:71222a1e3c17 83 setMeasurementTimingBudget(200000);
injaeyoon 0:71222a1e3c17 84 }
injaeyoon 0:71222a1e3c17 85 void VL53L0X::setAddress(uint8_t new_addr)
injaeyoon 0:71222a1e3c17 86 {
injaeyoon 0:71222a1e3c17 87 writeReg(I2C_SLAVE_DEVICE_ADDRESS, new_addr & 0x7F);
injaeyoon 0:71222a1e3c17 88 address = new_addr<<1;
injaeyoon 0:71222a1e3c17 89 }
injaeyoon 0:71222a1e3c17 90
injaeyoon 0:71222a1e3c17 91 // Initialize sensor using sequence based on VL53L0X_DataInit(),
injaeyoon 0:71222a1e3c17 92 // VL53L0X_StaticInit(), and VL53L0X_PerformRefCalibration().
injaeyoon 0:71222a1e3c17 93 // This function does not perform reference SPAD calibration
injaeyoon 0:71222a1e3c17 94 // (VL53L0X_PerformRefSpadManagement()), since the API user manual says that it
injaeyoon 0:71222a1e3c17 95 // is performed by ST on the bare modules; it seems like that should work well
injaeyoon 0:71222a1e3c17 96 // enough unless a cover glass is added.
injaeyoon 0:71222a1e3c17 97 // If io_2v8 (optional) is true or not given, the sensor is configured for 2V8
injaeyoon 0:71222a1e3c17 98 // mode.
injaeyoon 0:71222a1e3c17 99 bool VL53L0X::init(bool io_2v8)
injaeyoon 0:71222a1e3c17 100 {
injaeyoon 0:71222a1e3c17 101 // VL53L0X_DataInit() begin
injaeyoon 0:71222a1e3c17 102 _tmr.start();
injaeyoon 0:71222a1e3c17 103 // sensor uses 1V8 mode for I/O by default; switch to 2V8 mode if necessary
injaeyoon 0:71222a1e3c17 104 if (io_2v8)
injaeyoon 0:71222a1e3c17 105 {
injaeyoon 0:71222a1e3c17 106 writeReg(VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV,
injaeyoon 0:71222a1e3c17 107 readReg(VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV) | 0x01); // set bit 0
injaeyoon 0:71222a1e3c17 108 }
injaeyoon 0:71222a1e3c17 109 // "Set I2C standard mode"
injaeyoon 0:71222a1e3c17 110 writeReg(0x88, 0x00);
injaeyoon 0:71222a1e3c17 111
injaeyoon 0:71222a1e3c17 112 writeReg(0x80, 0x01);
injaeyoon 0:71222a1e3c17 113 writeReg(0xFF, 0x01);
injaeyoon 0:71222a1e3c17 114 writeReg(0x00, 0x00);
injaeyoon 0:71222a1e3c17 115 stop_variable = readReg(0x91);
injaeyoon 0:71222a1e3c17 116 writeReg(0x00, 0x01);
injaeyoon 0:71222a1e3c17 117 writeReg(0xFF, 0x00);
injaeyoon 0:71222a1e3c17 118 writeReg(0x80, 0x00);
injaeyoon 0:71222a1e3c17 119
injaeyoon 0:71222a1e3c17 120 // disable SIGNAL_RATE_MSRC (bit 1) and SIGNAL_RATE_PRE_RANGE (bit 4) limit checks
injaeyoon 0:71222a1e3c17 121 writeReg(MSRC_CONFIG_CONTROL, readReg(MSRC_CONFIG_CONTROL) | 0x12);
injaeyoon 0:71222a1e3c17 122
injaeyoon 0:71222a1e3c17 123 // set final range signal rate limit to 0.25 MCPS (million counts per second)
injaeyoon 0:71222a1e3c17 124 setSignalRateLimit(0.25);
injaeyoon 0:71222a1e3c17 125
injaeyoon 0:71222a1e3c17 126 writeReg(SYSTEM_SEQUENCE_CONFIG, 0xFF);
injaeyoon 0:71222a1e3c17 127
injaeyoon 0:71222a1e3c17 128 // VL53L0X_DataInit() end
injaeyoon 0:71222a1e3c17 129
injaeyoon 0:71222a1e3c17 130 // VL53L0X_StaticInit() begin
injaeyoon 0:71222a1e3c17 131
injaeyoon 0:71222a1e3c17 132 uint8_t spad_count;
injaeyoon 0:71222a1e3c17 133 bool spad_type_is_aperture;
injaeyoon 0:71222a1e3c17 134 if (!getSpadInfo(&spad_count, &spad_type_is_aperture)) { return false; }
injaeyoon 0:71222a1e3c17 135
injaeyoon 0:71222a1e3c17 136 // The SPAD map (RefGoodSpadMap) is read by VL53L0X_get_info_from_device() in
injaeyoon 0:71222a1e3c17 137 // the API, but the same data seems to be more easily readable from
injaeyoon 0:71222a1e3c17 138 // GLOBAL_CONFIG_SPAD_ENABLES_REF_0 through _6, so read it from there
injaeyoon 0:71222a1e3c17 139 uint8_t ref_spad_map[6];
injaeyoon 0:71222a1e3c17 140 readMulti(GLOBAL_CONFIG_SPAD_ENABLES_REF_0, ref_spad_map, 6);
injaeyoon 0:71222a1e3c17 141
injaeyoon 0:71222a1e3c17 142 // -- VL53L0X_set_reference_spads() begin (assume NVM values are valid)
injaeyoon 0:71222a1e3c17 143
injaeyoon 0:71222a1e3c17 144 writeReg(0xFF, 0x01);
injaeyoon 0:71222a1e3c17 145 writeReg(DYNAMIC_SPAD_REF_EN_START_OFFSET, 0x00);
injaeyoon 0:71222a1e3c17 146 writeReg(DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD, 0x2C);
injaeyoon 0:71222a1e3c17 147 writeReg(0xFF, 0x00);
injaeyoon 0:71222a1e3c17 148 writeReg(GLOBAL_CONFIG_REF_EN_START_SELECT, 0xB4);
injaeyoon 0:71222a1e3c17 149
injaeyoon 0:71222a1e3c17 150 uint8_t first_spad_to_enable = spad_type_is_aperture ? 12 : 0; // 12 is the first aperture spad
injaeyoon 0:71222a1e3c17 151 uint8_t spads_enabled = 0;
injaeyoon 0:71222a1e3c17 152
injaeyoon 0:71222a1e3c17 153 for (uint8_t i = 0; i < 48; i++)
injaeyoon 0:71222a1e3c17 154 {
injaeyoon 0:71222a1e3c17 155 if (i < first_spad_to_enable || spads_enabled == spad_count)
injaeyoon 0:71222a1e3c17 156 {
injaeyoon 0:71222a1e3c17 157 // This bit is lower than the first one that should be enabled, or
injaeyoon 0:71222a1e3c17 158 // (reference_spad_count) bits have already been enabled, so zero this bit
injaeyoon 0:71222a1e3c17 159 ref_spad_map[i / 8] &= ~(1 << (i % 8));
injaeyoon 0:71222a1e3c17 160 }
injaeyoon 0:71222a1e3c17 161 else if ((ref_spad_map[i / 8] >> (i % 8)) & 0x1)
injaeyoon 0:71222a1e3c17 162 {
injaeyoon 0:71222a1e3c17 163 spads_enabled++;
injaeyoon 0:71222a1e3c17 164 }
injaeyoon 0:71222a1e3c17 165 }
injaeyoon 0:71222a1e3c17 166
injaeyoon 0:71222a1e3c17 167 writeMulti(GLOBAL_CONFIG_SPAD_ENABLES_REF_0, ref_spad_map, 6);
injaeyoon 0:71222a1e3c17 168
injaeyoon 0:71222a1e3c17 169 // -- VL53L0X_set_reference_spads() end
injaeyoon 0:71222a1e3c17 170
injaeyoon 0:71222a1e3c17 171 // -- VL53L0X_load_tuning_settings() begin
injaeyoon 0:71222a1e3c17 172 // DefaultTuningSettings from vl53l0x_tuning.h
injaeyoon 0:71222a1e3c17 173
injaeyoon 0:71222a1e3c17 174 writeReg(0xFF, 0x01);
injaeyoon 0:71222a1e3c17 175 writeReg(0x00, 0x00);
injaeyoon 0:71222a1e3c17 176
injaeyoon 0:71222a1e3c17 177 writeReg(0xFF, 0x00);
injaeyoon 0:71222a1e3c17 178 writeReg(0x09, 0x00);
injaeyoon 0:71222a1e3c17 179 writeReg(0x10, 0x00);
injaeyoon 0:71222a1e3c17 180 writeReg(0x11, 0x00);
injaeyoon 0:71222a1e3c17 181
injaeyoon 0:71222a1e3c17 182 writeReg(0x24, 0x01);
injaeyoon 0:71222a1e3c17 183 writeReg(0x25, 0xFF);
injaeyoon 0:71222a1e3c17 184 writeReg(0x75, 0x00);
injaeyoon 0:71222a1e3c17 185
injaeyoon 0:71222a1e3c17 186 writeReg(0xFF, 0x01);
injaeyoon 0:71222a1e3c17 187 writeReg(0x4E, 0x2C);
injaeyoon 0:71222a1e3c17 188 writeReg(0x48, 0x00);
injaeyoon 0:71222a1e3c17 189 writeReg(0x30, 0x20);
injaeyoon 0:71222a1e3c17 190
injaeyoon 0:71222a1e3c17 191 writeReg(0xFF, 0x00);
injaeyoon 0:71222a1e3c17 192 writeReg(0x30, 0x09);
injaeyoon 0:71222a1e3c17 193 writeReg(0x54, 0x00);
injaeyoon 0:71222a1e3c17 194 writeReg(0x31, 0x04);
injaeyoon 0:71222a1e3c17 195 writeReg(0x32, 0x03);
injaeyoon 0:71222a1e3c17 196 writeReg(0x40, 0x83);
injaeyoon 0:71222a1e3c17 197 writeReg(0x46, 0x25);
injaeyoon 0:71222a1e3c17 198 writeReg(0x60, 0x00);
injaeyoon 0:71222a1e3c17 199 writeReg(0x27, 0x00);
injaeyoon 0:71222a1e3c17 200 writeReg(0x50, 0x06);
injaeyoon 0:71222a1e3c17 201 writeReg(0x51, 0x00);
injaeyoon 0:71222a1e3c17 202 writeReg(0x52, 0x96);
injaeyoon 0:71222a1e3c17 203 writeReg(0x56, 0x08);
injaeyoon 0:71222a1e3c17 204 writeReg(0x57, 0x30);
injaeyoon 0:71222a1e3c17 205 writeReg(0x61, 0x00);
injaeyoon 0:71222a1e3c17 206 writeReg(0x62, 0x00);
injaeyoon 0:71222a1e3c17 207 writeReg(0x64, 0x00);
injaeyoon 0:71222a1e3c17 208 writeReg(0x65, 0x00);
injaeyoon 0:71222a1e3c17 209 writeReg(0x66, 0xA0);
injaeyoon 0:71222a1e3c17 210
injaeyoon 0:71222a1e3c17 211 writeReg(0xFF, 0x01);
injaeyoon 0:71222a1e3c17 212 writeReg(0x22, 0x32);
injaeyoon 0:71222a1e3c17 213 writeReg(0x47, 0x14);
injaeyoon 0:71222a1e3c17 214 writeReg(0x49, 0xFF);
injaeyoon 0:71222a1e3c17 215 writeReg(0x4A, 0x00);
injaeyoon 0:71222a1e3c17 216
injaeyoon 0:71222a1e3c17 217 writeReg(0xFF, 0x00);
injaeyoon 0:71222a1e3c17 218 writeReg(0x7A, 0x0A);
injaeyoon 0:71222a1e3c17 219 writeReg(0x7B, 0x00);
injaeyoon 0:71222a1e3c17 220 writeReg(0x78, 0x21);
injaeyoon 0:71222a1e3c17 221
injaeyoon 0:71222a1e3c17 222 writeReg(0xFF, 0x01);
injaeyoon 0:71222a1e3c17 223 writeReg(0x23, 0x34);
injaeyoon 0:71222a1e3c17 224 writeReg(0x42, 0x00);
injaeyoon 0:71222a1e3c17 225 writeReg(0x44, 0xFF);
injaeyoon 0:71222a1e3c17 226 writeReg(0x45, 0x26);
injaeyoon 0:71222a1e3c17 227 writeReg(0x46, 0x05);
injaeyoon 0:71222a1e3c17 228 writeReg(0x40, 0x40);
injaeyoon 0:71222a1e3c17 229 writeReg(0x0E, 0x06);
injaeyoon 0:71222a1e3c17 230 writeReg(0x20, 0x1A);
injaeyoon 0:71222a1e3c17 231 writeReg(0x43, 0x40);
injaeyoon 0:71222a1e3c17 232
injaeyoon 0:71222a1e3c17 233 writeReg(0xFF, 0x00);
injaeyoon 0:71222a1e3c17 234 writeReg(0x34, 0x03);
injaeyoon 0:71222a1e3c17 235 writeReg(0x35, 0x44);
injaeyoon 0:71222a1e3c17 236
injaeyoon 0:71222a1e3c17 237 writeReg(0xFF, 0x01);
injaeyoon 0:71222a1e3c17 238 writeReg(0x31, 0x04);
injaeyoon 0:71222a1e3c17 239 writeReg(0x4B, 0x09);
injaeyoon 0:71222a1e3c17 240 writeReg(0x4C, 0x05);
injaeyoon 0:71222a1e3c17 241 writeReg(0x4D, 0x04);
injaeyoon 0:71222a1e3c17 242
injaeyoon 0:71222a1e3c17 243 writeReg(0xFF, 0x00);
injaeyoon 0:71222a1e3c17 244 writeReg(0x44, 0x00);
injaeyoon 0:71222a1e3c17 245 writeReg(0x45, 0x20);
injaeyoon 0:71222a1e3c17 246 writeReg(0x47, 0x08);
injaeyoon 0:71222a1e3c17 247 writeReg(0x48, 0x28);
injaeyoon 0:71222a1e3c17 248 writeReg(0x67, 0x00);
injaeyoon 0:71222a1e3c17 249 writeReg(0x70, 0x04);
injaeyoon 0:71222a1e3c17 250 writeReg(0x71, 0x01);
injaeyoon 0:71222a1e3c17 251 writeReg(0x72, 0xFE);
injaeyoon 0:71222a1e3c17 252 writeReg(0x76, 0x00);
injaeyoon 0:71222a1e3c17 253 writeReg(0x77, 0x00);
injaeyoon 0:71222a1e3c17 254
injaeyoon 0:71222a1e3c17 255 writeReg(0xFF, 0x01);
injaeyoon 0:71222a1e3c17 256 writeReg(0x0D, 0x01);
injaeyoon 0:71222a1e3c17 257
injaeyoon 0:71222a1e3c17 258 writeReg(0xFF, 0x00);
injaeyoon 0:71222a1e3c17 259 writeReg(0x80, 0x01);
injaeyoon 0:71222a1e3c17 260 writeReg(0x01, 0xF8);
injaeyoon 0:71222a1e3c17 261
injaeyoon 0:71222a1e3c17 262 writeReg(0xFF, 0x01);
injaeyoon 0:71222a1e3c17 263 writeReg(0x8E, 0x01);
injaeyoon 0:71222a1e3c17 264 writeReg(0x00, 0x01);
injaeyoon 0:71222a1e3c17 265 writeReg(0xFF, 0x00);
injaeyoon 0:71222a1e3c17 266 writeReg(0x80, 0x00);
injaeyoon 0:71222a1e3c17 267
injaeyoon 0:71222a1e3c17 268 // -- VL53L0X_load_tuning_settings() end
injaeyoon 0:71222a1e3c17 269
injaeyoon 0:71222a1e3c17 270 // "Set interrupt config to new sample ready"
injaeyoon 0:71222a1e3c17 271 // -- VL53L0X_SetGpioConfig() begin
injaeyoon 0:71222a1e3c17 272
injaeyoon 0:71222a1e3c17 273 writeReg(SYSTEM_INTERRUPT_CONFIG_GPIO, 0x04);
injaeyoon 0:71222a1e3c17 274 writeReg(GPIO_HV_MUX_ACTIVE_HIGH, readReg(GPIO_HV_MUX_ACTIVE_HIGH) & ~0x10); // active low
injaeyoon 0:71222a1e3c17 275 writeReg(SYSTEM_INTERRUPT_CLEAR, 0x01);
injaeyoon 0:71222a1e3c17 276
injaeyoon 0:71222a1e3c17 277 // -- VL53L0X_SetGpioConfig() end
injaeyoon 0:71222a1e3c17 278
injaeyoon 0:71222a1e3c17 279 measurement_timing_budget_us = getMeasurementTimingBudget();
injaeyoon 0:71222a1e3c17 280
injaeyoon 0:71222a1e3c17 281 // "Disable MSRC and TCC by default"
injaeyoon 0:71222a1e3c17 282 // MSRC = Minimum Signal Rate Check
injaeyoon 0:71222a1e3c17 283 // TCC = Target CentreCheck
injaeyoon 0:71222a1e3c17 284 // -- VL53L0X_SetSequenceStepEnable() begin
injaeyoon 0:71222a1e3c17 285
injaeyoon 0:71222a1e3c17 286 writeReg(SYSTEM_SEQUENCE_CONFIG, 0xE8);
injaeyoon 0:71222a1e3c17 287
injaeyoon 0:71222a1e3c17 288 // -- VL53L0X_SetSequenceStepEnable() end
injaeyoon 0:71222a1e3c17 289
injaeyoon 0:71222a1e3c17 290 // "Recalculate timing budget"
injaeyoon 0:71222a1e3c17 291 setMeasurementTimingBudget(measurement_timing_budget_us);
injaeyoon 0:71222a1e3c17 292
injaeyoon 0:71222a1e3c17 293 // VL53L0X_StaticInit() end
injaeyoon 0:71222a1e3c17 294
injaeyoon 0:71222a1e3c17 295 // VL53L0X_PerformRefCalibration() begin (VL53L0X_perform_ref_calibration())
injaeyoon 0:71222a1e3c17 296
injaeyoon 0:71222a1e3c17 297 // -- VL53L0X_perform_vhv_calibration() begin
injaeyoon 0:71222a1e3c17 298
injaeyoon 0:71222a1e3c17 299 writeReg(SYSTEM_SEQUENCE_CONFIG, 0x01);
injaeyoon 0:71222a1e3c17 300 if (!performSingleRefCalibration(0x40)) { return false; }
injaeyoon 0:71222a1e3c17 301
injaeyoon 0:71222a1e3c17 302 // -- VL53L0X_perform_vhv_calibration() end
injaeyoon 0:71222a1e3c17 303
injaeyoon 0:71222a1e3c17 304 // -- VL53L0X_perform_phase_calibration() begin
injaeyoon 0:71222a1e3c17 305
injaeyoon 0:71222a1e3c17 306 writeReg(SYSTEM_SEQUENCE_CONFIG, 0x02);
injaeyoon 0:71222a1e3c17 307 if (!performSingleRefCalibration(0x00)) { return false; }
injaeyoon 0:71222a1e3c17 308
injaeyoon 0:71222a1e3c17 309 // -- VL53L0X_perform_phase_calibration() end
injaeyoon 0:71222a1e3c17 310
injaeyoon 0:71222a1e3c17 311 // "restore the previous Sequence Config"
injaeyoon 0:71222a1e3c17 312 writeReg(SYSTEM_SEQUENCE_CONFIG, 0xE8);
injaeyoon 0:71222a1e3c17 313
injaeyoon 0:71222a1e3c17 314 // VL53L0X_PerformRefCalibration() end
injaeyoon 0:71222a1e3c17 315
injaeyoon 0:71222a1e3c17 316 return true;
injaeyoon 0:71222a1e3c17 317 }
injaeyoon 0:71222a1e3c17 318
injaeyoon 0:71222a1e3c17 319 // Write an 8-bit register
injaeyoon 0:71222a1e3c17 320 void VL53L0X::writeReg(uint8_t reg, uint8_t value)
injaeyoon 0:71222a1e3c17 321 {
injaeyoon 0:71222a1e3c17 322 buffer[0] = reg;
injaeyoon 0:71222a1e3c17 323 buffer[1] = value;
injaeyoon 0:71222a1e3c17 324 last_status = _i2c.write(address, (const char *)buffer, 2, 0);
injaeyoon 0:71222a1e3c17 325 }
injaeyoon 0:71222a1e3c17 326
injaeyoon 0:71222a1e3c17 327 // Write a 16-bit register
injaeyoon 0:71222a1e3c17 328 void VL53L0X::writeReg16Bit(uint8_t reg, uint16_t value)
injaeyoon 0:71222a1e3c17 329 {
injaeyoon 0:71222a1e3c17 330 buffer[0]=reg;
injaeyoon 0:71222a1e3c17 331 buffer[1]=(value >> 8) & 0xFF;
injaeyoon 0:71222a1e3c17 332 buffer[2]=value & 0xFF;
injaeyoon 0:71222a1e3c17 333 last_status = _i2c.write(address, (const char *)buffer, 3, 0);
injaeyoon 0:71222a1e3c17 334 }
injaeyoon 0:71222a1e3c17 335
injaeyoon 0:71222a1e3c17 336 // Write a 32-bit register
injaeyoon 0:71222a1e3c17 337 void VL53L0X::writeReg32Bit(uint8_t reg, uint32_t value)
injaeyoon 0:71222a1e3c17 338 {
injaeyoon 0:71222a1e3c17 339 buffer[0] = reg;
injaeyoon 0:71222a1e3c17 340 buffer[1] = (value >> 24) & 0xFF;
injaeyoon 0:71222a1e3c17 341 buffer[2] = (value >> 16) & 0xFF;
injaeyoon 0:71222a1e3c17 342 buffer[3] = (value >> 8) & 0xFF;
injaeyoon 0:71222a1e3c17 343 buffer[4] = value & 0xFF;
injaeyoon 0:71222a1e3c17 344 last_status = _i2c.write(address, (const char *)buffer, 5, 0);
injaeyoon 0:71222a1e3c17 345 }
injaeyoon 0:71222a1e3c17 346
injaeyoon 0:71222a1e3c17 347 // Read an 8-bit register
injaeyoon 0:71222a1e3c17 348 uint8_t VL53L0X::readReg(uint8_t reg)
injaeyoon 0:71222a1e3c17 349 {
injaeyoon 0:71222a1e3c17 350 uint8_t value;
injaeyoon 0:71222a1e3c17 351 const char REG[] = {reg};
injaeyoon 0:71222a1e3c17 352 _i2c.write(address, REG, 1,0);
injaeyoon 0:71222a1e3c17 353 last_status = _i2c.read(address, (char*)&value,1,0);
injaeyoon 0:71222a1e3c17 354 return value;
injaeyoon 0:71222a1e3c17 355 }
injaeyoon 0:71222a1e3c17 356
injaeyoon 0:71222a1e3c17 357 uint16_t VL53L0X::readReg16Bit(uint8_t reg)
injaeyoon 0:71222a1e3c17 358 {
injaeyoon 0:71222a1e3c17 359 uint16_t value;
injaeyoon 0:71222a1e3c17 360 const char REG[] = {reg};
injaeyoon 0:71222a1e3c17 361 _i2c.write(address, REG, 1,0);
injaeyoon 0:71222a1e3c17 362 last_status = _i2c.read(address, (char*)buffer,2,0);
injaeyoon 0:71222a1e3c17 363 value = (uint16_t)buffer[0] << 8; // value high byte
injaeyoon 0:71222a1e3c17 364 value |= buffer[1]; // value low byte
injaeyoon 0:71222a1e3c17 365 return value;
injaeyoon 0:71222a1e3c17 366 }
injaeyoon 0:71222a1e3c17 367
injaeyoon 0:71222a1e3c17 368 // Read a 32-bit register
injaeyoon 0:71222a1e3c17 369 uint32_t VL53L0X::readReg32Bit(uint8_t reg)
injaeyoon 0:71222a1e3c17 370 {
injaeyoon 0:71222a1e3c17 371 uint32_t value;
injaeyoon 0:71222a1e3c17 372 const char REG[] = {reg};
injaeyoon 0:71222a1e3c17 373 _i2c.write(address, REG, 1,0);
injaeyoon 0:71222a1e3c17 374 last_status = _i2c.read(address, (char*)buffer,4,0);
injaeyoon 0:71222a1e3c17 375 value = (uint32_t)buffer[0] << 24; // value highest byte
injaeyoon 0:71222a1e3c17 376 value |= (uint32_t)buffer[1] << 16;
injaeyoon 0:71222a1e3c17 377 value |= (uint16_t)buffer[2] << 8;
injaeyoon 0:71222a1e3c17 378 value |= buffer[3]; // value lowest byte
injaeyoon 0:71222a1e3c17 379 return value;
injaeyoon 0:71222a1e3c17 380 }
injaeyoon 0:71222a1e3c17 381
injaeyoon 0:71222a1e3c17 382 // Write an arbitrary number of bytes from the given array to the sensor,
injaeyoon 0:71222a1e3c17 383 // starting at the given register
injaeyoon 0:71222a1e3c17 384 void VL53L0X::writeMulti(uint8_t reg, uint8_t const * src, uint8_t count)
injaeyoon 0:71222a1e3c17 385 {
injaeyoon 0:71222a1e3c17 386 buffer[0]=reg;
injaeyoon 0:71222a1e3c17 387 for(int i=1;i<count+1;++i)
injaeyoon 0:71222a1e3c17 388 buffer[i]=*(src+i-1);
injaeyoon 0:71222a1e3c17 389 last_status = _i2c.write(address, (const char*)buffer, count+1, 0);
injaeyoon 0:71222a1e3c17 390 }
injaeyoon 0:71222a1e3c17 391
injaeyoon 0:71222a1e3c17 392 // Read an arbitrary number of bytes from the sensor, starting at the given
injaeyoon 0:71222a1e3c17 393 // register, into the given array
injaeyoon 0:71222a1e3c17 394 void VL53L0X::readMulti(uint8_t reg, uint8_t * dst, uint8_t count)
injaeyoon 0:71222a1e3c17 395 {
injaeyoon 0:71222a1e3c17 396 const char REG[] = {reg};
injaeyoon 0:71222a1e3c17 397 _i2c.write(address, REG, 1,0);
injaeyoon 0:71222a1e3c17 398 last_status = _i2c.read(address, (char*)dst,count,0);
injaeyoon 0:71222a1e3c17 399 }
injaeyoon 0:71222a1e3c17 400
injaeyoon 0:71222a1e3c17 401 // Set the return signal rate limit check value in units of MCPS (mega counts
injaeyoon 0:71222a1e3c17 402 // per second). "This represents the amplitude of the signal reflected from the
injaeyoon 0:71222a1e3c17 403 // target and detected by the device"; setting this limit presumably determines
injaeyoon 0:71222a1e3c17 404 // the minimum measurement necessary for the sensor to report a valid reading.
injaeyoon 0:71222a1e3c17 405 // Setting a lower limit increases the potential range of the sensor but also
injaeyoon 0:71222a1e3c17 406 // seems to increase the likelihood of getting an inaccurate reading because of
injaeyoon 0:71222a1e3c17 407 // unwanted reflections from objects other than the intended target.
injaeyoon 0:71222a1e3c17 408 // Defaults to 0.25 MCPS as initialized by the ST API and this library.
injaeyoon 0:71222a1e3c17 409 bool VL53L0X::setSignalRateLimit(float limit_Mcps)
injaeyoon 0:71222a1e3c17 410 {
injaeyoon 0:71222a1e3c17 411 if (limit_Mcps < 0.0f || limit_Mcps > 511.99f) { return false; }
injaeyoon 0:71222a1e3c17 412
injaeyoon 0:71222a1e3c17 413 // Q9.7 fixed point format (9 integer bits, 7 fractional bits)
injaeyoon 0:71222a1e3c17 414 writeReg16Bit(FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT, limit_Mcps * (1 << 7));
injaeyoon 0:71222a1e3c17 415 return true;
injaeyoon 0:71222a1e3c17 416 }
injaeyoon 0:71222a1e3c17 417
injaeyoon 0:71222a1e3c17 418 // Get the return signal rate limit check value in MCPS
injaeyoon 0:71222a1e3c17 419 float VL53L0X::getSignalRateLimit(void)
injaeyoon 0:71222a1e3c17 420 {
injaeyoon 0:71222a1e3c17 421 return (float)readReg16Bit(FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT) / (1 << 7);
injaeyoon 0:71222a1e3c17 422 }
injaeyoon 0:71222a1e3c17 423
injaeyoon 0:71222a1e3c17 424 // Set the measurement timing budget in microseconds, which is the time allowed
injaeyoon 0:71222a1e3c17 425 // for one measurement; the ST API and this library take care of splitting the
injaeyoon 0:71222a1e3c17 426 // timing budget among the sub-steps in the ranging sequence. A longer timing
injaeyoon 0:71222a1e3c17 427 // budget allows for more accurate measurements. Increasing the budget by a
injaeyoon 0:71222a1e3c17 428 // factor of N decreases the range measurement standard deviation by a factor of
injaeyoon 0:71222a1e3c17 429 // sqrt(N). Defaults to about 33 milliseconds; the minimum is 20 ms.
injaeyoon 0:71222a1e3c17 430 // based on VL53L0X_set_measurement_timing_budget_micro_seconds()
injaeyoon 0:71222a1e3c17 431 bool VL53L0X::setMeasurementTimingBudget(uint32_t budget_us)
injaeyoon 0:71222a1e3c17 432 {
injaeyoon 0:71222a1e3c17 433 SequenceStepEnables enables;
injaeyoon 0:71222a1e3c17 434 SequenceStepTimeouts timeouts;
injaeyoon 0:71222a1e3c17 435
injaeyoon 0:71222a1e3c17 436 uint16_t const StartOverhead = 1320; // note that this is different than the value in get_
injaeyoon 0:71222a1e3c17 437 uint16_t const EndOverhead = 960;
injaeyoon 0:71222a1e3c17 438 uint16_t const MsrcOverhead = 660;
injaeyoon 0:71222a1e3c17 439 uint16_t const TccOverhead = 590;
injaeyoon 0:71222a1e3c17 440 uint16_t const DssOverhead = 690;
injaeyoon 0:71222a1e3c17 441 uint16_t const PreRangeOverhead = 660;
injaeyoon 0:71222a1e3c17 442 uint16_t const FinalRangeOverhead = 550;
injaeyoon 0:71222a1e3c17 443
injaeyoon 0:71222a1e3c17 444 uint32_t const MinTimingBudget = 20000;
injaeyoon 0:71222a1e3c17 445
injaeyoon 0:71222a1e3c17 446 if (budget_us < MinTimingBudget) { return false; }
injaeyoon 0:71222a1e3c17 447
injaeyoon 0:71222a1e3c17 448 uint32_t used_budget_us = StartOverhead + EndOverhead;
injaeyoon 0:71222a1e3c17 449
injaeyoon 0:71222a1e3c17 450 getSequenceStepEnables(&enables);
injaeyoon 0:71222a1e3c17 451 getSequenceStepTimeouts(&enables, &timeouts);
injaeyoon 0:71222a1e3c17 452
injaeyoon 0:71222a1e3c17 453 if (enables.tcc)
injaeyoon 0:71222a1e3c17 454 {
injaeyoon 0:71222a1e3c17 455 used_budget_us += (timeouts.msrc_dss_tcc_us + TccOverhead);
injaeyoon 0:71222a1e3c17 456 }
injaeyoon 0:71222a1e3c17 457
injaeyoon 0:71222a1e3c17 458 if (enables.dss)
injaeyoon 0:71222a1e3c17 459 {
injaeyoon 0:71222a1e3c17 460 used_budget_us += 2 * (timeouts.msrc_dss_tcc_us + DssOverhead);
injaeyoon 0:71222a1e3c17 461 }
injaeyoon 0:71222a1e3c17 462 else if (enables.msrc)
injaeyoon 0:71222a1e3c17 463 {
injaeyoon 0:71222a1e3c17 464 used_budget_us += (timeouts.msrc_dss_tcc_us + MsrcOverhead);
injaeyoon 0:71222a1e3c17 465 }
injaeyoon 0:71222a1e3c17 466
injaeyoon 0:71222a1e3c17 467 if (enables.pre_range)
injaeyoon 0:71222a1e3c17 468 {
injaeyoon 0:71222a1e3c17 469 used_budget_us += (timeouts.pre_range_us + PreRangeOverhead);
injaeyoon 0:71222a1e3c17 470 }
injaeyoon 0:71222a1e3c17 471
injaeyoon 0:71222a1e3c17 472 if (enables.final_range)
injaeyoon 0:71222a1e3c17 473 {
injaeyoon 0:71222a1e3c17 474 used_budget_us += FinalRangeOverhead;
injaeyoon 0:71222a1e3c17 475
injaeyoon 0:71222a1e3c17 476 // "Note that the final range timeout is determined by the timing
injaeyoon 0:71222a1e3c17 477 // budget and the sum of all other timeouts within the sequence.
injaeyoon 0:71222a1e3c17 478 // If there is no room for the final range timeout, then an error
injaeyoon 0:71222a1e3c17 479 // will be set. Otherwise the remaining time will be applied to
injaeyoon 0:71222a1e3c17 480 // the final range."
injaeyoon 0:71222a1e3c17 481
injaeyoon 0:71222a1e3c17 482 if (used_budget_us > budget_us)
injaeyoon 0:71222a1e3c17 483 {
injaeyoon 0:71222a1e3c17 484 // "Requested timeout too big."
injaeyoon 0:71222a1e3c17 485 return false;
injaeyoon 0:71222a1e3c17 486 }
injaeyoon 0:71222a1e3c17 487
injaeyoon 0:71222a1e3c17 488 uint32_t final_range_timeout_us = budget_us - used_budget_us;
injaeyoon 0:71222a1e3c17 489
injaeyoon 0:71222a1e3c17 490 // set_sequence_step_timeout() begin
injaeyoon 0:71222a1e3c17 491 // (SequenceStepId == VL53L0X_SEQUENCESTEP_FINAL_RANGE)
injaeyoon 0:71222a1e3c17 492
injaeyoon 0:71222a1e3c17 493 // "For the final range timeout, the pre-range timeout
injaeyoon 0:71222a1e3c17 494 // must be added. To do this both final and pre-range
injaeyoon 0:71222a1e3c17 495 // timeouts must be expressed in macro periods MClks
injaeyoon 0:71222a1e3c17 496 // because they have different vcsel periods."
injaeyoon 0:71222a1e3c17 497
injaeyoon 0:71222a1e3c17 498 uint16_t final_range_timeout_mclks =
injaeyoon 0:71222a1e3c17 499 timeoutMicrosecondsToMclks(final_range_timeout_us,
injaeyoon 0:71222a1e3c17 500 timeouts.final_range_vcsel_period_pclks);
injaeyoon 0:71222a1e3c17 501
injaeyoon 0:71222a1e3c17 502 if (enables.pre_range)
injaeyoon 0:71222a1e3c17 503 {
injaeyoon 0:71222a1e3c17 504 final_range_timeout_mclks += timeouts.pre_range_mclks;
injaeyoon 0:71222a1e3c17 505 }
injaeyoon 0:71222a1e3c17 506
injaeyoon 0:71222a1e3c17 507 writeReg16Bit(FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI,
injaeyoon 0:71222a1e3c17 508 encodeTimeout(final_range_timeout_mclks));
injaeyoon 0:71222a1e3c17 509
injaeyoon 0:71222a1e3c17 510 // set_sequence_step_timeout() end
injaeyoon 0:71222a1e3c17 511
injaeyoon 0:71222a1e3c17 512 measurement_timing_budget_us = budget_us; // store for internal reuse
injaeyoon 0:71222a1e3c17 513 }
injaeyoon 0:71222a1e3c17 514 return true;
injaeyoon 0:71222a1e3c17 515 }
injaeyoon 0:71222a1e3c17 516
injaeyoon 0:71222a1e3c17 517 // Get the measurement timing budget in microseconds
injaeyoon 0:71222a1e3c17 518 // based on VL53L0X_get_measurement_timing_budget_micro_seconds()
injaeyoon 0:71222a1e3c17 519 // in us
injaeyoon 0:71222a1e3c17 520 uint32_t VL53L0X::getMeasurementTimingBudget(void)
injaeyoon 0:71222a1e3c17 521 {
injaeyoon 0:71222a1e3c17 522 SequenceStepEnables enables;
injaeyoon 0:71222a1e3c17 523 SequenceStepTimeouts timeouts;
injaeyoon 0:71222a1e3c17 524
injaeyoon 0:71222a1e3c17 525 uint16_t const StartOverhead = 1910; // note that this is different than the value in set_
injaeyoon 0:71222a1e3c17 526 uint16_t const EndOverhead = 960;
injaeyoon 0:71222a1e3c17 527 uint16_t const MsrcOverhead = 660;
injaeyoon 0:71222a1e3c17 528 uint16_t const TccOverhead = 590;
injaeyoon 0:71222a1e3c17 529 uint16_t const DssOverhead = 690;
injaeyoon 0:71222a1e3c17 530 uint16_t const PreRangeOverhead = 660;
injaeyoon 0:71222a1e3c17 531 uint16_t const FinalRangeOverhead = 550;
injaeyoon 0:71222a1e3c17 532
injaeyoon 0:71222a1e3c17 533 // "Start and end overhead times always present"
injaeyoon 0:71222a1e3c17 534 uint32_t budget_us = StartOverhead + EndOverhead;
injaeyoon 0:71222a1e3c17 535
injaeyoon 0:71222a1e3c17 536 getSequenceStepEnables(&enables);
injaeyoon 0:71222a1e3c17 537 getSequenceStepTimeouts(&enables, &timeouts);
injaeyoon 0:71222a1e3c17 538
injaeyoon 0:71222a1e3c17 539 if (enables.tcc)
injaeyoon 0:71222a1e3c17 540 {
injaeyoon 0:71222a1e3c17 541 budget_us += (timeouts.msrc_dss_tcc_us + TccOverhead);
injaeyoon 0:71222a1e3c17 542 }
injaeyoon 0:71222a1e3c17 543
injaeyoon 0:71222a1e3c17 544 if (enables.dss)
injaeyoon 0:71222a1e3c17 545 {
injaeyoon 0:71222a1e3c17 546 budget_us += 2 * (timeouts.msrc_dss_tcc_us + DssOverhead);
injaeyoon 0:71222a1e3c17 547 }
injaeyoon 0:71222a1e3c17 548 else if (enables.msrc)
injaeyoon 0:71222a1e3c17 549 {
injaeyoon 0:71222a1e3c17 550 budget_us += (timeouts.msrc_dss_tcc_us + MsrcOverhead);
injaeyoon 0:71222a1e3c17 551 }
injaeyoon 0:71222a1e3c17 552
injaeyoon 0:71222a1e3c17 553 if (enables.pre_range)
injaeyoon 0:71222a1e3c17 554 {
injaeyoon 0:71222a1e3c17 555 budget_us += (timeouts.pre_range_us + PreRangeOverhead);
injaeyoon 0:71222a1e3c17 556 }
injaeyoon 0:71222a1e3c17 557
injaeyoon 0:71222a1e3c17 558 if (enables.final_range)
injaeyoon 0:71222a1e3c17 559 {
injaeyoon 0:71222a1e3c17 560 budget_us += (timeouts.final_range_us + FinalRangeOverhead);
injaeyoon 0:71222a1e3c17 561 }
injaeyoon 0:71222a1e3c17 562
injaeyoon 0:71222a1e3c17 563 measurement_timing_budget_us = budget_us; // store for internal reuse
injaeyoon 0:71222a1e3c17 564 return budget_us;
injaeyoon 0:71222a1e3c17 565 }
injaeyoon 0:71222a1e3c17 566
injaeyoon 0:71222a1e3c17 567 // Set the VCSEL (vertical cavity surface emitting laser) pulse period for the
injaeyoon 0:71222a1e3c17 568 // given period type (pre-range or final range) to the given value in PCLKs.
injaeyoon 0:71222a1e3c17 569 // Longer periods seem to increase the potential range of the sensor.
injaeyoon 0:71222a1e3c17 570 // Valid values are (even numbers only):
injaeyoon 0:71222a1e3c17 571 // pre: 12 to 18 (initialized default: 14)
injaeyoon 0:71222a1e3c17 572 // final: 8 to 14 (initialized default: 10)
injaeyoon 0:71222a1e3c17 573 // based on VL53L0X_set_vcsel_pulse_period()
injaeyoon 0:71222a1e3c17 574 bool VL53L0X::setVcselPulsePeriod(vcselPeriodType type, uint8_t period_pclks)
injaeyoon 0:71222a1e3c17 575 {
injaeyoon 0:71222a1e3c17 576 uint8_t vcsel_period_reg = encodeVcselPeriod(period_pclks);
injaeyoon 0:71222a1e3c17 577
injaeyoon 0:71222a1e3c17 578 SequenceStepEnables enables;
injaeyoon 0:71222a1e3c17 579 SequenceStepTimeouts timeouts;
injaeyoon 0:71222a1e3c17 580
injaeyoon 0:71222a1e3c17 581 getSequenceStepEnables(&enables);
injaeyoon 0:71222a1e3c17 582 getSequenceStepTimeouts(&enables, &timeouts);
injaeyoon 0:71222a1e3c17 583
injaeyoon 0:71222a1e3c17 584 // "Apply specific settings for the requested clock period"
injaeyoon 0:71222a1e3c17 585 // "Re-calculate and apply timeouts, in macro periods"
injaeyoon 0:71222a1e3c17 586
injaeyoon 0:71222a1e3c17 587 // "When the VCSEL period for the pre or final range is changed,
injaeyoon 0:71222a1e3c17 588 // the corresponding timeout must be read from the device using
injaeyoon 0:71222a1e3c17 589 // the current VCSEL period, then the new VCSEL period can be
injaeyoon 0:71222a1e3c17 590 // applied. The timeout then must be written back to the device
injaeyoon 0:71222a1e3c17 591 // using the new VCSEL period.
injaeyoon 0:71222a1e3c17 592 //
injaeyoon 0:71222a1e3c17 593 // For the MSRC timeout, the same applies - this timeout being
injaeyoon 0:71222a1e3c17 594 // dependant on the pre-range vcsel period."
injaeyoon 0:71222a1e3c17 595
injaeyoon 0:71222a1e3c17 596
injaeyoon 0:71222a1e3c17 597 if (type == VcselPeriodPreRange)
injaeyoon 0:71222a1e3c17 598 {
injaeyoon 0:71222a1e3c17 599 // "Set phase check limits"
injaeyoon 0:71222a1e3c17 600 switch (period_pclks)
injaeyoon 0:71222a1e3c17 601 {
injaeyoon 0:71222a1e3c17 602 case 12:
injaeyoon 0:71222a1e3c17 603 writeReg(PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x18);
injaeyoon 0:71222a1e3c17 604 break;
injaeyoon 0:71222a1e3c17 605
injaeyoon 0:71222a1e3c17 606 case 14:
injaeyoon 0:71222a1e3c17 607 writeReg(PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x30);
injaeyoon 0:71222a1e3c17 608 break;
injaeyoon 0:71222a1e3c17 609
injaeyoon 0:71222a1e3c17 610 case 16:
injaeyoon 0:71222a1e3c17 611 writeReg(PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x40);
injaeyoon 0:71222a1e3c17 612 break;
injaeyoon 0:71222a1e3c17 613
injaeyoon 0:71222a1e3c17 614 case 18:
injaeyoon 0:71222a1e3c17 615 writeReg(PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x50);
injaeyoon 0:71222a1e3c17 616 break;
injaeyoon 0:71222a1e3c17 617
injaeyoon 0:71222a1e3c17 618 default:
injaeyoon 0:71222a1e3c17 619 // invalid period
injaeyoon 0:71222a1e3c17 620 return false;
injaeyoon 0:71222a1e3c17 621 }
injaeyoon 0:71222a1e3c17 622 writeReg(PRE_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
injaeyoon 0:71222a1e3c17 623
injaeyoon 0:71222a1e3c17 624 // apply new VCSEL period
injaeyoon 0:71222a1e3c17 625 writeReg(PRE_RANGE_CONFIG_VCSEL_PERIOD, vcsel_period_reg);
injaeyoon 0:71222a1e3c17 626
injaeyoon 0:71222a1e3c17 627 // update timeouts
injaeyoon 0:71222a1e3c17 628
injaeyoon 0:71222a1e3c17 629 // set_sequence_step_timeout() begin
injaeyoon 0:71222a1e3c17 630 // (SequenceStepId == VL53L0X_SEQUENCESTEP_PRE_RANGE)
injaeyoon 0:71222a1e3c17 631
injaeyoon 0:71222a1e3c17 632 uint16_t new_pre_range_timeout_mclks =
injaeyoon 0:71222a1e3c17 633 timeoutMicrosecondsToMclks(timeouts.pre_range_us, period_pclks);
injaeyoon 0:71222a1e3c17 634
injaeyoon 0:71222a1e3c17 635 writeReg16Bit(PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI,
injaeyoon 0:71222a1e3c17 636 encodeTimeout(new_pre_range_timeout_mclks));
injaeyoon 0:71222a1e3c17 637
injaeyoon 0:71222a1e3c17 638 // set_sequence_step_timeout() end
injaeyoon 0:71222a1e3c17 639
injaeyoon 0:71222a1e3c17 640 // set_sequence_step_timeout() begin
injaeyoon 0:71222a1e3c17 641 // (SequenceStepId == VL53L0X_SEQUENCESTEP_MSRC)
injaeyoon 0:71222a1e3c17 642
injaeyoon 0:71222a1e3c17 643 uint16_t new_msrc_timeout_mclks =
injaeyoon 0:71222a1e3c17 644 timeoutMicrosecondsToMclks(timeouts.msrc_dss_tcc_us, period_pclks);
injaeyoon 0:71222a1e3c17 645
injaeyoon 0:71222a1e3c17 646 writeReg(MSRC_CONFIG_TIMEOUT_MACROP,
injaeyoon 0:71222a1e3c17 647 (new_msrc_timeout_mclks > 256) ? 255 : (new_msrc_timeout_mclks - 1));
injaeyoon 0:71222a1e3c17 648
injaeyoon 0:71222a1e3c17 649 // set_sequence_step_timeout() end
injaeyoon 0:71222a1e3c17 650 }
injaeyoon 0:71222a1e3c17 651 else if (type == VcselPeriodFinalRange)
injaeyoon 0:71222a1e3c17 652 {
injaeyoon 0:71222a1e3c17 653 switch (period_pclks)
injaeyoon 0:71222a1e3c17 654 {
injaeyoon 0:71222a1e3c17 655 case 8:
injaeyoon 0:71222a1e3c17 656 writeReg(FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x10);
injaeyoon 0:71222a1e3c17 657 writeReg(FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
injaeyoon 0:71222a1e3c17 658 writeReg(GLOBAL_CONFIG_VCSEL_WIDTH, 0x02);
injaeyoon 0:71222a1e3c17 659 writeReg(ALGO_PHASECAL_CONFIG_TIMEOUT, 0x0C);
injaeyoon 0:71222a1e3c17 660 writeReg(0xFF, 0x01);
injaeyoon 0:71222a1e3c17 661 writeReg(ALGO_PHASECAL_LIM, 0x30);
injaeyoon 0:71222a1e3c17 662 writeReg(0xFF, 0x00);
injaeyoon 0:71222a1e3c17 663 break;
injaeyoon 0:71222a1e3c17 664
injaeyoon 0:71222a1e3c17 665 case 10:
injaeyoon 0:71222a1e3c17 666 writeReg(FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x28);
injaeyoon 0:71222a1e3c17 667 writeReg(FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
injaeyoon 0:71222a1e3c17 668 writeReg(GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
injaeyoon 0:71222a1e3c17 669 writeReg(ALGO_PHASECAL_CONFIG_TIMEOUT, 0x09);
injaeyoon 0:71222a1e3c17 670 writeReg(0xFF, 0x01);
injaeyoon 0:71222a1e3c17 671 writeReg(ALGO_PHASECAL_LIM, 0x20);
injaeyoon 0:71222a1e3c17 672 writeReg(0xFF, 0x00);
injaeyoon 0:71222a1e3c17 673 break;
injaeyoon 0:71222a1e3c17 674
injaeyoon 0:71222a1e3c17 675 case 12:
injaeyoon 0:71222a1e3c17 676 writeReg(FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x38);
injaeyoon 0:71222a1e3c17 677 writeReg(FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
injaeyoon 0:71222a1e3c17 678 writeReg(GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
injaeyoon 0:71222a1e3c17 679 writeReg(ALGO_PHASECAL_CONFIG_TIMEOUT, 0x08);
injaeyoon 0:71222a1e3c17 680 writeReg(0xFF, 0x01);
injaeyoon 0:71222a1e3c17 681 writeReg(ALGO_PHASECAL_LIM, 0x20);
injaeyoon 0:71222a1e3c17 682 writeReg(0xFF, 0x00);
injaeyoon 0:71222a1e3c17 683 break;
injaeyoon 0:71222a1e3c17 684
injaeyoon 0:71222a1e3c17 685 case 14:
injaeyoon 0:71222a1e3c17 686 writeReg(FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x48);
injaeyoon 0:71222a1e3c17 687 writeReg(FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
injaeyoon 0:71222a1e3c17 688 writeReg(GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
injaeyoon 0:71222a1e3c17 689 writeReg(ALGO_PHASECAL_CONFIG_TIMEOUT, 0x07);
injaeyoon 0:71222a1e3c17 690 writeReg(0xFF, 0x01);
injaeyoon 0:71222a1e3c17 691 writeReg(ALGO_PHASECAL_LIM, 0x20);
injaeyoon 0:71222a1e3c17 692 writeReg(0xFF, 0x00);
injaeyoon 0:71222a1e3c17 693 break;
injaeyoon 0:71222a1e3c17 694
injaeyoon 0:71222a1e3c17 695 default:
injaeyoon 0:71222a1e3c17 696 // invalid period
injaeyoon 0:71222a1e3c17 697 return false;
injaeyoon 0:71222a1e3c17 698 }
injaeyoon 0:71222a1e3c17 699
injaeyoon 0:71222a1e3c17 700 // apply new VCSEL period
injaeyoon 0:71222a1e3c17 701 writeReg(FINAL_RANGE_CONFIG_VCSEL_PERIOD, vcsel_period_reg);
injaeyoon 0:71222a1e3c17 702
injaeyoon 0:71222a1e3c17 703 // update timeouts
injaeyoon 0:71222a1e3c17 704
injaeyoon 0:71222a1e3c17 705 // set_sequence_step_timeout() begin
injaeyoon 0:71222a1e3c17 706 // (SequenceStepId == VL53L0X_SEQUENCESTEP_FINAL_RANGE)
injaeyoon 0:71222a1e3c17 707
injaeyoon 0:71222a1e3c17 708 // "For the final range timeout, the pre-range timeout
injaeyoon 0:71222a1e3c17 709 // must be added. To do this both final and pre-range
injaeyoon 0:71222a1e3c17 710 // timeouts must be expressed in macro periods MClks
injaeyoon 0:71222a1e3c17 711 // because they have different vcsel periods."
injaeyoon 0:71222a1e3c17 712
injaeyoon 0:71222a1e3c17 713 uint16_t new_final_range_timeout_mclks =
injaeyoon 0:71222a1e3c17 714 timeoutMicrosecondsToMclks(timeouts.final_range_us, period_pclks);
injaeyoon 0:71222a1e3c17 715
injaeyoon 0:71222a1e3c17 716 if (enables.pre_range)
injaeyoon 0:71222a1e3c17 717 {
injaeyoon 0:71222a1e3c17 718 new_final_range_timeout_mclks += timeouts.pre_range_mclks;
injaeyoon 0:71222a1e3c17 719 }
injaeyoon 0:71222a1e3c17 720
injaeyoon 0:71222a1e3c17 721 writeReg16Bit(FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI,
injaeyoon 0:71222a1e3c17 722 encodeTimeout(new_final_range_timeout_mclks));
injaeyoon 0:71222a1e3c17 723
injaeyoon 0:71222a1e3c17 724 // set_sequence_step_timeout end
injaeyoon 0:71222a1e3c17 725 }
injaeyoon 0:71222a1e3c17 726 else
injaeyoon 0:71222a1e3c17 727 {
injaeyoon 0:71222a1e3c17 728 // invalid type
injaeyoon 0:71222a1e3c17 729 return false;
injaeyoon 0:71222a1e3c17 730 }
injaeyoon 0:71222a1e3c17 731
injaeyoon 0:71222a1e3c17 732 // "Finally, the timing budget must be re-applied"
injaeyoon 0:71222a1e3c17 733
injaeyoon 0:71222a1e3c17 734 setMeasurementTimingBudget(measurement_timing_budget_us);
injaeyoon 0:71222a1e3c17 735
injaeyoon 0:71222a1e3c17 736 // "Perform the phase calibration. This is needed after changing on vcsel period."
injaeyoon 0:71222a1e3c17 737 // VL53L0X_perform_phase_calibration() begin
injaeyoon 0:71222a1e3c17 738
injaeyoon 0:71222a1e3c17 739 uint8_t sequence_config = readReg(SYSTEM_SEQUENCE_CONFIG);
injaeyoon 0:71222a1e3c17 740 writeReg(SYSTEM_SEQUENCE_CONFIG, 0x02);
injaeyoon 0:71222a1e3c17 741 performSingleRefCalibration(0x0);
injaeyoon 0:71222a1e3c17 742 writeReg(SYSTEM_SEQUENCE_CONFIG, sequence_config);
injaeyoon 0:71222a1e3c17 743
injaeyoon 0:71222a1e3c17 744 // VL53L0X_perform_phase_calibration() end
injaeyoon 0:71222a1e3c17 745
injaeyoon 0:71222a1e3c17 746 return true;
injaeyoon 0:71222a1e3c17 747 }
injaeyoon 0:71222a1e3c17 748
injaeyoon 0:71222a1e3c17 749 // Get the VCSEL pulse period in PCLKs for the given period type.
injaeyoon 0:71222a1e3c17 750 // based on VL53L0X_get_vcsel_pulse_period()
injaeyoon 0:71222a1e3c17 751 uint8_t VL53L0X::getVcselPulsePeriod(vcselPeriodType type)
injaeyoon 0:71222a1e3c17 752 {
injaeyoon 0:71222a1e3c17 753 if (type == VcselPeriodPreRange)
injaeyoon 0:71222a1e3c17 754 {
injaeyoon 0:71222a1e3c17 755 return decodeVcselPeriod(readReg(PRE_RANGE_CONFIG_VCSEL_PERIOD));
injaeyoon 0:71222a1e3c17 756 }
injaeyoon 0:71222a1e3c17 757 else if (type == VcselPeriodFinalRange)
injaeyoon 0:71222a1e3c17 758 {
injaeyoon 0:71222a1e3c17 759 return decodeVcselPeriod(readReg(FINAL_RANGE_CONFIG_VCSEL_PERIOD));
injaeyoon 0:71222a1e3c17 760 }
injaeyoon 0:71222a1e3c17 761 else { return 255; }
injaeyoon 0:71222a1e3c17 762 }
injaeyoon 0:71222a1e3c17 763
injaeyoon 0:71222a1e3c17 764 // Start continuous ranging measurements. If period_ms (optional) is 0 or not
injaeyoon 0:71222a1e3c17 765 // given, continuous back-to-back mode is used (the sensor takes measurements as
injaeyoon 0:71222a1e3c17 766 // often as possible); otherwise, continuous timed mode is used, with the given
injaeyoon 0:71222a1e3c17 767 // inter-measurement period in milliseconds determining how often the sensor
injaeyoon 0:71222a1e3c17 768 // takes a measurement.
injaeyoon 0:71222a1e3c17 769 // based on VL53L0X_StartMeasurement()
injaeyoon 0:71222a1e3c17 770 void VL53L0X::startContinuous(uint32_t period_ms)
injaeyoon 0:71222a1e3c17 771 {
injaeyoon 0:71222a1e3c17 772 writeReg(0x80, 0x01);
injaeyoon 0:71222a1e3c17 773 writeReg(0xFF, 0x01);
injaeyoon 0:71222a1e3c17 774 writeReg(0x00, 0x00);
injaeyoon 0:71222a1e3c17 775 writeReg(0x91, stop_variable);
injaeyoon 0:71222a1e3c17 776 writeReg(0x00, 0x01);
injaeyoon 0:71222a1e3c17 777 writeReg(0xFF, 0x00);
injaeyoon 0:71222a1e3c17 778 writeReg(0x80, 0x00);
injaeyoon 0:71222a1e3c17 779
injaeyoon 0:71222a1e3c17 780 if (period_ms != 0)
injaeyoon 0:71222a1e3c17 781 {
injaeyoon 0:71222a1e3c17 782 // continuous timed mode
injaeyoon 0:71222a1e3c17 783
injaeyoon 0:71222a1e3c17 784 // VL53L0X_SetInterMeasurementPeriodMilliSeconds() begin
injaeyoon 0:71222a1e3c17 785
injaeyoon 0:71222a1e3c17 786 uint16_t osc_calibrate_val = readReg16Bit(OSC_CALIBRATE_VAL);
injaeyoon 0:71222a1e3c17 787
injaeyoon 0:71222a1e3c17 788 if (osc_calibrate_val != 0)
injaeyoon 0:71222a1e3c17 789 {
injaeyoon 0:71222a1e3c17 790 period_ms *= osc_calibrate_val;
injaeyoon 0:71222a1e3c17 791 }
injaeyoon 0:71222a1e3c17 792
injaeyoon 0:71222a1e3c17 793 writeReg32Bit(SYSTEM_INTERMEASUREMENT_PERIOD, period_ms);
injaeyoon 0:71222a1e3c17 794
injaeyoon 0:71222a1e3c17 795 // VL53L0X_SetInterMeasurementPeriodMilliSeconds() end
injaeyoon 0:71222a1e3c17 796
injaeyoon 0:71222a1e3c17 797 writeReg(SYSRANGE_START, 0x04); // VL53L0X_REG_SYSRANGE_MODE_TIMED
injaeyoon 0:71222a1e3c17 798 }
injaeyoon 0:71222a1e3c17 799 else
injaeyoon 0:71222a1e3c17 800 {
injaeyoon 0:71222a1e3c17 801 // continuous back-to-back mode
injaeyoon 0:71222a1e3c17 802 writeReg(SYSRANGE_START, 0x02); // VL53L0X_REG_SYSRANGE_MODE_BACKTOBACK
injaeyoon 0:71222a1e3c17 803 }
injaeyoon 0:71222a1e3c17 804 _int.fall(callback(this, &VL53L0X::dataReady));
injaeyoon 0:71222a1e3c17 805 }
injaeyoon 0:71222a1e3c17 806
injaeyoon 0:71222a1e3c17 807 // Stop continuous measurements
injaeyoon 0:71222a1e3c17 808 // based on VL53L0X_StopMeasurement()
injaeyoon 0:71222a1e3c17 809 void VL53L0X::stopContinuous(void)
injaeyoon 0:71222a1e3c17 810 {
injaeyoon 0:71222a1e3c17 811 writeReg(SYSRANGE_START, 0x01); // VL53L0X_REG_SYSRANGE_MODE_SINGLESHOT
injaeyoon 0:71222a1e3c17 812
injaeyoon 0:71222a1e3c17 813 writeReg(0xFF, 0x01);
injaeyoon 0:71222a1e3c17 814 writeReg(0x00, 0x00);
injaeyoon 0:71222a1e3c17 815 writeReg(0x91, 0x00);
injaeyoon 0:71222a1e3c17 816 writeReg(0x00, 0x01);
injaeyoon 0:71222a1e3c17 817 writeReg(0xFF, 0x00);
injaeyoon 0:71222a1e3c17 818 }
injaeyoon 0:71222a1e3c17 819
injaeyoon 0:71222a1e3c17 820 // Returns a range reading in millimeters when continuous mode is active
injaeyoon 0:71222a1e3c17 821 // (readRangeSingleMillimeters() also calls this function after starting a
injaeyoon 0:71222a1e3c17 822 // single-shot range measurement)
injaeyoon 0:71222a1e3c17 823 uint16_t VL53L0X::readRangeContinuousMillimeters(bool blocking)
injaeyoon 0:71222a1e3c17 824 {
injaeyoon 0:71222a1e3c17 825 uint16_t range = 65535;
injaeyoon 0:71222a1e3c17 826
injaeyoon 0:71222a1e3c17 827 if (blocking)
injaeyoon 0:71222a1e3c17 828 {
injaeyoon 0:71222a1e3c17 829 startTimeout();
injaeyoon 0:71222a1e3c17 830 while ((readReg(RESULT_INTERRUPT_STATUS) & 0x07) == 0)
injaeyoon 0:71222a1e3c17 831 {
injaeyoon 0:71222a1e3c17 832 if (checkTimeoutExpired())
injaeyoon 0:71222a1e3c17 833 {
injaeyoon 0:71222a1e3c17 834 _wasTimeout = true;
injaeyoon 0:71222a1e3c17 835 return range;
injaeyoon 0:71222a1e3c17 836 }
injaeyoon 0:71222a1e3c17 837 }
injaeyoon 0:71222a1e3c17 838 // assumptions: Linearity Corrective Gain is 1000 (default);
injaeyoon 0:71222a1e3c17 839 // fractional ranging is not enabled
injaeyoon 0:71222a1e3c17 840 range = readReg16Bit(RESULT_RANGE_STATUS + 10);
injaeyoon 0:71222a1e3c17 841
injaeyoon 0:71222a1e3c17 842 writeReg(SYSTEM_INTERRUPT_CLEAR, 0x01);
injaeyoon 0:71222a1e3c17 843 }
injaeyoon 0:71222a1e3c17 844 else
injaeyoon 0:71222a1e3c17 845 {
injaeyoon 0:71222a1e3c17 846 if (readReg(RESULT_INTERRUPT_STATUS) & 0x07)
injaeyoon 0:71222a1e3c17 847 {
injaeyoon 0:71222a1e3c17 848 range = readReg16Bit(RESULT_RANGE_STATUS + 10);
injaeyoon 0:71222a1e3c17 849 writeReg(SYSTEM_INTERRUPT_CLEAR, 0x01);
injaeyoon 0:71222a1e3c17 850 }
injaeyoon 0:71222a1e3c17 851 }
injaeyoon 0:71222a1e3c17 852
injaeyoon 0:71222a1e3c17 853 return range;
injaeyoon 0:71222a1e3c17 854 }
injaeyoon 0:71222a1e3c17 855 uint16_t VL53L0X::readRange(){
injaeyoon 0:71222a1e3c17 856 if(_dataRdy){
injaeyoon 0:71222a1e3c17 857 _dataRdy=false;
injaeyoon 0:71222a1e3c17 858 _range = readReg16Bit(RESULT_RANGE_STATUS + 10);
injaeyoon 0:71222a1e3c17 859 writeReg(SYSTEM_INTERRUPT_CLEAR, 0x01);
injaeyoon 0:71222a1e3c17 860 }
injaeyoon 0:71222a1e3c17 861 return _range;
injaeyoon 0:71222a1e3c17 862 }
injaeyoon 0:71222a1e3c17 863 // Performs a single-shot range measurement and returns the reading in
injaeyoon 0:71222a1e3c17 864 // millimeters
injaeyoon 0:71222a1e3c17 865 // based on VL53L0X_PerformSingleRangingMeasurement()
injaeyoon 0:71222a1e3c17 866 uint16_t VL53L0X::readRangeSingleMillimeters(void)
injaeyoon 0:71222a1e3c17 867 {
injaeyoon 0:71222a1e3c17 868 writeReg(0x80, 0x01);
injaeyoon 0:71222a1e3c17 869 writeReg(0xFF, 0x01);
injaeyoon 0:71222a1e3c17 870 writeReg(0x00, 0x00);
injaeyoon 0:71222a1e3c17 871 writeReg(0x91, stop_variable);
injaeyoon 0:71222a1e3c17 872 writeReg(0x00, 0x01);
injaeyoon 0:71222a1e3c17 873 writeReg(0xFF, 0x00);
injaeyoon 0:71222a1e3c17 874 writeReg(0x80, 0x00);
injaeyoon 0:71222a1e3c17 875
injaeyoon 0:71222a1e3c17 876 writeReg(SYSRANGE_START, 0x01);
injaeyoon 0:71222a1e3c17 877
injaeyoon 0:71222a1e3c17 878 // "Wait until start bit has been cleared"
injaeyoon 0:71222a1e3c17 879 startTimeout();
injaeyoon 0:71222a1e3c17 880 while (readReg(SYSRANGE_START) & 0x01)
injaeyoon 0:71222a1e3c17 881 {
injaeyoon 0:71222a1e3c17 882 if (checkTimeoutExpired())
injaeyoon 0:71222a1e3c17 883 {
injaeyoon 0:71222a1e3c17 884 _wasTimeout = true;
injaeyoon 0:71222a1e3c17 885 return 65535;
injaeyoon 0:71222a1e3c17 886 }
injaeyoon 0:71222a1e3c17 887 }
injaeyoon 0:71222a1e3c17 888
injaeyoon 0:71222a1e3c17 889 return readRangeContinuousMillimeters();
injaeyoon 0:71222a1e3c17 890 }
injaeyoon 0:71222a1e3c17 891
injaeyoon 0:71222a1e3c17 892 // Did a timeout occur in one of the read functions since the last call to
injaeyoon 0:71222a1e3c17 893 // timeoutOccurred()?
injaeyoon 0:71222a1e3c17 894 bool VL53L0X::timeoutOccurred()
injaeyoon 0:71222a1e3c17 895 {
injaeyoon 0:71222a1e3c17 896 bool tmp = _wasTimeout;
injaeyoon 0:71222a1e3c17 897 _wasTimeout = false;
injaeyoon 0:71222a1e3c17 898 return tmp;
injaeyoon 0:71222a1e3c17 899 }
injaeyoon 0:71222a1e3c17 900
injaeyoon 0:71222a1e3c17 901 // Private Methods /////////////////////////////////////////////////////////////
injaeyoon 0:71222a1e3c17 902
injaeyoon 0:71222a1e3c17 903 // Get reference SPAD (single photon avalanche diode) count and type
injaeyoon 0:71222a1e3c17 904 // based on VL53L0X_get_info_from_device(),
injaeyoon 0:71222a1e3c17 905 // but only gets reference SPAD count and type
injaeyoon 0:71222a1e3c17 906 bool VL53L0X::getSpadInfo(uint8_t * count, bool * type_is_aperture)
injaeyoon 0:71222a1e3c17 907 {
injaeyoon 0:71222a1e3c17 908 uint8_t tmp;
injaeyoon 0:71222a1e3c17 909
injaeyoon 0:71222a1e3c17 910 writeReg(0x80, 0x01);
injaeyoon 0:71222a1e3c17 911 writeReg(0xFF, 0x01);
injaeyoon 0:71222a1e3c17 912 writeReg(0x00, 0x00);
injaeyoon 0:71222a1e3c17 913
injaeyoon 0:71222a1e3c17 914 writeReg(0xFF, 0x06);
injaeyoon 0:71222a1e3c17 915 writeReg(0x83, readReg(0x83) | 0x04);
injaeyoon 0:71222a1e3c17 916 writeReg(0xFF, 0x07);
injaeyoon 0:71222a1e3c17 917 writeReg(0x81, 0x01);
injaeyoon 0:71222a1e3c17 918
injaeyoon 0:71222a1e3c17 919 writeReg(0x80, 0x01);
injaeyoon 0:71222a1e3c17 920
injaeyoon 0:71222a1e3c17 921 writeReg(0x94, 0x6b);
injaeyoon 0:71222a1e3c17 922 writeReg(0x83, 0x00);
injaeyoon 0:71222a1e3c17 923 startTimeout();
injaeyoon 0:71222a1e3c17 924 while (readReg(0x83) == 0x00)
injaeyoon 0:71222a1e3c17 925 {
injaeyoon 0:71222a1e3c17 926 if (checkTimeoutExpired()) { return false; }
injaeyoon 0:71222a1e3c17 927 }
injaeyoon 0:71222a1e3c17 928 writeReg(0x83, 0x01);
injaeyoon 0:71222a1e3c17 929 tmp = readReg(0x92);
injaeyoon 0:71222a1e3c17 930
injaeyoon 0:71222a1e3c17 931 *count = tmp & 0x7f;
injaeyoon 0:71222a1e3c17 932 *type_is_aperture = (tmp >> 7) & 0x01;
injaeyoon 0:71222a1e3c17 933
injaeyoon 0:71222a1e3c17 934 writeReg(0x81, 0x00);
injaeyoon 0:71222a1e3c17 935 writeReg(0xFF, 0x06);
injaeyoon 0:71222a1e3c17 936 writeReg(0x83, readReg(0x83) & ~0x04);
injaeyoon 0:71222a1e3c17 937 writeReg(0xFF, 0x01);
injaeyoon 0:71222a1e3c17 938 writeReg(0x00, 0x01);
injaeyoon 0:71222a1e3c17 939
injaeyoon 0:71222a1e3c17 940 writeReg(0xFF, 0x00);
injaeyoon 0:71222a1e3c17 941 writeReg(0x80, 0x00);
injaeyoon 0:71222a1e3c17 942
injaeyoon 0:71222a1e3c17 943 return true;
injaeyoon 0:71222a1e3c17 944 }
injaeyoon 0:71222a1e3c17 945
injaeyoon 0:71222a1e3c17 946 // Get sequence step enables
injaeyoon 0:71222a1e3c17 947 // based on VL53L0X_GetSequenceStepEnables()
injaeyoon 0:71222a1e3c17 948 void VL53L0X::getSequenceStepEnables(SequenceStepEnables * enables)
injaeyoon 0:71222a1e3c17 949 {
injaeyoon 0:71222a1e3c17 950 uint8_t sequence_config = readReg(SYSTEM_SEQUENCE_CONFIG);
injaeyoon 0:71222a1e3c17 951
injaeyoon 0:71222a1e3c17 952 enables->tcc = (sequence_config >> 4) & 0x1;
injaeyoon 0:71222a1e3c17 953 enables->dss = (sequence_config >> 3) & 0x1;
injaeyoon 0:71222a1e3c17 954 enables->msrc = (sequence_config >> 2) & 0x1;
injaeyoon 0:71222a1e3c17 955 enables->pre_range = (sequence_config >> 6) & 0x1;
injaeyoon 0:71222a1e3c17 956 enables->final_range = (sequence_config >> 7) & 0x1;
injaeyoon 0:71222a1e3c17 957 }
injaeyoon 0:71222a1e3c17 958
injaeyoon 0:71222a1e3c17 959 // Get sequence step timeouts
injaeyoon 0:71222a1e3c17 960 // based on get_sequence_step_timeout(),
injaeyoon 0:71222a1e3c17 961 // but gets all timeouts instead of just the requested one, and also stores
injaeyoon 0:71222a1e3c17 962 // intermediate values
injaeyoon 0:71222a1e3c17 963 void VL53L0X::getSequenceStepTimeouts(SequenceStepEnables const * enables, SequenceStepTimeouts * timeouts)
injaeyoon 0:71222a1e3c17 964 {
injaeyoon 0:71222a1e3c17 965 timeouts->pre_range_vcsel_period_pclks = getVcselPulsePeriod(VcselPeriodPreRange);
injaeyoon 0:71222a1e3c17 966
injaeyoon 0:71222a1e3c17 967 timeouts->msrc_dss_tcc_mclks = readReg(MSRC_CONFIG_TIMEOUT_MACROP) + 1;
injaeyoon 0:71222a1e3c17 968 timeouts->msrc_dss_tcc_us =
injaeyoon 0:71222a1e3c17 969 timeoutMclksToMicroseconds(timeouts->msrc_dss_tcc_mclks,
injaeyoon 0:71222a1e3c17 970 timeouts->pre_range_vcsel_period_pclks);
injaeyoon 0:71222a1e3c17 971
injaeyoon 0:71222a1e3c17 972 timeouts->pre_range_mclks =
injaeyoon 0:71222a1e3c17 973 decodeTimeout(readReg16Bit(PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI));
injaeyoon 0:71222a1e3c17 974 timeouts->pre_range_us =
injaeyoon 0:71222a1e3c17 975 timeoutMclksToMicroseconds(timeouts->pre_range_mclks,
injaeyoon 0:71222a1e3c17 976 timeouts->pre_range_vcsel_period_pclks);
injaeyoon 0:71222a1e3c17 977
injaeyoon 0:71222a1e3c17 978 timeouts->final_range_vcsel_period_pclks = getVcselPulsePeriod(VcselPeriodFinalRange);
injaeyoon 0:71222a1e3c17 979
injaeyoon 0:71222a1e3c17 980 timeouts->final_range_mclks =
injaeyoon 0:71222a1e3c17 981 decodeTimeout(readReg16Bit(FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI));
injaeyoon 0:71222a1e3c17 982
injaeyoon 0:71222a1e3c17 983 if (enables->pre_range)
injaeyoon 0:71222a1e3c17 984 {
injaeyoon 0:71222a1e3c17 985 timeouts->final_range_mclks -= timeouts->pre_range_mclks;
injaeyoon 0:71222a1e3c17 986 }
injaeyoon 0:71222a1e3c17 987
injaeyoon 0:71222a1e3c17 988 timeouts->final_range_us =
injaeyoon 0:71222a1e3c17 989 timeoutMclksToMicroseconds(timeouts->final_range_mclks,
injaeyoon 0:71222a1e3c17 990 timeouts->final_range_vcsel_period_pclks);
injaeyoon 0:71222a1e3c17 991 }
injaeyoon 0:71222a1e3c17 992
injaeyoon 0:71222a1e3c17 993 // Decode sequence step timeout in MCLKs from register value
injaeyoon 0:71222a1e3c17 994 // based on VL53L0X_decode_timeout()
injaeyoon 0:71222a1e3c17 995 // Note: the original function returned a uint32_t, but the return value is
injaeyoon 0:71222a1e3c17 996 // always stored in a uint16_t.
injaeyoon 0:71222a1e3c17 997 uint16_t VL53L0X::decodeTimeout(uint16_t reg_val)
injaeyoon 0:71222a1e3c17 998 {
injaeyoon 0:71222a1e3c17 999 // format: "(LSByte * 2^MSByte) + 1"
injaeyoon 0:71222a1e3c17 1000 return (uint16_t)((reg_val & 0x00FF) <<
injaeyoon 0:71222a1e3c17 1001 (uint16_t)((reg_val & 0xFF00) >> 8)) + 1;
injaeyoon 0:71222a1e3c17 1002 }
injaeyoon 0:71222a1e3c17 1003
injaeyoon 0:71222a1e3c17 1004 // Encode sequence step timeout register value from timeout in MCLKs
injaeyoon 0:71222a1e3c17 1005 // based on VL53L0X_encode_timeout()
injaeyoon 0:71222a1e3c17 1006 // Note: the original function took a uint16_t, but the argument passed to it
injaeyoon 0:71222a1e3c17 1007 // is always a uint16_t.
injaeyoon 0:71222a1e3c17 1008 uint16_t VL53L0X::encodeTimeout(uint16_t timeout_mclks)
injaeyoon 0:71222a1e3c17 1009 {
injaeyoon 0:71222a1e3c17 1010 // format: "(LSByte * 2^MSByte) + 1"
injaeyoon 0:71222a1e3c17 1011
injaeyoon 0:71222a1e3c17 1012 uint32_t ls_byte = 0;
injaeyoon 0:71222a1e3c17 1013 uint16_t ms_byte = 0;
injaeyoon 0:71222a1e3c17 1014
injaeyoon 0:71222a1e3c17 1015 if (timeout_mclks > 0)
injaeyoon 0:71222a1e3c17 1016 {
injaeyoon 0:71222a1e3c17 1017 ls_byte = timeout_mclks - 1;
injaeyoon 0:71222a1e3c17 1018
injaeyoon 0:71222a1e3c17 1019 while ((ls_byte & 0xFFFFFF00) > 0)
injaeyoon 0:71222a1e3c17 1020 {
injaeyoon 0:71222a1e3c17 1021 ls_byte >>= 1;
injaeyoon 0:71222a1e3c17 1022 ms_byte++;
injaeyoon 0:71222a1e3c17 1023 }
injaeyoon 0:71222a1e3c17 1024
injaeyoon 0:71222a1e3c17 1025 return (ms_byte << 8) | (ls_byte & 0xFF);
injaeyoon 0:71222a1e3c17 1026 }
injaeyoon 0:71222a1e3c17 1027 else { return 0; }
injaeyoon 0:71222a1e3c17 1028 }
injaeyoon 0:71222a1e3c17 1029
injaeyoon 0:71222a1e3c17 1030 // Convert sequence step timeout from MCLKs to microseconds with given VCSEL period in PCLKs
injaeyoon 0:71222a1e3c17 1031 // based on VL53L0X_calc_timeout_us()
injaeyoon 0:71222a1e3c17 1032 uint32_t VL53L0X::timeoutMclksToMicroseconds(uint16_t timeout_period_mclks, uint8_t vcsel_period_pclks)
injaeyoon 0:71222a1e3c17 1033 {
injaeyoon 0:71222a1e3c17 1034 uint32_t macro_period_ns = calcMacroPeriod(vcsel_period_pclks);
injaeyoon 0:71222a1e3c17 1035
injaeyoon 0:71222a1e3c17 1036 return ((timeout_period_mclks * macro_period_ns) + (macro_period_ns / 2)) / 1000;
injaeyoon 0:71222a1e3c17 1037 }
injaeyoon 0:71222a1e3c17 1038
injaeyoon 0:71222a1e3c17 1039 // Convert sequence step timeout from microseconds to MCLKs with given VCSEL period in PCLKs
injaeyoon 0:71222a1e3c17 1040 // based on VL53L0X_calc_timeout_mclks()
injaeyoon 0:71222a1e3c17 1041 uint32_t VL53L0X::timeoutMicrosecondsToMclks(uint32_t timeout_period_us, uint8_t vcsel_period_pclks)
injaeyoon 0:71222a1e3c17 1042 {
injaeyoon 0:71222a1e3c17 1043 uint32_t macro_period_ns = calcMacroPeriod(vcsel_period_pclks);
injaeyoon 0:71222a1e3c17 1044
injaeyoon 0:71222a1e3c17 1045 return (((timeout_period_us * 1000) + (macro_period_ns / 2)) / macro_period_ns);
injaeyoon 0:71222a1e3c17 1046 }
injaeyoon 0:71222a1e3c17 1047
injaeyoon 0:71222a1e3c17 1048
injaeyoon 0:71222a1e3c17 1049 // based on VL53L0X_perform_single_ref_calibration()
injaeyoon 0:71222a1e3c17 1050 bool VL53L0X::performSingleRefCalibration(uint8_t vhv_init_byte)
injaeyoon 0:71222a1e3c17 1051 {
injaeyoon 0:71222a1e3c17 1052 writeReg(SYSRANGE_START, 0x01 | vhv_init_byte); // VL53L0X_REG_SYSRANGE_MODE_START_STOP
injaeyoon 0:71222a1e3c17 1053
injaeyoon 0:71222a1e3c17 1054 startTimeout();
injaeyoon 0:71222a1e3c17 1055 while ((readReg(RESULT_INTERRUPT_STATUS) & 0x07) == 0)
injaeyoon 0:71222a1e3c17 1056 {
injaeyoon 0:71222a1e3c17 1057 if (checkTimeoutExpired()) { return false; }
injaeyoon 0:71222a1e3c17 1058 }
injaeyoon 0:71222a1e3c17 1059
injaeyoon 0:71222a1e3c17 1060 writeReg(SYSTEM_INTERRUPT_CLEAR, 0x01);
injaeyoon 0:71222a1e3c17 1061
injaeyoon 0:71222a1e3c17 1062 writeReg(SYSRANGE_START, 0x00);
injaeyoon 0:71222a1e3c17 1063
injaeyoon 0:71222a1e3c17 1064 return true;
injaeyoon 0:71222a1e3c17 1065 }