From 45e1b56f17592fe54cf644f241cec2fc15d4f2b7 Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Tue, 23 Feb 2021 22:05:37 +0000
Subject: [PATCH] AP_OSD: generalise ESC telemetry to allow data from other
 ESCs

---
 libraries/AP_OSD/AP_OSD.h          | 10 ++----
 libraries/AP_OSD/AP_OSD_Screen.cpp | 55 +++++++++++-------------------
 2 files changed, 23 insertions(+), 42 deletions(-)

diff --git a/libraries/AP_OSD/AP_OSD.h b/libraries/AP_OSD/AP_OSD.h
index 24eead6ea9..6d163fb3f7 100644
--- a/libraries/AP_OSD/AP_OSD.h
+++ b/libraries/AP_OSD/AP_OSD.h
@@ -19,7 +19,7 @@
 #include <AP_HAL/AP_HAL.h>
 #include <AP_Param/AP_Param.h>
 #include <AP_Math/AP_Math.h>
-#include <AP_BLHeli/AP_BLHeli.h>
+#include <AP_ESC_Telem/AP_ESC_Telem.h>
 #include <RC_Channel/RC_Channel.h>
 #include <AP_Param/AP_Param.h>
 #include <GCS_MAVLink/GCS.h>
@@ -155,13 +155,11 @@ private:
     AP_OSD_Setting aspd1{false, 0, 0};
     AP_OSD_Setting aspd2{false, 0, 0};
     AP_OSD_Setting vspeed{true, 24, 9};
-
-#ifdef HAVE_AP_BLHELI_SUPPORT
+#if HAL_WITH_ESC_TELEM
     AP_OSD_Setting blh_temp {false, 24, 13};
     AP_OSD_Setting blh_rpm{false, 22, 12};
     AP_OSD_Setting blh_amps{false, 24, 14};
 #endif
-
     AP_OSD_Setting gps_latitude{true, 9, 13};
     AP_OSD_Setting gps_longitude{true, 9, 14};
     AP_OSD_Setting roll_angle{false, 0, 0};
@@ -228,13 +226,11 @@ private:
     //helper functions
     void draw_speed(uint8_t x, uint8_t y, float angle_rad, float magnitude);
     void draw_distance(uint8_t x, uint8_t y, float distance);
-
-#ifdef HAVE_AP_BLHELI_SUPPORT
+#if HAL_WITH_ESC_TELEM
     void draw_blh_temp(uint8_t x, uint8_t y);
     void draw_blh_rpm(uint8_t x, uint8_t y);
     void draw_blh_amps(uint8_t x, uint8_t y);
 #endif
-
     void draw_gps_latitude(uint8_t x, uint8_t y);
     void draw_gps_longitude(uint8_t x, uint8_t y);
     void draw_roll_angle(uint8_t x, uint8_t y);
diff --git a/libraries/AP_OSD/AP_OSD_Screen.cpp b/libraries/AP_OSD/AP_OSD_Screen.cpp
index 9707fe64a2..9a52d84a9d 100644
--- a/libraries/AP_OSD/AP_OSD_Screen.cpp
+++ b/libraries/AP_OSD/AP_OSD_Screen.cpp
@@ -343,8 +343,7 @@ const AP_Param::GroupInfo AP_OSD_Screen::var_info[] = {
     // @Description: Vertical position on screen
     // @Range: 0 15
     AP_SUBGROUPINFO(vspeed, "VSPEED", 20, AP_OSD_Screen, AP_OSD_Setting),
-
-#ifdef HAVE_AP_BLHELI_SUPPORT
+#if HAL_WITH_ESC_TELEM
     // @Param: BLHTEMP_EN
     // @DisplayName: BLHTEMP_EN
     // @Description: Displays first esc's temp
@@ -393,7 +392,6 @@ const AP_Param::GroupInfo AP_OSD_Screen::var_info[] = {
     // @Range: 0 15
     AP_SUBGROUPINFO(blh_amps, "BLHAMPS", 23, AP_OSD_Screen, AP_OSD_Setting),
 #endif
-
     // @Param: GPSLAT_EN
     // @DisplayName: GPSLAT_EN
     // @Description: Displays GPS latitude
@@ -1605,52 +1603,39 @@ void AP_OSD_Screen::draw_vspeed(uint8_t x, uint8_t y)
     }
 }
 
-#ifdef HAVE_AP_BLHELI_SUPPORT
-
+#if HAL_WITH_ESC_TELEM
 void AP_OSD_Screen::draw_blh_temp(uint8_t x, uint8_t y)
 {
-    AP_BLHeli *blheli = AP_BLHeli::get_singleton();
-    if (blheli) {
-        AP_BLHeli::telem_data td;
-        // first parameter is index into array of ESC's.  Hardwire to zero (first) for now.
-        if (!blheli->get_telem_data(0, td)) {
-            return;
-        }
-
-        // AP_BLHeli & blh = AP_BLHeli::AP_BLHeli();
-        uint8_t esc_temp = td.temperature;
-        backend->write(x, y, false, "%3d%c", (int)u_scale(TEMPERATURE, esc_temp), u_icon(TEMPERATURE));
+    int16_t etemp;
+    // first parameter is index into array of ESC's.  Hardwire to zero (first) for now.
+    if (!AP::esc_telem().get_temperature(0, etemp)) {
+        return;
     }
+
+    uint8_t esc_temp = uint8_t(etemp);
+    backend->write(x, y, false, "%3d%c", (int)u_scale(TEMPERATURE, esc_temp), u_icon(TEMPERATURE));
 }
 
 void AP_OSD_Screen::draw_blh_rpm(uint8_t x, uint8_t y)
 {
-    AP_BLHeli *blheli = AP_BLHeli::get_singleton();
-    if (blheli) {
-        AP_BLHeli::telem_data td;
-        // first parameter is index into array of ESC's.  Hardwire to zero (first) for now.
-        if (!blheli->get_telem_data(0, td)) {
-            return;
-        }
-        backend->write(x, y, false, "%3.1f%c%c", td.rpm * 0.001f, SYM_KILO, SYM_RPM);
+    float rpm;
+    // first parameter is index into array of ESC's.  Hardwire to zero (first) for now.
+    if (!AP::esc_telem().get_rpm(0, rpm)) {
+        return;
     }
+    backend->write(x, y, false, "%3.1f%c%c", uint16_t(rpm) * 0.001f, SYM_KILO, SYM_RPM);
 }
 
 void AP_OSD_Screen::draw_blh_amps(uint8_t x, uint8_t y)
 {
-    AP_BLHeli *blheli = AP_BLHeli::get_singleton();
-    if (blheli) {
-        AP_BLHeli::telem_data td;
-        // first parameter is index into array of ESC's.  Hardwire to zero (first) for now.
-        if (!blheli->get_telem_data(0, td)) {
-            return;
-        }
-
-        float esc_amps = td.current * 0.01;
-        backend->write(x, y, false, "%4.1f%c", esc_amps, SYM_AMP);
+    float esc_amps;
+    // first parameter is index into array of ESC's.  Hardwire to zero (first) for now.
+    if (!AP::esc_telem().get_current(0, esc_amps)) {
+        return;
     }
+    backend->write(x, y, false, "%4.1f%c", esc_amps, SYM_AMP);
 }
-#endif  //HAVE_AP_BLHELI_SUPPORT
+#endif
 
 void AP_OSD_Screen::draw_gps_latitude(uint8_t x, uint8_t y)
 {