From b2a5283cdc2d505da9d9f5a3bb766cf19e3a7062 Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Wed, 22 May 2024 17:21:06 +0100
Subject: [PATCH] AP_RCTelemetry: use get_max_rpm_esc()

---
 .../AP_RCTelemetry/AP_Spektrum_Telem.cpp      | 32 ++++++++-----------
 1 file changed, 13 insertions(+), 19 deletions(-)

diff --git a/libraries/AP_RCTelemetry/AP_Spektrum_Telem.cpp b/libraries/AP_RCTelemetry/AP_Spektrum_Telem.cpp
index 3e669206ec..428c957240 100644
--- a/libraries/AP_RCTelemetry/AP_Spektrum_Telem.cpp
+++ b/libraries/AP_RCTelemetry/AP_Spektrum_Telem.cpp
@@ -33,9 +33,7 @@
 #include <AP_Baro/AP_Baro.h>
 #include <AP_RTC/AP_RTC.h>
 #include <AP_SerialManager/AP_SerialManager.h>
-#ifdef HAVE_AP_BLHELI_SUPPORT
-#include <AP_BLheli/AP_BLHeli.h>
-#endif
+#include <AP_ESC_Telem/AP_ESC_Telem.h>
 #include <math.h>
 
 #if HAL_SPEKTRUM_TELEM_ENABLED
@@ -564,29 +562,25 @@ void AP_Spektrum_Telem::calc_gps_status()
 // prepare ESC information - B/E
 void AP_Spektrum_Telem::calc_esc()
 {
-#ifdef HAVE_AP_BLHELI_SUPPORT
-    AP_BLHeli* blh = AP_BLHeli::get_singleton();
-
-    if (blh == nullptr) {
-        return;
-    }
-
-    AP_BLHeli::telem_data td;
-
-    if (!blh->get_telem_data(0, td)) {
-        return;
+#if HAL_WITH_ESC_TELEM
+    uint8_t esc = AP::esc_telem().get_max_rpm_esc();
+    const volatile AP_ESC_Telem_Backend::TelemetryData& td = AP::esc_telem().get_telem_data(esc); // ideally should rotate between ESCs
+    float rpm = 0.0f;
+    uint16_t rpmdata = 0xFFFFU;
+    if (AP::esc_telem().get_rpm(esc, rpm)) {
+        rpmdata = uint16_t(roundf(rpm));
     }
 
 	_telem.esc.identifier = TELE_DEVICE_ESC;	    // Source device = 0x20
 	_telem.esc.sID = 0;									// Secondary ID
-	_telem.esc.RPM = htobe16(uint16_t(roundf(blh->get_average_motor_frequency_hz() * 60)));	// Electrical RPM, 10RPM (0-655340 RPM)  0xFFFF --> "No data"
-	_telem.esc.voltsInput = htobe16(td.voltage);	    // Volts, 0.01v (0-655.34V)       0xFFFF --> "No data"
-	_telem.esc.tempFET = htobe16(td.temperature * 10);	// Temperature, 0.1C (0-6553.4C)  0xFFFF --> "No data"
-	_telem.esc.currentMotor = htobe16(td.current);		// Current, 10mA (0-655.34A)      0xFFFF --> "No data"
+	_telem.esc.RPM = htobe16(rpmdata);	// Electrical RPM, 10RPM (0-655340 RPM)  0xFFFF --> "No data"
+	_telem.esc.voltsInput = htobe16(td.voltage * 100);	    // Volts, 0.01v (0-655.34V)       0xFFFF --> "No data"
+	_telem.esc.tempFET = htobe16(td.temperature_cdeg * 10);	// Temperature, 0.1C (0-6553.4C)  0xFFFF --> "No data"
+	_telem.esc.currentMotor = htobe16(td.current * 100);		// Current, 10mA (0-655.34A)      0xFFFF --> "No data"
 	_telem.esc.tempBEC = 0xFFFF;						// Temperature, 0.1C (0-6553.4C)  0xFFFF --> "No data"
 	_telem.esc.currentBEC = 0xFF;						// BEC Current, 100mA (0-25.4A)   0xFF ----> "No data"
 	_telem.esc.voltsBEC = 0xFF;							// BEC Volts, 0.05V (0-12.70V)    0xFF ----> "No data"
-	_telem.esc.throttle = 0xFF;							// 0.5% (0-100%)                  0xFF ----> "No data"
+    _telem.esc.throttle = 0xFF;							// 0.5% (0-100%)                  0xFF ----> "No data"
 	_telem.esc.powerOut = 0xFF;							// Power Output, 0.5% (0-127%)    0xFF ----> "No data"
     _telem_pending = true;
 #endif