From b9918cd0d5c0b30b68bc4bfc79c0e8a4ccdb7c8a Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Tue, 27 Jul 2021 15:48:53 -0700 Subject: [PATCH] AP_ADSB: added Ping200X driver /w UCP protocol --- libraries/AP_ADSB/AP_ADSB.cpp | 24 +- libraries/AP_ADSB/AP_ADSB.h | 34 + .../AP_ADSB/AP_ADSB_uAvionix_MAVLink.cpp | 3 - libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.cpp | 425 ++++++++++++ libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.h | 91 +++ .../GDL90_protocol/GDL90_Message_Structs.h | 615 ++++++++++++++++++ .../AP_ADSB/GDL90_protocol/hostGDL90Support.h | 83 +++ 7 files changed, 1270 insertions(+), 5 deletions(-) create mode 100644 libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.cpp create mode 100644 libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.h create mode 100644 libraries/AP_ADSB/GDL90_protocol/GDL90_Message_Structs.h create mode 100644 libraries/AP_ADSB/GDL90_protocol/hostGDL90Support.h diff --git a/libraries/AP_ADSB/AP_ADSB.cpp b/libraries/AP_ADSB/AP_ADSB.cpp index f82aa4e004..61c65fcbf5 100644 --- a/libraries/AP_ADSB/AP_ADSB.cpp +++ b/libraries/AP_ADSB/AP_ADSB.cpp @@ -24,6 +24,7 @@ #if HAL_ADSB_ENABLED #include "AP_ADSB_uAvionix_MAVLink.h" +#include "AP_ADSB_uAvionix_UCP.h" #include "AP_ADSB_Sagetech.h" #include #include @@ -54,7 +55,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,2:Sagetech + // @Values: 0:Disabled,1:uAvionix-MAVLink,2:Sagetech,3:uAvionix-UCP // @User: Standard // @RebootRequired: True AP_GROUPINFO_FLAGS("TYPE", 0, AP_ADSB, _type[0], 0, AP_PARAM_FLAG_ENABLE), @@ -80,7 +81,7 @@ const AP_Param::GroupInfo AP_ADSB::var_info[] = { // @Param: ICAO_ID // @DisplayName: ICAO_ID vehicle identification number - // @Description: ICAO_ID unique vehicle identification number of this aircraft. This is a integer limited to 24bits. If set to 0 then one will be randomly generated. If set to -1 then static information is not sent, transceiver is assumed pre-programmed. + // @Description: ICAO_ID unique vehicle identification number of this aircraft. This is an integer limited to 24bits. If set to 0 then one will be randomly generated. If set to -1 then static information is not sent, transceiver is assumed pre-programmed. // @Range: -1 16777215 // @User: Advanced AP_GROUPINFO("ICAO_ID", 4, AP_ADSB, out_state.cfg.ICAO_id_param, 0), @@ -156,6 +157,12 @@ const AP_Param::GroupInfo AP_ADSB::var_info[] = { // @User: Advanced AP_GROUPINFO("LOG", 14, AP_ADSB, _log, 1), + // @Param: OPTIONS + // @DisplayName: ADS-B Options + // @Bitmask: 0:Ping200X Send GPS,1:Squawk 7400 on RC failsafe,2:Squawk 7400 on GCS failsafe + // @User: Advanced + AP_GROUPINFO("OPTIONS", 15, AP_ADSB, _options, 0), + AP_GROUPEND }; @@ -250,17 +257,30 @@ void AP_ADSB::detect_instance(uint8_t instance) return; case Type::uAvionix_MAVLink: +#if HAL_ADSB_UAVIONIX_MAVLINK_ENABLED if (AP_ADSB_uAvionix_MAVLink::detect()) { _backend[instance] = new AP_ADSB_uAvionix_MAVLink(*this, instance); return; } +#endif + break; + + case Type::uAvionix_UCP: +#if HAL_ADSB_UCP_ENABLED + if (AP_ADSB_uAvionix_UCP::detect()) { + _backend[instance] = new AP_ADSB_uAvionix_UCP(*this, instance); + return; + } +#endif break; case Type::Sagetech: +#if HAL_ADSB_SAGETECH_ENABLED if (AP_ADSB_Sagetech::detect()) { _backend[instance] = new AP_ADSB_Sagetech(*this, instance); return; } +#endif break; } } diff --git a/libraries/AP_ADSB/AP_ADSB.h b/libraries/AP_ADSB/AP_ADSB.h index 11353f08fb..11ba531b72 100644 --- a/libraries/AP_ADSB/AP_ADSB.h +++ b/libraries/AP_ADSB/AP_ADSB.h @@ -36,12 +36,16 @@ #define ADSB_MAX_INSTANCES 1 // Maximum number of ADSB sensor instances available on this platform +#define ADSB_BITBASK_RF_CAPABILITIES_UAT_IN (1 << 0) +#define ADSB_BITBASK_RF_CAPABILITIES_1090ES_IN (1 << 1) + class AP_ADSB_Backend; class AP_ADSB { public: friend class AP_ADSB_Backend; friend class AP_ADSB_uAvionix_MAVLink; + friend class AP_ADSB_uAvionix_UCP; friend class AP_ADSB_Sagetech; // constructor @@ -61,6 +65,7 @@ public: None = 0, uAvionix_MAVLink = 1, Sagetech = 2, + uAvionix_UCP = 3, }; struct adsb_vehicle_t { @@ -68,6 +73,13 @@ public: uint32_t last_update_ms; // last time this was refreshed, allows timeouts }; + // enum for adsb optional features + enum class AdsbOption { + Ping200X_Send_GPS = (1<<0), + Squawk_7400_FS_RC = (1<<1), + Squawk_7400_FS_GCS = (1<<2), + }; + // for holding parameters static const struct AP_Param::GroupInfo var_info[]; @@ -138,6 +150,21 @@ public: // confirm a value is a valid callsign static bool is_valid_callsign(uint16_t octal) WARN_IF_UNUSED; + // Mode-S IDENT is active. While true, we are currently a large "HEY LOOK AT ME" symbol on the Air Traffic Controllers' radar screen. + bool ident_is_active() const { + return out_state.ident_is_active; + } + + // Trigger a Mode 3/A transponder IDENT. This should only be done when requested to do so by an Air Traffic Controller. + // See wikipedia for IDENT explaination https://en.wikipedia.org/wiki/Transponder_(aeronautics) + bool ident_start() { + if (ident_is_active() || !healthy() || ((out_state.cfg.rfSelect & UAVIONIX_ADSB_OUT_RF_SELECT_TX_ENABLED) == 0)) { + return false; + } + out_state.ident_pending = true; + return true; + } + AP_ADSB::Type get_type(uint8_t instance) const; private: @@ -209,6 +236,11 @@ private: bool is_flying; bool is_in_auto_mode; + // Mode 3/A transponder IDENT. This triggers, or shows status of, an active IDENT status should only be done when requested to do so by an Air Traffic Controller. + // See wikipedia for IDENT explaination https://en.wikipedia.org/wiki/Transponder_(aeronautics) + bool ident_pending; + bool ident_is_active; + // ADSB-OUT configuration struct { int32_t ICAO_id; @@ -235,6 +267,8 @@ private: // special ICAO of interest that ignored filters when != 0 AP_Int32 _special_ICAO_target; + AP_Int32 _options; + static const uint8_t _max_samples = 30; ObjectBuffer _samples{_max_samples}; diff --git a/libraries/AP_ADSB/AP_ADSB_uAvionix_MAVLink.cpp b/libraries/AP_ADSB/AP_ADSB_uAvionix_MAVLink.cpp index 4162f03cec..b364b2a6f2 100644 --- a/libraries/AP_ADSB/AP_ADSB_uAvionix_MAVLink.cpp +++ b/libraries/AP_ADSB/AP_ADSB_uAvionix_MAVLink.cpp @@ -25,9 +25,6 @@ #define ADSB_CHAN_TIMEOUT_MS 15000 -#define ADSB_BITBASK_RF_CAPABILITIES_UAT_IN (1 << 0) -#define ADSB_BITBASK_RF_CAPABILITIES_1090ES_IN (1 << 1) - extern const AP_HAL::HAL& hal; diff --git a/libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.cpp b/libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.cpp new file mode 100644 index 0000000000..55b1882353 --- /dev/null +++ b/libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.cpp @@ -0,0 +1,425 @@ +/* + Copyright (C) 2021 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 . + + Author: Tom Pittenger + */ + +#include "AP_ADSB_uAvionix_UCP.h" + +#if HAL_ADSB_UCP_ENABLED + +#include +#include +#include +#include +#include +#include +#include + +extern const AP_HAL::HAL &hal; + +#define AP_ADSB_UAVIONIX_GCS_LOST_COMMS_LONG_TIMEOUT_MINUTES (15UL) +#define AP_ADSB_UAVIONIX_GCS_LOST_COMMS_LONG_TIMEOUT_MS (1000UL * 60UL * AP_ADSB_UAVIONIX_GCS_LOST_COMMS_LONG_TIMEOUT_MINUTES) + +#define AP_ADSB_UAVIONIX_DETECT_GROUNDSTATE 0 +#define AP_ADSB_UAVIONIX_ALLOW_USING_AUTOPILOT_BARO 0 +#define AP_ADSB_UAVIONIX_EMERGENCY_STATUS_ON_LOST_LINK 0 + +// detect if any port is configured as uAvionix_UCP +bool AP_ADSB_uAvionix_UCP::detect() +{ + return (AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_ADSB, 0) != nullptr); +} + + +// Init, called once after class is constructed +bool AP_ADSB_uAvionix_UCP::init() +{ + _port = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_ADSB, 0); + if (_port == nullptr) { + return false; + } + + request_msg(GDL90_ID_IDENTIFICATION); + return true; +} + + +void AP_ADSB_uAvionix_UCP::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(), 10UL * GDL90_RX_MAX_PACKET_LENGTH); + while (nbytes-- > 0) { + const int16_t data = (uint8_t)_port->read(); + if (data < 0) { + break; + } + + if (parseByte((uint8_t)data, rx.msg, rx.status)) { + rx.last_msg_ms = now_ms; + handle_msg(rx.msg); + } + } // while nbytes + + if (now_ms - run_state.last_packet_Transponder_Control_ms >= 1000) { + run_state.last_packet_Transponder_Control_ms = now_ms; + send_Transponder_Control(); + } + + if ((now_ms - run_state.last_packet_GPS_ms >= 200) && (_frontend._options & uint32_t(AP_ADSB::AdsbOption::Ping200X_Send_GPS)) != 0) { + run_state.last_packet_GPS_ms = now_ms; + send_GPS_Data(); + } +} + + +void AP_ADSB_uAvionix_UCP::handle_msg(const GDL90_RX_MESSAGE &msg) +{ + switch(msg.messageId) { + case GDL90_ID_HEARTBEAT: + // The Heartbeat message provides real-time indications of the status and operation of the + // transponder. The message will be transmitted with a period of one second for the UCP + // protocol. + memcpy(&rx.decoded.heartbeat, msg.raw, sizeof(rx.decoded.heartbeat)); + _frontend.out_state.ident_is_active = rx.decoded.heartbeat.status.one.ident; + if (rx.decoded.heartbeat.status.one.ident) { + // if we're identing, clear the pending send request + _frontend.out_state.ident_pending = false; + } + break; + + case GDL90_ID_IDENTIFICATION: + // The Identification message contains information used to identify the connected device. The + // Identification message will be transmitted with a period of one second regardless of data status + // or update for the UCP protocol and will be transmitted upon request for the UCP-HD protocol. + if (memcmp(&rx.decoded.identification, msg.raw, sizeof(rx.decoded.identification)) != 0) { + memcpy(&rx.decoded.identification, msg.raw, sizeof(rx.decoded.identification)); + + // Firmware Part Number (not null terminated, but null padded if part number is less than 15 characters). + // Copy into a temporary string that is 1 char longer so we ensure it's null terminated + const uint8_t str_len = sizeof(rx.decoded.identification.primaryFwPartNumber); + char primaryFwPartNumber[str_len+1]; + memcpy(&primaryFwPartNumber, rx.decoded.identification.primaryFwPartNumber, str_len); + primaryFwPartNumber[str_len] = 0; + + GCS_SEND_TEXT(MAV_SEVERITY_DEBUG,"ADSB:Detected %s v%u.%u.%u SN:%u %s", + get_hardware_name(rx.decoded.identification.primary.hwId), + (unsigned)rx.decoded.identification.primary.fwMajorVersion, + (unsigned)rx.decoded.identification.primary.fwMinorVersion, + (unsigned)rx.decoded.identification.primary.fwBuildVersion, + (unsigned)rx.decoded.identification.primary.serialNumber, + primaryFwPartNumber); + } + break; + + case GDL90_ID_TRANSPONDER_CONFIG: + memcpy(&rx.decoded.transponder_config, msg.raw, sizeof(rx.decoded.transponder_config)); + break; + +#if AP_ADSB_UAVIONIX_UCP_CAPTURE_ALL_RX_PACKETS + case GDL90_ID_OWNSHIP_REPORT: + // The Ownship message contains information on the GNSS position. If the Ownship GNSS + // position fix is invalid, the Latitude, Longitude, and NIC fields will all have the ZERO value. The + // Ownship message will be transmitted with a period of one second regardless of data status or + // update for the UCP protocol. All fields in the ownship message are transmitted MSB first + memcpy(&rx.decoded.ownship_report, msg.raw, sizeof(rx.decoded.ownship_report)); + break; + + case GDL90_ID_OWNSHIP_GEOMETRIC_ALTITUDE: + // An Ownship Geometric Altitude message will be transmitted with a period of one second when + // the GNSS fix is valid for the UCP protocol. All fields in the Geometric Ownship Altitude + // message are transmitted MSB first. + memcpy(&rx.decoded.ownship_geometric_altitude, msg.raw, sizeof(rx.decoded.ownship_geometric_altitude)); + break; + + case GDL90_ID_SENSOR_MESSAGE: + memcpy(&rx.decoded.sensor_message, msg.raw, sizeof(rx.decoded.sensor_message)); + break; + + case GDL90_ID_TRANSPONDER_STATUS: + memcpy(&rx.decoded.transponder_status, msg.raw, sizeof(rx.decoded.transponder_status)); + break; +#endif // AP_ADSB_UAVIONIX_UCP_CAPTURE_ALL_RX_PACKETS + + case GDL90_ID_TRANSPONDER_CONTROL: + case GDL90_ID_GPS_DATA: + case GDL90_ID_MESSAGE_REQUEST: + // not handled, outbound only + break; + default: + //GCS_SEND_TEXT(MAV_SEVERITY_DEBUG,"ADSB:Unknown msg %d", (int)msg.messageId); + break; + } +} + + +const char* AP_ADSB_uAvionix_UCP::get_hardware_name(const uint8_t hwId) +{ + switch(hwId) { + case 0x09: return "Ping200s"; + case 0x0A: return "Ping20s"; + case 0x18: return "Ping200C"; + case 0x27: return "Ping20Z"; + case 0x2D: return "SkyBeaconX"; // (certified) + case 0x26: //return "Ping200Z/Ping200X"; // (uncertified). Let's fallthrough and use Ping200X + case 0x2F: return "Ping200X"; // (certified) + case 0x30: return "TailBeaconX"; // (certified) + } // switch hwId + return "Unknown HW"; +} + +void AP_ADSB_uAvionix_UCP::send_Transponder_Control() +{ + GDL90_TRANSPONDER_CONTROL_MSG msg {}; + msg.messageId = GDL90_ID_TRANSPONDER_CONTROL; + msg.version = 1; + +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + // when using the simulator, always declare we're on the ground to help + // inhibit chaos if this ias actually being broadcasted on real hardware + msg.airGroundState = ADSB_ON_GROUND; +#elif AP_ADSB_UAVIONIX_DETECT_GROUNDSTATE + msg.airGroundState = _frontend.out_state.is_flying ? ADSB_AIRBORNE_SUBSONIC : ADSB_ON_GROUND; +#else + msg.airGroundState = ADSB_AIRBORNE_SUBSONIC; +#endif + + msg.baroCrossChecked = ADSB_NIC_BARO_UNVERIFIED; + msg.identActive = _frontend.out_state.ident_pending && !_frontend.out_state.ident_is_active; // set when pending via user but not already active + msg.modeAEnabled = true; + msg.modeCEnabled = true; + msg.modeSEnabled = true; + msg.es1090TxEnabled = (_frontend.out_state.cfg.rfSelect & UAVIONIX_ADSB_OUT_RF_SELECT_TX_ENABLED) != 0; + +#if AP_ADSB_UAVIONIX_ALLOW_USING_AUTOPILOT_BARO + if ((rx.decoded.transponder_config.baroAltSource == GDL90_BARO_DATA_SOURCE_EXTERNAL) && AP::baro().healthy()) { + msg.externalBaroAltitude_mm = AP::baro().get_altitude() * 1000; // convert m to mm + } else +#endif + { + msg.externalBaroAltitude_mm = INT32_MAX; + } + + // if enabled via param ADSB_OPTIONS, use squawk 7400 while in any Loss-Comms related failsafe + // https://www.faa.gov/documentLibrary/media/Notice/N_JO_7110.724_5-2-9_UAS_Lost_Link_2.pdf + const AP_Notify& notify = AP::notify(); + if (((_frontend._options & uint32_t(AP_ADSB::AdsbOption::Squawk_7400_FS_RC)) && notify.flags.failsafe_radio) || + ((_frontend._options & uint32_t(AP_ADSB::AdsbOption::Squawk_7400_FS_GCS)) && notify.flags.failsafe_gcs)) { + msg.squawkCode = 7400; + } else { + msg.squawkCode = _frontend.out_state.cfg.squawk_octal; + } + +#if AP_ADSB_UAVIONIX_EMERGENCY_STATUS_ON_LOST_LINK + const uint32_t last_gcs_ms = gcs().sysid_myggcs_last_seen_time_ms(); + const bool gcs_lost_comms = (last_gcs_ms != 0) && (AP_HAL::millis() - last_gcs_ms > AP_ADSB_UAVIONIX_GCS_LOST_COMMS_LONG_TIMEOUT_MS); + msg.emergencyState = gcs_lost_comms ? ADSB_EMERGENCY_STATUS::ADSB_EMERGENCY_UAS_LOST_LINK : ADSB_EMERGENCY_STATUS::ADSB_EMERGENCY_NONE; +#else + msg.emergencyState = ADSB_EMERGENCY_STATUS::ADSB_EMERGENCY_NONE; +#endif + + memcpy(msg.callsign, _frontend.out_state.cfg.callsign, sizeof(msg.callsign)); + + gdl90Transmit((GDL90_TX_MESSAGE&)msg, sizeof(msg)); +} + + +void AP_ADSB_uAvionix_UCP::send_GPS_Data() +{ + GDL90_GPS_DATA_V2 msg {}; + msg.messageId = GDL90_ID_GPS_DATA; + msg.version = 2; + + const AP_GPS &gps = AP::gps(); + const GPS_FIX fix = (GPS_FIX)gps.status(); + const bool fix_is_good = (fix >= GPS_FIX_3D); + const Vector3f velocity = fix_is_good ? gps.velocity() : Vector3f(); + + msg.utcTime_s = gps.time_epoch_usec() * 1E-6; + msg.latitude_ddE7 = fix_is_good ? _frontend._my_loc.lat : INT32_MAX; + msg.longitude_ddE7 = fix_is_good ? _frontend._my_loc.lng : INT32_MAX; + msg.altitudeGnss_mm = fix_is_good ? _frontend._my_loc.alt : INT32_MAX; + + // Protection Limits. FD or SBAS-based depending on state bits + msg.HPL_mm = UINT32_MAX; + msg.VPL_cm = UINT32_MAX; + + // Figure of Merits + float accHoriz; + msg.horizontalFOM_mm = gps.horizontal_accuracy(accHoriz) ? accHoriz * 1E3 : UINT32_MAX; + float accVert; + msg.verticalFOM_cm = gps.vertical_accuracy(accVert) ? accVert * 1E2 : UINT16_MAX; + float accVel; + msg.horizontalVelocityFOM_mmps = gps.speed_accuracy(accVel) ? accVel * 1E3 : UINT16_MAX; + msg.verticalVelocityFOM_mmps = msg.horizontalVelocityFOM_mmps; + + // Velocities + msg.verticalVelocity_cmps = fix_is_good ? velocity.z * 1E2 : INT16_MAX; + msg.northVelocity_mmps = fix_is_good ? velocity.x * 1E3 : INT32_MAX; + msg.eastVelocity_mmps = fix_is_good ? velocity.y * 1E3 : INT32_MAX; + + // State + msg.fixType = fix; + + GDL90_GPS_NAV_STATE nav_state {}; + nav_state.HPLfdeActive = 1; + nav_state.fault = 0; + nav_state.HrdMagNorth = 0; // 1 means "north" is magnetic north + + msg.navState = nav_state; + msg.satsUsed = AP::gps().num_sats(); + + gdl90Transmit((GDL90_TX_MESSAGE&)msg, sizeof(msg)); +} + + +bool AP_ADSB_uAvionix_UCP::hostTransmit(uint8_t *buffer, uint16_t length) +{ + if (_port == nullptr || _port->txspace() < length) { + return false; + } + _port->write(buffer, length); + return true; +} + + +bool AP_ADSB_uAvionix_UCP::request_msg(const GDL90_MESSAGE_ID msg_id) +{ + const GDL90_TRANSPONDER_MESSAGE_REQUEST_V2 msg { + messageId : GDL90_ID_MESSAGE_REQUEST, + version : 2, + reqMsgId : msg_id + }; + return gdl90Transmit((GDL90_TX_MESSAGE&)msg, sizeof(msg)) != 0; +} + + +uint16_t AP_ADSB_uAvionix_UCP::gdl90Transmit(GDL90_TX_MESSAGE &message, const uint16_t length) +{ + uint8_t gdl90FrameBuffer[GDL90_TX_MAX_FRAME_LENGTH] {}; + + const uint16_t frameCrc = crc16_ccitt_GDL90((uint8_t*)&message.raw, length, 0); + + // Set flag byte in frame buffer + gdl90FrameBuffer[0] = GDL90_FLAG_BYTE; + uint16_t frameIndex = 1; + + // Copy and stuff all payload bytes into frame buffer + for (uint16_t i = 0; i < length+2; i++) { + // Check for overflow of frame buffer + if (frameIndex >= GDL90_TX_MAX_FRAME_LENGTH) { + return 0; + } + + uint8_t data; + // Append CRC to payload + if (i == length) { + data = LOWBYTE(frameCrc); + } else if (i == length+1) { + data = HIGHBYTE(frameCrc); + } else { + data = message.raw[i]; + } + + if (data == GDL90_FLAG_BYTE || data == GDL90_CONTROL_ESCAPE_BYTE) { + // Check for frame buffer overflow on stuffed byte + if (frameIndex + 2 > GDL90_TX_MAX_FRAME_LENGTH) { + return 0; + } + + // Set control break and stuff this byte + gdl90FrameBuffer[frameIndex++] = GDL90_CONTROL_ESCAPE_BYTE; + gdl90FrameBuffer[frameIndex++] = data ^ GDL90_STUFF_BYTE; + } else { + gdl90FrameBuffer[frameIndex++] = data; + } + } + + // Add end of frame indication + gdl90FrameBuffer[frameIndex++] = GDL90_FLAG_BYTE; + + // Push packet to UART + if (hostTransmit(gdl90FrameBuffer, frameIndex)) { + return frameIndex; + } + + return 0; +} + + +bool AP_ADSB_uAvionix_UCP::parseByte(const uint8_t data, GDL90_RX_MESSAGE &msg, GDL90_RX_STATUS &status) +{ + switch (status.state) + { + case GDL90_RX_IDLE: + if (data == GDL90_FLAG_BYTE && status.prev_data == GDL90_FLAG_BYTE) { + status.length = 0; + status.state = GDL90_RX_IN_PACKET; + } + break; + + case GDL90_RX_IN_PACKET: + if (data == GDL90_CONTROL_ESCAPE_BYTE) { + status.state = GDL90_RX_UNSTUFF; + + } else if (data == GDL90_FLAG_BYTE) { + // packet complete! Check CRC and restart packet cycle on all pass or fail scenarios + status.state = GDL90_RX_IDLE; + + if (status.length < GDL90_OVERHEAD_LENGTH) { + // something is wrong, there's no actual data + return false; + } + + const uint8_t crc_LSB = msg.raw[status.length - 2]; + const uint8_t crc_MSB = msg.raw[status.length - 1]; + + // NOTE: status.length contains messageId, payload and CRC16. So status.length-3 is effective payload length + msg.crc = (uint16_t)crc_LSB | ((uint16_t)crc_MSB << 8); + const uint16_t crc = crc16_ccitt_GDL90((uint8_t*)&msg.raw, status.length-2, 0); + if (crc == msg.crc) { + status.prev_data = data; + // NOTE: this is the only path that returns true + return true; + } + + } else if (status.length < GDL90_RX_MAX_PACKET_LENGTH) { + msg.raw[status.length++] = data; + + } else { + status.state = GDL90_RX_IDLE; + } + break; + + case GDL90_RX_UNSTUFF: + msg.raw[status.length++] = data ^ GDL90_STUFF_BYTE; + status.state = GDL90_RX_IN_PACKET; + break; + } + status.prev_data = data; + return false; +} + +#endif // HAL_ADSB_UCP_ENABLED + diff --git a/libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.h b/libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.h new file mode 100644 index 0000000000..93fc9f1635 --- /dev/null +++ b/libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.h @@ -0,0 +1,91 @@ +/* + Copyright (C) 2021 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 . + + Author: Tom Pittenger + */ + +#pragma once +#include "AP_ADSB_Backend.h" + +#ifndef HAL_ADSB_UCP_ENABLED +#define HAL_ADSB_UCP_ENABLED HAL_ADSB_ENABLED +#endif + +#if HAL_ADSB_UCP_ENABLED + +#define AP_ADSB_UAVIONIX_UCP_CAPTURE_ALL_RX_PACKETS 1 + + +#include +#include +#include + +#include "GDL90_protocol/GDL90_Message_Structs.h" +#include "GDL90_protocol/hostGDL90Support.h" + +class AP_ADSB_uAvionix_UCP : 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: + + void handle_msg(const GDL90_RX_MESSAGE &msg); + bool request_msg(const GDL90_MESSAGE_ID msg_id); + + void send_GPS_Data(); + void send_Transponder_Control(); + const char* get_hardware_name(const uint8_t hwId); + + bool hostTransmit(uint8_t *buffer, uint16_t length); + uint16_t gdl90Transmit(GDL90_TX_MESSAGE &message, const uint16_t length); + static bool parseByte(const uint8_t data, GDL90_RX_MESSAGE &msg, GDL90_RX_STATUS &status); + + struct { + uint32_t last_msg_ms; + GDL90_RX_MESSAGE msg; + GDL90_RX_STATUS status; + + // cache local copies so we always have the latest info of everything. + struct { + GDL90_IDENTIFICATION_V3 identification; + GDL90_TRANSPONDER_CONFIG_MSG_V4_V5 transponder_config; + GDL90_HEARTBEAT heartbeat; +#if AP_ADSB_UAVIONIX_UCP_CAPTURE_ALL_RX_PACKETS + GDL90_OWNSHIP_REPORT ownship_report; + GDL90_OWNSHIP_GEO_ALTITUDE ownship_geometric_altitude; + GDL90_SENSOR_BARO_MESSAGE sensor_message; + GDL90_TRANSPONDER_STATUS_MSG transponder_status; +#endif + } decoded; + } rx; + + struct { + uint32_t last_packet_GPS_ms; + uint32_t last_packet_Transponder_Control_ms; + } run_state; + +}; +#endif // HAL_ADSB_UCP_ENABLED + diff --git a/libraries/AP_ADSB/GDL90_protocol/GDL90_Message_Structs.h b/libraries/AP_ADSB/GDL90_protocol/GDL90_Message_Structs.h new file mode 100644 index 0000000000..d89bd7fefa --- /dev/null +++ b/libraries/AP_ADSB/GDL90_protocol/GDL90_Message_Structs.h @@ -0,0 +1,615 @@ +/* + Copyright (C) 2021 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 . + + + Author: GDL90/UCP protocol by uAvionix, 2021. + Implemented by: Tom Pittenger + */ + +#include +#include +#include + +typedef enum __attribute__((__packed__)) +{ + GDL90_ID_HEARTBEAT = 0, + GDL90_ID_OWNSHIP_REPORT = 10, // 0x0A + GDL90_ID_OWNSHIP_GEOMETRIC_ALTITUDE = 11, // 0x0B + GDL90_ID_IDENTIFICATION = 37, // 0x25 + GDL90_ID_SENSOR_MESSAGE = 40, // 0x28 + GDL90_ID_TRANSPONDER_CONFIG = 43, // 0x2B + GDL90_ID_MESSAGE_REQUEST = 44, // 0x2C + GDL90_ID_TRANSPONDER_CONTROL = 45, // 0x2D + GDL90_ID_GPS_DATA = 46, // 0x2E + GDL90_ID_TRANSPONDER_STATUS = 47, // 0x2F +} GDL90_MESSAGE_ID; + +typedef enum __attribute__((__packed__)) +{ + ADSB_NIC_BARO_UNVERIFIED = 0, // Baro is Gilman bases, and not cross checked + ADSB_NIC_BARO_VERIFIED = 1, // Baro is cross-checked, or not Gilman based +} ADSB_NIC_BARO; // Barometric Altitude Integrity Code + +typedef enum __attribute__((__packed__)) +{ + ADSB_AIRBORNE_SUBSONIC = 0, + ADSB_AIRBORNE_SUPERSONIC = 1, + ADSB_ON_GROUND = 2, + // 3 Reserved +} ADSB_AIR_GROUND_STATE; // Determines how horizontal velocity fields are processed in UAT + +typedef enum __attribute__((__packed__)) +{ + ADSB_EMERGENCY_NONE = 0, + ADSB_EMERGENCY_GENERAL = 1, + ADSB_EMERGENCY_MEDICAL = 2, + ADSB_EMERGENCY_MINIMUM_FUEL = 3, + ADSB_EMERGENCY_NO_COMMUNICATION = 4, + ADSB_EMERGNECY_INTERFERENCE = 5, + ADSB_EMERGENCY_DOWNED_AIRCRAFT = 6, + ADSB_EMERGENCY_UAS_LOST_LINK = 7, + // 7 Reserved +} ADSB_EMERGENCY_STATUS; + +typedef struct __attribute__((__packed__)) +{ + GDL90_MESSAGE_ID messageId; + uint8_t version; + ADSB_NIC_BARO baroCrossChecked : 1; + ADSB_AIR_GROUND_STATE airGroundState : 2; + uint8_t identActive : 1; + uint8_t modeAEnabled : 1; + uint8_t modeCEnabled : 1; + uint8_t modeSEnabled : 1; + uint8_t es1090TxEnabled : 1; + int32_t externalBaroAltitude_mm; + uint16_t squawkCode; + ADSB_EMERGENCY_STATUS emergencyState; + uint8_t callsign[8]; +} GDL90_TRANSPONDER_CONTROL_MSG; + +typedef struct __attribute__((__packed__)) +{ + GDL90_MESSAGE_ID messageId; + uint8_t version; + uint8_t indicatingOnGround : 1; + uint8_t interrogatedSinceLast : 1; + uint8_t fault : 1; + uint8_t identActive : 1; + uint8_t modeAEnabled : 1; + uint8_t modeCEnabled : 1; + uint8_t modeSEnabled : 1; + uint8_t es1090TxEnabled : 1; + uint8_t latitude[3]; + uint8_t longitude[3]; + uint32_t track_Heading : 8; + uint32_t horizontalVelocity :12; + uint32_t altitude :12; + uint16_t squawkCode; + uint8_t NIC : 4; + uint8_t NACp : 4; + uint8_t temperature; + uint16_t crc; +} GDL90_TRANSPONDER_STATUS_MSG; +#define GDL90_TRANSPONDER_STATUS_VERSION (3) +#define GDL90_STATUS_MAX_ALTITUDE_FT (101338) +#define GDL90_STATUS_MIN_ALTITUDE_FT (-1000) + + + +typedef struct __attribute__((__packed__)) +{ + uint8_t HPLfdeActive : 1; + uint8_t fault : 1; + uint8_t HrdMagNorth : 1; + uint8_t reserved : 5; +} GDL90_GPS_NAV_STATE; + +typedef enum __attribute__((__packed__)) +{ + GPS_FIX_NONE = 0, + GPS_FIX_NO_FIX = 1, + GPS_FIX_2D = 2, + GPS_FIX_3D = 3, + GPS_FIX_DIFFERENTIAL = 4, + GPS_FIX_RTK = 5, +} GPS_FIX; + +typedef struct __attribute__((__packed__)) +{ + GDL90_MESSAGE_ID messageId; + uint8_t version; + uint32_t utcTime_s; // Time since GPS epoch + int32_t latitude_ddE7; + int32_t longitude_ddE7; + int32_t altitudeGnss_mm; // Height about WGS84 ellipsoid + // Protection Limits. FD or SBAS-based depending on state bits + uint32_t HPL_mm; + uint32_t VPL_cm; + // FOMS + uint32_t horizontalFOM_mm; + uint16_t verticalFOM_cm; + uint16_t horizontalVelocityFOM_mmps; + uint16_t verticalVelocityFOM_mmps; + // Velocities + int16_t verticalVelocity_cmps; + int32_t northVelocity_mmps; // millimeter/s + int32_t eastVelocity_mmps; + // State + GPS_FIX fixType; + GDL90_GPS_NAV_STATE navState; + uint8_t satsUsed; +} GDL90_GPS_DATA_V2; +#define GDL90_GPS_DATA_VERSION (2) + +typedef struct __attribute__((__packed__)) +{ + GDL90_MESSAGE_ID messageId; + uint8_t version; + GDL90_MESSAGE_ID reqMsgId; +} GDL90_TRANSPONDER_MESSAGE_REQUEST_V2; + +typedef enum __attribute__((__packed__)) +{ + GDL90_BARO_DATA_SOURCE_INTERNAL = 0, + GDL90_BARO_DATA_SOURCE_EXTERNAL, +}GDL90_BARO_DATA_SOURCE; + +typedef enum __attribute__((__packed__)) +{ + ADSB_SDA_UNKNOWN = 0, + ADSB_SDA_10_NEG3 = 1, + ADSB_SDA_10_NEG5 = 2, + ADSB_SDA_10_NEG7 = 3, +} ADSB_SDA; // System Design Assurance + +typedef enum __attribute__((__packed__)) +{ + ADSB_SIL_UNKNOWN = 0, + ADSB_SIL_10_NEG3 = 1, + ADSB_SIL_10_NEG5 = 2, + ADSB_SIL_10_NEG7 = 3, +} ADSB_SIL; // Source Integrity Level + +typedef enum __attribute__((__packed__)) +{ + ADSB_AV_LW_NO_DATA = 0, + ADSB_AV_LW_15M_23M = 1, + ADSB_AV_LW_25M_28P5M = 2, + ADSB_AV_LW_25M_34M = 3, + ADSB_AV_LW_35M_33M = 4, + ADSB_AV_LW_35M_38M = 5, + ADSB_AV_LW_45M_39P5M = 6, + ADSB_AV_LW_45M_45M = 7, + ADSB_AV_LW_55M_45M = 8, + ADSB_AV_LW_55M_52M = 9, + ADSB_AV_LW_65M_59P5M = 10, + ADSB_AV_LW_65M_67M = 11, + ADSB_AV_LW_75M_72P5M = 12, + ADSB_AV_LW_75M_80M = 13, + ADSB_AV_LW_85M_80M = 14, + ADSB_AV_LW_85M_90M = 15, +} ADSB_AIRCRAFT_LENGTH_WIDTH; + +typedef enum __attribute__((__packed__)) +{ + ADSB_NOT_UAT_IN_CAPABLE = 0, + ADSB_UAT_IN_CAPABLE = 1 +} ADSB_UAT_IN_CAPABILITY; + +typedef enum __attribute__((__packed__)) +{ + ADSB_NOT_1090ES_IN_CAPABLE = 0, + ADSB_1090ES_IN_CAPABLE = 1 +} ADSB_1090ES_IN_CAPABILITY; + +typedef enum __attribute__((__packed__)) +{ + ADSB_GPS_LON_NO_DATA = 0, + ADSB_GPS_LON_FROM_SENSOR = 1, + // 2 - 31 valid values in 2 meter increments +} ADSB_GPS_LONGITUDINAL_OFFSET; + +typedef enum __attribute__((__packed__)) +{ + ADSB_GPS_LAT_NO_DATA = 0, + ADSB_GPS_LAT_LEFT_2M = 1, + ADSB_GPS_LAT_LEFT_4M = 2, + ADSB_GPS_LAT_LEFT_6M = 3, + ADSB_GPS_LAT_0M = 4, + ADSB_GPS_LAT_RIGHT_2M = 5, + ADSB_GPS_LAT_RIGHT_4M = 6, + ADSB_GPS_LAT_RIGHT_6M = 7, +} ADSB_GPS_LATERAL_OFFSET; + +typedef enum __attribute__((__packed__)) +{ + ADSB_EMITTER_NO_INFO = 0, + ADSB_EMITTER_LIGHT = 1, + ADSB_EMITTER_SMALL = 2, + ADSB_EMITTER_LARGE = 3, + ADSB_EMITTER_HIGH_VORTEX_LARGE = 4, + ADSB_EMITTER_HEAVY = 5, + ADSB_EMITTER_HIGHLY_MANUV = 6, + ADSB_EMITTER_ROTOCRAFT = 7, + // 8 Unassigned + ADSB_EMITTER_GLIDER = 9, + ADSB_EMITTER_LIGHTER_AIR = 10, + ADSB_EMITTER_PARACHUTE = 11, + ADSB_EMITTER_ULTRA_LIGHT = 12, + // 13 Unassigned + ADSB_EMITTER_UAV = 14, + ADSB_EMITTER_SPACE = 15, + // 16 Unassigned + + // Surface types + ADSB_EMITTER_EMERGENCY_SURFACE = 17, + ADSB_EMITTER_SERVICE_SURFACE = 18, + + // Obstacle types + ADSB_EMITTER_POINT_OBSTACLE = 19, + ADSB_EMITTER_CLUSTER_OBSTACLE = 20, + ADSB_EMITTER_LINE_OBSTACLE = 21, + // 22 - 39 Reserved +} ADSB_EMITTER; // ADSB Emitter Category + +typedef enum __attribute__((__packed__)) +{ + PING_COM_1200_BAUD = 0, + PING_COM_2400_BAUD = 1, + PING_COM_4800_BAUD = 2, + PING_COM_9600_BAUD = 3, + PING_COM_19200_BAUD = 4, + PING_COM_38400_BAUD = 5, + PING_COM_57600_BAUD = 6, + PING_COM_115200_BAUD = 7, + PING_COM_921600_BAUD = 8, +} PING_COM_RATE; + +typedef enum __attribute__((__packed__)) +{ + CONFIG_VALIDITY_ICAO = 1 << 0, + CONFIG_VALIDITY_SIL = 1 << 1, + CONFIG_VALIDITY_SDA = 1 << 2, + CONFIG_VALIDITY_BARO_ALT_SOURCE = 1 << 3, + CONFIG_VALIDITY_AIRCRAFT_MAX_SPEED = 1 << 4, + CONFIG_VALIDITY_TEST_MODE = 1 << 5, + CONFIG_VALIDITY_ADSB_IN_CAP = 1 << 6, + CONFIG_VALIDITY_AIRCRAFT_LEN_WIDTH = 1 << 7, + CONFIG_VALIDITY_ANT_LAT_OFFSET = 1 << 8, + CONFIG_VALIDITY_ANT_LONG_OFFSET = 1 << 9, + CONFIG_VALIDITY_AIRCRAFT_REG = 1 << 10, + CONFIG_VALIDITY_AIRCRAFT_STALL_SPEED = 1 << 11, + CONFIG_VALIDITY_AIRCRAFT_EMITTER_TYPE = 1 << 12, + CONFIG_VALIDITY_DEF_1090ES_TX_MODE = 1 << 13, + CONFIG_VALIDITY_DEF_MODES_REPLY_MODE = 1 << 14, + CONFIG_VALIDITY_DEF_MODEC_REPLY_MODE = 1 << 15, + CONFIG_VALIDITY_DEF_MODEA_REPLY_MODE = 1 << 16, + CONFIG_VALIDITY_SERIAL_BAUD_RATE = 1 << 17, + CONFIG_VALIDITY_DEF_MODEA_SQUAWK = 1 << 18, + CONFIG_VALIDITY_BARO_100 = 1 << 19, + CONFIG_VALIDITY_IN_PROTOCOL = 1 << 20, + CONFIG_VALIDITY_OUT_PROTOCOL = 1 << 21, +} CONFIG_VALIDITY; + +typedef union __attribute__((__packed__)) +{ + struct __attribute__((__packed__)) + { + uint32_t icaoValid : 1; + uint32_t silValid : 1; + uint32_t sdaValid : 1; + uint32_t baroAltSourceValid : 1; + uint32_t aircraftMaxSpeedValid : 1; + uint32_t testModeValid : 1; + uint32_t adsbInCapValid : 1; + uint32_t aircraftLenWidthValid : 1; + uint32_t aircraftLatOffsetValid : 1; + uint32_t aircraftLongOffsetValid : 1; + uint32_t aircraftRegValid : 1; + uint32_t aircraftStallSpeedValid : 1; + uint32_t aircraftEmitterCatValid : 1; + uint32_t default1090ExTxModeValid : 1; + uint32_t defaultModeSReplyModeValid : 1; + uint32_t defaultModeCReplyModeValid : 1; + uint32_t defaultModeAReplyModeValid : 1; + uint32_t serialBaudRateValid : 1; + uint32_t defaultModeASquawkValid : 1; + uint32_t baro100Valid : 1; + uint32_t inProtocolValid : 1; + uint32_t outProtocolValid : 1; + uint32_t reserved : 10; + }; + CONFIG_VALIDITY raw; +} CONFIG_VALIDITY_BITMASK; + +typedef enum __attribute__((__packed__)) +{ + PING_PROTOCOL_NONE = 0, + PING_PROTOCOL_MAVLINK = 1 << 0, + PING_PROTOCOL_UCP = 1 << 1, + PING_PROTOCOL_APOLLO = 1 << 9, + PING_PROTOCOL_UCP_HD = 1 << 10, +} PING_PROTOCOL; + +typedef union +{ + struct __attribute__((__packed__)) + { + uint16_t mavlink : 1; + uint16_t ucp : 1; + uint16_t reserved1 : 7; + uint16_t apollo : 1; + uint16_t ucphd : 1; + uint16_t reserved2 : 5; + }; + PING_PROTOCOL raw; +} PING_PROTOCOL_MASK; + +typedef struct __attribute__((__packed__)) +{ + GDL90_MESSAGE_ID messageId; + uint8_t version; + uint8_t icaoAddress[3]; + uint8_t maxSpeed : 3; + GDL90_BARO_DATA_SOURCE baroAltSource : 1; + ADSB_SDA SDA : 2; + ADSB_SIL SIL : 2; + ADSB_AIRCRAFT_LENGTH_WIDTH lengthWidth : 4; + ADSB_1090ES_IN_CAPABILITY es1090InCapable : 1; + ADSB_UAT_IN_CAPABILITY uatInCapable : 1; + uint8_t testMode : 2; + ADSB_GPS_LONGITUDINAL_OFFSET longitudinalOffset : 5; + ADSB_GPS_LATERAL_OFFSET lateralOffset : 3; + uint8_t registration[8]; + uint16_t stallSpeed_cmps; + ADSB_EMITTER emitterType; + PING_COM_RATE baudRate : 4; + uint8_t modeAEnabled : 1; + uint8_t modeCEnabled : 1; + uint8_t modeSEnabled : 1; + uint8_t es1090TxEnabled : 1; + uint16_t defaultSquawk; + CONFIG_VALIDITY_BITMASK valdityBitmask; + uint8_t rfu : 7; + uint8_t baro100 : 1; + PING_PROTOCOL_MASK inProtocol; + PING_PROTOCOL_MASK outProtocol; + uint16_t crc; +} GDL90_TRANSPONDER_CONFIG_MSG_V4_V5; + + +typedef struct __attribute__((__packed__)) +{ + uint8_t fwMajorVersion; + uint8_t fwMinorVersion; + uint8_t fwBuildVersion; + uint8_t hwId; // TODO Ugh should be 16 bits + uint64_t serialNumber; +} GDL90_DEVICE_ID; + +typedef struct __attribute__((__packed__)) +{ + GDL90_MESSAGE_ID messageId; + uint8_t protocolVersion; + GDL90_DEVICE_ID primary; + GDL90_DEVICE_ID secondary; + uint8_t primaryFWID; + uint32_t primaryCRC; + uint8_t secondaryFWID; + uint32_t secondaryCRC; + uint8_t primaryFwPartNumber[15]; + uint8_t secondaryFwPartNumber[15]; + uint16_t crc; +} GDL90_IDENTIFICATION_V3; +#define GDL90_IDENT_PROTOCOL_VERSION (3) + + +typedef struct __attribute__((__packed__)) +{ + struct + { + uint8_t uatInitialized : 1; + + // GDL90 public spec defines next bit as reserved + // uAvionix maps extra failure condition + uint8_t functionFailureGnssDataFrequency : 1; + + uint8_t ratcs : 1; + uint8_t gpsBatteryLow : 1; + uint8_t addressType : 1; + uint8_t ident : 1; + uint8_t maintenanceRequired : 1; + uint8_t gpsPositionValid : 1; + } one; + struct __attribute__((__packed__)) + { + + uint8_t utcOk : 1; + + // GDL90 public spec defines next four bits as reserved + // uAvionix maps extra failure conditions + uint8_t functionFailureGnssUnavailable : 1; + uint8_t functionFailureGnssNo3dFix : 1; + uint8_t functionFailureBroadcastMonitor : 1; + uint8_t functionFailureTransmitSystem : 1; + + uint8_t csaNotAvailable : 1; + uint8_t csaRequested : 1; + uint8_t timestampMsb : 1; + } two; +} GDL90_HEARTBEAT_STATUS; + +typedef struct __attribute__((__packed__)) +{ + GDL90_MESSAGE_ID messageId; + GDL90_HEARTBEAT_STATUS status; + uint16_t timestamp; + + // Need to flip before TX + union + { + struct __attribute__((__packed__)) + { + uint16_t uatMessages : 10; + uint16_t rfu : 1; + uint16_t uplinkMessages : 5; + }; + uint16_t messageCount; + }; + uint16_t crc; +} GDL90_HEARTBEAT; + +typedef enum __attribute__((__packed__)) +{ + GDL90_ADDRESS_ADSB_ICAO, + GDL90_ADDRESS_ADSB_SELF_ASSIGNED, + GDL90_ADDRESS_TISB_ICAO, + GDL90_ADDRESS_TISB_TRACK_ID, + GDL90_ADDRESS_SURFACE, + GDL90_ADDRESS_GROUND_BEACON, +} GDL90_ADDRESS_TYPE; + +typedef enum __attribute__((__packed__)) +{ + GDL90_NO_ALERT, + GDL90_ALERT, +} GDL90_TRAFFIC_ALERT; + +typedef enum __attribute__((__packed__)) +{ + GDL90_MISC_INVALID, + GDL90_MISC_TRUE_TRACK, + GDL90_MISC_HEADING_MAGNETIC, + GDL90_MISC_HEADING_TRUE, +} GDL90_MISC_TRACK_TYPE; + +typedef enum __attribute__((__packed__)) +{ + GDL90_MISC_REPORT_UPDATED, + GDL90_MISC_REPORT_EXTRAPOLATED, +} GDL90_MISC_REPORT_TYPE; + +typedef enum __attribute__((__packed__)) +{ + GDL90_MISC_ON_GROUND, + GDL90_MISC_AIRBORNE, +} GDL90_MISC_AG_STATE; + +typedef union +{ + struct __attribute__((__packed__)) + { + GDL90_MISC_TRACK_TYPE track : 2; + GDL90_MISC_REPORT_TYPE reportType : 1; + GDL90_MISC_AG_STATE agState : 1; + }; + uint8_t data; +} GDL90_MISCELLANEOUS; + +typedef struct __attribute__((__packed__)) +{ + GDL90_ADDRESS_TYPE addressType: 4; + GDL90_TRAFFIC_ALERT trafficAlert : 4; + + uint8_t address[3]; + + uint8_t latitude[3]; // 180 deg / 2^23 + uint8_t longitude[3]; // 180 deg / 2^23 + + // Byte order must be flipped before TX + union + { + struct __attribute__((__packed__)) + { + uint16_t misc : 4; + uint16_t altitude : 12; + }; + uint16_t altitudeMisc; + }; + + uint8_t NACp : 4; + uint8_t NIC : 4; + + // Byte order must be flipped before TX + union + { + struct __attribute__((__packed__)) + { + uint32_t heading : 8; + uint32_t verticalVelocity : 12; + uint32_t horizontalVelocity : 12; + }; + uint32_t velocities; + }; + + uint8_t emitterCategory; + uint8_t callsign[8]; + + uint8_t rfu : 4; + uint8_t emergencyCode : 4; +} GDL90_REPORT; + +typedef struct __attribute__((__packed__)) +{ + GDL90_MESSAGE_ID messageId; + GDL90_REPORT report; + uint16_t crc; +} GDL90_OWNSHIP_REPORT; +typedef GDL90_OWNSHIP_REPORT GDL90_TRAFFIC_REPORT; + +typedef enum __attribute__((__packed__)) +{ + GDL90_NO_WARNING, + GDL90_WARNING, +} GDL90_VERTICAL_WARNING; + +typedef struct __attribute__((__packed__)) +{ + GDL90_MESSAGE_ID messageId; + uint16_t geometricAltitude; // 5 ft resolution + + // Must be endian swapped before TX + union + { + struct __attribute__((__packed__)) + { + uint16_t verticalFOM : 15; + GDL90_VERTICAL_WARNING verticalWarning : 1; + }; + uint16_t veritcalMetrics; + }; + uint16_t crc; +} GDL90_OWNSHIP_GEO_ALTITUDE; + +typedef enum __attribute__((__packed__)) +{ + GDL90_SENSOR_AHRS = 0, + GDL90_SENSOR_BARO = 1, + GDL90_SENSOR_CO = 2, + GDL90_SENSOR_DEVICE = 3 +} GDL90_SENSOR_TYPE; + +typedef struct __attribute__((__packed__)) +{ + GDL90_MESSAGE_ID messageId; + GDL90_SENSOR_TYPE sensorType; + uint32_t pressure_mbarE2; + int32_t pressureAlt_mm; + int16_t temperature_cE2; + uint16_t crc; +} GDL90_SENSOR_BARO_MESSAGE; + diff --git a/libraries/AP_ADSB/GDL90_protocol/hostGDL90Support.h b/libraries/AP_ADSB/GDL90_protocol/hostGDL90Support.h new file mode 100644 index 0000000000..2020ab437b --- /dev/null +++ b/libraries/AP_ADSB/GDL90_protocol/hostGDL90Support.h @@ -0,0 +1,83 @@ +/* + Copyright (C) 2021 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 . + + + Author: GDL90/UCP protocol by uAvionix, 2021. + Implemented by: Tom Pittenger + */ + +#ifndef _GDL90_H_ +#define _GDL90_H_ + +#include + +#define GDL90_QUEUE_LENGTH (2) + +#define GDL90_FLAG_BYTE (0x7E) +#define GDL90_CONTROL_ESCAPE_BYTE (0x7D) +#define GDL90_STUFF_BYTE (0x20) +#define GDL90_OVERHEAD_LENGTH (3) // Not counting framing bytes + + +// Transmit message sizes +#define GDL90_TX_MAX_PAYLOAD_LENGTH (552) +#define GDL90_TX_MAX_PACKET_LENGTH (GDL90_TX_MAX_PAYLOAD_LENGTH + GDL90_OVERHEAD_LENGTH) +#define GDL90_TX_MAX_FRAME_LENGTH (2 + ((15 * GDL90_TX_MAX_PACKET_LENGTH) / 10)) // IF every other byte was stuffed + +// Receive message sizes +#define GDL90_RX_MAX_PAYLOAD_LENGTH (128) +#define GDL90_RX_MAX_PACKET_LENGTH (GDL90_RX_MAX_PAYLOAD_LENGTH + GDL90_OVERHEAD_LENGTH) + +typedef union __attribute__((__packed__)) +{ + struct __attribute__((__packed__)) + { + GDL90_MESSAGE_ID messageId; + uint8_t payload[GDL90_TX_MAX_PAYLOAD_LENGTH]; + uint16_t crc; // Actually CRC location varies. This is a placeholder + }; + uint8_t raw[GDL90_TX_MAX_PACKET_LENGTH]; +} GDL90_TX_MESSAGE; + +typedef union __attribute__((__packed__)) +{ + struct __attribute__((__packed__)) + { + GDL90_MESSAGE_ID messageId; + uint8_t payload[GDL90_RX_MAX_PAYLOAD_LENGTH]; + uint16_t crc; // Actually CRC location varies. This is a placeholder + }; + uint8_t raw[GDL90_RX_MAX_PACKET_LENGTH]; +} GDL90_RX_MESSAGE; + +typedef enum +{ + GDL90_RX_IDLE, + GDL90_RX_IN_PACKET, + GDL90_RX_UNSTUFF, + //GDL90_RX_END, +} GDL90_RX_STATE; + +typedef struct +{ + GDL90_RX_STATE state; + uint16_t length; + //uint32_t overflow; + //uint32_t complete; + uint8_t prev_data; +} GDL90_RX_STATUS; + +#endif