Making an ESP32-based quadcopter from scratch
Python arduinoautopilotcontrol-theorydroneesp32flight-controlflight-controllermavlinkquadcopterquadcopter-firmwareuav
Implement RC fail-safe
Oleg Kalachevec832d4e379a
diff --git a/flix/control.ino b/flix/control.inoindex 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.inonew file mode 100644index 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.inoindex 33f9761..8183e55 100644--- a/flix/flix.ino+++ b/flix/flix.ino@@ -28,6 +28,7 @@ float dt; // time delta from previous step, sfloat loopFreq; // loop frequency, Hzint16_t channels[RC_CHANNELS]; // raw rc channelsfloat controls[RC_CHANNELS]; // normalized controls in range [-1..1] ([0..1] for throttle)+float controlsTime; // time of the last controls updateVector gyro; // gyroscope dataVector acc; // accelerometer data, m/s/sVector rates; // filtered angular rates, rad/sdiff --git a/flix/mavlink.ino b/flix/mavlink.inoindex 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 modecontrols[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.inoindex 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 datanormalizeRC();+ controlsTime = t;}}diff --git a/gazebo/flix.h b/gazebo/flix.hindex 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 channelsfloat 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);// mocksdiff --git a/gazebo/simulator.cpp b/gazebo/simulator.cppindex 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;