Making an ESP32-based quadcopter from scratch
Python arduinoautopilotcontrol-theorydroneesp32flight-controlflight-controllermavlinkquadcopterquadcopter-firmwareuav
Add tilt disarm failsafe
Oleg Kalachev406f7a4af547
diff --git a/flix/parameters.ino b/flix/parameters.inoindex 0d3e19a..ebabaaf 100644--- a/flix/parameters.ino+++ b/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() {diff --git a/flix/safety.ino b/flix/safety.inoindex 7cbd986..4e7e9dc 100644--- a/flix/safety.ino+++ b/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;+ }+}diff --git a/gazebo/flix.h b/gazebo/flix.hindex 0cc02c2..1d88343 100644--- a/gazebo/flix.h+++ b/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);