mirror of https://github.com/ArduPilot/ardupilot
AP_ADSB: refactor and renamed things and general cleanup for pre-backend split
This commit is contained in:
Tom Pittenger
Peter Barker
parent
8bcc32db78
commit
7a09f88e83
|
|
@ -20,8 +20,8 @@
|
|||
https://en.wikipedia.org/wiki/Automatic_dependent_surveillance_%E2%80%93_broadcast
|
||||
*/
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include "AP_ADSB.h"
|
||||
|
||||
#if HAL_ADSB_ENABLED
|
||||
#include <GCS_MAVLink/GCS_MAVLink.h>
|
||||
#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 ADSB_VEHICLE_LIST_SIZE_DEFAULT 25
|
||||
#define ADSB_VEHICLE_LIST_SIZE_MAX 100
|
||||
|
||||
#define ADSB_CHAN_TIMEOUT_MS 15000
|
||||
#define ADSB_SQUAWK_OCTAL_DEFAULT 1200
|
||||
|
||||
#define ADSB_BITBASK_RF_CAPABILITIES_UAT_IN (1 << 0)
|
||||
#define ADSB_BITBASK_RF_CAPABILITIES_1090ES_IN (1 << 1)
|
||||
|
||||
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
|
||||
#define ADSB_LIST_RADIUS_DEFAULT 10000 // in meters
|
||||
#else // APM_BUILD_TYPE(APM_BUILD_ArduCopter), Rover, Boat
|
||||
#define ADSB_LIST_RADIUS_DEFAULT 2000 // in meters
|
||||
#ifndef ADSB_VEHICLE_LIST_SIZE_DEFAULT
|
||||
#define ADSB_VEHICLE_LIST_SIZE_DEFAULT 25
|
||||
#endif
|
||||
|
||||
#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
|
||||
|
||||
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.
|
||||
// @Range: 1 100
|
||||
// @User: Advanced
|
||||
// @RebootRequired: True
|
||||
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.
|
||||
// @Values: 0:NoData,1:Left2m,2:Left4m,3:Left6m,4:Center,5:Right2m,6:Right4m,7:Right6m
|
||||
// @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
|
||||
// @DisplayName: GPS antenna longitudinal offset
|
||||
// @Description: GPS antenna longitudinal offset. This is usually set to 1, Applied By Sensor
|
||||
// @Values: 0:NO_DATA,1:AppliedBySensor
|
||||
// @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
|
||||
// @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.
|
||||
// @Values: 0:Disabled,1:Rx-Only,2:Tx-Only,3:Rx and Tx Enabled
|
||||
// @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.
|
||||
// @Bitmask: 0:Rx,1:Tx
|
||||
// @User: Advanced
|
||||
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_Param::setup_object_defaults(this, var_info);
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||
if (_singleton != nullptr) {
|
||||
AP_HAL::panic("AP_ADSB must be singleton");
|
||||
}
|
||||
#endif
|
||||
_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)
|
||||
{
|
||||
// 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();
|
||||
}
|
||||
in_state.list_size = in_state.list_size_param;
|
||||
in_state.vehicle_list = new adsb_vehicle_t[in_state.list_size];
|
||||
// sanity check param
|
||||
in_state.list_size_param = constrain_int16(in_state.list_size_param, 1, INT16_MAX);
|
||||
|
||||
in_state.vehicle_list = new adsb_vehicle_t[in_state.list_size_param];
|
||||
|
||||
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);
|
||||
// dynamic RAM allocation of in_state.vehicle_list[] failed
|
||||
_init_failed = true; // this keeps us from constantly trying to init forever in main update
|
||||
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)
|
||||
{
|
||||
in_state.vehicle_count = 0;
|
||||
if (in_state.vehicle_list != nullptr) {
|
||||
delete [] in_state.vehicle_list;
|
||||
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)
|
||||
{
|
||||
// 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)
|
||||
{
|
||||
// update _my_loc
|
||||
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();
|
||||
if (!check_startup()) {
|
||||
return;
|
||||
}
|
||||
|
||||
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
|
||||
uint16_t index = 0;
|
||||
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) {
|
||||
// param changed, check that it's a valid octal
|
||||
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
|
||||
out_state.last_config_ms = now;
|
||||
|
||||
} else if (out_state.cfg.ICAO_id == 0 ||
|
||||
out_state.cfg.ICAO_id_param_prev != out_state.cfg.ICAO_id_param) {
|
||||
} else if ((out_state.cfg.rfSelect & UAVIONIX_ADSB_OUT_RF_SELECT_TX_ENABLED) &&
|
||||
(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 (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_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);
|
||||
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
|
||||
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
|
||||
// TODO: reset out_state.chan
|
||||
out_state.chan = -1;
|
||||
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);
|
||||
if (now - out_state.last_config_ms >= 5000 && HAVE_PAYLOAD_SPACE(chan, UAVIONIX_ADSB_OUT_CFG)) {
|
||||
out_state.last_config_ms = now;
|
||||
|
|
@ -352,8 +360,8 @@ void AP_ADSB::determine_furthest_aircraft(void)
|
|||
}
|
||||
} // for index
|
||||
|
||||
furthest_vehicle_index = max_distance_index;
|
||||
furthest_vehicle_distance = max_distance;
|
||||
in_state.furthest_vehicle_index = max_distance_index;
|
||||
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 (index == furthest_vehicle_index && furthest_vehicle_distance > 0) {
|
||||
furthest_vehicle_distance = 0;
|
||||
furthest_vehicle_index = 0;
|
||||
if (index == in_state.furthest_vehicle_index && in_state.furthest_vehicle_distance > 0) {
|
||||
in_state.furthest_vehicle_distance = 0;
|
||||
in_state.furthest_vehicle_index = 0;
|
||||
}
|
||||
if (index != (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)
|
||||
{
|
||||
if (in_state.vehicle_list == nullptr) {
|
||||
// We are only null when disabled. Updating is inhibited.
|
||||
if (!check_startup()) {
|
||||
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 bool my_loc_is_zero = _my_loc.is_zero();
|
||||
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
|
||||
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
|
||||
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) {
|
||||
// nothing else to do
|
||||
furthest_vehicle_distance = 0;
|
||||
furthest_vehicle_index = 0;
|
||||
in_state.furthest_vehicle_distance = 0;
|
||||
in_state.furthest_vehicle_index = 0;
|
||||
|
||||
} else {
|
||||
if (furthest_vehicle_distance <= 0) {
|
||||
if (in_state.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
|
||||
if (my_loc_distance_to_vehicle < in_state.furthest_vehicle_distance) { // is closer than the furthest
|
||||
// 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
|
||||
furthest_vehicle_distance = 0;
|
||||
furthest_vehicle_index = 0;
|
||||
in_state.furthest_vehicle_distance = 0;
|
||||
in_state.furthest_vehicle_index = 0;
|
||||
}
|
||||
}
|
||||
} // 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
|
||||
*/
|
||||
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
|
||||
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)
|
||||
{
|
||||
if (in_state.vehicle_list == nullptr || in_state.vehicle_count == 0) {
|
||||
if (!check_startup() || in_state.vehicle_count == 0) {
|
||||
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 Vector3f &gps_velocity = gps.velocity();
|
||||
|
|
@ -600,7 +594,7 @@ void AP_ADSB::send_dynamic_out(const mavlink_channel_t chan)
|
|||
}
|
||||
|
||||
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;
|
||||
}
|
||||
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 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.
|
||||
// This encoding is required for uAvionix devices that break the MAVLink spec.
|
||||
|
||||
// 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 |= 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 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.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.lengthWidth = packet.aircraftSize;
|
||||
out_state.cfg.gpsLatOffset = packet.gpsOffsetLat;
|
||||
out_state.cfg.gpsLonOffset = packet.gpsOffsetLon;
|
||||
out_state.cfg.gpsOffsetLat = packet.gpsOffsetLat;
|
||||
out_state.cfg.gpsOffsetLon = packet.gpsOffsetLon;
|
||||
out_state.cfg.rfSelect = packet.rfSelect;
|
||||
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;
|
||||
} else {
|
||||
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(
|
||||
|
|
@ -786,8 +777,8 @@ void AP_ADSB::send_configure(const mavlink_channel_t chan)
|
|||
(const char*)callsign,
|
||||
(uint8_t)out_state.cfg.emitterType,
|
||||
(uint8_t)out_state.cfg.lengthWidth,
|
||||
(uint8_t)out_state.cfg.gpsLatOffset,
|
||||
(uint8_t)out_state.cfg.gpsLonOffset,
|
||||
(uint8_t)out_state.cfg.gpsOffsetLat,
|
||||
(uint8_t)out_state.cfg.gpsOffsetLon,
|
||||
out_state.cfg.stall_speed_cm,
|
||||
(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
|
||||
* 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_message_t &msg)
|
||||
void AP_ADSB::handle_transceiver_report(const mavlink_channel_t chan, const mavlink_uavionix_adsb_transceiver_health_report_t &packet)
|
||||
{
|
||||
mavlink_uavionix_adsb_transceiver_health_report_t packet {};
|
||||
mavlink_msg_uavionix_adsb_transceiver_health_report_decode(&msg, &packet);
|
||||
|
||||
if (out_state.chan != 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.
|
||||
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
|
||||
const AP_GPS &gps = AP::gps();
|
||||
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)
|
||||
{
|
||||
samples.push(vehicle);
|
||||
_samples.push(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)
|
||||
{
|
||||
switch (msg.msgid) {
|
||||
case MAVLINK_MSG_ID_ADSB_VEHICLE:
|
||||
handle_vehicle(msg);
|
||||
break;
|
||||
case MAVLINK_MSG_ID_UAVIONIX_ADSB_TRANSCEIVER_HEALTH_REPORT:
|
||||
handle_transceiver_report(chan, msg);
|
||||
case MAVLINK_MSG_ID_ADSB_VEHICLE: {
|
||||
adsb_vehicle_t vehicle {};
|
||||
mavlink_msg_adsb_vehicle_decode(&msg, &vehicle.info);
|
||||
vehicle.last_update_ms = AP_HAL::millis() - uint32_t(vehicle.info.tslc * 1000U);
|
||||
handle_adsb_vehicle(vehicle);
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_CFG:
|
||||
handle_out_cfg(msg);
|
||||
case MAVLINK_MSG_ID_UAVIONIX_ADSB_TRANSCEIVER_HEALTH_REPORT: {
|
||||
mavlink_uavionix_adsb_transceiver_health_report_t packet {};
|
||||
mavlink_msg_uavionix_adsb_transceiver_health_report_decode(&msg, &packet);
|
||||
handle_transceiver_report(chan, packet);
|
||||
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:
|
||||
// unhandled, this is an outbound packet only
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
|
|
@ -932,7 +927,7 @@ bool AP_ADSB::get_vehicle_by_ICAO(const uint32_t icao, adsb_vehicle_t &vehicle)
|
|||
/*
|
||||
* 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) {
|
||||
case logging::SPECIAL_ONLY:
|
||||
|
|
@ -969,4 +964,3 @@ AP_ADSB *AP::ADSB()
|
|||
return AP_ADSB::get_singleton();
|
||||
}
|
||||
#endif // HAL_ADSB_ENABLED
|
||||
|
||||
|
|
|
|||
|
|
@ -23,16 +23,16 @@
|
|||
|
||||
#include <AP_HAL/AP_HAL.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
|
||||
#define HAL_ADSB_ENABLED !HAL_MINIMIZE_FEATURES && BOARD_FLASH_SIZE > 1024
|
||||
#endif
|
||||
|
||||
#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 {
|
||||
public:
|
||||
|
|
@ -43,6 +43,11 @@ public:
|
|||
AP_ADSB(const AP_ADSB &other) = delete;
|
||||
AP_ADSB &operator=(const AP_ADSB&) = delete;
|
||||
|
||||
// get singleton instance
|
||||
static AP_ADSB *get_singleton(void) {
|
||||
return _singleton;
|
||||
}
|
||||
|
||||
struct adsb_vehicle_t {
|
||||
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
|
||||
|
|
@ -54,20 +59,27 @@ public:
|
|||
// periodic task that maintains vehicle_list
|
||||
void update(void);
|
||||
|
||||
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);
|
||||
|
||||
void set_stall_speed_cm(const uint16_t stall_speed_cm) { out_state.cfg.stall_speed_cm = stall_speed_cm; }
|
||||
void set_max_speed(int16_t max_speed) {
|
||||
if (!out_state.cfg.was_set_externally) {
|
||||
// convert m/s to knots
|
||||
out_state.cfg.maxAircraftSpeed_knots = (float)max_speed * 1.94384f;
|
||||
bool set_stall_speed_cm(const uint16_t stall_speed_cm) {
|
||||
if (out_state.cfg.was_set_externally) {
|
||||
return false;
|
||||
}
|
||||
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; }
|
||||
|
||||
UAVIONIX_ADSB_RF_HEALTH get_transceiver_status(void) { return out_state.status; }
|
||||
|
|
@ -78,9 +90,18 @@ public:
|
|||
bool enabled() const {
|
||||
return _enabled;
|
||||
}
|
||||
|
||||
bool init_failed() const {
|
||||
return _init_failed;
|
||||
}
|
||||
|
||||
bool healthy() {
|
||||
return check_startup();
|
||||
}
|
||||
|
||||
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);
|
||||
|
||||
// mavlink message handler
|
||||
|
|
@ -96,20 +117,16 @@ public:
|
|||
// confirm a value is a valid callsign
|
||||
static bool is_valid_callsign(uint16_t octal) WARN_IF_UNUSED;
|
||||
|
||||
// get singleton instance
|
||||
static AP_ADSB *get_singleton(void) {
|
||||
return _singleton;
|
||||
}
|
||||
|
||||
private:
|
||||
static AP_ADSB *_singleton;
|
||||
|
||||
// initialize _vehicle_list
|
||||
// initialize vehicle_list
|
||||
void init();
|
||||
|
||||
// free _vehicle_list
|
||||
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
|
||||
void determine_furthest_aircraft(void);
|
||||
|
||||
|
|
@ -122,48 +139,50 @@ private:
|
|||
void set_vehicle(const uint16_t index, const adsb_vehicle_t &vehicle);
|
||||
|
||||
// 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
|
||||
void set_callsign(const char* str, const bool append_icao);
|
||||
|
||||
// send static and dynamic data to ADSB transceiver
|
||||
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
|
||||
uint32_t get_encoded_icao(void);
|
||||
uint32_t encode_icao(const uint32_t icao_id) const;
|
||||
uint8_t get_encoded_callsign_null_char(void);
|
||||
|
||||
// add or update vehicle_list from inbound mavlink msg
|
||||
void handle_vehicle(const mavlink_message_t &msg);
|
||||
// configure ADSB-out transceivers
|
||||
void handle_out_cfg(const mavlink_uavionix_adsb_out_cfg_t &packet);
|
||||
|
||||
// handle ADS-B transceiver report for ping2020
|
||||
void handle_transceiver_report(mavlink_channel_t chan, const mavlink_message_t &msg);
|
||||
|
||||
void handle_out_cfg(const mavlink_message_t &msg);
|
||||
// mavlink handler
|
||||
void handle_transceiver_report(const mavlink_channel_t chan, const mavlink_uavionix_adsb_transceiver_health_report_t &packet);
|
||||
|
||||
AP_Int8 _enabled;
|
||||
|
||||
Location _my_loc;
|
||||
|
||||
bool _init_failed;
|
||||
|
||||
// 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 list_size_allocated;
|
||||
adsb_vehicle_t *vehicle_list;
|
||||
uint16_t vehicle_count;
|
||||
AP_Int32 list_radius;
|
||||
AP_Int16 list_altitude;
|
||||
|
||||
// index of and distance to furthest vehicle in list
|
||||
uint16_t furthest_vehicle_index;
|
||||
float furthest_vehicle_distance;
|
||||
|
||||
// streamrate stuff
|
||||
uint32_t send_start_ms[MAVLINK_COMM_NUM_BUFFERS];
|
||||
uint16_t send_index[MAVLINK_COMM_NUM_BUFFERS];
|
||||
} in_state;
|
||||
|
||||
|
||||
// ADSB-OUT state. Maintains export data
|
||||
struct {
|
||||
uint32_t last_config_ms; // send once every 10s
|
||||
|
|
@ -172,7 +191,7 @@ private:
|
|||
uint32_t chan_last_ms;
|
||||
UAVIONIX_ADSB_RF_HEALTH status; // transceiver status
|
||||
bool is_flying;
|
||||
bool _is_in_auto_mode;
|
||||
bool is_in_auto_mode;
|
||||
|
||||
// ADSB-OUT configuration
|
||||
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).
|
||||
AP_Int8 emitterType;
|
||||
AP_Int8 lengthWidth; // Aircraft length and width encoding (table 2-35 of DO-282B)
|
||||
AP_Int8 gpsLatOffset;
|
||||
AP_Int8 gpsLonOffset;
|
||||
AP_Int8 gpsOffsetLat;
|
||||
AP_Int8 gpsOffsetLon;
|
||||
uint16_t stall_speed_cm;
|
||||
AP_Int8 rfSelect;
|
||||
AP_Int16 squawk_octal_param;
|
||||
|
|
@ -196,22 +215,17 @@ private:
|
|||
} 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
|
||||
AP_Int32 _special_ICAO_target;
|
||||
|
||||
static const uint8_t max_samples = 30;
|
||||
ObjectBuffer<adsb_vehicle_t> samples{max_samples};
|
||||
static const uint8_t _max_samples = 30;
|
||||
ObjectBuffer<adsb_vehicle_t> _samples{_max_samples};
|
||||
|
||||
void push_sample(const adsb_vehicle_t &vehicle);
|
||||
|
||||
// logging
|
||||
AP_Int8 _log;
|
||||
void write_log(const adsb_vehicle_t &vehicle);
|
||||
void write_log(const adsb_vehicle_t &vehicle) const;
|
||||
enum logging {
|
||||
NONE = 0,
|
||||
SPECIAL_ONLY = 1,
|
||||
|
|
@ -219,6 +233,8 @@ private:
|
|||
};
|
||||
|
||||
|
||||
void update_uavionix();
|
||||
|
||||
};
|
||||
|
||||
namespace AP {
|
||||
|
|
|
|||
Loading…
Reference in New Issue