Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-26 09:45:37 +03:00
Code Issues Wiki Activity

Merge pull request #11878 from tbolin/ag_antiwindup_fix

Browse source 
Make anti-windup affect the gain from Anti Gravity
This commit is contained in:
haslinghuis 2022-10-13 04:15:37 +02:00 • committed by GitHub
commit a0e3eb5ce7
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
2 changed files with 27 additions and 5 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -884,13 +884,12 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
} }
DEBUG_SET(DEBUG_ANTI_GRAVITY, 2, lrintf((1 + (pidRuntime.itermAccelerator / pidRuntime.pidCoefficient[FD_PITCH].Ki)) * 1000)); DEBUG_SET(DEBUG_ANTI_GRAVITY, 2, lrintf((1 + (pidRuntime.itermAccelerator / pidRuntime.pidCoefficient[FD_PITCH].Ki)) * 1000));
// amount of antigravity added relative to user's pitch iTerm coefficient // amount of antigravity added relative to user's pitch iTerm coefficient
pidRuntime.itermAccelerator *= pidRuntime.dT;
// used later to increase iTerm // used later to increase iTerm
// iTerm windup (attenuation of iTerm if motorMix range is large) // iTerm windup (attenuation of iTerm if motorMix range is large)
float dynCi = pidRuntime.dT; float dynCi = 1.0;
if (pidRuntime.itermWindupPointInv > 1.0f) { if (pidRuntime.itermWindupPointInv > 1.0f) {
dynCi *= constrainf((1.0f - getMotorMixRange()) * pidRuntime.itermWindupPointInv, 0.0f, 1.0f); dynCi = constrainf((1.0f - getMotorMixRange()) * pidRuntime.itermWindupPointInv, 0.0f, 1.0f);
} }
// Precalculate gyro delta for D-term here, this allows loop unrolling // Precalculate gyro delta for D-term here, this allows loop unrolling
@ -1016,8 +1015,8 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
pidRuntime.itermAccelerator = 0.0f; // no antigravity on yaw iTerm pidRuntime.itermAccelerator = 0.0f; // no antigravity on yaw iTerm
} }
} }
const float iTermChange = (Ki + pidRuntime.itermAccelerator) * dynCi * pidRuntime.dT * itermErrorRate;
pidData[axis].I = constrainf(previousIterm + (Ki * dynCi + pidRuntime.itermAccelerator) * itermErrorRate, -pidRuntime.itermLimit, pidRuntime.itermLimit); pidData[axis].I = constrainf(previousIterm + iTermChange, -pidRuntime.itermLimit, pidRuntime.itermLimit);
// -----calculate pidSetpointDelta // -----calculate pidSetpointDelta
float pidSetpointDelta = 0; float pidSetpointDelta = 0;

23
src/test/unit/pid_unittest.cc
View file

@ -498,6 +498,29 @@ TEST(pidControllerTest, testMixerSaturation)
EXPECT_LT(pidData[FD_ROLL].I, rollTestIterm); EXPECT_LT(pidData[FD_ROLL].I, rollTestIterm);
EXPECT_GE(pidData[FD_PITCH].I, pitchTestIterm); EXPECT_GE(pidData[FD_PITCH].I, pitchTestIterm);
EXPECT_LT(pidData[FD_YAW].I, yawTestIterm); EXPECT_LT(pidData[FD_YAW].I, yawTestIterm);
// Test that the added i term gain from Anti Gravity
// is also affected by itermWindup
resetTest();
ENABLE_ARMING_FLAG(ARMED);
pidStabilisationState(PID_STABILISATION_ON);
setStickPosition(FD_ROLL, 1.0f);
setStickPosition(FD_PITCH, -1.0f);
setStickPosition(FD_YAW, 1.0f);
simulatedMotorMixRange = 2.0f;
const bool prevAgState = pidRuntime.antiGravityEnabled;
const float prevAgTrhottleD = pidRuntime.antiGravityThrottleD;
pidRuntime.antiGravityEnabled = true;
pidRuntime.antiGravityThrottleD = 1.0;
pidController(pidProfile, currentTestTime());
pidRuntime.antiGravityEnabled = prevAgState;
pidRuntime.antiGravityThrottleD = prevAgTrhottleD;
// Expect no iterm accumulation for all axes
EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].I);
EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].I);
EXPECT_FLOAT_EQ(0, pidData[FD_YAW].I);
} }
// TODO - Add more scenarios // TODO - Add more scenarios