Merge pull request #6682 from etracer65/crash_recovery_term_relax_fix
Fix iterm relax interfering with crash recovery
This commit is contained in:
commit
5c8d08889d
|
@ -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).
|
||||
|
|
Loading…
Reference in New Issue