Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-15 04:15:44 +03:00
Code Issues Wiki Activity

Merge pull request #11806 from ctzsnooze/Restore-iTerm-Windup-for-all-axes

Browse source 
Restore iTerm Windup to all axes, active by default
This commit is contained in:
J Blackman 2022-09-27 08:42:55 +10:00 • committed by GitHub
commit d72f18fb3f
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
2 changed files with 12 additions and 15 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1004,22 +1004,19 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
// -----calculate I component // -----calculate I component
float Ki = pidRuntime.pidCoefficient[axis].Ki; float Ki = pidRuntime.pidCoefficient[axis].Ki;
float axisDynCi = pidRuntime.dT;
#ifdef USE_LAUNCH_CONTROL #ifdef USE_LAUNCH_CONTROL
// if launch control is active override the iterm gains and apply iterm windup protection to all axes // if launch control is active override the iterm gains and apply iterm windup protection to all axes
if (launchControlActive) { if (launchControlActive) {
Ki = pidRuntime.launchControlKi; Ki = pidRuntime.launchControlKi;
axisDynCi = dynCi;
} else } else
#endif #endif
{ {
if (axis == FD_YAW) { if (axis == FD_YAW) {
axisDynCi = dynCi; // only apply windup protection to yaw
pidRuntime.itermAccelerator = 0.0f; // no antigravity on yaw iTerm pidRuntime.itermAccelerator = 0.0f; // no antigravity on yaw iTerm
} }
} }
pidData[axis].I = constrainf(previousIterm + (Ki * axisDynCi + pidRuntime.itermAccelerator) * itermErrorRate, -pidRuntime.itermLimit, pidRuntime.itermLimit); pidData[axis].I = constrainf(previousIterm + (Ki * dynCi + pidRuntime.itermAccelerator) * itermErrorRate, -pidRuntime.itermLimit, pidRuntime.itermLimit);
// -----calculate pidSetpointDelta // -----calculate pidSetpointDelta
float pidSetpointDelta = 0; float pidSetpointDelta = 0;

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

@ -322,8 +322,8 @@ TEST(pidControllerTest, testPidLoop) {
// Simulate Iterm behaviour during mixer saturation // Simulate Iterm behaviour during mixer saturation
simulatedMotorMixRange = 1.2f; simulatedMotorMixRange = 1.2f;
pidController(pidProfile, currentTestTime()); pidController(pidProfile, currentTestTime());
EXPECT_NEAR(-31.3, pidData[FD_ROLL].I, calculateTolerance(-31.3)); EXPECT_NEAR(-23.5, pidData[FD_ROLL].I, calculateTolerance(-23.5));
EXPECT_NEAR(29.3, pidData[FD_PITCH].I, calculateTolerance(29.3)); EXPECT_NEAR(19.6, pidData[FD_PITCH].I, calculateTolerance(19.6));
EXPECT_NEAR(-8.8, pidData[FD_YAW].I, calculateTolerance(-8.8)); EXPECT_NEAR(-8.8, pidData[FD_YAW].I, calculateTolerance(-8.8));
simulatedMotorMixRange = 0; simulatedMotorMixRange = 0;
@ -339,8 +339,8 @@ TEST(pidControllerTest, testPidLoop) {
EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].P); EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].P);
EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].P); EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].P);
EXPECT_FLOAT_EQ(0, pidData[FD_YAW].P); EXPECT_FLOAT_EQ(0, pidData[FD_YAW].P);
EXPECT_NEAR(-31.3, pidData[FD_ROLL].I, calculateTolerance(-31.3)); EXPECT_NEAR(-23.5, pidData[FD_ROLL].I, calculateTolerance(-23.5));
EXPECT_NEAR(29.3, pidData[FD_PITCH].I, calculateTolerance(29.3)); EXPECT_NEAR(19.6, pidData[FD_PITCH].I, calculateTolerance(19.6));
EXPECT_NEAR(-10.6, pidData[FD_YAW].I, calculateTolerance(-10.6)); EXPECT_NEAR(-10.6, pidData[FD_YAW].I, calculateTolerance(-10.6));
EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].D); EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].D);
EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].D); EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].D);
@ -457,12 +457,12 @@ TEST(pidControllerTest, testMixerSaturation) {
simulatedMotorMixRange = 2.0f; simulatedMotorMixRange = 2.0f;
pidController(pidProfile, currentTestTime()); pidController(pidProfile, currentTestTime());
// Expect no iterm accumulation for yaw // Expect no iterm accumulation for all axes
EXPECT_FLOAT_EQ(150, pidData[FD_ROLL].I); EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].I);
EXPECT_FLOAT_EQ(-150, pidData[FD_PITCH].I); EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].I);
EXPECT_FLOAT_EQ(0, pidData[FD_YAW].I); EXPECT_FLOAT_EQ(0, pidData[FD_YAW].I);
// Test itermWindup limit (note: windup limit now only affects yaw) // Test itermWindup limit
// First store values without exceeding iterm windup limit // First store values without exceeding iterm windup limit
resetTest(); resetTest();
ENABLE_ARMING_FLAG(ARMED); ENABLE_ARMING_FLAG(ARMED);
@ -483,10 +483,10 @@ TEST(pidControllerTest, testMixerSaturation) {
setStickPosition(FD_ROLL, 0.1f); setStickPosition(FD_ROLL, 0.1f);
setStickPosition(FD_PITCH, -0.1f); setStickPosition(FD_PITCH, -0.1f);
setStickPosition(FD_YAW, 0.1f); setStickPosition(FD_YAW, 0.1f);
simulatedMotorMixRange = (pidProfile->itermWindupPointPercent + 1) / 100.0f; simulatedMotorMixRange = (pidProfile->itermWindupPointPercent + 2) / 100.0f;
pidController(pidProfile, currentTestTime()); pidController(pidProfile, currentTestTime());
EXPECT_FLOAT_EQ(pidData[FD_ROLL].I, rollTestIterm); EXPECT_LT(pidData[FD_ROLL].I, rollTestIterm);
EXPECT_FLOAT_EQ(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);
} }