Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-06 07:53:57 -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
// @Values: 0:None,1:Loiter,2:LoiterAndDescend
// @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
// @DisplayName: ADSB vehicle list size
// @Description: ADSB list size of nearest vehicles. Longer lists take longer to refresh with lower SRx_ADSB values.
// @Range: 1 100
// @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
};
@ -58,25 +59,28 @@ const AP_Param::GroupInfo AP_ADSB::var_info[] = {
*/
void AP_ADSB::init(void)
{
_vehicle_count = 0;
if (_vehicle_list == nullptr) {
if (_list_size_param != constrain_int16(_list_size_param, 1, ADSB_VEHICLE_LIST_SIZE_MAX)) {
_list_size_param.set_and_notify(ADSB_VEHICLE_LIST_SIZE_DEFAULT);
_list_size_param.save();
// in_state
in_state.vehicle_count = 0;
if (in_state.vehicle_list == nullptr) {
if (in_state.list_size_param != constrain_int16(in_state.list_size_param, 1, ADSB_VEHICLE_LIST_SIZE_MAX)) {
in_state.list_size_param.set_and_notify(ADSB_VEHICLE_LIST_SIZE_DEFAULT);
in_state.list_size_param.save();
}
_list_size = _list_size_param;
_vehicle_list = new adsb_vehicle_t[_list_size];
in_state.list_size = in_state.list_size_param;
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
hal.console->printf("Unable to initialize ADS-B vehicle list\n");
_enabled.set_and_notify(0);
}
}
_lowest_threat_distance = 0;
_highest_threat_distance = 0;
_another_vehicle_within_radius = false;
_is_evading_threat = false;
// 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;
}
/*
@ -84,10 +88,10 @@ void AP_ADSB::init(void)
*/
void AP_ADSB::deinit(void)
{
_vehicle_count = 0;
if (_vehicle_list != nullptr) {
delete [] _vehicle_list;
_vehicle_list = nullptr;
in_state.vehicle_count = 0;
if (in_state.vehicle_list != nullptr) {
delete [] in_state.vehicle_list;
in_state.vehicle_list = nullptr;
}
}
@ -96,24 +100,32 @@ void AP_ADSB::deinit(void)
*/
void AP_ADSB::update(void)
{
// update _my_loc
if (!_ahrs.get_position(_my_loc)) {
_my_loc.zero();
}
if (!_enabled) {
if (_vehicle_list != nullptr) {
if (in_state.vehicle_list != nullptr) {
deinit();
}
// nothing to do
return;
} else if (_vehicle_list == nullptr) {
} else if (in_state.vehicle_list == nullptr) {
init();
return;
} else if (_list_size != _list_size_param) {
} else if (in_state.list_size != in_state.list_size_param) {
deinit();
return;
}
uint32_t now = AP_HAL::millis();
// check current list for vehicles that time out
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
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
// also, if we're disabled then clear the list
delete_vehicle(index);
@ -123,7 +135,6 @@ 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);
}
/*
@ -131,13 +142,11 @@ void AP_ADSB::update(void)
*/
void AP_ADSB::perform_threat_detection(void)
{
Location my_loc;
if (_vehicle_count == 0 ||
_ahrs.get_position(my_loc) == false) {
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
_another_vehicle_within_radius = false;
_lowest_threat_distance = 0; // 0 means invalid
_highest_threat_distance = 0; // 0 means invalid
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;
}
@ -152,8 +161,8 @@ void AP_ADSB::perform_threat_detection(void)
uint16_t min_distance_index = 0;
uint16_t max_distance_index = 0;
for (uint16_t index = 0; index < _vehicle_count; index++) {
float distance = get_distance(my_loc, get_location(_vehicle_list[index]));
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;
@ -164,24 +173,24 @@ void AP_ADSB::perform_threat_detection(void)
}
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 {
_vehicle_list[index].threat_level = ADSB_THREAT_LOW;
in_state.vehicle_list[index].threat_level = ADSB_THREAT_LOW;
}
} // for index
_highest_threat_index = min_distance_index;
_highest_threat_distance = min_distance;
avoid_state.highest_threat_index = min_distance_index;
avoid_state.highest_threat_distance = min_distance;
_lowest_threat_index = max_distance_index;
_lowest_threat_distance = max_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(_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;
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;
}
}
@ -204,21 +213,21 @@ Location AP_ADSB::get_location(const adsb_vehicle_t &vehicle) const
*/
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 (index == _lowest_threat_index) {
_lowest_threat_distance = 0;
if (index == avoid_state.lowest_threat_index) {
avoid_state.lowest_threat_distance = 0;
}
if (index == _highest_threat_index) {
_highest_threat_distance = 0;
if (index == avoid_state.highest_threat_index) {
avoid_state.highest_threat_distance = 0;
}
if (index != (_vehicle_count-1)) {
_vehicle_list[index] = _vehicle_list[_vehicle_count-1];
if (index != (in_state.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
memset(&_vehicle_list[_vehicle_count-1], 0, sizeof(adsb_vehicle_t));
_vehicle_count--;
memset(&in_state.vehicle_list[in_state.vehicle_count-1], 0, sizeof(adsb_vehicle_t));
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
{
for (uint16_t i = 0; i < _vehicle_count; i++) {
if (_vehicle_list[i].info.ICAO_address == vehicle.info.ICAO_address) {
for (uint16_t i = 0; i < in_state.vehicle_count; i++) {
if (in_state.vehicle_list[i].info.ICAO_address == vehicle.info.ICAO_address) {
*index = i;
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)
{
if (_vehicle_list == nullptr) {
if (in_state.vehicle_list == nullptr) {
// We are only null when disabled. Updating is inhibited.
return;
}
@ -258,36 +267,34 @@ void AP_ADSB::update_vehicle(const mavlink_message_t* packet)
// found, update it
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
set_vehicle(_vehicle_count, vehicle);
_vehicle_count++;
set_vehicle(in_state.vehicle_count, vehicle);
in_state.vehicle_count++;
} else {
// 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
if (!is_zero(avoid_state.lowest_threat_distance) && !_my_loc.is_zero()) { // nonzero means it is valid
float distance = get_distance(my_loc, get_location(vehicle));
if (distance < _lowest_threat_distance) { // is closer than the furthest
float distance = _my_loc.get_distance(get_location(vehicle));
if (distance < avoid_state.lowest_threat_distance) { // is closer than the furthest
// overwrite the lowest_threat/furthest
index = _lowest_threat_index;
index = avoid_state.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;
avoid_state.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 (avoid_state.highest_threat_distance > distance) {
avoid_state.highest_threat_distance = distance;
avoid_state.highest_threat_index = index;
}
} // 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)
{
if (index < _list_size) {
_vehicle_list[index] = vehicle;
_vehicle_list[index].last_update_ms = AP_HAL::millis();
if (index < in_state.list_size) {
in_state.vehicle_list[index] = vehicle;
in_state.vehicle_list[index].last_update_ms = AP_HAL::millis();
}
}
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;
}
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
if (now - send_start_ms[chan] < 1000) {
if (now - in_state.send_start_ms[chan] < 1000) {
// too soon to start a new one
return;
} else {
// start new list
send_start_ms[chan] = now;
send_index[chan] = 0;
in_state.send_start_ms[chan] = now;
in_state.send_index[chan] = 0;
}
}
if (send_index[chan] < _vehicle_count) {
mavlink_adsb_vehicle_t vehicle = _vehicle_list[send_index[chan]].info;
send_index[chan]++;
if (in_state.send_index[chan] < in_state.vehicle_count) {
mavlink_adsb_vehicle_t vehicle = in_state.vehicle_list[in_state.send_index[chan]].info;
in_state.send_index[chan]++;
mavlink_msg_adsb_vehicle_send(chan,
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
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_is_evading_threat() { return _enabled && _is_evading_threat; }
void set_is_evading_threat(bool is_evading) { if (_enabled) { _is_evading_threat = is_evading; } }
uint16_t get_vehicle_count() { return _vehicle_count; }
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(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
void send_adsb_vehicle(mavlink_channel_t chan);
@ -106,24 +107,38 @@ private:
const AP_AHRS &_ahrs;
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
uint16_t _lowest_threat_index = 0;
float _lowest_threat_distance = 0;
Location_Class _my_loc;
// index of and distance to vehicle with highest threat
uint16_t _highest_threat_index = 0;
float _highest_threat_distance = 0;
// ADSB-IN state. Maintains list of external vehicles
struct {
// 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
uint32_t send_start_ms[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;
};