From 3b804b90cb58d8184c7532bfa2c059dd6a66560a Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 21 Jan 2023 19:15:43 +0000 Subject: [PATCH] GCS_MAVLink: add support for AIRSPEED message --- libraries/GCS_MAVLink/GCS_Common.cpp | 14 ++++++++++++++ libraries/GCS_MAVLink/ap_message.h | 1 + 2 files changed, 15 insertions(+) diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 053e7294aaeeeb..cfeac7f31fa5cc 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -1070,6 +1070,9 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c #endif #if AP_MAVLINK_MSG_RELAY_STATUS_ENABLED { MAVLINK_MSG_ID_RELAY_STATUS, MSG_RELAY_STATUS}, +#endif +#if AP_AIRSPEED_ENABLED + { MAVLINK_MSG_ID_AIRSPEED, MSG_AIRSPEED}, #endif }; @@ -5957,6 +5960,17 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) send_scaled_pressure3(); break; +#if AP_AIRSPEED_ENABLED + case MSG_AIRSPEED: + { + AP_Airspeed *airspeed = AP_Airspeed::get_singleton(); + if (airspeed != nullptr) { + airspeed->send_mavlink_airspeed(*this); + } + } + break; +#endif + case MSG_SERVO_OUTPUT_RAW: CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW); send_servo_output_raw(); diff --git a/libraries/GCS_MAVLink/ap_message.h b/libraries/GCS_MAVLink/ap_message.h index 973c48d51bad10..75437268dd89ed 100644 --- a/libraries/GCS_MAVLink/ap_message.h +++ b/libraries/GCS_MAVLink/ap_message.h @@ -90,5 +90,6 @@ enum ap_message : uint8_t { MSG_HYGROMETER, MSG_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE, MSG_RELAY_STATUS, + MSG_AIRSPEED, MSG_LAST // MSG_LAST must be the last entry in this enum };