From 2be32e9daaf2e0edd98c8966e430d84113a217f0 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Mon, 1 Aug 2016 15:19:48 -0700 Subject: [PATCH] AP_ADSB: remove all threat logic in adsb lib in favor of avoidance lib --- libraries/AP_ADSB/AP_ADSB.cpp | 117 ++++++++++------------------------ libraries/AP_ADSB/AP_ADSB.h | 40 ++---------- 2 files changed, 38 insertions(+), 119 deletions(-) diff --git a/libraries/AP_ADSB/AP_ADSB.cpp b/libraries/AP_ADSB/AP_ADSB.cpp index a24a0c8cf1..48d06cd944 100644 --- a/libraries/AP_ADSB/AP_ADSB.cpp +++ b/libraries/AP_ADSB/AP_ADSB.cpp @@ -29,7 +29,6 @@ #include #include -#define VEHICLE_THREAT_RADIUS_M 1000 #define VEHICLE_TIMEOUT_MS 5000 // if no updates in this time, drop it from the list #define ADSB_VEHICLE_LIST_SIZE_DEFAULT 25 #define ADSB_VEHICLE_LIST_SIZE_MAX 100 @@ -52,12 +51,8 @@ const AP_Param::GroupInfo AP_ADSB::var_info[] = { // @User: Standard AP_GROUPINFO("ENABLE", 0, AP_ADSB, _enabled, 1), - // @Param: BEHAVIOR - // @DisplayName: ADSB based Collision Avoidance Behavior - // @Description: ADSB based Collision Avoidance Behavior selector - // @Values: 0:None,1:Loiter,2:LoiterAndDescend - // @User: Advanced - AP_GROUPINFO("BEHAVIOR", 1, AP_ADSB, avoid_state.behavior, ADSB_BEHAVIOR_NONE), + + // index 2 is reserved - was BEHAVIOR // @Param: LIST_MAX // @DisplayName: ADSB vehicle list size @@ -143,11 +138,8 @@ void AP_ADSB::init(void) } } - // avoid_state - avoid_state.lowest_threat_distance = 0; - avoid_state.highest_threat_distance = 0; - avoid_state.another_vehicle_within_radius = false; - avoid_state.is_evading_threat = false; + furthest_vehicle_distance = 0; + furthest_vehicle_index = 0; // out_state set_callsign("PING1234", false); @@ -204,23 +196,15 @@ void AP_ADSB::update(void) } } -// ----------------------- if (_my_loc.is_zero()) { // if we don't have a GPS lock then there's nothing else to do return; } -// ----------------------- - - perform_threat_detection(); - - - // ----------------------- if (out_state.chan < 0) { // if there's no transceiver detected then do not set ICAO and do not service the transceiver return; } - // ----------------------- // ensure it's positive 24bit but allow -1 @@ -267,60 +251,24 @@ void AP_ADSB::update(void) } /* - * calculate threat vectors + * determine index and distance of furthest vehicle. This is + * used to bump it off when a new closer aircraft is detected */ -void AP_ADSB::perform_threat_detection(void) +void AP_ADSB::determine_furthest_aircraft(void) { - if (in_state.vehicle_count == 0 || _my_loc.is_zero()) { - // nothing to do or current location is unknown so we can't calculate any collisions - avoid_state.another_vehicle_within_radius = false; - avoid_state.lowest_threat_distance = 0; // 0 means invalid - avoid_state.highest_threat_distance = 0; // 0 means invalid - return; - } - - // TODO: compute lowest_threat using the 3D flight vector with respect to - // time-to-collision and probability of collision instead of furthest 2D distance - - // TODO: compute highest_threat using the 3D flight vector with respect to - // time-to-collision and probability of collision instead of closest 2D distance - - float min_distance = 0; float max_distance = 0; - uint16_t min_distance_index = 0; uint16_t max_distance_index = 0; for (uint16_t index = 0; index < in_state.vehicle_count; index++) { float distance = _my_loc.get_distance(get_location(in_state.vehicle_list[index])); - if (min_distance > distance || index == 0) { - min_distance = distance; - min_distance_index = index; - } if (max_distance < distance || index == 0) { max_distance = distance; max_distance_index = index; } - - if (distance <= VEHICLE_THREAT_RADIUS_M) { - in_state.vehicle_list[index].threat_level = ADSB_THREAT_HIGH; - } else { - in_state.vehicle_list[index].threat_level = ADSB_THREAT_LOW; - } } // for index - avoid_state.highest_threat_index = min_distance_index; - avoid_state.highest_threat_distance = min_distance; - - avoid_state.lowest_threat_index = max_distance_index; - avoid_state.lowest_threat_distance = max_distance; - - // if within radius, set flag and enforce a double radius to clear flag - if (is_zero(avoid_state.highest_threat_distance) || // 0 means invalid - avoid_state.highest_threat_distance > 2*VEHICLE_THREAT_RADIUS_M) { - avoid_state.another_vehicle_within_radius = false; - } else if (avoid_state.highest_threat_distance <= VEHICLE_THREAT_RADIUS_M) { - avoid_state.another_vehicle_within_radius = true; - } + furthest_vehicle_index = max_distance_index; + furthest_vehicle_distance = max_distance; } /* @@ -344,14 +292,11 @@ Location_Class AP_ADSB::get_location(const adsb_vehicle_t &vehicle) const void AP_ADSB::delete_vehicle(const uint16_t index) { if (index < in_state.vehicle_count) { - // if the vehicle is the lowest/highest threat, invalidate it - if (index == avoid_state.lowest_threat_index) { - avoid_state.lowest_threat_distance = 0; + // if the vehicle is the furthest, invalidate it. It has been bumped + if (index == furthest_vehicle_index && furthest_vehicle_distance > 0) { + furthest_vehicle_distance = 0; + furthest_vehicle_index = 0; } - if (index == avoid_state.highest_threat_index) { - avoid_state.highest_threat_distance = 0; - } - if (index != (in_state.vehicle_count-1)) { in_state.vehicle_list[index] = in_state.vehicle_list[in_state.vehicle_count-1]; } @@ -425,25 +370,29 @@ void AP_ADSB::update_vehicle(const mavlink_message_t* packet) set_vehicle(in_state.vehicle_count, vehicle); in_state.vehicle_count++; - } else if (!my_loc_is_zero && - !is_zero(avoid_state.lowest_threat_distance) && - my_loc_distance_to_vehicle < avoid_state.lowest_threat_distance) { // is closer than the furthest + } else { + // buffer is full. if new vehicle is closer than furthest, replace furthest with new - // buffer is full, replace the vehicle with lowest threat as long as it's not further away - // overwrite the lowest_threat/furthest - index = avoid_state.lowest_threat_index; - set_vehicle(index, vehicle); + if (my_loc_is_zero) { + // nothing else to do + furthest_vehicle_distance = 0; + furthest_vehicle_index = 0; - // this is now invalid because the vehicle was overwritten, need - // to run perform_threat_detection() to determine new one because - // we aren't keeping track of the second-furthest vehicle. - avoid_state.lowest_threat_distance = 0; + } else { + if (furthest_vehicle_distance <= 0) { + // ensure this is populated + determine_furthest_aircraft(); + } - // is it the nearest? Then it's the highest threat. That's an easy check - // that we don't need to run perform_threat_detection() to determine - if (avoid_state.highest_threat_distance > my_loc_distance_to_vehicle) { - avoid_state.highest_threat_distance = my_loc_distance_to_vehicle; - avoid_state.highest_threat_index = index; + if (my_loc_distance_to_vehicle < furthest_vehicle_distance) { // is closer than the furthest + // replace with the furthest vehicle + set_vehicle(furthest_vehicle_index, vehicle); + + // furthest_vehicle_index is now invalid because the vehicle was overwritten, need + // to run determine_furthest_aircraft() to determine a new one next time + furthest_vehicle_distance = 0; + furthest_vehicle_index = 0; + } } } // if buffer full diff --git a/libraries/AP_ADSB/AP_ADSB.h b/libraries/AP_ADSB/AP_ADSB.h index f63fd8f2eb..fd68a6142e 100644 --- a/libraries/AP_ADSB/AP_ADSB.h +++ b/libraries/AP_ADSB/AP_ADSB.h @@ -33,22 +33,9 @@ class AP_ADSB { public: - enum ADSB_BEHAVIOR { - ADSB_BEHAVIOR_NONE = 0, - ADSB_BEHAVIOR_LOITER = 1, - ADSB_BEHAVIOR_LOITER_AND_DESCEND = 2, - ADSB_BEHAVIOR_GUIDED = 3 - }; - - enum ADSB_THREAT_LEVEL { - ADSB_THREAT_LOW = 0, - ADSB_THREAT_HIGH = 1 - }; - struct adsb_vehicle_t { mavlink_adsb_vehicle_t info; // the whole mavlink struct with all the juicy details. sizeof() == 38 uint32_t last_update_ms; // last time this was refreshed, allows timeouts - ADSB_THREAT_LEVEL threat_level; // basic threat level }; @@ -71,11 +58,6 @@ public: // handle ADS-B transceiver report void transceiver_report(mavlink_channel_t chan, const mavlink_message_t* msg); - bool get_possible_threat() { return _enabled && avoid_state.another_vehicle_within_radius; } - - ADSB_BEHAVIOR get_behavior() { return (ADSB_BEHAVIOR)(avoid_state.behavior.get()); } - bool get_is_evading_threat() { return _enabled && avoid_state.is_evading_threat; } - void set_is_evading_threat(const bool is_evading) { if (_enabled) { avoid_state.is_evading_threat = is_evading; } } uint16_t get_vehicle_count() { return in_state.vehicle_count; } // send ADSB_VEHICLE mavlink message, usually as a StreamRate @@ -83,7 +65,7 @@ public: void set_stall_speed_cm(const uint16_t stall_speed_cm) { out_state.cfg.stall_speed_cm = stall_speed_cm; } - void set_is_auto_mode(const bool is_in_auto_mode) { _is_in_auto_mode = is_in_auto_mode; } + void set_is_auto_mode(const bool is_in_auto_mode) { out_state._is_in_auto_mode = is_in_auto_mode; } void set_is_flying(const bool is_flying) { out_state.is_flying = is_flying; } UAVIONIX_ADSB_RF_HEALTH get_transceiver_status(void) { return out_state.status; } @@ -105,7 +87,7 @@ private: void deinit(); // compares current vector against vehicle_list to detect threats - void perform_threat_detection(void); + void determine_furthest_aircraft(void); // return index of given vehicle if ICAO_ADDRESS matches. return -1 if no match bool find_index(const adsb_vehicle_t &vehicle, uint16_t *index) const; @@ -177,21 +159,9 @@ private: } out_state; - // Avoidance state - struct { - AP_Int8 behavior; - - bool another_vehicle_within_radius; - bool is_evading_threat; - - // index of and distance to vehicle with lowest threat - uint16_t lowest_threat_index; - float lowest_threat_distance; - - // index of and distance to vehicle with highest threat - uint16_t highest_threat_index; - float highest_threat_distance; - } avoid_state; + // index of and distance to furthest vehicle in list + uint16_t furthest_vehicle_index; + float furthest_vehicle_distance; static const uint8_t max_samples = 30; AP_Buffer samples;