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_Proximity.h"
17 :
18 : #if HAL_PROXIMITY_ENABLED
19 : #include "AP_Proximity_Backend.h"
20 : #include "AP_Proximity_RPLidarA2.h"
21 : #include "AP_Proximity_TeraRangerTower.h"
22 : #include "AP_Proximity_TeraRangerTowerEvo.h"
23 : #include "AP_Proximity_RangeFinder.h"
24 : #include "AP_Proximity_MAV.h"
25 : #include "AP_Proximity_LightWareSF40C.h"
26 : #include "AP_Proximity_LightWareSF45B.h"
27 : #include "AP_Proximity_SITL.h"
28 : #include "AP_Proximity_AirSimSITL.h"
29 : #include "AP_Proximity_Cygbot_D1.h"
30 : #include "AP_Proximity_DroneCAN.h"
31 : #include "AP_Proximity_Scripting.h"
32 : #include "AP_Proximity_LD06.h"
33 : #include "AP_Proximity_MR72_CAN.h"
34 :
35 :
36 : #include <AP_Logger/AP_Logger.h>
37 :
38 : extern const AP_HAL::HAL &hal;
39 :
40 : // table of user settable parameters
41 : const AP_Param::GroupInfo AP_Proximity::var_info[] = {
42 : // 0 is reserved for possible addition of an ENABLED parameter
43 :
44 : // 1 was _TYPE
45 : // 2 was _ORIENT
46 : // 3 was _YAW_CORR
47 : // 4 to 15 was _IGN_ANG1 to _IGN_WID6
48 :
49 : // @Param{Copter}: _IGN_GND
50 : // @DisplayName: Proximity sensor land detection
51 : // @Description: Ignore proximity data that is within 1 meter of the ground below the vehicle. This requires a downward facing rangefinder
52 : // @Values: 0:Disabled, 1:Enabled
53 : // @User: Standard
54 : AP_GROUPINFO_FRAME("_IGN_GND", 16, AP_Proximity, _ign_gnd_enable, 0, AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_HELI | AP_PARAM_FRAME_TRICOPTER),
55 :
56 : // @Param: _LOG_RAW
57 : // @DisplayName: Proximity raw distances log
58 : // @Description: Set this parameter to one if logging unfiltered(raw) distances from sensor should be enabled
59 : // @Values: 0:Off, 1:On
60 : // @User: Advanced
61 : AP_GROUPINFO("_LOG_RAW", 17, AP_Proximity, _raw_log_enable, 0),
62 :
63 : // @Param: _FILT
64 : // @DisplayName: Proximity filter cutoff frequency
65 : // @Description: Cutoff frequency for low pass filter applied to each face in the proximity boundary
66 : // @Units: Hz
67 : // @Range: 0 20
68 : // @User: Advanced
69 : AP_GROUPINFO("_FILT", 18, AP_Proximity, _filt_freq, 0.25f),
70 :
71 : // 19 was _MIN
72 : // 20 was _MAX
73 :
74 : // @Param{Copter}: _ALT_MIN
75 : // @DisplayName: Proximity lowest altitude.
76 : // @Description: Minimum altitude below which proximity should not work.
77 : // @Units: m
78 : // @Range: 0 10
79 : // @User: Advanced
80 : AP_GROUPINFO_FRAME("_ALT_MIN", 25, AP_Proximity, _alt_min_m, 1.0f, AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_HELI | AP_PARAM_FRAME_TRICOPTER),
81 :
82 : // @Group: 1
83 : // @Path: AP_Proximity_Params.cpp
84 : AP_SUBGROUPINFO(params[0], "1", 21, AP_Proximity, AP_Proximity_Params),
85 :
86 : // @Group: 1_
87 : // @Path: AP_Proximity_MR72_CAN.cpp
88 : AP_SUBGROUPVARPTR(drivers[0], "1_", 26, AP_Proximity, backend_var_info[0]),
89 :
90 : #if AP_PROXIMITY_MAX_INSTANCES > 1
91 : // @Group: 2
92 : // @Path: AP_Proximity_Params.cpp
93 : AP_SUBGROUPINFO(params[1], "2", 22, AP_Proximity, AP_Proximity_Params),
94 :
95 : // @Group: 2_
96 : // @Path: AP_Proximity_MR72_CAN.cpp
97 : AP_SUBGROUPVARPTR(drivers[1], "2_", 27, AP_Proximity, backend_var_info[1]),
98 : #endif
99 :
100 : #if AP_PROXIMITY_MAX_INSTANCES > 2
101 : // @Group: 3
102 : // @Path: AP_Proximity_Params.cpp
103 : AP_SUBGROUPINFO(params[2], "3", 23, AP_Proximity, AP_Proximity_Params),
104 :
105 : // @Group: 3_
106 : // @Path: AP_Proximity_MR72_CAN.cpp
107 : AP_SUBGROUPVARPTR(drivers[2], "3_", 28, AP_Proximity, backend_var_info[2]),
108 : #endif
109 :
110 : #if AP_PROXIMITY_MAX_INSTANCES > 3
111 : // @Group: 4
112 : // @Path: AP_Proximity_Params.cpp
113 : AP_SUBGROUPINFO(params[3], "4", 24, AP_Proximity, AP_Proximity_Params),
114 :
115 : // @Group: 4_
116 : // @Path: AP_Proximity_MR72_CAN.cpp
117 : AP_SUBGROUPVARPTR(drivers[3], "4_", 29, AP_Proximity, backend_var_info[3]),
118 : #endif
119 :
120 : #if AP_PROXIMITY_MAX_INSTANCES > 4
121 : // @Group: 5
122 : // @Path: AP_Proximity_Params.cpp
123 : AP_SUBGROUPINFO(params[4], "5", 30, AP_Proximity, AP_Proximity_Params),
124 :
125 : // @Group: 5_
126 : // @Path: AP_Proximity_MR72_CAN.cpp
127 : AP_SUBGROUPVARPTR(drivers[4], "5_", 31, AP_Proximity, backend_var_info[4]),
128 : #endif
129 :
130 : AP_GROUPEND
131 : };
132 :
133 : const AP_Param::GroupInfo *AP_Proximity::backend_var_info[AP_PROXIMITY_MAX_INSTANCES];
134 :
135 8214 : AP_Proximity::AP_Proximity()
136 : {
137 1369 : AP_Param::setup_object_defaults(this, var_info);
138 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
139 1369 : if (_singleton != nullptr) {
140 0 : AP_HAL::panic("AP_Proximity must be singleton");
141 : }
142 : #endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL
143 1369 : _singleton = this;
144 1369 : }
145 :
146 : // initialise the Proximity class. We do detection of attached sensors here
147 : // we don't allow for hot-plugging of sensors (i.e. reboot required)
148 1283 : void AP_Proximity::init()
149 : {
150 1283 : if (num_instances != 0) {
151 : // init called a 2nd time?
152 0 : return;
153 : }
154 :
155 : // instantiate backends
156 1283 : uint8_t serial_instance = 0;
157 : (void)serial_instance; // in case no serial backends are compiled in
158 7698 : for (uint8_t instance=0; instance<AP_PROXIMITY_MAX_INSTANCES; instance++) {
159 6415 : switch (get_type(instance)) {
160 6400 : case Type::None:
161 6400 : break;
162 : #if AP_PROXIMITY_RPLIDARA2_ENABLED
163 3 : case Type::RPLidarA2:
164 3 : if (AP_Proximity_RPLidarA2::detect(serial_instance)) {
165 3 : state[instance].instance = instance;
166 3 : drivers[instance] = NEW_NOTHROW AP_Proximity_RPLidarA2(*this, state[instance], params[instance], serial_instance);
167 3 : serial_instance++;
168 : }
169 3 : break;
170 : #endif
171 : #if AP_PROXIMITY_MAV_ENABLED
172 4 : case Type::MAV:
173 4 : state[instance].instance = instance;
174 4 : drivers[instance] = NEW_NOTHROW AP_Proximity_MAV(*this, state[instance], params[instance]);
175 4 : break;
176 : #endif
177 : #if AP_PROXIMITY_TERARANGERTOWER_ENABLED
178 1 : case Type::TRTOWER:
179 1 : if (AP_Proximity_TeraRangerTower::detect(serial_instance)) {
180 1 : state[instance].instance = instance;
181 1 : drivers[instance] = NEW_NOTHROW AP_Proximity_TeraRangerTower(*this, state[instance], params[instance], serial_instance);
182 1 : serial_instance++;
183 : }
184 1 : break;
185 : #endif
186 : #if AP_PROXIMITY_TERARANGERTOWEREVO_ENABLED
187 0 : case Type::TRTOWEREVO:
188 0 : if (AP_Proximity_TeraRangerTowerEvo::detect(serial_instance)) {
189 0 : state[instance].instance = instance;
190 0 : drivers[instance] = NEW_NOTHROW AP_Proximity_TeraRangerTowerEvo(*this, state[instance], params[instance], serial_instance);
191 0 : serial_instance++;
192 : }
193 0 : break;
194 : #endif
195 : #if AP_PROXIMITY_RANGEFINDER_ENABLED
196 0 : case Type::RangeFinder:
197 0 : state[instance].instance = instance;
198 0 : drivers[instance] = NEW_NOTHROW AP_Proximity_RangeFinder(*this, state[instance], params[instance]);
199 0 : break;
200 : #endif
201 : #if AP_PROXIMITY_LIGHTWARE_SF40C_ENABLED
202 0 : case Type::SF40C:
203 0 : if (AP_Proximity_LightWareSF40C::detect(serial_instance)) {
204 0 : state[instance].instance = instance;
205 0 : drivers[instance] = NEW_NOTHROW AP_Proximity_LightWareSF40C(*this, state[instance], params[instance], serial_instance);
206 0 : serial_instance++;
207 : }
208 0 : break;
209 : #endif
210 : #if AP_PROXIMITY_LIGHTWARE_SF45B_ENABLED
211 1 : case Type::SF45B:
212 1 : if (AP_Proximity_LightWareSF45B::detect(serial_instance)) {
213 1 : state[instance].instance = instance;
214 1 : drivers[instance] = NEW_NOTHROW AP_Proximity_LightWareSF45B(*this, state[instance], params[instance], serial_instance);
215 1 : serial_instance++;
216 : }
217 1 : break;
218 : #endif
219 : #if AP_PROXIMITY_CYGBOT_ENABLED
220 0 : case Type::CYGBOT_D1:
221 0 : if (AP_Proximity_Cygbot_D1::detect(serial_instance)) {
222 0 : state[instance].instance = instance;
223 0 : drivers[instance] = NEW_NOTHROW AP_Proximity_Cygbot_D1(*this, state[instance], params[instance], serial_instance);
224 0 : serial_instance++;
225 : }
226 0 : break;
227 : # endif
228 : #if AP_PROXIMITY_DRONECAN_ENABLED
229 0 : case Type::DroneCAN:
230 0 : num_instances = instance+1;
231 0 : break;
232 : #endif
233 : #if AP_PROXIMITY_SCRIPTING_ENABLED
234 0 : case Type::Scripting:
235 0 : state[instance].instance = instance;
236 0 : drivers[instance] = NEW_NOTHROW AP_Proximity_Scripting(*this, state[instance], params[instance]);
237 0 : break;
238 : #endif
239 : #if AP_PROXIMITY_MR72_DRIVER_ENABLED
240 : #if AP_PROXIMITY_MR72_ENABLED
241 0 : case Type::MR72:
242 : #endif // AP_PROXIMITY_MR72_ENABLED
243 : #if AP_PROXIMITY_HEXSOONRADAR_ENABLED
244 : case Type::Hexsoon_Radar:
245 : #endif // AP_PROXIMITY_HEXSOONRADAR_ENABLED
246 0 : state[instance].instance = instance;
247 0 : drivers[instance] = NEW_NOTHROW AP_Proximity_MR72_CAN(*this, state[instance], params[instance]);
248 0 : break;
249 : #endif // AP_PROXIMITY_MR72_DRIVER_ENABLED
250 : #if AP_PROXIMITY_SITL_ENABLED
251 5 : case Type::SITL:
252 5 : state[instance].instance = instance;
253 5 : drivers[instance] = NEW_NOTHROW AP_Proximity_SITL(*this, state[instance], params[instance]);
254 5 : break;
255 : #endif
256 : #if AP_PROXIMITY_AIRSIMSITL_ENABLED
257 0 : case Type::AirSimSITL:
258 0 : state[instance].instance = instance;
259 0 : drivers[instance] = NEW_NOTHROW AP_Proximity_AirSimSITL(*this, state[instance], params[instance]);
260 0 : break;
261 : #endif
262 : #if AP_PROXIMITY_LD06_ENABLED
263 1 : case Type::LD06:
264 1 : if (AP_Proximity_LD06::detect(serial_instance)) {
265 1 : state[instance].instance = instance;
266 1 : drivers[instance] = NEW_NOTHROW AP_Proximity_LD06(*this, state[instance], params[instance], serial_instance);
267 1 : serial_instance++;
268 : }
269 1 : break;
270 : #endif
271 : }
272 :
273 6415 : if (drivers[instance] != nullptr) {
274 : // we loaded a driver for this instance, so it must be
275 : // present (although it may not be healthy)
276 15 : num_instances = instance+1;
277 : }
278 :
279 : // initialise status
280 6415 : state[instance].status = Status::NotConnected;
281 :
282 : // if the backend has some local parameters then make those available in the tree
283 6415 : if (drivers[instance] && state[instance].var_info) {
284 0 : backend_var_info[instance] = state[instance].var_info;
285 0 : AP_Param::load_object_from_eeprom(drivers[instance], backend_var_info[instance]);
286 : }
287 : }
288 : }
289 :
290 : // update Proximity state for all instances. This should be called at a high rate by the main loop
291 15835009 : void AP_Proximity::update()
292 : {
293 15974971 : for (uint8_t i=0; i<num_instances; i++) {
294 139962 : if (!valid_instance(i)) {
295 1274 : continue;
296 : }
297 138688 : drivers[i]->update();
298 : }
299 :
300 : // set boundary cutoff freq for low pass filter
301 15835009 : boundary.set_filter_freq(get_filter_freq());
302 :
303 : // check if any face has valid distance when it should not
304 15835009 : boundary.check_face_timeout();
305 15835009 : }
306 :
307 11084 : AP_Proximity::Type AP_Proximity::get_type(uint8_t instance) const
308 : {
309 11084 : if (instance < AP_PROXIMITY_MAX_INSTANCES) {
310 11084 : return (Type)((uint8_t)params[instance].type);
311 : }
312 0 : return Type::None;
313 : }
314 :
315 : // return sensor health
316 19803 : AP_Proximity::Status AP_Proximity::get_instance_status(uint8_t instance) const
317 : {
318 : // sanity check instance number
319 19803 : if (!valid_instance(instance)) {
320 214 : return Status::NotConnected;
321 : }
322 :
323 19589 : return state[instance].status;
324 : }
325 :
326 : // Returns status of first good sensor. If no good sensor found, returns status of last instance sensor
327 1206294 : AP_Proximity::Status AP_Proximity::get_status() const
328 : {
329 1206294 : Status sensors_status = Status::NotConnected;
330 1209556 : for (uint8_t i=0; i<num_instances; i++) {
331 15864 : sensors_status = get_instance_status(i);
332 15864 : if (sensors_status == Status::Good) {
333 : // return first good status
334 12602 : return sensors_status;
335 : }
336 : }
337 : // no good sensor found
338 1193692 : return sensors_status;
339 : }
340 :
341 : // return proximity backend for Lua scripting
342 0 : AP_Proximity_Backend *AP_Proximity::get_backend(uint8_t id) const
343 : {
344 0 : if (!valid_instance(id)) {
345 0 : return nullptr;
346 : }
347 0 : return drivers[id];
348 : }
349 :
350 : // prearm checks
351 112630 : bool AP_Proximity::prearm_healthy(char *failure_msg, const uint8_t failure_msg_len) const
352 : {
353 113029 : for (uint8_t i=0; i<num_instances; i++) {
354 634 : switch (get_instance_status(i)) {
355 221 : case Status::NoData:
356 221 : hal.util->snprintf(failure_msg, failure_msg_len, "PRX%d: No Data", i + 1);
357 221 : return false;
358 14 : case Status::NotConnected:
359 14 : hal.util->snprintf(failure_msg, failure_msg_len, "PRX%d: Not Connected", i + 1);
360 14 : return false;
361 399 : case Status::Good:
362 399 : break;
363 : }
364 : }
365 112395 : return true;
366 : }
367 :
368 : // get maximum and minimum distances (in meters)
369 607131 : float AP_Proximity::distance_max_m() const
370 : {
371 607131 : float dist_max = 0;
372 :
373 : // return longest distance from all backends
374 655324 : for (uint8_t i=0; i<num_instances; i++) {
375 48193 : if (valid_instance(i)) {
376 47758 : dist_max = MAX(dist_max, drivers[i]->distance_max_m());
377 : }
378 : }
379 607131 : return dist_max;
380 : }
381 563607 : float AP_Proximity::distance_min_m() const
382 : {
383 563607 : float dist_min = 0;
384 563607 : bool found_dist_min = false;
385 :
386 : // calculate shortest distance from all backends
387 568276 : for (uint8_t i=0; i<num_instances; i++) {
388 4669 : if (valid_instance(i)) {
389 4619 : const float disti_min = drivers[i]->distance_min_m();
390 4619 : if (!found_dist_min || (disti_min <= dist_min)) {
391 4619 : dist_min = disti_min;
392 4619 : found_dist_min = true;
393 : }
394 : }
395 : }
396 :
397 563607 : if (found_dist_min) {
398 4619 : return dist_min;
399 : }
400 558988 : return 0;
401 : }
402 :
403 :
404 : // get distances in 8 directions. used for sending distances to ground station
405 3364 : bool AP_Proximity::get_horizontal_distances(Proximity_Distance_Array &prx_dist_array) const
406 : {
407 : Proximity_Distance_Array prx_filt_dist_array; // unused
408 3364 : return boundary.get_layer_distances(PROXIMITY_MIDDLE_LAYER, distance_max_m(), prx_dist_array, prx_filt_dist_array);
409 : }
410 :
411 : // get total number of obstacles, used in GPS based Simple Avoidance
412 2006560 : uint8_t AP_Proximity::get_obstacle_count() const
413 : {
414 2006560 : return boundary.get_obstacle_count();
415 : }
416 :
417 : // get vector to obstacle based on obstacle_num passed, used in GPS based Simple Avoidance
418 80262400 : bool AP_Proximity::get_obstacle(uint8_t obstacle_num, Vector3f& vec_to_obstacle) const
419 : {
420 80262400 : return boundary.get_obstacle(obstacle_num, vec_to_obstacle);
421 : }
422 :
423 : // returns shortest distance to "obstacle_num" obstacle, from a line segment formed between "seg_start" and "seg_end"
424 : // returns FLT_MAX if it's an invalid instance.
425 1176 : bool AP_Proximity::closest_point_from_segment_to_obstacle(uint8_t obstacle_num, const Vector3f& seg_start, const Vector3f& seg_end, Vector3f& closest_point) const
426 : {
427 1176 : return boundary.closest_point_from_segment_to_obstacle(obstacle_num , seg_start, seg_end, closest_point);
428 : }
429 :
430 : // get distance and angle to closest object (used for pre-arm check)
431 : // returns true on success, false if no valid readings
432 93666 : bool AP_Proximity::get_closest_object(float& angle_deg, float &distance) const
433 : {
434 93666 : return boundary.get_closest_object(angle_deg, distance);
435 : }
436 :
437 : // get number of objects, angle and distance - used for non-GPS avoidance
438 0 : uint8_t AP_Proximity::get_object_count() const
439 : {
440 0 : return boundary.get_horizontal_object_count();
441 : }
442 :
443 : // get number of objects, angle and distance - used for non-GPS avoidance
444 0 : bool AP_Proximity::get_object_angle_and_distance(uint8_t object_number, float& angle_deg, float &distance) const
445 : {
446 0 : return boundary.get_horizontal_object_angle_and_distance(object_number, angle_deg, distance);
447 : }
448 :
449 : // get obstacle pitch and angle for a particular obstacle num
450 0 : bool AP_Proximity::get_obstacle_info(uint8_t obstacle_num, float &angle_deg, float &pitch, float &distance) const
451 : {
452 0 : return boundary.get_obstacle_info(obstacle_num, angle_deg, pitch, distance);
453 : }
454 :
455 : // handle mavlink messages
456 876 : void AP_Proximity::handle_msg(const mavlink_message_t &msg)
457 : {
458 1336 : for (uint8_t i=0; i<num_instances; i++) {
459 460 : if (valid_instance(i)) {
460 460 : drivers[i]->handle_msg(msg);
461 : }
462 : }
463 876 : }
464 :
465 : // methods for mavlink SYS_STATUS message (send_sys_status)
466 386443 : bool AP_Proximity::sensor_present() const
467 : {
468 386443 : return get_status() != Status::NotConnected;
469 : }
470 :
471 386443 : bool AP_Proximity::sensor_enabled() const
472 : {
473 : // check atleast one sensor is enabled
474 386443 : return (num_instances > 0);
475 : }
476 :
477 386443 : bool AP_Proximity::sensor_failed() const
478 : {
479 388982 : for (uint8_t i=0; i<num_instances; i++) {
480 3305 : if (get_instance_status(i) != Status::Good) {
481 : // return first bad status
482 766 : return true;
483 : }
484 : }
485 385677 : return false;
486 : }
487 :
488 : // get distance in meters upwards, returns true on success
489 27745 : bool AP_Proximity::get_upward_distance(uint8_t instance, float &distance_m) const
490 : {
491 27745 : if (!valid_instance(instance)) {
492 100 : return false;
493 : }
494 : // get upward distance from backend
495 27645 : return drivers[instance]->get_upward_distance(distance_m);
496 : }
497 :
498 1966349 : bool AP_Proximity::get_upward_distance(float &distance_m) const
499 : {
500 : // get upward distance from backend
501 1983405 : for (uint8_t i=0; i<num_instances; i++) {
502 : // return first good upward distance
503 27745 : if (get_upward_distance(i, distance_m)) {
504 10689 : return true;
505 : }
506 : }
507 1955660 : return false;
508 : }
509 :
510 : #if HAL_LOGGING_ENABLED
511 : // Write proximity sensor distances
512 848162 : void AP_Proximity::log()
513 : {
514 : // exit immediately if no sensors
515 848162 : if (num_sensors() == 0) {
516 840130 : return;
517 : }
518 :
519 8032 : Proximity_Distance_Array dist_array{}; // raw distances stored here
520 8032 : Proximity_Distance_Array filt_dist_array{}; //filtered distances stored here
521 8032 : auto &logger { AP::logger() };
522 48192 : for (uint8_t i = 0; i < boundary.get_num_layers(); i++) {
523 40160 : const bool active = boundary.get_layer_distances(i, distance_max_m(), dist_array, filt_dist_array);
524 40160 : if (!active) {
525 : // nothing on this layer
526 34998 : continue;
527 : }
528 : float dist_up;
529 5162 : if (!get_upward_distance(dist_up)) {
530 2628 : dist_up = 0.0f;
531 : }
532 :
533 5162 : float closest_ang = 0.0f;
534 5162 : float closest_dist = 0.0f;
535 5162 : get_closest_object(closest_ang, closest_dist);
536 :
537 5162 : const struct log_Proximity pkt_proximity{
538 : LOG_PACKET_HEADER_INIT(LOG_PROXIMITY_MSG),
539 5162 : time_us : AP_HAL::micros64(),
540 : instance : i,
541 5162 : health : (uint8_t)get_status(),
542 5162 : dist0 : filt_dist_array.distance[0],
543 5162 : dist45 : filt_dist_array.distance[1],
544 5162 : dist90 : filt_dist_array.distance[2],
545 5162 : dist135 : filt_dist_array.distance[3],
546 5162 : dist180 : filt_dist_array.distance[4],
547 5162 : dist225 : filt_dist_array.distance[5],
548 5162 : dist270 : filt_dist_array.distance[6],
549 5162 : dist315 : filt_dist_array.distance[7],
550 : distup : dist_up,
551 : closest_angle : closest_ang,
552 : closest_dist : closest_dist
553 5162 : };
554 5162 : logger.WriteBlock(&pkt_proximity, sizeof(pkt_proximity));
555 :
556 5162 : if (_raw_log_enable) {
557 1722 : const struct log_Proximity_raw pkt_proximity_raw{
558 : LOG_PACKET_HEADER_INIT(LOG_RAW_PROXIMITY_MSG),
559 1722 : time_us : AP_HAL::micros64(),
560 : instance : i,
561 1722 : raw_dist0 : dist_array.distance[0],
562 1722 : raw_dist45 : dist_array.distance[1],
563 1722 : raw_dist90 : dist_array.distance[2],
564 1722 : raw_dist135 : dist_array.distance[3],
565 1722 : raw_dist180 : dist_array.distance[4],
566 1722 : raw_dist225 : dist_array.distance[5],
567 1722 : raw_dist270 : dist_array.distance[6],
568 1722 : raw_dist315 : dist_array.distance[7],
569 1722 : };
570 1722 : logger.WriteBlock(&pkt_proximity_raw, sizeof(pkt_proximity_raw));
571 : }
572 : }
573 : }
574 : #endif
575 :
576 : // return true if the given instance exists
577 240832 : bool AP_Proximity::valid_instance(uint8_t i) const
578 : {
579 240832 : if (i >= AP_PROXIMITY_MAX_INSTANCES) {
580 0 : return false;
581 : }
582 :
583 240832 : if (drivers[i] == nullptr) {
584 0 : return false;
585 : }
586 240832 : return (Type)params[i].type.get() != Type::None;
587 : }
588 :
589 : AP_Proximity *AP_Proximity::_singleton;
590 :
591 : namespace AP {
592 :
593 3578092 : AP_Proximity *proximity()
594 : {
595 3578092 : return AP_Proximity::get_singleton();
596 : }
597 :
598 : }
599 :
600 : #endif // HAL_PROXIMITY_ENABLED
|
