From c81cb699921e94a5e27d9ee73eeab90dd389e903 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Thu, 9 Jan 2025 15:44:31 +1100 Subject: [PATCH] AP_ESC_Telem: fix timeout race The timeout for non-RPM telemetry is vulnerable to a similar race as the RPM. This change makes the timeout logic consistent between the two. --- libraries/AP_ESC_Telem/AP_ESC_Telem.cpp | 17 ++++++++++------- libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.cpp | 6 +++--- libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h | 5 ++++- 3 files changed, 17 insertions(+), 11 deletions(-) diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp index 63e067525bda0..2b15c02f794fd 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp @@ -104,13 +104,12 @@ uint8_t AP_ESC_Telem::get_motor_frequencies_hz(uint8_t nfreqs, float* freqs) con // ESC_TELEM_DATA_TIMEOUT_MS/ESC_RPM_DATA_TIMEOUT_US uint32_t AP_ESC_Telem::get_active_esc_mask() const { uint32_t ret = 0; - const uint32_t now = AP_HAL::millis(); for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) { if (_telem_data[i].last_update_ms == 0 && !was_rpm_data_ever_reported(_rpm_data[i])) { // have never seen telem from this ESC continue; } - if (_telem_data[i].stale(now) && !_rpm_data[i].data_valid) { + if (_telem_data[i].stale() && !_rpm_data[i].data_valid) { continue; } ret |= (1U << i); @@ -123,13 +122,12 @@ uint8_t AP_ESC_Telem::get_max_rpm_esc() const { uint32_t ret = 0; float max_rpm = 0; - const uint32_t now = AP_HAL::millis(); for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) { if (_telem_data[i].last_update_ms == 0 && !was_rpm_data_ever_reported(_rpm_data[i])) { // have never seen telem from this ESC continue; } - if (_telem_data[i].stale(now) && !_rpm_data[i].data_valid) { + if (_telem_data[i].stale() && !_rpm_data[i].data_valid) { continue; } if (_rpm_data[i].rpm > max_rpm) { @@ -405,8 +403,6 @@ void AP_ESC_Telem::send_esc_telemetry_mavlink(uint8_t mav_chan) return; } - const uint32_t now = AP_HAL::millis(); - // loop through groups of 4 ESCs const uint8_t esc_offset = constrain_int16(mavlink_offset, 0, ESC_TELEM_MAX_ESCS-1); @@ -427,7 +423,7 @@ void AP_ESC_Telem::send_esc_telemetry_mavlink(uint8_t mav_chan) for (uint8_t j=0; j<4; j++) { const uint8_t esc_id = (i * 4 + j) + esc_offset; if (esc_id < ESC_TELEM_MAX_ESCS && - (!_telem_data[esc_id].stale(now) || _rpm_data[esc_id].data_valid)) { + (!_telem_data[esc_id].stale() || _rpm_data[esc_id].data_valid)) { all_stale = false; break; } @@ -584,6 +580,7 @@ void AP_ESC_Telem::update_telem_data(const uint8_t esc_index, const AP_ESC_Telem telemdata.count++; telemdata.types |= data_mask; telemdata.last_update_ms = AP_HAL::millis(); + telemdata.any_data_valid = true; } // record an update to the RPM together with timestamp, this allows the notch values to be slewed @@ -807,6 +804,12 @@ void AP_ESC_Telem::update() if (AP_HAL::timeout_expired(last_updated_us, now_us, ESC_RPM_DATA_TIMEOUT_US)) { _rpm_data[i].data_valid = false; } + const uint32_t last_telem_data_ms = _telem_data[i].last_update_ms; + const uint32_t now_ms = AP_HAL::millis(); + // Invalidate telemetry data if not received for too long + if (AP_HAL::timeout_expired(last_telem_data_ms, now_ms, ESC_TELEM_DATA_TIMEOUT_MS)) { + _telem_data[i].any_data_valid = false; + } } } diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.cpp index a63c0191c5c85..a197f6bdc1287 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.cpp @@ -48,9 +48,9 @@ void AP_ESC_Telem_Backend::update_telem_data(const uint8_t esc_index, const Tele /* return true if the data is stale */ -bool AP_ESC_Telem_Backend::TelemetryData::stale(uint32_t now_ms) const volatile +bool AP_ESC_Telem_Backend::TelemetryData::stale() const volatile { - return last_update_ms == 0 || now_ms - last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS; + return last_update_ms == 0 || !any_data_valid; } /* @@ -62,7 +62,7 @@ bool AP_ESC_Telem_Backend::TelemetryData::valid(const uint16_t type_mask) const // Requested type not available return false; } - return !stale(AP_HAL::millis()); + return !stale(); } #endif diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h b/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h index f127c0d71ab92..b41dc48519e87 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h @@ -30,8 +30,11 @@ class AP_ESC_Telem_Backend { uint8_t power_percentage; // Percentage of output power #endif // AP_EXTENDED_ESC_TELEM_ENABLED + // set to false if no data has been received within the timeout period + bool any_data_valid; + // return true if the data is stale - bool stale(uint32_t now_ms) const volatile; + bool stale() const volatile; // return true if the requested type of data is available and not stale bool valid(const uint16_t type_mask) const volatile;