From 1dc5f88820141fec6b875849b78111a5c8ef5c3c Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 23 Mar 2017 18:06:03 +0000 Subject: [PATCH] Added crash detection and recovery --- src/main/fc/cli.c | 22 ++++++++++++++----- src/main/fc/fc_core.c | 8 +++---- src/main/flight/pid.c | 50 +++++++++++++++++++++++++++++++++++++++++-- src/main/flight/pid.h | 15 ++++++++++++- 4 files changed, 83 insertions(+), 12 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 1050444ed7..e614a072d0 100755 --- a/src/main/fc/cli.c +++ b/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) }, - { "accel_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 1, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, rateAccelLimit) }, + { "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) }, + { "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_yaw", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 100, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, pidSumLimitYaw) }, + { "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) }, { "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]) }, diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 6c0cf3b10b..33ccf57fb5 100644 --- a/src/main/fc/fc_core.c +++ b/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; } diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 7452a5a91c..90b5ffeee7 100644 --- a/src/main/flight/pid.c +++ b/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); diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index ce8acddfb9..80799e6720 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -18,6 +18,7 @@ #pragma once #include +#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;