diff --git a/libraries/AP_ADSB/AP_ADSB_Backend.cpp b/libraries/AP_ADSB/AP_ADSB_Backend.cpp index 78ba460126..3797694300 100644 --- a/libraries/AP_ADSB/AP_ADSB_Backend.cpp +++ b/libraries/AP_ADSB/AP_ADSB_Backend.cpp @@ -1,30 +1,30 @@ -/* - 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 . - */ - -#include "AP_ADSB_Backend.h" - -#if HAL_ADSB_ENABLED - -/* - base class constructor. -*/ -AP_ADSB_Backend::AP_ADSB_Backend(AP_ADSB &frontend, uint8_t instance) : - _frontend(frontend), - _instance(instance) -{ -} - -#endif // HAL_ADSB_ENABLED - +/* + 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 . + */ + +#include "AP_ADSB_Backend.h" + +#if HAL_ADSB_ENABLED + +/* + base class constructor. +*/ +AP_ADSB_Backend::AP_ADSB_Backend(AP_ADSB &frontend, uint8_t instance) : + _frontend(frontend), + _instance(instance) +{ +} + +#endif // HAL_ADSB_ENABLED + diff --git a/libraries/AP_ADSB/AP_ADSB_Sagetech.cpp b/libraries/AP_ADSB/AP_ADSB_Sagetech.cpp index d1d902dbcd..13ebc1bf5c 100644 --- a/libraries/AP_ADSB/AP_ADSB_Sagetech.cpp +++ b/libraries/AP_ADSB/AP_ADSB_Sagetech.cpp @@ -1,553 +1,553 @@ -/* - 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 . - */ - -#include "AP_ADSB_Sagetech.h" - -#if HAL_ADSB_SAGETECH_ENABLED -#include -#include -#include -#include -#include -#include -#include -#include - -#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().have_serial(AP_SerialManager::SerialProtocol_ADSB, 0); -} - -// 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(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 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) { - static const char *mode_names[] = {"OFF", "STBY", "ON", "ON-ALT"}; - if (last_ack_transponder_mode < ARRAY_SIZE(mode_names)) { - gcs().send_text(MAV_SEVERITY_INFO, "ADSB: RF Mode: %s", mode_names[last_ack_transponder_mode]); - } - } -} - -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(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(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.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 = speed.length() * 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 - +/* + 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 . + */ + +#include "AP_ADSB_Sagetech.h" + +#if HAL_ADSB_SAGETECH_ENABLED +#include +#include +#include +#include +#include +#include +#include +#include + +#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().have_serial(AP_SerialManager::SerialProtocol_ADSB, 0); +} + +// 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(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 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) { + static const char *mode_names[] = {"OFF", "STBY", "ON", "ON-ALT"}; + if (last_ack_transponder_mode < ARRAY_SIZE(mode_names)) { + gcs().send_text(MAV_SEVERITY_INFO, "ADSB: RF Mode: %s", mode_names[last_ack_transponder_mode]); + } + } +} + +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(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(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.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 = speed.length() * 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 + diff --git a/libraries/AP_ADSB/AP_ADSB_Sagetech.h b/libraries/AP_ADSB/AP_ADSB_Sagetech.h index e7c109a454..bf825174f7 100644 --- a/libraries/AP_ADSB/AP_ADSB_Sagetech.h +++ b/libraries/AP_ADSB/AP_ADSB_Sagetech.h @@ -1,161 +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 . - */ - -#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 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 - +#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 . + */ + +#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 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 + diff --git a/libraries/AP_ADSB/AP_ADSB_uAvionix_MAVLink.cpp b/libraries/AP_ADSB/AP_ADSB_uAvionix_MAVLink.cpp index fe5feee924..14a3b04808 100644 --- a/libraries/AP_ADSB/AP_ADSB_uAvionix_MAVLink.cpp +++ b/libraries/AP_ADSB/AP_ADSB_uAvionix_MAVLink.cpp @@ -1,256 +1,256 @@ -/* - 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 . - */ - -#include "AP_ADSB_uAvionix_MAVLink.h" - -#if HAL_ADSB_UAVIONIX_MAVLINK_ENABLED -#include // for sprintf -#include -#include -#include -#include -#include - -#define ADSB_CHAN_TIMEOUT_MS 15000 - - -extern const AP_HAL::HAL& hal; - -// detect if an port is configured as MAVLink -bool AP_ADSB_uAvionix_MAVLink::detect() -{ - // 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 - return true; -} - -void AP_ADSB_uAvionix_MAVLink::update() -{ - const uint32_t now = AP_HAL::millis(); - - // 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) { - // haven't gotten a heartbeat health status packet in a while, assume hardware failure - // TODO: reset out_state.chan - _frontend.out_state.chan = -1; - 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) { - 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)) { - _frontend.out_state.last_config_ms = now; - send_configure(chan); - } // last_config_ms - - // send dynamic data to transceiver at 5Hz - if (now - _frontend.out_state.last_report_ms >= 200 && HAVE_PAYLOAD_SPACE(chan, UAVIONIX_ADSB_OUT_DYNAMIC)) { - _frontend.out_state.last_report_ms = now; - send_dynamic_out(chan); - } // last_report_ms - } // chan_last_ms -} - -void AP_ADSB_uAvionix_MAVLink::send_dynamic_out(const mavlink_channel_t chan) const -{ - const AP_GPS &gps = AP::gps(); - const Vector3f &gps_velocity = gps.velocity(); - - const int32_t latitude = _frontend._my_loc.lat; - const int32_t longitude = _frontend._my_loc.lng; - 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 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 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 numSats = gps.num_sats(); - const uint16_t squawk = _frontend.out_state.cfg.squawk_octal; - - uint32_t accHoriz = UINT_MAX; - float accHoriz_f; - if (gps.horizontal_accuracy(accHoriz_f)) { - accHoriz = accHoriz_f * 1E3; // convert m to mm - } - - uint16_t accVert = USHRT_MAX; - float accVert_f; - if (gps.vertical_accuracy(accVert_f)) { - accVert = accVert_f * 1E2; // convert m to cm - } - - uint16_t accVel = USHRT_MAX; - float accVel_f; - if (gps.speed_accuracy(accVel_f)) { - accVel = accVel_f * 1E3; // convert m/s to mm/s - } - - uint16_t state = 0; - if (_frontend.out_state.is_in_auto_mode) { - state |= UAVIONIX_ADSB_OUT_DYNAMIC_STATE_AUTOPILOT_ENABLED; - } - if (!_frontend.out_state.is_flying) { - state |= UAVIONIX_ADSB_OUT_DYNAMIC_STATE_ON_GROUND; - } - - // TODO: confirm this sets utcTime correctly - const uint64_t gps_time = gps.time_epoch_usec(); - const uint32_t utcTime = gps_time / 1000000ULL; - - const AP_Baro &baro = AP::baro(); - int32_t altPres = INT_MAX; - if (baro.healthy()) { - // 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; - } - - - - mavlink_msg_uavionix_adsb_out_dynamic_send( - chan, - utcTime, - latitude, - longitude, - altGNSS, - fixType, - numSats, - altPres, - accHoriz, - accVert, - accVel, - velVert, - nsVog, - ewVog, - emStatus, - state, - squawk); -} - - -/* - * 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 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 -{ - // 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 = icao_id & 0x00FFFFFF; - - encoded_icao &= ~0x20000000; // useGnssAltitude should always be FALSE - encoded_icao |= 0x10000000; // csidLogic should always be TRUE - - //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 &= ~0x0C000000; // SIL should always be FALSE - - 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 - * 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. - */ -uint8_t AP_ADSB_uAvionix_MAVLink::get_encoded_callsign_null_char() -{ -// Encoding of the 8bit null char -// (LSB) - knots -// bit.1 - knots -// bit.2 - knots -// bit.3 - (unused) -// bit.4 - flag - ADSB_BITBASK_RF_CAPABILITIES_1090ES_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 -// (MSB) - (unused) - - uint8_t encoded_null = 0; - - if (_frontend.out_state.cfg.maxAircraftSpeed_knots <= 0) { - // not set or unknown. no bits set - } else if (_frontend.out_state.cfg.maxAircraftSpeed_knots <= 75) { - encoded_null |= 0x01; - } else if (_frontend.out_state.cfg.maxAircraftSpeed_knots <= 150) { - encoded_null |= 0x02; - } else if (_frontend.out_state.cfg.maxAircraftSpeed_knots <= 300) { - encoded_null |= 0x03; - } else if (_frontend.out_state.cfg.maxAircraftSpeed_knots <= 600) { - encoded_null |= 0x04; - } else if (_frontend.out_state.cfg.maxAircraftSpeed_knots <= 1200) { - encoded_null |= 0x05; - } else { - encoded_null |= 0x06; - } - - - if (_frontend.out_state.cfg.rf_capable & ADSB_BITBASK_RF_CAPABILITIES_1090ES_IN) { - encoded_null |= 0x10; - } - if (_frontend.out_state.cfg.rf_capable & ADSB_BITBASK_RF_CAPABILITIES_UAT_IN) { - 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) - 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 it should be left blank (all 0's) - */ - - // using the above logic, we must always assign the squawk. once we get configured - // 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); - memset(&_frontend.out_state.cfg.callsign[4], 0, 5); // clear remaining 5 chars - encoded_null |= 0x40; - - return encoded_null; -} - -/* - * populate and send MAVLINK_MSG_UAVIONIX_ADSB_OUT_CFG - */ -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. - // Here we temporarily set some flags in that null char to signify the callsign - // may be a flightplanID instead - int8_t callsign[sizeof(_frontend.out_state.cfg.callsign)]; - uint32_t icao; - - memcpy(callsign, _frontend.out_state.cfg.callsign, sizeof(_frontend.out_state.cfg.callsign)); - - if (_frontend.out_state.cfg.was_set_externally) { - // take values as-is - icao = _frontend.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 = encode_icao((uint32_t)_frontend.out_state.cfg.ICAO_id); - } - - mavlink_msg_uavionix_adsb_out_cfg_send( - chan, - icao, - (const char*)callsign, - (uint8_t)_frontend.out_state.cfg.emitterType, - (uint8_t)_frontend.out_state.cfg.lengthWidth, - (uint8_t)_frontend.out_state.cfg.gpsOffsetLat, - (uint8_t)_frontend.out_state.cfg.gpsOffsetLon, - _frontend.out_state.cfg.stall_speed_cm, - (uint8_t)_frontend.out_state.cfg.rfSelect); -} - -#endif // HAL_ADSB_ENABLED +/* + 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 . + */ + +#include "AP_ADSB_uAvionix_MAVLink.h" + +#if HAL_ADSB_UAVIONIX_MAVLINK_ENABLED +#include // for sprintf +#include +#include +#include +#include +#include + +#define ADSB_CHAN_TIMEOUT_MS 15000 + + +extern const AP_HAL::HAL& hal; + +// detect if an port is configured as MAVLink +bool AP_ADSB_uAvionix_MAVLink::detect() +{ + // 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 + return true; +} + +void AP_ADSB_uAvionix_MAVLink::update() +{ + const uint32_t now = AP_HAL::millis(); + + // 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) { + // haven't gotten a heartbeat health status packet in a while, assume hardware failure + // TODO: reset out_state.chan + _frontend.out_state.chan = -1; + 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) { + 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)) { + _frontend.out_state.last_config_ms = now; + send_configure(chan); + } // last_config_ms + + // send dynamic data to transceiver at 5Hz + if (now - _frontend.out_state.last_report_ms >= 200 && HAVE_PAYLOAD_SPACE(chan, UAVIONIX_ADSB_OUT_DYNAMIC)) { + _frontend.out_state.last_report_ms = now; + send_dynamic_out(chan); + } // last_report_ms + } // chan_last_ms +} + +void AP_ADSB_uAvionix_MAVLink::send_dynamic_out(const mavlink_channel_t chan) const +{ + const AP_GPS &gps = AP::gps(); + const Vector3f &gps_velocity = gps.velocity(); + + const int32_t latitude = _frontend._my_loc.lat; + const int32_t longitude = _frontend._my_loc.lng; + 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 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 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 numSats = gps.num_sats(); + const uint16_t squawk = _frontend.out_state.cfg.squawk_octal; + + uint32_t accHoriz = UINT_MAX; + float accHoriz_f; + if (gps.horizontal_accuracy(accHoriz_f)) { + accHoriz = accHoriz_f * 1E3; // convert m to mm + } + + uint16_t accVert = USHRT_MAX; + float accVert_f; + if (gps.vertical_accuracy(accVert_f)) { + accVert = accVert_f * 1E2; // convert m to cm + } + + uint16_t accVel = USHRT_MAX; + float accVel_f; + if (gps.speed_accuracy(accVel_f)) { + accVel = accVel_f * 1E3; // convert m/s to mm/s + } + + uint16_t state = 0; + if (_frontend.out_state.is_in_auto_mode) { + state |= UAVIONIX_ADSB_OUT_DYNAMIC_STATE_AUTOPILOT_ENABLED; + } + if (!_frontend.out_state.is_flying) { + state |= UAVIONIX_ADSB_OUT_DYNAMIC_STATE_ON_GROUND; + } + + // TODO: confirm this sets utcTime correctly + const uint64_t gps_time = gps.time_epoch_usec(); + const uint32_t utcTime = gps_time / 1000000ULL; + + const AP_Baro &baro = AP::baro(); + int32_t altPres = INT_MAX; + if (baro.healthy()) { + // 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; + } + + + + mavlink_msg_uavionix_adsb_out_dynamic_send( + chan, + utcTime, + latitude, + longitude, + altGNSS, + fixType, + numSats, + altPres, + accHoriz, + accVert, + accVel, + velVert, + nsVog, + ewVog, + emStatus, + state, + squawk); +} + + +/* + * 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 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 +{ + // 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 = icao_id & 0x00FFFFFF; + + encoded_icao &= ~0x20000000; // useGnssAltitude should always be FALSE + encoded_icao |= 0x10000000; // csidLogic should always be TRUE + + //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 &= ~0x0C000000; // SIL should always be FALSE + + 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 + * 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. + */ +uint8_t AP_ADSB_uAvionix_MAVLink::get_encoded_callsign_null_char() +{ +// Encoding of the 8bit null char +// (LSB) - knots +// bit.1 - knots +// bit.2 - knots +// bit.3 - (unused) +// bit.4 - flag - ADSB_BITBASK_RF_CAPABILITIES_1090ES_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 +// (MSB) - (unused) + + uint8_t encoded_null = 0; + + if (_frontend.out_state.cfg.maxAircraftSpeed_knots <= 0) { + // not set or unknown. no bits set + } else if (_frontend.out_state.cfg.maxAircraftSpeed_knots <= 75) { + encoded_null |= 0x01; + } else if (_frontend.out_state.cfg.maxAircraftSpeed_knots <= 150) { + encoded_null |= 0x02; + } else if (_frontend.out_state.cfg.maxAircraftSpeed_knots <= 300) { + encoded_null |= 0x03; + } else if (_frontend.out_state.cfg.maxAircraftSpeed_knots <= 600) { + encoded_null |= 0x04; + } else if (_frontend.out_state.cfg.maxAircraftSpeed_knots <= 1200) { + encoded_null |= 0x05; + } else { + encoded_null |= 0x06; + } + + + if (_frontend.out_state.cfg.rf_capable & ADSB_BITBASK_RF_CAPABILITIES_1090ES_IN) { + encoded_null |= 0x10; + } + if (_frontend.out_state.cfg.rf_capable & ADSB_BITBASK_RF_CAPABILITIES_UAT_IN) { + 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) + 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 it should be left blank (all 0's) + */ + + // using the above logic, we must always assign the squawk. once we get configured + // 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); + memset(&_frontend.out_state.cfg.callsign[4], 0, 5); // clear remaining 5 chars + encoded_null |= 0x40; + + return encoded_null; +} + +/* + * populate and send MAVLINK_MSG_UAVIONIX_ADSB_OUT_CFG + */ +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. + // Here we temporarily set some flags in that null char to signify the callsign + // may be a flightplanID instead + int8_t callsign[sizeof(_frontend.out_state.cfg.callsign)]; + uint32_t icao; + + memcpy(callsign, _frontend.out_state.cfg.callsign, sizeof(_frontend.out_state.cfg.callsign)); + + if (_frontend.out_state.cfg.was_set_externally) { + // take values as-is + icao = _frontend.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 = encode_icao((uint32_t)_frontend.out_state.cfg.ICAO_id); + } + + mavlink_msg_uavionix_adsb_out_cfg_send( + chan, + icao, + (const char*)callsign, + (uint8_t)_frontend.out_state.cfg.emitterType, + (uint8_t)_frontend.out_state.cfg.lengthWidth, + (uint8_t)_frontend.out_state.cfg.gpsOffsetLat, + (uint8_t)_frontend.out_state.cfg.gpsOffsetLon, + _frontend.out_state.cfg.stall_speed_cm, + (uint8_t)_frontend.out_state.cfg.rfSelect); +} + +#endif // HAL_ADSB_ENABLED diff --git a/libraries/AP_ADSB/AP_ADSB_uAvionix_MAVLink.h b/libraries/AP_ADSB/AP_ADSB_uAvionix_MAVLink.h index 6e1f699af6..59ec7f3d7e 100644 --- a/libraries/AP_ADSB/AP_ADSB_uAvionix_MAVLink.h +++ b/libraries/AP_ADSB/AP_ADSB_uAvionix_MAVLink.h @@ -1,44 +1,44 @@ -#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 . - */ - -#include "AP_ADSB_Backend.h" - -#ifndef HAL_ADSB_UAVIONIX_MAVLINK_ENABLED -#define HAL_ADSB_UAVIONIX_MAVLINK_ENABLED HAL_ADSB_ENABLED -#endif - -#if HAL_ADSB_UAVIONIX_MAVLINK_ENABLED -class AP_ADSB_uAvionix_MAVLink : public AP_ADSB_Backend { -public: - using AP_ADSB_Backend::AP_ADSB_Backend; - - void update() override; - - // static detection function - static bool detect(); - -private: - // 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) const; - - // special helpers for uAvionix workarounds - uint32_t encode_icao(const uint32_t icao_id) const; - uint8_t get_encoded_callsign_null_char(void); -}; -#endif // HAL_ADSB_ENABLED - +#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 . + */ + +#include "AP_ADSB_Backend.h" + +#ifndef HAL_ADSB_UAVIONIX_MAVLINK_ENABLED +#define HAL_ADSB_UAVIONIX_MAVLINK_ENABLED HAL_ADSB_ENABLED +#endif + +#if HAL_ADSB_UAVIONIX_MAVLINK_ENABLED +class AP_ADSB_uAvionix_MAVLink : public AP_ADSB_Backend { +public: + using AP_ADSB_Backend::AP_ADSB_Backend; + + void update() override; + + // static detection function + static bool detect(); + +private: + // 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) const; + + // special helpers for uAvionix workarounds + uint32_t encode_icao(const uint32_t icao_id) const; + uint8_t get_encoded_callsign_null_char(void); +}; +#endif // HAL_ADSB_ENABLED +