From 6a3a2b40bf67ffb6dbeeba5c79225efd0a60a484 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Thu, 2 Jan 2025 12:13:25 +1100 Subject: [PATCH] AP_ESC_Telem: fix RPM timeout race The RPM telemetry data structure can get updated by another thread at any time. This can cause (now - last_update_us) to be negative, which looks like the data is nearly UINT32_MAX microseconds stale. To fix this, we need to use signed integers for the time difference. The data_valid flag prevents us from accidentally counting extremely stale data as valid when the signed time difference wraps around, so this is safe. --- libraries/AP_ESC_Telem/AP_ESC_Telem.cpp | 12 +++++++----- libraries/AP_ESC_Telem/AP_ESC_Telem.h | 4 ++-- 2 files changed, 9 insertions(+), 7 deletions(-) diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp index f4ff9dea05a22..3486803d255b8 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp @@ -27,7 +27,7 @@ //#define ESC_TELEM_DEBUG -#define ESC_RPM_CHECK_TIMEOUT_US 210000UL // timeout for motor running validity +#define ESC_RPM_CHECK_TIMEOUT_US 210000L // timeout for motor running validity extern const AP_HAL::HAL& hal; @@ -202,7 +202,7 @@ bool AP_ESC_Telem::get_rpm(uint8_t esc_index, float& rpm) const const uint32_t now = AP_HAL::micros(); if (rpm_data_within_timeout(rpmdata, now, ESC_RPM_DATA_TIMEOUT_US)) { - const float slew = MIN(1.0f, (now - rpmdata.last_update_us) * rpmdata.update_rate_hz * (1.0f / 1e6f)); + const float slew = constrain_float(int32_t(now - rpmdata.last_update_us) * rpmdata.update_rate_hz * (1.0f / 1e6f), 0, 1); rpm = (rpmdata.prev_rpm + (rpmdata.rpm - rpmdata.prev_rpm) * slew); #if AP_SCRIPTING_ENABLED @@ -811,16 +811,18 @@ void AP_ESC_Telem::update() const uint32_t now_us = AP_HAL::micros(); for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) { // Invalidate RPM data if not received for too long - if ((now_us - _rpm_data[i].last_update_us) > ESC_RPM_DATA_TIMEOUT_US) { + // (last_update_us might be fresher than now_us, so we use a signed difference) + if (int32_t(now_us - _rpm_data[i].last_update_us) > ESC_RPM_DATA_TIMEOUT_US) { _rpm_data[i].data_valid = false; } } } -bool AP_ESC_Telem::rpm_data_within_timeout(const volatile AP_ESC_Telem_Backend::RpmData &instance, const uint32_t now_us, const uint32_t timeout_us) +bool AP_ESC_Telem::rpm_data_within_timeout(const volatile AP_ESC_Telem_Backend::RpmData &instance, const uint32_t now_us, const int32_t timeout_us) { // easy case, has the time window been crossed so it's invalid - if ((now_us - instance.last_update_us) > timeout_us) { + // (last_update_us might be fresher than now_us, so we use a signed difference) + if (int32_t(now_us - instance.last_update_us) > timeout_us) { return false; } // we never got a valid data, to it's invalid diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.h b/libraries/AP_ESC_Telem/AP_ESC_Telem.h index cf4a5a9cc2d57..703be0af399e1 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.h +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.h @@ -13,7 +13,7 @@ static_assert(ESC_TELEM_MAX_ESCS > 0, "Cannot have 0 ESC telemetry instances"); #define ESC_TELEM_DATA_TIMEOUT_MS 5000UL -#define ESC_RPM_DATA_TIMEOUT_US 1000000UL +#define ESC_RPM_DATA_TIMEOUT_US 1000000L class AP_ESC_Telem { public: @@ -134,7 +134,7 @@ class AP_ESC_Telem { private: // helper that validates RPM data - static bool rpm_data_within_timeout (const volatile AP_ESC_Telem_Backend::RpmData &instance, const uint32_t now_us, const uint32_t timeout_us); + static bool rpm_data_within_timeout (const volatile AP_ESC_Telem_Backend::RpmData &instance, const uint32_t now_us, const int32_t timeout_us); static bool was_rpm_data_ever_reported (const volatile AP_ESC_Telem_Backend::RpmData &instance); #if AP_EXTENDED_DSHOT_TELEM_V2_ENABLED