aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_ADSB: remove all threat logic in adsb lib in favor of avoidance lib

Browse Source
This commit is contained in:
Tom Pittenger 2016-08-01 15:19:48 -07:00
parent b2b63b4d80
commit 2be32e9daa
2 changed files with 38 additions and 119 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

117
libraries/AP_ADSB/AP_ADSB.cpp
View File

@ -29,7 +29,6 @@
#include <AP_Vehicle/AP_Vehicle.h>
#include <GCS_MAVLink/GCS.h>
#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

40
libraries/AP_ADSB/AP_ADSB.h
View File

@ -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<adsb_vehicle_t, max_samples> samples;