Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_RCTelemetry: use get_max_rpm_esc()

This commit is contained in:
Andy Piper 2024-05-22 17:21:06 +01:00 committed by Randy Mackay
parent 1254ad51d4
commit 0acd075746
1 changed files with 13 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
libraries/AP_RCTelemetry/AP_Spektrum_Telem.cpp
View File

@ -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