Implement RC fail-safe

flix/control.ino

@@ -52,6 +52,7 @@ float thrustTarget;
 
 void control() {
 	interpretRC();
+	failsafe();
 	if (mode == STAB) {
 		controlAttitude();
 		controlRate();

flix/failsafe.ino

@@ -0,0 +1,22 @@
+// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
+// Repository: https://github.com/okalachev/flix
+
+// Fail-safe for RC loss
+
+#define RC_LOSS_TIMEOUT 0.2
+#define DESCEND_TIME 3.0 // time to descend from full throttle to zero
+
+void failsafe() {
+	if (t - controlsTime > RC_LOSS_TIMEOUT) {
+		descend();
+	}
+}
+
+void descend() {
+	// Smooth descend on RC lost
+	mode = STAB;
+	controls[RC_CHANNEL_ROLL] = 0;
+	controls[RC_CHANNEL_PITCH] = 0;
+	controls[RC_CHANNEL_YAW] = 0;
+	controls[RC_CHANNEL_THROTTLE] -= dt / DESCEND_TIME;
+}

flix/flix.ino

@@ -28,6 +28,7 @@ float dt; // time delta from previous step, s
 float loopFreq; // loop frequency, Hz
 int16_t channels[RC_CHANNELS]; // raw rc channels
 float controls[RC_CHANNELS]; // normalized controls in range [-1..1] ([0..1] for throttle)
+float controlsTime; // time of the last controls update
 Vector gyro; // gyroscope data
 Vector acc; // accelerometer data, m/s/s
 Vector rates; // filtered angular rates, rad/s

flix/mavlink.ino

@@ -93,6 +93,7 @@ void handleMavlink(const void *_msg) {
 		controls[RC_CHANNEL_YAW] = manualControl.r / 1000.0f * MAVLINK_CONTROL_SCALE;
 		controls[RC_CHANNEL_MODE] = 1; // STAB mode
 		controls[RC_CHANNEL_ARMED] = 1; // armed
+		controlsTime = t;
 
 		if (abs(controls[RC_CHANNEL_YAW]) < MAVLINK_CONTROL_YAW_DEAD_ZONE) controls[RC_CHANNEL_YAW] = 0;
 	}

flix/rc.ino

@@ -21,6 +21,7 @@ void readRC() {
 		SBUSData data = RC.data();
 		memcpy(channels, data.ch, sizeof(channels)); // copy channels data
 		normalizeRC();
+		controlsTime = t;
 	}
 }
 

gazebo/flix.h

@@ -25,6 +25,7 @@ float loopFreq;
 float motors[4];
 int16_t channels[16]; // raw rc channels
 float controls[RC_CHANNELS];
+float controlsTime;
 Vector acc;
 Vector gyro;
 Vector rates;
@@ -48,6 +49,8 @@ void sendMavlink();
 void sendMessage(const void *msg);
 void receiveMavlink();
 void handleMavlink(const void *_msg);
+void failsafe();
+void descend();
 inline Quaternion FLU2FRD(const Quaternion &q);
 
 // mocks

gazebo/simulator.cpp

@@ -25,6 +25,7 @@
 #include "log.ino"
 #include "cli.ino"
 #include "mavlink.ino"
+#include "failsafe.ino"
 #include "lpf.h"
 
 using ignition::math::Vector3d;