From 960442e91757092b81d186a5f513d83173125634 Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Sun, 2 Sep 2018 09:58:16 -0400 Subject: [PATCH] Fix iterm relax interfering with crash recovery Iterm Relax was inserted before crash recovery and this was causing the recovery to be ineffective or fail completely. The problem was that the subsequent iterm calculation was not using the calculated recovery error and instead was using the setpoint error. As such I ended up accumulating and opposing P based on recovery error. This prevented the efforts of crash recovery to level the quad and since it couldn't reach the level state crash recovery wouldn't shut off. This resulted in the pilot only having control over yaw and throttle while crash recovery was still ineffectually trying to control roll/pitch. The fix is to move crash recovery handling ahead of the iterm relax calculations and make sure that iterm relax uses the error calculated from crash recovery if active. This also allows crash recovery to reset the iterm accumulation as originally designed. --- src/main/flight/pid.c | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 7f46d52a3..6d8d32ddd 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -898,6 +898,10 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT // -----calculate error rate const float gyroRate = gyro.gyroADCf[axis]; // Process variable from gyro output in deg/sec + float errorRate = currentPidSetpoint - gyroRate; // r - y + handleCrashRecovery( + pidProfile->crash_recovery, angleTrim, axis, currentTimeUs, gyroRate, + ¤tPidSetpoint, &errorRate); #ifdef USE_ABSOLUTE_CONTROL float acCorrection = 0; @@ -905,7 +909,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT #endif const float ITerm = pidData[axis].I; - float itermErrorRate = currentPidSetpoint - gyroRate; + float itermErrorRate = errorRate; #if defined(USE_ITERM_RELAX) if (itermRelax && (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY || itermRelax == ITERM_RELAX_RPY_INC)) { @@ -968,11 +972,6 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT } #endif - float errorRate = currentPidSetpoint - gyroRate; // r - y - handleCrashRecovery( - pidProfile->crash_recovery, angleTrim, axis, currentTimeUs, gyroRate, - ¤tPidSetpoint, &errorRate); - // --------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 (feedforward weight) can be tuned (amount derivative on measurement or error).