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 : #include "AP_RangeFinder.h"
17 :
18 : #if AP_RANGEFINDER_ENABLED
19 :
20 : #include "AP_RangeFinder_analog.h"
21 : #include "AP_RangeFinder_PulsedLightLRF.h"
22 : #include "AP_RangeFinder_MaxsonarI2CXL.h"
23 : #include "AP_RangeFinder_MaxsonarSerialLV.h"
24 : #include "AP_RangeFinder_BBB_PRU.h"
25 : #include "AP_RangeFinder_LightWareI2C.h"
26 : #include "AP_RangeFinder_LightWareSerial.h"
27 : #if AP_RANGEFINDER_BEBOP_ENABLED
28 : #include "AP_RangeFinder_Bebop.h"
29 : #endif
30 : #include "AP_RangeFinder_Backend.h"
31 : #include "AP_RangeFinder_Backend_Serial.h"
32 : #include "AP_RangeFinder_MAVLink.h"
33 : #include "AP_RangeFinder_LeddarOne.h"
34 : #include "AP_RangeFinder_USD1_Serial.h"
35 : #include "AP_RangeFinder_TeraRangerI2C.h"
36 : #include "AP_RangeFinder_TeraRanger_Serial.h"
37 : #include "AP_RangeFinder_VL53L0X.h"
38 : #include "AP_RangeFinder_VL53L1X.h"
39 : #include "AP_RangeFinder_NMEA.h"
40 : #include "AP_RangeFinder_Wasp.h"
41 : #include "AP_RangeFinder_Benewake_TF02.h"
42 : #include "AP_RangeFinder_Benewake_TF03.h"
43 : #include "AP_RangeFinder_Benewake_TFMini.h"
44 : #include "AP_RangeFinder_Benewake_TFMiniPlus.h"
45 : #include "AP_RangeFinder_Benewake_TFS20L.h"
46 : #include "AP_RangeFinder_PWM.h"
47 : #include "AP_RangeFinder_GYUS42v2.h"
48 : #include "AP_RangeFinder_HC_SR04.h"
49 : #include "AP_RangeFinder_Bebop.h"
50 : #include "AP_RangeFinder_BLPing.h"
51 : #include "AP_RangeFinder_DroneCAN.h"
52 : #include "AP_RangeFinder_Lanbao.h"
53 : #include "AP_RangeFinder_LeddarVu8.h"
54 : #include "AP_RangeFinder_SITL.h"
55 : #include "AP_RangeFinder_MSP.h"
56 : #include "AP_RangeFinder_USD1_CAN.h"
57 : #include "AP_RangeFinder_Benewake_CAN.h"
58 : #include "AP_RangeFinder_Lua.h"
59 : #include "AP_RangeFinder_NoopLoop.h"
60 : #include "AP_RangeFinder_TOFSenseP_CAN.h"
61 : #include "AP_RangeFinder_NRA24_CAN.h"
62 : #include "AP_RangeFinder_TOFSenseF_I2C.h"
63 : #include "AP_RangeFinder_JRE_Serial.h"
64 : #include "AP_RangeFinder_Ainstein_LR_D1.h"
65 : #include "AP_RangeFinder_RDS02UF.h"
66 : #include "AP_RangeFinder_LightWare_GRF.h"
67 : #include "AP_RangeFinder_LightWare_GRF_I2C.h"
68 : #include "AP_RangeFinder_DTS6012M.h"
69 :
70 : #include <AP_BoardConfig/AP_BoardConfig.h>
71 : #include <AP_Logger/AP_Logger.h>
72 : #include <AP_SerialManager/AP_SerialManager.h>
73 : #include <AP_Vehicle/AP_Vehicle_Type.h>
74 : #include <AP_HAL/I2CDevice.h>
75 : #include <AP_InternalError/AP_InternalError.h>
76 :
77 : extern const AP_HAL::HAL &hal;
78 :
79 : // table of user settable parameters
80 : const AP_Param::GroupInfo RangeFinder::var_info[] = {
81 :
82 : // @Group: 1_
83 : // @Path: AP_RangeFinder_Params.cpp
84 : AP_SUBGROUPINFO(params[0], "1_", 25, RangeFinder, AP_RangeFinder_Params),
85 :
86 : // @Group: 1_
87 : // @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Backend_CAN.cpp,AP_RangeFinder_LightWare_GRF.cpp,AP_RangeFinder_LightWare_GRF_I2C.cpp
88 : AP_SUBGROUPVARPTR(drivers[0], "1_", 57, RangeFinder, backend_var_info[0]),
89 :
90 : #if RANGEFINDER_MAX_INSTANCES > 1
91 : // @Group: 2_
92 : // @Path: AP_RangeFinder_Params.cpp
93 : AP_SUBGROUPINFO(params[1], "2_", 27, RangeFinder, AP_RangeFinder_Params),
94 :
95 : // @Group: 2_
96 : // @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Backend_CAN.cpp,AP_RangeFinder_LightWare_GRF.cpp,AP_RangeFinder_LightWare_GRF_I2C.cpp
97 : AP_SUBGROUPVARPTR(drivers[1], "2_", 58, RangeFinder, backend_var_info[1]),
98 : #endif
99 :
100 : #if RANGEFINDER_MAX_INSTANCES > 2
101 : // @Group: 3_
102 : // @Path: AP_RangeFinder_Params.cpp
103 : AP_SUBGROUPINFO(params[2], "3_", 29, RangeFinder, AP_RangeFinder_Params),
104 :
105 : // @Group: 3_
106 : // @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Backend_CAN.cpp,AP_RangeFinder_LightWare_GRF.cpp,AP_RangeFinder_LightWare_GRF_I2C.cpp
107 : AP_SUBGROUPVARPTR(drivers[2], "3_", 59, RangeFinder, backend_var_info[2]),
108 : #endif
109 :
110 : #if RANGEFINDER_MAX_INSTANCES > 3
111 : // @Group: 4_
112 : // @Path: AP_RangeFinder_Params.cpp
113 : AP_SUBGROUPINFO(params[3], "4_", 31, RangeFinder, AP_RangeFinder_Params),
114 :
115 : // @Group: 4_
116 : // @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Backend_CAN.cpp,AP_RangeFinder_LightWare_GRF.cpp,AP_RangeFinder_LightWare_GRF_I2C.cpp
117 : AP_SUBGROUPVARPTR(drivers[3], "4_", 60, RangeFinder, backend_var_info[3]),
118 : #endif
119 :
120 : #if RANGEFINDER_MAX_INSTANCES > 4
121 : // @Group: 5_
122 : // @Path: AP_RangeFinder_Params.cpp
123 : AP_SUBGROUPINFO(params[4], "5_", 33, RangeFinder, AP_RangeFinder_Params),
124 :
125 : // @Group: 5_
126 : // @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Backend_CAN.cpp,AP_RangeFinder_LightWare_GRF.cpp,AP_RangeFinder_LightWare_GRF_I2C.cpp
127 : AP_SUBGROUPVARPTR(drivers[4], "5_", 34, RangeFinder, backend_var_info[4]),
128 : #endif
129 :
130 : #if RANGEFINDER_MAX_INSTANCES > 5
131 : // @Group: 6_
132 : // @Path: AP_RangeFinder_Params.cpp
133 : AP_SUBGROUPINFO(params[5], "6_", 35, RangeFinder, AP_RangeFinder_Params),
134 :
135 : // @Group: 6_
136 : // @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Backend_CAN.cpp,AP_RangeFinder_LightWare_GRF.cpp,AP_RangeFinder_LightWare_GRF_I2C.cpp
137 : AP_SUBGROUPVARPTR(drivers[5], "6_", 36, RangeFinder, backend_var_info[5]),
138 : #endif
139 :
140 : #if RANGEFINDER_MAX_INSTANCES > 6
141 : // @Group: 7_
142 : // @Path: AP_RangeFinder_Params.cpp
143 : AP_SUBGROUPINFO(params[6], "7_", 37, RangeFinder, AP_RangeFinder_Params),
144 :
145 : // @Group: 7_
146 : // @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Backend_CAN.cpp,AP_RangeFinder_LightWare_GRF.cpp,AP_RangeFinder_LightWare_GRF_I2C.cpp
147 : AP_SUBGROUPVARPTR(drivers[6], "7_", 38, RangeFinder, backend_var_info[6]),
148 : #endif
149 :
150 : #if RANGEFINDER_MAX_INSTANCES > 7
151 : // @Group: 8_
152 : // @Path: AP_RangeFinder_Params.cpp
153 : AP_SUBGROUPINFO(params[7], "8_", 39, RangeFinder, AP_RangeFinder_Params),
154 :
155 : // @Group: 8_
156 : // @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Backend_CAN.cpp,AP_RangeFinder_LightWare_GRF.cpp,AP_RangeFinder_LightWare_GRF_I2C.cpp
157 : AP_SUBGROUPVARPTR(drivers[7], "8_", 40, RangeFinder, backend_var_info[7]),
158 : #endif
159 :
160 : #if RANGEFINDER_MAX_INSTANCES > 8
161 : // @Group: 9_
162 : // @Path: AP_RangeFinder_Params.cpp
163 : AP_SUBGROUPINFO(params[8], "9_", 41, RangeFinder, AP_RangeFinder_Params),
164 :
165 : // @Group: 9_
166 : // @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Backend_CAN.cpp,AP_RangeFinder_LightWare_GRF.cpp,AP_RangeFinder_LightWare_GRF_I2C.cpp
167 : AP_SUBGROUPVARPTR(drivers[8], "9_", 42, RangeFinder, backend_var_info[8]),
168 : #endif
169 :
170 : #if RANGEFINDER_MAX_INSTANCES > 9
171 : // @Group: A_
172 : // @Path: AP_RangeFinder_Params.cpp
173 : AP_SUBGROUPINFO(params[9], "A_", 43, RangeFinder, AP_RangeFinder_Params),
174 :
175 : // @Group: A_
176 : // @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Backend_CAN.cpp,AP_RangeFinder_LightWare_GRF.cpp,AP_RangeFinder_LightWare_GRF_I2C.cpp
177 : AP_SUBGROUPVARPTR(drivers[9], "A_", 44, RangeFinder, backend_var_info[9]),
178 : #endif
179 :
180 : AP_GROUPEND
181 : };
182 :
183 : const AP_Param::GroupInfo *RangeFinder::backend_var_info[RANGEFINDER_MAX_INSTANCES];
184 :
185 23078 : RangeFinder::RangeFinder()
186 : {
187 2098 : AP_Param::setup_object_defaults(this, var_info);
188 :
189 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
190 2096 : if (_singleton != nullptr) {
191 0 : AP_HAL::panic("Rangefinder must be singleton");
192 : }
193 : #endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL
194 2098 : _singleton = this;
195 2098 : }
196 :
197 2057 : void RangeFinder::convert_params(void)
198 : {
199 : // PARAMETER_CONVERSION - Added: Dec-2024 for ArduPilot-4.7
200 22627 : for (auto &p : params) {
201 20570 : p.convert_min_max_params();
202 : }
203 2057 : }
204 :
205 : /*
206 : initialise the RangeFinder class. We do detection of attached range
207 : finders here. For now we won't allow for hot-plugging of
208 : rangefinders.
209 : */
210 2057 : __INITFUNC__ void RangeFinder::init(enum Rotation orientation_default)
211 : {
212 2057 : convert_params();
213 :
214 2057 : if (num_instances != 0) {
215 : // don't re-init if we've found some sensors already
216 0 : return;
217 : }
218 :
219 : // set orientation defaults
220 22627 : for (uint8_t i=0; i<RANGEFINDER_MAX_INSTANCES; i++) {
221 20570 : params[i].orientation.set_default(orientation_default);
222 : }
223 :
224 22627 : for (uint8_t i=0, serial_instance = 0; i<RANGEFINDER_MAX_INSTANCES; i++) {
225 : // serial_instance will be increased inside detect_instance
226 : // if a serial driver is loaded for this instance
227 20570 : WITH_SEMAPHORE(detect_sem);
228 20570 : detect_instance(i, serial_instance);
229 20570 : if (drivers[i] != nullptr) {
230 : // we loaded a driver for this instance, so it must be
231 : // present (although it may not be healthy). We use MAX()
232 : // here as a UAVCAN rangefinder may already have been
233 : // found
234 203 : num_instances = MAX(num_instances, i+1);
235 : }
236 :
237 : // initialise status
238 20570 : state[i].status = Status::NotConnected;
239 20570 : state[i].range_valid_count = 0;
240 : // initialize signal_quality_pct for drivers that don't handle it.
241 20570 : state[i].signal_quality_pct = SIGNAL_QUALITY_UNKNOWN;
242 : #if AP_TEMPERATURE_SENSOR_ENABLED
243 20570 : state[i].temperature_valid = false;
244 20570 : state[i].temperature_update_ms = 0;
245 : #endif
246 20570 : }
247 : }
248 :
249 : /*
250 : update RangeFinder state for all instances. This should be called at
251 : around 10Hz by main loop
252 : */
253 4994283 : void RangeFinder::update(void)
254 : {
255 5379497 : for (uint8_t i=0; i<num_instances; i++) {
256 385214 : if (drivers[i] != nullptr) {
257 358541 : if ((Type)params[i].type.get() == Type::NONE) {
258 : // allow user to disable a rangefinder at runtime
259 1682 : state[i].status = Status::NotConnected;
260 1682 : state[i].range_valid_count = 0;
261 1682 : continue;
262 : }
263 356859 : drivers[i]->update();
264 : }
265 : }
266 : #if HAL_LOGGING_ENABLED
267 4994283 : Log_RFND();
268 : #endif
269 4994283 : }
270 :
271 209 : __INITFUNC__ bool RangeFinder::_add_backend(AP_RangeFinder_Backend *backend, uint8_t instance, uint8_t serial_instance)
272 : {
273 209 : if (!backend) {
274 6 : return false;
275 : }
276 203 : if (instance >= RANGEFINDER_MAX_INSTANCES) {
277 0 : AP_HAL::panic("Too many RANGERS backends");
278 : }
279 203 : if (drivers[instance] != nullptr) {
280 : // we've allocated the same instance twice
281 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
282 : }
283 203 : backend->init_serial(serial_instance);
284 203 : drivers[instance] = backend;
285 203 : num_instances = MAX(num_instances, instance+1);
286 :
287 203 : return true;
288 : }
289 :
290 : /*
291 : * probe all i2c buses for a backend type:
292 : */
293 14 : __INITFUNC__ void RangeFinder::probe_i2c_buses(uint8_t instance, uint8_t addr, i2c_probe_fn_t detect_fn)
294 : {
295 20 : FOREACH_I2C(i) {
296 20 : auto *device_ptr = hal.i2c_mgr->get_device_ptr(i, addr);
297 20 : if (device_ptr == nullptr) {
298 0 : continue;
299 : }
300 20 : if (_add_backend(detect_fn(state[instance], params[instance], *device_ptr),
301 : instance)) {
302 14 : return;
303 : }
304 6 : delete device_ptr;
305 : }
306 : }
307 :
308 : /*
309 : detect if an instance of a rangefinder is connected.
310 : */
311 20570 : __INITFUNC__ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
312 : {
313 20570 : AP_RangeFinder_Backend_Serial *(*serial_create_fn)(RangeFinder::RangeFinder_State&, AP_RangeFinder_Params&) = nullptr;
314 :
315 20570 : const Type _type = (Type)params[instance].type.get();
316 20570 : switch (_type) {
317 : #if AP_RANGEFINDER_PULSEDLIGHTLRF_ENABLED
318 0 : case Type::PLI2C:
319 : case Type::PLI2CV3:
320 : case Type::PLI2CV3HP:
321 0 : FOREACH_I2C(i) {
322 0 : if (_add_backend(AP_RangeFinder_PulsedLightLRF::detect(i, state[instance], params[instance], _type),
323 : instance)) {
324 0 : break;
325 : }
326 : }
327 0 : break;
328 : #endif
329 : #if AP_RANGEFINDER_MAXSONARI2CXL_ENABLED
330 8 : case Type::MBI2C: {
331 8 : uint8_t addr = AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR;
332 8 : if (params[instance].address != 0) {
333 6 : addr = params[instance].address;
334 : }
335 8 : probe_i2c_buses(instance, addr, AP_RangeFinder_MaxsonarI2CXL::detect);
336 8 : break;
337 : }
338 : #endif
339 : #if AP_RANGEFINDER_LWI2C_ENABLED
340 1 : case Type::LWI2C:
341 1 : if (params[instance].address) {
342 : // the LW20 needs a long time to boot up, so we delay 1.5s here
343 : #ifndef HAL_BUILD_AP_PERIPH
344 1 : if (!hal.util->was_watchdog_armed()) {
345 1 : hal.scheduler->delay(1500);
346 : }
347 : #endif
348 : #ifdef HAL_RANGEFINDER_LIGHTWARE_I2C_BUS
349 : auto *device_ptr = hal.i2c_mgr->get_device_ptr(HAL_RANGEFINDER_LIGHTWARE_I2C_BUS, params[instance].address);
350 : if (device_ptr) {
351 : _add_backend(AP_RangeFinder_LightWareI2C::detect(state[instance], params[instance],
352 : *device_ptr),
353 : instance);
354 : }
355 : #else
356 1 : probe_i2c_buses(instance, params[instance].address, AP_RangeFinder_LightWareI2C::detect);
357 : #endif
358 : }
359 1 : break;
360 : #endif // AP_RANGEFINDER_LWI2C_ENABLED
361 : #if AP_RANGEFINDER_TRI2C_ENABLED
362 1 : case Type::TRI2C:
363 1 : if (params[instance].address) {
364 1 : probe_i2c_buses(instance, params[instance].address, AP_RangeFinder_TeraRangerI2C::detect);
365 : }
366 1 : break;
367 : #endif
368 0 : case Type::VL53L0X:
369 : case Type::VL53L1X_Short:
370 0 : FOREACH_I2C(i) {
371 : #if AP_RANGEFINDER_VL53L0X_ENABLED
372 0 : if (_add_backend(AP_RangeFinder_VL53L0X::detect(state[instance], params[instance],
373 0 : hal.i2c_mgr->get_device(i, params[instance].address)),
374 : instance)) {
375 0 : break;
376 : }
377 : #endif
378 : #if AP_RANGEFINDER_VL53L1X_ENABLED
379 0 : if (_add_backend(AP_RangeFinder_VL53L1X::detect(state[instance], params[instance],
380 0 : hal.i2c_mgr->get_device(i, params[instance].address),
381 : _type == Type::VL53L1X_Short ? AP_RangeFinder_VL53L1X::DistanceMode::Short :
382 : AP_RangeFinder_VL53L1X::DistanceMode::Long),
383 : instance)) {
384 0 : break;
385 : }
386 : #endif
387 : }
388 0 : break;
389 : #if AP_RANGEFINDER_BENEWAKE_TFMINIPLUS_ENABLED
390 2 : case Type::BenewakeTFminiPlus: {
391 2 : uint8_t addr = TFMINIPLUS_ADDR_DEFAULT;
392 2 : if (params[instance].address != 0) {
393 2 : addr = params[instance].address;
394 : }
395 2 : probe_i2c_buses(instance, addr, AP_RangeFinder_Benewake_TFMiniPlus::detect);
396 2 : break;
397 : }
398 : #endif
399 : #if AP_RANGEFINDER_BENEWAKE_TFS20L_ENABLED
400 1 : case Type::BenewakeTFS20L: {
401 1 : uint8_t addr = TFS20L_ADDR_DEFAULT;
402 1 : if (params[instance].address != 0) {
403 1 : addr = params[instance].address;
404 : }
405 1 : probe_i2c_buses(instance, addr, AP_RangeFinder_Benewake_TFS20L::detect);
406 1 : break;
407 : }
408 : #endif
409 : #if AP_RANGEFINDER_PWM_ENABLED
410 : case Type::PX4_PWM:
411 : // to ease moving from PX4 to ChibiOS we'll lie a little about
412 : // the backend driver...
413 : if (AP_RangeFinder_PWM::detect()) {
414 : _add_backend(NEW_NOTHROW AP_RangeFinder_PWM(state[instance], params[instance]), instance);
415 : }
416 : break;
417 : #endif
418 : #if AP_RANGEFINDER_BBB_PRU_ENABLED
419 : case Type::BBB_PRU:
420 : if (AP_RangeFinder_BBB_PRU::detect()) {
421 : _add_backend(NEW_NOTHROW AP_RangeFinder_BBB_PRU(state[instance], params[instance]), instance);
422 : }
423 : break;
424 : #endif
425 : #if AP_RANGEFINDER_LIGHTWARE_SERIAL_ENABLED
426 3 : case Type::LWSER:
427 3 : serial_create_fn = AP_RangeFinder_LightWareSerial::create;
428 3 : break;
429 : #endif
430 : #if AP_RANGEFINDER_LEDDARONE_ENABLED
431 1 : case Type::LEDDARONE:
432 1 : serial_create_fn = AP_RangeFinder_LeddarOne::create;
433 1 : break;
434 : #endif
435 : #if AP_RANGEFINDER_USD1_SERIAL_ENABLED
436 2 : case Type::USD1_Serial:
437 2 : serial_create_fn = AP_RangeFinder_USD1_Serial::create;
438 2 : break;
439 : #endif
440 : #if AP_RANGEFINDER_BEBOP_ENABLED
441 : case Type::BEBOP:
442 : if (AP_RangeFinder_Bebop::detect()) {
443 : _add_backend(NEW_NOTHROW AP_RangeFinder_Bebop(state[instance], params[instance]), instance);
444 : }
445 : break;
446 : #endif
447 : #if AP_RANGEFINDER_MAVLINK_ENABLED
448 4 : case Type::MAVLink:
449 4 : if (AP_RangeFinder_MAVLink::detect()) {
450 4 : _add_backend(NEW_NOTHROW AP_RangeFinder_MAVLink(state[instance], params[instance]), instance);
451 : }
452 4 : break;
453 : #endif
454 : #if AP_RANGEFINDER_MAXBOTIX_SERIAL_ENABLED
455 1 : case Type::MBSER:
456 1 : serial_create_fn = AP_RangeFinder_MaxsonarSerialLV::create;
457 1 : break;
458 : #endif
459 : #if AP_RANGEFINDER_ANALOG_ENABLED
460 130 : case Type::ANALOG:
461 : // note that analog will always come back as present if the pin is valid
462 130 : if (AP_RangeFinder_analog::detect(params[instance])) {
463 130 : _add_backend(NEW_NOTHROW AP_RangeFinder_analog(state[instance], params[instance]), instance);
464 : }
465 130 : break;
466 : #endif
467 : #if AP_RANGEFINDER_HC_SR04_ENABLED
468 0 : case Type::HC_SR04:
469 : // note that this will always come back as present if the pin is valid
470 0 : if (AP_RangeFinder_HC_SR04::detect(params[instance])) {
471 0 : _add_backend(NEW_NOTHROW AP_RangeFinder_HC_SR04(state[instance], params[instance]), instance);
472 : }
473 0 : break;
474 : #endif
475 : #if AP_RANGEFINDER_NMEA_ENABLED
476 3 : case Type::NMEA:
477 3 : serial_create_fn = AP_RangeFinder_NMEA::create;
478 3 : break;
479 : #endif
480 : #if AP_RANGEFINDER_WASP_ENABLED
481 1 : case Type::WASP:
482 1 : serial_create_fn = AP_RangeFinder_Wasp::create;
483 1 : break;
484 : #endif
485 : #if AP_RANGEFINDER_BENEWAKE_TF02_ENABLED
486 2 : case Type::BenewakeTF02:
487 2 : serial_create_fn = AP_RangeFinder_Benewake_TF02::create;
488 2 : break;
489 : #endif
490 : #if AP_RANGEFINDER_BENEWAKE_TFMINI_ENABLED
491 1 : case Type::BenewakeTFmini:
492 1 : serial_create_fn = AP_RangeFinder_Benewake_TFMini::create;
493 1 : break;
494 : #endif
495 : #if AP_RANGEFINDER_BENEWAKE_TF03_ENABLED
496 1 : case Type::BenewakeTF03:
497 1 : serial_create_fn = AP_RangeFinder_Benewake_TF03::create;
498 1 : break;
499 : #endif
500 : #if AP_RANGEFINDER_TERARANGER_SERIAL_ENABLED
501 1 : case Type::TeraRanger_Serial:
502 1 : serial_create_fn = AP_RangeFinder_TeraRanger_Serial::create;
503 1 : break;
504 : #endif
505 : #if AP_RANGEFINDER_PWM_ENABLED
506 : case Type::PWM:
507 : if (AP_RangeFinder_PWM::detect()) {
508 : _add_backend(NEW_NOTHROW AP_RangeFinder_PWM(state[instance], params[instance]), instance);
509 : }
510 : break;
511 : #endif
512 : #if AP_RANGEFINDER_BLPING_ENABLED
513 1 : case Type::BLPing:
514 1 : serial_create_fn = AP_RangeFinder_BLPing::create;
515 1 : break;
516 : #endif
517 : #if AP_RANGEFINDER_LANBAO_ENABLED
518 1 : case Type::Lanbao:
519 1 : serial_create_fn = AP_RangeFinder_Lanbao::create;
520 1 : break;
521 : #endif
522 : #if AP_RANGEFINDER_LEDDARVU8_ENABLED
523 0 : case Type::LeddarVu8_Serial:
524 0 : serial_create_fn = AP_RangeFinder_LeddarVu8::create;
525 0 : break;
526 : #endif
527 :
528 : #if AP_RANGEFINDER_DRONECAN_ENABLED
529 0 : case Type::UAVCAN:
530 : /*
531 : the UAVCAN driver gets created when we first receive a
532 : measurement. We take the instance slot now, even if we don't
533 : yet have the driver
534 : */
535 0 : num_instances = MAX(num_instances, instance+1);
536 0 : break;
537 : #endif
538 :
539 : #if AP_RANGEFINDER_GYUS42V2_ENABLED
540 1 : case Type::GYUS42v2:
541 1 : serial_create_fn = AP_RangeFinder_GYUS42v2::create;
542 1 : break;
543 : #endif
544 :
545 : #if AP_RANGEFINDER_SIM_ENABLED
546 23 : case Type::SIM:
547 23 : _add_backend(NEW_NOTHROW AP_RangeFinder_SITL(state[instance], params[instance], instance), instance);
548 23 : break;
549 : #endif
550 :
551 : #if HAL_MSP_RANGEFINDER_ENABLED
552 0 : case Type::MSP:
553 0 : if (AP_RangeFinder_MSP::detect()) {
554 0 : _add_backend(NEW_NOTHROW AP_RangeFinder_MSP(state[instance], params[instance]), instance);
555 : }
556 0 : break;
557 : #endif // HAL_MSP_RANGEFINDER_ENABLED
558 :
559 : #if AP_RANGEFINDER_USD1_CAN_ENABLED
560 0 : case Type::USD1_CAN:
561 0 : _add_backend(NEW_NOTHROW AP_RangeFinder_USD1_CAN(state[instance], params[instance]), instance);
562 0 : break;
563 : #endif
564 : #if AP_RANGEFINDER_BENEWAKE_CAN_ENABLED
565 0 : case Type::Benewake_CAN:
566 0 : _add_backend(NEW_NOTHROW AP_RangeFinder_Benewake_CAN(state[instance], params[instance]), instance);
567 0 : break;
568 : #endif
569 :
570 : #if AP_RANGEFINDER_LUA_ENABLED
571 5 : case Type::Lua_Scripting:
572 5 : _add_backend(NEW_NOTHROW AP_RangeFinder_Lua(state[instance], params[instance]), instance);
573 5 : break;
574 : #endif
575 :
576 : #if AP_RANGEFINDER_NOOPLOOP_ENABLED
577 1 : case Type::NoopLoop_P:
578 1 : serial_create_fn = AP_RangeFinder_NoopLoop::create;
579 1 : break;
580 : #endif
581 :
582 : #if AP_RANGEFINDER_AINSTEIN_LR_D1_ENABLED
583 3 : case Type::Ainstein_LR_D1:
584 3 : serial_create_fn = AP_RangeFinder_Ainstein_LR_D1::create;
585 3 : break;
586 : #endif
587 :
588 : #if AP_RANGEFINDER_TOFSENSEP_CAN_ENABLED
589 0 : case Type::TOFSenseP_CAN:
590 0 : _add_backend(NEW_NOTHROW AP_RangeFinder_TOFSenseP_CAN(state[instance], params[instance]), instance);
591 0 : break;
592 : #endif
593 : #if AP_RANGEFINDER_NRA24_CAN_DRIVER_ENABLED
594 : #if AP_RANGEFINDER_NRA24_CAN_ENABLED
595 0 : case Type::NRA24_CAN:
596 : #endif
597 : #if AP_RANGEFINDER_HEXSOONRADAR_ENABLED
598 : case Type::HEXSOON_RADAR:
599 : #endif
600 0 : _add_backend(NEW_NOTHROW AP_RangeFinder_NRA24_CAN(state[instance], params[instance]), instance);
601 0 : break;
602 : #endif // AP_RANGEFINDER_NRA24_CAN_DRIVER_ENABLED
603 : #if AP_RANGEFINDER_TOFSENSEF_I2C_ENABLED
604 1 : case Type::TOFSenseF_I2C: {
605 1 : uint8_t addr = TOFSENSEP_I2C_DEFAULT_ADDR;
606 1 : if (params[instance].address != 0) {
607 1 : addr = params[instance].address;
608 : }
609 1 : FOREACH_I2C(i) {
610 1 : auto *device_ptr = hal.i2c_mgr->get_device_ptr(i, addr);
611 1 : if (device_ptr == nullptr) {
612 0 : continue;
613 : }
614 1 : if (_add_backend(AP_RangeFinder_TOFSenseF_I2C::detect(state[instance], params[instance],
615 : *device_ptr),
616 : instance)) {
617 1 : break;
618 : }
619 0 : delete device_ptr;
620 : }
621 1 : break;
622 : }
623 : #endif
624 : #if AP_RANGEFINDER_JRE_SERIAL_ENABLED
625 0 : case Type::JRE_Serial:
626 0 : serial_create_fn = AP_RangeFinder_JRE_Serial::create;
627 0 : break;
628 : #endif
629 :
630 : #if AP_RANGEFINDER_RDS02UF_ENABLED
631 1 : case Type::RDS02UF:
632 1 : serial_create_fn = AP_RangeFinder_RDS02UF::create;
633 1 : break;
634 : #endif
635 :
636 : #if AP_RANGEFINDER_LIGHTWARE_GRF_ENABLED
637 1 : case Type::LightWare_GRF:
638 1 : serial_create_fn = AP_RangeFinder_LightWareGRF::create;
639 1 : break;
640 : #endif // AP_RANGEFINDER_LIGHTWARE_GRF_ENABLED
641 :
642 : #if AP_RANGEFINDER_LIGHTWARE_GRF_I2C_ENABLED
643 1 : case Type::LightWare_GRF_I2C: {
644 2 : const uint8_t addr = params[instance].address ? params[instance].address
645 1 : : AP_RANGEFINDER_LIGHTWARE_GRF_I2C_DEFAULT_ADDR;
646 1 : probe_i2c_buses(instance, addr, AP_RangeFinder_LightWare_GRF_I2C::detect);
647 1 : break;
648 : }
649 : #endif // AP_RANGEFINDER_LIGHTWARE_GRF_I2C_ENABLED
650 :
651 : #if AP_RANGEFINDER_DTS6012M_ENABLED
652 1 : case Type::DTS6012M:
653 1 : serial_create_fn = AP_RangeFinder_DTS6012M::create;
654 1 : break;
655 : #endif // AP_RANGEFINDER_DTS6012M_ENABLED
656 :
657 20367 : case Type::NONE:
658 20367 : break;
659 : }
660 :
661 20570 : if (serial_create_fn != nullptr) {
662 26 : if (AP::serialmanager().have_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance)) {
663 26 : auto *b = serial_create_fn(state[instance], params[instance]);
664 26 : if (b != nullptr) {
665 26 : _add_backend(b, instance, serial_instance++);
666 : }
667 : }
668 : }
669 :
670 : // if the backend has some local parameters then make those available in the tree
671 20570 : if (drivers[instance] && state[instance].var_info) {
672 3 : backend_var_info[instance] = state[instance].var_info;
673 3 : AP_Param::load_object_from_eeprom(drivers[instance], backend_var_info[instance]);
674 :
675 : // param count could have changed
676 3 : AP_Param::invalidate_count();
677 : }
678 20570 : }
679 :
680 80824434 : AP_RangeFinder_Backend *RangeFinder::get_backend(uint8_t id) const {
681 80824434 : if (id >= num_instances) {
682 58378145 : return nullptr;
683 : }
684 22446289 : if (drivers[id] != nullptr) {
685 22288225 : if (drivers[id]->type() == Type::NONE) {
686 : // pretend it isn't here; disabled at runtime?
687 136774 : return nullptr;
688 : }
689 : }
690 22309514 : return drivers[id];
691 : };
692 :
693 5902870 : RangeFinder::Status RangeFinder::status_orient(enum Rotation orientation) const
694 : {
695 5902870 : AP_RangeFinder_Backend *backend = find_instance(orientation);
696 5902870 : if (backend == nullptr) {
697 5583538 : return Status::NotConnected;
698 : }
699 319332 : return backend->status();
700 : }
701 :
702 497 : void RangeFinder::handle_msg(const mavlink_message_t &msg)
703 : {
704 : uint8_t i;
705 988 : for (i=0; i<num_instances; i++) {
706 491 : if ((drivers[i] != nullptr) && ((Type)params[i].type.get() != Type::NONE)) {
707 486 : drivers[i]->handle_msg(msg);
708 : }
709 : }
710 497 : }
711 :
712 : #if HAL_MSP_RANGEFINDER_ENABLED
713 0 : void RangeFinder::handle_msp(const MSP::msp_rangefinder_data_message_t &pkt)
714 : {
715 : uint8_t i;
716 0 : for (i=0; i<num_instances; i++) {
717 0 : if ((drivers[i] != nullptr) && ((Type)params[i].type.get() == Type::MSP)) {
718 0 : drivers[i]->handle_msp(pkt);
719 : }
720 : }
721 0 : }
722 : #endif // HAL_MSP_RANGEFINDER_ENABLED
723 :
724 : // return true if we have a range finder with the specified orientation
725 1528918 : bool RangeFinder::has_orientation(enum Rotation orientation) const
726 : {
727 1528918 : if ((find_instance(orientation) != nullptr)) {
728 : // we have a rangefinder with this orientation
729 70512 : return true;
730 : }
731 :
732 : // special case for DroneCAN
733 : // DroneCAN rangefinder backend is not created until we receive a
734 : // measurement, so we need to check the params directly
735 : #if AP_RANGEFINDER_DRONECAN_ENABLED
736 16042466 : for (uint8_t i = 0; i < RANGEFINDER_MAX_INSTANCES; i++) {
737 14584060 : if ((RangeFinder::Type)params[i].type.get() == RangeFinder::Type::UAVCAN) {
738 0 : if (params[i].orientation.get() == orientation) {
739 0 : return true;
740 : }
741 : }
742 : }
743 : #endif
744 :
745 : // no rangefinder with this orientation
746 1458406 : return false;
747 : }
748 :
749 : // find first range finder instance with the specified orientation
750 23224817 : AP_RangeFinder_Backend *RangeFinder::find_instance(enum Rotation orientation) const
751 : {
752 : // first try for a rangefinder that is in range
753 29045587 : for (uint8_t i=0; i<num_instances; i++) {
754 11751471 : AP_RangeFinder_Backend *backend = get_backend(i);
755 11616776 : if (backend != nullptr &&
756 23368246 : backend->orientation() == orientation &&
757 11060310 : backend->status() == Status::Good) {
758 5930701 : return backend;
759 : }
760 : }
761 : // if none in range then return first with correct orientation
762 17876610 : for (uint8_t i=0; i<num_instances; i++) {
763 4934188 : AP_RangeFinder_Backend *backend = get_backend(i);
764 9816760 : if (backend != nullptr &&
765 4882572 : backend->orientation() == orientation) {
766 4351694 : return backend;
767 : }
768 : }
769 12942422 : return nullptr;
770 : }
771 :
772 5892381 : float RangeFinder::distance_orient(enum Rotation orientation) const
773 : {
774 5892381 : AP_RangeFinder_Backend *backend = find_instance(orientation);
775 5892381 : if (backend == nullptr) {
776 5583503 : return 0;
777 : }
778 308878 : return backend->distance();
779 : }
780 :
781 109027 : int8_t RangeFinder::signal_quality_pct_orient(enum Rotation orientation) const
782 : {
783 109027 : AP_RangeFinder_Backend *backend = find_instance(orientation);
784 109027 : if (backend == nullptr) {
785 1445 : return RangeFinder::SIGNAL_QUALITY_UNKNOWN;
786 : }
787 107582 : return backend->signal_quality_pct();
788 : }
789 :
790 109016 : float RangeFinder::min_distance_orient(enum Rotation orientation) const
791 : {
792 109016 : AP_RangeFinder_Backend *backend = find_instance(orientation);
793 109016 : if (backend == nullptr) {
794 1445 : return 0; // consider NaN
795 : }
796 107571 : return backend->min_distance();
797 : }
798 :
799 4779630 : float RangeFinder::max_distance_orient(enum Rotation orientation) const
800 : {
801 4779630 : AP_RangeFinder_Backend *backend = find_instance(orientation);
802 4779630 : if (backend == nullptr) {
803 152861 : return 0; // consider NaN
804 : }
805 4626769 : return backend->max_distance();
806 : }
807 :
808 4663689 : float RangeFinder::ground_clearance_orient(enum Rotation orientation) const
809 : {
810 4663689 : AP_RangeFinder_Backend *backend = find_instance(orientation);
811 4663689 : if (backend == nullptr) {
812 151416 : return 0; // consider NaN
813 : }
814 4512273 : return backend->ground_clearance();
815 : }
816 :
817 116022 : bool RangeFinder::has_data_orient(enum Rotation orientation) const
818 : {
819 116022 : AP_RangeFinder_Backend *backend = find_instance(orientation);
820 116022 : if (backend == nullptr) {
821 9808 : return false;
822 : }
823 106214 : return backend->has_data();
824 : }
825 :
826 118867 : uint8_t RangeFinder::range_valid_count_orient(enum Rotation orientation) const
827 : {
828 118867 : AP_RangeFinder_Backend *backend = find_instance(orientation);
829 118867 : if (backend == nullptr) {
830 0 : return 0;
831 : }
832 118867 : return backend->range_valid_count();
833 : }
834 :
835 0 : const Vector3f &RangeFinder::get_pos_offset_orient(enum Rotation orientation) const
836 : {
837 0 : AP_RangeFinder_Backend *backend = find_instance(orientation);
838 0 : if (backend == nullptr) {
839 0 : return pos_offset_zero;
840 : }
841 0 : return backend->get_pos_offset();
842 : }
843 :
844 0 : uint32_t RangeFinder::last_reading_ms(enum Rotation orientation) const
845 : {
846 0 : AP_RangeFinder_Backend *backend = find_instance(orientation);
847 0 : if (backend == nullptr) {
848 0 : return 0;
849 : }
850 0 : return backend->last_reading_ms();
851 : }
852 :
853 0 : MAV_DISTANCE_SENSOR RangeFinder::get_mav_distance_sensor_type_orient(enum Rotation orientation) const
854 : {
855 0 : AP_RangeFinder_Backend *backend = find_instance(orientation);
856 0 : if (backend == nullptr) {
857 0 : return MAV_DISTANCE_SENSOR_UNKNOWN;
858 : }
859 0 : return backend->get_mav_distance_sensor_type();
860 : }
861 :
862 : // get temperature reading in C. returns true on success and populates temp argument
863 0 : bool RangeFinder::get_temp(enum Rotation orientation, float &temp) const
864 : {
865 0 : AP_RangeFinder_Backend *backend = find_instance(orientation);
866 0 : if (backend == nullptr) {
867 0 : return false;
868 : }
869 0 : return backend->get_temp(temp);
870 : }
871 :
872 : #if AP_TEMPERATURE_SENSOR_ENABLED
873 : // set an externally-measured temperature (C) for a rangefinder instance.
874 : // used by AP_TemperatureSensor when TEMPx_SRC is set to Rangefinder.
875 409 : void RangeFinder::set_temperature_C(uint8_t instance, float temperature_C)
876 : {
877 409 : if (instance >= RANGEFINDER_MAX_INSTANCES) {
878 0 : return;
879 : }
880 409 : state[instance].temperature_C = temperature_C;
881 409 : state[instance].temperature_valid = true;
882 409 : state[instance].temperature_update_ms = AP_HAL::millis();
883 : }
884 : #endif
885 :
886 : #if HAL_LOGGING_ENABLED
887 : // Write an RFND (rangefinder) packet
888 4994283 : void RangeFinder::Log_RFND() const
889 : {
890 4994283 : if (_log_rfnd_bit == uint32_t(-1)) {
891 0 : return;
892 : }
893 :
894 4994283 : AP_Logger &logger = AP::logger();
895 4994283 : if (!logger.should_log(_log_rfnd_bit)) {
896 225608 : return;
897 : }
898 :
899 52455425 : for (uint8_t i=0; i<RANGEFINDER_MAX_INSTANCES; i++) {
900 47686750 : const AP_RangeFinder_Backend *s = get_backend(i);
901 47686750 : if (s == nullptr) {
902 47342173 : continue;
903 : }
904 :
905 344577 : float temperature = logger.quiet_nanf();
906 : float temp;
907 344577 : if (s->get_temp(temp)) {
908 12484 : temperature = temp;
909 : }
910 344577 : const struct log_RFND pkt = {
911 : LOG_PACKET_HEADER_INIT(LOG_RFND_MSG),
912 344577 : time_us : AP_HAL::micros64(),
913 : instance : i,
914 344577 : dist : s->distance(),
915 344577 : status : (uint8_t)s->status(),
916 344577 : orient : s->orientation(),
917 344577 : quality : s->signal_quality_pct(),
918 : temperature : temperature,
919 344577 : };
920 344577 : AP::logger().WriteBlock(&pkt, sizeof(pkt));
921 : }
922 : }
923 : #endif // HAL_LOGGING_ENABLED
924 :
925 133080 : bool RangeFinder::prearm_healthy(char *failure_msg, const uint8_t failure_msg_len) const
926 : {
927 1463082 : for (uint8_t i = 0; i < RANGEFINDER_MAX_INSTANCES; i++) {
928 1330082 : if ((Type)params[i].type.get() == Type::NONE) {
929 1326256 : continue;
930 : }
931 :
932 3826 : if (drivers[i] == nullptr) {
933 58 : hal.util->snprintf(failure_msg, failure_msg_len, "Rangefinder %X: Not Detected", i + 1);
934 58 : return false;
935 : }
936 :
937 : // backend-specific checks. This might end up drivers[i]->arming_checks(...).
938 3768 : switch (drivers[i]->allocated_type()) {
939 : #if AP_RANGEFINDER_ANALOG_ENABLED || AP_RANGEFINDER_PWM_ENABLED
940 : #if AP_RANGEFINDER_ANALOG_ENABLED
941 1378 : case Type::ANALOG:
942 : #endif
943 : #if AP_RANGEFINDER_PWM_ENABLED
944 : case Type::PX4_PWM:
945 : case Type::PWM:
946 : #endif
947 : {
948 : // ensure pin is configured
949 1378 : if (params[i].pin == -1) {
950 0 : hal.util->snprintf(failure_msg, failure_msg_len, "RNGFND%u_PIN not set", unsigned(i + 1));
951 0 : return false;
952 : }
953 : #if AP_RANGEFINDER_ANALOG_ENABLED
954 1378 : if (drivers[i]->allocated_type() == Type::ANALOG) {
955 : // Analog backend does not use GPIO pin
956 1378 : break;
957 : }
958 : #endif
959 :
960 : // ensure that the pin we're configured to use is available
961 0 : if (!hal.gpio->valid_pin(params[i].pin)) {
962 : uint8_t servo_ch;
963 0 : if (hal.gpio->pin_to_servo_channel(params[i].pin, servo_ch)) {
964 0 : hal.util->snprintf(failure_msg, failure_msg_len, "RNGFND%u_PIN=%d, set SERVO%u_FUNCTION=-1", unsigned(i + 1), int(params[i].pin.get()), unsigned(servo_ch+1));
965 : } else {
966 0 : hal.util->snprintf(failure_msg, failure_msg_len, "RNGFND%u_PIN=%d invalid", unsigned(i + 1), int(params[i].pin.get()));
967 : }
968 0 : return false;
969 : }
970 0 : break;
971 : }
972 : #endif // AP_RANGEFINDER_ANALOG_ENABLED || AP_RANGEFINDER_PWM_ENABLED
973 :
974 : #if AP_RANGEFINDER_NRA24_CAN_DRIVER_ENABLED
975 : #if AP_RANGEFINDER_NRA24_CAN_ENABLED
976 0 : case Type::NRA24_CAN:
977 : #endif
978 : #if AP_RANGEFINDER_HEXSOONRADAR_ENABLED
979 : case Type::HEXSOON_RADAR:
980 : #endif
981 : {
982 0 : if (drivers[i]->status() == Status::NoData) {
983 : // This sensor stops sending data if there is no relative motion. This will mostly happen during takeoff, before arming
984 : // To avoid pre-arm failure, return true even though there is no data.
985 : // This sensor also sends a "heartbeat" so we can differentiate between "NoData" and "NotConnected"
986 0 : return true;
987 : }
988 0 : break;
989 : }
990 : #endif // AP_RANGEFINDER_NRA24_CAN_DRIVER_ENABLED
991 :
992 2390 : default:
993 2390 : break;
994 : }
995 :
996 3768 : switch (drivers[i]->status()) {
997 0 : case Status::PoweredDown:
998 0 : hal.util->snprintf(failure_msg, failure_msg_len, "Rangefinder %X: Powered Down", i + 1);
999 0 : return false;
1000 22 : case Status::NoData:
1001 22 : hal.util->snprintf(failure_msg, failure_msg_len, "Rangefinder %X: No Data", i + 1);
1002 22 : return false;
1003 0 : case Status::NotConnected:
1004 0 : hal.util->snprintf(failure_msg, failure_msg_len, "Rangefinder %X: Not Connected", i + 1);
1005 0 : return false;
1006 3746 : case Status::OutOfRangeLow:
1007 : case Status::OutOfRangeHigh:
1008 : case Status::Good:
1009 3746 : break;
1010 : }
1011 : }
1012 :
1013 133000 : return true;
1014 : }
1015 :
1016 : RangeFinder *RangeFinder::_singleton;
1017 :
1018 : namespace AP {
1019 :
1020 6528079 : RangeFinder *rangefinder()
1021 : {
1022 6528079 : return RangeFinder::get_singleton();
1023 : }
1024 :
1025 : }
1026 :
1027 : #endif // AP_RANGEFINDER_ENABLED
|
