From fca69251294d858a68b5e98c1b548c058afc3e78 Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Wed, 3 Mar 2021 09:03:38 +0000
Subject: [PATCH] GCS_MAVLink: use generic send_esc_telemetry_mavlink() from
 AP_ESC_Telem

---
 libraries/GCS_MAVLink/GCS_Common.cpp | 55 ++--------------------------
 1 file changed, 3 insertions(+), 52 deletions(-)

diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp
index 0af9ea76ff..f759422154 100644
--- a/libraries/GCS_MAVLink/GCS_Common.cpp
+++ b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -5058,60 +5058,11 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
         send_autopilot_version();
         break;
 
-    case MSG_ESC_TELEMETRY: {
-#ifdef HAVE_AP_BLHELI_SUPPORT
-        CHECK_PAYLOAD_SIZE(ESC_TELEMETRY_1_TO_4);
-        AP_BLHeli *blheli = AP_BLHeli::get_singleton();
-        if (blheli) {
-            blheli->send_esc_telemetry_mavlink(uint8_t(chan));
-        }
-#endif
-#if HAL_MAX_CAN_PROTOCOL_DRIVERS
-        uint8_t num_drivers = AP::can().get_num_drivers();
-
-        for (uint8_t i = 0; i < num_drivers; i++) {
-            switch (AP::can().get_driver_type(i)) {
-                case AP_CANManager::Driver_Type_KDECAN: {
-// To be replaced with macro saying if KDECAN library is included
-#if APM_BUILD_TYPE(APM_BUILD_ArduCopter) || APM_BUILD_TYPE(APM_BUILD_ArduPlane) || APM_BUILD_TYPE(APM_BUILD_ArduSub)
-                    AP_KDECAN *ap_kdecan = AP_KDECAN::get_kdecan(i);
-                    if (ap_kdecan != nullptr) {
-                        ap_kdecan->send_mavlink(uint8_t(chan));
-                    }
-#endif
-                    break;
-                }
-                case AP_CANManager::Driver_Type_ToshibaCAN: {
-                    AP_ToshibaCAN *ap_tcan = AP_ToshibaCAN::get_tcan(i);
-                    if (ap_tcan != nullptr) {
-                        ap_tcan->send_esc_telemetry_mavlink(uint8_t(chan));
-                    }
-                    break;
-                }
-#if HAL_PICCOLO_CAN_ENABLE
-                case AP_CANManager::Driver_Type_PiccoloCAN: {
-                    AP_PiccoloCAN *ap_pcan = AP_PiccoloCAN::get_pcan(i);
-                    if (ap_pcan != nullptr) {
-                        ap_pcan->send_esc_telemetry_mavlink(uint8_t(chan));
-                    }
-                    break;
-                }
-#endif
-                case AP_CANManager::Driver_Type_UAVCAN: {
-                    AP_UAVCAN *ap_uavcan = AP_UAVCAN::get_uavcan(i);
-                    if (ap_uavcan != nullptr) {
-                        ap_uavcan->send_esc_telemetry_mavlink(uint8_t(chan));
-                    }
-                    break;
-                }
-                case AP_CANManager::Driver_Type_None:
-                default:
-                    break;
-            }
-        }
+    case MSG_ESC_TELEMETRY:
+#if HAL_WITH_ESC_TELEM
+        AP::esc_telem().send_esc_telemetry_mavlink(uint8_t(chan));
 #endif
         break;
-    }
 
     case MSG_EFI_STATUS: {
 #if EFI_ENABLED