From 1276d220a6fea37e124ae65508d78e2176c54f83 Mon Sep 17 00:00:00 2001
From: Oleg Kalachev <okalachev@gmail.com>
Date: Sat, 18 Jul 2026 21:35:13 +0300
Subject: [PATCH] Add tilt disarm failsafe

---
 flix/parameters.ino |  3 ++-
 flix/safety.ino     | 14 ++++++++++++++
 gazebo/flix.h       |  1 +
 3 files changed, 17 insertions(+), 1 deletion(-)

diff --git a/flix/parameters.ino b/flix/parameters.ino
index 76e8ebe..1b78803 100644
--- a/flix/parameters.ino
+++ b/flix/parameters.ino
@@ -10,7 +10,7 @@ extern int channelZero[16], channelMax[16];
 extern int rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel;
 extern int rcRxPin, voltagePin;
 extern int wifiMode, wifiLongRange, udpLocalPort, udpRemotePort, espnowChannel;
-extern float rcLossTimeout, descendTime;
+extern float rcLossTimeout, descendTime, disarmTilt;
 extern float voltageScale;
 extern LowPassFilter<float> voltageFilter;
 
@@ -146,6 +146,7 @@ Parameter parameters[] = {
 	// safety
 	{"SF_RC_LOSS_TIME", &rcLossTimeout},
 	{"SF_DESCEND_TIME", &descendTime},
+	{"SF_DISARM_TILT", &disarmTilt}
 };
 
 void setupParameters() {
diff --git a/flix/safety.ino b/flix/safety.ino
index 05fd758..855e3d1 100644
--- a/flix/safety.ino
+++ b/flix/safety.ino
@@ -8,10 +8,12 @@ extern float controlRoll, controlPitch, controlThrottle, controlYaw;
 
 float rcLossTimeout = 1;
 float descendTime = 10;
+float disarmTilt = radians(120);
 
 void failsafe() {
 	rcLossFailsafe();
 	autoFailsafe();
+	tiltFailsafe();
 }
 
 // RC loss failsafe
@@ -45,3 +47,15 @@ void autoFailsafe() {
 	yaw = controlYaw;
 	throttle = controlThrottle;
 }
+
+// Disarm if tilted too much
+void tiltFailsafe() {
+	if (!armed) return;
+	if (mode != STAB) return;
+
+	Vector up = Quaternion::rotateVector(Vector(0, 0, 1), attitude);
+	float tilt = acos(up.z);
+	if (disarmTilt && tilt > disarmTilt) {
+		armed = false;
+	}
+}
diff --git a/gazebo/flix.h b/gazebo/flix.h
index 217c1a5..67dd5d9 100644
--- a/gazebo/flix.h
+++ b/gazebo/flix.h
@@ -72,6 +72,7 @@ void failsafe();
 void rcLossFailsafe();
 void descend();
 void autoFailsafe();
+void tiltFailsafe();
 int parametersCount();
 const char *getParameterName(int index);
 float getParameter(int index);