/*
* Update the vehicle list. If the vehicle is already in the
* list then it will update it, otherwise it will be added.
*/
void AP_ADSB::update_vehicle(const mavlink_message_t* packet)
{
if (in_state.vehicle_list == nullptr) {
// We are only null when disabled. Updating is inhibited.
return;
}
uint16_t index = in_state.list_size + 1; // initialize with invalid index
adsb_vehicle_t vehicle {};
mavlink_msg_adsb_vehicle_decode(packet, &vehicle.info);
Location_Class vehicle_loc = Location_Class(AP_ADSB::get_location(vehicle));
bool my_loc_is_zero = _my_loc.is_zero();
float my_loc_distance_to_vehicle = _my_loc.get_distance(vehicle_loc);
bool out_of_range = in_state.list_radius > 0 && !my_loc_is_zero && my_loc_distance_to_vehicle > in_state.list_radius;
bool is_tracked_in_list = find_index(vehicle, &index);
if (vehicle_loc.is_zero() || out_of_range) {
// vehicle is out of range or invalid lat/lng. If we're tracking it, delete from list. Otherwise ignore it.
if (is_tracked_in_list) {
delete_vehicle(index);
}
return;
} else if (is_tracked_in_list) {
// found, update it
set_vehicle(index, vehicle);
} 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(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
// 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);
// 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;
// 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 buffer full
vehicle.last_update_ms = AP_HAL::millis() - (vehicle.info.tslc * 1000);
push_sample(vehicle); // note that set_vehicle modifies vehicle
}
/*
* Update the vehicle list. If the vehicle is already in the
* list then it will update it, otherwise it will be added.
*/
void AP_ADSB::update_vehicle(const mavlink_message_t* packet)
{
if (_vehicle_list == NULL) {
// We are only null when disabled. Updating is inhibited.
return;
}
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) {
// not found and there's room, add it to the end of the list
set_vehicle(_vehicle_count, vehicle);
_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
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
}
/*
* Update the vehicle list. If the vehicle is already in the
* list then it will update it, otherwise it will be added.
*/
void AP_ADSB::update_vehicle(const mavlink_message_t* packet)
{
if (_vehicle_list == NULL) {
// We are only null when disabled. Updating is inhibited.
return;
}
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
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
}
}
/*
* Update the vehicle list. If the vehicle is already in the
* list then it will update it, otherwise it will be added.
*/
void AP_ADSB::handle_vehicle(const mavlink_message_t* packet)
{
if (in_state.vehicle_list == nullptr) {
// We are only null when disabled. Updating is inhibited.
return;
}
uint16_t index = in_state.list_size + 1; // initialize with invalid index
adsb_vehicle_t vehicle {};
mavlink_msg_adsb_vehicle_decode(packet, &vehicle.info);
Location_Class vehicle_loc = Location_Class(AP_ADSB::get_location(vehicle));
bool my_loc_is_zero = _my_loc.is_zero();
float my_loc_distance_to_vehicle = _my_loc.get_distance(vehicle_loc);
bool out_of_range = in_state.list_radius > 0 && !my_loc_is_zero && my_loc_distance_to_vehicle > in_state.list_radius;
bool is_tracked_in_list = find_index(vehicle, &index);
uint32_t now = AP_HAL::millis();
// note the last time the receiver got a packet from the aircraft
vehicle.last_update_ms = now - (vehicle.info.tslc * 1000);
const uint16_t required_flags_position = ADSB_FLAGS_VALID_COORDS | ADSB_FLAGS_VALID_ALTITUDE;
const bool detected_ourself = (out_state.cfg.ICAO_id != 0) && ((uint32_t)out_state.cfg.ICAO_id == vehicle.info.ICAO_address);
if (vehicle_loc.is_zero() ||
out_of_range ||
detected_ourself ||
(vehicle.info.ICAO_address > 0x00FFFFFF) || // ICAO address is 24bits, so ignore higher values.
!(vehicle.info.flags & required_flags_position) ||
now - vehicle.last_update_ms > VEHICLE_TIMEOUT_MS) {
// vehicle is out of range or invalid lat/lng. If we're tracking it, delete from list. Otherwise ignore it.
if (is_tracked_in_list) {
delete_vehicle(index);
}
return;
} else if (is_tracked_in_list) {
// found, update it
set_vehicle(index, vehicle);
} 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(in_state.vehicle_count, vehicle);
in_state.vehicle_count++;
} else {
// buffer is full. if new vehicle is closer than furthest, replace furthest with new
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();
}
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
const uint16_t required_flags_avoidance =
ADSB_FLAGS_VALID_COORDS |
ADSB_FLAGS_VALID_ALTITUDE |
ADSB_FLAGS_VALID_HEADING |
ADSB_FLAGS_VALID_VELOCITY;
if (vehicle.info.flags & required_flags_avoidance) {
push_sample(vehicle); // note that set_vehicle modifies vehicle
}
}
