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 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);