mirror of https://github.com/ArduPilot/ardupilot
AP_ADSB: add Sagetech XP driver
This commit is contained in:
Tom Pittenger
Tom Pittenger
parent
ffd3d1da1d
commit
8e6278f269
|
|
@ -24,6 +24,7 @@
|
|||
|
||||
#if HAL_ADSB_ENABLED
|
||||
#include "AP_ADSB_uAvionix_MAVLink.h"
|
||||
#include "AP_ADSB_Sagetech.h"
|
||||
#include <AP_Vehicle/AP_Vehicle.h>
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
#include <AP_Logger/AP_Logger.h>
|
||||
|
|
@ -53,7 +54,7 @@ const AP_Param::GroupInfo AP_ADSB::var_info[] = {
|
|||
// @Param: TYPE
|
||||
// @DisplayName: ADSB Type
|
||||
// @Description: Type of ADS-B hardware for ADSB-in and ADSB-out configuration and operation. If any type is selected then MAVLink based ADSB-in messages will always be enabled
|
||||
// @Values: 0:Disabled,1:uAvionix-MAVLink
|
||||
// @Values: 0:Disabled,1:uAvionix-MAVLink,2:Sagetech
|
||||
// @User: Standard
|
||||
// @RebootRequired: True
|
||||
AP_GROUPINFO_FLAGS("TYPE", 0, AP_ADSB, _type[0], 0, AP_PARAM_FLAG_ENABLE),
|
||||
|
|
@ -198,11 +199,16 @@ void AP_ADSB::init(void)
|
|||
if (detected_num_instances == 0) {
|
||||
for (uint8_t i=0; i<ADSB_MAX_INSTANCES; i++) {
|
||||
detect_instance(i);
|
||||
if (_backend[i] != nullptr) {
|
||||
// we loaded a driver for this instance, so it must be
|
||||
// present (although it may not be healthy)
|
||||
detected_num_instances = i+1;
|
||||
if (_backend[i] == nullptr) {
|
||||
continue;
|
||||
}
|
||||
if (!_backend[i]->init()) {
|
||||
delete _backend[i];
|
||||
_backend[i] = nullptr;
|
||||
continue;
|
||||
}
|
||||
// success
|
||||
detected_num_instances = i+1;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -243,12 +249,20 @@ void AP_ADSB::detect_instance(uint8_t instance)
|
|||
switch (get_type(instance)) {
|
||||
case Type::None:
|
||||
return;
|
||||
|
||||
case Type::uAvionix_MAVLink:
|
||||
if (AP_ADSB_uAvionix_MAVLink::detect()) {
|
||||
_backend[instance] = new AP_ADSB_uAvionix_MAVLink(*this, instance);
|
||||
return;
|
||||
}
|
||||
break;
|
||||
|
||||
case Type::Sagetech:
|
||||
if (AP_ADSB_Sagetech::detect()) {
|
||||
_backend[instance] = new AP_ADSB_Sagetech(*this, instance);
|
||||
return;
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -42,6 +42,7 @@ class AP_ADSB {
|
|||
public:
|
||||
friend class AP_ADSB_Backend;
|
||||
friend class AP_ADSB_uAvionix_MAVLink;
|
||||
friend class AP_ADSB_Sagetech;
|
||||
|
||||
// constructor
|
||||
AP_ADSB();
|
||||
|
|
@ -59,6 +60,7 @@ public:
|
|||
enum class Type {
|
||||
None = 0,
|
||||
uAvionix_MAVLink = 1,
|
||||
Sagetech = 2,
|
||||
};
|
||||
|
||||
struct adsb_vehicle_t {
|
||||
|
|
|
|||
|
|
@ -31,10 +31,15 @@ public:
|
|||
static bool detect();
|
||||
|
||||
virtual void update() = 0;
|
||||
|
||||
virtual bool init() { return true; }
|
||||
|
||||
protected:
|
||||
|
||||
uint8_t _instance;
|
||||
|
||||
AP_HAL::UARTDriver *_port;
|
||||
|
||||
// references
|
||||
AP_ADSB &_frontend;
|
||||
};
|
||||
|
|
|
|||
|
|
@ -0,0 +1,557 @@
|
|||
/*
|
||||
Copyright (C) 2020 Kraus Hamdani Aerospace Inc. All rights reserved.
|
||||
|
||||
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
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include "AP_ADSB_Sagetech.h"
|
||||
|
||||
#if HAL_ADSB_SAGETECH_ENABLED
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
#include <AP_AHRS/AP_AHRS.h>
|
||||
#include <AP_RTC/AP_RTC.h>
|
||||
#include <AP_HAL/utility/sparse-endian.h>
|
||||
#include <stdio.h>
|
||||
#include <time.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
#define SAGETECH_SCALER_LATLNG (1.0f/2.145767E-5f) // 180/(2^23)
|
||||
#define SAGETECH_SCALER_KNOTS_TO_CMS ((KNOTS_TO_M_PER_SEC/0.125f) * 100.0f)
|
||||
#define SAGETECH_SCALER_ALTITUDE (1.0f/0.015625f)
|
||||
#define SAGETECH_SCALER_HEADING_CM ((360.0f/256.0f) * 100.0f)
|
||||
|
||||
#define SAGETECH_VALIDFLAG_LATLNG (1U<<0)
|
||||
#define SAGETECH_VALIDFLAG_ALTITUDE (1U<<1)
|
||||
#define SAGETECH_VALIDFLAG_VELOCITY (1U<<2)
|
||||
#define SAGETECH_VALIDFLAG_GND_SPEED (1U<<3)
|
||||
#define SAGETECH_VALIDFLAG_HEADING (1U<<4)
|
||||
#define SAGETECH_VALIDFLAG_V_RATE_GEO (1U<<5)
|
||||
#define SAGETECH_VALIDFLAG_V_RATE_BARO (1U<<6)
|
||||
#define SAGETECH_VALIDFLAG_EST_LATLNG (1U<<7)
|
||||
#define SAGETECH_VALIDFLAG_EST_VELOCITY (1U<<8)
|
||||
|
||||
// detect if any port is configured as Sagetech
|
||||
bool AP_ADSB_Sagetech::detect()
|
||||
{
|
||||
return (AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_ADSB, 0) != nullptr);
|
||||
}
|
||||
|
||||
// Init, called once after class is constructed
|
||||
bool AP_ADSB_Sagetech::init()
|
||||
{
|
||||
_port = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_ADSB, 0);
|
||||
|
||||
return (_port != nullptr);
|
||||
}
|
||||
|
||||
void AP_ADSB_Sagetech::update()
|
||||
{
|
||||
if (_port == nullptr) {
|
||||
return;
|
||||
}
|
||||
|
||||
const uint32_t now_ms = AP_HAL::millis();
|
||||
|
||||
// -----------------------------
|
||||
// read any available data on serial port
|
||||
// -----------------------------
|
||||
uint32_t nbytes = MIN(_port->available(), 10 * PAYLOAD_XP_MAX_SIZE);
|
||||
while (nbytes-- > 0) {
|
||||
const int16_t data = (uint8_t)_port->read();
|
||||
if (data < 0) {
|
||||
break;
|
||||
}
|
||||
if (parse_byte_XP((uint8_t)data)) {
|
||||
handle_packet_XP(message_in.packet);
|
||||
}
|
||||
} // while nbytes
|
||||
|
||||
|
||||
// -----------------------------
|
||||
// handle timers for generating data
|
||||
// -----------------------------
|
||||
if (!last_packet_initialize_ms || (now_ms - last_packet_initialize_ms >= 5000)) {
|
||||
last_packet_initialize_ms = now_ms;
|
||||
send_packet(MsgType_XP::Installation_Set);
|
||||
|
||||
} else if (!last_packet_PreFlight_ms || (now_ms - last_packet_PreFlight_ms >= 8200)) {
|
||||
last_packet_PreFlight_ms = now_ms;
|
||||
// TODO: allow callsign to not require a reboot
|
||||
send_packet(MsgType_XP::Preflight_Set);
|
||||
|
||||
} else if (now_ms - last_packet_Operating_ms >= 1000 && (
|
||||
last_packet_Operating_ms == 0 || // send once at boot
|
||||
// send as data changes
|
||||
last_operating_squawk != _frontend.out_state.cfg.squawk_octal ||
|
||||
abs(last_operating_alt - _frontend._my_loc.alt) > 1555 || // 1493cm == 49ft. The output resolution is 100ft per bit
|
||||
last_operating_rf_select != _frontend.out_state.cfg.rfSelect))
|
||||
{
|
||||
last_packet_Operating_ms = now_ms;
|
||||
last_operating_squawk = _frontend.out_state.cfg.squawk_octal;
|
||||
last_operating_alt = _frontend._my_loc.alt;
|
||||
last_operating_rf_select = _frontend.out_state.cfg.rfSelect;
|
||||
send_packet(MsgType_XP::Operating_Set);
|
||||
|
||||
} else if (now_ms - last_packet_GPS_ms >= (_frontend.out_state.is_flying ? 200 : 1000)) {
|
||||
// 1Hz when not flying, 5Hz when flying
|
||||
last_packet_GPS_ms = now_ms;
|
||||
send_packet(MsgType_XP::GPS_Set);
|
||||
}
|
||||
}
|
||||
|
||||
void AP_ADSB_Sagetech::send_packet(const MsgType_XP type)
|
||||
{
|
||||
switch (type) {
|
||||
case MsgType_XP::Installation_Set:
|
||||
send_msg_Installation();
|
||||
break;
|
||||
case MsgType_XP::Preflight_Set:
|
||||
send_msg_PreFlight();
|
||||
break;
|
||||
case MsgType_XP::Operating_Set:
|
||||
send_msg_Operating();
|
||||
break;
|
||||
case MsgType_XP::GPS_Set:
|
||||
send_msg_GPS();
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void AP_ADSB_Sagetech::request_packet(const MsgType_XP type)
|
||||
{
|
||||
// set all bytes in packet to 0 via {} so we only need to set the ones we need to
|
||||
Packet_XP pkt {};
|
||||
|
||||
pkt.type = MsgType_XP::Request;
|
||||
pkt.id = 0;
|
||||
pkt.payload_length = 4;
|
||||
|
||||
pkt.payload[0] = static_cast<uint8_t>(type);
|
||||
|
||||
send_msg(pkt);
|
||||
}
|
||||
|
||||
|
||||
void AP_ADSB_Sagetech::handle_packet_XP(const Packet_XP &msg)
|
||||
{
|
||||
switch (msg.type) {
|
||||
case MsgType_XP::ACK:
|
||||
handle_ack(msg);
|
||||
break;
|
||||
|
||||
case MsgType_XP::Installation_Response:
|
||||
case MsgType_XP::Preflight_Response:
|
||||
case MsgType_XP::Status_Response:
|
||||
// TODO add support for these
|
||||
break;
|
||||
|
||||
case MsgType_XP::ADSB_StateVector_Report:
|
||||
case MsgType_XP::ADSB_ModeStatus_Report:
|
||||
case MsgType_XP::TISB_StateVector_Report:
|
||||
case MsgType_XP::TISB_ModeStatus_Report:
|
||||
case MsgType_XP::TISB_CorasePos_Report:
|
||||
case MsgType_XP::TISB_ADSB_Mgr_Report:
|
||||
handle_adsb_in_msg(msg);
|
||||
break;
|
||||
|
||||
case MsgType_XP::Installation_Set:
|
||||
case MsgType_XP::Preflight_Set:
|
||||
case MsgType_XP::Operating_Set:
|
||||
case MsgType_XP::GPS_Set:
|
||||
case MsgType_XP::Request:
|
||||
// these are out-bound only and are not expected to be received
|
||||
case MsgType_XP::INVALID:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void AP_ADSB_Sagetech::handle_ack(const Packet_XP &msg)
|
||||
{
|
||||
// ACK received!
|
||||
const uint8_t system_state = msg.payload[2];
|
||||
transponder_type = (Transponder_Type)msg.payload[6];
|
||||
|
||||
const char* rfmode = "RF mode: ";
|
||||
const uint8_t prev_transponder_mode = last_ack_transponder_mode;
|
||||
last_ack_transponder_mode = (system_state >> 6) & 0x03;
|
||||
if (prev_transponder_mode != last_ack_transponder_mode) {
|
||||
switch (last_ack_transponder_mode) {
|
||||
case 0: gcs().send_text(MAV_SEVERITY_INFO, "ADSB: %sOFF", rfmode); break;
|
||||
case 1: gcs().send_text(MAV_SEVERITY_INFO, "ADSB: %sSTBY", rfmode); break;
|
||||
case 2: gcs().send_text(MAV_SEVERITY_INFO, "ADSB: %sON", rfmode); break;
|
||||
case 3: gcs().send_text(MAV_SEVERITY_INFO, "ADSB: %sON-ALT",rfmode); break;
|
||||
default: break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void AP_ADSB_Sagetech::handle_adsb_in_msg(const Packet_XP &msg)
|
||||
{
|
||||
AP_ADSB::adsb_vehicle_t vehicle {};
|
||||
|
||||
vehicle.last_update_ms = AP_HAL::millis();
|
||||
|
||||
switch (msg.type) {
|
||||
case MsgType_XP::ADSB_StateVector_Report: { // 0x91
|
||||
const uint16_t validFlags = le16toh_ptr(&msg.payload[8]);
|
||||
vehicle.info.ICAO_address = le24toh_ptr(&msg.payload[10]);
|
||||
|
||||
if (validFlags & SAGETECH_VALIDFLAG_LATLNG) {
|
||||
vehicle.info.lat = ((int32_t)le24toh_ptr(&msg.payload[20])) * SAGETECH_SCALER_LATLNG;
|
||||
vehicle.info.lon = ((int32_t)le24toh_ptr(&msg.payload[23])) * SAGETECH_SCALER_LATLNG;
|
||||
vehicle.info.flags |= ADSB_FLAGS_VALID_COORDS;
|
||||
}
|
||||
|
||||
if (validFlags & SAGETECH_VALIDFLAG_ALTITUDE) {
|
||||
vehicle.info.altitude = (int32_t)le24toh_ptr(&msg.payload[26]);
|
||||
vehicle.info.flags |= ADSB_FLAGS_VALID_ALTITUDE;
|
||||
}
|
||||
|
||||
if (validFlags & SAGETECH_VALIDFLAG_VELOCITY) {
|
||||
const float velNS = ((int32_t)le16toh_ptr(&msg.payload[29])) * SAGETECH_SCALER_KNOTS_TO_CMS;
|
||||
const float velEW = ((int32_t)le16toh_ptr(&msg.payload[31])) * SAGETECH_SCALER_KNOTS_TO_CMS;
|
||||
vehicle.info.hor_velocity = Vector2f(velEW, velNS).length();
|
||||
vehicle.info.flags |= ADSB_FLAGS_VALID_VELOCITY;
|
||||
}
|
||||
|
||||
if (validFlags & SAGETECH_VALIDFLAG_HEADING) {
|
||||
vehicle.info.heading = ((float)msg.payload[29]) * SAGETECH_SCALER_HEADING_CM;
|
||||
vehicle.info.flags |= ADSB_FLAGS_VALID_HEADING;
|
||||
}
|
||||
|
||||
if ((validFlags & SAGETECH_VALIDFLAG_V_RATE_GEO) || (validFlags & SAGETECH_VALIDFLAG_V_RATE_BARO)) {
|
||||
vehicle.info.ver_velocity = (int16_t)le16toh_ptr(&msg.payload[38]);
|
||||
vehicle.info.flags |= ADSB_FLAGS_VERTICAL_VELOCITY_VALID;
|
||||
}
|
||||
|
||||
_frontend.handle_adsb_vehicle(vehicle);
|
||||
break;
|
||||
}
|
||||
case MsgType_XP::ADSB_ModeStatus_Report: // 0x92
|
||||
vehicle.info.ICAO_address = le24toh_ptr(&msg.payload[9]);
|
||||
|
||||
if (msg.payload[16] != 0) {
|
||||
// if string is non-null, consider it valid
|
||||
memcpy(&vehicle.info, &msg.payload[16], 8);
|
||||
vehicle.info.flags |= ADSB_FLAGS_VALID_CALLSIGN;
|
||||
}
|
||||
|
||||
_frontend.handle_adsb_vehicle(vehicle);
|
||||
break;
|
||||
case MsgType_XP::TISB_StateVector_Report:
|
||||
case MsgType_XP::TISB_ModeStatus_Report:
|
||||
case MsgType_XP::TISB_CorasePos_Report:
|
||||
case MsgType_XP::TISB_ADSB_Mgr_Report:
|
||||
// TODO
|
||||
return;
|
||||
|
||||
default:
|
||||
return;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// handling inbound byte and process it in the state machine
|
||||
// return true when a full packet has been received
|
||||
bool AP_ADSB_Sagetech::parse_byte_XP(const uint8_t data)
|
||||
{
|
||||
switch (message_in.state) {
|
||||
default:
|
||||
case ParseState::WaitingFor_Start:
|
||||
if (data == 0xA5) {
|
||||
message_in.state = ParseState::WaitingFor_AssmAddr;
|
||||
}
|
||||
break;
|
||||
case ParseState::WaitingFor_AssmAddr:
|
||||
message_in.state = (data == 0x01) ? ParseState::WaitingFor_MsgType : ParseState::WaitingFor_Start;
|
||||
break;
|
||||
case ParseState::WaitingFor_MsgType:
|
||||
message_in.packet.type = static_cast<MsgType_XP>(data);
|
||||
message_in.state = ParseState::WaitingFor_MsgId;
|
||||
break;
|
||||
case ParseState::WaitingFor_MsgId:
|
||||
message_in.packet.id = data;
|
||||
message_in.state = ParseState::WaitingFor_PayloadLen;
|
||||
break;
|
||||
case ParseState::WaitingFor_PayloadLen:
|
||||
message_in.packet.payload_length = data;
|
||||
message_in.index = 0;
|
||||
message_in.state = (data == 0) ? ParseState::WaitingFor_ChecksumFletcher : ParseState::WaitingFor_PayloadContents;
|
||||
break;
|
||||
|
||||
case ParseState::WaitingFor_PayloadContents:
|
||||
message_in.packet.payload[message_in.index++] = data;
|
||||
if (message_in.index >= message_in.packet.payload_length) {
|
||||
message_in.state = ParseState::WaitingFor_ChecksumFletcher;
|
||||
message_in.index = 0;
|
||||
}
|
||||
break;
|
||||
|
||||
case ParseState::WaitingFor_ChecksumFletcher:
|
||||
message_in.packet.checksumFletcher = data;
|
||||
message_in.state = ParseState::WaitingFor_Checksum;
|
||||
break;
|
||||
case ParseState::WaitingFor_Checksum:
|
||||
message_in.packet.checksum = data;
|
||||
message_in.state = ParseState::WaitingFor_End;
|
||||
if (checksum_verify_XP(message_in.packet)) {
|
||||
handle_packet_XP(message_in.packet);
|
||||
}
|
||||
break;
|
||||
|
||||
case ParseState::WaitingFor_End:
|
||||
// we don't care if the end value matches
|
||||
message_in.state = ParseState::WaitingFor_Start;
|
||||
break;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
// compute Sum and FletcherSum values into a single value
|
||||
// returns uint16_t with MSByte as Sum and LSByte FletcherSum
|
||||
uint16_t AP_ADSB_Sagetech::checksum_generate_XP(Packet_XP &msg) const
|
||||
{
|
||||
uint8_t sum = 0;
|
||||
uint8_t sumFletcher = 0;
|
||||
|
||||
const uint8_t header_message_format[5] {
|
||||
0xA5, // start
|
||||
0x01, // assembly address
|
||||
static_cast<uint8_t>(msg.type),
|
||||
msg.id,
|
||||
msg.payload_length
|
||||
};
|
||||
|
||||
for (uint8_t i=0; i<5; i++) {
|
||||
sum += header_message_format[i];
|
||||
sumFletcher += sum;
|
||||
}
|
||||
for (uint8_t i=0; i<msg.payload_length; i++) {
|
||||
sum += msg.payload[i];
|
||||
sumFletcher += sum;
|
||||
}
|
||||
|
||||
return UINT16_VALUE(sum, sumFletcher);
|
||||
}
|
||||
|
||||
// computes checksum and returns true if it matches msg checksum
|
||||
bool AP_ADSB_Sagetech::checksum_verify_XP(Packet_XP &msg) const
|
||||
{
|
||||
const uint16_t checksum = checksum_generate_XP(msg);
|
||||
return (HIGHBYTE(checksum) == msg.checksum) && (LOWBYTE(checksum) == msg.checksumFletcher);
|
||||
}
|
||||
|
||||
// computes checksum and assigns checksum values to msg
|
||||
void AP_ADSB_Sagetech::checksum_assign_XP(Packet_XP &msg)
|
||||
{
|
||||
const uint16_t checksum = checksum_generate_XP(msg);
|
||||
msg.checksum = HIGHBYTE(checksum);
|
||||
msg.checksumFletcher = LOWBYTE(checksum);
|
||||
}
|
||||
|
||||
// send message to serial port
|
||||
void AP_ADSB_Sagetech::send_msg(Packet_XP &msg)
|
||||
{
|
||||
// generate and populate checksums.
|
||||
checksum_assign_XP(msg);
|
||||
|
||||
const uint8_t message_format_header[5] {
|
||||
0xA5, // start
|
||||
0x01, // assembly address
|
||||
static_cast<uint8_t>(msg.type),
|
||||
msg.id,
|
||||
msg.payload_length
|
||||
};
|
||||
const uint8_t message_format_tail[3] {
|
||||
msg.checksumFletcher,
|
||||
msg.checksum,
|
||||
0x5A // end
|
||||
};
|
||||
|
||||
if (_port != nullptr) {
|
||||
_port->write(message_format_header, sizeof(message_format_header));
|
||||
_port->write(msg.payload, msg.payload_length);
|
||||
_port->write(message_format_tail, sizeof(message_format_tail));
|
||||
}
|
||||
}
|
||||
|
||||
void AP_ADSB_Sagetech::send_msg_Installation()
|
||||
{
|
||||
Packet_XP pkt {};
|
||||
|
||||
pkt.type = MsgType_XP::Installation_Set;
|
||||
pkt.payload_length = 28; // 28== 0x1C
|
||||
|
||||
// Mode C = 3, Mode S = 0
|
||||
pkt.id = (transponder_type == Transponder_Type::Mode_C) ? 3 : 0;
|
||||
|
||||
// // convert a decimal 123456 to 0x123456
|
||||
// TODO: do a proper conversion. The param contains "131313" but what gets transmitted over the air is 0x200F1.
|
||||
const uint32_t icao_hex = convert_base_to_decimal(16, _frontend.out_state.cfg.ICAO_id_param);
|
||||
put_le24_ptr(&pkt.payload[0], icao_hex);
|
||||
|
||||
memcpy(&pkt.payload[3], &_frontend.out_state.cfg.callsign, 8);
|
||||
|
||||
pkt.payload[11] = 0; // airspeed MAX
|
||||
|
||||
pkt.payload[12] = 0; // COM Port 0 baud, fixed at 57600
|
||||
pkt.payload[13] = 0; // COM Port 1 baud, fixed at 57600
|
||||
pkt.payload[14] = 0; // COM Port 2 baud, fixed at 57600
|
||||
|
||||
pkt.payload[15] = 1; // GPS from COM port 0 (this port)
|
||||
pkt.payload[16] = 1; // GPS Integrity
|
||||
|
||||
pkt.payload[17] = _frontend.out_state.cfg.emitterType / 8; // Emitter Set
|
||||
pkt.payload[18] = _frontend.out_state.cfg.emitterType & 0x0F; // Emitter Type
|
||||
|
||||
pkt.payload[19] = _frontend.out_state.cfg.lengthWidth; // Aircraft Size
|
||||
|
||||
pkt.payload[20] = 0; // Altitude Encoder Offset
|
||||
pkt.payload[21] = 0; // Altitude Encoder Offset
|
||||
|
||||
pkt.payload[22] = 0x07; // ADSB In Control, enable reading everything
|
||||
pkt.payload[23] = 30; // ADSB In Report max length COM Port 0 (this one)
|
||||
pkt.payload[24] = 0; // ADSB In Report max length COM Port 1
|
||||
|
||||
send_msg(pkt);
|
||||
}
|
||||
|
||||
void AP_ADSB_Sagetech::send_msg_PreFlight()
|
||||
{
|
||||
Packet_XP pkt {};
|
||||
|
||||
pkt.type = MsgType_XP::Preflight_Set;
|
||||
pkt.id = 0;
|
||||
pkt.payload_length = 10;
|
||||
|
||||
memcpy(&pkt.payload[0], &_frontend.out_state.cfg.callsign, 8);
|
||||
|
||||
memset(&pkt.payload[8], 0, 2);
|
||||
|
||||
send_msg(pkt);
|
||||
}
|
||||
|
||||
void AP_ADSB_Sagetech::send_msg_Operating()
|
||||
{
|
||||
Packet_XP pkt {};
|
||||
|
||||
pkt.type = MsgType_XP::Operating_Set;
|
||||
pkt.id = 0;
|
||||
pkt.payload_length = 8;
|
||||
|
||||
// squawk
|
||||
// param is saved as native octal so we need convert back to
|
||||
// decimal because Sagetech will convert it back to octal
|
||||
uint16_t squawk = convert_base_to_decimal(8, last_operating_squawk);
|
||||
put_le16_ptr(&pkt.payload[0], squawk);
|
||||
|
||||
// altitude
|
||||
if (_frontend.out_state.cfg.rf_capable & 0x01) {
|
||||
const float alt_meters = last_operating_alt * 0.01f;
|
||||
const int32_t alt_feet = (int32_t)(alt_meters * FEET_TO_METERS);
|
||||
const int16_t alt_feet_adj = (alt_feet + 50) / 100; // 1 = 100 feet, 1 = 149 feet, 5 = 500 feet
|
||||
put_le16_ptr(&pkt.payload[2], alt_feet_adj);
|
||||
|
||||
} else {
|
||||
// use integrated altitude - recommend by sagetech
|
||||
pkt.payload[2] = 0x80;
|
||||
pkt.payload[3] = 0x00;
|
||||
}
|
||||
|
||||
// RF mode
|
||||
pkt.payload[4] = last_operating_rf_select;
|
||||
send_msg(pkt);
|
||||
}
|
||||
|
||||
void AP_ADSB_Sagetech::send_msg_GPS()
|
||||
{
|
||||
Packet_XP pkt {};
|
||||
|
||||
pkt.type = MsgType_XP::GPS_Set;
|
||||
pkt.payload_length = 52;
|
||||
pkt.id = 0;
|
||||
|
||||
const int32_t longitude = _frontend._my_loc.lng;
|
||||
const int32_t latitude = _frontend._my_loc.lat;
|
||||
|
||||
// longitude and latitude
|
||||
// NOTE: these MUST be done in double or else we get roundoff in the maths
|
||||
const double lon_deg = longitude * (double)1.0e-7 * (longitude < 0 ? -1 : 1);
|
||||
const double lon_minutes = (lon_deg - int(lon_deg)) * 60;
|
||||
snprintf((char*)&pkt.payload[0], 12, "%03u%02u.%05u", (unsigned)lon_deg, (unsigned)lon_minutes, unsigned((lon_minutes - (int)lon_minutes) * 1.0E5));
|
||||
|
||||
const double lat_deg = latitude * (double)1.0e-7 * (latitude < 0 ? -1 : 1);
|
||||
const double lat_minutes = (lat_deg - int(lat_deg)) * 60;
|
||||
snprintf((char*)&pkt.payload[11], 11, "%02u%02u.%05u", (unsigned)lat_deg, (unsigned)lat_minutes, unsigned((lat_minutes - (int)lat_minutes) * 1.0E5));
|
||||
|
||||
// ground speed
|
||||
const Vector2f speed = AP::ahrs().groundspeed_vector();
|
||||
float speed_knots = norm(speed.x, speed.y) * M_PER_SEC_TO_KNOTS;
|
||||
snprintf((char*)&pkt.payload[21], 7, "%03u.%02u", (unsigned)speed_knots, unsigned((speed_knots - (int)speed_knots) * 1.0E2));
|
||||
|
||||
// heading
|
||||
float heading = wrap_360(degrees(speed.angle()));
|
||||
snprintf((char*)&pkt.payload[27], 10, "%03u.%04u", unsigned(heading), unsigned((heading - (int)heading) * 1.0E4));
|
||||
|
||||
// hemisphere
|
||||
uint8_t hemisphere = 0;
|
||||
hemisphere |= (latitude >= 0) ? 0x01 : 0; // isNorth
|
||||
hemisphere |= (longitude >= 0) ? 0x02 : 0; // isEast
|
||||
hemisphere |= (AP::gps().status() < AP_GPS::GPS_OK_FIX_2D) ? 0x80 : 0; // isInvalid
|
||||
pkt.payload[35] = hemisphere;
|
||||
|
||||
// time
|
||||
uint64_t time_usec;
|
||||
if (AP::rtc().get_utc_usec(time_usec)) {
|
||||
// not completely accurate, our time includes leap seconds and time_t should be without
|
||||
const time_t time_sec = time_usec / 1000000;
|
||||
struct tm* tm = gmtime(&time_sec);
|
||||
|
||||
// format time string
|
||||
snprintf((char*)&pkt.payload[36], 11, "%02u%02u%06.3f", tm->tm_hour, tm->tm_min, tm->tm_sec + (time_usec % 1000000) * 1.0e-6);
|
||||
} else {
|
||||
memset(&pkt.payload[36],' ', 10);
|
||||
}
|
||||
|
||||
send_msg(pkt);
|
||||
}
|
||||
|
||||
/*
|
||||
* 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 over mavlink as a float which is always a decimal.
|
||||
* baseIn: base of input number
|
||||
* inputNumber: value currently in base "baseIn" to be converted to base "baseOut"
|
||||
*
|
||||
* Example: convert ADSB squawk octal "1200" stored in memory as 0x0280 to 0x04B0
|
||||
* uint16_t squawk_decimal = convertMathBase(8, squawk_octal);
|
||||
*/
|
||||
uint32_t AP_ADSB_Sagetech::convert_base_to_decimal(const uint8_t baseIn, uint32_t inputNumber)
|
||||
{
|
||||
// Our only sensible input bases are 16 and 8
|
||||
if (baseIn != 8 && baseIn != 16) {
|
||||
return inputNumber;
|
||||
}
|
||||
|
||||
uint32_t outputNumber = 0;
|
||||
for (uint8_t i=0; inputNumber != 0; i++) {
|
||||
outputNumber += (inputNumber % 10) * powf(10, i);
|
||||
inputNumber /= 10;
|
||||
}
|
||||
return outputNumber;
|
||||
}
|
||||
|
||||
#endif // HAL_ADSB_SAGETECH_ENABLED
|
||||
|
||||
|
|
@ -0,0 +1,161 @@
|
|||
#pragma once
|
||||
|
||||
/*
|
||||
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
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include "AP_ADSB_Backend.h"
|
||||
|
||||
#ifndef HAL_ADSB_SAGETECH_ENABLED
|
||||
#define HAL_ADSB_SAGETECH_ENABLED HAL_ADSB_ENABLED
|
||||
#endif
|
||||
|
||||
#if HAL_ADSB_SAGETECH_ENABLED
|
||||
class AP_ADSB_Sagetech : public AP_ADSB_Backend {
|
||||
public:
|
||||
using AP_ADSB_Backend::AP_ADSB_Backend;
|
||||
|
||||
// init - performs any required initialisation for this instance
|
||||
bool init() override;
|
||||
|
||||
// update - should be called periodically
|
||||
void update() override;
|
||||
|
||||
// static detection function
|
||||
static bool detect();
|
||||
|
||||
private:
|
||||
|
||||
static const uint32_t PAYLOAD_XP_MAX_SIZE = 52;
|
||||
|
||||
enum class SystemStateBits {
|
||||
Error_Transponder = (1U<<0),
|
||||
Altitidue_Source = (1U<<1),
|
||||
Error_GPS = (1U<<2),
|
||||
Error_ICAO = (1U<<3),
|
||||
Error_Over_Temperature = (1U<<4),
|
||||
Error_Extended_Squitter = (1U<<5),
|
||||
Mode_Transponder = (3U<<6), // 2 bit status:
|
||||
};
|
||||
|
||||
enum class Transponder_Type {
|
||||
Mode_C = 0x00,
|
||||
Mode_S_ADSB_OUT = 0x01,
|
||||
Mode_S_ADSB_OUT_and_IN = 0x02,
|
||||
Unknown = 0xFF,
|
||||
};
|
||||
|
||||
enum class MsgType_XP {
|
||||
INVALID = 0,
|
||||
Installation_Set = 0x01,
|
||||
Preflight_Set = 0x02,
|
||||
Operating_Set = 0x03,
|
||||
GPS_Set = 0x04,
|
||||
Request = 0x05,
|
||||
|
||||
ACK = 0x80,
|
||||
Installation_Response = 0x81,
|
||||
Preflight_Response = 0x82,
|
||||
Status_Response = 0x83,
|
||||
ADSB_StateVector_Report = 0x91,
|
||||
ADSB_ModeStatus_Report = 0x92,
|
||||
TISB_StateVector_Report = 0x93,
|
||||
TISB_ModeStatus_Report = 0x94,
|
||||
TISB_CorasePos_Report = 0x95,
|
||||
TISB_ADSB_Mgr_Report = 0x96,
|
||||
};
|
||||
|
||||
enum class ParseState {
|
||||
WaitingFor_Start,
|
||||
WaitingFor_AssmAddr,
|
||||
WaitingFor_MsgType,
|
||||
WaitingFor_MsgId,
|
||||
WaitingFor_PayloadLen,
|
||||
WaitingFor_PayloadContents,
|
||||
WaitingFor_ChecksumFletcher,
|
||||
WaitingFor_Checksum,
|
||||
WaitingFor_End,
|
||||
};
|
||||
|
||||
struct Packet_XP {
|
||||
const uint8_t start = 0xA5;
|
||||
const uint8_t assemAddr = 0x01;
|
||||
MsgType_XP type;
|
||||
uint8_t id;
|
||||
uint8_t payload_length;
|
||||
uint8_t payload[PAYLOAD_XP_MAX_SIZE];
|
||||
uint8_t checksumFletcher;
|
||||
uint8_t checksum;
|
||||
const uint8_t end = 0x5A;
|
||||
};
|
||||
|
||||
struct {
|
||||
ParseState state;
|
||||
uint8_t index;
|
||||
Packet_XP packet;
|
||||
} message_in;
|
||||
|
||||
// compute Sum and FletcherSum values
|
||||
uint16_t checksum_generate_XP(Packet_XP &msg) const;
|
||||
bool checksum_verify_XP(Packet_XP &msg) const;
|
||||
void checksum_assign_XP(Packet_XP &msg);
|
||||
|
||||
|
||||
// handling inbound byte and process it in the state machine
|
||||
bool parse_byte_XP(const uint8_t data);
|
||||
|
||||
// handle inbound packet
|
||||
void handle_packet_XP(const Packet_XP &msg);
|
||||
|
||||
// send message to serial port
|
||||
void send_msg(Packet_XP &msg);
|
||||
|
||||
// handle inbound msgs
|
||||
void handle_adsb_in_msg(const Packet_XP &msg);
|
||||
void handle_ack(const Packet_XP &msg);
|
||||
|
||||
// send messages to to transceiver
|
||||
void send_msg_Installation();
|
||||
void send_msg_PreFlight();
|
||||
void send_msg_Operating();
|
||||
void send_msg_GPS();
|
||||
|
||||
// send packet by type
|
||||
void send_packet(const MsgType_XP type);
|
||||
|
||||
// send msg to request a packet by type
|
||||
void request_packet(const MsgType_XP type);
|
||||
|
||||
// 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
|
||||
// over mavlink as a float which is always a decimal.
|
||||
uint32_t convert_base_to_decimal(const uint8_t baseIn, uint32_t inputNumber);
|
||||
|
||||
// timers for each out-bound packet
|
||||
uint32_t last_packet_initialize_ms;
|
||||
uint32_t last_packet_PreFlight_ms;
|
||||
uint32_t last_packet_GPS_ms;
|
||||
uint32_t last_packet_Operating_ms;
|
||||
|
||||
// cached variables to compare against params so we can send msg on param change.
|
||||
uint16_t last_operating_squawk;
|
||||
int32_t last_operating_alt;
|
||||
uint8_t last_operating_rf_select;
|
||||
|
||||
// track status changes in acks
|
||||
uint8_t last_ack_transponder_mode;
|
||||
Transponder_Type transponder_type = Transponder_Type::Unknown;
|
||||
};
|
||||
#endif // HAL_ADSB_SAGETECH_ENABLED
|
||||
|
||||
Loading…
Reference in New Issue