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

AP_ADSB: fix line ending

Browse Source
This commit is contained in:
Pierre Kancir 2022-05-25 09:55:51 +02:00 committed by Randy Mackay
parent b43dd7ba27
commit 22dae61c80
5 changed files with 1044 additions and 1044 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
libraries/AP_ADSB/AP_ADSB_Backend.cpp
View File

@ -1,30 +1,30 @@
/* /*
This program is free software: you can redistribute it and/or modify This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or the Free Software Foundation, either version 3 of the License, or
(at your option) any later version. (at your option) any later version.
This program is distributed in the hope that it will be useful, This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details. GNU General Public License for more details.
You should have received a copy of the GNU General Public License You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>. along with this program. If not, see <http://www.gnu.org/licenses/>.
*/ */
#include "AP_ADSB_Backend.h" #include "AP_ADSB_Backend.h"
#if HAL_ADSB_ENABLED #if HAL_ADSB_ENABLED
/* /*
base class constructor. base class constructor.
*/ */
AP_ADSB_Backend::AP_ADSB_Backend(AP_ADSB &frontend, uint8_t instance) : AP_ADSB_Backend::AP_ADSB_Backend(AP_ADSB &frontend, uint8_t instance) :
_frontend(frontend), _frontend(frontend),
_instance(instance) _instance(instance)
{ {
} }
#endif // HAL_ADSB_ENABLED #endif // HAL_ADSB_ENABLED

1106
libraries/AP_ADSB/AP_ADSB_Sagetech.cpp
View File

File diff suppressed because it is too large Load Diff

322
libraries/AP_ADSB/AP_ADSB_Sagetech.h
View File

@ -1,161 +1,161 @@
#pragma once #pragma once
/* /*
This program is free software: you can redistribute it and/or modify This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or the Free Software Foundation, either version 3 of the License, or
(at your option) any later version. (at your option) any later version.
This program is distributed in the hope that it will be useful, This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details. GNU General Public License for more details.
You should have received a copy of the GNU General Public License You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>. along with this program. If not, see <http://www.gnu.org/licenses/>.
*/ */
#include "AP_ADSB_Backend.h" #include "AP_ADSB_Backend.h"
#ifndef HAL_ADSB_SAGETECH_ENABLED #ifndef HAL_ADSB_SAGETECH_ENABLED
#define HAL_ADSB_SAGETECH_ENABLED HAL_ADSB_ENABLED #define HAL_ADSB_SAGETECH_ENABLED HAL_ADSB_ENABLED
#endif #endif
#if HAL_ADSB_SAGETECH_ENABLED #if HAL_ADSB_SAGETECH_ENABLED
class AP_ADSB_Sagetech : public AP_ADSB_Backend { class AP_ADSB_Sagetech : public AP_ADSB_Backend {
public: public:
using AP_ADSB_Backend::AP_ADSB_Backend; using AP_ADSB_Backend::AP_ADSB_Backend;
// init - performs any required initialisation for this instance // init - performs any required initialisation for this instance
bool init() override; bool init() override;
// update - should be called periodically // update - should be called periodically
void update() override; void update() override;
// static detection function // static detection function
static bool detect(); static bool detect();
private: private:
static const uint32_t PAYLOAD_XP_MAX_SIZE = 52; static const uint32_t PAYLOAD_XP_MAX_SIZE = 52;
enum class SystemStateBits { enum class SystemStateBits {
Error_Transponder = (1U<<0), Error_Transponder = (1U<<0),
Altitidue_Source = (1U<<1), Altitidue_Source = (1U<<1),
Error_GPS = (1U<<2), Error_GPS = (1U<<2),
Error_ICAO = (1U<<3), Error_ICAO = (1U<<3),
Error_Over_Temperature = (1U<<4), Error_Over_Temperature = (1U<<4),
Error_Extended_Squitter = (1U<<5), Error_Extended_Squitter = (1U<<5),
Mode_Transponder = (3U<<6), // 2 bit status: Mode_Transponder = (3U<<6), // 2 bit status:
}; };
enum class Transponder_Type { enum class Transponder_Type {
Mode_C = 0x00, Mode_C = 0x00,
Mode_S_ADSB_OUT = 0x01, Mode_S_ADSB_OUT = 0x01,
Mode_S_ADSB_OUT_and_IN = 0x02, Mode_S_ADSB_OUT_and_IN = 0x02,
Unknown = 0xFF, Unknown = 0xFF,
}; };
enum class MsgType_XP { enum class MsgType_XP {
INVALID = 0, INVALID = 0,
Installation_Set = 0x01, Installation_Set = 0x01,
Preflight_Set = 0x02, Preflight_Set = 0x02,
Operating_Set = 0x03, Operating_Set = 0x03,
GPS_Set = 0x04, GPS_Set = 0x04,
Request = 0x05, Request = 0x05,
ACK = 0x80, ACK = 0x80,
Installation_Response = 0x81, Installation_Response = 0x81,
Preflight_Response = 0x82, Preflight_Response = 0x82,
Status_Response = 0x83, Status_Response = 0x83,
ADSB_StateVector_Report = 0x91, ADSB_StateVector_Report = 0x91,
ADSB_ModeStatus_Report = 0x92, ADSB_ModeStatus_Report = 0x92,
TISB_StateVector_Report = 0x93, TISB_StateVector_Report = 0x93,
TISB_ModeStatus_Report = 0x94, TISB_ModeStatus_Report = 0x94,
TISB_CorasePos_Report = 0x95, TISB_CorasePos_Report = 0x95,
TISB_ADSB_Mgr_Report = 0x96, TISB_ADSB_Mgr_Report = 0x96,
}; };
enum class ParseState { enum class ParseState {
WaitingFor_Start, WaitingFor_Start,
WaitingFor_AssmAddr, WaitingFor_AssmAddr,
WaitingFor_MsgType, WaitingFor_MsgType,
WaitingFor_MsgId, WaitingFor_MsgId,
WaitingFor_PayloadLen, WaitingFor_PayloadLen,
WaitingFor_PayloadContents, WaitingFor_PayloadContents,
WaitingFor_ChecksumFletcher, WaitingFor_ChecksumFletcher,
WaitingFor_Checksum, WaitingFor_Checksum,
WaitingFor_End, WaitingFor_End,
}; };
struct Packet_XP { struct Packet_XP {
const uint8_t start = 0xA5; const uint8_t start = 0xA5;
const uint8_t assemAddr = 0x01; const uint8_t assemAddr = 0x01;
MsgType_XP type; MsgType_XP type;
uint8_t id; uint8_t id;
uint8_t payload_length; uint8_t payload_length;
uint8_t payload[PAYLOAD_XP_MAX_SIZE]; uint8_t payload[PAYLOAD_XP_MAX_SIZE];
uint8_t checksumFletcher; uint8_t checksumFletcher;
uint8_t checksum; uint8_t checksum;
const uint8_t end = 0x5A; const uint8_t end = 0x5A;
}; };
struct { struct {
ParseState state; ParseState state;
uint8_t index; uint8_t index;
Packet_XP packet; Packet_XP packet;
} message_in; } message_in;
// compute Sum and FletcherSum values // compute Sum and FletcherSum values
uint16_t checksum_generate_XP(Packet_XP &msg) const; uint16_t checksum_generate_XP(Packet_XP &msg) const;
bool checksum_verify_XP(Packet_XP &msg) const; bool checksum_verify_XP(Packet_XP &msg) const;
void checksum_assign_XP(Packet_XP &msg); void checksum_assign_XP(Packet_XP &msg);
// handling inbound byte and process it in the state machine // handling inbound byte and process it in the state machine
bool parse_byte_XP(const uint8_t data); bool parse_byte_XP(const uint8_t data);
// handle inbound packet // handle inbound packet
void handle_packet_XP(const Packet_XP &msg); void handle_packet_XP(const Packet_XP &msg);
// send message to serial port // send message to serial port
void send_msg(Packet_XP &msg); void send_msg(Packet_XP &msg);
// handle inbound msgs // handle inbound msgs
void handle_adsb_in_msg(const Packet_XP &msg); void handle_adsb_in_msg(const Packet_XP &msg);
void handle_ack(const Packet_XP &msg); void handle_ack(const Packet_XP &msg);
// send messages to transceiver // send messages to transceiver
void send_msg_Installation(); void send_msg_Installation();
void send_msg_PreFlight(); void send_msg_PreFlight();
void send_msg_Operating(); void send_msg_Operating();
void send_msg_GPS(); void send_msg_GPS();
// send packet by type // send packet by type
void send_packet(const MsgType_XP type); void send_packet(const MsgType_XP type);
// send msg to request a packet by type // send msg to request a packet by type
void request_packet(const MsgType_XP type); void request_packet(const MsgType_XP type);
// Convert base 8 or 16 to decimal. Used to convert an octal/hexadecimal value // Convert base 8 or 16 to decimal. Used to convert an octal/hexadecimal value
// stored on a GCS as a string field in different format, but then transmitted // stored on a GCS as a string field in different format, but then transmitted
// over mavlink as a float which is always a decimal. // over mavlink as a float which is always a decimal.
uint32_t convert_base_to_decimal(const uint8_t baseIn, uint32_t inputNumber); uint32_t convert_base_to_decimal(const uint8_t baseIn, uint32_t inputNumber);
// timers for each out-bound packet // timers for each out-bound packet
uint32_t last_packet_initialize_ms; uint32_t last_packet_initialize_ms;
uint32_t last_packet_PreFlight_ms; uint32_t last_packet_PreFlight_ms;
uint32_t last_packet_GPS_ms; uint32_t last_packet_GPS_ms;
uint32_t last_packet_Operating_ms; uint32_t last_packet_Operating_ms;
// cached variables to compare against params so we can send msg on param change. // cached variables to compare against params so we can send msg on param change.
uint16_t last_operating_squawk; uint16_t last_operating_squawk;
int32_t last_operating_alt; int32_t last_operating_alt;
uint8_t last_operating_rf_select; uint8_t last_operating_rf_select;
// track status changes in acks // track status changes in acks
uint8_t last_ack_transponder_mode; uint8_t last_ack_transponder_mode;
Transponder_Type transponder_type = Transponder_Type::Unknown; Transponder_Type transponder_type = Transponder_Type::Unknown;
}; };
#endif // HAL_ADSB_SAGETECH_ENABLED #endif // HAL_ADSB_SAGETECH_ENABLED

