Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-11 10:23:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_ADSB: organize variables into in_state and avoid_state structs

Browse Source
This commit is contained in:
Tom Pittenger 2016-07-10 16:20:25 -07:00
parent b78255cc25
commit 454560ca4c
2 changed files with 119 additions and 97 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

165
libraries/AP_ADSB/AP_ADSB.cpp
View File

@ -41,14 +41,15 @@ const AP_Param::GroupInfo AP_ADSB::var_info[] = {
// @Description: ADSB based Collision Avoidance Behavior selector // @Description: ADSB based Collision Avoidance Behavior selector
// @Values: 0:None,1:Loiter,2:LoiterAndDescend // @Values: 0:None,1:Loiter,2:LoiterAndDescend
// @User: Advanced // @User: Advanced
AP_GROUPINFO("BEHAVIOR", 1, AP_ADSB, _behavior, ADSB_BEHAVIOR_NONE), AP_GROUPINFO("BEHAVIOR", 1, AP_ADSB, avoid_state.behavior, ADSB_BEHAVIOR_NONE),
// @Param: LIST_MAX // @Param: LIST_MAX
// @DisplayName: ADSB vehicle list size // @DisplayName: ADSB vehicle list size
// @Description: ADSB list size of nearest vehicles. Longer lists take longer to refresh with lower SRx_ADSB values. // @Description: ADSB list size of nearest vehicles. Longer lists take longer to refresh with lower SRx_ADSB values.
// @Range: 1 100 // @Range: 1 100
// @User: Advanced // @User: Advanced
AP_GROUPINFO("LIST_MAX", 2, AP_ADSB, _list_size_param, ADSB_VEHICLE_LIST_SIZE_DEFAULT), AP_GROUPINFO("LIST_MAX", 2, AP_ADSB, in_state.list_size_param, ADSB_VEHICLE_LIST_SIZE_DEFAULT),
AP_GROUPEND AP_GROUPEND
}; };
@ -58,25 +59,28 @@ const AP_Param::GroupInfo AP_ADSB::var_info[] = {
*/ */
void AP_ADSB::init(void) void AP_ADSB::init(void)
{ {
_vehicle_count = 0; // in_state
if (_vehicle_list == nullptr) { in_state.vehicle_count = 0;
if (_list_size_param != constrain_int16(_list_size_param, 1, ADSB_VEHICLE_LIST_SIZE_MAX)) { if (in_state.vehicle_list == nullptr) {
_list_size_param.set_and_notify(ADSB_VEHICLE_LIST_SIZE_DEFAULT); if (in_state.list_size_param != constrain_int16(in_state.list_size_param, 1, ADSB_VEHICLE_LIST_SIZE_MAX)) {
_list_size_param.save(); in_state.list_size_param.set_and_notify(ADSB_VEHICLE_LIST_SIZE_DEFAULT);
in_state.list_size_param.save();
} }
_list_size = _list_size_param; in_state.list_size = in_state.list_size_param;
_vehicle_list = new adsb_vehicle_t[_list_size]; in_state.vehicle_list = new adsb_vehicle_t[in_state.list_size];
if (_vehicle_list == nullptr) { if (in_state.vehicle_list == nullptr) {
// dynamic RAM allocation of _vehicle_list[] failed, disable gracefully // dynamic RAM allocation of _vehicle_list[] failed, disable gracefully
hal.console->printf("Unable to initialize ADS-B vehicle list\n"); hal.console->printf("Unable to initialize ADS-B vehicle list\n");
_enabled.set_and_notify(0); _enabled.set_and_notify(0);
} }
} }
_lowest_threat_distance = 0;
_highest_threat_distance = 0; // avoid_state
_another_vehicle_within_radius = false; avoid_state.lowest_threat_distance = 0;
_is_evading_threat = false; avoid_state.highest_threat_distance = 0;
avoid_state.another_vehicle_within_radius = false;
avoid_state.is_evading_threat = false;
} }
/* /*
@ -84,10 +88,10 @@ void AP_ADSB::init(void)
*/ */
void AP_ADSB::deinit(void) void AP_ADSB::deinit(void)
{ {
_vehicle_count = 0; in_state.vehicle_count = 0;
if (_vehicle_list != nullptr) { if (in_state.vehicle_list != nullptr) {
delete [] _vehicle_list; delete [] in_state.vehicle_list;
_vehicle_list = nullptr; in_state.vehicle_list = nullptr;
} }
} }
@ -96,24 +100,32 @@ void AP_ADSB::deinit(void)
*/ */
void AP_ADSB::update(void) void AP_ADSB::update(void)
{ {
// update _my_loc
if (!_ahrs.get_position(_my_loc)) {
_my_loc.zero();
}
if (!_enabled) { if (!_enabled) {
if (_vehicle_list != nullptr) { if (in_state.vehicle_list != nullptr) {
deinit(); deinit();
} }
// nothing to do // nothing to do
return; return;
} else if (_vehicle_list == nullptr) { } else if (in_state.vehicle_list == nullptr) {
init(); init();
return; return;
} else if (_list_size != _list_size_param) { } else if (in_state.list_size != in_state.list_size_param) {
deinit(); deinit();
return; return;
} }
uint32_t now = AP_HAL::millis();
// check current list for vehicles that time out
uint16_t index = 0; uint16_t index = 0;
while (index < _vehicle_count) { while (index < in_state.vehicle_count) {
// check list and drop stale vehicles. When disabled, the list will get flushed // check list and drop stale vehicles. When disabled, the list will get flushed
if (AP_HAL::millis() - _vehicle_list[index].last_update_ms > VEHICLE_TIMEOUT_MS) { if (now - in_state.vehicle_list[index].last_update_ms > VEHICLE_TIMEOUT_MS) {
// don't increment index, we want to check this same index again because the contents changed // don't increment index, we want to check this same index again because the contents changed
// also, if we're disabled then clear the list // also, if we're disabled then clear the list
delete_vehicle(index); delete_vehicle(index);
@ -123,7 +135,6 @@ void AP_ADSB::update(void)
} }
perform_threat_detection(); perform_threat_detection();
//hal.console->printf("ADSB: cnt %u, lowT %.0f, highT %.0f\r", _vehicle_count, _lowest_threat_distance, _highest_threat_distance);
} }
/* /*
@ -131,13 +142,11 @@ void AP_ADSB::update(void)
*/ */
void AP_ADSB::perform_threat_detection(void) void AP_ADSB::perform_threat_detection(void)
{ {
Location my_loc; if (in_state.vehicle_count == 0 || _my_loc.is_zero()) {
if (_vehicle_count == 0 ||
_ahrs.get_position(my_loc) == false) {
// nothing to do or current location is unknown so we can't calculate any collisions // nothing to do or current location is unknown so we can't calculate any collisions
_another_vehicle_within_radius = false; avoid_state.another_vehicle_within_radius = false;
_lowest_threat_distance = 0; // 0 means invalid avoid_state.lowest_threat_distance = 0; // 0 means invalid
_highest_threat_distance = 0; // 0 means invalid avoid_state.highest_threat_distance = 0; // 0 means invalid
return; return;
} }
@ -152,8 +161,8 @@ void AP_ADSB::perform_threat_detection(void)
uint16_t min_distance_index = 0; uint16_t min_distance_index = 0;
uint16_t max_distance_index = 0; uint16_t max_distance_index = 0;
for (uint16_t index = 0; index < _vehicle_count; index++) { for (uint16_t index = 0; index < in_state.vehicle_count; index++) {
float distance = get_distance(my_loc, get_location(_vehicle_list[index])); float distance = _my_loc.get_distance(get_location(in_state.vehicle_list[index]));
if (min_distance > distance || index == 0) { if (min_distance > distance || index == 0) {
min_distance = distance; min_distance = distance;
min_distance_index = index; min_distance_index = index;
@ -164,24 +173,24 @@ void AP_ADSB::perform_threat_detection(void)
} }
if (distance <= VEHICLE_THREAT_RADIUS_M) { if (distance <= VEHICLE_THREAT_RADIUS_M) {
_vehicle_list[index].threat_level = ADSB_THREAT_HIGH; in_state.vehicle_list[index].threat_level = ADSB_THREAT_HIGH;
} else { } else {
_vehicle_list[index].threat_level = ADSB_THREAT_LOW; in_state.vehicle_list[index].threat_level = ADSB_THREAT_LOW;
} }
} // for index } // for index
_highest_threat_index = min_distance_index; avoid_state.highest_threat_index = min_distance_index;
_highest_threat_distance = min_distance; avoid_state.highest_threat_distance = min_distance;
_lowest_threat_index = max_distance_index; avoid_state.lowest_threat_index = max_distance_index;
_lowest_threat_distance = max_distance; avoid_state.lowest_threat_distance = max_distance;
// if within radius, set flag and enforce a double radius to clear flag // if within radius, set flag and enforce a double radius to clear flag
if (is_zero(_highest_threat_distance) || // 0 means invalid if (is_zero(avoid_state.highest_threat_distance) || // 0 means invalid
_highest_threat_distance > 2*VEHICLE_THREAT_RADIUS_M) { avoid_state.highest_threat_distance > 2*VEHICLE_THREAT_RADIUS_M) {
_another_vehicle_within_radius = false; avoid_state.another_vehicle_within_radius = false;
} else if (_highest_threat_distance <= VEHICLE_THREAT_RADIUS_M) { } else if (avoid_state.highest_threat_distance <= VEHICLE_THREAT_RADIUS_M) {
_another_vehicle_within_radius = true; avoid_state.another_vehicle_within_radius = true;
} }
} }
@ -204,21 +213,21 @@ Location AP_ADSB::get_location(const adsb_vehicle_t &vehicle) const
*/ */
void AP_ADSB::delete_vehicle(uint16_t index) void AP_ADSB::delete_vehicle(uint16_t index)
{ {
if (index < _vehicle_count) { if (index < in_state.vehicle_count) {
// if the vehicle is the lowest/highest threat, invalidate it // if the vehicle is the lowest/highest threat, invalidate it
if (index == _lowest_threat_index) { if (index == avoid_state.lowest_threat_index) {
_lowest_threat_distance = 0; avoid_state.lowest_threat_distance = 0;
} }
if (index == _highest_threat_index) { if (index == avoid_state.highest_threat_index) {
_highest_threat_distance = 0; avoid_state.highest_threat_distance = 0;
} }
if (index != (_vehicle_count-1)) { if (index != (in_state.vehicle_count-1)) {
_vehicle_list[index] = _vehicle_list[_vehicle_count-1]; in_state.vehicle_list[index] = in_state.vehicle_list[in_state.vehicle_count-1];
} }
// TODO: is memset needed? When we decrement the index we essentially forget about it // 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)); memset(&in_state.vehicle_list[in_state.vehicle_count-1], 0, sizeof(adsb_vehicle_t));
_vehicle_count--; in_state.vehicle_count--;
} }
} }
@ -229,8 +238,8 @@ void AP_ADSB::delete_vehicle(uint16_t index)
*/ */
bool AP_ADSB::find_index(const adsb_vehicle_t &vehicle, uint16_t *index) const bool AP_ADSB::find_index(const adsb_vehicle_t &vehicle, uint16_t *index) const
{ {
for (uint16_t i = 0; i < _vehicle_count; i++) { for (uint16_t i = 0; i < in_state.vehicle_count; i++) {
if (_vehicle_list[i].info.ICAO_address == vehicle.info.ICAO_address) { if (in_state.vehicle_list[i].info.ICAO_address == vehicle.info.ICAO_address) {
*index = i; *index = i;
return true; return true;
} }
@ -244,7 +253,7 @@ bool AP_ADSB::find_index(const adsb_vehicle_t &vehicle, uint16_t *index) const
*/ */
void AP_ADSB::update_vehicle(const mavlink_message_t* packet) void AP_ADSB::update_vehicle(const mavlink_message_t* packet)
{ {
if (_vehicle_list == nullptr) { if (in_state.vehicle_list == nullptr) {
// We are only null when disabled. Updating is inhibited. // We are only null when disabled. Updating is inhibited.
return; return;
} }
@ -258,36 +267,34 @@ void AP_ADSB::update_vehicle(const mavlink_message_t* packet)
// found, update it // found, update it
set_vehicle(index, vehicle); set_vehicle(index, vehicle);
} else if (_vehicle_count < _list_size) { } else if (in_state.vehicle_count < in_state.list_size) {
// not found and there's room, add it to the end of the list // not found and there's room, add it to the end of the list
set_vehicle(_vehicle_count, vehicle); set_vehicle(in_state.vehicle_count, vehicle);
_vehicle_count++; in_state.vehicle_count++;
} else { } else {
// buffer is full, replace the vehicle with lowest threat as long as it's not further away // buffer is full, replace the vehicle with lowest threat as long as it's not further away
Location my_loc; if (!is_zero(avoid_state.lowest_threat_distance) && !_my_loc.is_zero()) { // nonzero means it is valid
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)); float distance = _my_loc.get_distance(get_location(vehicle));
if (distance < _lowest_threat_distance) { // is closer than the furthest if (distance < avoid_state.lowest_threat_distance) { // is closer than the furthest
// overwrite the lowest_threat/furthest // overwrite the lowest_threat/furthest
index = _lowest_threat_index; index = avoid_state.lowest_threat_index;
set_vehicle(index, vehicle); set_vehicle(index, vehicle);
// this is now invalid because the vehicle was overwritten, need // this is now invalid because the vehicle was overwritten, need
// to run perform_threat_detection() to determine new one because // to run perform_threat_detection() to determine new one because
// we aren't keeping track of the second-furthest vehicle. // we aren't keeping track of the second-furthest vehicle.
_lowest_threat_distance = 0; avoid_state.lowest_threat_distance = 0;
// is it the nearest? Then it's the highest threat. That's an easy check // 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 // that we don't need to run perform_threat_detection() to determine
if (_highest_threat_distance > distance) { if (avoid_state.highest_threat_distance > distance) {
_highest_threat_distance = distance; avoid_state.highest_threat_distance = distance;
_highest_threat_index = index; avoid_state.highest_threat_index = index;
} }
} // if distance } // if distance
@ -300,35 +307,35 @@ void AP_ADSB::update_vehicle(const mavlink_message_t* packet)
*/ */
void AP_ADSB::set_vehicle(uint16_t index, const adsb_vehicle_t &vehicle) void AP_ADSB::set_vehicle(uint16_t index, const adsb_vehicle_t &vehicle)
{ {
if (index < _list_size) { if (index < in_state.list_size) {
_vehicle_list[index] = vehicle; in_state.vehicle_list[index] = vehicle;
_vehicle_list[index].last_update_ms = AP_HAL::millis(); in_state.vehicle_list[index].last_update_ms = AP_HAL::millis();
} }
} }
void AP_ADSB::send_adsb_vehicle(mavlink_channel_t chan) void AP_ADSB::send_adsb_vehicle(mavlink_channel_t chan)
{ {
if (_vehicle_list == nullptr || _vehicle_count == 0) { if (in_state.vehicle_list == nullptr || in_state.vehicle_count == 0) {
return; return;
} }
uint32_t now = AP_HAL::millis(); uint32_t now = AP_HAL::millis();
if (send_index[chan] >= _vehicle_count) { if (in_state.send_index[chan] >= in_state.vehicle_count) {
// we've finished a list // we've finished a list
if (now - send_start_ms[chan] < 1000) { if (now - in_state.send_start_ms[chan] < 1000) {
// too soon to start a new one // too soon to start a new one
return; return;
} else { } else {
// start new list // start new list
send_start_ms[chan] = now; in_state.send_start_ms[chan] = now;
send_index[chan] = 0; in_state.send_index[chan] = 0;
} }
} }
if (send_index[chan] < _vehicle_count) { if (in_state.send_index[chan] < in_state.vehicle_count) {
mavlink_adsb_vehicle_t vehicle = _vehicle_list[send_index[chan]].info; mavlink_adsb_vehicle_t vehicle = in_state.vehicle_list[in_state.send_index[chan]].info;
send_index[chan]++; in_state.send_index[chan]++;
mavlink_msg_adsb_vehicle_send(chan, mavlink_msg_adsb_vehicle_send(chan,
vehicle.ICAO_address, vehicle.ICAO_address,

51
libraries/AP_ADSB/AP_ADSB.h
View File

@ -69,12 +69,13 @@ public:
// add or update vehicle_list from inbound mavlink msg // add or update vehicle_list from inbound mavlink msg
void update_vehicle(const mavlink_message_t* msg); void update_vehicle(const mavlink_message_t* msg);
bool get_possible_threat() { return _enabled && _another_vehicle_within_radius; }
ADSB_BEHAVIOR get_behavior() { return (ADSB_BEHAVIOR)(_behavior.get()); } bool get_possible_threat() { return _enabled && avoid_state.another_vehicle_within_radius; }
bool get_is_evading_threat() { return _enabled && _is_evading_threat; }
void set_is_evading_threat(bool is_evading) { if (_enabled) { _is_evading_threat = is_evading; } } ADSB_BEHAVIOR get_behavior() { return (ADSB_BEHAVIOR)(avoid_state.behavior.get()); }
uint16_t get_vehicle_count() { return _vehicle_count; } bool get_is_evading_threat() { return _enabled && avoid_state.is_evading_threat; }
void set_is_evading_threat(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 // send ADSB_VEHICLE mavlink message, usually as a StreamRate
void send_adsb_vehicle(mavlink_channel_t chan); void send_adsb_vehicle(mavlink_channel_t chan);
@ -106,24 +107,38 @@ private:
const AP_AHRS &_ahrs; const AP_AHRS &_ahrs;
AP_Int8 _enabled; AP_Int8 _enabled;
AP_Int8 _behavior;
AP_Int16 _list_size_param;
uint16_t _list_size = 1; // start with tiny list, then change to param-defined size. This ensures it doesn't fail on start
adsb_vehicle_t *_vehicle_list;
uint16_t _vehicle_count = 0;
bool _another_vehicle_within_radius = false;
bool _is_evading_threat = false;
// index of and distance to vehicle with lowest threat Location_Class _my_loc;
uint16_t _lowest_threat_index = 0;
float _lowest_threat_distance = 0;
// index of and distance to vehicle with highest threat // ADSB-IN state. Maintains list of external vehicles
uint16_t _highest_threat_index = 0; struct {
float _highest_threat_distance = 0; // list management
AP_Int16 list_size_param;
uint16_t list_size = 1; // start with tiny list, then change to param-defined size. This ensures it doesn't fail on start
adsb_vehicle_t *vehicle_list = nullptr;
uint16_t vehicle_count = 0;
// streamrate stuff // streamrate stuff
uint32_t send_start_ms[MAVLINK_COMM_NUM_BUFFERS]; uint32_t send_start_ms[MAVLINK_COMM_NUM_BUFFERS];
uint16_t send_index[MAVLINK_COMM_NUM_BUFFERS]; uint16_t send_index[MAVLINK_COMM_NUM_BUFFERS];
} in_state;
// Avoidance state
struct {
AP_Int8 behavior;
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;
} avoid_state;
}; };