From cd757c8a89f1116a3b6ea9d97ac9f596626a70e0 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Tue, 22 Aug 2017 08:41:32 +0100 Subject: [PATCH] Use crash detection to limit yaw --- src/main/fc/settings.c | 2 +- src/main/flight/pid.c | 41 ++++++++++++++++++++++------------------- src/main/flight/pid.h | 2 +- 3 files changed, 24 insertions(+), 21 deletions(-) diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index c5e0bff5d1..281a9372b2 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -563,13 +563,13 @@ const clivalue_t valueTable[] = { { "crash_delay", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_delay) }, { "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_limit_yaw", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_limit_yaw) }, { "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) }, - { "gyro_limit_yaw", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 100, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, gyroRateLimitYaw) }, { "p_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_PITCH].P) }, { "i_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_PITCH].I) }, diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 76c4017c81..e78f0a1580 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -106,7 +106,7 @@ void resetPidProfile(pidProfile_t *pidProfile) .itermThrottleThreshold = 350, .itermAcceleratorGain = 1000, .crash_time = 500, // ms - .crash_delay = 0, // ms + .crash_delay = 0, // ms .crash_recovery_angle = 10, // degrees .crash_recovery_rate = 100, // degrees/second .crash_dthreshold = 50, // degrees/second/second @@ -115,7 +115,7 @@ void resetPidProfile(pidProfile_t *pidProfile) .crash_recovery = PID_CRASH_RECOVERY_OFF, // off by default .horizon_tilt_effect = 75, .horizon_tilt_expert_mode = false, - .gyroRateLimitYaw = 1000 + .crash_limit_yaw = 200 ); } @@ -239,7 +239,6 @@ void pidInitFilters(const pidProfile_t *pidProfile) static float Kp[3], Ki[3], Kd[3]; static float maxVelocity[3]; -static float gyroRateLimitYaw; static float relaxFactor; static float dtermSetpointWeight; static float levelGain, horizonGain, horizonTransition, horizonCutoffDegrees, horizonFactorRatio; @@ -252,6 +251,7 @@ static float crashRecoveryRate; static float crashDtermThreshold; static float crashGyroThreshold; static float crashSetpointThreshold; +static float crashLimitYaw; void pidInitConfig(const pidProfile_t *pidProfile) { @@ -260,7 +260,6 @@ void pidInitConfig(const pidProfile_t *pidProfile) Ki[axis] = ITERM_SCALE * pidProfile->pid[axis].I; Kd[axis] = DTERM_SCALE * pidProfile->pid[axis].D; } - gyroRateLimitYaw = pidProfile->gyroRateLimitYaw; dtermSetpointWeight = pidProfile->dtermSetpointWeight / 127.0f; relaxFactor = 1.0f / (pidProfile->setpointRelaxRatio / 100.0f); levelGain = pidProfile->pid[PID_LEVEL].P / 10.0f; @@ -280,6 +279,7 @@ void pidInitConfig(const pidProfile_t *pidProfile) crashGyroThreshold = pidProfile->crash_gthreshold; crashDtermThreshold = pidProfile->crash_dthreshold; crashSetpointThreshold = pidProfile->crash_setpoint_threshold; + crashLimitYaw = pidProfile->crash_limit_yaw; } void pidInit(const pidProfile_t *pidProfile) @@ -394,46 +394,49 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an // ----------PID controller---------- for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { float currentPidSetpoint = getSetpointRate(axis); - if (maxVelocity[axis]) { currentPidSetpoint = accelerationLimit(axis, currentPidSetpoint); } - // Yaw control is GYRO based, direct sticks control is applied to rate PID if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) { currentPidSetpoint = pidLevel(axis, pidProfile, angleTrim, currentPidSetpoint); } - if (inCrashRecoveryMode && axis != FD_YAW && cmpTimeUs(currentTimeUs, crashDetectedAtUs) > crashTimeDelayUs) { - // self-level - errorAngle is deviation from horizontal + // -----calculate error rate + const float gyroRate = gyro.gyroADCf[axis]; // Process variable from gyro output in deg/sec + float errorRate = currentPidSetpoint - gyroRate; // r - y + + if (inCrashRecoveryMode && cmpTimeUs(currentTimeUs, crashDetectedAtUs) > crashTimeDelayUs) { if (pidProfile->crash_recovery == PID_CRASH_RECOVERY_BEEP) { - BEEP_ON; + BEEP_ON; + } + if (axis == FD_YAW) { + // on yaw axis, prevent "yaw spin to the moon" after crash by constraining errorRate + errorRate = constrainf(errorRate, -crashLimitYaw, crashLimitYaw); + } else { + // on roll and pitch axes calculate currentPidSetpoint and errorRate to level the aircraft to recover from crash + // errorAngle is deviation from horizontal + const float errorAngle = -(attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f; + currentPidSetpoint = errorAngle * levelGain; + errorRate = currentPidSetpoint - gyroRate; } - 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) + && ABS(gyro.gyroADCf[FD_PITCH]) < crashRecoveryRate + && ABS(gyro.gyroADCf[FD_YAW]) < crashRecoveryRate) ) { inCrashRecoveryMode = false; BEEP_OFF; } } - float gyroRate = gyro.gyroADCf[axis]; // Process variable from gyro output in deg/sec - if (axis == FD_YAW) { - gyroRate = constrainf(gyroRate, -gyroRateLimitYaw, gyroRateLimitYaw); - } // --------low-level gyro-based PID based on 2DOF PID controller. ---------- // 2-DOF PID controller with optional filter on derivative term. // b = 1 and only c (dtermSetpointWeight) can be tuned (amount derivative on measurement or error). - // -----calculate error rate - const float errorRate = currentPidSetpoint - gyroRate; // r - y - // -----calculate P component and add Dynamic Part based on stick input axisPID_P[axis] = Kp[axis] * errorRate * tpaFactor; if (axis == FD_YAW) { diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index b61c5b3d5d..850d9b2e8f 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -105,7 +105,7 @@ typedef struct pidProfile_s { 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 - uint16_t gyroRateLimitYaw; // limits yaw gyroRate, so crashes don't cause huge throttle increase + uint16_t crash_limit_yaw; // limits yaw errorRate, so crashes don't cause huge throttle increase } pidProfile_t; PG_DECLARE_ARRAY(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles);