From 6bc3e17bd49f69941615cfd40f64abe2c2ee5b04 Mon Sep 17 00:00:00 2001
From: Jacob Dahl <37091262+dakejahl@users.noreply.github.com>
Date: Tue, 14 Apr 2026 12:18:12 -0800
Subject: [PATCH] fix(dshot): retry offline serial telemetry channels when
 disarmed (#27085)

Once a serial telem channel hit 10 consecutive timeouts it was added
to a skip mask and never polled again. Channels that came online late
(e.g. ESC power-cycled after the autopilot boots) stayed permanently
offline, and with both serial and bdshot enabled the motor was
reported offline overall because the combined health check requires
both sources.

Periodically clear the skip mask while disarmed so recovered channels
get another chance. If still broken they are re-skipped after the
normal timeout threshold. Disarmed-only to avoid timeout blips on
healthy channels during flight.
---
 src/drivers/dshot/DShot.cpp | 16 ++++++++++++++++
 src/drivers/dshot/DShot.h   |  2 ++
 2 files changed, 18 insertions(+)

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;