From 682db4b8ac8cbf326cc4e957f676bcfe2eac9312 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Igor=20Mi=C5=A1i=C4=87?= <igy1000mb@gmail.com>
Date: Wed, 9 Jun 2021 09:47:04 +0200
Subject: [PATCH] dshot: timeout if commands are not processed

---
 src/drivers/dshot/DShot.cpp | 11 ++++++++++-
 1 file changed, 10 insertions(+), 1 deletion(-)

diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp
index 49f8a87071..eeb0365950 100644
--- a/src/drivers/dshot/DShot.cpp
+++ b/src/drivers/dshot/DShot.cpp
@@ -242,9 +242,18 @@ int DShot::send_command_thread_safe(const dshot_command_t command, const int num
 	cmd.num_repetitions = num_repetitions;
 	_new_command.store(&cmd);
 
+	hrt_abstime timestamp_for_timeout = hrt_absolute_time();
+
 	// wait until main thread processed it
 	while (_new_command.load()) {
-		px4_usleep(1000);
+
+		if (hrt_elapsed_time(&timestamp_for_timeout) < 2_s) {
+			px4_usleep(1000);
+
+		} else {
+			_new_command.store(nullptr);
+			PX4_WARN("DShot command timeout!");
+		}
 	}
 
 	return 0;