rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #6682 from etracer65/crash_recovery_term_relax_fix 

Fix iterm relax interfering with crash recovery
This commit is contained in:
Michael Keller 2018-09-03 23:04:52 +12:00 committed by GitHub
parent adc965327f 960442e917
commit 5c8d08889d
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 5 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
src/main/flight/pid.c
View File

@ -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,
&currentPidSetpoint, &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,
&currentPidSetpoint, &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).