AP_ESC_Telem: Fix some time wrap issues that can lead to using stale data if a ESC stops responding

libraries/AP_ESC_Telem/AP_ESC_Telem.cpp

@@ -86,7 +86,7 @@ uint8_t AP_ESC_Telem::get_motor_frequencies_hz(uint8_t nfreqs, float* freqs) con
         float rpm;
         if (get_rpm(i, rpm)) {
             freqs[valid_escs++] = rpm * (1.0f / 60.0f);
-        } else if (_rpm_data[i].last_update_us > 0) {
+        } else if (was_rpm_data_ever_reported(_rpm_data[i])) {
             // 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;
@@ -103,12 +103,12 @@ uint32_t AP_ESC_Telem::get_active_esc_mask() const {
     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++) {
-        if (now - _telem_data[i].last_update_ms >= ESC_TELEM_DATA_TIMEOUT_MS
+        if (_telem_data[i].last_update_ms == 0 && !was_rpm_data_ever_reported(_rpm_data[i])) {
-            && now_us - _rpm_data[i].last_update_us >= ESC_RPM_DATA_TIMEOUT_US) {
+            // have never seen telem from this ESC
             continue;
         }
-        if (_telem_data[i].last_update_ms == 0 && _rpm_data[i].last_update_us == 0) {
+        if (now - _telem_data[i].last_update_ms >= ESC_TELEM_DATA_TIMEOUT_MS
-            // have never seen telem from this ESC
+            && rpm_data_within_timeout(_rpm_data[i], now_us, ESC_RPM_DATA_TIMEOUT_US)) {
             continue;
         }
         ret |= (1U << i);
@@ -131,7 +131,7 @@ bool AP_ESC_Telem::are_motors_running(uint32_t servo_channel_mask, float min_rpm
         if (BIT_IS_SET(servo_channel_mask, i)) {
             const volatile AP_ESC_Telem_Backend::RpmData& rpmdata = _rpm_data[i];
             // we choose a relatively strict measure of health so that failsafe actions can rely on the results
-            if (now < rpmdata.last_update_us || now - rpmdata.last_update_us > ESC_RPM_CHECK_TIMEOUT_US) {
+            if (!rpm_data_within_timeout(rpmdata, now, ESC_RPM_CHECK_TIMEOUT_US)) {
                 return false;
             }
             if (rpmdata.rpm < min_rpm) {
@@ -151,7 +151,7 @@ bool AP_ESC_Telem::is_telemetry_active(uint32_t servo_channel_mask) const
     for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) {
         if (BIT_IS_SET(servo_channel_mask, i)) {
             // no data received
-            if (get_last_telem_data_ms(i) == 0 && _rpm_data[i].last_update_us == 0) {
+            if (get_last_telem_data_ms(i) == 0 && !was_rpm_data_ever_reported(_rpm_data[i])) {
                 return false;
             }
         }
@@ -173,8 +173,7 @@ bool AP_ESC_Telem::get_rpm(uint8_t esc_index, float& rpm) const
     }
 
     const uint32_t now = AP_HAL::micros();
-    if (rpmdata.last_update_us > 0 && (now >= rpmdata.last_update_us)
+    if (rpm_data_within_timeout(rpmdata, now, ESC_RPM_DATA_TIMEOUT_US)) {
-        && (now - rpmdata.last_update_us < ESC_RPM_DATA_TIMEOUT_US)) {
         const float slew = MIN(1.0f, (now - rpmdata.last_update_us) * rpmdata.update_rate_hz * (1.0f / 1e6f));
         rpm = (rpmdata.prev_rpm + (rpmdata.rpm - rpmdata.prev_rpm) * slew);
 
@@ -200,7 +199,7 @@ bool AP_ESC_Telem::get_raw_rpm(uint8_t esc_index, float& rpm) const
 
     const uint32_t now = AP_HAL::micros();
 
-    if (now < rpmdata.last_update_us || now - rpmdata.last_update_us > ESC_RPM_DATA_TIMEOUT_US) {
+    if (!rpm_data_within_timeout(rpmdata, now, ESC_RPM_DATA_TIMEOUT_US)) {
         return false;
     }
 
@@ -305,8 +304,8 @@ void AP_ESC_Telem::send_esc_telemetry_mavlink(uint8_t mav_chan)
         return;
     }
 
-    uint32_t now = AP_HAL::millis();
+    const uint32_t now = AP_HAL::millis();
-    uint32_t now_us = AP_HAL::micros();
+    const uint32_t now_us = AP_HAL::micros();
 
     // loop through groups of 4 ESCs
     const uint8_t esc_offset = constrain_int16(mavlink_offset, 0, ESC_TELEM_MAX_ESCS-1);
@@ -326,7 +325,7 @@ void AP_ESC_Telem::send_esc_telemetry_mavlink(uint8_t mav_chan)
             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 ||
-                 now_us - _rpm_data[esc_id].last_update_us <= ESC_RPM_DATA_TIMEOUT_US)) {
+                 rpm_data_within_timeout(_rpm_data[esc_id], now_us, ESC_RPM_DATA_TIMEOUT_US))) {
                 all_stale = false;
                 break;
             }
@@ -469,17 +468,16 @@ void AP_ESC_Telem::update_rpm(const uint8_t esc_index, const float new_rpm, cons
 
     _have_data = true;
 
-    const uint32_t now = AP_HAL::micros();
+    const uint32_t now = MAX(1U ,AP_HAL::micros()); // don't allow a value of 0 in, as we use this as a flag in places
     volatile AP_ESC_Telem_Backend::RpmData& rpmdata = _rpm_data[esc_index];
     const auto last_update_us = rpmdata.last_update_us;
 
     rpmdata.prev_rpm = rpmdata.rpm;
     rpmdata.rpm = new_rpm;
-    if (now > last_update_us) { // cope with wrapping
+    rpmdata.update_rate_hz = 1.0e6f / constrain_uint32((now - last_update_us), 100, 1000000U*10U); // limit the update rate 0.1Hz to 10KHz 
-        rpmdata.update_rate_hz = 1.0e6f / (now - last_update_us);
-    }
     rpmdata.last_update_us = now;
     rpmdata.error_rate = error_rate;
+    rpmdata.data_valid = true;
 
 #ifdef ESC_TELEM_DEBUG
     hal.console->printf("RPM: rate=%.1fhz, rpm=%f)\n", rpmdata.update_rate_hz, new_rpm);
@@ -490,10 +488,11 @@ void AP_ESC_Telem::update()
 {
     AP_Logger *logger = AP_Logger::get_singleton();
 
-    // Push received telemetry data into the logging system
+    const uint32_t now_us = AP_HAL::micros();
-    if (logger && logger->logging_enabled()) {
 
-        for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) {
+    for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) {
+        // Push received telemetry data into the logging system
+        if (logger && logger->logging_enabled()) {
             if (_telem_data[i].last_update_ms != _last_telem_log_ms[i]
                 || _rpm_data[i].last_update_us != _last_rpm_log_us[i]) {
 
@@ -529,9 +528,32 @@ void AP_ESC_Telem::update()
                 _last_rpm_log_us[i] = _rpm_data[i].last_update_us;
             }
         }
+
+        if ((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)
+{
+    // easy case, has the time window been crossed so it's invalid
+    if ((now_us - instance.last_update_us) > timeout_us) {
+        return false;
+    }
+    // we never got a valid data, to it's invalid
+    if (instance.last_update_us == 0) {
+        return false;
+    }
+    // check if things generally expired on us, this is done to handle time wrapping
+    return instance.data_valid;
+}
+
+bool AP_ESC_Telem::was_rpm_data_ever_reported (const volatile AP_ESC_Telem_Backend::RpmData &instance)
+{
+    return instance.last_update_us > 0;
+}
+
 #if AP_SCRIPTING_ENABLED
 /*
   set RPM scale factor from script

libraries/AP_ESC_Telem/AP_ESC_Telem.h

@@ -109,6 +109,10 @@ private:
     // callback to update the data in the frontend, should be called by the driver when new data is available
     void update_telem_data(const uint8_t esc_index, const AP_ESC_Telem_Backend::TelemetryData& new_data, const uint16_t data_mask);
 
+    // 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 was_rpm_data_ever_reported (const volatile AP_ESC_Telem_Backend::RpmData &instance);
+
     // rpm data
     volatile AP_ESC_Telem_Backend::RpmData _rpm_data[ESC_TELEM_MAX_ESCS];
     // telemetry data

libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h

@@ -25,8 +25,9 @@ public:
         float    rpm;               // rpm
         float    prev_rpm;          // previous rpm
         float    error_rate;        // error rate in percent
-        uint32_t last_update_us;    // last update time, determines whether active
+        uint32_t last_update_us;    // last update time, greater then 0 means we've gotten data at some point
         float    update_rate_hz;
+        bool     data_valid;        // if this isn't set to true, then the ESC data should be ignored
     };
 
     enum TelemetryType {