Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_ADSB: delete furthest when buffer is full 

- added lowest/highest_threat tracking. This is currently defined as 2D distance. Room for improvement to make it 3D and be flight vector based instead of distance
- when trying to add a vehicle but  the buffer is full, overwrite the lowest_threat/furthest
- added basic THREAT enum of high/low which means in or our of the 200m radius. Room for improvement here.
This commit is contained in:
Tom Pittenger 2015-11-23 13:54:16 -08:00 committed by Andrew Tridgell
parent 4b40a884e9
commit decac5cb15
2 changed files with 99 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

104
libraries/AP_ADSB/AP_ADSB.cpp
View File

@ -60,8 +60,8 @@ void AP_ADSB::init(void)
}
}
_vehicle_count = 0;
_furthest_vehicle_index = 0;
_furthest_vehicle_distance = 0;
_lowest_threat_distance = 0;
_highest_threat_distance = 0;
_another_vehicle_within_radius = false;
_is_evading_threat = false;
}
@ -107,6 +107,7 @@ void AP_ADSB::update(void)
}
perform_threat_detection();
//hal.console->printf("ADSB: cnt %u, lowT %.0f, highT %.0f\r", _vehicle_count, _lowest_threat_distance, _highest_threat_distance);
}
/*
@ -119,28 +120,53 @@ void AP_ADSB::perform_threat_detection(void)
_ahrs.get_position(my_loc) == false) {
// nothing to do or current location is unknown so we can't calculate any collisions
_another_vehicle_within_radius = false;
_lowest_threat_distance = 0; // 0 means invalid
_highest_threat_distance = 0; // 0 means invalid
return;
}
bool no_threat_within_radius = true;
// 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 < _vehicle_count; index++) {
// TODO: perform more advanced threat detection
Location vehicle_loc = get_location(_vehicle_list[index]);
float distance = get_distance(my_loc, get_location(_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 within radius, set flag and enforce a double radius to clear flag
float threat_distance = get_distance(vehicle_loc, my_loc);
if (threat_distance <= 2*VEHICLE_THREAT_RADIUS_M) {
no_threat_within_radius = false;
if (threat_distance <= VEHICLE_THREAT_RADIUS_M) {
_another_vehicle_within_radius = true;
}
} // if get
} // for
if (distance <= VEHICLE_THREAT_RADIUS_M) {
_vehicle_list[index].threat_level = ADSB_THREAT_HIGH;
} else {
_vehicle_list[index].threat_level = ADSB_THREAT_LOW;
}
} // for index
if (no_threat_within_radius) {
_highest_threat_index = min_distance_index;
_highest_threat_distance = min_distance;
_lowest_threat_index = max_distance_index;
_lowest_threat_distance = max_distance;
// if within radius, set flag and enforce a double radius to clear flag
if (is_zero(_highest_threat_distance) || // 0 means invalid
_highest_threat_distance > 2*VEHICLE_THREAT_RADIUS_M) {
_another_vehicle_within_radius = false;
} else if (_highest_threat_distance <= VEHICLE_THREAT_RADIUS_M) {
_another_vehicle_within_radius = true;
}
}
/*
@ -163,9 +189,18 @@ Location AP_ADSB::get_location(const adsb_vehicle_t &vehicle) const
void AP_ADSB::delete_vehicle(uint16_t index)
{
if (index < _vehicle_count) {
// if the vehicle is the lowest/highest threat, invalidate it
if (index == _lowest_threat_index) {
_lowest_threat_distance = 0;
}
if (index == _highest_threat_index) {
_highest_threat_distance = 0;
}
if (index != _vehicle_count-1) {
_vehicle_list[index] = _vehicle_list[_vehicle_count-1];
}
// TODO: is memset needed? When we decrement the index we essentially forget about it
memset(&_vehicle_list[_vehicle_count-1], 0, sizeof(adsb_vehicle_t));
_vehicle_count--;
}
@ -200,19 +235,48 @@ void AP_ADSB::update_vehicle(const mavlink_message_t* packet)
uint16_t index;
adsb_vehicle_t vehicle {};
mavlink_msg_adsb_vehicle_decode(packet, &vehicle.info);
if (find_index(vehicle, &index)) {
// found, update it
set_vehicle(index, vehicle);
} else if (_vehicle_count < VEHICLE_LIST_LENGTH-1) {
// not found, add it if there's room
// not found and there's room, add it to the end of the list
set_vehicle(_vehicle_count, vehicle);
_vehicle_count++;
} else {
// TODO: buffer is full, delete the vehicle that is furthest away since it is probably not a useful threat to monitor
}
// buffer is full, replace the vehicle with lowest threat as long as it's not further away
Location my_loc;
if (!is_zero(_lowest_threat_distance) && // nonzero means it is valid
_ahrs.get_position(my_loc)) { // true means my_loc is valid
float distance = get_distance(my_loc, get_location(vehicle));
if (distance < _lowest_threat_distance) { // is closer than the furthest
// overwrite the lowest_threat/furthest
index = _lowest_threat_index;
set_vehicle(index, vehicle);
// 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.
_lowest_threat_distance = 0;
// 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 (_highest_threat_distance > distance) {
_highest_threat_distance = distance;
_highest_threat_index = index;
}
} // if distance
} // if !zero
} // if buffer full
}
/*

18
libraries/AP_ADSB/AP_ADSB.h
View File

@ -28,7 +28,7 @@
#include <GCS_MAVLink/GCS.h>
#define VEHICLE_THREAT_RADIUS_M 200
#define VEHICLE_LIST_LENGTH 25 // # of ADS-B vehicles to remember. Additional ones are ignored
#define VEHICLE_LIST_LENGTH 25 // # of ADS-B vehicles to remember at any given time
#define VEHICLE_TIMEOUT_MS 10000 // if no updates in this time, drop it from the list
class AP_ADSB
@ -40,9 +40,15 @@ public:
ADSB_BEHAVIOR_LOITER_AND_DESCEND = 2
};
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() == 42
uint32_t last_update_ms; // last time this was refreshed, allows timeouts
ADSB_THREAT_LEVEL threat_level; // basic threat level
};
@ -98,9 +104,15 @@ private:
AP_Int8 _behavior;
adsb_vehicle_t *_vehicle_list;
uint16_t _vehicle_count = 0;
uint16_t _furthest_vehicle_index = 0;
float _furthest_vehicle_distance = 0;
bool _another_vehicle_within_radius = false;
bool _is_evading_threat = false;
// index of and distance to vehicle with lowest threat
uint16_t _lowest_threat_index = 0;
float _lowest_threat_distance = 0;
// index of and distance to vehicle with highest threat
uint16_t _highest_threat_index = 0;
float _highest_threat_distance = 0;
};
#endif // AP_ADSB_H