512
libraries/AP_ADSB/AP_ADSB_uAvionix_MAVLink.cpp
View File

@ -1,256 +1,256 @@
/* /*
This program is free software: you can redistribute it and/or modify This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or the Free Software Foundation, either version 3 of the License, or
(at your option) any later version. (at your option) any later version.
This program is distributed in the hope that it will be useful, This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details. GNU General Public License for more details.
You should have received a copy of the GNU General Public License You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>. along with this program. If not, see <http://www.gnu.org/licenses/>.
*/ */
#include "AP_ADSB_uAvionix_MAVLink.h" #include "AP_ADSB_uAvionix_MAVLink.h"
#if HAL_ADSB_UAVIONIX_MAVLINK_ENABLED #if HAL_ADSB_UAVIONIX_MAVLINK_ENABLED
#include <stdio.h> // for sprintf #include <stdio.h> // for sprintf
#include <limits.h> #include <limits.h>
#include <GCS_MAVLink/GCS.h> #include <GCS_MAVLink/GCS.h>
#include <AP_GPS/AP_GPS.h> #include <AP_GPS/AP_GPS.h>
#include <AP_Baro/AP_Baro.h> #include <AP_Baro/AP_Baro.h>
#include <AP_AHRS/AP_AHRS.h> #include <AP_AHRS/AP_AHRS.h>
#define ADSB_CHAN_TIMEOUT_MS 15000 #define ADSB_CHAN_TIMEOUT_MS 15000
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
// detect if an port is configured as MAVLink // detect if an port is configured as MAVLink
bool AP_ADSB_uAvionix_MAVLink::detect() bool AP_ADSB_uAvionix_MAVLink::detect()
{ {
// this actually requires SerialProtocol_MAVLink or SerialProtocol_MAVLink2 but // this actually requires SerialProtocol_MAVLink or SerialProtocol_MAVLink2 but
// we can't have a running system with that, so its safe to assume it's already defined // we can't have a running system with that, so its safe to assume it's already defined
return true; return true;
} }
void AP_ADSB_uAvionix_MAVLink::update() void AP_ADSB_uAvionix_MAVLink::update()
{ {
const uint32_t now = AP_HAL::millis(); const uint32_t now = AP_HAL::millis();
// send static configuration data to transceiver, every 5s // send static configuration data to transceiver, every 5s
if (_frontend.out_state.chan_last_ms > 0 && now - _frontend.out_state.chan_last_ms > ADSB_CHAN_TIMEOUT_MS) { if (_frontend.out_state.chan_last_ms > 0 && now - _frontend.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
_frontend.out_state.chan = -1; _frontend.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 (_frontend.out_state.chan >= 0 && !_frontend._my_loc.is_zero() && _frontend.out_state.chan < MAVLINK_COMM_NUM_BUFFERS) { } else if (_frontend.out_state.chan >= 0 && !_frontend._my_loc.is_zero() && _frontend.out_state.chan < MAVLINK_COMM_NUM_BUFFERS) {
const mavlink_channel_t chan = (mavlink_channel_t)(MAVLINK_COMM_0 + _frontend.out_state.chan); const mavlink_channel_t chan = (mavlink_channel_t)(MAVLINK_COMM_0 + _frontend.out_state.chan);
if (now - _frontend.out_state.last_config_ms >= 5000 && HAVE_PAYLOAD_SPACE(chan, UAVIONIX_ADSB_OUT_CFG)) { if (now - _frontend.out_state.last_config_ms >= 5000 && HAVE_PAYLOAD_SPACE(chan, UAVIONIX_ADSB_OUT_CFG)) {
_frontend.out_state.last_config_ms = now; _frontend.out_state.last_config_ms = now;
send_configure(chan); send_configure(chan);
} // last_config_ms } // last_config_ms
// send dynamic data to transceiver at 5Hz // send dynamic data to transceiver at 5Hz
if (now - _frontend.out_state.last_report_ms >= 200 && HAVE_PAYLOAD_SPACE(chan, UAVIONIX_ADSB_OUT_DYNAMIC)) { if (now - _frontend.out_state.last_report_ms >= 200 && HAVE_PAYLOAD_SPACE(chan, UAVIONIX_ADSB_OUT_DYNAMIC)) {
_frontend.out_state.last_report_ms = now; _frontend.out_state.last_report_ms = now;
send_dynamic_out(chan); send_dynamic_out(chan);
} // last_report_ms } // last_report_ms
} // chan_last_ms } // chan_last_ms
} }
void AP_ADSB_uAvionix_MAVLink::send_dynamic_out(const mavlink_channel_t chan) const void AP_ADSB_uAvionix_MAVLink::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();
const int32_t latitude = _frontend._my_loc.lat; const int32_t latitude = _frontend._my_loc.lat;
const int32_t longitude = _frontend._my_loc.lng; const int32_t longitude = _frontend._my_loc.lng;
const int32_t altGNSS = _frontend._my_loc.alt * 10; // convert cm to mm const int32_t altGNSS = _frontend._my_loc.alt * 10; // convert cm to mm
const int16_t velVert = -1.0f * gps_velocity.z * 1E2; // convert m/s to cm/s const int16_t velVert = -1.0f * gps_velocity.z * 1E2; // convert m/s to cm/s
const int16_t nsVog = gps_velocity.x * 1E2; // convert m/s to cm/s const int16_t nsVog = gps_velocity.x * 1E2; // convert m/s to cm/s
const int16_t ewVog = gps_velocity.y * 1E2; // convert m/s to cm/s const int16_t ewVog = gps_velocity.y * 1E2; // convert m/s to cm/s
const uint8_t fixType = gps.status(); // this lines up perfectly with our enum const uint8_t fixType = gps.status(); // this lines up perfectly with our enum
const uint8_t emStatus = 0; // TODO: implement this ENUM. no emergency = 0 const uint8_t emStatus = 0; // TODO: implement this ENUM. no emergency = 0
const uint8_t numSats = gps.num_sats(); const uint8_t numSats = gps.num_sats();
const uint16_t squawk = _frontend.out_state.cfg.squawk_octal; const uint16_t squawk = _frontend.out_state.cfg.squawk_octal;
uint32_t accHoriz = UINT_MAX; uint32_t accHoriz = UINT_MAX;
float accHoriz_f; float accHoriz_f;
if (gps.horizontal_accuracy(accHoriz_f)) { if (gps.horizontal_accuracy(accHoriz_f)) {
accHoriz = accHoriz_f * 1E3; // convert m to mm accHoriz = accHoriz_f * 1E3; // convert m to mm
} }
uint16_t accVert = USHRT_MAX; uint16_t accVert = USHRT_MAX;
float accVert_f; float accVert_f;
if (gps.vertical_accuracy(accVert_f)) { if (gps.vertical_accuracy(accVert_f)) {
accVert = accVert_f * 1E2; // convert m to cm accVert = accVert_f * 1E2; // convert m to cm
} }
uint16_t accVel = USHRT_MAX; uint16_t accVel = USHRT_MAX;
float accVel_f; float accVel_f;
if (gps.speed_accuracy(accVel_f)) { if (gps.speed_accuracy(accVel_f)) {
accVel = accVel_f * 1E3; // convert m/s to mm/s accVel = accVel_f * 1E3; // convert m/s to mm/s
} }
uint16_t state = 0; uint16_t state = 0;
if (_frontend.out_state.is_in_auto_mode) { if (_frontend.out_state.is_in_auto_mode) {
state |= UAVIONIX_ADSB_OUT_DYNAMIC_STATE_AUTOPILOT_ENABLED; state |= UAVIONIX_ADSB_OUT_DYNAMIC_STATE_AUTOPILOT_ENABLED;
} }
if (!_frontend.out_state.is_flying) { if (!_frontend.out_state.is_flying) {
state |= UAVIONIX_ADSB_OUT_DYNAMIC_STATE_ON_GROUND; state |= UAVIONIX_ADSB_OUT_DYNAMIC_STATE_ON_GROUND;
} }
// TODO: confirm this sets utcTime correctly // TODO: confirm this sets utcTime correctly
const uint64_t gps_time = gps.time_epoch_usec(); const uint64_t gps_time = gps.time_epoch_usec();
const uint32_t utcTime = gps_time / 1000000ULL; const uint32_t utcTime = gps_time / 1000000ULL;
const AP_Baro &baro = AP::baro(); const AP_Baro &baro = AP::baro();
int32_t altPres = INT_MAX; int32_t altPres = INT_MAX;
if (baro.healthy()) { if (baro.healthy()) {
// Altitude difference between sea level pressure and current pressure. Result in millimeters // Altitude difference between sea level pressure and current pressure. Result in millimeters
altPres = baro.get_altitude_difference(SSL_AIR_PRESSURE, baro.get_pressure()) * 1E3; // convert m to mm; altPres = baro.get_altitude_difference(SSL_AIR_PRESSURE, baro.get_pressure()) * 1E3; // convert m to mm;
} }
mavlink_msg_uavionix_adsb_out_dynamic_send( mavlink_msg_uavionix_adsb_out_dynamic_send(
chan, chan,
utcTime, utcTime,
latitude, latitude,
longitude, longitude,
altGNSS, altGNSS,
fixType, fixType,
numSats, numSats,
altPres, altPres,
accHoriz, accHoriz,
accVert, accVert,
accVel, accVel,
velVert, velVert,
nsVog, nsVog,
ewVog, ewVog,
emStatus, emStatus,
state, state,
squawk); squawk);
} }
/* /*
* To expand functionality in their HW, uAvionix has extended a few of the unused MAVLink bits to pack in more new features * To expand functionality in their HW, uAvionix has extended a few of the unused MAVLink bits to pack in more new features
* 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_uAvionix_MAVLink::encode_icao(const uint32_t icao_id) const uint32_t AP_ADSB_uAvionix_MAVLink::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 = 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
//SIL/SDA are special fields that should be set to 0 with only expert user adjustment //SIL/SDA are special fields that should be set to 0 with only expert user adjustment
encoded_icao &= ~0x03000000; // SDA should always be FALSE encoded_icao &= ~0x03000000; // SDA should always be FALSE
encoded_icao &= ~0x0C000000; // SIL should always be FALSE encoded_icao &= ~0x0C000000; // SIL should always be FALSE
return encoded_icao; return encoded_icao;
} }
/* /*
* To expand functionality in their HW, uAvionix has extended a few of the unused MAVLink bits to pack in more new features * To expand functionality in their HW, uAvionix has extended a few of the unused MAVLink bits to pack in more new features
* This function will override the usually-null ending char of the callsign. It always encodes the last byte [8], even if * This function will override the usually-null ending char of the callsign. It always encodes the last byte [8], even if
* the callsign string is less than 9 chars and there are other zero-padded nulls. * the callsign string is less than 9 chars and there are other zero-padded nulls.
*/ */
uint8_t AP_ADSB_uAvionix_MAVLink::get_encoded_callsign_null_char() uint8_t AP_ADSB_uAvionix_MAVLink::get_encoded_callsign_null_char()
{ {
// Encoding of the 8bit null char // Encoding of the 8bit null char
// (LSB) - knots // (LSB) - knots
// bit.1 - knots // bit.1 - knots
// bit.2 - knots // bit.2 - knots
// bit.3 - (unused) // bit.3 - (unused)
// bit.4 - flag - ADSB_BITBASK_RF_CAPABILITIES_1090ES_IN // bit.4 - flag - ADSB_BITBASK_RF_CAPABILITIES_1090ES_IN
// bit.5 - flag - ADSB_BITBASK_RF_CAPABILITIES_UAT_IN // bit.5 - flag - ADSB_BITBASK_RF_CAPABILITIES_UAT_IN
// bit.6 - flag - 0 = callsign is treated as callsign, 1 = callsign is treated as flightPlanID/Squawk // bit.6 - flag - 0 = callsign is treated as callsign, 1 = callsign is treated as flightPlanID/Squawk
// (MSB) - (unused) // (MSB) - (unused)
uint8_t encoded_null = 0; uint8_t encoded_null = 0;
if (_frontend.out_state.cfg.maxAircraftSpeed_knots <= 0) { if (_frontend.out_state.cfg.maxAircraftSpeed_knots <= 0) {
// not set or unknown. no bits set // not set or unknown. no bits set
} else if (_frontend.out_state.cfg.maxAircraftSpeed_knots <= 75) { } else if (_frontend.out_state.cfg.maxAircraftSpeed_knots <= 75) {
encoded_null |= 0x01; encoded_null |= 0x01;
} else if (_frontend.out_state.cfg.maxAircraftSpeed_knots <= 150) { } else if (_frontend.out_state.cfg.maxAircraftSpeed_knots <= 150) {
encoded_null |= 0x02; encoded_null |= 0x02;
} else if (_frontend.out_state.cfg.maxAircraftSpeed_knots <= 300) { } else if (_frontend.out_state.cfg.maxAircraftSpeed_knots <= 300) {
encoded_null |= 0x03; encoded_null |= 0x03;
} else if (_frontend.out_state.cfg.maxAircraftSpeed_knots <= 600) { } else if (_frontend.out_state.cfg.maxAircraftSpeed_knots <= 600) {
encoded_null |= 0x04; encoded_null |= 0x04;
} else if (_frontend.out_state.cfg.maxAircraftSpeed_knots <= 1200) { } else if (_frontend.out_state.cfg.maxAircraftSpeed_knots <= 1200) {
encoded_null |= 0x05; encoded_null |= 0x05;
} else { } else {
encoded_null |= 0x06; encoded_null |= 0x06;
} }
if (_frontend.out_state.cfg.rf_capable & ADSB_BITBASK_RF_CAPABILITIES_1090ES_IN) { if (_frontend.out_state.cfg.rf_capable & ADSB_BITBASK_RF_CAPABILITIES_1090ES_IN) {
encoded_null |= 0x10; encoded_null |= 0x10;
} }
if (_frontend.out_state.cfg.rf_capable & ADSB_BITBASK_RF_CAPABILITIES_UAT_IN) { if (_frontend.out_state.cfg.rf_capable & ADSB_BITBASK_RF_CAPABILITIES_UAT_IN) {
encoded_null |= 0x20; encoded_null |= 0x20;
} }
/* /*
If the user has an 8 digit flightPlanID assigned from a filed flight plan, this should be assigned to FlightPlanID, (assigned by remote app) If the user has an 8 digit flightPlanID assigned from a filed flight plan, this should be assigned to FlightPlanID, (assigned by remote app)
else if the user has an assigned squawk code from ATC this should be converted from 4 digit octal to 4 character alpha string and assigned to FlightPlanID, else if the user has an assigned squawk code from ATC this should be converted from 4 digit octal to 4 character alpha string and assigned to FlightPlanID,
else if a tail number is known it should be set to the tail number of the aircraft, (assigned by remote app) else if a tail number is known it should be set to the tail number of the aircraft, (assigned by remote app)
else it should be left blank (all 0's) else it should be left blank (all 0's)
*/ */
// using the above logic, we must always assign the squawk. once we get configured // using the above logic, we must always assign the squawk. once we get configured
// externally then get_encoded_callsign_null_char() stops getting called // externally then get_encoded_callsign_null_char() stops getting called
snprintf(_frontend.out_state.cfg.callsign, 5, "%04d", unsigned(_frontend.out_state.cfg.squawk_octal) & 0x1FFF); snprintf(_frontend.out_state.cfg.callsign, 5, "%04d", unsigned(_frontend.out_state.cfg.squawk_octal) & 0x1FFF);
memset(&_frontend.out_state.cfg.callsign[4], 0, 5); // clear remaining 5 chars memset(&_frontend.out_state.cfg.callsign[4], 0, 5); // clear remaining 5 chars
encoded_null |= 0x40; encoded_null |= 0x40;
return encoded_null; return encoded_null;
} }
/* /*
* populate and send MAVLINK_MSG_UAVIONIX_ADSB_OUT_CFG * populate and send MAVLINK_MSG_UAVIONIX_ADSB_OUT_CFG
*/ */
void AP_ADSB_uAvionix_MAVLink::send_configure(const mavlink_channel_t chan) void AP_ADSB_uAvionix_MAVLink::send_configure(const mavlink_channel_t chan)
{ {
// MAVLink spec says the 9 byte callsign field is 8 byte string with 9th byte as null. // MAVLink spec says the 9 byte callsign field is 8 byte string with 9th byte as null.
// Here we temporarily set some flags in that null char to signify the callsign // Here we temporarily set some flags in that null char to signify the callsign
// may be a flightplanID instead // may be a flightplanID instead
int8_t callsign[sizeof(_frontend.out_state.cfg.callsign)]; int8_t callsign[sizeof(_frontend.out_state.cfg.callsign)];
uint32_t icao; uint32_t icao;
memcpy(callsign, _frontend.out_state.cfg.callsign, sizeof(_frontend.out_state.cfg.callsign)); memcpy(callsign, _frontend.out_state.cfg.callsign, sizeof(_frontend.out_state.cfg.callsign));
if (_frontend.out_state.cfg.was_set_externally) { if (_frontend.out_state.cfg.was_set_externally) {
// take values as-is // take values as-is
icao = _frontend.out_state.cfg.ICAO_id; icao = _frontend.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 = encode_icao((uint32_t)_frontend.out_state.cfg.ICAO_id); icao = encode_icao((uint32_t)_frontend.out_state.cfg.ICAO_id);
} }
mavlink_msg_uavionix_adsb_out_cfg_send( mavlink_msg_uavionix_adsb_out_cfg_send(
chan, chan,
icao, icao,
(const char*)callsign, (const char*)callsign,
(uint8_t)_frontend.out_state.cfg.emitterType, (uint8_t)_frontend.out_state.cfg.emitterType,
(uint8_t)_frontend.out_state.cfg.lengthWidth, (uint8_t)_frontend.out_state.cfg.lengthWidth,
(uint8_t)_frontend.out_state.cfg.gpsOffsetLat, (uint8_t)_frontend.out_state.cfg.gpsOffsetLat,
(uint8_t)_frontend.out_state.cfg.gpsOffsetLon, (uint8_t)_frontend.out_state.cfg.gpsOffsetLon,
_frontend.out_state.cfg.stall_speed_cm, _frontend.out_state.cfg.stall_speed_cm,
(uint8_t)_frontend.out_state.cfg.rfSelect); (uint8_t)_frontend.out_state.cfg.rfSelect);
} }
#endif // HAL_ADSB_ENABLED #endif // HAL_ADSB_ENABLED

88
libraries/AP_ADSB/AP_ADSB_uAvionix_MAVLink.h
View File

@ -1,44 +1,44 @@
#pragma once #pragma once
/* /*
This program is free software: you can redistribute it and/or modify This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or the Free Software Foundation, either version 3 of the License, or
(at your option) any later version. (at your option) any later version.
This program is distributed in the hope that it will be useful, This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details. GNU General Public License for more details.
You should have received a copy of the GNU General Public License You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>. along with this program. If not, see <http://www.gnu.org/licenses/>.
*/ */
#include "AP_ADSB_Backend.h" #include "AP_ADSB_Backend.h"
#ifndef HAL_ADSB_UAVIONIX_MAVLINK_ENABLED #ifndef HAL_ADSB_UAVIONIX_MAVLINK_ENABLED
#define HAL_ADSB_UAVIONIX_MAVLINK_ENABLED HAL_ADSB_ENABLED #define HAL_ADSB_UAVIONIX_MAVLINK_ENABLED HAL_ADSB_ENABLED
#endif #endif
#if HAL_ADSB_UAVIONIX_MAVLINK_ENABLED #if HAL_ADSB_UAVIONIX_MAVLINK_ENABLED
class AP_ADSB_uAvionix_MAVLink : public AP_ADSB_Backend { class AP_ADSB_uAvionix_MAVLink : public AP_ADSB_Backend {
public: public:
using AP_ADSB_Backend::AP_ADSB_Backend; using AP_ADSB_Backend::AP_ADSB_Backend;
void update() override; void update() override;
// static detection function // static detection function
static bool detect(); static bool detect();
private: private:
// 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) const; void send_dynamic_out(const mavlink_channel_t chan) const;
// special helpers for uAvionix workarounds // special helpers for uAvionix workarounds
uint32_t encode_icao(const uint32_t icao_id) const; 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);
}; };
#endif // HAL_ADSB_ENABLED #endif // HAL_ADSB_ENABLED