AP_ESC_Telem: prevent use of bad telem data at startup

ArduPilot · Nov 1, 2023 · 5fab0e9 · 5fab0e9
1 parent 898ac0a
commit 5fab0e9
Showing 1 changed file with 2 additions and 2 deletions.
diff --git 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].last_update_ms == 0 || now - _telem_data[i].last_update_ms >= ESC_TELEM_DATA_TIMEOUT_MS)
             && !rpm_data_within_timeout(_rpm_data[i], now_us, ESC_RPM_DATA_TIMEOUT_US)) {
             continue;
         }
@@ -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].last_update_ms != 0 && now - _telem_data[esc_id].last_update_ms <= ESC_TELEM_DATA_TIMEOUT_MS) ||
                  rpm_data_within_timeout(_rpm_data[esc_id], now_us, ESC_RPM_DATA_TIMEOUT_US))) {
                 all_stale = false;
                 break;