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

Fix to calculation of ITerm anti-windup

This commit is contained in:
Martin Budden 2017-02-02 12:15:55 +00:00 committed by borisbstyle
parent 70807d016b
commit 080f9b7923
1 changed files with 6 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -220,7 +220,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
const float motorMixRange = getMotorMixRange(); const float motorMixRange = getMotorMixRange();
// Dynamic ki component to gradually scale back integration when above windup point // Dynamic ki component to gradually scale back integration when above windup point
float dynKi = MIN((1.0f - motorMixRange) * ITermWindupPointInv, 1.0f); const float dynKi = MIN((1.0f - motorMixRange) * ITermWindupPointInv, 1.0f);
// ----------PID controller---------- // ----------PID controller----------
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
@ -251,11 +251,12 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
// -----calculate I component // -----calculate I component
float ITerm = previousGyroIf[axis]; float ITerm = previousGyroIf[axis];
if (motorMixRange < 1.0f) { if (motorMixRange < 1.0f) {
// Only increase ITerm if motor output is not saturated and errorRate exceeds noise threshold // Only increase ITerm if motor output is not saturated
// Iterm will only be accelerated below steady rate threshold
if (ABS(currentPidSetpoint) < itermAcceleratorRateLimit)
dynKi *= itermAccelerator;
float ITermDelta = Ki[axis] * errorRate * dT * dynKi; float ITermDelta = Ki[axis] * errorRate * dT * dynKi;
if (ABS(currentPidSetpoint) < itermAcceleratorRateLimit) {
// ITerm will only be accelerated below steady rate threshold
ITermDelta *= itermAccelerator;
}
ITerm += ITermDelta; ITerm += ITermDelta;
previousGyroIf[axis] = ITerm; previousGyroIf[axis] = ITerm;
} }