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 VL53L1X lidar
17 :
18 : Many thanks to Pololu, https://github.com/pololu/vl53l1x-arduino and
19 : the ST example code
20 : */
21 : #include "AP_RangeFinder_VL53L1X.h"
22 :
23 : #if AP_RANGEFINDER_VL53L1X_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 : static const uint8_t MEASUREMENT_TIME_MS = 50; // Start continuous readings at a rate of one measurement every 50 ms
34 :
35 0 : AP_RangeFinder_VL53L1X::AP_RangeFinder_VL53L1X(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params, AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev)
36 : : AP_RangeFinder_Backend(_state, _params)
37 0 : , dev(std::move(_dev)) {}
38 :
39 : /*
40 : detect if a VL53L1X rangefinder is connected. We'll detect by
41 : trying to take a reading on I2C. If we get a result the sensor is
42 : there.
43 : */
44 0 : AP_RangeFinder_Backend *AP_RangeFinder_VL53L1X::detect(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params, AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev, DistanceMode mode)
45 : {
46 0 : if (!dev) {
47 0 : return nullptr;
48 : }
49 :
50 : AP_RangeFinder_VL53L1X *sensor
51 0 : = NEW_NOTHROW AP_RangeFinder_VL53L1X(_state, _params, std::move(dev));
52 :
53 0 : if (!sensor) {
54 0 : delete sensor;
55 0 : return nullptr;
56 : }
57 :
58 0 : sensor->dev->get_semaphore()->take_blocking();
59 :
60 0 : if (!sensor->check_id() || !sensor->init(mode)) {
61 0 : sensor->dev->get_semaphore()->give();
62 0 : delete sensor;
63 0 : return nullptr;
64 : }
65 :
66 0 : sensor->dev->get_semaphore()->give();
67 :
68 0 : return sensor;
69 : }
70 :
71 : // check sensor ID registers
72 0 : bool AP_RangeFinder_VL53L1X::check_id(void)
73 : {
74 : uint8_t v1, v2;
75 0 : if (!(read_register(0x010F, v1) &&
76 0 : read_register(0x0110, v2))) {
77 0 : return false;
78 : }
79 :
80 0 : if ((v1 != 0xEA) ||
81 0 : (v2 != 0xCC)) {
82 0 : return false;
83 : }
84 0 : printf("Detected VL53L1X on bus 0x%x\n", unsigned(dev->get_bus_id()));
85 0 : return true;
86 : }
87 :
88 0 : bool AP_RangeFinder_VL53L1X::reset(void) {
89 0 : if (dev->get_bus_id()!=0x29) {
90 : // if sensor is on a different port than the default do not reset sensor otherwise we will lose the addess.
91 : // we assume it is already confirgured.
92 0 : return true;
93 : }
94 0 : if (!write_register(SOFT_RESET, 0x00)) {
95 0 : return false;
96 : }
97 0 : hal.scheduler->delay_microseconds(100);
98 0 : if (!write_register(SOFT_RESET, 0x01)) {
99 0 : return false;
100 : }
101 0 : hal.scheduler->delay(1000);
102 0 : return true;
103 : }
104 :
105 : /*
106 : initialise sensor
107 : */
108 0 : bool AP_RangeFinder_VL53L1X::init(DistanceMode mode)
109 : {
110 : // we need to do resets and delays in order to configure the sensor, don't do this if we are trying to fast boot
111 0 : if (hal.util->was_watchdog_armed()) {
112 0 : return false;
113 : }
114 :
115 0 : uint8_t pad_i2c_hv_extsup_config = 0;
116 0 : uint16_t mm_config_outer_offset_mm = 0;
117 0 : if (!(reset() && // reset the chip, we make assumptions later on that we are on a clean power on of the sensor
118 : // setup for 2.8V operation
119 0 : read_register(PAD_I2C_HV__EXTSUP_CONFIG, pad_i2c_hv_extsup_config) &&
120 0 : write_register(PAD_I2C_HV__EXTSUP_CONFIG,
121 0 : pad_i2c_hv_extsup_config | 0x01) &&
122 :
123 : // store oscillator info for later use
124 0 : read_register16(OSC_MEASURED__FAST_OSC__FREQUENCY, fast_osc_frequency) &&
125 0 : read_register16(RESULT__OSC_CALIBRATE_VAL, osc_calibrate_val) &&
126 :
127 : // static config
128 0 : write_register16(DSS_CONFIG__TARGET_TOTAL_RATE_MCPS, TargetRate) && // should already be this value after reset
129 0 : write_register(GPIO__TIO_HV_STATUS, 0x02) &&
130 0 : write_register(SIGMA_ESTIMATOR__EFFECTIVE_PULSE_WIDTH_NS, 8) && // tuning parm default
131 0 : write_register(SIGMA_ESTIMATOR__EFFECTIVE_AMBIENT_WIDTH_NS, 16) && // tuning parm default
132 0 : write_register(ALGO__CROSSTALK_COMPENSATION_VALID_HEIGHT_MM, 0x01) &&
133 0 : write_register(ALGO__RANGE_IGNORE_VALID_HEIGHT_MM, 0xFF) &&
134 0 : write_register(ALGO__RANGE_MIN_CLIP, 0) && // tuning parm default
135 0 : write_register(ALGO__CONSISTENCY_CHECK__TOLERANCE, 2) && // tuning parm default
136 :
137 : // general config
138 0 : write_register16(SYSTEM__THRESH_RATE_HIGH, 0x0000) &&
139 0 : write_register16(SYSTEM__THRESH_RATE_LOW, 0x0000) &&
140 0 : write_register(DSS_CONFIG__APERTURE_ATTENUATION, 0x38) &&
141 :
142 : // timing config
143 0 : write_register16(RANGE_CONFIG__SIGMA_THRESH, 360) && // tuning parm default
144 0 : write_register16(RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS, 192) && // tuning parm default
145 :
146 : // dynamic config
147 0 : write_register(SYSTEM__GROUPED_PARAMETER_HOLD_0, 0x01) &&
148 0 : write_register(SYSTEM__GROUPED_PARAMETER_HOLD_1, 0x01) &&
149 0 : write_register(SD_CONFIG__QUANTIFIER, 2) && // tuning parm default
150 :
151 : // from VL53L1_preset_mode_timed_ranging_*
152 : // GPH is 0 after reset, but writing GPH0 and GPH1 above seem to set GPH to 1,
153 : // and things don't seem to work if we don't set GPH back to 0 (which the API
154 : // does here).
155 0 : write_register(SYSTEM__GROUPED_PARAMETER_HOLD, 0x00) &&
156 0 : write_register(SYSTEM__SEED_CONFIG, 1) && // tuning parm default
157 :
158 : // from VL53L1_config_low_power_auto_mode
159 0 : write_register(SYSTEM__SEQUENCE_CONFIG, 0x8B) && // VHV, PHASECAL, DSS1, RANGE
160 0 : write_register16(DSS_CONFIG__MANUAL_EFFECTIVE_SPADS_SELECT, 200 << 8) &&
161 0 : write_register(DSS_CONFIG__ROI_MODE_CONTROL, 2) && // REQUESTED_EFFFECTIVE_SPADS
162 0 : read_register16(MM_CONFIG__OUTER_OFFSET_MM, mm_config_outer_offset_mm) &&
163 0 : setDistanceMode(mode) &&
164 0 : setMeasurementTimingBudget(40000) &&
165 : // the API triggers this change in VL53L1_init_and_start_range() once a
166 : // measurement is started; assumes MM1 and MM2 are disabled
167 0 : write_register16(ALGO__PART_TO_PART_RANGE_OFFSET_MM, mm_config_outer_offset_mm * 4) &&
168 : // set continuous mode
169 0 : startContinuous(MEASUREMENT_TIME_MS)
170 : )) {
171 0 : return false;
172 : }
173 :
174 : // call timer() every MEASUREMENT_TIME_MS. We expect new data to be available every MEASUREMENT_TIME_MS
175 0 : dev->register_periodic_callback(MEASUREMENT_TIME_MS * 1000,
176 : FUNCTOR_BIND_MEMBER(&AP_RangeFinder_VL53L1X::timer, void));
177 :
178 0 : return true;
179 : }
180 :
181 : // set distance mode to Short, Medium, or Long
182 : // based on VL53L1_SetDistanceMode()
183 0 : bool AP_RangeFinder_VL53L1X::setDistanceMode(DistanceMode distance_mode)
184 : {
185 : // save existing timing budget
186 0 : uint32_t budget_us = 0;
187 0 : if (!getMeasurementTimingBudget(budget_us)) {
188 0 : return false;
189 : }
190 :
191 0 : switch (distance_mode) {
192 0 : case DistanceMode::Short:
193 : // from VL53L1_preset_mode_standard_ranging_short_range()
194 :
195 0 : if (!(// timing config
196 0 : write_register(RANGE_CONFIG__VCSEL_PERIOD_A, 0x07) &&
197 0 : write_register(RANGE_CONFIG__VCSEL_PERIOD_B, 0x05) &&
198 0 : write_register(RANGE_CONFIG__VALID_PHASE_HIGH, 0x38) &&
199 :
200 : // dynamic config
201 0 : write_register(SD_CONFIG__WOI_SD0, 0x07) &&
202 0 : write_register(SD_CONFIG__WOI_SD1, 0x05) &&
203 0 : write_register(SD_CONFIG__INITIAL_PHASE_SD0, 6) && // tuning parm default
204 0 : write_register(SD_CONFIG__INITIAL_PHASE_SD1, 6))) { // tuning parm default
205 0 : return false;
206 : }
207 :
208 0 : break;
209 :
210 0 : case DistanceMode::Medium:
211 : // from VL53L1_preset_mode_standard_ranging()
212 :
213 0 : if (!(// timing config
214 0 : write_register(RANGE_CONFIG__VCSEL_PERIOD_A, 0x0B) &&
215 0 : write_register(RANGE_CONFIG__VCSEL_PERIOD_B, 0x09) &&
216 0 : write_register(RANGE_CONFIG__VALID_PHASE_HIGH, 0x78) &&
217 :
218 : // dynamic config
219 0 : write_register(SD_CONFIG__WOI_SD0, 0x0B) &&
220 0 : write_register(SD_CONFIG__WOI_SD1, 0x09) &&
221 0 : write_register(SD_CONFIG__INITIAL_PHASE_SD0, 10) && // tuning parm default
222 0 : write_register(SD_CONFIG__INITIAL_PHASE_SD1, 10))) { // tuning parm default
223 0 : return false;
224 : }
225 :
226 0 : break;
227 :
228 0 : case DistanceMode::Long:
229 : // from VL53L1_preset_mode_standard_ranging_long_range()
230 :
231 0 : if (!(// timing config
232 0 : write_register(RANGE_CONFIG__VCSEL_PERIOD_A, 0x0F) &&
233 0 : write_register(RANGE_CONFIG__VCSEL_PERIOD_B, 0x0D) &&
234 0 : write_register(RANGE_CONFIG__VALID_PHASE_HIGH, 0xB8) &&
235 :
236 : // dynamic config
237 0 : write_register(SD_CONFIG__WOI_SD0, 0x0F) &&
238 0 : write_register(SD_CONFIG__WOI_SD1, 0x0D) &&
239 0 : write_register(SD_CONFIG__INITIAL_PHASE_SD0, 14) && // tuning parm default
240 0 : write_register(SD_CONFIG__INITIAL_PHASE_SD1, 14))) { // tuning parm default
241 0 : return false;
242 : }
243 :
244 0 : break;
245 :
246 0 : default:
247 : // unrecognized mode - do nothing
248 0 : return false;
249 : }
250 :
251 : // reapply timing budget
252 0 : return setMeasurementTimingBudget(budget_us);
253 : }
254 :
255 : // Set the measurement timing budget in microseconds, which is the time allowed
256 : // for one measurement. A longer timing budget allows for more accurate measurements.
257 : // based on VL53L1_SetMeasurementTimingBudgetMicroSeconds()
258 0 : bool AP_RangeFinder_VL53L1X::setMeasurementTimingBudget(uint32_t budget_us)
259 : {
260 : // assumes PresetMode is LOWPOWER_AUTONOMOUS
261 0 : if (budget_us <= TimingGuard) {
262 0 : return false;
263 : }
264 :
265 0 : uint32_t range_config_timeout_us = budget_us - TimingGuard;
266 0 : if (range_config_timeout_us > 1100000) {
267 0 : return false; // FDA_MAX_TIMING_BUDGET_US * 2
268 : }
269 :
270 0 : range_config_timeout_us /= 2;
271 :
272 : // VL53L1_calc_timeout_register_values() begin
273 :
274 0 : uint8_t range_config_vcsel_period = 0;
275 0 : if (!read_register(RANGE_CONFIG__VCSEL_PERIOD_A, range_config_vcsel_period)) {
276 0 : return false;
277 : }
278 :
279 : // "Update Macro Period for Range A VCSEL Period"
280 0 : uint32_t macro_period_us = calcMacroPeriod(range_config_vcsel_period);
281 :
282 : // "Update Phase timeout - uses Timing A"
283 : // Timeout of 1000 is tuning parm default (TIMED_PHASECAL_CONFIG_TIMEOUT_US_DEFAULT)
284 : // via VL53L1_get_preset_mode_timing_cfg().
285 0 : uint32_t phasecal_timeout_mclks = timeoutMicrosecondsToMclks(1000, macro_period_us);
286 0 : if (phasecal_timeout_mclks > 0xFF) {
287 0 : phasecal_timeout_mclks = 0xFF;
288 : }
289 :
290 0 : if (!( write_register(PHASECAL_CONFIG__TIMEOUT_MACROP, phasecal_timeout_mclks) &&
291 :
292 : // "Update MM Timing A timeout"
293 : // Timeout of 1 is tuning parm default (LOWPOWERAUTO_MM_CONFIG_TIMEOUT_US_DEFAULT)
294 : // via VL53L1_get_preset_mode_timing_cfg(). With the API, the register
295 : // actually ends up with a slightly different value because it gets assigned,
296 : // retrieved, recalculated with a different macro period, and reassigned,
297 : // but it probably doesn't matter because it seems like the MM ("mode
298 : // mitigation"?) sequence steps are disabled in low power auto mode anyway.
299 0 : write_register16(MM_CONFIG__TIMEOUT_MACROP_A, encodeTimeout(
300 0 : timeoutMicrosecondsToMclks(1, macro_period_us))) &&
301 :
302 : // "Update Range Timing A timeout"
303 0 : write_register16(RANGE_CONFIG__TIMEOUT_MACROP_A, encodeTimeout(
304 : timeoutMicrosecondsToMclks(range_config_timeout_us, macro_period_us))) &&
305 :
306 : // "Update Macro Period for Range B VCSEL Period"
307 0 : read_register(RANGE_CONFIG__VCSEL_PERIOD_B, range_config_vcsel_period)
308 : )) {
309 0 : return false;
310 : }
311 :
312 : // "Update Macro Period for Range B VCSEL Period"
313 0 : macro_period_us = calcMacroPeriod(range_config_vcsel_period);
314 :
315 : // "Update MM Timing B timeout"
316 : // (See earlier comment about MM Timing A timeout.)
317 0 : return write_register16(MM_CONFIG__TIMEOUT_MACROP_B, encodeTimeout(
318 0 : timeoutMicrosecondsToMclks(1, macro_period_us))) &&
319 :
320 : // "Update Range Timing B timeout"
321 0 : write_register16(RANGE_CONFIG__TIMEOUT_MACROP_B, encodeTimeout(
322 0 : timeoutMicrosecondsToMclks(range_config_timeout_us, macro_period_us)));
323 : }
324 :
325 : // Get the measurement timing budget in microseconds
326 : // based on VL53L1_SetMeasurementTimingBudgetMicroSeconds()
327 0 : bool AP_RangeFinder_VL53L1X::getMeasurementTimingBudget(uint32_t &budget)
328 : {
329 : // assumes PresetMode is LOWPOWER_AUTONOMOUS and these sequence steps are
330 : // enabled: VHV, PHASECAL, DSS1, RANGE
331 :
332 : // "Update Macro Period for Range A VCSEL Period"
333 0 : uint8_t range_config_vcsel_period_a = 0;
334 0 : if (!read_register(RANGE_CONFIG__VCSEL_PERIOD_A, range_config_vcsel_period_a)) {
335 0 : return false;
336 : }
337 :
338 0 : uint32_t macro_period_us = calcMacroPeriod(range_config_vcsel_period_a);
339 :
340 0 : uint16_t timeout_macrop_a = 0;
341 0 : if (!read_register16(RANGE_CONFIG__TIMEOUT_MACROP_A, timeout_macrop_a)) {
342 0 : return false;
343 : }
344 :
345 : // "Get Range Timing A timeout"
346 0 : uint32_t range_config_timeout_us = timeoutMclksToMicroseconds(decodeTimeout(timeout_macrop_a), macro_period_us);
347 :
348 0 : budget = 2 * range_config_timeout_us + TimingGuard;
349 0 : return true;
350 : }
351 :
352 : // Start continuous ranging measurements, with the given inter-measurement
353 : // period in milliseconds determining how often the sensor takes a measurement.
354 0 : bool AP_RangeFinder_VL53L1X::startContinuous(uint32_t period_ms)
355 : {
356 : // fix for actual measurement period shorter than set
357 0 : uint32_t adjusted_period_ms = period_ms + (period_ms * 64 / 1000);
358 :
359 : // from VL53L1_set_inter_measurement_period_ms()
360 0 : return write_register32(SYSTEM__INTERMEASUREMENT_PERIOD, adjusted_period_ms * osc_calibrate_val) &&
361 0 : write_register(SYSTEM__INTERRUPT_CLEAR, 0x01) && // sys_interrupt_clear_range
362 0 : write_register(SYSTEM__MODE_START, 0x40); // mode_range__timed
363 : }
364 :
365 : // Decode sequence step timeout in MCLKs from register value
366 : // based on VL53L1_decode_timeout()
367 0 : uint32_t AP_RangeFinder_VL53L1X::decodeTimeout(uint16_t reg_val)
368 : {
369 0 : return ((uint32_t)(reg_val & 0xFF) << (reg_val >> 8)) + 1;
370 : }
371 :
372 : // Encode sequence step timeout register value from timeout in MCLKs
373 : // based on VL53L1_encode_timeout()
374 0 : uint16_t AP_RangeFinder_VL53L1X::encodeTimeout(uint32_t timeout_mclks)
375 : {
376 : // encoded format: "(LSByte * 2^MSByte) + 1"
377 0 : uint32_t ls_byte = 0;
378 0 : uint16_t ms_byte = 0;
379 :
380 0 : if (timeout_mclks > 0) {
381 0 : ls_byte = timeout_mclks - 1;
382 0 : while ((ls_byte & 0xFFFFFF00) > 0) {
383 0 : ls_byte >>= 1;
384 0 : ms_byte++;
385 : }
386 0 : return (ms_byte << 8) | (ls_byte & 0xFF);
387 : }
388 : else {
389 0 : return 0;
390 : }
391 : }
392 :
393 : // Convert sequence step timeout from macro periods to microseconds with given
394 : // macro period in microseconds (12.12 format)
395 : // based on VL53L1_calc_timeout_us()
396 0 : uint32_t AP_RangeFinder_VL53L1X::timeoutMclksToMicroseconds(uint32_t timeout_mclks, uint32_t macro_period_us)
397 : {
398 0 : return ((uint64_t)timeout_mclks * macro_period_us + 0x800) >> 12;
399 : }
400 :
401 : // Convert sequence step timeout from microseconds to macro periods with given
402 : // macro period in microseconds (12.12 format)
403 : // based on VL53L1_calc_timeout_mclks()
404 0 : uint32_t AP_RangeFinder_VL53L1X::timeoutMicrosecondsToMclks(uint32_t timeout_us, uint32_t macro_period_us)
405 : {
406 0 : return (((uint32_t)timeout_us << 12) + (macro_period_us >> 1)) / macro_period_us;
407 : }
408 :
409 : // Calculate macro period in microseconds (12.12 format) with given VCSEL period
410 : // assumes fast_osc_frequency has been read and stored
411 : // based on VL53L1_calc_macro_period_us()
412 0 : uint32_t AP_RangeFinder_VL53L1X::calcMacroPeriod(uint8_t vcsel_period) const
413 : {
414 : // from VL53L1_calc_pll_period_us()
415 : // fast osc frequency in 4.12 format; PLL period in 0.24 format
416 0 : uint32_t pll_period_us = ((uint32_t)0x01 << 30) / fast_osc_frequency;
417 :
418 : // from VL53L1_decode_vcsel_period()
419 0 : uint8_t vcsel_period_pclks = (vcsel_period + 1) << 1;
420 :
421 : // VL53L1_MACRO_PERIOD_VCSEL_PERIODS = 2304
422 0 : uint32_t macro_period_us = (uint32_t)2304 * pll_period_us;
423 0 : macro_period_us >>= 6;
424 0 : macro_period_us *= vcsel_period_pclks;
425 0 : macro_period_us >>= 6;
426 :
427 0 : return macro_period_us;
428 : }
429 :
430 : // "Setup ranges after the first one in low power auto mode by turning off
431 : // FW calibration steps and programming static values"
432 : // based on VL53L1_low_power_auto_setup_manual_calibration()
433 0 : bool AP_RangeFinder_VL53L1X::setupManualCalibration(void)
434 : {
435 0 : uint8_t saved_vhv_init = 0;
436 0 : uint8_t saved_vhv_timeout = 0;
437 0 : uint8_t phasecal_result_vcsel_start = 0;
438 :
439 : return // "save original vhv configs"
440 0 : read_register(VHV_CONFIG__INIT, saved_vhv_init) &&
441 0 : read_register(VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND, saved_vhv_timeout) &&
442 :
443 : // "disable VHV init"
444 0 : write_register(VHV_CONFIG__INIT, saved_vhv_init & 0x7F) &&
445 :
446 : // "set loop bound to tuning param"
447 0 : write_register(VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND,
448 0 : (saved_vhv_timeout & 0x03) + (3 << 2)) && // tuning parm default (LOWPOWERAUTO_VHV_LOOP_BOUND_DEFAULT)
449 :
450 : // "override phasecal"
451 0 : write_register(PHASECAL_CONFIG__OVERRIDE, 0x01) &&
452 0 : read_register(PHASECAL_RESULT__VCSEL_START, phasecal_result_vcsel_start) &&
453 0 : write_register(CAL_CONFIG__VCSEL_START, phasecal_result_vcsel_start);
454 : }
455 :
456 : // check if sensor has new reading available
457 : // assumes interrupt is active low (GPIO_HV_MUX__CTRL bit 4 is 1)
458 0 : bool AP_RangeFinder_VL53L1X::dataReady(void)
459 : {
460 0 : uint8_t gpio_tio_hv_status = 0;
461 :
462 0 : return read_register(GPIO__TIO_HV_STATUS, gpio_tio_hv_status) &&
463 0 : ((gpio_tio_hv_status & 0x01) == 0);
464 : }
465 :
466 : // read - return last value measured by sensor
467 0 : bool AP_RangeFinder_VL53L1X::get_reading(uint16_t &reading_mm)
468 : {
469 0 : uint8_t tries = 10;
470 0 : while (!dataReady()) {
471 0 : tries--;
472 0 : hal.scheduler->delay(1);
473 0 : if (tries == 0) {
474 0 : return false;
475 : }
476 : }
477 :
478 0 : uint8_t range_status = 0;
479 :
480 0 : if (!(read_register(RESULT__RANGE_STATUS, range_status) &&
481 0 : read_register16(RESULT__FINAL_CROSSTALK_CORRECTED_RANGE_MM_SD0, reading_mm))) {
482 0 : return false;
483 : }
484 :
485 : // "apply correction gain"
486 : // gain factor of 2011 is tuning parm default (VL53L1_TUNINGPARM_LITE_RANGING_GAIN_FACTOR_DEFAULT)
487 : // Basically, this appears to scale the result by 2011/2048, or about 98%
488 : // (with the 1024 added for proper rounding).
489 0 : reading_mm = ((uint32_t)reading_mm * 2011 + 0x0400) / 0x0800;
490 :
491 0 : if (!write_register(SYSTEM__INTERRUPT_CLEAR, 0x01)) { // sys_interrupt_clear_range
492 0 : return false;
493 : }
494 :
495 0 : switch ((DeviceError)range_status) {
496 0 : case RANGECOMPLETE:
497 0 : break;
498 :
499 0 : default:
500 : #ifdef VL53L1X_DEBUG
501 : hal.console->printf("VL53L1X: %d ms status %d\n", AP_HAL::millis(), (int)range_status);
502 : #endif // VL53L1X_DEBUG
503 0 : return false;
504 : }
505 :
506 0 : if (!calibrated) {
507 0 : calibrated = setupManualCalibration();
508 : }
509 :
510 0 : return calibrated;
511 : }
512 :
513 0 : bool AP_RangeFinder_VL53L1X::read_register(uint16_t reg, uint8_t &value)
514 : {
515 0 : uint8_t b[2] = { uint8_t(reg >> 8), uint8_t(reg & 0xFF) };
516 0 : return dev->transfer(b, 2, &value, 1);
517 : }
518 :
519 0 : bool AP_RangeFinder_VL53L1X::read_register16(uint16_t reg, uint16_t & value)
520 : {
521 0 : uint16_t v = 0;
522 0 : uint8_t b[2] = { uint8_t(reg >> 8), uint8_t(reg & 0xFF) };
523 0 : if (!dev->transfer(b, 2, (uint8_t *)&v, 2)) {
524 0 : return false;
525 : }
526 0 : value = be16toh(v);
527 0 : return true;
528 : }
529 :
530 0 : bool AP_RangeFinder_VL53L1X::write_register(uint16_t reg, uint8_t value)
531 : {
532 0 : uint8_t b[3] = { uint8_t(reg >> 8), uint8_t(reg & 0xFF), value };
533 0 : return dev->transfer(b, 3, nullptr, 0);
534 : }
535 :
536 0 : bool AP_RangeFinder_VL53L1X::write_register16(uint16_t reg, uint16_t value)
537 : {
538 0 : uint8_t b[4] = { uint8_t(reg >> 8), uint8_t(reg & 0xFF), uint8_t(value >> 8), uint8_t(value & 0xFF) };
539 0 : return dev->transfer(b, 4, nullptr, 0);
540 : }
541 :
542 0 : bool AP_RangeFinder_VL53L1X::write_register32(uint16_t reg, uint32_t value)
543 : {
544 0 : uint8_t b[6] = { uint8_t(reg >> 8),
545 : uint8_t(reg & 0xFF),
546 0 : uint8_t((value >> 24) & 0xFF),
547 0 : uint8_t((value >> 16) & 0xFF),
548 0 : uint8_t((value >> 8) & 0xFF),
549 0 : uint8_t((value) & 0xFF) };
550 0 : return dev->transfer(b, 6, nullptr, 0);
551 : }
552 :
553 : /*
554 : timer called at 20Hz
555 : */
556 0 : void AP_RangeFinder_VL53L1X::timer(void)
557 : {
558 : uint16_t range_mm;
559 0 : if ((get_reading(range_mm)) && (range_mm <= 4000)) {
560 0 : WITH_SEMAPHORE(_sem);
561 0 : sum_mm += range_mm;
562 0 : counter++;
563 0 : }
564 0 : }
565 :
566 : /*
567 : update the state of the sensor
568 : */
569 0 : void AP_RangeFinder_VL53L1X::update(void)
570 : {
571 0 : WITH_SEMAPHORE(_sem);
572 0 : if (counter > 0) {
573 0 : state.distance_m = (sum_mm * 0.001f) / counter;
574 0 : state.last_reading_ms = AP_HAL::millis();
575 0 : update_status();
576 0 : sum_mm = 0;
577 0 : counter = 0;
578 0 : } else if (AP_HAL::millis() - state.last_reading_ms > 200) {
579 : // if no updates for 0.2s set no-data
580 0 : set_status(RangeFinder::Status::NoData);
581 : }
582 0 : }
583 :
584 : #endif // AP_RANGEFINDER_VL53L1X_ENABLED
|
