Add tilt disarm failsafe

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;
 
@@ -128,6 +128,7 @@ Parameter parameters[] = {
 	// safety
 	{"SF_RC_LOSS_TIME", &rcLossTimeout},
 	{"SF_DESCEND_TIME", &descendTime},
+	{"SF_DISARM_TILT", &disarmTilt},
 };
 
 void setupParameters() {

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;
+	}
+}

gazebo/flix.h

@@ -63,6 +63,7 @@ void failsafe();
 void rcLossFailsafe();
 void descend();
 void autoFailsafe();
+void tiltFailsafe();
 int parametersCount();
 const char *getParameterName(int index);
 float getParameter(int index);