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

AP_ADSB: refactor and renamed things and general cleanup for pre-backend split

Browse Source
This commit is contained in:
Tom Pittenger 2020-10-28 06:44:49 -07:00 committed by Peter Barker
parent 8bcc32db78
commit 7a09f88e83
2 changed files with 180 additions and 170 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

246
libraries/AP_ADSB/AP_ADSB.cpp
View File

@ -20,8 +20,8 @@
https://en.wikipedia.org/wiki/Automatic_dependent_surveillance_%E2%80%93_broadcast https://en.wikipedia.org/wiki/Automatic_dependent_surveillance_%E2%80%93_broadcast
*/ */
#include <AP_HAL/AP_HAL.h>
#include "AP_ADSB.h" #include "AP_ADSB.h"
#if HAL_ADSB_ENABLED #if HAL_ADSB_ENABLED
#include <GCS_MAVLink/GCS_MAVLink.h> #include <GCS_MAVLink/GCS_MAVLink.h>
#include <stdio.h> // for sprintf #include <stdio.h> // for sprintf
@ -35,18 +35,23 @@
#define VEHICLE_TIMEOUT_MS 5000 // if no updates in this time, drop it from the list #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
#define ADSB_CHAN_TIMEOUT_MS 15000 #define ADSB_CHAN_TIMEOUT_MS 15000
#define ADSB_SQUAWK_OCTAL_DEFAULT 1200 #define ADSB_SQUAWK_OCTAL_DEFAULT 1200
#define ADSB_BITBASK_RF_CAPABILITIES_UAT_IN (1 << 0) #define ADSB_BITBASK_RF_CAPABILITIES_UAT_IN (1 << 0)
#define ADSB_BITBASK_RF_CAPABILITIES_1090ES_IN (1 << 1) #define ADSB_BITBASK_RF_CAPABILITIES_1090ES_IN (1 << 1)
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane) #ifndef ADSB_VEHICLE_LIST_SIZE_DEFAULT
#define ADSB_LIST_RADIUS_DEFAULT 10000 // in meters #define ADSB_VEHICLE_LIST_SIZE_DEFAULT 25
#else // APM_BUILD_TYPE(APM_BUILD_ArduCopter), Rover, Boat #endif
#define ADSB_LIST_RADIUS_DEFAULT 2000 // in meters
#ifndef ADSB_LIST_RADIUS_DEFAULT
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
#define ADSB_LIST_RADIUS_DEFAULT 10000 // in meters
#else
#define ADSB_LIST_RADIUS_DEFAULT 2000 // in meters
#endif
#endif #endif
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
@ -69,6 +74,7 @@ const AP_Param::GroupInfo AP_ADSB::var_info[] = {
// @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
// @RebootRequired: True
AP_GROUPINFO("LIST_MAX", 2, AP_ADSB, in_state.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),
@ -106,19 +112,19 @@ const AP_Param::GroupInfo AP_ADSB::var_info[] = {
// @Description: GPS antenna lateral offset. This describes the physical location offest from center of the GPS antenna on the aircraft. // @Description: GPS antenna lateral offset. This describes the physical location offest from center of the GPS antenna on the aircraft.
// @Values: 0:NoData,1:Left2m,2:Left4m,3:Left6m,4:Center,5:Right2m,6:Right4m,7:Right6m // @Values: 0:NoData,1:Left2m,2:Left4m,3:Left6m,4:Center,5:Right2m,6:Right4m,7:Right6m
// @User: Advanced // @User: Advanced
AP_GROUPINFO("OFFSET_LAT", 7, AP_ADSB, out_state.cfg.gpsLatOffset, UAVIONIX_ADSB_OUT_CFG_GPS_OFFSET_LAT_RIGHT_0M), AP_GROUPINFO("OFFSET_LAT", 7, AP_ADSB, out_state.cfg.gpsOffsetLat, UAVIONIX_ADSB_OUT_CFG_GPS_OFFSET_LAT_RIGHT_0M),
// @Param: OFFSET_LON // @Param: OFFSET_LON
// @DisplayName: GPS antenna longitudinal offset // @DisplayName: GPS antenna longitudinal offset
// @Description: GPS antenna longitudinal offset. This is usually set to 1, Applied By Sensor // @Description: GPS antenna longitudinal offset. This is usually set to 1, Applied By Sensor
// @Values: 0:NO_DATA,1:AppliedBySensor // @Values: 0:NO_DATA,1:AppliedBySensor
// @User: Advanced // @User: Advanced
AP_GROUPINFO("OFFSET_LON", 8, AP_ADSB, out_state.cfg.gpsLonOffset, UAVIONIX_ADSB_OUT_CFG_GPS_OFFSET_LON_APPLIED_BY_SENSOR), AP_GROUPINFO("OFFSET_LON", 8, AP_ADSB, out_state.cfg.gpsOffsetLon, UAVIONIX_ADSB_OUT_CFG_GPS_OFFSET_LON_APPLIED_BY_SENSOR),
// @Param: RF_SELECT // @Param: RF_SELECT
// @DisplayName: Transceiver RF selection // @DisplayName: Transceiver RF selection
// @Description: Transceiver RF selection for Rx enable and/or Tx enable. This only effects devices that can Tx and Rx. Rx-only devices override this to always be Rx-only. // @Description: Transceiver RF selection for Rx enable and/or Tx enable. This only effects devices that can Tx and/or Rx. Rx-only devices should override this to always be Rx-only.
// @Values: 0:Disabled,1:Rx-Only,2:Tx-Only,3:Rx and Tx Enabled // @Bitmask: 0:Rx,1:Tx
// @User: Advanced // @User: Advanced
AP_GROUPINFO("RF_SELECT", 9, AP_ADSB, out_state.cfg.rfSelect, UAVIONIX_ADSB_OUT_RF_SELECT_RX_ENABLED), AP_GROUPINFO("RF_SELECT", 9, AP_ADSB, out_state.cfg.rfSelect, UAVIONIX_ADSB_OUT_RF_SELECT_RX_ENABLED),
@ -166,10 +172,16 @@ const AP_Param::GroupInfo AP_ADSB::var_info[] = {
AP_ADSB::AP_ADSB() AP_ADSB::AP_ADSB()
{ {
AP_Param::setup_object_defaults(this, var_info); AP_Param::setup_object_defaults(this, var_info);
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
if (_singleton != nullptr) { if (_singleton != nullptr) {
AP_HAL::panic("AP_ADSB must be singleton"); AP_HAL::panic("AP_ADSB must be singleton");
} }
#endif
_singleton = this; _singleton = this;
#ifdef ADSB_STATIC_CALLSIGN
strncpy(&out_state.cfg.callsign, ADSB_STATIC_CALLSIGN, sizeof(out_state.cfg.callsign));
#endif
} }
/* /*
@ -177,28 +189,20 @@ AP_ADSB::AP_ADSB()
*/ */
void AP_ADSB::init(void) void AP_ADSB::init(void)
{ {
// in_state
in_state.vehicle_count = 0;
if (in_state.vehicle_list == nullptr) { 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)) { // sanity check param
in_state.list_size_param.set_and_notify(ADSB_VEHICLE_LIST_SIZE_DEFAULT); in_state.list_size_param = constrain_int16(in_state.list_size_param, 1, INT16_MAX);
in_state.list_size_param.save();
} in_state.vehicle_list = new adsb_vehicle_t[in_state.list_size_param];
in_state.list_size = in_state.list_size_param;
in_state.vehicle_list = new adsb_vehicle_t[in_state.list_size];
if (in_state.vehicle_list == nullptr) { if (in_state.vehicle_list == nullptr) {
// dynamic RAM allocation of _vehicle_list[] failed, disable gracefully // dynamic RAM allocation of in_state.vehicle_list[] failed
hal.console->printf("Unable to initialize ADS-B vehicle list\n"); _init_failed = true; // this keeps us from constantly trying to init forever in main update
_enabled.set_and_notify(0); gcs().send_text(MAV_SEVERITY_INFO, "ADSB: Unable to initialize ADSB vehicle list");
return;
} }
in_state.list_size_allocated = in_state.list_size_param;
} }
furthest_vehicle_distance = 0;
furthest_vehicle_index = 0;
// out_state
set_callsign("PING1234", false);
} }
/* /*
@ -206,13 +210,27 @@ void AP_ADSB::init(void)
*/ */
void AP_ADSB::deinit(void) void AP_ADSB::deinit(void)
{ {
in_state.vehicle_count = 0;
if (in_state.vehicle_list != nullptr) { if (in_state.vehicle_list != nullptr) {
delete [] in_state.vehicle_list; delete [] in_state.vehicle_list;
in_state.vehicle_list = nullptr; in_state.vehicle_list = nullptr;
} }
} }
bool AP_ADSB::check_startup()
{
if (!_enabled || _init_failed) {
if (in_state.vehicle_list != nullptr) {
deinit();
}
// nothing to do
return false;
}
if (in_state.vehicle_list == nullptr) {
init();
}
return in_state.vehicle_list != nullptr;
}
bool AP_ADSB::is_valid_callsign(uint16_t octal) bool AP_ADSB::is_valid_callsign(uint16_t octal)
{ {
// treat "octal" as decimal and test if any decimal digit is > 7 // treat "octal" as decimal and test if any decimal digit is > 7
@ -235,27 +253,16 @@ bool AP_ADSB::is_valid_callsign(uint16_t octal)
*/ */
void AP_ADSB::update(void) void AP_ADSB::update(void)
{ {
// update _my_loc if (!check_startup()) {
if (!AP::ahrs().get_position(_my_loc)) {
_my_loc.zero();
}
if (!_enabled) {
if (in_state.vehicle_list != nullptr) {
deinit();
}
// nothing to do
return;
} else if (in_state.vehicle_list == nullptr) {
init();
return;
} else if (in_state.list_size != in_state.list_size_param) {
deinit();
return; return;
} }
const uint32_t now = AP_HAL::millis(); const uint32_t now = AP_HAL::millis();
if (!AP::ahrs().get_position(_my_loc)) {
_my_loc.zero();
}
// check current list for vehicles that time out // check current list for vehicles that time out
uint16_t index = 0; uint16_t index = 0;
while (index < in_state.vehicle_count) { while (index < in_state.vehicle_count) {
@ -269,17 +276,6 @@ 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;
}
if (out_state.chan < 0) {
// if there's no transceiver detected then do not set ICAO and do not service the transceiver
return;
}
if (out_state.cfg.squawk_octal_param != out_state.cfg.squawk_octal) { if (out_state.cfg.squawk_octal_param != out_state.cfg.squawk_octal) {
// param changed, check that it's a valid octal // param changed, check that it's a valid octal
if (!is_valid_callsign(out_state.cfg.squawk_octal_param)) { if (!is_valid_callsign(out_state.cfg.squawk_octal_param)) {
@ -295,8 +291,8 @@ void AP_ADSB::update(void)
// reset timer constantly so it never reaches 10s so it never sends // reset timer constantly so it never reaches 10s so it never sends
out_state.last_config_ms = now; out_state.last_config_ms = now;
} else if (out_state.cfg.ICAO_id == 0 || } else if ((out_state.cfg.rfSelect & UAVIONIX_ADSB_OUT_RF_SELECT_TX_ENABLED) &&
out_state.cfg.ICAO_id_param_prev != out_state.cfg.ICAO_id_param) { (out_state.cfg.ICAO_id == 0 || out_state.cfg.ICAO_id_param_prev != out_state.cfg.ICAO_id_param)) {
// if param changed then regenerate. This allows the param to be changed back to zero to trigger a re-generate // if param changed then regenerate. This allows the param to be changed back to zero to trigger a re-generate
if (out_state.cfg.ICAO_id_param == 0) { if (out_state.cfg.ICAO_id_param == 0) {
@ -305,19 +301,31 @@ void AP_ADSB::update(void)
out_state.cfg.ICAO_id = out_state.cfg.ICAO_id_param; out_state.cfg.ICAO_id = out_state.cfg.ICAO_id_param;
} }
out_state.cfg.ICAO_id_param_prev = out_state.cfg.ICAO_id_param; out_state.cfg.ICAO_id_param_prev = out_state.cfg.ICAO_id_param;
set_callsign("PING", true);
#ifndef ADSB_STATIC_CALLSIGN
if (!out_state.cfg.was_set_externally) {
set_callsign("PING", true);
}
#endif
gcs().send_text(MAV_SEVERITY_INFO, "ADSB: Using ICAO_id %d and Callsign %s", (int)out_state.cfg.ICAO_id, out_state.cfg.callsign); gcs().send_text(MAV_SEVERITY_INFO, "ADSB: Using ICAO_id %d and Callsign %s", (int)out_state.cfg.ICAO_id, out_state.cfg.callsign);
out_state.last_config_ms = 0; // send now out_state.last_config_ms = 0; // send now
} }
update_uavionix();
}
void AP_ADSB::update_uavionix()
{
const uint32_t now = AP_HAL::millis();
// send static configuration data to transceiver, every 5s // send static configuration data to transceiver, every 5s
if (out_state.chan_last_ms > 0 && now - out_state.chan_last_ms > ADSB_CHAN_TIMEOUT_MS) { if (out_state.chan_last_ms > 0 && now - out_state.chan_last_ms > ADSB_CHAN_TIMEOUT_MS) {
// haven't gotten a heartbeat health status packet in a while, assume hardware failure // haven't gotten a heartbeat health status packet in a while, assume hardware failure
// TODO: reset out_state.chan // TODO: reset out_state.chan
out_state.chan = -1; out_state.chan = -1;
gcs().send_text(MAV_SEVERITY_ERROR, "ADSB: Transceiver heartbeat timed out"); gcs().send_text(MAV_SEVERITY_ERROR, "ADSB: Transceiver heartbeat timed out");
} else if (out_state.chan < MAVLINK_COMM_NUM_BUFFERS) { } else if (out_state.chan >= 0 && !_my_loc.is_zero() && out_state.chan < MAVLINK_COMM_NUM_BUFFERS) {
const mavlink_channel_t chan = (mavlink_channel_t)(MAVLINK_COMM_0 + out_state.chan); const mavlink_channel_t chan = (mavlink_channel_t)(MAVLINK_COMM_0 + out_state.chan);
if (now - out_state.last_config_ms >= 5000 && HAVE_PAYLOAD_SPACE(chan, UAVIONIX_ADSB_OUT_CFG)) { if (now - out_state.last_config_ms >= 5000 && HAVE_PAYLOAD_SPACE(chan, UAVIONIX_ADSB_OUT_CFG)) {
out_state.last_config_ms = now; out_state.last_config_ms = now;
@ -352,8 +360,8 @@ void AP_ADSB::determine_furthest_aircraft(void)
} }
} // for index } // for index
furthest_vehicle_index = max_distance_index; in_state.furthest_vehicle_index = max_distance_index;
furthest_vehicle_distance = max_distance; in_state.furthest_vehicle_distance = max_distance;
} }
/* /*
@ -382,9 +390,9 @@ void AP_ADSB::delete_vehicle(const uint16_t index)
} }
// if the vehicle is the furthest, invalidate it. It has been bumped // if the vehicle is the furthest, invalidate it. It has been bumped
if (index == furthest_vehicle_index && furthest_vehicle_distance > 0) { if (index == in_state.furthest_vehicle_index && in_state.furthest_vehicle_distance > 0) {
furthest_vehicle_distance = 0; in_state.furthest_vehicle_distance = 0;
furthest_vehicle_index = 0; in_state.furthest_vehicle_index = 0;
} }
if (index != (in_state.vehicle_count-1)) { if (index != (in_state.vehicle_count-1)) {
in_state.vehicle_list[index] = in_state.vehicle_list[in_state.vehicle_count-1]; in_state.vehicle_list[index] = in_state.vehicle_list[in_state.vehicle_count-1];
@ -416,12 +424,11 @@ bool AP_ADSB::find_index(const adsb_vehicle_t &vehicle, uint16_t *index) const
*/ */
void AP_ADSB::handle_adsb_vehicle(const adsb_vehicle_t &vehicle) void AP_ADSB::handle_adsb_vehicle(const adsb_vehicle_t &vehicle)
{ {
if (in_state.vehicle_list == nullptr) { if (!check_startup()) {
// We are only null when disabled. Updating is inhibited.
return; return;
} }
uint16_t index = in_state.list_size + 1; // initialize with invalid index uint16_t index = in_state.list_size_allocated + 1; // initialize with invalid index
const Location vehicle_loc = AP_ADSB::get_location(vehicle); const Location vehicle_loc = AP_ADSB::get_location(vehicle);
const bool my_loc_is_zero = _my_loc.is_zero(); const bool my_loc_is_zero = _my_loc.is_zero();
const float my_loc_distance_to_vehicle = _my_loc.get_distance(vehicle_loc); const float my_loc_distance_to_vehicle = _my_loc.get_distance(vehicle_loc);
@ -453,7 +460,7 @@ void AP_ADSB::handle_adsb_vehicle(const adsb_vehicle_t &vehicle)
// found, update it // found, update it
set_vehicle(index, vehicle); set_vehicle(index, vehicle);
} else if (in_state.vehicle_count < in_state.list_size) { } else if (in_state.vehicle_count < in_state.list_size_allocated) {
// 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(in_state.vehicle_count, vehicle); set_vehicle(in_state.vehicle_count, vehicle);
@ -464,23 +471,23 @@ void AP_ADSB::handle_adsb_vehicle(const adsb_vehicle_t &vehicle)
if (my_loc_is_zero) { if (my_loc_is_zero) {
// nothing else to do // nothing else to do
furthest_vehicle_distance = 0; in_state.furthest_vehicle_distance = 0;
furthest_vehicle_index = 0; in_state.furthest_vehicle_index = 0;
} else { } else {
if (furthest_vehicle_distance <= 0) { if (in_state.furthest_vehicle_distance <= 0) {
// ensure this is populated // ensure this is populated
determine_furthest_aircraft(); determine_furthest_aircraft();
} }
if (my_loc_distance_to_vehicle < furthest_vehicle_distance) { // is closer than the furthest if (my_loc_distance_to_vehicle < in_state.furthest_vehicle_distance) { // is closer than the furthest
// replace with the furthest vehicle // replace with the furthest vehicle
set_vehicle(furthest_vehicle_index, vehicle); set_vehicle(in_state.furthest_vehicle_index, vehicle);
// furthest_vehicle_index is now invalid because the vehicle was overwritten, need // in_state.furthest_vehicle_index is now invalid because the vehicle was overwritten, need
// to run determine_furthest_aircraft() to determine a new one next time // to run determine_furthest_aircraft() to determine a new one next time
furthest_vehicle_distance = 0; in_state.furthest_vehicle_distance = 0;
furthest_vehicle_index = 0; in_state.furthest_vehicle_index = 0;
} }
} }
} // if buffer full } // if buffer full
@ -496,25 +503,12 @@ void AP_ADSB::handle_adsb_vehicle(const adsb_vehicle_t &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::handle_vehicle(const mavlink_message_t &packet)
{
adsb_vehicle_t vehicle {};
const uint32_t now = AP_HAL::millis();
mavlink_msg_adsb_vehicle_decode(&packet, &vehicle.info);
vehicle.last_update_ms = now - (vehicle.info.tslc * 1000);
handle_adsb_vehicle(vehicle);
}
/* /*
* Copy a vehicle's data into the list * Copy a vehicle's data into the list
*/ */
void AP_ADSB::set_vehicle(const uint16_t index, const adsb_vehicle_t &vehicle) void AP_ADSB::set_vehicle(const uint16_t index, const adsb_vehicle_t &vehicle)
{ {
if (index >= in_state.list_size) { if (index >= in_state.list_size_allocated) {
// out of range // out of range
return; return;
} }
@ -525,7 +519,7 @@ void AP_ADSB::set_vehicle(const uint16_t index, const adsb_vehicle_t &vehicle)
void AP_ADSB::send_adsb_vehicle(const mavlink_channel_t chan) void AP_ADSB::send_adsb_vehicle(const mavlink_channel_t chan)
{ {
if (in_state.vehicle_list == nullptr || in_state.vehicle_count == 0) { if (!check_startup() || in_state.vehicle_count == 0) {
return; return;
} }
@ -565,7 +559,7 @@ void AP_ADSB::send_adsb_vehicle(const mavlink_channel_t chan)
} }
void AP_ADSB::send_dynamic_out(const mavlink_channel_t chan) void AP_ADSB::send_dynamic_out(const mavlink_channel_t chan) const
{ {
const AP_GPS &gps = AP::gps(); const AP_GPS &gps = AP::gps();
const Vector3f &gps_velocity = gps.velocity(); const Vector3f &gps_velocity = gps.velocity();
@ -600,7 +594,7 @@ void AP_ADSB::send_dynamic_out(const mavlink_channel_t chan)
} }
uint16_t state = 0; uint16_t state = 0;
if (out_state._is_in_auto_mode) { if (out_state.is_in_auto_mode) {
state |= UAVIONIX_ADSB_OUT_DYNAMIC_STATE_AUTOPILOT_ENABLED; state |= UAVIONIX_ADSB_OUT_DYNAMIC_STATE_AUTOPILOT_ENABLED;
} }
if (!out_state.is_flying) { if (!out_state.is_flying) {
@ -646,13 +640,13 @@ void AP_ADSB::send_dynamic_out(const mavlink_channel_t chan)
* This function will override the MSB byte of the 24bit ICAO address. To ensure an invalid >24bit ICAO is never broadcasted, * This function will override the MSB byte of the 24bit ICAO address. To ensure an invalid >24bit ICAO is never broadcasted,
* this function is used to create the encoded verison without ever writing to the actual ICAO number. It's created on-demand * this function is used to create the encoded verison without ever writing to the actual ICAO number. It's created on-demand
*/ */
uint32_t AP_ADSB::get_encoded_icao(void) uint32_t AP_ADSB::encode_icao(const uint32_t icao_id) const
{ {
// utilize the upper unused 8bits of the icao with special flags. // utilize the upper unused 8bits of the icao with special flags.
// This encoding is required for uAvionix devices that break the MAVLink spec. // This encoding is required for uAvionix devices that break the MAVLink spec.
// ensure the user assignable icao is 24 bits // ensure the user assignable icao is 24 bits
uint32_t encoded_icao = (uint32_t)out_state.cfg.ICAO_id & 0x00FFFFFF; uint32_t encoded_icao = icao_id & 0x00FFFFFF;
encoded_icao &= ~0x20000000; // useGnssAltitude should always be FALSE encoded_icao &= ~0x20000000; // useGnssAltitude should always be FALSE
encoded_icao |= 0x10000000; // csidLogic should always be TRUE encoded_icao |= 0x10000000; // csidLogic should always be TRUE
@ -729,11 +723,8 @@ uint8_t AP_ADSB::get_encoded_callsign_null_char()
* This allows a GCS to send cfg info through the autopilot to the ADSB hardware. * This allows a GCS to send cfg info through the autopilot to the ADSB hardware.
* This is done indirectly by reading and storing the packet and then another mechanism sends it out periodically * This is done indirectly by reading and storing the packet and then another mechanism sends it out periodically
*/ */
void AP_ADSB::handle_out_cfg(const mavlink_message_t &msg) void AP_ADSB::handle_out_cfg(const mavlink_uavionix_adsb_out_cfg_t &packet)
{ {
mavlink_uavionix_adsb_out_cfg_t packet {};
mavlink_msg_uavionix_adsb_out_cfg_decode(&msg, &packet);
out_state.cfg.was_set_externally = true; out_state.cfg.was_set_externally = true;
out_state.cfg.ICAO_id = packet.ICAO; out_state.cfg.ICAO_id = packet.ICAO;
@ -744,8 +735,8 @@ void AP_ADSB::handle_out_cfg(const mavlink_message_t &msg)
out_state.cfg.emitterType = packet.emitterType; out_state.cfg.emitterType = packet.emitterType;
out_state.cfg.lengthWidth = packet.aircraftSize; out_state.cfg.lengthWidth = packet.aircraftSize;
out_state.cfg.gpsLatOffset = packet.gpsOffsetLat; out_state.cfg.gpsOffsetLat = packet.gpsOffsetLat;
out_state.cfg.gpsLonOffset = packet.gpsOffsetLon; out_state.cfg.gpsOffsetLon = packet.gpsOffsetLon;
out_state.cfg.rfSelect = packet.rfSelect; out_state.cfg.rfSelect = packet.rfSelect;
out_state.cfg.stall_speed_cm = packet.stallSpeed; out_state.cfg.stall_speed_cm = packet.stallSpeed;
@ -777,7 +768,7 @@ void AP_ADSB::send_configure(const mavlink_channel_t chan)
icao = out_state.cfg.ICAO_id; icao = out_state.cfg.ICAO_id;
} else { } else {
callsign[MAVLINK_MSG_UAVIONIX_ADSB_OUT_CFG_FIELD_CALLSIGN_LEN-1] = (int8_t)get_encoded_callsign_null_char(); callsign[MAVLINK_MSG_UAVIONIX_ADSB_OUT_CFG_FIELD_CALLSIGN_LEN-1] = (int8_t)get_encoded_callsign_null_char();
icao = get_encoded_icao(); icao = encode_icao((uint32_t)out_state.cfg.ICAO_id);
} }
mavlink_msg_uavionix_adsb_out_cfg_send( mavlink_msg_uavionix_adsb_out_cfg_send(
@ -786,8 +777,8 @@ void AP_ADSB::send_configure(const mavlink_channel_t chan)
(const char*)callsign, (const char*)callsign,
(uint8_t)out_state.cfg.emitterType, (uint8_t)out_state.cfg.emitterType,
(uint8_t)out_state.cfg.lengthWidth, (uint8_t)out_state.cfg.lengthWidth,
(uint8_t)out_state.cfg.gpsLatOffset, (uint8_t)out_state.cfg.gpsOffsetLat,
(uint8_t)out_state.cfg.gpsLonOffset, (uint8_t)out_state.cfg.gpsOffsetLon,
out_state.cfg.stall_speed_cm, out_state.cfg.stall_speed_cm,
(uint8_t)out_state.cfg.rfSelect); (uint8_t)out_state.cfg.rfSelect);
} }
@ -796,12 +787,8 @@ void AP_ADSB::send_configure(const mavlink_channel_t chan)
* this is a message from the transceiver reporting it's health. Using this packet * this is a message from the transceiver reporting it's health. Using this packet
* we determine which channel is on so we don't have to send out_state to all channels * we determine which channel is on so we don't have to send out_state to all channels
*/ */
void AP_ADSB::handle_transceiver_report(const mavlink_channel_t chan, const mavlink_uavionix_adsb_transceiver_health_report_t &packet)
void AP_ADSB::handle_transceiver_report(const mavlink_channel_t chan, const mavlink_message_t &msg)
{ {
mavlink_uavionix_adsb_transceiver_health_report_t packet {};
mavlink_msg_uavionix_adsb_transceiver_health_report_decode(&msg, &packet);
if (out_state.chan != chan) { if (out_state.chan != chan) {
gcs().send_text(MAV_SEVERITY_DEBUG, "ADSB: Found transceiver on channel %d", chan); gcs().send_text(MAV_SEVERITY_DEBUG, "ADSB: Found transceiver on channel %d", chan);
} }
@ -814,12 +801,10 @@ void AP_ADSB::handle_transceiver_report(const mavlink_channel_t chan, const mavl
/* /*
@brief Generates pseudorandom ICAO from gps time, lat, and lon. @brief Generates pseudorandom ICAO from gps time, lat, and lon.
Reference: DO282B, 2.2.4.5.1.3.2 Reference: DO282B, 2.2.4.5.1.3.2
Note gps.time is the number of seconds since UTC midnight
*/ */
uint32_t AP_ADSB::genICAO(const Location &loc) uint32_t AP_ADSB::genICAO(const Location &loc) const
{ {
// gps_time is not seconds since UTC midnight, but it is an equivalent random number // gps_time is used as a pseudo-random number instead of seconds since UTC midnight
// TODO: use UTC time instead of GPS time // TODO: use UTC time instead of GPS time
const AP_GPS &gps = AP::gps(); const AP_GPS &gps = AP::gps();
const uint64_t gps_time = gps.time_epoch_usec(); const uint64_t gps_time = gps.time_epoch_usec();
@ -882,31 +867,41 @@ void AP_ADSB::set_callsign(const char* str, const bool append_icao)
void AP_ADSB::push_sample(const adsb_vehicle_t &vehicle) void AP_ADSB::push_sample(const adsb_vehicle_t &vehicle)
{ {
samples.push(vehicle); _samples.push(vehicle);
} }
bool AP_ADSB::next_sample(adsb_vehicle_t &vehicle) bool AP_ADSB::next_sample(adsb_vehicle_t &vehicle)
{ {
return samples.pop(vehicle); return _samples.pop(vehicle);
} }
void AP_ADSB::handle_message(const mavlink_channel_t chan, const mavlink_message_t &msg) void AP_ADSB::handle_message(const mavlink_channel_t chan, const mavlink_message_t &msg)
{ {
switch (msg.msgid) { switch (msg.msgid) {
case MAVLINK_MSG_ID_ADSB_VEHICLE: case MAVLINK_MSG_ID_ADSB_VEHICLE: {
handle_vehicle(msg); adsb_vehicle_t vehicle {};
break; mavlink_msg_adsb_vehicle_decode(&msg, &vehicle.info);
case MAVLINK_MSG_ID_UAVIONIX_ADSB_TRANSCEIVER_HEALTH_REPORT: vehicle.last_update_ms = AP_HAL::millis() - uint32_t(vehicle.info.tslc * 1000U);
handle_transceiver_report(chan, msg); handle_adsb_vehicle(vehicle);
break; break;
}
case MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_CFG: case MAVLINK_MSG_ID_UAVIONIX_ADSB_TRANSCEIVER_HEALTH_REPORT: {
handle_out_cfg(msg); mavlink_uavionix_adsb_transceiver_health_report_t packet {};
mavlink_msg_uavionix_adsb_transceiver_health_report_decode(&msg, &packet);
handle_transceiver_report(chan, packet);
break; break;
}
case MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_CFG: {
mavlink_uavionix_adsb_out_cfg_t packet {};
mavlink_msg_uavionix_adsb_out_cfg_decode(&msg, &packet);
handle_out_cfg(packet);
break;
}
case MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_DYNAMIC: case MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_DYNAMIC:
// unhandled, this is an outbound packet only // unhandled, this is an outbound packet only
default:
break; break;
} }
@ -932,7 +927,7 @@ bool AP_ADSB::get_vehicle_by_ICAO(const uint32_t icao, adsb_vehicle_t &vehicle)
/* /*
* Write vehicle to log * Write vehicle to log
*/ */
void AP_ADSB::write_log(const adsb_vehicle_t &vehicle) void AP_ADSB::write_log(const adsb_vehicle_t &vehicle) const
{ {
switch (_log) { switch (_log) {
case logging::SPECIAL_ONLY: case logging::SPECIAL_ONLY:
@ -969,4 +964,3 @@ AP_ADSB *AP::ADSB()
return AP_ADSB::get_singleton(); return AP_ADSB::get_singleton();
} }
#endif // HAL_ADSB_ENABLED #endif // HAL_ADSB_ENABLED

104
libraries/AP_ADSB/AP_ADSB.h
View File

@ -23,16 +23,16 @@
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include <AP_HAL/AP_HAL_Boards.h> #include <AP_HAL/AP_HAL_Boards.h>
#include <AP_Common/AP_Common.h>
#include <AP_Param/AP_Param.h>
#include <AP_Common/Location.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
#ifndef HAL_ADSB_ENABLED #ifndef HAL_ADSB_ENABLED
#define HAL_ADSB_ENABLED !HAL_MINIMIZE_FEATURES && BOARD_FLASH_SIZE > 1024 #define HAL_ADSB_ENABLED !HAL_MINIMIZE_FEATURES && BOARD_FLASH_SIZE > 1024
#endif #endif
#if HAL_ADSB_ENABLED #if HAL_ADSB_ENABLED
#include <AP_Common/AP_Common.h>
#include <AP_Param/AP_Param.h>
#include <AP_Common/Location.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
class AP_ADSB { class AP_ADSB {
public: public:
@ -43,6 +43,11 @@ public:
AP_ADSB(const AP_ADSB &other) = delete; AP_ADSB(const AP_ADSB &other) = delete;
AP_ADSB &operator=(const AP_ADSB&) = delete; AP_ADSB &operator=(const AP_ADSB&) = delete;
// get singleton instance
static AP_ADSB *get_singleton(void) {
return _singleton;
}
struct adsb_vehicle_t { struct adsb_vehicle_t {
mavlink_adsb_vehicle_t info; // the whole mavlink struct with all the juicy details. sizeof() == 38 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 uint32_t last_update_ms; // last time this was refreshed, allows timeouts
@ -54,20 +59,27 @@ public:
// periodic task that maintains vehicle_list // periodic task that maintains vehicle_list
void update(void); void update(void);
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);
void set_stall_speed_cm(const uint16_t stall_speed_cm) { out_state.cfg.stall_speed_cm = stall_speed_cm; } bool set_stall_speed_cm(const uint16_t stall_speed_cm) {
void set_max_speed(int16_t max_speed) { if (out_state.cfg.was_set_externally) {
if (!out_state.cfg.was_set_externally) { return false;
// convert m/s to knots
out_state.cfg.maxAircraftSpeed_knots = (float)max_speed * 1.94384f;
} }
out_state.cfg.stall_speed_cm = stall_speed_cm;
return true;
} }
void set_is_auto_mode(const bool is_in_auto_mode) { out_state._is_in_auto_mode = is_in_auto_mode; } bool set_max_speed(int16_t max_speed) {
if (out_state.cfg.was_set_externally) {
return false;
}
// convert m/s to knots
out_state.cfg.maxAircraftSpeed_knots = (float)max_speed * M_PER_SEC_TO_KNOTS;
return true;
}
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; } 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; } UAVIONIX_ADSB_RF_HEALTH get_transceiver_status(void) { return out_state.status; }
@ -78,9 +90,18 @@ public:
bool enabled() const { bool enabled() const {
return _enabled; return _enabled;
} }
bool init_failed() const {
return _init_failed;
}
bool healthy() {
return check_startup();
}
bool next_sample(adsb_vehicle_t &obstacle); bool next_sample(adsb_vehicle_t &obstacle);
// handle a adsb_vehicle_t from an external source (used for UAVCAN) // handle a adsb_vehicle_t from an external source
void handle_adsb_vehicle(const adsb_vehicle_t &vehicle); void handle_adsb_vehicle(const adsb_vehicle_t &vehicle);
// mavlink message handler // mavlink message handler
@ -96,20 +117,16 @@ public:
// confirm a value is a valid callsign // confirm a value is a valid callsign
static bool is_valid_callsign(uint16_t octal) WARN_IF_UNUSED; static bool is_valid_callsign(uint16_t octal) WARN_IF_UNUSED;
// get singleton instance
static AP_ADSB *get_singleton(void) {
return _singleton;
}
private: private:
static AP_ADSB *_singleton; static AP_ADSB *_singleton;
// initialize _vehicle_list // initialize vehicle_list
void init(); void init();
// free _vehicle_list
void deinit(); void deinit();
// check to see if we are initialized (and possibly do initialization)
bool check_startup();
// compares current vector against vehicle_list to detect threats // compares current vector against vehicle_list to detect threats
void determine_furthest_aircraft(void); void determine_furthest_aircraft(void);
@ -122,48 +139,50 @@ private:
void set_vehicle(const uint16_t index, const adsb_vehicle_t &vehicle); void set_vehicle(const uint16_t index, const adsb_vehicle_t &vehicle);
// Generates pseudorandom ICAO from gps time, lat, and lon // Generates pseudorandom ICAO from gps time, lat, and lon
uint32_t genICAO(const Location &loc); uint32_t genICAO(const Location &loc) const;
// set callsign: 8char string (plus null termination) then optionally append last 4 digits of icao // set callsign: 8char string (plus null termination) then optionally append last 4 digits of icao
void set_callsign(const char* str, const bool append_icao); void set_callsign(const char* str, const bool append_icao);
// send static and dynamic data to ADSB transceiver // send static and dynamic data to ADSB transceiver
void send_configure(const mavlink_channel_t chan); void send_configure(const mavlink_channel_t chan);
void send_dynamic_out(const mavlink_channel_t chan); void send_dynamic_out(const mavlink_channel_t chan) const;
// special helpers for uAvionix workarounds // special helpers for uAvionix workarounds
uint32_t get_encoded_icao(void); uint32_t encode_icao(const uint32_t icao_id) const;
uint8_t get_encoded_callsign_null_char(void); uint8_t get_encoded_callsign_null_char(void);
// add or update vehicle_list from inbound mavlink msg // configure ADSB-out transceivers
void handle_vehicle(const mavlink_message_t &msg); void handle_out_cfg(const mavlink_uavionix_adsb_out_cfg_t &packet);
// handle ADS-B transceiver report for ping2020 // mavlink handler
void handle_transceiver_report(mavlink_channel_t chan, const mavlink_message_t &msg); void handle_transceiver_report(const mavlink_channel_t chan, const mavlink_uavionix_adsb_transceiver_health_report_t &packet);
void handle_out_cfg(const mavlink_message_t &msg);
AP_Int8 _enabled; AP_Int8 _enabled;
Location _my_loc; Location _my_loc;
bool _init_failed;
// ADSB-IN state. Maintains list of external vehicles // ADSB-IN state. Maintains list of external vehicles
struct { struct {
// list management // list management
AP_Int16 list_size_param; 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 uint16_t list_size_allocated;
adsb_vehicle_t *vehicle_list = nullptr; adsb_vehicle_t *vehicle_list;
uint16_t vehicle_count; uint16_t vehicle_count;
AP_Int32 list_radius; AP_Int32 list_radius;
AP_Int16 list_altitude; AP_Int16 list_altitude;
// index of and distance to furthest vehicle in list
uint16_t furthest_vehicle_index;
float furthest_vehicle_distance;
// 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; } in_state;
// ADSB-OUT state. Maintains export data // ADSB-OUT state. Maintains export data
struct { struct {
uint32_t last_config_ms; // send once every 10s uint32_t last_config_ms; // send once every 10s
@ -172,7 +191,7 @@ private:
uint32_t chan_last_ms; uint32_t chan_last_ms;
UAVIONIX_ADSB_RF_HEALTH status; // transceiver status UAVIONIX_ADSB_RF_HEALTH status; // transceiver status
bool is_flying; bool is_flying;
bool _is_in_auto_mode; bool is_in_auto_mode;
// ADSB-OUT configuration // ADSB-OUT configuration
struct { struct {
@ -182,8 +201,8 @@ private:
char callsign[MAVLINK_MSG_UAVIONIX_ADSB_OUT_CFG_FIELD_CALLSIGN_LEN]; //Vehicle identifier (8 characters, null terminated, valid characters are A-Z, 0-9, " " only). char callsign[MAVLINK_MSG_UAVIONIX_ADSB_OUT_CFG_FIELD_CALLSIGN_LEN]; //Vehicle identifier (8 characters, null terminated, valid characters are A-Z, 0-9, " " only).
AP_Int8 emitterType; AP_Int8 emitterType;
AP_Int8 lengthWidth; // Aircraft length and width encoding (table 2-35 of DO-282B) AP_Int8 lengthWidth; // Aircraft length and width encoding (table 2-35 of DO-282B)
AP_Int8 gpsLatOffset; AP_Int8 gpsOffsetLat;
AP_Int8 gpsLonOffset; AP_Int8 gpsOffsetLon;
uint16_t stall_speed_cm; uint16_t stall_speed_cm;
AP_Int8 rfSelect; AP_Int8 rfSelect;
AP_Int16 squawk_octal_param; AP_Int16 squawk_octal_param;
@ -196,22 +215,17 @@ private:
} out_state; } out_state;
// index of and distance to furthest vehicle in list
uint16_t furthest_vehicle_index;
float furthest_vehicle_distance;
// special ICAO of interest that ignored filters when != 0 // special ICAO of interest that ignored filters when != 0
AP_Int32 _special_ICAO_target; AP_Int32 _special_ICAO_target;
static const uint8_t max_samples = 30; static const uint8_t _max_samples = 30;
ObjectBuffer<adsb_vehicle_t> samples{max_samples}; ObjectBuffer<adsb_vehicle_t> _samples{_max_samples};
void push_sample(const adsb_vehicle_t &vehicle); void push_sample(const adsb_vehicle_t &vehicle);
// logging // logging
AP_Int8 _log; AP_Int8 _log;
void write_log(const adsb_vehicle_t &vehicle); void write_log(const adsb_vehicle_t &vehicle) const;
enum logging { enum logging {
NONE = 0, NONE = 0,
SPECIAL_ONLY = 1, SPECIAL_ONLY = 1,
@ -219,6 +233,8 @@ private:
}; };
void update_uavionix();
}; };
namespace AP { namespace AP {