Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 00:35:34 +03:00
Code Issues Wiki Activity

pid windup backcalc fix

Browse source
This commit is contained in:
shota 2023-08-07 22:32:23 +09:00
parent 57bba67f86
commit ecad9fe749
2 changed files with 7 additions and 1 deletions
Download patch file Download diff file Expand all files Collapse all files

1
src/main/common/maths.h
View file

@ -91,6 +91,7 @@
) )
#define MIN(a, b) _CHOOSE(<, a, b) #define MIN(a, b) _CHOOSE(<, a, b)
#define MAX(a, b) _CHOOSE(>, a, b) #define MAX(a, b) _CHOOSE(>, a, b)
#define SIGN(a) ((a >= 0) ? 1 : -1)
#define _ABS_II(x, var) \ #define _ABS_II(x, var) \
( __extension__ ({ \ ( __extension__ ({ \

7
src/main/flight/pid.c
View file

@ -816,6 +816,11 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid
const float newOutput = newPTerm + newDTerm + pidState->errorGyroIf + newCDTerm; const float newOutput = newPTerm + newDTerm + pidState->errorGyroIf + newCDTerm;
const float newOutputLimited = constrainf(newOutput, -pidState->pidSumLimit, +pidState->pidSumLimit); const float newOutputLimited = constrainf(newOutput, -pidState->pidSumLimit, +pidState->pidSumLimit);
float backCalc = newOutputLimited - newOutput;//back-calculation anti-windup
if (SIGN(backCalc) == SIGN(pidState->errorGyroIf)) {
//back calculation anti-windup can only shrink integrator, will not push it to the opposite direction
backCalc = 0.0f;
}
float itermErrorRate = applyItermRelax(axis, rateTarget, rateError); float itermErrorRate = applyItermRelax(axis, rateTarget, rateError);
#ifdef USE_ANTIGRAVITY #ifdef USE_ANTIGRAVITY
@ -824,7 +829,7 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid
if (!pidState->itermFreezeActive) { if (!pidState->itermFreezeActive) {
pidState->errorGyroIf += (itermErrorRate * pidState->kI * antiWindupScaler * dT) pidState->errorGyroIf += (itermErrorRate * pidState->kI * antiWindupScaler * dT)
+ ((newOutputLimited - newOutput) * pidState->kT * antiWindupScaler * dT); + (backCalc * pidState->kT * antiWindupScaler * dT);
} }
if (pidProfile()->pidItermLimitPercent != 0){ if (pidProfile()->pidItermLimitPercent != 0){