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

AP_ESC_Telem: don't provide zero udpates for ESCs that have never been used

This commit is contained in:
Andy Piper 2022-08-20 12:06:58 +02:00 committed by Andrew Tridgell
parent c68b886700
commit 28d0086a97
1 changed files with 6 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_ESC_Telem/AP_ESC_Telem.cpp
View File

@ -79,13 +79,14 @@ uint8_t AP_ESC_Telem::get_motor_frequencies_hz(uint8_t nfreqs, float* freqs) con
uint8_t valid_escs = 0;
// average the rpm of each motor as reported by BLHeli and convert to Hz
for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS && i < nfreqs; i++) {
for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS && valid_escs < nfreqs; i++) {
float rpm;
if (get_rpm(i, rpm)) {
freqs[i] = rpm * (1.0f / 60.0f);
valid_escs++;
} else {
freqs[i] = 0.0f;
freqs[valid_escs++] = rpm * (1.0f / 60.0f);
} else if (_rpm_data[i].last_update_us > 0) {
// if we have ever received data on an ESC, mark it as valid but with no data
// this prevents large frequency shifts when ESCs disappear
freqs[valid_escs++] = 0.0f;
}
}