Skip to content

armgit / flix

Public Imported from GitHub
0

Making an ESP32-based quadcopter from scratch

Implement RC fail-safe

Oleg Kalachevec832d4e379a
diff --git a/flix/control.ino b/flix/control.ino
index f6c4a03..77c4a87 100644
--- a/flix/control.ino
+++ b/flix/control.ino
@@ -52,6 +52,7 @@ float thrustTarget;
void control() {
interpretRC();
+ failsafe();
if (mode == STAB) {
controlAttitude();
controlRate();
diff --git a/flix/failsafe.ino b/flix/failsafe.ino
new file mode 100644
index 0000000..b076361
--- /dev/null
+++ b/flix/failsafe.ino
@@ -0,0 +1,22 @@
+// Copyright (c) 2023 Oleg Kalachev <[email protected]>
+// 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;
+}
diff --git a/flix/flix.ino b/flix/flix.ino
index 33f9761..8183e55 100644
--- a/flix/flix.ino
+++ b/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
diff --git a/flix/mavlink.ino b/flix/mavlink.ino
index de7602a..3bf73a1 100644
--- a/flix/mavlink.ino
+++ b/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;
}
diff --git a/flix/rc.ino b/flix/rc.ino
index c11618d..adc723a 100644
--- a/flix/rc.ino
+++ b/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;
}
}
diff --git a/gazebo/flix.h b/gazebo/flix.h
index c4cbe63..bc15f94 100644
--- a/gazebo/flix.h
+++ b/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
diff --git a/gazebo/simulator.cpp b/gazebo/simulator.cpp
index e67ea10..6684c6b 100644
--- a/gazebo/simulator.cpp
+++ b/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;