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
|
// -----calculate error rate
|
||||||
const float gyroRate = gyro.gyroADCf[axis]; // Process variable from gyro output in deg/sec
|
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
|
#ifdef USE_ABSOLUTE_CONTROL
|
||||||
float acCorrection = 0;
|
float acCorrection = 0;
|
||||||
|
@ -905,7 +909,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
const float ITerm = pidData[axis].I;
|
const float ITerm = pidData[axis].I;
|
||||||
float itermErrorRate = currentPidSetpoint - gyroRate;
|
float itermErrorRate = errorRate;
|
||||||
|
|
||||||
#if defined(USE_ITERM_RELAX)
|
#if defined(USE_ITERM_RELAX)
|
||||||
if (itermRelax && (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY || itermRelax == ITERM_RELAX_RPY_INC)) {
|
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
|
#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. ----------
|
// --------low-level gyro-based PID based on 2DOF PID controller. ----------
|
||||||
// 2-DOF PID controller with optional filter on derivative term.
|
// 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).
|
// b = 1 and only c (feedforward weight) can be tuned (amount derivative on measurement or error).
|
||||||
|
|
Loading…
Reference in New Issue