Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/libraries/AP_ADSB/AP_ADSB_Sagetech.cpp

757 lines
26 KiB
C++
Raw Blame History

/*
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_ENABLED
#include <GCS_MAVLink/GCS.h>
#include <AP_AHRS/AP_AHRS.h>
#include <AP_RTC/AP_RTC.h>
#include <stdio.h>
#include <time.h>
#include <string.h>
#include <AP_Math/bitwise.h>
extern const AP_HAL::HAL& hal;
#define SAGETECH_SCALER_LATLNG (1.0f/2.145767E-5f) // 180/(2^23)
#define SAGETECH_SCALER_KNOTS_TO_CM ((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 (1<<0)
#define SAGETECH_VALIDFLAG_ALTITUDE (1<<1)
#define SAGETECH_VALIDFLAG_VELOCITY (1<<2)
#define SAGETECH_VALIDFLAG_GND_SPEED (1<<3)
#define SAGETECH_VALIDFLAG_HEADING (1<<4)
#define SAGETECH_VALIDFLAG_V_RATE_GEO (1<<5)
#define SAGETECH_VALIDFLAG_V_RATE_BARO (1<<6)
#define SAGETECH_VALIDFLAG_EST_LATLNG (1<<7)
#define SAGETECH_VALIDFLAG_EST_VELOCITY (1<<8)
#define SAGETECH_ENFORCE_ACKS 0
#define SAGETECH_DEBUG_ACK_TIMEOUTS 0
#define SAGETECH_DEBUG_TX_ID_ONLY 0
#define SAGETECH_DEBUG_TX_ID_PAYLOAD 0
#define SAGETECH_DEBUG_TX_ALL_RAW 0
#define SAGETECH_DEBUG_RX 0
#define SAGETECH_DEBUG_RX_ACK 0
// constructor
AP_ADSB_Sagetech::AP_ADSB_Sagetech(AP_ADSB &adsb) :
AP_ADSB_Backend(adsb)
{
uart = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_Sagetech, 0);
if (uart != nullptr) {
baudrate = AP::serialmanager().find_baudrate(AP_SerialManager::SerialProtocol_Sagetech, 0);
uart->begin(baudrate);
// no sagtech hardware have flow control pins exposed
uart->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
}
}
// detect if a sensor is connected by looking for a configured serial port
bool AP_ADSB_Sagetech::detect()
{
return AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_Sagetech, 0) != nullptr;
}
void AP_ADSB_Sagetech::init()
{
if (uart == nullptr) {
gcs().send_text(MAV_SEVERITY_DEBUG, "%sInit failed, check SERIALx_PROTOCOL cfg", frontend.GcsHeader);
}
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
if (uart != nullptr) {
gcs().send_text(MAV_SEVERITY_DEBUG, "%sUART Init successful", frontend.GcsHeader);
}
#endif
}
void AP_ADSB_Sagetech::update()
{
const uint32_t now_ms = AP_HAL::millis();
// -----------------------------
// read any available data on serial port
// -----------------------------
int32_t nbytes = 0;
if (uart != nullptr) {
nbytes = uart->available();
}
while (nbytes-- > 0) {
const uint8_t data = (uint8_t)uart->read();
switch (protocol) {
default:
case AP_ADSB_Sagetech::Protocol::NONE:
// parse all protocols until we find one that works
if (parse_byte_XP(data)) {
protocol = AP_ADSB_Sagetech::Protocol::XP;
parse_packet_XP(message_in_xp.packet);
}
if (parse_byte_MX(data)) {
protocol = AP_ADSB_Sagetech::Protocol::MX;
parse_packet_MX();
}
break;
case AP_ADSB_Sagetech::Protocol::XP:
if (parse_byte_XP(data)) {
parse_packet_XP(message_in_xp.packet);
}
break;
case AP_ADSB_Sagetech::Protocol::MX:
if (parse_byte_MX(data)) {
parse_packet_MX();
}
break;
}
}
// -----------------------------
// handle timers for generating data
// -----------------------------
#if SAGETECH_ENFORCE_ACKS
if (last_packet_type_sent != MsgTypes_XP::INVALID) {
// we're expecting an ACK
if (now_ms - last_packet_send_ms >= 1000) {
response_timeout_count++;
#if SAGETECH_DEBUG_ACK_TIMEOUTS
gcs().send_text(MAV_SEVERITY_DEBUG, "%sACK Timeout type=%u, cnt=%u", frontend.GcsHeader, (unsigned)last_packet_type_sent, (unsigned)response_timeout_count);
#endif
// retry forever until we get an ACK
send_packet(last_packet_type_sent);
}
} else
#endif
{
//if (!last_packet_initialize_ms) {
if (!last_packet_initialize_ms || (now_ms - last_packet_initialize_ms >= 5000)) {
last_packet_initialize_ms = now_ms;
send_packet(MsgTypes_XP::Installation_Set);
//} else if (last_packet_PreFlight_ms == 0) {
} else if (!last_packet_PreFlight_ms || (now_ms - last_packet_PreFlight_ms >= 8200)) {
// send once, for now..
last_packet_PreFlight_ms = now_ms;
// TODO: allow callsign to not require a reboot
send_packet(MsgTypes_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(MsgTypes_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(MsgTypes_XP::GPS_Set);
}
}
}
void AP_ADSB_Sagetech::send_packet(const MsgTypes_XP type)
{
switch (type) {
case MsgTypes_XP::Installation_Set:
send_Installation();
break;
case MsgTypes_XP::Preflight_Set:
send_PreFlight();
break;
case MsgTypes_XP::Operating_Set:
send_Operating();
break;
case MsgTypes_XP::GPS_Set:
send_GPS();
break;
default:
break;
}
}
void AP_ADSB_Sagetech::request_packet(const MsgTypes_XP type)
{
Packet_XP pkt {};
pkt.type = MsgTypes_XP::Request;
pkt.id = 0;
pkt.payload_length = 4;
pkt.payload[0] = static_cast<uint8_t>(type);
send_msg(pkt);
}
void AP_ADSB_Sagetech::parse_packet_XP(const Packet_XP &msg)
{
#if SAGETECH_DEBUG_RX
gcs().send_text(MAV_SEVERITY_DEBUG, "%sRX type=%u, id=%d", frontend.GcsHeader, (unsigned)msg.type, msg.id);
#endif
switch (msg.type) {
case MsgTypes_XP::ACK: {
// 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, "%s%sOFF", frontend.GcsHeader, rfmode); break;
case 1: gcs().send_text(MAV_SEVERITY_INFO, "%s%sSTBY", frontend.GcsHeader, rfmode); break;
case 2: gcs().send_text(MAV_SEVERITY_INFO, "%s%sON", frontend.GcsHeader, rfmode); break;
case 3: gcs().send_text(MAV_SEVERITY_INFO, "%s%sON-ALT",frontend.GcsHeader, rfmode); break;
default: break;
}
}
#if SAGETECH_DEBUG_RX_ACK
const uint8_t acked_type = msg.payload[0];
const uint8_t acked_id = msg.payload[1];
const int32_t pres_altitude = (int32_t)fetchU32(&msg.payload[3]);
const uint16_t squawk = fetchU16(&msg.payload[7]);
gcs().send_text(MAV_SEVERITY_DEBUG, "%sACK %u, %u, 0x%02X, %d, %u, %u %u", frontend.GcsHeader,
acked_type,
acked_id,
system_state,
pres_altitude,
transponder_type,
squawk,
last_ack_transponder_mode); // mode
for (uint8_t i=0; i<6; i++) {
const uint8_t stateBits = (system_state & (1<< i));
if (stateBits != 0 && stateBits != SystemStateBits::Altitidue_Source) {
gcs().send_text(MAV_SEVERITY_DEBUG, "%sACK status: %s", frontend.GcsHeader, systemStatsBits_to_str((SystemStateBits)stateBits));
}
}
#endif
}
break;
case MsgTypes_XP::Installatioin_Response:
case MsgTypes_XP::Preflight_Response:
case MsgTypes_XP::Status_Response:
// TODO add support for these
break;
case MsgTypes_XP::ADSB_StateVector_Report:
case MsgTypes_XP::ADSB_ModeStatus_Report:
case MsgTypes_XP::TISB_StateVector_Report:
case MsgTypes_XP::TISB_ModeStatus_Report:
case MsgTypes_XP::TISB_CorasePos_Report:
case MsgTypes_XP::TISB_ADSB_Mgr_Report:
handle_adsb_in_msg(msg);
break;
case MsgTypes_XP::Installation_Set:
case MsgTypes_XP::Preflight_Set:
case MsgTypes_XP::Operating_Set:
case MsgTypes_XP::GPS_Set:
case MsgTypes_XP::Request:
// these are out-bound only and are not expected to be received
FALLTHROUGH;
default:
last_packet_type_sent = INVALID;
break;
}
}
void AP_ADSB_Sagetech::handle_adsb_in_msg(const Packet_XP &msg)
{
AP_ADSB::adsb_vehicle_t vehicle {};
const uint32_t now = AP_HAL::millis();
vehicle.last_update_ms = now;
uint16_t validFlags;
switch (msg.type) {
case MsgTypes_XP::ADSB_StateVector_Report: // 0x91
validFlags = fetchU16(&msg.payload[8]);
vehicle.info.ICAO_address = fetchU24(&msg.payload[10]);
if (validFlags & SAGETECH_VALIDFLAG_LATLNG) {
vehicle.info.lat = ((int32_t)fetchU24(&msg.payload[20])) * SAGETECH_SCALER_LATLNG;
vehicle.info.lon = ((int32_t)fetchU24(&msg.payload[23])) * SAGETECH_SCALER_LATLNG;
vehicle.info.flags |= ADSB_FLAGS_VALID_COORDS;
}
if (validFlags & SAGETECH_VALIDFLAG_ALTITUDE) {
vehicle.info.altitude = (int32_t)fetchU24(&msg.payload[26]);
vehicle.info.flags |= ADSB_FLAGS_VALID_ALTITUDE;
}
if (validFlags & SAGETECH_VALIDFLAG_VELOCITY) {
const float velNS = ((int32_t)fetchU16(&msg.payload[29])) * SAGETECH_SCALER_KNOTS_TO_CM;
const float velEW = ((int32_t)fetchU16(&msg.payload[31])) * SAGETECH_SCALER_KNOTS_TO_CM;
vehicle.info.hor_velocity = Vector2f(velEW, velNS).angle();
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)fetchU16(&msg.payload[38]);
vehicle.info.flags |= ADSB_FLAGS_VERTICAL_VELOCITY_VALID;
}
if (vehicle.info.flags != 0) {
frontend.handle_adsb_vehicle(vehicle);
}
break;
case MsgTypes_XP::ADSB_ModeStatus_Report: // 0x92
validFlags = msg.payload[8];
vehicle.info.ICAO_address = fetchU24(&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;
}
if (vehicle.info.flags != 0) {
frontend.handle_adsb_vehicle(vehicle);
}
break;
case MsgTypes_XP::TISB_StateVector_Report:
case MsgTypes_XP::TISB_ModeStatus_Report:
case MsgTypes_XP::TISB_CorasePos_Report:
case MsgTypes_XP::TISB_ADSB_Mgr_Report:
// TODO
return;
default:
return;
}
}
bool AP_ADSB_Sagetech::parse_byte_XP(const uint8_t data)
{
switch (message_in_xp.state) {
default:
case ParseState::WaitingFor_Start:
if (data == 0xA5) {
message_in_xp.state = ParseState::WaitingFor_AssmAddr;
}
break;
case ParseState::WaitingFor_AssmAddr:
message_in_xp.state = (data == 0x01) ? ParseState::WaitingFor_MsgType : ParseState::WaitingFor_Start;
break;
case ParseState::WaitingFor_MsgType:
message_in_xp.packet.type = static_cast<MsgTypes_XP>(data);
message_in_xp.state = ParseState::WaitingFor_MsgId;
break;
case ParseState::WaitingFor_MsgId:
message_in_xp.packet.id = data;
message_in_xp.state = ParseState::WaitingFor_PayloadLen;
break;
case ParseState::WaitingFor_PayloadLen:
message_in_xp.packet.payload_length = data;
message_in_xp.index = 0;
message_in_xp.state = (data == 0) ? ParseState::WaitingFor_ChecksumFletcher : ParseState::WaitingFor_PayloadContents;
break;
case ParseState::WaitingFor_PayloadContents:
message_in_xp.packet.payload[message_in_xp.index++] = data;
if (message_in_xp.index >= message_in_xp.packet.payload_length) {
message_in_xp.state = ParseState::WaitingFor_ChecksumFletcher;
message_in_xp.index = 0;
}
break;
case ParseState::WaitingFor_ChecksumFletcher:
message_in_xp.packet.checksumFletcher = data;
message_in_xp.state = ParseState::WaitingFor_Checksum;
break;
case ParseState::WaitingFor_Checksum:
message_in_xp.packet.checksum = data;
message_in_xp.state = ParseState::WaitingFor_End;
if (checksum_XP(message_in_xp.packet)) {
parse_packet_XP(message_in_xp.packet);
}
break;
case ParseState::WaitingFor_End:
// we don't care if the end value matches
message_in_xp.state = ParseState::WaitingFor_Start;
break;
}
return false;
}
// compute Sum and FletcherSum and write them into msg.
// returns true if the values in msg were already the correct ones
//
// this allows a single function to tell you if inbound msg is correct wirh true result, and ignore result for outbound
bool AP_ADSB_Sagetech::checksum_XP(Packet_XP &msg)
{
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;
}
// the calculated sums already match the values in the msg
if ((sum == msg.checksum) && (sumFletcher == msg.checksumFletcher)) {
return true;
}
msg.checksum = sum;
msg.checksumFletcher = sumFletcher;
return false;
}
void AP_ADSB_Sagetech::send_msg(Packet_XP &msg)
{
// generate and populate checksums.
checksum_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 (uart != nullptr) {
uart->write(message_format_header, sizeof(message_format_header));
uart->write(msg.payload, msg.payload_length);
uart->write(message_format_tail, sizeof(message_format_tail));
}
last_packet_type_sent = msg.type;
last_packet_send_ms = AP_HAL::millis();
#if SAGETECH_DEBUG_TX_ALL_RAW
gcs().send_text(MAV_SEVERITY_DEBUG, "%s-------", frontend.GcsHeader);
for (uint8_t i=0; i<sizeof(message_format_header); i++) {
gcs().send_text(MAV_SEVERITY_DEBUG, "%shead[%2u] = 0x%02x", frontend.GcsHeader, i, message_format_header[i]);
}
#endif
#if SAGETECH_DEBUG_TX_ID_ONLY
gcs().send_text(MAV_SEVERITY_DEBUG, "%sTx type=%d %s", frontend.GcsHeader, static_cast<uint8_t>(msg.type), type_to_str(msg.type));
#endif
#if SAGETECH_DEBUG_TX_ID_PAYLOAD || SAGETECH_DEBUG_TX_ALL_RAW
#if SAGETECH_DEBUG_TX_ID_PAYLOAD
gcs().send_text(MAV_SEVERITY_DEBUG, "%sTx type=%u, payload.len=%u", frontend.GcsHeader, (unsigned)msg.type, (unsigned)msg.payload_length);
#endif
for (uint8_t i=0; i<msg.payload_length; i++) {
const uint8_t data = msg.payload[i];
if (isalnum(data) || data == '.' || data == '-' || data == ' ') {
gcs().send_text(MAV_SEVERITY_DEBUG, "%sdata[%2u] = 0x%02x %c", frontend.GcsHeader, i, data, (char)data);
} else {
gcs().send_text(MAV_SEVERITY_DEBUG, "%sdata[%2u] = 0x%02x", frontend.GcsHeader, i, data);
}
}
#endif
#if SAGETECH_DEBUG_TX_ALL_RAW
for (uint8_t i=0; i<sizeof(message_format_tail); i++) {
gcs().send_text(MAV_SEVERITY_DEBUG, "%stail[%2u] = 0x%02x", frontend.GcsHeader, (unsigned)(i + sizeof(message_format_header) + msg.payload_length), message_format_tail[i]);
}
#endif
}
void AP_ADSB_Sagetech::send_Installation()
{
uint8_t adsb_in_vehicles_per_second = 0;
uint8_t baud_value = 0;
switch (baudrate) {
case 2400:
baud_value = 1;
adsb_in_vehicles_per_second = 0;
break;
case 4800:
baud_value = 2;
adsb_in_vehicles_per_second = 0;
break;
case 9600:
baud_value = 3;
adsb_in_vehicles_per_second = 5;
break;
case 19200:
baud_value = 4;
adsb_in_vehicles_per_second = 10;
break;
case 38:
case 38400:
baud_value = 5;
adsb_in_vehicles_per_second = 20;
break;
default:
case 57:
case 57600:
baud_value = 0;
adsb_in_vehicles_per_second = 30;
break;
case 115:
case 115000:
case 115200:
baud_value = 6;
adsb_in_vehicles_per_second = 60;
break;
case 230:
case 230000:
case 230400:
baud_value = 7;
adsb_in_vehicles_per_second = 100;
break;
case 460:
case 460000:
case 460800:
baud_value = 8;
adsb_in_vehicles_per_second = 100;
break;
}
Packet_XP pkt {};
pkt.type = MsgTypes_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 = convertMathBase(16, 10, frontend.out_state.cfg.ICAO_id_param);
loadUint(&pkt.payload[0], icao_hex, 24);
memcpy(&pkt.payload[3], &frontend.out_state.cfg.callsign, 8);
pkt.payload[11] = 0; // airspeed MAX
pkt.payload[12] = baud_value; // COM Port 0 baud
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] = adsb_in_vehicles_per_second; // 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_PreFlight()
{
Packet_XP pkt {};
pkt.type = MsgTypes_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_Operating()
{
Packet_XP pkt {};
pkt.type = MsgTypes_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 = convertMathBase(8, 10, last_operating_squawk);
loadUint(&pkt.payload[0], squawk, 16);
// 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
loadUint(&pkt.payload[2], alt_feet_adj, 16);
} else {
// use integrated altitude - recommend by sagetech
pkt.payload[2] = 0x80;
pkt.payload[3] = 0x00;
}
// RF mode
pkt.payload[4] = last_operating_rf_select;
if (last_operating_rf_select & 0x04) {
// Ident should only be sent once. It's held on in the hw and then automatically
// disabled after 18 seconds. It can not be turned off externally once enabled
gcs().send_text(MAV_SEVERITY_DEBUG, "%sIdent!", frontend.GcsHeader);
frontend.out_state.cfg.rfSelect.set_and_save_and_notify(last_operating_rf_select & ~0x04);
}
send_msg(pkt);
}
void AP_ADSB_Sagetech::send_GPS()
{
Packet_XP pkt {};
pkt.type = MsgTypes_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(atan2f(speed.x, speed.y)));
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)) {
memset(&pkt.payload[36],' ', 10);
} else {
// 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);
}
send_msg(pkt);
}
const char* AP_ADSB_Sagetech::type_to_str(const uint8_t type)
{
switch (type) {
case 0x01: return "Install";
case 0x02: return "PreFlight";
case 0x03: return "Operate";
case 0x04: return "GPS Data";
case 0x05: return "DataReq";
default: return "Unknown";
}
}
const char* AP_ADSB_Sagetech::systemStatsBits_to_str(const SystemStateBits systemStateBits)
{
switch (systemStateBits) {
case SystemStateBits::Error_Transponder: return "Error_Transponder";
case SystemStateBits::Altitidue_Source: return "Altitidue_Source";
case SystemStateBits::Error_GPS: return "Error_GPS";
case SystemStateBits::Error_ICAO: return "Error_ICAO";
case SystemStateBits::Error_Over_Temperature: return "Error_Over_Temperature";
case SystemStateBits::Error_Extended_Squitter: return "Error_Extended_Squitter";
default: return "Unknown";
}
}
#endif // HAL_ADSB_ENABLED
Reference in New Issue View Git Blame Copy Permalink