zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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

c415-sdk
Tom Pittenger 4 years ago committed by Peter Barker
parent
8bcc32db78
commit
7a09f88e83
2 changed files with 179 additions and 169 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 244
      libraries/AP_ADSB/AP_ADSB.cpp
  2. 104
      libraries/AP_ADSB/AP_ADSB.h

244
libraries/AP_ADSB/AP_ADSB.cpp
Unescape View File

@ -20,8 +20,8 @@ @@ -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 @@ @@ -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[] = { @@ -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[] = { @@ -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[] = { @@ -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() @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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 @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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() @@ -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) @@ -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) @@ -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) @@ -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) @@ -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 @@ -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) @@ -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);
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_TRANSCEIVER_HEALTH_REPORT:
handle_transceiver_report(chan, 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:
handle_out_cfg(msg);
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) @@ -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() @@ -969,4 +964,3 @@ AP_ADSB *AP::ADSB()
return AP_ADSB::get_singleton();
}
#endif // HAL_ADSB_ENABLED

104
libraries/AP_ADSB/AP_ADSB.h
Unescape View File

@ -23,16 +23,16 @@ @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -219,6 +233,8 @@ private:
};
void update_uavionix();
};
namespace AP {

Write Preview
Loading…
Cancel
Save