From a4c3f679d7629e85ce8482ce115865de9d9e976b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 25 Oct 2023 17:48:48 +1100 Subject: [PATCH] AP_ESC_Telem: added stale() method for ESC telem prevents use of stale data when close to zero time --- libraries/AP_ESC_Telem/AP_ESC_Telem.cpp | 16 ++++++++-------- libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.cpp | 13 ++++++++++++- libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h | 3 +++ 3 files changed, 23 insertions(+), 9 deletions(-) diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp index 9945d16ce4578..0ebc163c2d27e 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp @@ -107,7 +107,7 @@ uint32_t AP_ESC_Telem::get_active_esc_mask() const { // have never seen telem from this ESC continue; } - if (now - _telem_data[i].last_update_ms >= ESC_TELEM_DATA_TIMEOUT_MS + if (_telem_data[i].stale(now) && !rpm_data_within_timeout(_rpm_data[i], now_us, ESC_RPM_DATA_TIMEOUT_US)) { continue; } @@ -211,7 +211,7 @@ bool AP_ESC_Telem::get_raw_rpm(uint8_t esc_index, float& rpm) const bool AP_ESC_Telem::get_temperature(uint8_t esc_index, int16_t& temp) const { if (esc_index >= ESC_TELEM_MAX_ESCS - || AP_HAL::millis() - _telem_data[esc_index].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS + || _telem_data[esc_index].stale() || !(_telem_data[esc_index].types & (AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE | AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE_EXTERNAL))) { return false; } @@ -223,7 +223,7 @@ bool AP_ESC_Telem::get_temperature(uint8_t esc_index, int16_t& temp) const bool AP_ESC_Telem::get_motor_temperature(uint8_t esc_index, int16_t& temp) const { if (esc_index >= ESC_TELEM_MAX_ESCS - || AP_HAL::millis() - _telem_data[esc_index].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS + || _telem_data[esc_index].stale() || !(_telem_data[esc_index].types & (AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE | AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE_EXTERNAL))) { return false; } @@ -251,7 +251,7 @@ bool AP_ESC_Telem::get_highest_motor_temperature(int16_t& temp) const bool AP_ESC_Telem::get_current(uint8_t esc_index, float& amps) const { if (esc_index >= ESC_TELEM_MAX_ESCS - || AP_HAL::millis() - _telem_data[esc_index].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS + || _telem_data[esc_index].stale() || !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::CURRENT)) { return false; } @@ -263,7 +263,7 @@ bool AP_ESC_Telem::get_current(uint8_t esc_index, float& amps) const bool AP_ESC_Telem::get_voltage(uint8_t esc_index, float& volts) const { if (esc_index >= ESC_TELEM_MAX_ESCS - || AP_HAL::millis() - _telem_data[esc_index].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS + || _telem_data[esc_index].stale() || !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::VOLTAGE)) { return false; } @@ -275,7 +275,7 @@ bool AP_ESC_Telem::get_voltage(uint8_t esc_index, float& volts) const bool AP_ESC_Telem::get_consumption_mah(uint8_t esc_index, float& consumption_mah) const { if (esc_index >= ESC_TELEM_MAX_ESCS - || AP_HAL::millis() - _telem_data[esc_index].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS + || _telem_data[esc_index].stale() || !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::CONSUMPTION)) { return false; } @@ -287,7 +287,7 @@ bool AP_ESC_Telem::get_consumption_mah(uint8_t esc_index, float& consumption_mah bool AP_ESC_Telem::get_usage_seconds(uint8_t esc_index, uint32_t& usage_s) const { if (esc_index >= ESC_TELEM_MAX_ESCS - || AP_HAL::millis() - _telem_data[esc_index].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS + || _telem_data[esc_index].stale() || !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::USAGE)) { return false; } @@ -324,7 +324,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 && - (now - _telem_data[esc_id].last_update_ms <= ESC_TELEM_DATA_TIMEOUT_MS || + (!_telem_data[esc_id].stale(now) || rpm_data_within_timeout(_rpm_data[esc_id], now_us, ESC_RPM_DATA_TIMEOUT_US))) { all_stale = false; break; diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.cpp index b104c83562821..0048571452f23 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.cpp @@ -40,4 +40,15 @@ void AP_ESC_Telem_Backend::update_telem_data(const uint8_t esc_index, const Tele _frontend->update_telem_data(esc_index, new_data, data_present_mask); } -#endif \ No newline at end of file +/* + return true if the data is stale + */ +bool AP_ESC_Telem_Backend::TelemetryData::stale(uint32_t now_ms) const volatile +{ + if (now_ms == 0) { + now_ms = AP_HAL::millis(); + } + return last_update_ms == 0 || now_ms - last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS; +} + +#endif diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h b/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h index e40c09b64a3c9..b6d6725a55fa1 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h @@ -19,6 +19,9 @@ class AP_ESC_Telem_Backend { uint32_t last_update_ms; // last update time in milliseconds, determines whether active uint16_t types; // telemetry types present uint16_t count; // number of times updated + + // return true if the data is stale + bool stale(uint32_t now_ms=0) const volatile; }; struct RpmData {