diff --git a/libraries/AP_RCTelemetry/AP_Spektrum_Telem.cpp b/libraries/AP_RCTelemetry/AP_Spektrum_Telem.cpp index 4084abe15a..7caa87cfc9 100644 --- a/libraries/AP_RCTelemetry/AP_Spektrum_Telem.cpp +++ b/libraries/AP_RCTelemetry/AP_Spektrum_Telem.cpp @@ -33,7 +33,7 @@ #include #include #include -#include +#include #include #if HAL_SPEKTRUM_TELEM_ENABLED @@ -577,25 +577,15 @@ void AP_Spektrum_Telem::calc_gps_status() // prepare ESC information - B/E void AP_Spektrum_Telem::calc_esc() { -#if 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 + const volatile AP_ESC_Telem_Backend::TelemetryData& td = AP::esc_telem().get_telem_data(0); // ideally should rotate between ESCs _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(uint16_t(roundf(AP::esc_telem().get_average_motor_frequency_hz() * 60))); // 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"