Added crash detection and recovery

src/main/fc/cli.c

@@ -197,6 +197,10 @@ static const char * const lookupTableOffOn[] = {
     "OFF", "ON"
 };
 
+static const char * const lookupTableCrashRecovery[] = {
+    "OFF", "ON" ,"BEEP"
+};
+
 static const char * const lookupTableUnit[] = {
     "IMPERIAL", "METRIC"
 };
@@ -383,6 +387,7 @@ typedef enum {
     TABLE_RC_INTERPOLATION_CHANNELS,
     TABLE_LOWPASS_TYPE,
     TABLE_FAILSAFE,
+    TABLE_CRASH_RECOVERY,
 #ifdef OSD
     TABLE_OSD,
 #endif
@@ -426,6 +431,7 @@ static const lookupTableEntry_t lookupTables[] = {
     { lookupTableRcInterpolationChannels, sizeof(lookupTableRcInterpolationChannels) / sizeof(char *) },
     { lookupTableLowpassType, sizeof(lookupTableLowpassType) / sizeof(char *) },
     { lookupTableFailsafe, sizeof(lookupTableFailsafe) / sizeof(char *) },
+    { lookupTableCrashRecovery, sizeof(lookupTableCrashRecovery) / sizeof(char *) },
 #ifdef OSD
     { lookupTableOsdType, sizeof(lookupTableOsdType) / sizeof(char *) },
 #endif
@@ -726,16 +732,22 @@ static const clivalue_t valueTable[] = {
     { "vbat_pid_gain",              VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, vbatPidCompensation) },
     { "pid_at_min_throttle",        VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, pidAtMinThrottle) },
     { "anti_gravity_thresh",        VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 20, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermThrottleThreshold) },
-    { "anti_gravity_gain",          VAR_UINT16  | PROFILE_VALUE, .config.minmax = { 1, 30000 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermAcceleratorGain) },
+    { "anti_gravity_gain",          VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 1, 30000 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermAcceleratorGain) },
     { "setpoint_relax_ratio",       VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, setpointRelaxRatio) },
     { "d_setpoint_weight",          VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0, 254 }, PG_PID_PROFILE, offsetof(pidProfile_t, dtermSetpointWeight) },
-    { "yaw_accel_limit",            VAR_UINT16  | PROFILE_VALUE, .config.minmax = { 1, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, yawRateAccelLimit) },
+    { "yaw_accel_limit",            VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 1, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, yawRateAccelLimit) },
-    { "accel_limit",                VAR_UINT16  | PROFILE_VALUE, .config.minmax = { 1, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, rateAccelLimit) },
+    { "accel_limit",                VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 1, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, rateAccelLimit) },
+    { "crash_dthreshold",           VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 2000 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_dthreshold) },
+    { "crash_gthreshold",           VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 2000 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_gthreshold) },
+    { "crash_time",                 VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 5000 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_time) },
+    { "crash_recovery_angle",       VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0, 30 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_recovery_angle) },
+    { "crash_recovery_rate",        VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_recovery_rate) },
+    { "crash_recovery",             VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_CRASH_RECOVERY }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_recovery) },
 
     { "iterm_windup",               VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 30, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermWindupPointPercent) },
     { "yaw_lowpass",                VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, yaw_lpf_hz) },
-    { "pidsum_limit",               VAR_UINT16  | PROFILE_VALUE, .config.minmax = { 100, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, pidSumLimit) },
+    { "pidsum_limit",               VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 100, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, pidSumLimit) },
-    { "pidsum_limit_yaw",           VAR_UINT16  | PROFILE_VALUE, .config.minmax = { 100, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, pidSumLimitYaw) },
+    { "pidsum_limit_yaw",           VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 100, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, pidSumLimitYaw) },
 
     { "p_pitch",                    VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, P8[PITCH]) },
     { "i_pitch",                    VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, I8[PITCH]) },

src/main/fc/fc_core.c

@@ -175,7 +175,7 @@ void mwDisarm(void)
             finishBlackbox();
         }
 #endif
-
+        BEEP_OFF;
         beeper(BEEPER_DISARMING);      // emit disarm tone
     }
 }
