diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp index 9945d16ce45784..09722b74802dae 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].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;