From f4ff8859e480c30c9d27973c82d95151db38cd7a Mon Sep 17 00:00:00 2001 From: nicholas-inocencio Date: Wed, 13 Oct 2021 16:31:14 -0700 Subject: [PATCH] AP_ADSB: I implement new mavlink packets ADSB_OUT_STATUS and ADSB_OUT_CONTROL --- libraries/AP_ADSB/AP_ADSB.cpp | 84 +++++++--- libraries/AP_ADSB/AP_ADSB.h | 63 ++++++-- libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.cpp | 150 +++++++++++++++--- libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.h | 2 + .../GDL90_protocol/GDL90_Message_Structs.h | 49 +++++- 5 files changed, 287 insertions(+), 61 deletions(-) diff --git a/libraries/AP_ADSB/AP_ADSB.cpp b/libraries/AP_ADSB/AP_ADSB.cpp index 61c65fcbf5..cea3ff2cf9 100644 --- a/libraries/AP_ADSB/AP_ADSB.cpp +++ b/libraries/AP_ADSB/AP_ADSB.cpp @@ -632,6 +632,26 @@ void AP_ADSB::handle_out_cfg(const mavlink_uavionix_adsb_out_cfg_t &packet) out_state.last_config_ms = 0; } +/* + * handle incoming packet UAVIONIX_ADSB_OUT_CONTROL + * allows a GCS to set the contents of the control message sent by ardupilot to the transponder + */ +void AP_ADSB::handle_out_control(const mavlink_uavionix_adsb_out_control_t &packet) +{ + out_state.ctrl.baroCrossChecked = packet.state & UAVIONIX_ADSB_OUT_CONTROL_STATE::UAVIONIX_ADSB_OUT_CONTROL_STATE_EXTERNAL_BARO_CROSSCHECKED; + out_state.ctrl.airGroundState = packet.state & UAVIONIX_ADSB_OUT_CONTROL_STATE::UAVIONIX_ADSB_OUT_CONTROL_STATE_ON_GROUND; + out_state.ctrl.identActive = packet.state & UAVIONIX_ADSB_OUT_CONTROL_STATE::UAVIONIX_ADSB_OUT_CONTROL_STATE_IDENT_BUTTON_ACTIVE; + out_state.ctrl.modeAEnabled = packet.state & UAVIONIX_ADSB_OUT_CONTROL_STATE::UAVIONIX_ADSB_OUT_CONTROL_STATE_MODE_A_ENABLED; + out_state.ctrl.modeCEnabled = packet.state & UAVIONIX_ADSB_OUT_CONTROL_STATE::UAVIONIX_ADSB_OUT_CONTROL_STATE_MODE_C_ENABLED; + out_state.ctrl.modeSEnabled = packet.state & UAVIONIX_ADSB_OUT_CONTROL_STATE::UAVIONIX_ADSB_OUT_CONTROL_STATE_MODE_S_ENABLED; + out_state.ctrl.es1090TxEnabled = packet.state & UAVIONIX_ADSB_OUT_CONTROL_STATE::UAVIONIX_ADSB_OUT_CONTROL_STATE_1090ES_TX_ENABLED; + out_state.ctrl.externalBaroAltitude_mm = packet.baroAltMSL; + out_state.ctrl.squawkCode = packet.squawk; + out_state.ctrl.emergencyState = packet.emergencyStatus; + memcpy(out_state.ctrl.callsign, packet.flight_id, sizeof(out_state.ctrl.callsign)); + out_state.ctrl.x_bit = packet.x_bit; +} + /* * this is a message from the transceiver reporting it's health. Using this packet * we determine which channel is on so we don't have to send out_state to all channels @@ -647,6 +667,19 @@ void AP_ADSB::handle_transceiver_report(const mavlink_channel_t chan, const mavl out_state.status = (UAVIONIX_ADSB_RF_HEALTH)packet.rfHealth; } +/* + * send a periodic report of the ADSB out status + */ +void AP_ADSB::send_adsb_out_status(const mavlink_channel_t chan) const +{ + for (uint8_t i=0; i < ADSB_MAX_INSTANCES; i++) { + if (_type[i] == (int8_t)(AP_ADSB::Type::uAvionix_UCP)) { + mavlink_msg_uavionix_adsb_out_status_send_struct(chan, &out_state.tx_status); + return; + } + } +} + /* @brief Generates pseudorandom ICAO from gps time, lat, and lon. Reference: DO282B, 2.2.4.5.1.3.2 @@ -727,31 +760,38 @@ bool AP_ADSB::next_sample(adsb_vehicle_t &vehicle) void AP_ADSB::handle_message(const mavlink_channel_t chan, const mavlink_message_t &msg) { switch (msg.msgid) { - case MAVLINK_MSG_ID_ADSB_VEHICLE: { - adsb_vehicle_t vehicle {}; - mavlink_msg_adsb_vehicle_decode(&msg, &vehicle.info); - vehicle.last_update_ms = AP_HAL::millis() - uint32_t(vehicle.info.tslc * 1000U); - handle_adsb_vehicle(vehicle); - break; - } + case MAVLINK_MSG_ID_ADSB_VEHICLE: { + adsb_vehicle_t vehicle {}; + mavlink_msg_adsb_vehicle_decode(&msg, &vehicle.info); + vehicle.last_update_ms = AP_HAL::millis() - uint32_t(vehicle.info.tslc * 1000U); + handle_adsb_vehicle(vehicle); + break; + } - case MAVLINK_MSG_ID_UAVIONIX_ADSB_TRANSCEIVER_HEALTH_REPORT: { - mavlink_uavionix_adsb_transceiver_health_report_t packet {}; - mavlink_msg_uavionix_adsb_transceiver_health_report_decode(&msg, &packet); - handle_transceiver_report(chan, packet); - break; - } + case MAVLINK_MSG_ID_UAVIONIX_ADSB_TRANSCEIVER_HEALTH_REPORT: { + mavlink_uavionix_adsb_transceiver_health_report_t packet {}; + mavlink_msg_uavionix_adsb_transceiver_health_report_decode(&msg, &packet); + handle_transceiver_report(chan, packet); + break; + } - case MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_CFG: { - mavlink_uavionix_adsb_out_cfg_t packet {}; - mavlink_msg_uavionix_adsb_out_cfg_decode(&msg, &packet); - handle_out_cfg(packet); - break; - } + case MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_CFG: { + mavlink_uavionix_adsb_out_cfg_t packet {}; + mavlink_msg_uavionix_adsb_out_cfg_decode(&msg, &packet); + handle_out_cfg(packet); + break; + } - case MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_DYNAMIC: - // unhandled, this is an outbound packet only - break; + case MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_DYNAMIC: + // unhandled, this is an outbound packet only + break; + + case MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_CONTROL: { + mavlink_uavionix_adsb_out_control_t packet {}; + mavlink_msg_uavionix_adsb_out_control_decode(&msg, &packet); + handle_out_control(packet); + break; + } } } diff --git a/libraries/AP_ADSB/AP_ADSB.h b/libraries/AP_ADSB/AP_ADSB.h index 11ba531b72..2dbf1092bd 100644 --- a/libraries/AP_ADSB/AP_ADSB.h +++ b/libraries/AP_ADSB/AP_ADSB.h @@ -140,6 +140,8 @@ public: // mavlink message handler void handle_message(const mavlink_channel_t chan, const mavlink_message_t &msg); + void send_adsb_out_status(const mavlink_channel_t chan) const; + // when true, a vehicle with that ICAO was found in database and the vehicle is populated. bool get_vehicle_by_ICAO(const uint32_t icao, adsb_vehicle_t &vehicle) const; @@ -150,23 +152,46 @@ 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)) { + if (!healthy() || ((out_state.cfg.rfSelect & UAVIONIX_ADSB_OUT_RF_SELECT_TX_ENABLED) == 0)) { return false; } - out_state.ident_pending = true; + out_state.ctrl.identActive = true; return true; } AP_ADSB::Type get_type(uint8_t instance) const; + // struct { + // // from gdl90 status message and ownship message + // bool airborne; + // bool interrogatedSinceLast; + // bool identActive; + // bool x_bit; + // bool modeAEnabled; + // bool modeCEnabled; + // bool modeSEnabled; + // bool es1090TxEnabled; + // uint16_t squawkCode; + // uint8_t NIC; + // uint8_t NACp; + // uint8_t temperature; + // char flight_id[8]; + + // uint8_t fault; + // // // no connection to ping200x + // // bool noComms = 1; + + // // // from gdl90 heartbeat message + // // bool functionFailureGnssNo3dFix; + // // bool functionFailureGnssUnavailable; + // // bool functionFailureTransmitSystem; + // // bool maintenanceRequired; + + // } tx_status; + private: static AP_ADSB *_singleton; @@ -196,6 +221,9 @@ private: // configure ADSB-out transceivers void handle_out_cfg(const mavlink_uavionix_adsb_out_cfg_t &packet); + // control ADSB-out transcievers + void handle_out_control(const mavlink_uavionix_adsb_out_control_t &packet); + // mavlink handler void handle_transceiver_report(const mavlink_channel_t chan, const mavlink_uavionix_adsb_transceiver_health_report_t &packet); @@ -236,11 +264,6 @@ 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; @@ -260,6 +283,22 @@ private: bool was_set_externally; } cfg; + struct { + bool baroCrossChecked; + uint8_t airGroundState; + bool identActive; + bool modeAEnabled; + bool modeCEnabled; + bool modeSEnabled; + bool es1090TxEnabled; + int32_t externalBaroAltitude_mm; + uint16_t squawkCode; + uint8_t emergencyState; + uint8_t callsign[8]; + bool x_bit; + } ctrl; + + mavlink_uavionix_adsb_out_status_t tx_status; } out_state; uint8_t detected_num_instances; diff --git a/libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.cpp b/libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.cpp index 8d278f75b8..86ceb7e6d7 100644 --- a/libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.cpp +++ b/libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.cpp @@ -94,13 +94,13 @@ void AP_ADSB_uAvionix_UCP::update() send_GPS_Data(); } - if (run_state.last_packet_Transponder_Status_ms == 0) { - // never seen a status packet. Let us linger in initializing for up to 10s before we declare it failed - _frontend.out_state.status = (now_ms < 10000) ? UAVIONIX_ADSB_RF_HEALTH_INITIALIZING : UAVIONIX_ADSB_RF_HEALTH_FAIL_TX; - } else if (now_ms - run_state.last_packet_Transponder_Status_ms < AP_ADSB_UAVIONIX_HEALTH_TIMEOUT_MS) { - _frontend.out_state.status = rx.decoded.transponder_status.fault ? UAVIONIX_ADSB_RF_HEALTH_FAIL_TX : UAVIONIX_ADSB_RF_HEALTH_OK; - } else { - _frontend.out_state.status = UAVIONIX_ADSB_RF_HEALTH_FAIL_TX; + // 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)) + { + _frontend.out_state.tx_status.fault |= UAVIONIX_ADSB_OUT_STATUS_FAULT_STATUS_MESSAGE_UNAVAIL; } } @@ -108,15 +108,42 @@ void AP_ADSB_uAvionix_UCP::update() void AP_ADSB_uAvionix_UCP::handle_msg(const GDL90_RX_MESSAGE &msg) { switch(msg.messageId) { - case GDL90_ID_HEARTBEAT: + 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; + 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; + + if (rx.decoded.heartbeat.status.one.maintenanceRequired) { + _frontend.out_state.tx_status.fault |= UAVIONIX_ADSB_OUT_STATUS_FAULT_MAINT_REQ; + } else { + _frontend.out_state.tx_status.fault &= ~UAVIONIX_ADSB_OUT_STATUS_FAULT_MAINT_REQ; + } + + if (rx.decoded.heartbeat.status.two.functionFailureGnssUnavailable) { + _frontend.out_state.tx_status.fault |= UAVIONIX_ADSB_OUT_STATUS_FAULT_GPS_UNAVAIL; + } else { + _frontend.out_state.tx_status.fault &= ~UAVIONIX_ADSB_OUT_STATUS_FAULT_GPS_UNAVAIL; + } + + if (rx.decoded.heartbeat.status.two.functionFailureGnssNo3dFix) { + _frontend.out_state.tx_status.fault |= UAVIONIX_ADSB_OUT_STATUS_FAULT_GPS_NO_POS; + } else { + _frontend.out_state.tx_status.fault &= ~UAVIONIX_ADSB_OUT_STATUS_FAULT_GPS_NO_POS; + } + + if (rx.decoded.heartbeat.status.two.functionFailureTransmitSystem) { + _frontend.out_state.tx_status.fault |= UAVIONIX_ADSB_OUT_STATUS_FAULT_TX_SYSTEM_FAIL; + } 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; @@ -155,6 +182,10 @@ void AP_ADSB_uAvionix_UCP::handle_msg(const GDL90_RX_MESSAGE &msg) // 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)); + 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: @@ -167,12 +198,81 @@ void AP_ADSB_uAvionix_UCP::handle_msg(const GDL90_RX_MESSAGE &msg) case GDL90_ID_SENSOR_MESSAGE: memcpy(&rx.decoded.sensor_message, msg.raw, sizeof(rx.decoded.sensor_message)); break; -#endif // AP_ADSB_UAVIONIX_UCP_CAPTURE_ALL_RX_PACKETS 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; + } + + 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.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.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; + + _frontend.out_state.tx_status.fault &= ~UAVIONIX_ADSB_OUT_STATUS_FAULT_STATUS_MESSAGE_UNAVAIL; + + 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(); + gcs().send_message(MSG_UAVIONIX_ADSB_OUT_STATUS); break; +#endif // AP_ADSB_UAVIONIX_UCP_CAPTURE_ALL_RX_PACKETS case GDL90_ID_TRANSPONDER_CONTROL: case GDL90_ID_GPS_DATA: @@ -205,7 +305,7 @@ void AP_ADSB_uAvionix_UCP::send_Transponder_Control() { GDL90_TRANSPONDER_CONTROL_MSG msg {}; msg.messageId = GDL90_ID_TRANSPONDER_CONTROL; - msg.version = 1; + msg.version = GDL90_TRANSPONDER_CONTROL_VERSION; #if CONFIG_HAL_BOARD == HAL_BOARD_SITL // when using the simulator, always declare we're on the ground to help @@ -218,12 +318,12 @@ void AP_ADSB_uAvionix_UCP::send_Transponder_Control() #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; - msg.externalBaroAltitude_mm = INT32_MAX; + msg.identActive = _frontend.out_state.ctrl.identActive; + _frontend.out_state.ctrl.identActive = false; // only send identButtonActive once per request + msg.modeAEnabled = _frontend.out_state.ctrl.modeAEnabled; + msg.modeCEnabled = _frontend.out_state.ctrl.modeCEnabled; + msg.modeSEnabled = _frontend.out_state.ctrl.modeSEnabled; + msg.es1090TxEnabled = _frontend.out_state.ctrl.es1090TxEnabled; // 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 @@ -232,7 +332,7 @@ void AP_ADSB_uAvionix_UCP::send_Transponder_Control() ((_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; + msg.squawkCode = _frontend.out_state.ctrl.squawkCode; } #if AP_ADSB_UAVIONIX_EMERGENCY_STATUS_ON_LOST_LINK @@ -243,7 +343,11 @@ void AP_ADSB_uAvionix_UCP::send_Transponder_Control() msg.emergencyState = ADSB_EMERGENCY_STATUS::ADSB_EMERGENCY_NONE; #endif - memcpy(msg.callsign, _frontend.out_state.cfg.callsign, sizeof(msg.callsign)); +#if GDL90_TRANSPONDER_CONTROL_VERSION == 2 + msg.x_bit = 0; +#endif + + memcpy(msg.callsign, _frontend.out_state.ctrl.callsign, sizeof(msg.callsign)); gdl90Transmit((GDL90_TX_MESSAGE&)msg, sizeof(msg)); } @@ -310,7 +414,7 @@ bool AP_ADSB_uAvionix_UCP::hostTransmit(uint8_t *buffer, uint16_t length) bool AP_ADSB_uAvionix_UCP::request_msg(const GDL90_MESSAGE_ID msg_id) { - const GDL90_TRANSPONDER_MESSAGE_REQUEST_V2 msg { + const GDL90_TRANSPONDER_MESSAGE_REQUEST_V2 msg = { messageId : GDL90_ID_MESSAGE_REQUEST, version : 2, reqMsgId : msg_id diff --git a/libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.h b/libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.h index c5aab0aa93..9b8397bfb4 100644 --- a/libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.h +++ b/libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.h @@ -85,6 +85,8 @@ private: 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; } 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 d89bd7fefa..4a386c7386 100644 --- a/libraries/AP_ADSB/GDL90_protocol/GDL90_Message_Structs.h +++ b/libraries/AP_ADSB/GDL90_protocol/GDL90_Message_Structs.h @@ -64,6 +64,8 @@ typedef enum __attribute__((__packed__)) // 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; @@ -80,7 +82,49 @@ typedef struct __attribute__((__packed__)) 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_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; + 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; +} GDL90_TRANSPONDER_STATUS_MSG; +#endif +#if GDL90_TRANSPONDER_STATUS_VERSION == 3 typedef struct __attribute__((__packed__)) { GDL90_MESSAGE_ID messageId; @@ -104,10 +148,7 @@ typedef struct __attribute__((__packed__)) 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) - +#endif typedef struct __attribute__((__packed__))