Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 : /*
16 : driver for ST VL53L0X lidar
17 :
18 : Many thanks to Pololu, https://github.com/pololu/vl53l0x-arduino and
19 : the ST example code
20 : */
21 : #include "AP_RangeFinder_VL53L0X.h"
22 :
23 : #if AP_RANGEFINDER_VL53L0X_ENABLED
24 :
25 : #include <utility>
26 :
27 : #include <AP_HAL/AP_HAL.h>
28 : #include <AP_HAL/utility/sparse-endian.h>
29 : #include <stdio.h>
30 :
31 : extern const AP_HAL::HAL& hal;
32 :
33 : enum regAddr
34 : {
35 : SYSRANGE_START = 0x00,
36 :
37 : SYSTEM_THRESH_HIGH = 0x0C,
38 : SYSTEM_THRESH_LOW = 0x0E,
39 :
40 : SYSTEM_SEQUENCE_CONFIG = 0x01,
41 : SYSTEM_RANGE_CONFIG = 0x09,
42 : SYSTEM_INTERMEASUREMENT_PERIOD = 0x04,
43 :
44 : SYSTEM_INTERRUPT_CONFIG_GPIO = 0x0A,
45 :
46 : GPIO_HV_MUX_ACTIVE_HIGH = 0x84,
47 :
48 : SYSTEM_INTERRUPT_CLEAR = 0x0B,
49 :
50 : RESULT_INTERRUPT_STATUS = 0x13,
51 : RESULT_RANGE_STATUS = 0x14,
52 :
53 : RESULT_CORE_AMBIENT_WINDOW_EVENTS_RTN = 0xBC,
54 : RESULT_CORE_RANGING_TOTAL_EVENTS_RTN = 0xC0,
55 : RESULT_CORE_AMBIENT_WINDOW_EVENTS_REF = 0xD0,
56 : RESULT_CORE_RANGING_TOTAL_EVENTS_REF = 0xD4,
57 : RESULT_PEAK_SIGNAL_RATE_REF = 0xB6,
58 :
59 : ALGO_PART_TO_PART_RANGE_OFFSET_MM = 0x28,
60 :
61 : I2C_SLAVE_DEVICE_ADDRESS = 0x8A,
62 :
63 : MSRC_CONFIG_CONTROL = 0x60,
64 :
65 : PRE_RANGE_CONFIG_MIN_SNR = 0x27,
66 : PRE_RANGE_CONFIG_VALID_PHASE_LOW = 0x56,
67 : PRE_RANGE_CONFIG_VALID_PHASE_HIGH = 0x57,
68 : PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT = 0x64,
69 :
70 : FINAL_RANGE_CONFIG_MIN_SNR = 0x67,
71 : FINAL_RANGE_CONFIG_VALID_PHASE_LOW = 0x47,
72 : FINAL_RANGE_CONFIG_VALID_PHASE_HIGH = 0x48,
73 : FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT = 0x44,
74 :
75 : PRE_RANGE_CONFIG_SIGMA_THRESH_HI = 0x61,
76 : PRE_RANGE_CONFIG_SIGMA_THRESH_LO = 0x62,
77 :
78 : PRE_RANGE_CONFIG_VCSEL_PERIOD = 0x50,
79 : PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI = 0x51,
80 : PRE_RANGE_CONFIG_TIMEOUT_MACROP_LO = 0x52,
81 :
82 : SYSTEM_HISTOGRAM_BIN = 0x81,
83 : HISTOGRAM_CONFIG_INITIAL_PHASE_SELECT = 0x33,
84 : HISTOGRAM_CONFIG_READOUT_CTRL = 0x55,
85 :
86 : FINAL_RANGE_CONFIG_VCSEL_PERIOD = 0x70,
87 : FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI = 0x71,
88 : FINAL_RANGE_CONFIG_TIMEOUT_MACROP_LO = 0x72,
89 : CROSSTALK_COMPENSATION_PEAK_RATE_MCPS = 0x20,
90 :
91 : MSRC_CONFIG_TIMEOUT_MACROP = 0x46,
92 :
93 : SOFT_RESET_GO2_SOFT_RESET_N = 0xBF,
94 : IDENTIFICATION_MODEL_ID = 0xC0,
95 : IDENTIFICATION_REVISION_ID = 0xC2,
96 :
97 : OSC_CALIBRATE_VAL = 0xF8,
98 :
99 : GLOBAL_CONFIG_VCSEL_WIDTH = 0x32,
100 : GLOBAL_CONFIG_SPAD_ENABLES_REF_0 = 0xB0,
101 : GLOBAL_CONFIG_SPAD_ENABLES_REF_1 = 0xB1,
102 : GLOBAL_CONFIG_SPAD_ENABLES_REF_2 = 0xB2,
103 : GLOBAL_CONFIG_SPAD_ENABLES_REF_3 = 0xB3,
104 : GLOBAL_CONFIG_SPAD_ENABLES_REF_4 = 0xB4,
105 : GLOBAL_CONFIG_SPAD_ENABLES_REF_5 = 0xB5,
106 :
107 : GLOBAL_CONFIG_REF_EN_START_SELECT = 0xB6,
108 : DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD = 0x4E,
109 : DYNAMIC_SPAD_REF_EN_START_OFFSET = 0x4F,
110 : POWER_MANAGEMENT_GO1_POWER_FORCE = 0x80,
111 :
112 : VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV = 0x89,
113 :
114 : ALGO_PHASECAL_LIM = 0x30,
115 : ALGO_PHASECAL_CONFIG_TIMEOUT = 0x30,
116 : };
117 :
118 : // tuning register settings
119 : const AP_RangeFinder_VL53L0X::RegData AP_RangeFinder_VL53L0X::tuning_data[] =
120 : {
121 : { 0xFF, 0x01 },
122 : { 0x00, 0x00 },
123 :
124 : { 0xFF, 0x00 },
125 : { 0x09, 0x00 },
126 : { 0x10, 0x00 },
127 : { 0x11, 0x00 },
128 :
129 : { 0x24, 0x01 },
130 : { 0x25, 0xFF },
131 : { 0x75, 0x00 },
132 :
133 : { 0xFF, 0x01 },
134 : { 0x4E, 0x2C },
135 : { 0x48, 0x00 },
136 : { 0x30, 0x20 },
137 :
138 : { 0xFF, 0x00 },
139 : { 0x30, 0x09 },
140 : { 0x54, 0x00 },
141 : { 0x31, 0x04 },
142 : { 0x32, 0x03 },
143 : { 0x40, 0x83 },
144 : { 0x46, 0x25 },
145 : { 0x60, 0x00 },
146 : { 0x27, 0x00 },
147 : { 0x50, 0x06 },
148 : { 0x51, 0x00 },
149 : { 0x52, 0x96 },
150 : { 0x56, 0x08 },
151 : { 0x57, 0x30 },
152 : { 0x61, 0x00 },
153 : { 0x62, 0x00 },
154 : { 0x64, 0x00 },
155 : { 0x65, 0x00 },
156 : { 0x66, 0xA0 },
157 :
158 : { 0xFF, 0x01 },
159 : { 0x22, 0x32 },
160 : { 0x47, 0x14 },
161 : { 0x49, 0xFF },
162 : { 0x4A, 0x00 },
163 :
164 : { 0xFF, 0x00 },
165 : { 0x7A, 0x0A },
166 : { 0x7B, 0x00 },
167 : { 0x78, 0x21 },
168 :
169 : { 0xFF, 0x01 },
170 : { 0x23, 0x34 },
171 : { 0x42, 0x00 },
172 : { 0x44, 0xFF },
173 : { 0x45, 0x26 },
174 : { 0x46, 0x05 },
175 : { 0x40, 0x40 },
176 : { 0x0E, 0x06 },
177 : { 0x20, 0x1A },
178 : { 0x43, 0x40 },
179 :
180 : { 0xFF, 0x00 },
181 : { 0x34, 0x03 },
182 : { 0x35, 0x44 },
183 :
184 : { 0xFF, 0x01 },
185 : { 0x31, 0x04 },
186 : { 0x4B, 0x09 },
187 : { 0x4C, 0x05 },
188 : { 0x4D, 0x04 },
189 :
190 : { 0xFF, 0x00 },
191 : { 0x44, 0x00 },
192 : { 0x45, 0x20 },
193 : { 0x47, 0x08 },
194 : { 0x48, 0x28 },
195 : { 0x67, 0x00 },
196 : { 0x70, 0x04 },
197 : { 0x71, 0x01 },
198 : { 0x72, 0xFE },
199 : { 0x76, 0x00 },
200 : { 0x77, 0x00 },
201 :
202 : { 0xFF, 0x01 },
203 : { 0x0D, 0x01 },
204 :
205 : { 0xFF, 0x00 },
206 : { 0x80, 0x01 },
207 : { 0x01, 0xF8 },
208 :
209 : { 0xFF, 0x01 },
210 : { 0x8E, 0x01 },
211 : { 0x00, 0x01 },
212 : { 0xFF, 0x00 },
213 : { 0x80, 0x00 },
214 : };
215 :
216 0 : AP_RangeFinder_VL53L0X::AP_RangeFinder_VL53L0X(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params, AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev)
217 : : AP_RangeFinder_Backend(_state, _params)
218 0 : , dev(std::move(_dev)) {}
219 :
220 :
221 : /*
222 : detect if a VL53L0X rangefinder is connected. We'll detect by
223 : trying to take a reading on I2C. If we get a result the sensor is
224 : there.
225 : */
226 0 : AP_RangeFinder_Backend *AP_RangeFinder_VL53L0X::detect(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params, AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev)
227 : {
228 0 : if (!dev) {
229 0 : return nullptr;
230 : }
231 : AP_RangeFinder_VL53L0X *sensor
232 0 : = NEW_NOTHROW AP_RangeFinder_VL53L0X(_state, _params, std::move(dev));
233 :
234 0 : if (!sensor) {
235 0 : delete sensor;
236 0 : return nullptr;
237 : }
238 :
239 0 : sensor->dev->get_semaphore()->take_blocking();
240 :
241 0 : if (!sensor->check_id() || !sensor->init()) {
242 0 : sensor->dev->get_semaphore()->give();
243 0 : delete sensor;
244 0 : return nullptr;
245 : }
246 :
247 0 : sensor->dev->get_semaphore()->give();
248 :
249 0 : return sensor;
250 : }
251 :
252 : // check sensor ID registers
253 0 : bool AP_RangeFinder_VL53L0X::check_id(void)
254 : {
255 : uint8_t v1, v2;
256 0 : if (!dev->read_registers(0xC0, &v1, 1) ||
257 0 : !dev->read_registers(0xC1, &v2, 1) ||
258 0 : v1 != 0xEE ||
259 0 : v2 != 0xAA) {
260 0 : return false;
261 : }
262 0 : printf("Detected VL53L0X on bus 0x%x\n", unsigned(dev->get_bus_id()));
263 0 : return true;
264 : }
265 :
266 : // Get reference SPAD (single photon avalanche diode) count and type
267 : // based on VL53L0X_get_info_from_device(),
268 : // but only gets reference SPAD count and type
269 0 : bool AP_RangeFinder_VL53L0X::get_SPAD_info(uint8_t * count, bool *type_is_aperture)
270 : {
271 : uint8_t tmp;
272 :
273 0 : write_register(0x80, 0x01);
274 0 : write_register(0xFF, 0x01);
275 0 : write_register(0x00, 0x00);
276 :
277 0 : write_register(0xFF, 0x06);
278 0 : write_register(0x83, read_register(0x83) | 0x04);
279 0 : write_register(0xFF, 0x07);
280 0 : write_register(0x81, 0x01);
281 :
282 0 : write_register(0x80, 0x01);
283 :
284 0 : write_register(0x94, 0x6b);
285 0 : write_register(0x83, 0x00);
286 :
287 0 : uint8_t tries = 50;
288 0 : while (read_register(0x83) == 0x00) {
289 0 : tries--;
290 0 : if (tries == 0) {
291 0 : return false;
292 : }
293 0 : hal.scheduler->delay(1);
294 : }
295 0 : write_register(0x83, 0x01);
296 0 : tmp = read_register(0x92);
297 :
298 0 : *count = tmp & 0x7f;
299 0 : *type_is_aperture = (tmp >> 7) & 0x01;
300 :
301 0 : write_register(0x81, 0x00);
302 0 : write_register(0xFF, 0x06);
303 0 : write_register(0x83, read_register(0x83) & ~0x04);
304 0 : write_register(0xFF, 0x01);
305 0 : write_register(0x00, 0x01);
306 :
307 0 : write_register(0xFF, 0x00);
308 0 : write_register(0x80, 0x00);
309 :
310 0 : return true;
311 : }
312 :
313 : // Get sequence step enables
314 : // based on VL53L0X_GetSequenceStepEnables()
315 0 : void AP_RangeFinder_VL53L0X::getSequenceStepEnables(SequenceStepEnables * enables)
316 : {
317 0 : uint8_t sequence_config = read_register(SYSTEM_SEQUENCE_CONFIG);
318 :
319 0 : enables->tcc = (sequence_config >> 4) & 0x1;
320 0 : enables->dss = (sequence_config >> 3) & 0x1;
321 0 : enables->msrc = (sequence_config >> 2) & 0x1;
322 0 : enables->pre_range = (sequence_config >> 6) & 0x1;
323 0 : enables->final_range = (sequence_config >> 7) & 0x1;
324 0 : }
325 :
326 : // Get the VCSEL pulse period in PCLKs for the given period type.
327 : // based on VL53L0X_get_vcsel_pulse_period()
328 0 : uint8_t AP_RangeFinder_VL53L0X::getVcselPulsePeriod(vcselPeriodType _type)
329 : {
330 : #define decodeVcselPeriod(reg_val) (((reg_val) + 1) << 1)
331 0 : if (_type == VcselPeriodPreRange) {
332 0 : return decodeVcselPeriod(read_register(PRE_RANGE_CONFIG_VCSEL_PERIOD));
333 0 : } else if (_type == VcselPeriodFinalRange) {
334 0 : return decodeVcselPeriod(read_register(FINAL_RANGE_CONFIG_VCSEL_PERIOD));
335 : }
336 0 : return 255;
337 : }
338 :
339 : // Convert sequence step timeout from MCLKs to microseconds with given VCSEL period in PCLKs
340 : // based on VL53L0X_calc_timeout_us()
341 0 : uint32_t AP_RangeFinder_VL53L0X::timeoutMclksToMicroseconds(uint16_t timeout_period_mclks, uint8_t vcsel_period_pclks)
342 : {
343 : #define calcMacroPeriod(vcsel_period_pclks) ((((uint32_t)2304 * (vcsel_period_pclks) * 1655) + 500) / 1000)
344 0 : uint32_t macro_period_ns = calcMacroPeriod(vcsel_period_pclks);
345 :
346 0 : return ((timeout_period_mclks * macro_period_ns) + (macro_period_ns / 2)) / 1000;
347 : }
348 :
349 : // Decode sequence step timeout in MCLKs from register value
350 : // based on VL53L0X_decode_timeout()
351 : // Note: the original function returned a uint32_t, but the return value is
352 : // always stored in a uint16_t.
353 0 : uint16_t AP_RangeFinder_VL53L0X::decodeTimeout(uint16_t reg_val)
354 : {
355 : // format: "(LSByte * 2^MSByte) + 1"
356 0 : return (uint16_t)((reg_val & 0x00FF) <<
357 0 : (uint16_t)((reg_val & 0xFF00) >> 8)) + 1;
358 : }
359 :
360 : // Get sequence step timeouts
361 : // based on get_sequence_step_timeout(),
362 : // but gets all timeouts instead of just the requested one, and also stores
363 : // intermediate values
364 0 : void AP_RangeFinder_VL53L0X::getSequenceStepTimeouts(SequenceStepEnables const * enables, SequenceStepTimeouts * timeouts)
365 : {
366 0 : timeouts->pre_range_vcsel_period_pclks = getVcselPulsePeriod(VcselPeriodPreRange);
367 :
368 0 : timeouts->msrc_dss_tcc_mclks = read_register(MSRC_CONFIG_TIMEOUT_MACROP) + 1;
369 0 : timeouts->msrc_dss_tcc_us =
370 0 : timeoutMclksToMicroseconds(timeouts->msrc_dss_tcc_mclks,
371 0 : timeouts->pre_range_vcsel_period_pclks);
372 :
373 0 : timeouts->pre_range_mclks =
374 0 : decodeTimeout(read_register16(PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI));
375 0 : timeouts->pre_range_us =
376 0 : timeoutMclksToMicroseconds(timeouts->pre_range_mclks,
377 0 : timeouts->pre_range_vcsel_period_pclks);
378 :
379 0 : timeouts->final_range_vcsel_period_pclks = getVcselPulsePeriod(VcselPeriodFinalRange);
380 :
381 0 : timeouts->final_range_mclks =
382 0 : decodeTimeout(read_register16(FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI));
383 :
384 0 : if (enables->pre_range) {
385 0 : timeouts->final_range_mclks -= timeouts->pre_range_mclks;
386 : }
387 :
388 0 : timeouts->final_range_us =
389 0 : timeoutMclksToMicroseconds(timeouts->final_range_mclks,
390 0 : timeouts->final_range_vcsel_period_pclks);
391 0 : }
392 :
393 :
394 : // Get the measurement timing budget in microseconds
395 : // based on VL53L0X_get_measurement_timing_budget_micro_seconds()
396 : // in us
397 0 : uint32_t AP_RangeFinder_VL53L0X::getMeasurementTimingBudget(void)
398 : {
399 : SequenceStepEnables enables;
400 : SequenceStepTimeouts timeouts;
401 :
402 0 : uint16_t const StartOverhead = 1910; // note that this is different than the value in set_
403 0 : uint16_t const EndOverhead = 960;
404 0 : uint16_t const MsrcOverhead = 660;
405 0 : uint16_t const TccOverhead = 590;
406 0 : uint16_t const DssOverhead = 690;
407 0 : uint16_t const PreRangeOverhead = 660;
408 0 : uint16_t const FinalRangeOverhead = 550;
409 :
410 : // "Start and end overhead times always present"
411 0 : uint32_t budget_us = StartOverhead + EndOverhead;
412 :
413 0 : getSequenceStepEnables(&enables);
414 0 : getSequenceStepTimeouts(&enables, &timeouts);
415 :
416 0 : if (enables.tcc) {
417 0 : budget_us += (timeouts.msrc_dss_tcc_us + TccOverhead);
418 : }
419 :
420 0 : if (enables.dss) {
421 0 : budget_us += 2 * (timeouts.msrc_dss_tcc_us + DssOverhead);
422 0 : } else if (enables.msrc) {
423 0 : budget_us += (timeouts.msrc_dss_tcc_us + MsrcOverhead);
424 : }
425 :
426 0 : if (enables.pre_range) {
427 0 : budget_us += (timeouts.pre_range_us + PreRangeOverhead);
428 : }
429 :
430 0 : if (enables.final_range) {
431 0 : budget_us += (timeouts.final_range_us + FinalRangeOverhead);
432 : }
433 :
434 0 : measurement_timing_budget_us = budget_us; // store for internal reuse
435 0 : return budget_us;
436 : }
437 :
438 : // Convert sequence step timeout from microseconds to MCLKs with given VCSEL period in PCLKs
439 : // based on VL53L0X_calc_timeout_mclks()
440 0 : uint32_t AP_RangeFinder_VL53L0X::timeoutMicrosecondsToMclks(uint32_t timeout_period_us, uint8_t vcsel_period_pclks)
441 : {
442 0 : uint32_t macro_period_ns = calcMacroPeriod(vcsel_period_pclks);
443 :
444 0 : return (((timeout_period_us * 1000) + (macro_period_ns / 2)) / macro_period_ns);
445 : }
446 :
447 : // Encode sequence step timeout register value from timeout in MCLKs
448 : // based on VL53L0X_encode_timeout()
449 : // Note: the original function took a uint16_t, but the argument passed to it
450 : // is always a uint16_t.
451 0 : uint16_t AP_RangeFinder_VL53L0X::encodeTimeout(uint16_t timeout_mclks)
452 : {
453 : // format: "(LSByte * 2^MSByte) + 1"
454 :
455 0 : uint32_t ls_byte = 0;
456 0 : uint16_t ms_byte = 0;
457 :
458 0 : if (timeout_mclks > 0) {
459 0 : ls_byte = timeout_mclks - 1;
460 :
461 0 : while ((ls_byte & 0xFFFFFF00) > 0) {
462 0 : ls_byte >>= 1;
463 0 : ms_byte++;
464 : }
465 :
466 0 : return (ms_byte << 8) | (ls_byte & 0xFF);
467 : }
468 0 : return 0;
469 : }
470 :
471 : // Set the measurement timing budget in microseconds, which is the time allowed
472 : // for one measurement; the ST API and this library take care of splitting the
473 : // timing budget among the sub-steps in the ranging sequence. A longer timing
474 : // budget allows for more accurate measurements. Increasing the budget by a
475 : // factor of N decreases the range measurement standard deviation by a factor of
476 : // sqrt(N). Defaults to about 33 milliseconds; the minimum is 20 ms.
477 : // based on VL53L0X_set_measurement_timing_budget_micro_seconds()
478 0 : bool AP_RangeFinder_VL53L0X::setMeasurementTimingBudget(uint32_t budget_us)
479 : {
480 : SequenceStepEnables enables;
481 : SequenceStepTimeouts timeouts;
482 :
483 0 : uint16_t const StartOverhead = 1320; // note that this is different than the value in get_
484 0 : uint16_t const EndOverhead = 960;
485 0 : uint16_t const MsrcOverhead = 660;
486 0 : uint16_t const TccOverhead = 590;
487 0 : uint16_t const DssOverhead = 690;
488 0 : uint16_t const PreRangeOverhead = 660;
489 0 : uint16_t const FinalRangeOverhead = 550;
490 :
491 0 : uint32_t const MinTimingBudget = 20000;
492 :
493 0 : if (budget_us < MinTimingBudget) { return false; }
494 :
495 0 : uint32_t used_budget_us = StartOverhead + EndOverhead;
496 :
497 0 : getSequenceStepEnables(&enables);
498 0 : getSequenceStepTimeouts(&enables, &timeouts);
499 :
500 0 : if (enables.tcc) {
501 0 : used_budget_us += (timeouts.msrc_dss_tcc_us + TccOverhead);
502 : }
503 :
504 0 : if (enables.dss) {
505 0 : used_budget_us += 2 * (timeouts.msrc_dss_tcc_us + DssOverhead);
506 0 : } else if (enables.msrc) {
507 0 : used_budget_us += (timeouts.msrc_dss_tcc_us + MsrcOverhead);
508 : }
509 :
510 0 : if (enables.pre_range) {
511 0 : used_budget_us += (timeouts.pre_range_us + PreRangeOverhead);
512 : }
513 :
514 0 : if (enables.final_range) {
515 0 : used_budget_us += FinalRangeOverhead;
516 :
517 : // "Note that the final range timeout is determined by the timing
518 : // budget and the sum of all other timeouts within the sequence.
519 : // If there is no room for the final range timeout, then an error
520 : // will be set. Otherwise the remaining time will be applied to
521 : // the final range."
522 :
523 0 : if (used_budget_us > budget_us) {
524 : // "Requested timeout too big."
525 0 : return false;
526 : }
527 :
528 0 : uint32_t final_range_timeout_us = budget_us - used_budget_us;
529 :
530 : // set_sequence_step_timeout() begin
531 : // (SequenceStepId == VL53L0X_SEQUENCESTEP_FINAL_RANGE)
532 :
533 : // "For the final range timeout, the pre-range timeout
534 : // must be added. To do this both final and pre-range
535 : // timeouts must be expressed in macro periods MClks
536 : // because they have different vcsel periods."
537 :
538 : uint16_t final_range_timeout_mclks =
539 0 : timeoutMicrosecondsToMclks(final_range_timeout_us,
540 0 : timeouts.final_range_vcsel_period_pclks);
541 :
542 0 : if (enables.pre_range) {
543 0 : final_range_timeout_mclks += timeouts.pre_range_mclks;
544 : }
545 :
546 0 : write_register16(FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI,
547 0 : encodeTimeout(final_range_timeout_mclks));
548 :
549 : // set_sequence_step_timeout() end
550 0 : measurement_timing_budget_us = budget_us; // store for internal reuse
551 : }
552 0 : return true;
553 : }
554 :
555 0 : bool AP_RangeFinder_VL53L0X::init()
556 : {
557 : // setup for 2.8V operation
558 0 : write_register(VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV,
559 0 : read_register(VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV) | 0x01);
560 :
561 : // "Set I2C standard mode"
562 0 : write_register(0x88, 0x00);
563 :
564 0 : write_register(0x80, 0x01);
565 0 : write_register(0xFF, 0x01);
566 0 : write_register(0x00, 0x00);
567 0 : stop_variable = read_register(0x91);
568 0 : write_register(0x00, 0x01);
569 0 : write_register(0xFF, 0x00);
570 0 : write_register(0x80, 0x00);
571 :
572 : // disable SIGNAL_RATE_MSRC (bit 1) and SIGNAL_RATE_PRE_RANGE (bit 4) limit checks
573 0 : write_register(MSRC_CONFIG_CONTROL, read_register(MSRC_CONFIG_CONTROL) | 0x12);
574 :
575 : // set final range signal rate limit to 0.25 MCPS (million counts per second)
576 0 : write_register16(FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT, uint16_t(0.25 * (1 << 7)));
577 :
578 0 : write_register(SYSTEM_SEQUENCE_CONFIG, 0xFF);
579 :
580 : uint8_t spad_count;
581 : bool spad_type_is_aperture;
582 0 : if (!get_SPAD_info(&spad_count, &spad_type_is_aperture)) {
583 0 : printf("VL53L0X: Failed to get SPAD info\n");
584 0 : return false;
585 : }
586 :
587 : // The SPAD map (RefGoodSpadMap) is read by VL53L0X_get_info_from_device() in
588 : // the API, but the same data seems to be more easily readable from
589 : // GLOBAL_CONFIG_SPAD_ENABLES_REF_0 through _6, so read it from there
590 : uint8_t ref_spad_map[6];
591 0 : if (!dev->read_registers(GLOBAL_CONFIG_SPAD_ENABLES_REF_0, ref_spad_map, 6)) {
592 0 : printf("VL53L0X: Failed to read SPAD map\n");
593 0 : return false;
594 : }
595 :
596 : // -- VL53L0X_set_reference_spads() begin (assume NVM values are valid)
597 0 : write_register(0xFF, 0x01);
598 0 : write_register(DYNAMIC_SPAD_REF_EN_START_OFFSET, 0x00);
599 0 : write_register(DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD, 0x2C);
600 0 : write_register(0xFF, 0x00);
601 0 : write_register(GLOBAL_CONFIG_REF_EN_START_SELECT, 0xB4);
602 :
603 0 : uint8_t first_spad_to_enable = spad_type_is_aperture ? 12 : 0; // 12 is the first aperture spad
604 0 : uint8_t spads_enabled = 0;
605 :
606 0 : for (uint8_t i = 0; i < 48; i++) {
607 0 : if (i < first_spad_to_enable || spads_enabled == spad_count) {
608 : // This bit is lower than the first one that should be enabled, or
609 : // (reference_spad_count) bits have already been enabled, so zero this bit
610 0 : ref_spad_map[i / 8] &= ~(1 << (i % 8));
611 0 : } else if ((ref_spad_map[i / 8] >> (i % 8)) & 0x1) {
612 0 : spads_enabled++;
613 : }
614 : }
615 :
616 0 : uint8_t reg_spad_map[7] = { GLOBAL_CONFIG_SPAD_ENABLES_REF_0, };
617 0 : memcpy(®_spad_map[1], ref_spad_map, 6);
618 0 : dev->transfer(reg_spad_map, 7, nullptr, 0);
619 :
620 0 : for (uint16_t i=0; i<ARRAY_SIZE(tuning_data); i++) {
621 0 : write_register(tuning_data[i].reg, tuning_data[i].value);
622 : }
623 :
624 0 : write_register(SYSTEM_INTERRUPT_CONFIG_GPIO, 0x04);
625 0 : write_register(GPIO_HV_MUX_ACTIVE_HIGH, read_register(GPIO_HV_MUX_ACTIVE_HIGH) & ~0x10); // active low
626 0 : write_register(SYSTEM_INTERRUPT_CLEAR, 0x01);
627 :
628 : // -- VL53L0X_SetGpioConfig() end
629 :
630 0 : measurement_timing_budget_us = getMeasurementTimingBudget();
631 :
632 : // "Disable MSRC and TCC by default"
633 : // MSRC = Minimum Signal Rate Check
634 : // TCC = Target CentreCheck
635 : // -- VL53L0X_SetSequenceStepEnable() begin
636 :
637 0 : write_register(SYSTEM_SEQUENCE_CONFIG, 0xE8);
638 :
639 : // -- VL53L0X_SetSequenceStepEnable() end
640 :
641 : // "Recalculate timing budget"
642 0 : setMeasurementTimingBudget(measurement_timing_budget_us);
643 :
644 : // VL53L0X_StaticInit() end
645 :
646 : // VL53L0X_PerformRefCalibration() begin (VL53L0X_perform_ref_calibration())
647 :
648 : // -- VL53L0X_perform_vhv_calibration() begin
649 :
650 0 : write_register(SYSTEM_SEQUENCE_CONFIG, 0x01);
651 0 : if (!performSingleRefCalibration(0x40)) {
652 0 : printf("VL53L0X: Failed SingleRefCalibration1\n");
653 0 : return false;
654 : }
655 :
656 : // -- VL53L0X_perform_vhv_calibration() end
657 :
658 : // -- VL53L0X_perform_phase_calibration() begin
659 :
660 0 : write_register(SYSTEM_SEQUENCE_CONFIG, 0x02);
661 0 : if (!performSingleRefCalibration(0x00)) {
662 0 : printf("VL53L0X: Failed SingleRefCalibration2\n");
663 0 : return false;
664 : }
665 :
666 : // -- VL53L0X_perform_phase_calibration() end
667 :
668 : // "restore the previous Sequence Config"
669 0 : write_register(SYSTEM_SEQUENCE_CONFIG, 0xE8);
670 :
671 0 : start_continuous();
672 :
673 : // call timer() every 33ms. We expect new data to be available every 33ms
674 0 : dev->register_periodic_callback(33000,
675 : FUNCTOR_BIND_MEMBER(&AP_RangeFinder_VL53L0X::timer, void));
676 0 : return true;
677 : }
678 :
679 :
680 : // based on VL53L0X_perform_single_ref_calibration()
681 0 : bool AP_RangeFinder_VL53L0X::performSingleRefCalibration(uint8_t vhv_init_byte)
682 : {
683 0 : write_register(SYSRANGE_START, 0x01 | vhv_init_byte); // VL53L0X_REG_SYSRANGE_MODE_START_STOP
684 :
685 0 : uint8_t tries = 200;
686 0 : while ((read_register(RESULT_INTERRUPT_STATUS) & 0x07) == 0) {
687 0 : if (tries-- == 0) {
688 0 : return false;
689 : }
690 0 : hal.scheduler->delay(1);
691 : }
692 :
693 0 : write_register(SYSTEM_INTERRUPT_CLEAR, 0x01);
694 :
695 0 : write_register(SYSRANGE_START, 0x00);
696 :
697 0 : return true;
698 : }
699 :
700 :
701 : // Start continuous ranging measurements
702 0 : void AP_RangeFinder_VL53L0X::start_continuous(void)
703 : {
704 0 : write_register(0x80, 0x01);
705 0 : write_register(0xFF, 0x01);
706 0 : write_register(0x00, 0x00);
707 0 : write_register(0x91, stop_variable);
708 0 : write_register(0x00, 0x01);
709 0 : write_register(0xFF, 0x00);
710 0 : write_register(0x80, 0x00);
711 :
712 : // continuous back-to-back mode
713 0 : write_register(SYSRANGE_START, 0x02); // VL53L0X_REG_SYSRANGE_MODE_BACKTOBACK
714 :
715 0 : start_ms = AP_HAL::millis();
716 0 : }
717 :
718 : // read - return last value measured by sensor
719 0 : bool AP_RangeFinder_VL53L0X::get_reading(uint16_t &reading_mm)
720 : {
721 0 : if ((read_register(RESULT_INTERRUPT_STATUS) & 0x07) == 0) {
722 0 : if (AP_HAL::millis() - start_ms > 200) {
723 0 : start_continuous();
724 : }
725 0 : return false;
726 : }
727 :
728 : // assumptions: Linearity Corrective Gain is 1000 (default);
729 : // fractional ranging is not enabled
730 0 : reading_mm = read_register16(RESULT_RANGE_STATUS + 10);
731 0 : write_register(SYSTEM_INTERRUPT_CLEAR, 0x01);
732 :
733 0 : return true;
734 : }
735 :
736 0 : void AP_RangeFinder_VL53L0X::write_register16(uint8_t reg, uint16_t value)
737 : {
738 0 : uint8_t b[3] = { reg, uint8_t(value>>8), uint8_t(value) };
739 0 : dev->transfer(b, 3, nullptr, 0);
740 0 : }
741 :
742 0 : void AP_RangeFinder_VL53L0X::write_register(uint8_t reg, uint8_t value)
743 : {
744 0 : dev->write_register(reg, value);
745 0 : }
746 :
747 0 : uint8_t AP_RangeFinder_VL53L0X::read_register(uint8_t reg)
748 : {
749 0 : uint8_t v = 0;
750 0 : dev->read_registers(reg, &v, 1);
751 0 : return v;
752 : }
753 :
754 0 : uint16_t AP_RangeFinder_VL53L0X::read_register16(uint8_t reg)
755 : {
756 0 : uint16_t v = 0;
757 0 : dev->transfer(®, 1, (uint8_t *)&v, 2);
758 0 : return be16toh(v);
759 : }
760 :
761 : /*
762 : update the state of the sensor
763 : */
764 0 : void AP_RangeFinder_VL53L0X::update(void)
765 : {
766 0 : if (counter > 0) {
767 0 : state.distance_m = (sum_mm * 0.001f) / counter;
768 0 : state.last_reading_ms = AP_HAL::millis();
769 0 : sum_mm = 0;
770 0 : counter = 0;
771 0 : update_status();
772 : } else {
773 0 : set_status(RangeFinder::Status::NoData);
774 : }
775 0 : }
776 :
777 0 : void AP_RangeFinder_VL53L0X::timer(void)
778 : {
779 : uint16_t range_mm;
780 0 : if (get_reading(range_mm) && range_mm < 8000) {
781 0 : sum_mm += range_mm;
782 0 : counter++;
783 : }
784 0 : }
785 :
786 : #endif // AP_RANGEFINDER_VL53L0X_ENABLED
|
