diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 91f5176a95..2f0be9fd89 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -321,6 +321,7 @@ public: #if HAL_HIGH_LATENCY2_ENABLED void send_high_latency2() const; #endif // HAL_HIGH_LATENCY2_ENABLED + void send_uavionix_adsb_out_status() const; // lock a channel, preventing use by MAVLink void lock(bool _lock) { diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 990c86adbc..196b682f8e 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -890,6 +890,7 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c { MAVLINK_MSG_ID_WATER_DEPTH, MSG_WATER_DEPTH}, { MAVLINK_MSG_ID_HIGH_LATENCY2, MSG_HIGH_LATENCY2}, { MAVLINK_MSG_ID_AIS_VESSEL, MSG_AIS_VESSEL}, + { MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_STATUS, MSG_UAVIONIX_ADSB_OUT_STATUS}, }; for (uint8_t i=0; iident_is_active()) { - result = MAV_RESULT_TEMPORARILY_REJECTED; - } else if (AP::ADSB()->ident_start()) { + case MAV_CMD_DO_ADSB_OUT_IDENT: + if ((AP::ADSB() != nullptr) && AP::ADSB()->ident_start()) { result = MAV_RESULT_ACCEPTED; - } else { + } + else { result = MAV_RESULT_FAILED; } -#else - result = MAV_RESULT_UNSUPPORTED; -#endif break; +#endif case MAV_CMD_PREFLIGHT_UAVCAN: result = handle_command_preflight_can(packet); @@ -4936,6 +4933,16 @@ void GCS_MAVLINK::send_water_depth() const #endif } +void GCS_MAVLINK::send_uavionix_adsb_out_status() const +{ +#if HAL_ADSB_ENABLED + AP_ADSB *adsb = AP::ADSB(); + if (adsb != nullptr) { + adsb->send_adsb_out_status(chan); + } +#endif +} + bool GCS_MAVLINK::try_send_message(const enum ap_message id) { bool ret = true; @@ -5258,6 +5265,11 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) break; + case MSG_UAVIONIX_ADSB_OUT_STATUS: + CHECK_PAYLOAD_SIZE(UAVIONIX_ADSB_OUT_STATUS); + send_uavionix_adsb_out_status(); + break; + default: // try_send_message must always at some stage return true for // a message, or we will attempt to infinitely retry the diff --git a/libraries/GCS_MAVLink/ap_message.h b/libraries/GCS_MAVLink/ap_message.h index a9cbcdede5..a38c24b1ff 100644 --- a/libraries/GCS_MAVLink/ap_message.h +++ b/libraries/GCS_MAVLink/ap_message.h @@ -80,5 +80,6 @@ enum ap_message : uint8_t { MSG_HIGH_LATENCY2, MSG_AIS_VESSEL, MSG_MCU_STATUS, + MSG_UAVIONIX_ADSB_OUT_STATUS, MSG_LAST // MSG_LAST must be the last entry in this enum };