Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_ESC_Telem: make sure active mask takes into account rpm updates

Browse Source
This commit is contained in:
Andy Piper 2022-09-06 15:55:43 +01:00 committed by Peter Barker
parent 15dd2e6062
commit 7510b68a8e
1 changed files with 6 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_ESC_Telem/AP_ESC_Telem.cpp
View File

@ -95,16 +95,18 @@ uint8_t AP_ESC_Telem::get_motor_frequencies_hz(uint8_t nfreqs, float* freqs) con
return MIN(valid_escs, nfreqs); return MIN(valid_escs, nfreqs);
} }
// get mask of ESCs that sent valid telemetry data in the last // get mask of ESCs that sent valid telemetry and/or rpm data in the last
// ESC_TELEM_DATA_TIMEOUT_MS // ESC_TELEM_DATA_TIMEOUT_MS/ESC_RPM_DATA_TIMEOUT_US
uint32_t AP_ESC_Telem::get_active_esc_mask() const { uint32_t AP_ESC_Telem::get_active_esc_mask() const {
uint32_t ret = 0; uint32_t ret = 0;
const uint32_t now = AP_HAL::millis(); const uint32_t now = AP_HAL::millis();
uint32_t now_us = AP_HAL::micros();
for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) { for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) {
if (now - _telem_data[i].last_update_ms >= ESC_TELEM_DATA_TIMEOUT_MS) { if (now - _telem_data[i].last_update_ms >= ESC_TELEM_DATA_TIMEOUT_MS
&& now_us - _rpm_data[i].last_update_us >= ESC_RPM_DATA_TIMEOUT_US) {
continue; continue;
} }
if (_telem_data[i].last_update_ms == 0) { if (_telem_data[i].last_update_ms == 0 && _rpm_data[i].last_update_us == 0) {
// have never seen telem from this ESC // have never seen telem from this ESC
continue; continue;
} }