From 15fbe34d196279d1ed716d83b26db461c7bafee3 Mon Sep 17 00:00:00 2001
From: Oleg Kalachev <okalachev@gmail.com>
Date: Fri, 24 Jan 2025 16:23:59 +0300
Subject: [PATCH] Add failsafe to prevent arming without prior zero throttle

---
 flix/failsafe.ino | 19 +++++++++++++++++--
 gazebo/flix.h     |  2 ++
 2 files changed, 19 insertions(+), 2 deletions(-)

diff --git a/flix/failsafe.ino b/flix/failsafe.ino
index 13a1163..b217994 100644
--- a/flix/failsafe.ino
+++ b/flix/failsafe.ino
@@ -1,7 +1,7 @@
 // Copyright (c) 2024 Oleg Kalachev <okalachev@gmail.com>
 // Repository: https://github.com/okalachev/flix
 
-// Fail-safe for RC loss
+// Fail-safe functions
 
 #define RC_LOSS_TIMEOUT 0.2
 #define DESCEND_TIME 3.0 // time to descend from full throttle to zero
@@ -10,13 +10,28 @@ extern double controlsTime;
 extern int rollChannel, pitchChannel, throttleChannel, yawChannel;
 
 void failsafe() {
+	armingFailsafe();
+	rcLossFailsafe();
+}
+
+// Prevent arming without zero throttle input
+void armingFailsafe() {
+	static double zeroThrottleTime;
+	static double armingTime;
+	if (!armed) armingTime = t; // stores the last time when the drone was disarmed, therefore contains arming time
+	if (controlsTime > 0 && controls[throttleChannel] < 0.05) zeroThrottleTime = controlsTime;
+	if (armingTime - zeroThrottleTime > 0.1) armed = false; // prevent arming if there was no zero throttle for 0.1 sec
+}
+
+// RC loss failsafe
+void rcLossFailsafe() {
 	if (t - controlsTime > RC_LOSS_TIMEOUT) {
 		descend();
 	}
 }
 
+// Smooth descend on RC lost
 void descend() {
-	// Smooth descend on RC lost
 	mode = STAB;
 	controls[rollChannel] = 0;
 	controls[pitchChannel] = 0;
diff --git a/gazebo/flix.h b/gazebo/flix.h
index b460170..6ae749d 100644
--- a/gazebo/flix.h
+++ b/gazebo/flix.h
@@ -44,6 +44,8 @@ void sendMessage(const void *msg);
 void receiveMavlink();
 void handleMavlink(const void *_msg);
 void failsafe();
+void armingFailsafe();
+void rcLossFailsafe();
 void descend();
 inline Quaternion FLU2FRD(const Quaternion &q);