diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index e2ce3fef1a..05462829df 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -81,6 +81,7 @@ enum ap_message : uint8_t { MSG_BATTERY_STATUS, MSG_AOA_SSA, MSG_LANDING, + MSG_ESC_TELEMETRY, MSG_NAMED_FLOAT, MSG_LAST // MSG_LAST must be the last entry in this enum }; diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index d9fbdce7b4..2fc2d9625d 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include "GCS.h" @@ -2985,6 +2986,17 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) send_vibration(); break; +#ifdef HAVE_AP_BLHELI_SUPPORT + case MSG_ESC_TELEMETRY: { + CHECK_PAYLOAD_SIZE(ESC_TELEMETRY_1_TO_4); + AP_BLHeli *blheli = AP_BLHeli::get_instance(); + if (blheli) { + blheli->send_esc_telemetry_mavlink(uint8_t(chan)); + } + break; + } +#endif + default: // try_send_message must always at some stage return true for // a message, or we will attempt to infinitely retry the