From 6b119c6f6caa33eaed9df12447ae23705c2c52ae Mon Sep 17 00:00:00 2001 From: nicholas-inocencio Date: Wed, 9 Oct 2024 13:31:38 -0500 Subject: [PATCH 1/2] AP_ADSB: Bugfixes and improvements to ping200X integration AP_ADSB: uAvionix Transponder Status V3 + Current version of ping200X sends the v1 status message periodically and the v3 status message in response to the transponder control message, so ardupilot needs to handle both gracefully; version 1 and version 3 are very different in structure and naively assuming one version over another will cause errors. AP_ADSB: Process additional xpdr status v3 fields AP_ADSB: Send GCS xpdr status at least every 10s AP_ADSB: Send ping200X estimated HPL + When AP sends the ping200X the GPS data GDL90 message, it needs to provide a valid HPL for the ping200X to report a valid NIC. AP_ADSB: Don't send unsolicited transponder status AP_ADSB: Better initialization of xpdr id/config AP_ADSB: Better initialization of frontend status AP_ADSB: Suggestions from review --- libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.cpp | 239 +++++++++++++----- libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.h | 19 +- .../GDL90_protocol/GDL90_Message_Structs.h | 9 +- 3 files changed, 194 insertions(+), 73 deletions(-) diff --git a/libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.cpp b/libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.cpp index 5ecaffc5d7506..20d9e214948d5 100644 --- a/libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.cpp +++ b/libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.cpp @@ -59,8 +59,10 @@ bool AP_ADSB_uAvionix_UCP::init() return false; } - request_msg(GDL90_ID_IDENTIFICATION); - request_msg(GDL90_ID_TRANSPONDER_CONFIG); + _frontend.out_state.ctrl.squawkCode = 1200; + _frontend.out_state.tx_status.squawk = 1200; + _frontend.out_state.tx_status.fault |= UAVIONIX_ADSB_OUT_STATUS_FAULT_STATUS_MESSAGE_UNAVAIL; + return true; } @@ -88,8 +90,32 @@ void AP_ADSB_uAvionix_UCP::update() } } // while nbytes + + if (run_state.last_packet_Transponder_Id_ms == 0 && run_state.request_Transponder_Id_tries < 5) + { + if (now_ms - run_state.last_packet_Request_Transponder_Id_ms >= 1000) + { + request_msg(GDL90_ID_IDENTIFICATION); + run_state.request_Transponder_Id_tries++; + } + } + + if (run_state.last_packet_Transponder_Config_ms == 0 && run_state.request_Transponder_Config_tries < 5) + { + if (now_ms - run_state.last_packet_Request_Transponder_Config_ms >= 1000) + { + request_msg(GDL90_ID_TRANSPONDER_CONFIG); + run_state.request_Transponder_Config_tries++; + } + } + if (now_ms - run_state.last_packet_Transponder_Control_ms >= 1000) { run_state.last_packet_Transponder_Control_ms = now_ms; + + // We want to use the defaults stored on the ping200X, if possible. + // Until we get the config message (or we've tried requesting it several times), + // don't send the control message. + if (run_state.last_packet_Transponder_Config_ms != 0 || run_state.request_Transponder_Config_tries >= 5) send_Transponder_Control(); } @@ -99,12 +125,13 @@ void AP_ADSB_uAvionix_UCP::update() } // if the transponder has stopped giving us the data needed to - // fill the transponder status mavlink message, reset that data. - if ((now_ms - run_state.last_packet_Transponder_Status_ms >= 10000 && run_state.last_packet_Transponder_Status_ms != 0) - && (now_ms - run_state.last_packet_Transponder_Heartbeat_ms >= 10000 && run_state.last_packet_Transponder_Heartbeat_ms != 0) - && (now_ms - run_state.last_packet_Transponder_Ownship_ms >= 10000 && run_state.last_packet_Transponder_Ownship_ms != 0)) + // fill the transponder status mavlink message, indicate status unavailable. + if ((now_ms - run_state.last_packet_Transponder_Status_ms >= 10000) + && (now_ms - run_state.last_packet_Transponder_Heartbeat_ms >= 10000) + && (now_ms - run_state.last_packet_Transponder_Ownship_ms >= 10000)) { _frontend.out_state.tx_status.fault |= UAVIONIX_ADSB_OUT_STATUS_FAULT_STATUS_MESSAGE_UNAVAIL; + // TODO reset the data for each message when timeout occurs } } @@ -118,9 +145,7 @@ void AP_ADSB_uAvionix_UCP::handle_msg(const GDL90_RX_MESSAGE &msg) // protocol. memcpy(&rx.decoded.heartbeat, msg.raw, sizeof(rx.decoded.heartbeat)); run_state.last_packet_Transponder_Heartbeat_ms = AP_HAL::millis(); - - // this is always true. The "ground/air bit place" is set meaning we're always in the air - _frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_ON_GROUND; + _frontend.out_state.tx_status.fault &= ~UAVIONIX_ADSB_OUT_STATUS_FAULT_STATUS_MESSAGE_UNAVAIL; if (rx.decoded.heartbeat.status.one.maintenanceRequired) { _frontend.out_state.tx_status.fault |= UAVIONIX_ADSB_OUT_STATUS_FAULT_MAINT_REQ; @@ -145,13 +170,11 @@ void AP_ADSB_uAvionix_UCP::handle_msg(const GDL90_RX_MESSAGE &msg) } else { _frontend.out_state.tx_status.fault &= ~UAVIONIX_ADSB_OUT_STATUS_FAULT_TX_SYSTEM_FAIL; } - - _frontend.out_state.tx_status.fault &= ~UAVIONIX_ADSB_OUT_STATUS_FAULT_STATUS_MESSAGE_UNAVAIL; - } break; case GDL90_ID_IDENTIFICATION: + run_state.last_packet_Transponder_Id_ms = AP_HAL::millis(); // 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. @@ -176,11 +199,13 @@ void AP_ADSB_uAvionix_UCP::handle_msg(const GDL90_RX_MESSAGE &msg) break; case GDL90_ID_TRANSPONDER_CONFIG: + run_state.last_packet_Transponder_Config_ms = AP_HAL::millis(); 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: + _frontend.out_state.tx_status.fault &= ~UAVIONIX_ADSB_OUT_STATUS_FAULT_STATUS_MESSAGE_UNAVAIL; // 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 @@ -189,7 +214,6 @@ void AP_ADSB_uAvionix_UCP::handle_msg(const GDL90_RX_MESSAGE &msg) run_state.last_packet_Transponder_Ownship_ms = AP_HAL::millis(); _frontend.out_state.tx_status.NIC_NACp = rx.decoded.ownship_report.report.NIC | (rx.decoded.ownship_report.report.NACp << 4); memcpy(_frontend.out_state.tx_status.flight_id, rx.decoded.ownship_report.report.callsign, sizeof(_frontend.out_state.tx_status.flight_id)); - //_frontend.out_state.tx_status.temperature = rx.decoded.ownship_report.report.temperature; // there is no message in the vocabulary of the 200x that has board temperature break; case GDL90_ID_OWNSHIP_GEOMETRIC_ALTITUDE: @@ -204,61 +228,146 @@ void AP_ADSB_uAvionix_UCP::handle_msg(const GDL90_RX_MESSAGE &msg) break; case GDL90_ID_TRANSPONDER_STATUS: - memcpy(&rx.decoded.transponder_status, msg.raw, sizeof(rx.decoded.transponder_status)); - if (rx.decoded.transponder_status.identActive) { - _frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_IDENT_ACTIVE; - } else { - _frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_IDENT_ACTIVE; - } - - if (rx.decoded.transponder_status.modeAEnabled) { - _frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_MODE_A_ENABLED; - } else { - _frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_MODE_A_ENABLED; - } + { + _frontend.out_state.tx_status.fault &= ~UAVIONIX_ADSB_OUT_STATUS_FAULT_STATUS_MESSAGE_UNAVAIL; + switch (msg.payload[0]) + { + case 1: + { + // version 1 of the transponder status message is sent at 1 Hz (if UCP protocol out is enabled on the transponder) + memcpy(&rx.decoded.transponder_status, msg.raw, sizeof(rx.decoded.transponder_status)); + if (rx.decoded.transponder_status.identActive) { + _frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_IDENT_ACTIVE; + } else { + _frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_IDENT_ACTIVE; + } - if (rx.decoded.transponder_status.modeCEnabled) { - _frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_MODE_C_ENABLED; - } else { - _frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_MODE_C_ENABLED; - } + if (rx.decoded.transponder_status.modeAEnabled) { + _frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_MODE_A_ENABLED; + } else { + _frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_MODE_A_ENABLED; + } - if (rx.decoded.transponder_status.modeSEnabled) { - _frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_MODE_S_ENABLED; - } else { - _frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_MODE_S_ENABLED; - } + if (rx.decoded.transponder_status.modeCEnabled) { + _frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_MODE_C_ENABLED; + } else { + _frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_MODE_C_ENABLED; + } - if (rx.decoded.transponder_status.es1090TxEnabled) { - _frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_1090ES_TX_ENABLED; - } else { - _frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_1090ES_TX_ENABLED; - } + if (rx.decoded.transponder_status.modeSEnabled) { + _frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_MODE_S_ENABLED; + } else { + _frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_MODE_S_ENABLED; + } - if (rx.decoded.transponder_status.x_bit) { - _frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_XBIT_ENABLED; - } else { - _frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_XBIT_ENABLED; + if (rx.decoded.transponder_status.es1090TxEnabled) { + _frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_1090ES_TX_ENABLED; + } else { + _frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_1090ES_TX_ENABLED; + } + + if (rx.decoded.transponder_status.x_bit) { + _frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_XBIT_ENABLED; + } else { + _frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_XBIT_ENABLED; + } + + _frontend.out_state.tx_status.squawk = rx.decoded.transponder_status.squawkCode; + + if (run_state.last_packet_Transponder_Status_ms == 0 && run_state.last_packet_Transponder_Config_ms == 0) { + // If this is the first time we've seen a status message, + // and we haven't initialized the control message from the config message, + // set initial control message contents to match transponder's current behavior. + _frontend.out_state.ctrl.modeAEnabled = rx.decoded.transponder_status.modeAEnabled; + _frontend.out_state.ctrl.modeCEnabled = rx.decoded.transponder_status.modeCEnabled; + _frontend.out_state.ctrl.modeSEnabled = rx.decoded.transponder_status.modeSEnabled; + _frontend.out_state.ctrl.es1090TxEnabled = rx.decoded.transponder_status.es1090TxEnabled; + _frontend.out_state.ctrl.squawkCode = rx.decoded.transponder_status.squawkCode; + _frontend.out_state.ctrl.x_bit = rx.decoded.transponder_status.x_bit; + } + run_state.last_packet_Transponder_Status_ms = AP_HAL::millis(); +#if AP_MAVLINK_MSG_UAVIONIX_ADSB_OUT_STATUS_ENABLED + GCS_SEND_MESSAGE(MSG_UAVIONIX_ADSB_OUT_STATUS); + run_state.last_gcs_send_message_Transponder_Status_ms = AP_HAL::millis(); +#endif + break; } + case 2: + // deprecated + break; + case 3: + { + // Version 3 of the transponder status message is sent in response to the transponder control message (if UCP-HD protocol out is enabled on the transponder) + memcpy(&rx.decoded.transponder_status_v3, msg.raw, sizeof(rx.decoded.transponder_status_v3)); + + if (rx.decoded.transponder_status_v3.indicatingOnGround) { + _frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_ON_GROUND; + } else { + _frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_ON_GROUND; + } - _frontend.out_state.tx_status.squawk = rx.decoded.transponder_status.squawkCode; + if (rx.decoded.transponder_status_v3.fault) { + // unsure what fault is indicated, query heartbeat for more info + request_msg(GDL90_ID_HEARTBEAT); + } - _frontend.out_state.tx_status.fault &= ~UAVIONIX_ADSB_OUT_STATUS_FAULT_STATUS_MESSAGE_UNAVAIL; + if (rx.decoded.transponder_status_v3.identActive) { + _frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_IDENT_ACTIVE; + } else { + _frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_IDENT_ACTIVE; + } - if (run_state.last_packet_Transponder_Status_ms == 0) { - // set initial control message contents to transponder defaults - _frontend.out_state.ctrl.modeAEnabled = rx.decoded.transponder_status.modeAEnabled; - _frontend.out_state.ctrl.modeCEnabled = rx.decoded.transponder_status.modeCEnabled; - _frontend.out_state.ctrl.modeSEnabled = rx.decoded.transponder_status.modeSEnabled; - _frontend.out_state.ctrl.es1090TxEnabled = rx.decoded.transponder_status.es1090TxEnabled; - _frontend.out_state.ctrl.squawkCode = rx.decoded.transponder_status.squawkCode; - _frontend.out_state.ctrl.x_bit = rx.decoded.transponder_status.x_bit; - } - run_state.last_packet_Transponder_Status_ms = AP_HAL::millis(); + if (rx.decoded.transponder_status_v3.modeAEnabled) { + _frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_MODE_A_ENABLED; + } else { + _frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_MODE_A_ENABLED; + } + + if (rx.decoded.transponder_status_v3.modeCEnabled) { + _frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_MODE_C_ENABLED; + } else { + _frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_MODE_C_ENABLED; + } + + if (rx.decoded.transponder_status_v3.modeSEnabled) { + _frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_MODE_S_ENABLED; + } else { + _frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_MODE_S_ENABLED; + } + + if (rx.decoded.transponder_status_v3.es1090TxEnabled) { + _frontend.out_state.tx_status.state |= UAVIONIX_ADSB_OUT_STATUS_STATE_1090ES_TX_ENABLED; + } else { + _frontend.out_state.tx_status.state &= ~UAVIONIX_ADSB_OUT_STATUS_STATE_1090ES_TX_ENABLED; + } + + _frontend.out_state.tx_status.squawk = rx.decoded.transponder_status_v3.squawkCode; + _frontend.out_state.tx_status.NIC_NACp = rx.decoded.transponder_status_v3.NIC | (rx.decoded.transponder_status_v3.NACp << 4); + _frontend.out_state.tx_status.boardTemp = rx.decoded.transponder_status_v3.temperature; + + // TODO not the best approach + if (run_state.last_packet_Transponder_Status_ms == 0 && run_state.last_packet_Transponder_Config_ms == 0) { + // If this is the first time we've seen a status message, + // and we haven't initialized the control message from the config message, + // set initial control message contents to match transponder's current behavior. + _frontend.out_state.ctrl.modeAEnabled = rx.decoded.transponder_status_v3.modeAEnabled; + _frontend.out_state.ctrl.modeCEnabled = rx.decoded.transponder_status_v3.modeCEnabled; + _frontend.out_state.ctrl.modeSEnabled = rx.decoded.transponder_status_v3.modeSEnabled; + _frontend.out_state.ctrl.es1090TxEnabled = rx.decoded.transponder_status_v3.es1090TxEnabled; + _frontend.out_state.ctrl.squawkCode = rx.decoded.transponder_status_v3.squawkCode; + } + run_state.last_packet_Transponder_Status_ms = AP_HAL::millis(); #if AP_MAVLINK_MSG_UAVIONIX_ADSB_OUT_STATUS_ENABLED - GCS_SEND_MESSAGE(MSG_UAVIONIX_ADSB_OUT_STATUS); + GCS_SEND_MESSAGE(MSG_UAVIONIX_ADSB_OUT_STATUS); + run_state.last_gcs_send_message_Transponder_Status_ms = AP_HAL::millis(); #endif + break; + } + default: + break; + } break; + } #endif // AP_ADSB_UAVIONIX_UCP_CAPTURE_ALL_RX_PACKETS case GDL90_ID_TRANSPONDER_CONTROL: @@ -339,7 +448,6 @@ void AP_ADSB_uAvionix_UCP::send_Transponder_Control() gdl90Transmit((GDL90_TX_MESSAGE&)msg, sizeof(msg)); } - void AP_ADSB_uAvionix_UCP::send_GPS_Data() { GDL90_GPS_DATA_V2 msg {}; @@ -358,12 +466,19 @@ void AP_ADSB_uAvionix_UCP::send_GPS_Data() msg.altitudeGnss_mm = fix_is_good ? (_frontend._my_loc.alt * 10): INT32_MAX; // Protection Limits. FD or SBAS-based depending on state bits - msg.HPL_mm = UINT32_MAX; - msg.VPL_cm = UINT32_MAX; + // Estimate HPL based on horizontal accuracy/HFOM to a probability of 10^-7: + // Using the central limit theorem for a Gaussian distribution, + // this is 5.326724*stdDev. + // Conservatively round up to 6 as a scaling factor, + // and asssume HFOM of 95% was calculated as 2*stdDev*HDOP. + // This yields a factor of 3 to estimate HPL from horizontal accuracy. + float accHoriz; + bool gotAccHoriz = gps.horizontal_accuracy(accHoriz); + msg.HPL_mm = gotAccHoriz ? 3 * accHoriz * 1E3 : UINT32_MAX; // required to calculate NIC + msg.VPL_cm = UINT32_MAX; // unused by ping200X // Figure of Merits - float accHoriz; - msg.horizontalFOM_mm = gps.horizontal_accuracy(accHoriz) ? accHoriz * 1E3 : UINT32_MAX; + msg.horizontalFOM_mm = gotAccHoriz ? accHoriz * 1E3 : UINT32_MAX; float accVert; msg.verticalFOM_cm = gps.vertical_accuracy(accVert) ? accVert * 1E2 : UINT16_MAX; float accVel; diff --git a/libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.h b/libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.h index 742b7924a87cc..a7b77a40807a6 100644 --- a/libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.h +++ b/libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.h @@ -64,6 +64,7 @@ class AP_ADSB_uAvionix_UCP : public AP_ADSB_Backend { GDL90_TRANSPONDER_CONFIG_MSG_V4_V5 transponder_config; GDL90_HEARTBEAT heartbeat; GDL90_TRANSPONDER_STATUS_MSG transponder_status; + GDL90_TRANSPONDER_STATUS_MSG_V3 transponder_status_v3; #if AP_ADSB_UAVIONIX_UCP_CAPTURE_ALL_RX_PACKETS GDL90_OWNSHIP_REPORT ownship_report; GDL90_OWNSHIP_GEO_ALTITUDE ownship_geometric_altitude; @@ -73,11 +74,19 @@ class AP_ADSB_uAvionix_UCP : public AP_ADSB_Backend { } rx; struct { - uint32_t last_packet_GPS_ms; - uint32_t last_packet_Transponder_Control_ms; - uint32_t last_packet_Transponder_Status_ms; - uint32_t last_packet_Transponder_Heartbeat_ms; - uint32_t last_packet_Transponder_Ownship_ms; + uint32_t last_packet_GPS_ms; // out + uint32_t last_packet_Transponder_Control_ms; // out + uint32_t last_packet_Transponder_Status_ms; // in + uint32_t last_packet_Transponder_Heartbeat_ms; // in + uint32_t last_packet_Transponder_Ownship_ms; // in + uint32_t last_gcs_send_message_Transponder_Status_ms; // out + uint32_t last_packet_Request_Transponder_Config_ms; // out + uint32_t last_packet_Transponder_Config_ms; // in + uint32_t request_Transponder_Config_tries; + uint32_t last_packet_Request_Transponder_Id_ms; // out + uint32_t last_packet_Transponder_Id_ms; // in + uint32_t request_Transponder_Id_tries; + } run_state; }; diff --git a/libraries/AP_ADSB/GDL90_protocol/GDL90_Message_Structs.h b/libraries/AP_ADSB/GDL90_protocol/GDL90_Message_Structs.h index 4a386c73865f7..456852fb8db3e 100644 --- a/libraries/AP_ADSB/GDL90_protocol/GDL90_Message_Structs.h +++ b/libraries/AP_ADSB/GDL90_protocol/GDL90_Message_Structs.h @@ -103,10 +103,8 @@ typedef struct __attribute__((__packed__)) } GDL90_TRANSPONDER_CONTROL_MSG; #endif -#define GDL90_TRANSPONDER_STATUS_VERSION (1) // Version 1 is the correct UCP format; version 3 is half-duplex and not used by the ping200x #define GDL90_STATUS_MAX_ALTITUDE_FT (101338) #define GDL90_STATUS_MIN_ALTITUDE_FT (-1000) -#if GDL90_TRANSPONDER_STATUS_VERSION == 1 typedef struct __attribute__((__packed__)) { GDL90_MESSAGE_ID messageId; @@ -122,9 +120,9 @@ typedef struct __attribute__((__packed__)) uint16_t modecRepliesPerSecond; uint16_t modeSRepliesPerSecond; uint16_t squawkCode; + uint16_t crc; } GDL90_TRANSPONDER_STATUS_MSG; -#endif -#if GDL90_TRANSPONDER_STATUS_VERSION == 3 + typedef struct __attribute__((__packed__)) { GDL90_MESSAGE_ID messageId; @@ -147,8 +145,7 @@ typedef struct __attribute__((__packed__)) uint8_t NACp : 4; uint8_t temperature; uint16_t crc; -} GDL90_TRANSPONDER_STATUS_MSG; -#endif +} GDL90_TRANSPONDER_STATUS_MSG_V3; typedef struct __attribute__((__packed__)) From 06faa2095496248b28ec97e194e0afec59081058 Mon Sep 17 00:00:00 2001 From: nicholas-inocencio Date: Thu, 17 Oct 2024 09:52:25 -0500 Subject: [PATCH 2/2] AP_ADSB: Style fixes --- libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.cpp | 45 +- libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.h | 2 +- .../GDL90_protocol/GDL90_Message_Structs.h | 894 +++++++++--------- .../AP_ADSB/GDL90_protocol/hostGDL90Support.h | 91 +- 4 files changed, 511 insertions(+), 521 deletions(-) diff --git a/libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.cpp b/libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.cpp index 20d9e214948d5..0f92293c519f2 100644 --- a/libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.cpp +++ b/libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.cpp @@ -90,9 +90,8 @@ void AP_ADSB_uAvionix_UCP::update() } } // while nbytes - - if (run_state.last_packet_Transponder_Id_ms == 0 && run_state.request_Transponder_Id_tries < 5) - { + + if (run_state.last_packet_Transponder_Id_ms == 0 && run_state.request_Transponder_Id_tries < 5) { if (now_ms - run_state.last_packet_Request_Transponder_Id_ms >= 1000) { request_msg(GDL90_ID_IDENTIFICATION); @@ -100,8 +99,7 @@ void AP_ADSB_uAvionix_UCP::update() } } - if (run_state.last_packet_Transponder_Config_ms == 0 && run_state.request_Transponder_Config_tries < 5) - { + if (run_state.last_packet_Transponder_Config_ms == 0 && run_state.request_Transponder_Config_tries < 5) { if (now_ms - run_state.last_packet_Request_Transponder_Config_ms >= 1000) { request_msg(GDL90_ID_TRANSPONDER_CONFIG); @@ -114,9 +112,10 @@ void AP_ADSB_uAvionix_UCP::update() // We want to use the defaults stored on the ping200X, if possible. // Until we get the config message (or we've tried requesting it several times), - // don't send the control message. - if (run_state.last_packet_Transponder_Config_ms != 0 || run_state.request_Transponder_Config_tries >= 5) - send_Transponder_Control(); + // don't send the control message. + if (run_state.last_packet_Transponder_Config_ms != 0 || run_state.request_Transponder_Config_tries >= 5) { + send_Transponder_Control(); + } } if ((now_ms - run_state.last_packet_GPS_ms >= 200) && (_frontend._options & uint32_t(AP_ADSB::AdsbOption::Ping200X_Send_GPS)) != 0) { @@ -124,14 +123,12 @@ void AP_ADSB_uAvionix_UCP::update() send_GPS_Data(); } - // if the transponder has stopped giving us the data needed to + // if the transponder has stopped giving us the data needed to // fill the transponder status mavlink message, indicate status unavailable. if ((now_ms - run_state.last_packet_Transponder_Status_ms >= 10000) && (now_ms - run_state.last_packet_Transponder_Heartbeat_ms >= 10000) - && (now_ms - run_state.last_packet_Transponder_Ownship_ms >= 10000)) - { + && (now_ms - run_state.last_packet_Transponder_Ownship_ms >= 10000)) { _frontend.out_state.tx_status.fault |= UAVIONIX_ADSB_OUT_STATUS_FAULT_STATUS_MESSAGE_UNAVAIL; - // TODO reset the data for each message when timeout occurs } } @@ -187,7 +184,7 @@ void AP_ADSB_uAvionix_UCP::handle_msg(const GDL90_RX_MESSAGE &msg) 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, @@ -230,10 +227,8 @@ void AP_ADSB_uAvionix_UCP::handle_msg(const GDL90_RX_MESSAGE &msg) case GDL90_ID_TRANSPONDER_STATUS: { _frontend.out_state.tx_status.fault &= ~UAVIONIX_ADSB_OUT_STATUS_FAULT_STATUS_MESSAGE_UNAVAIL; - switch (msg.payload[0]) - { - case 1: - { + switch (msg.payload[0]) { + case 1: { // version 1 of the transponder status message is sent at 1 Hz (if UCP protocol out is enabled on the transponder) memcpy(&rx.decoded.transponder_status, msg.raw, sizeof(rx.decoded.transponder_status)); if (rx.decoded.transponder_status.identActive) { @@ -295,8 +290,7 @@ void AP_ADSB_uAvionix_UCP::handle_msg(const GDL90_RX_MESSAGE &msg) case 2: // deprecated break; - case 3: - { + case 3: { // Version 3 of the transponder status message is sent in response to the transponder control message (if UCP-HD protocol out is enabled on the transponder) memcpy(&rx.decoded.transponder_status_v3, msg.raw, sizeof(rx.decoded.transponder_status_v3)); @@ -345,7 +339,6 @@ void AP_ADSB_uAvionix_UCP::handle_msg(const GDL90_RX_MESSAGE &msg) _frontend.out_state.tx_status.NIC_NACp = rx.decoded.transponder_status_v3.NIC | (rx.decoded.transponder_status_v3.NACp << 4); _frontend.out_state.tx_status.boardTemp = rx.decoded.transponder_status_v3.temperature; - // TODO not the best approach if (run_state.last_packet_Transponder_Status_ms == 0 && run_state.last_packet_Transponder_Config_ms == 0) { // If this is the first time we've seen a status message, // and we haven't initialized the control message from the config message, @@ -535,14 +528,14 @@ uint16_t AP_ADSB_uAvionix_UCP::gdl90Transmit(GDL90_TX_MESSAGE &message, const ui // 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) { @@ -550,7 +543,7 @@ uint16_t AP_ADSB_uAvionix_UCP::gdl90Transmit(GDL90_TX_MESSAGE &message, const ui } else if (i == length+1) { data = HIGHBYTE(frameCrc); } else { - data = message.raw[i]; + data = message.raw[i]; } if (data == GDL90_FLAG_BYTE || data == GDL90_CONTROL_ESCAPE_BYTE) { @@ -558,7 +551,7 @@ uint16_t AP_ADSB_uAvionix_UCP::gdl90Transmit(GDL90_TX_MESSAGE &message, const ui 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; @@ -566,7 +559,7 @@ uint16_t AP_ADSB_uAvionix_UCP::gdl90Transmit(GDL90_TX_MESSAGE &message, const ui gdl90FrameBuffer[frameIndex++] = data; } } - + // Add end of frame indication gdl90FrameBuffer[frameIndex++] = GDL90_FLAG_BYTE; @@ -574,7 +567,7 @@ uint16_t AP_ADSB_uAvionix_UCP::gdl90Transmit(GDL90_TX_MESSAGE &message, const ui if (hostTransmit(gdl90FrameBuffer, frameIndex)) { return frameIndex; } - + return 0; } diff --git a/libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.h b/libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.h index a7b77a40807a6..abf76569aa3a1 100644 --- a/libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.h +++ b/libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.h @@ -82,7 +82,7 @@ class AP_ADSB_uAvionix_UCP : public AP_ADSB_Backend { uint32_t last_gcs_send_message_Transponder_Status_ms; // out uint32_t last_packet_Request_Transponder_Config_ms; // out uint32_t last_packet_Transponder_Config_ms; // in - uint32_t request_Transponder_Config_tries; + uint32_t request_Transponder_Config_tries; uint32_t last_packet_Request_Transponder_Id_ms; // out uint32_t last_packet_Transponder_Id_ms; // in uint32_t request_Transponder_Id_tries; diff --git a/libraries/AP_ADSB/GDL90_protocol/GDL90_Message_Structs.h b/libraries/AP_ADSB/GDL90_protocol/GDL90_Message_Structs.h index 456852fb8db3e..36a549d8af6c9 100644 --- a/libraries/AP_ADSB/GDL90_protocol/GDL90_Message_Structs.h +++ b/libraries/AP_ADSB/GDL90_protocol/GDL90_Message_Structs.h @@ -1,22 +1,22 @@ /* - Copyright (C) 2021 Kraus Hamdani Aerospace Inc. All rights reserved. + 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 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. + 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 . + 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 + + Author: GDL90/UCP protocol by uAvionix, 2021. + Implemented by: Tom Pittenger */ #include @@ -25,81 +25,81 @@ 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_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__)) +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_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_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 +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; #define GDL90_TRANSPONDER_CONTROL_VERSION (2) #if GDL90_TRANSPONDER_CONTROL_VERSION == 1 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_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; #elif GDL90_TRANSPONDER_CONTROL_VERSION == 2 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]; - uint8_t rfu : 7; - uint8_t x_bit : 1; + 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]; + uint8_t rfu : 7; + uint8_t x_bit : 1; } GDL90_TRANSPONDER_CONTROL_MSG; #endif @@ -107,547 +107,547 @@ typedef struct __attribute__((__packed__)) #define GDL90_STATUS_MIN_ALTITUDE_FT (-1000) typedef struct __attribute__((__packed__)) { - GDL90_MESSAGE_ID messageId; - uint8_t version; - uint8_t rfu : 2; - uint8_t x_bit : 1; - uint8_t identActive : 1; - uint8_t modeAEnabled : 1; - uint8_t modeCEnabled : 1; - uint8_t modeSEnabled : 1; - uint8_t es1090TxEnabled : 1; - uint16_t modeARepliesPerSecond; - uint16_t modecRepliesPerSecond; - uint16_t modeSRepliesPerSecond; - uint16_t squawkCode; - uint16_t crc; + GDL90_MESSAGE_ID messageId; + uint8_t version; + uint8_t rfu : 2; + uint8_t x_bit : 1; + uint8_t identActive : 1; + uint8_t modeAEnabled : 1; + uint8_t modeCEnabled : 1; + uint8_t modeSEnabled : 1; + uint8_t es1090TxEnabled : 1; + uint16_t modeARepliesPerSecond; + uint16_t modecRepliesPerSecond; + uint16_t modeSRepliesPerSecond; + uint16_t squawkCode; + uint16_t crc; } GDL90_TRANSPONDER_STATUS_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_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_V3; typedef struct __attribute__((__packed__)) { - uint8_t HPLfdeActive : 1; - uint8_t fault : 1; - uint8_t HrdMagNorth : 1; - uint8_t reserved : 5; + 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_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_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) +#define GDL90_GPS_DATA_VERSION (2) typedef struct __attribute__((__packed__)) { - GDL90_MESSAGE_ID messageId; - uint8_t version; - GDL90_MESSAGE_ID reqMsgId; + 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_INTERNAL = 0, + GDL90_BARO_DATA_SOURCE_EXTERNAL, }GDL90_BARO_DATA_SOURCE; -typedef enum __attribute__((__packed__)) +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_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_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_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_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_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_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_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_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_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_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; + 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_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; + 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_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; + 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_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; + 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; + GDL90_MESSAGE_ID messageId; + GDL90_HEARTBEAT_STATUS status; + uint16_t timestamp; - // Need to flip before TX - union - { - struct __attribute__((__packed__)) + // Need to flip before TX + union { - uint16_t uatMessages : 10; - uint16_t rfu : 1; - uint16_t uplinkMessages : 5; + struct __attribute__((__packed__)) + { + uint16_t uatMessages : 10; + uint16_t rfu : 1; + uint16_t uplinkMessages : 5; + }; + uint16_t messageCount; }; - uint16_t messageCount; - }; - uint16_t crc; + 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_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_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_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_UPDATED, + GDL90_MISC_REPORT_EXTRAPOLATED, } GDL90_MISC_REPORT_TYPE; typedef enum __attribute__((__packed__)) { - GDL90_MISC_ON_GROUND, - GDL90_MISC_AIRBORNE, + 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; + 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; + GDL90_ADDRESS_TYPE addressType: 4; + GDL90_TRAFFIC_ALERT trafficAlert : 4; - uint8_t address[3]; + uint8_t address[3]; - uint8_t latitude[3]; // 180 deg / 2^23 - uint8_t longitude[3]; // 180 deg / 2^23 + 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__)) + // Byte order must be flipped before TX + union { - uint16_t misc : 4; - uint16_t altitude : 12; + struct __attribute__((__packed__)) + { + uint16_t misc : 4; + uint16_t altitude : 12; + }; + uint16_t altitudeMisc; }; - uint16_t altitudeMisc; - }; - uint8_t NACp : 4; - uint8_t NIC : 4; + uint8_t NACp : 4; + uint8_t NIC : 4; - // Byte order must be flipped before TX - union - { - struct __attribute__((__packed__)) + // Byte order must be flipped before TX + union { - uint32_t heading : 8; - uint32_t verticalVelocity : 12; - uint32_t horizontalVelocity : 12; + struct __attribute__((__packed__)) + { + uint32_t heading : 8; + uint32_t verticalVelocity : 12; + uint32_t horizontalVelocity : 12; + }; + uint32_t velocities; }; - uint32_t velocities; - }; - uint8_t emitterCategory; - uint8_t callsign[8]; + uint8_t emitterCategory; + uint8_t callsign[8]; - uint8_t rfu : 4; - uint8_t emergencyCode : 4; + 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_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_NO_WARNING, + GDL90_WARNING, } GDL90_VERTICAL_WARNING; typedef struct __attribute__((__packed__)) { - GDL90_MESSAGE_ID messageId; - uint16_t geometricAltitude; // 5 ft resolution + GDL90_MESSAGE_ID messageId; + uint16_t geometricAltitude; // 5 ft resolution - // Must be endian swapped before TX - union - { - struct __attribute__((__packed__)) + // Must be endian swapped before TX + union { - uint16_t verticalFOM : 15; - GDL90_VERTICAL_WARNING verticalWarning : 1; + struct __attribute__((__packed__)) + { + uint16_t verticalFOM : 15; + GDL90_VERTICAL_WARNING verticalWarning : 1; + }; + uint16_t veritcalMetrics; }; - uint16_t veritcalMetrics; - }; - uint16_t crc; + 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_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_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 index 2020ab437b49a..0d4d07d0aa431 100644 --- a/libraries/AP_ADSB/GDL90_protocol/hostGDL90Support.h +++ b/libraries/AP_ADSB/GDL90_protocol/hostGDL90Support.h @@ -1,22 +1,22 @@ /* - Copyright (C) 2021 Kraus Hamdani Aerospace Inc. All rights reserved. + 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 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. + 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 . + 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 + + Author: GDL90/UCP protocol by uAvionix, 2021. + Implemented by: Tom Pittenger */ #ifndef _GDL90_H_ @@ -24,60 +24,57 @@ #include -#define GDL90_QUEUE_LENGTH (2) +#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 +#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 +#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) +#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]; + 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]; + 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_IDLE, + GDL90_RX_IN_PACKET, + GDL90_RX_UNSTUFF, } GDL90_RX_STATE; typedef struct { - GDL90_RX_STATE state; - uint16_t length; - //uint32_t overflow; - //uint32_t complete; - uint8_t prev_data; + GDL90_RX_STATE state; + uint16_t length; + uint8_t prev_data; } GDL90_RX_STATUS; #endif