diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index 2920ff668f..837c974c60 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -440,6 +440,22 @@ bool DShot::process_serial_telemetry() return false; } + // Periodically retry skipped motors when disarmed so channels that came online late + // (e.g. ESC power-cycled after boot) can recover. We don't retry while armed to avoid + // timeout blips on healthy channels during flight. + bool armed = _esc_status.esc_armed_flags != 0; + + if (!armed && _serial_telem_skip_mask != 0 + && hrt_elapsed_time(&_serial_telem_last_retry) > SERIAL_TELEM_RETRY_INTERVAL) { + _serial_telem_skip_mask = 0; + + for (int i = 0; i < DSHOT_MAX_MOTORS; i++) { + _serial_telem_consecutive_timeouts[i] = 0; + } + + _serial_telem_last_retry = hrt_absolute_time(); + } + bool all_telem_sampled = false; if (!_telemetry.commandResponseFinished()) { diff --git a/src/drivers/dshot/DShot.h b/src/drivers/dshot/DShot.h index e12cc88f82..a4a54a5691 100644 --- a/src/drivers/dshot/DShot.h +++ b/src/drivers/dshot/DShot.h @@ -170,8 +170,10 @@ private: // Serial telemetry adaptive skip: stop polling motors that never respond static constexpr int SERIAL_TELEM_SKIP_THRESHOLD = 10; // consecutive timeouts before skipping + static constexpr hrt_abstime SERIAL_TELEM_RETRY_INTERVAL = 3_s; // disarmed retry period uint16_t _serial_telem_skip_mask = 0; // motors to skip in round-robin uint8_t _serial_telem_consecutive_timeouts[DSHOT_MAX_MOTORS] = {}; + hrt_abstime _serial_telem_last_retry = 0; // Serial Telemetry DShotTelemetry _telemetry;