@@ -475,12 +475,12 @@ void processRx(timeUs_t currentTimeUs)
 #endif
 }
 
-static void subTaskPidController(void)
+static void subTaskPidController(timeUs_t currentTimeUs)
 {
     uint32_t startTime = 0;
     if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
     // PID - note this is function pointer set by setPIDController()
-    pidController(currentPidProfile, &accelerometerConfig()->accelerometerTrims);
+    pidController(currentPidProfile, &accelerometerConfig()->accelerometerTrims, currentTimeUs);
     DEBUG_SET(DEBUG_PIDLOOP, 1, micros() - startTime);
 }
 
@@ -630,7 +630,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
         pidUpdateCountdown--;
     } else {
         pidUpdateCountdown = setPidUpdateCountDown();
-        subTaskPidController();
+        subTaskPidController(currentTimeUs);
         subTaskMotorUpdate();
         runTaskMainSubprocesses = true;
     }

src/main/flight/pid.c

@@ -34,6 +34,8 @@
 #include "config/parameter_group_ids.h"
 #include "config/config_profile.h"
 
+#include "drivers/sound_beeper.h"
+
 #include "fc/fc_core.h"
 #include "fc/fc_rc.h"
 
@@ -119,7 +121,13 @@ void resetPidProfile(pidProfile_t *pidProfile)
         .yawRateAccelLimit = 100,
         .rateAccelLimit = 0,
         .itermThrottleThreshold = 350,
-        .itermAcceleratorGain = 1000
+        .itermAcceleratorGain = 1000,
+        .crash_time = 500,          // 500ms
+        .crash_recovery_angle = 10, //  10 degrees
+        .crash_recovery_rate = 100, // 100 degrees/second
+        .crash_dthreshold = 50,     //  50 degrees/second/second
+        .crash_gthreshold = 200,    // 200 degrees/second
+        .crash_recovery = PID_CRASH_RECOVERY_OFF // off by default
     );
 }
 
@@ -230,6 +238,11 @@ static float Kp[3], Ki[3], Kd[3], maxVelocity[3];
 static float relaxFactor;
 static float dtermSetpointWeight;
 static float levelGain, horizonGain, horizonTransition, ITermWindupPoint, ITermWindupPointInv;
