Rename failsafe.ino to safety.ino

To aggregate all the safety related functionality.

flix/safety.ino

@@ -0,0 +1,48 @@
+// Copyright (c) 2024 Oleg Kalachev <okalachev@gmail.com>
+// Repository: https://github.com/okalachev/flix
+
+// Fail-safe functions
+
+#define RC_LOSS_TIMEOUT 1
+#define DESCEND_TIME 10
+
+extern double controlTime;
+extern float controlRoll, controlPitch, controlThrottle, controlYaw;
+
+void failsafe() {
+	rcLossFailsafe();
+	autoFailsafe();
+}
+
+// RC loss failsafe
+void rcLossFailsafe() {
+	if (controlTime == 0) return; // no RC at all
+	if (!armed) return;
+	if (t - controlTime > RC_LOSS_TIMEOUT) {
+		descend();
+	}
+}
+
+// Smooth descend on RC lost
+void descend() {
+	mode = AUTO;
+	attitudeTarget = Quaternion();
+	thrustTarget -= dt / DESCEND_TIME;
+	if (thrustTarget < 0) {
+		thrustTarget = 0;
+		armed = false;
+	}
+}
+
+// Allow pilot to interrupt automatic flight
+void autoFailsafe() {
+	static float roll, pitch, yaw, throttle;
+	if (roll != controlRoll || pitch != controlPitch || yaw != controlYaw || abs(throttle - controlThrottle) > 0.05) {
+		// controls changed
+		if (mode == AUTO) mode = STAB; // regain control by the pilot
+	}
+	roll = controlRoll;
+	pitch = controlPitch;
+	yaw = controlYaw;
+	throttle = controlThrottle;
+}