Line data Source code
1 : #include "AP_Avoidance_config.h"
2 :
3 : #if AP_ADSB_AVOIDANCE_ENABLED
4 :
5 : #include "AP_Avoidance.h"
6 :
7 : extern const AP_HAL::HAL& hal;
8 :
9 : #include <limits>
10 : #include <AP_AHRS/AP_AHRS.h>
11 : #include <GCS_MAVLink/GCS.h>
12 : #include <AP_Vehicle/AP_Vehicle_Type.h>
13 :
14 : #define AVOIDANCE_DEBUGGING 0
15 :
16 : #if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
17 : #define AP_AVOIDANCE_WARN_TIME_DEFAULT 30
18 : #define AP_AVOIDANCE_FAIL_TIME_DEFAULT 30
19 : #define AP_AVOIDANCE_WARN_DISTANCE_XY_DEFAULT 1000
20 : #define AP_AVOIDANCE_WARN_DISTANCE_Z_DEFAULT 300
21 : #define AP_AVOIDANCE_FAIL_DISTANCE_XY_DEFAULT 300
22 : #define AP_AVOIDANCE_FAIL_DISTANCE_Z_DEFAULT 100
23 : #define AP_AVOIDANCE_RECOVERY_DEFAULT RecoveryAction::RESUME_IF_AUTO_ELSE_LOITER
24 : #define AP_AVOIDANCE_FAIL_ACTION_DEFAULT MAV_COLLISION_ACTION_REPORT
25 : #else // APM_BUILD_TYPE(APM_BUILD_ArduCopter),Heli, Rover, Boat
26 : #define AP_AVOIDANCE_WARN_TIME_DEFAULT 30
27 : #define AP_AVOIDANCE_FAIL_TIME_DEFAULT 30
28 : #define AP_AVOIDANCE_WARN_DISTANCE_XY_DEFAULT 300
29 : #define AP_AVOIDANCE_WARN_DISTANCE_Z_DEFAULT 300
30 : #define AP_AVOIDANCE_FAIL_DISTANCE_XY_DEFAULT 100
31 : #define AP_AVOIDANCE_FAIL_DISTANCE_Z_DEFAULT 100
32 : #define AP_AVOIDANCE_RECOVERY_DEFAULT RecoveryAction::RTL
33 : #define AP_AVOIDANCE_FAIL_ACTION_DEFAULT MAV_COLLISION_ACTION_REPORT
34 : #endif
35 :
36 : #if AVOIDANCE_DEBUGGING
37 : #include <stdio.h>
38 : #define debug(fmt, args ...) do {::fprintf(stderr,"%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
39 : #else
40 : #define debug(fmt, args ...)
41 : #endif
42 :
43 : // table of user settable parameters
44 : const AP_Param::GroupInfo AP_Avoidance::var_info[] = {
45 :
46 : // @Param: ENABLE
47 : // @DisplayName: Enable Avoidance using ADSB
48 : // @Description: Enable Avoidance using ADSB
49 : // @Values: 0:Disabled,1:Enabled
50 : // @User: Advanced
51 : AP_GROUPINFO_FLAGS("ENABLE", 1, AP_Avoidance, _enabled, 0, AP_PARAM_FLAG_ENABLE),
52 :
53 : // @Param: F_ACTION
54 : // @DisplayName: Collision Avoidance Behavior
55 : // @Description: Specifies aircraft behaviour when a collision is imminent
56 : // @Values: 0:None,1:Report,2:Climb Or Descend,3:Move Horizontally,4:Move Perpendicularly in 3D,5:RTL,6:Hover
57 : // @User: Advanced
58 : AP_GROUPINFO("F_ACTION", 2, AP_Avoidance, _fail_action, AP_AVOIDANCE_FAIL_ACTION_DEFAULT),
59 :
60 : // @Param: W_ACTION
61 : // @DisplayName: Collision Avoidance Behavior - Warn
62 : // @Description: Specifies aircraft behaviour when a collision may occur
63 : // @Values: 0:None,1:Report
64 : // @User: Advanced
65 : AP_GROUPINFO("W_ACTION", 3, AP_Avoidance, _warn_action, MAV_COLLISION_ACTION_REPORT),
66 :
67 : // @Param: F_RCVRY
68 : // @DisplayName: Recovery behaviour after a fail event
69 : // @Description: Determines what the aircraft will do after a fail event is resolved
70 : // @Values: 0:Remain in AVOID_ADSB,1:Resume previous flight mode,2:RTL,3:Resume if AUTO else Loiter
71 : // @User: Advanced
72 : AP_GROUPINFO("F_RCVRY", 4, AP_Avoidance, _fail_recovery, uint8_t(AP_AVOIDANCE_RECOVERY_DEFAULT)),
73 :
74 : // @Param: OBS_MAX
75 : // @DisplayName: Maximum number of obstacles to track
76 : // @Description: Maximum number of obstacles to track
77 : // @User: Advanced
78 : AP_GROUPINFO("OBS_MAX", 5, AP_Avoidance, _obstacles_max, 20),
79 :
80 : // @Param: W_TIME
81 : // @DisplayName: Time Horizon Warn
82 : // @Description: Aircraft velocity vectors are multiplied by this time to determine closest approach. If this results in an approach closer than W_DIST_XY or W_DIST_Z then W_ACTION is undertaken (assuming F_ACTION is not undertaken)
83 : // @Units: s
84 : // @User: Advanced
85 : AP_GROUPINFO("W_TIME", 6, AP_Avoidance, _warn_time_horizon_s, AP_AVOIDANCE_WARN_TIME_DEFAULT),
86 :
87 : // @Param: F_TIME
88 : // @DisplayName: Time Horizon Fail
89 : // @Description: Aircraft velocity vectors are multiplied by this time to determine closest approach. If this results in an approach closer than F_DIST_XY or F_DIST_Z then F_ACTION is undertaken
90 : // @Units: s
91 : // @User: Advanced
92 : AP_GROUPINFO("F_TIME", 7, AP_Avoidance, _fail_time_horizon_s, AP_AVOIDANCE_FAIL_TIME_DEFAULT),
93 :
94 : // @Param: W_DIST_XY
95 : // @DisplayName: Distance Warn XY
96 : // @Description: Closest allowed projected distance before W_ACTION is undertaken
97 : // @Units: m
98 : // @User: Advanced
99 : AP_GROUPINFO("W_DIST_XY", 8, AP_Avoidance, _warn_distance_ne_m, AP_AVOIDANCE_WARN_DISTANCE_XY_DEFAULT),
100 :
101 : // @Param: F_DIST_XY
102 : // @DisplayName: Distance Fail XY
103 : // @Description: Closest allowed projected distance before F_ACTION is undertaken
104 : // @Units: m
105 : // @User: Advanced
106 : AP_GROUPINFO("F_DIST_XY", 9, AP_Avoidance, _fail_distance_ne_m, AP_AVOIDANCE_FAIL_DISTANCE_XY_DEFAULT),
107 :
108 : // @Param: W_DIST_Z
109 : // @DisplayName: Distance Warn Z
110 : // @Description: Closest allowed projected distance before BEHAVIOUR_W is undertaken
111 : // @Units: m
112 : // @User: Advanced
113 : AP_GROUPINFO("W_DIST_Z", 10, AP_Avoidance, _warn_distance_d_m, AP_AVOIDANCE_WARN_DISTANCE_Z_DEFAULT),
114 :
115 : // @Param: F_DIST_Z
116 : // @DisplayName: Distance Fail Z
117 : // @Description: Closest allowed projected distance before BEHAVIOUR_F is undertaken
118 : // @Units: m
119 : // @User: Advanced
120 : AP_GROUPINFO("F_DIST_Z", 11, AP_Avoidance, _fail_distance_d_m, AP_AVOIDANCE_FAIL_DISTANCE_Z_DEFAULT),
121 :
122 : // @Param: F_ALT_MIN
123 : // @DisplayName: ADS-B avoidance minimum altitude
124 : // @Description: Minimum AMSL (above mean sea level) altitude for ADS-B avoidance. If the vehicle is below this altitude, no avoidance action will take place. Useful to prevent ADS-B avoidance from activating while below the tree line or around structures. Default of 0 is no minimum.
125 : // @Units: m
126 : // @User: Advanced
127 : AP_GROUPINFO("F_ALT_MIN", 12, AP_Avoidance, _fail_altitude_min_m, 0),
128 :
129 : AP_GROUPEND
130 : };
131 :
132 1693 : AP_Avoidance::AP_Avoidance(AP_ADSB &adsb) :
133 1693 : _adsb(adsb)
134 : {
135 1693 : AP_Param::setup_object_defaults(this, var_info);
136 1693 : if (_singleton != nullptr) {
137 0 : AP_HAL::panic("AP_Avoidance must be singleton");
138 : }
139 1693 : _singleton = this;
140 1693 : }
141 :
142 : /*
143 : * Initialize variables and allocate memory for array
144 : */
145 7 : void AP_Avoidance::init(void)
146 : {
147 : debug("ADSB initialisation: %d obstacles", _obstacles_max.get());
148 7 : if (_obstacles == nullptr) {
149 147 : _obstacles = NEW_NOTHROW AP_Avoidance::Obstacle[_obstacles_max];
150 :
151 7 : if (_obstacles == nullptr) {
152 : // dynamic RAM allocation of _obstacles[] failed, disable gracefully
153 0 : DEV_PRINTF("Unable to initialize Avoidance obstacle list\n");
154 : // disable ourselves to avoid repeated allocation attempts
155 0 : _enabled.set(0);
156 0 : return;
157 : }
158 7 : _obstacles_allocated = _obstacles_max;
159 : }
160 7 : _obstacle_count = 0;
161 7 : _last_state_change_ms = 0;
162 7 : _threat_level = MAV_COLLISION_THREAT_LEVEL_NONE;
163 7 : _gcs_cleared_messages_first_sent = std::numeric_limits<uint32_t>::max();
164 7 : _current_most_serious_threat = -1;
165 : }
166 :
167 : /*
168 : * de-initialize and free up some memory
169 : */
170 4 : void AP_Avoidance::deinit(void)
171 : {
172 4 : if (_obstacles != nullptr) {
173 4 : delete [] _obstacles;
174 4 : _obstacles = nullptr;
175 4 : _obstacles_allocated = 0;
176 4 : handle_recovery(RecoveryAction::RTL);
177 : }
178 4 : _obstacle_count = 0;
179 4 : }
180 :
181 2452434 : bool AP_Avoidance::check_startup()
182 : {
183 2452434 : if (!_enabled) {
184 2446084 : if (_obstacles != nullptr) {
185 4 : deinit();
186 : }
187 : // nothing to do
188 2446084 : return false;
189 : }
190 6350 : if (_obstacles == nullptr) {
191 7 : init();
192 : }
193 6350 : return _obstacles != nullptr;
194 : }
195 :
196 : // vel_ned_ms is north/east/down
197 6 : void AP_Avoidance::add_obstacle(const uint32_t obstacle_timestamp_ms,
198 : const MAV_COLLISION_SRC src,
199 : const uint32_t src_id,
200 : const Location &loc,
201 : const Vector3f &vel_ned_ms)
202 : {
203 6 : if (! check_startup()) {
204 0 : return;
205 : }
206 6 : uint32_t oldest_timestamp = std::numeric_limits<uint32_t>::max();
207 6 : uint8_t oldest_index = 255; // avoid compiler warning with initialisation
208 6 : int16_t index = -1;
209 : uint8_t i;
210 6 : for (i=0; i<_obstacle_count; i++) {
211 0 : if (_obstacles[i].src_id == src_id &&
212 0 : _obstacles[i].src == src) {
213 : // pre-existing obstacle found; we will update its information
214 0 : index = i;
215 0 : break;
216 : }
217 0 : if (_obstacles[i].timestamp_ms < oldest_timestamp) {
218 0 : oldest_timestamp = _obstacles[i].timestamp_ms;
219 0 : oldest_index = i;
220 : }
221 : }
222 6 : WITH_SEMAPHORE(_rsem);
223 :
224 6 : if (index == -1) {
225 : // existing obstacle not found. See if we can store it anyway:
226 6 : if (i <_obstacles_allocated) {
227 : // have room to store more vehicles...
228 6 : index = _obstacle_count++;
229 0 : } else if (oldest_timestamp < obstacle_timestamp_ms) {
230 : // replace this very old entry with this new data
231 0 : index = oldest_index;
232 : } else {
233 : // no room for this (old?!) data
234 0 : return;
235 : }
236 :
237 6 : _obstacles[index].src = src;
238 6 : _obstacles[index].src_id = src_id;
239 : }
240 :
241 6 : _obstacles[index]._location = loc;
242 6 : _obstacles[index]._velocity_ned_ms = vel_ned_ms;
243 6 : _obstacles[index].timestamp_ms = obstacle_timestamp_ms;
244 6 : }
245 :
246 6 : void AP_Avoidance::add_obstacle(const uint32_t obstacle_timestamp_ms,
247 : const MAV_COLLISION_SRC src,
248 : const uint32_t src_id,
249 : const Location &loc,
250 : const float cog,
251 : const float speed_ne_ms,
252 : const float speed_d_ms)
253 : {
254 6 : Vector3f vel_ned_ms;
255 6 : vel_ned_ms[0] = speed_ne_ms * cosf(radians(cog));
256 6 : vel_ned_ms[1] = speed_ne_ms * sinf(radians(cog));
257 6 : vel_ned_ms[2] = speed_d_ms;
258 : // debug("cog=%f speed_ne_ms=%f veln=%f vele=%f", cog, speed_ne_ms, vel_ned_ms[0], vel[1]);
259 6 : return add_obstacle(obstacle_timestamp_ms, src, src_id, loc, vel_ned_ms);
260 : }
261 :
262 6 : uint32_t AP_Avoidance::src_id_for_adsb_vehicle(const AP_ADSB::adsb_vehicle_t &vehicle) const
263 : {
264 : // TODO: need to include squawk code and callsign
265 6 : return vehicle.info.ICAO_address;
266 : }
267 :
268 2631 : void AP_Avoidance::get_adsb_samples()
269 : {
270 : AP_ADSB::adsb_vehicle_t vehicle;
271 2637 : while (_adsb.next_sample(vehicle)) {
272 6 : uint32_t src_id = src_id_for_adsb_vehicle(vehicle);
273 6 : Location loc = _adsb.get_location(vehicle);
274 6 : add_obstacle(vehicle.last_update_ms,
275 : MAV_COLLISION_SRC_ADSB,
276 : src_id,
277 : loc,
278 6 : vehicle.info.heading * 0.01,
279 6 : vehicle.info.hor_velocity * 0.01,
280 6 : -vehicle.info.ver_velocity * 0.01); // convert cm-up to m-down
281 : }
282 2631 : }
283 :
284 191 : float closest_approach_NE_m(const Location &loc,
285 : const Vector3f &vel_ned_ms,
286 : const Location &obstacle_loc,
287 : const Vector3f &obstacle_vel_ned_ms,
288 : const uint8_t time_horizon_s)
289 : {
290 :
291 191 : Vector2f delta_vel_ne_ms = Vector2f(obstacle_vel_ned_ms[0] - vel_ned_ms[0], obstacle_vel_ned_ms[1] - vel_ned_ms[1]);
292 191 : const Vector2f delta_pos_ne_m = obstacle_loc.get_distance_NE(loc);
293 :
294 191 : Vector2f line_segment_ne_m = delta_vel_ne_ms * time_horizon_s;
295 :
296 : float dist_ne_m = Vector2<float>::closest_distance_between_radial_and_point
297 191 : (line_segment_ne_m,
298 : delta_pos_ne_m);
299 :
300 : debug(" time_horizon: (%d)", time_horizon_s);
301 : debug(" delta pos: (y=%f,x=%f)", delta_pos_ne_m[0], delta_pos_ne_m[1]);
302 : debug(" delta vel: (y=%f,x=%f)", delta_vel_ne_ms[0], delta_vel_ne_ms[1]);
303 : debug(" line segment: (y=%f,x=%f)", line_segment_ne_m[0], line_segment_ne_m[1]);
304 : debug(" closest: (%f)", dist_ne_m);
305 :
306 191 : return dist_ne_m;
307 : }
308 :
309 : // returns the closest these objects will get in the body z axis (in metres)
310 382 : float closest_approach_D_m(const Location &loc,
311 : const Vector3f &vel_ned_ms,
312 : const Location &obstacle_loc,
313 : const Vector3f &obstacle_vel_ned_ms,
314 : const uint8_t time_horizon_s)
315 : {
316 :
317 382 : float delta_vel_d_ms = obstacle_vel_ned_ms[2] - vel_ned_ms[2];
318 382 : float delta_pos_d_cm = obstacle_loc.alt - loc.alt;
319 :
320 : float dist_d_cm;
321 382 : if (delta_pos_d_cm >= 0 && delta_vel_d_ms >= 0) {
322 218 : dist_d_cm = delta_pos_d_cm;
323 164 : } else if (delta_pos_d_cm <= 0 && delta_vel_d_ms <= 0) {
324 0 : dist_d_cm = fabsf(delta_pos_d_cm);
325 : } else {
326 164 : dist_d_cm = fabsf(delta_pos_d_cm - delta_vel_d_ms * time_horizon_s * 100.0);
327 : }
328 :
329 : debug(" time_horizon: (%d)", time_horizon_s);
330 : debug(" delta pos: (%f) metres", delta_pos_d_cm*0.01f);
331 : debug(" delta vel: (%f) m/s", delta_vel_d_ms);
332 : debug(" closest: (%f) metres", dist_d_cm*0.01f);
333 :
334 382 : return dist_d_cm * 0.01f;
335 : }
336 :
337 191 : void AP_Avoidance::update_threat_level(const Location &loc,
338 : const Vector3f &vel_ned_ms,
339 : AP_Avoidance::Obstacle &obstacle)
340 : {
341 :
342 191 : Location &obstacle_loc = obstacle._location;
343 191 : Vector3f &obstacle_vel_ned_ms = obstacle._velocity_ned_ms;
344 :
345 191 : obstacle.threat_level = MAV_COLLISION_THREAT_LEVEL_NONE;
346 :
347 191 : const uint32_t obstacle_age_ms = AP_HAL::millis() - obstacle.timestamp_ms;
348 191 : float closest_ne_m = closest_approach_NE_m(loc, vel_ned_ms, obstacle_loc, obstacle_vel_ned_ms, _fail_time_horizon_s + obstacle_age_ms/1000);
349 191 : if (closest_ne_m < _fail_distance_ne_m) {
350 191 : obstacle.threat_level = MAV_COLLISION_THREAT_LEVEL_HIGH;
351 : } else {
352 0 : closest_ne_m = closest_approach_NE_m(loc, vel_ned_ms, obstacle_loc, obstacle_vel_ned_ms, _warn_time_horizon_s + obstacle_age_ms/1000);
353 0 : if (closest_ne_m < _warn_distance_ne_m) {
354 0 : obstacle.threat_level = MAV_COLLISION_THREAT_LEVEL_LOW;
355 : }
356 : }
357 :
358 : // check for vertical separation; our threat level is the minimum
359 : // of vertical and horizontal threat levels
360 191 : float closest_d_m = closest_approach_D_m(loc, vel_ned_ms, obstacle_loc, obstacle_vel_ned_ms, _warn_time_horizon_s + obstacle_age_ms/1000);
361 191 : if (obstacle.threat_level != MAV_COLLISION_THREAT_LEVEL_NONE) {
362 191 : if (closest_d_m > _warn_distance_d_m) {
363 0 : obstacle.threat_level = MAV_COLLISION_THREAT_LEVEL_NONE;
364 : } else {
365 191 : closest_d_m = closest_approach_D_m(loc, vel_ned_ms, obstacle_loc, obstacle_vel_ned_ms, _fail_time_horizon_s + obstacle_age_ms/1000);
366 191 : if (closest_d_m > _fail_distance_d_m) {
367 3 : obstacle.threat_level = MAV_COLLISION_THREAT_LEVEL_LOW;
368 : }
369 : }
370 : }
371 :
372 : // If we haven't heard from a vehicle then assume it is no threat
373 191 : if (obstacle_age_ms > MAX_OBSTACLE_AGE_MS) {
374 5 : obstacle.threat_level = MAV_COLLISION_THREAT_LEVEL_NONE;
375 : }
376 :
377 : // could optimise this to not calculate a lot of this if threat
378 : // level is none - but only *once the GCS has been informed*!
379 191 : obstacle.closest_approach_ne_m = closest_ne_m;
380 191 : obstacle.closest_approach_d_m = closest_d_m;
381 191 : float current_distance_ne_m = loc.get_distance(obstacle_loc);
382 191 : obstacle.distance_to_closest_approach_ned_m = current_distance_ne_m - closest_ne_m;
383 191 : Vector2f net_velocity_ne_ms = Vector2f(vel_ned_ms[0] - obstacle_vel_ned_ms[0], vel_ned_ms[1] - obstacle_vel_ned_ms[1]);
384 191 : obstacle.time_to_closest_approach_s = 0.0f;
385 214 : if (!is_zero(obstacle.distance_to_closest_approach_ned_m) &&
386 23 : ! is_zero(net_velocity_ne_ms.length())) {
387 23 : obstacle.time_to_closest_approach_s = obstacle.distance_to_closest_approach_ned_m / net_velocity_ne_ms.length();
388 : }
389 191 : }
390 :
391 0 : MAV_COLLISION_THREAT_LEVEL AP_Avoidance::current_threat_level() const {
392 0 : if (_obstacles == nullptr) {
393 0 : return MAV_COLLISION_THREAT_LEVEL_NONE;
394 : }
395 0 : if (_current_most_serious_threat == -1) {
396 0 : return MAV_COLLISION_THREAT_LEVEL_NONE;
397 : }
398 0 : return _obstacles[_current_most_serious_threat].threat_level;
399 : }
400 :
401 : #if HAL_GCS_ENABLED
402 19 : void AP_Avoidance::send_collision_all(const AP_Avoidance::Obstacle &threat, MAV_COLLISION_ACTION behaviour) const
403 : {
404 19 : const mavlink_collision_t packet{
405 19 : id: threat.src_id,
406 19 : time_to_minimum_delta: threat.time_to_closest_approach_s,
407 19 : altitude_minimum_delta: threat.closest_approach_d_m,
408 19 : horizontal_minimum_delta: threat.closest_approach_ne_m,
409 : src: MAV_COLLISION_SRC_ADSB,
410 : action: (uint8_t)behaviour,
411 19 : threat_level: (uint8_t)threat.threat_level,
412 19 : };
413 19 : gcs().send_to_active_channels(MAVLINK_MSG_ID_COLLISION, (const char *)&packet);
414 19 : }
415 : #endif
416 :
417 2631 : void AP_Avoidance::handle_threat_gcs_notify(AP_Avoidance::Obstacle *threat)
418 : {
419 2631 : if (threat == nullptr) {
420 2445 : return;
421 : }
422 :
423 186 : uint32_t now = AP_HAL::millis();
424 186 : if (threat->threat_level == MAV_COLLISION_THREAT_LEVEL_NONE) {
425 : // only send cleared messages for a few seconds:
426 0 : if (_gcs_cleared_messages_first_sent == 0) {
427 0 : _gcs_cleared_messages_first_sent = now;
428 : }
429 0 : if (now - _gcs_cleared_messages_first_sent > _gcs_cleared_messages_duration * 1000) {
430 0 : return;
431 : }
432 : } else {
433 186 : _gcs_cleared_messages_first_sent = 0;
434 : }
435 186 : if (now - threat->last_gcs_report_time > _gcs_notify_interval * 1000) {
436 19 : send_collision_all(*threat, mav_avoidance_action());
437 19 : threat->last_gcs_report_time = now;
438 : }
439 :
440 : }
441 :
442 186 : bool AP_Avoidance::obstacle_is_more_serious_threat(const AP_Avoidance::Obstacle &obstacle) const
443 : {
444 186 : if (_current_most_serious_threat == -1) {
445 : // any threat is more of a threat than no threat
446 186 : return true;
447 : }
448 0 : const AP_Avoidance::Obstacle ¤t = _obstacles[_current_most_serious_threat];
449 0 : if (obstacle.threat_level > current.threat_level) {
450 : // threat_level is updated by update_threat_level
451 0 : return true;
452 : }
453 0 : if (obstacle.threat_level == current.threat_level &&
454 0 : obstacle.time_to_closest_approach_s < current.time_to_closest_approach_s) {
455 0 : return true;
456 : }
457 0 : return false;
458 : }
459 :
460 2631 : void AP_Avoidance::check_for_threats()
461 : {
462 2631 : const AP_AHRS &_ahrs = AP::ahrs();
463 :
464 2631 : Location loc;
465 2631 : if (!_ahrs.get_location(loc)) {
466 : // if we don't know our own location we can't determine any threat level
467 246 : return;
468 : }
469 :
470 2385 : Vector3f vel_ned_ms;
471 2385 : if (!_ahrs.get_velocity_NED(vel_ned_ms)) {
472 : // assuming our own velocity to be zero here may cause us to
473 : // fly into something. Better not to attempt to avoid in this
474 : // case.
475 0 : return;
476 : }
477 :
478 : // we always check all obstacles to see if they are threats since it
479 : // is most likely our own position and/or velocity have changed
480 : // determine the current most-serious-threat
481 2385 : _current_most_serious_threat = -1;
482 2576 : for (uint8_t i=0; i<_obstacle_count; i++) {
483 :
484 191 : AP_Avoidance::Obstacle &obstacle = _obstacles[i];
485 191 : const uint32_t obstacle_age_ms = AP_HAL::millis() - obstacle.timestamp_ms;
486 : debug("i=%d src_id=%d timestamp=%u age=%d", i, obstacle.src_id, obstacle.timestamp_ms, obstacle_age_ms);
487 :
488 191 : update_threat_level(loc, vel_ned_ms, obstacle);
489 : debug(" threat-level=%d", obstacle.threat_level);
490 :
491 : // ignore any really old data:
492 191 : if (obstacle_age_ms > MAX_OBSTACLE_AGE_MS) {
493 : // shrink list if this is the last entry:
494 5 : if (i == _obstacle_count-1) {
495 5 : _obstacle_count -= 1;
496 : }
497 5 : continue;
498 : }
499 :
500 186 : if (obstacle_is_more_serious_threat(obstacle)) {
501 186 : _current_most_serious_threat = i;
502 : }
503 : }
504 2385 : if (_current_most_serious_threat != -1) {
505 : debug("Current most serious threat: %d level=%d", _current_most_serious_threat, _obstacles[_current_most_serious_threat].threat_level);
506 : }
507 : }
508 :
509 :
510 5262 : AP_Avoidance::Obstacle *AP_Avoidance::most_serious_threat()
511 : {
512 5262 : if (_current_most_serious_threat < 0) {
513 : // we *really_ should not have been called!
514 4890 : return nullptr;
515 : }
516 372 : return &_obstacles[_current_most_serious_threat];
517 : }
518 :
519 :
520 1306596 : void AP_Avoidance::update()
521 : {
522 1306596 : if (!check_startup()) {
523 1303965 : return;
524 : }
525 :
526 2631 : if (_adsb.enabled()) {
527 2631 : get_adsb_samples();
528 : }
529 :
530 2631 : check_for_threats();
531 :
532 : // avoid object (if necessary)
533 2631 : handle_avoidance_local(most_serious_threat());
534 :
535 : // notify GCS of most serious thread
536 2631 : handle_threat_gcs_notify(most_serious_threat());
537 : }
538 :
539 2631 : void AP_Avoidance::handle_avoidance_local(AP_Avoidance::Obstacle *threat)
540 : {
541 2631 : MAV_COLLISION_THREAT_LEVEL new_threat_level = MAV_COLLISION_THREAT_LEVEL_NONE;
542 2631 : MAV_COLLISION_ACTION action = MAV_COLLISION_ACTION_NONE;
543 :
544 2631 : if (threat != nullptr) {
545 186 : new_threat_level = threat->threat_level;
546 186 : if (new_threat_level == MAV_COLLISION_THREAT_LEVEL_HIGH) {
547 183 : action = (MAV_COLLISION_ACTION)_fail_action.get();
548 183 : Location loc;
549 199 : if (action != MAV_COLLISION_ACTION_NONE && _fail_altitude_min_m > 0 &&
550 382 : AP::ahrs().get_location(loc) && ((loc.alt * 0.01f) < _fail_altitude_min_m)) {
551 : // disable avoidance when close to ground, report only
552 16 : action = MAV_COLLISION_ACTION_REPORT;
553 : }
554 : }
555 : }
556 :
557 2631 : uint32_t now = AP_HAL::millis();
558 :
559 2631 : if (new_threat_level != _threat_level) {
560 : // transition to higher states immediately, recovery to lower states more slowly
561 25 : if (((now - _last_state_change_ms) > AP_AVOIDANCE_STATE_RECOVERY_TIME_MS) || (new_threat_level > _threat_level)) {
562 : // handle recovery from high threat level
563 11 : if (_threat_level == MAV_COLLISION_THREAT_LEVEL_HIGH) {
564 5 : handle_recovery(RecoveryAction(_fail_recovery.get()));
565 5 : _latest_action = MAV_COLLISION_ACTION_NONE;
566 : }
567 :
568 : // update state
569 11 : _last_state_change_ms = now;
570 11 : _threat_level = new_threat_level;
571 : }
572 : }
573 :
574 : // handle ongoing threat by calling vehicle specific handler
575 2631 : if ((threat != nullptr) && (_threat_level == MAV_COLLISION_THREAT_LEVEL_HIGH) && (action > MAV_COLLISION_ACTION_REPORT)) {
576 167 : _latest_action = handle_avoidance(threat, action);
577 : }
578 2631 : }
579 :
580 :
581 1145832 : void AP_Avoidance::handle_msg(const mavlink_message_t &msg)
582 : {
583 1145832 : if (!check_startup()) {
584 : // avoidance is not active / allocated
585 1145832 : return;
586 : }
587 :
588 3713 : if (msg.msgid != MAVLINK_MSG_ID_GLOBAL_POSITION_INT) {
589 : // we only take position from GLOBAL_POSITION_INT
590 3713 : return;
591 : }
592 :
593 0 : if (msg.sysid == mavlink_system.sysid) {
594 : // we do not obstruct ourselves....
595 0 : return;
596 : }
597 :
598 : // inform AP_Avoidance we have a new player
599 : mavlink_global_position_int_t packet;
600 0 : mavlink_msg_global_position_int_decode(&msg, &packet);
601 : const Location loc {
602 : packet.lat,
603 : packet.lon,
604 0 : int32_t(packet.alt * 0.1), // mm -> cm
605 : Location::AltFrame::ABSOLUTE
606 0 : };
607 : const Vector3f vel_ned_ms {
608 0 : packet.vx * 0.01f, // cm to m
609 0 : packet.vy * 0.01f,
610 0 : packet.vz * 0.01f
611 0 : };
612 0 : add_obstacle(AP_HAL::millis(),
613 : MAV_COLLISION_SRC_MAVLINK_GPS_GLOBAL_INT,
614 0 : msg.sysid,
615 : loc,
616 : vel_ned_ms);
617 : }
618 :
619 : // get unit vector away from the nearest obstacle
620 77 : bool AP_Avoidance::get_vector_perpendicular(const AP_Avoidance::Obstacle *obstacle, Vector3f &vec_neu_unit) const
621 : {
622 77 : if (obstacle == nullptr) {
623 : // why where we called?!
624 0 : return false;
625 : }
626 :
627 77 : Location current_loc;
628 77 : if (!AP::ahrs().get_location(current_loc)) {
629 : // we should not get to here! If we don't know our position
630 : // we can't know if there are any threats, for starters!
631 0 : return false;
632 : }
633 :
634 : // if their velocity is moving around close to zero then flying
635 : // perpendicular to that velocity may mean we do weird things.
636 : // Instead, we will fly directly away from them
637 77 : if (obstacle->_velocity_ned_ms.length() < _low_velocity_threshold) {
638 77 : const Vector2f delta_pos_ne_m = obstacle->_location.get_distance_NE(current_loc);
639 77 : const float delta_pos_u_cm = current_loc.alt - obstacle->_location.alt;
640 77 : Vector3f delta_pos_neu_m = Vector3f{delta_pos_ne_m.x, delta_pos_ne_m.y, delta_pos_u_cm * 0.01};
641 : // avoid div by zero
642 77 : if (delta_pos_neu_m.is_zero()) {
643 0 : return false;
644 : }
645 77 : delta_pos_neu_m.normalize();
646 77 : vec_neu_unit = delta_pos_neu_m;
647 77 : return true;
648 : } else {
649 0 : vec_neu_unit = perpendicular_neu_m(obstacle->_location, obstacle->_velocity_ned_ms, current_loc);
650 : // avoid div by zero
651 0 : if (vec_neu_unit.is_zero()) {
652 0 : return false;
653 : }
654 0 : vec_neu_unit.normalize();
655 0 : return true;
656 : }
657 : }
658 :
659 : // helper functions to calculate 3D destination to get us away from obstacle
660 : // v1_ned is NED
661 0 : Vector3f AP_Avoidance::perpendicular_neu_m(const Location &p1, const Vector3f &v1_ned, const Location &p2)
662 : {
663 0 : const Vector2f delta_p_ne_m = p1.get_distance_NE(p2);
664 0 : Vector3f delta_p_neu_m = Vector3f(delta_p_ne_m[0], delta_p_ne_m[1], (p2.alt - p1.alt) * 0.01f); //check this line
665 0 : Vector3f v1_neu = Vector3f(v1_ned[0], v1_ned[1], -v1_ned[2]);
666 0 : Vector3f ret_neu_m = Vector3f::perpendicular(delta_p_neu_m, v1_neu);
667 0 : return ret_neu_m;
668 : }
669 :
670 : // singleton instance
671 : AP_Avoidance *AP_Avoidance::_singleton;
672 :
673 : namespace AP {
674 :
675 5 : AP_Avoidance *ap_avoidance()
676 : {
677 5 : return AP_Avoidance::get_singleton();
678 : }
679 :
680 : }
681 :
682 : #endif // AP_ADSB_AVOIDANCE_ENABLED
|
