zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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

master
Tom Pittenger 9 years ago
parent
b2b63b4d80
commit
2be32e9daa
2 changed files with 38 additions and 119 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 117
      libraries/AP_ADSB/AP_ADSB.cpp
  2. 40
      libraries/AP_ADSB/AP_ADSB.h

117
libraries/AP_ADSB/AP_ADSB.cpp
Unescape View File

@ -29,7 +29,6 @@ @@ -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[] = { @@ -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) @@ -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) @@ -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) @@ -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 @@ -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 (index == avoid_state.highest_threat_index) {
avoid_state.highest_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 != (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) @@ -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;
} else {
if (furthest_vehicle_distance <= 0) {
// ensure this is populated
determine_furthest_aircraft();
}
// 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;
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);
// 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;
// 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
Unescape View File

@ -33,22 +33,9 @@ @@ -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: @@ -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: @@ -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: @@ -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: @@ -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;

Write Preview
Loading…
Cancel
Save