+static timeDelta_t crashTimeLimitUs;
+static int32_t crashRecoveryAngleDeciDegrees;
+static float crashRecoveryRate;
+static float crashDtermThreshold;
+static float crashGyroThreshold;
 
 void pidInitConfig(const pidProfile_t *pidProfile) {
     for(int axis = FD_ROLL; axis <= FD_YAW; axis++) {
@@ -246,6 +259,11 @@ void pidInitConfig(const pidProfile_t *pidProfile) {
     maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 100 * dT;
     ITermWindupPoint = (float)pidProfile->itermWindupPointPercent / 100.0f;
     ITermWindupPointInv = 1.0f / (1.0f - ITermWindupPoint);
+    crashTimeLimitUs = pidProfile->crash_time * 1000;
+    crashRecoveryAngleDeciDegrees = pidProfile->crash_recovery_angle * 10;
+    crashRecoveryRate = pidProfile->crash_recovery_rate;
+    crashGyroThreshold = pidProfile->crash_gthreshold;
+    crashDtermThreshold = pidProfile->crash_dthreshold;
 }
 
 void pidInit(const pidProfile_t *pidProfile)
@@ -298,11 +316,13 @@ static float accelerationLimit(int axis, float currentPidSetpoint) {
 
 // Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage.
 // Based on 2DOF reference design (matlab)
-void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim)
+void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, timeUs_t currentTimeUs)
 {
     static float previousRateError[2];
     const float tpaFactor = getThrottlePIDAttenuation();
     const float motorMixRange = getMotorMixRange();
+    static bool inCrashRecoveryMode = false;
+    static timeUs_t crashDetectedAtUs;
 
     // Dynamic ki component to gradually scale back integration when above windup point
     const float dynKi = MIN((1.0f - motorMixRange) * ITermWindupPointInv, 1.0f);
@@ -319,6 +339,21 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
             currentPidSetpoint = pidLevel(axis, pidProfile, angleTrim, currentPidSetpoint);
         }
 
+        if (inCrashRecoveryMode && axis != FD_YAW) {
+            // self-level - errorAngle is deviation from horizontal
+            const float errorAngle =  -(attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f;
+            currentPidSetpoint = errorAngle * levelGain;
+            if (cmpTimeUs(currentTimeUs, crashDetectedAtUs) > crashTimeLimitUs
+                || (motorMixRange < 1.0f
+                       && ABS(attitude.raw[FD_ROLL] - angleTrim->raw[FD_ROLL]) < crashRecoveryAngleDeciDegrees
+                       && ABS(attitude.raw[FD_PITCH] - angleTrim->raw[FD_PITCH]) < crashRecoveryAngleDeciDegrees
+                       && ABS(gyro.gyroADCf[FD_ROLL]) < crashRecoveryRate
+                       && ABS(gyro.gyroADCf[FD_PITCH]) < crashRecoveryRate)
+                       ) {
+                inCrashRecoveryMode = false;
+                BEEP_OFF;
+            }
+        }
         const float gyroRate = gyro.gyroADCf[axis]; // Process variable from gyro output in deg/sec
 
         // --------low-level gyro-based PID based on 2DOF PID controller. ----------
@@ -351,6 +386,17 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
             float delta = (rD - previousRateError[axis]) / dT;
             previousRateError[axis] = rD;
 
+            // if crash recovery is on check for a crash
+            if (pidProfile->crash_recovery) {
+                if (motorMixRange >= 1.0f && ABS(delta) > crashDtermThreshold && ABS(errorRate) > crashGyroThreshold) {
+                    inCrashRecoveryMode = true;
+                    crashDetectedAtUs = currentTimeUs;
+                    if (pidProfile->crash_recovery == PID_CRASH_RECOVERY_BEEP) {
+                        BEEP_ON;
+                    }
+                }
+            }
+
             // apply filters
             delta = dtermNotchFilterApplyFn(dtermFilterNotch[axis], delta);
             delta = dtermLpfApplyFn(dtermFilterLpf[axis], delta);

src/main/flight/pid.h

@@ -18,6 +18,7 @@
 #pragma once
 
 #include <stdbool.h>
+#include "common/time.h"
 #include "config/parameter_group.h"
 
 #define MAX_PID_PROCESS_DENOM       16
@@ -57,6 +58,12 @@ typedef enum {
     PID_STABILISATION_ON
 } pidStabilisationState_e;
 
+typedef enum {
+    PID_CRASH_RECOVERY_OFF = 0,
+    PID_CRASH_RECOVERY_ON,
+    PID_CRASH_RECOVERY_BEEP
+} pidCrashRecovery_e;
+
 typedef struct pidProfile_s {
     uint8_t P8[PID_ITEM_COUNT];
     uint8_t I8[PID_ITEM_COUNT];
@@ -83,6 +90,12 @@ typedef struct pidProfile_s {
     uint8_t dtermSetpointWeight;            // Setpoint weight for Dterm (0= measurement, 1= full error, 1 > agressive derivative)
     uint16_t yawRateAccelLimit;             // yaw accel limiter for deg/sec/ms
     uint16_t rateAccelLimit;                // accel limiter roll/pitch deg/sec/ms
+    uint16_t crash_dthreshold;              // dterm crash value
+    uint16_t crash_gthreshold;              // gyro crash value
+    uint16_t crash_time;                    // ms
+    uint8_t crash_recovery_angle;           // degrees
+    uint8_t crash_recovery_rate;            // degree/second
+    pidCrashRecovery_e crash_recovery;      // off, on, on and beeps when it is in crash recovery mode
 } pidProfile_t;
 
 #if FLASH_SIZE <= 128
@@ -99,7 +112,7 @@ typedef struct pidConfig_s {
 PG_DECLARE(pidConfig_t, pidConfig);
 
 union rollAndPitchTrims_u;
-void pidController(const pidProfile_t *pidProfile, const union rollAndPitchTrims_u *angleTrim);
+void pidController(const pidProfile_t *pidProfile, const union rollAndPitchTrims_u *angleTrim, timeUs_t currentTimeUs);
 
 extern float axisPID_P[3], axisPID_I[3], axisPID_D[3];
 bool airmodeWasActivated;