Skip to content

armgit / flix

Public Imported from GitHub
0

Making an ESP32-based quadcopter from scratch

Add tilt disarm failsafe

Oleg Kalachev406f7a4af547
diff --git a/flix/parameters.ino b/flix/parameters.ino
index 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.ino
index 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.h
index 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);