mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-24 08:45:36 +03:00
Apply iterm_windup per-axis by limiting iTerm based on pidSum (#13543)
This commit is contained in:
ctzsnooze
GitHub
parent
3e130de49c
commit
3fe4281a63
6 changed files with 162 additions and 77 deletions
|
|
@ -135,15 +135,15 @@ void setDefaultTestSettings(void)
|
|||
// Compensate for the upscaling done without 'use_integrated_yaw'
|
||||
pidProfile->pid[PID_YAW].I = pidProfile->pid[PID_YAW].I / 2.5f;
|
||||
|
||||
pidProfile->pidSumLimit = PIDSUM_LIMIT;
|
||||
pidProfile->pidSumLimitYaw = PIDSUM_LIMIT_YAW;
|
||||
pidProfile->pidSumLimit = PIDSUM_LIMIT; // 500
|
||||
pidProfile->pidSumLimitYaw = PIDSUM_LIMIT_YAW; // 400
|
||||
pidProfile->yaw_lowpass_hz = 0;
|
||||
pidProfile->dterm_lpf1_static_hz = 100;
|
||||
pidProfile->dterm_lpf2_static_hz = 0;
|
||||
pidProfile->dterm_notch_hz = 260;
|
||||
pidProfile->dterm_notch_cutoff = 160;
|
||||
pidProfile->dterm_lpf1_type = FILTER_BIQUAD;
|
||||
pidProfile->itermWindupPointPercent = 50;
|
||||
pidProfile->itermWindup = 80;
|
||||
pidProfile->pidAtMinThrottle = PID_STABILISATION_ON;
|
||||
pidProfile->angle_limit = 60;
|
||||
pidProfile->feedforward_transition = 100;
|
||||
|
|
@ -315,33 +315,44 @@ TEST(pidControllerTest, testPidLoop)
|
|||
EXPECT_NEAR(231.4, pidData[FD_PITCH].D, calculateTolerance(231.4));
|
||||
EXPECT_FLOAT_EQ(0, pidData[FD_YAW].D);
|
||||
|
||||
// Add some rotation on YAW to generate error
|
||||
gyro.gyroADCf[FD_YAW] = 100;
|
||||
// Add some rotation on YAW to generate error, but not enough to trigger pidSumLimitYaw
|
||||
gyro.gyroADCf[FD_YAW] = 10;
|
||||
pidController(pidProfile, currentTestTime());
|
||||
|
||||
// Loop 4 - Expect PID loop reaction to PITCH error, ROLL and PITCH are still in error
|
||||
EXPECT_NEAR(-128.1, pidData[FD_ROLL].P, calculateTolerance(-128.1));
|
||||
EXPECT_NEAR(185.8, pidData[FD_PITCH].P, calculateTolerance(185.8));
|
||||
EXPECT_NEAR(-224.2, pidData[FD_YAW].P, calculateTolerance(-224.2));
|
||||
EXPECT_NEAR(-22.4, pidData[FD_YAW].P, calculateTolerance(-22.4));
|
||||
EXPECT_NEAR(-23.5, pidData[FD_ROLL].I, calculateTolerance(-23.5));
|
||||
EXPECT_NEAR(19.6, pidData[FD_PITCH].I, calculateTolerance(19.6));
|
||||
EXPECT_NEAR(-8.7, pidData[FD_YAW].I, calculateTolerance(-8.7));
|
||||
EXPECT_NEAR(-0.87, pidData[FD_YAW].I, calculateTolerance(-0.87));
|
||||
EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].D);
|
||||
EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].D);
|
||||
EXPECT_NEAR(-132.25, pidData[FD_YAW].D, calculateTolerance(-132.25));
|
||||
|
||||
// Simulate Iterm behaviour during mixer saturation
|
||||
simulatedMotorMixRange = 1.2f;
|
||||
// Simulate Iterm growth if not saturated
|
||||
pidController(pidProfile, currentTestTime());
|
||||
EXPECT_NEAR(-23.5, pidData[FD_ROLL].I, calculateTolerance(-23.5));
|
||||
EXPECT_NEAR(19.6, pidData[FD_PITCH].I, calculateTolerance(19.6));
|
||||
EXPECT_NEAR(-8.8, pidData[FD_YAW].I, calculateTolerance(-8.8));
|
||||
simulatedMotorMixRange = 0;
|
||||
EXPECT_NEAR(-31.3, pidData[FD_ROLL].I, calculateTolerance(-31.3));
|
||||
EXPECT_NEAR(29.3, pidData[FD_PITCH].I, calculateTolerance(29.3));
|
||||
EXPECT_NEAR(-1.76, pidData[FD_YAW].I, calculateTolerance(-1.76));
|
||||
EXPECT_NEAR(-24.2, pidData[FD_YAW].Sum, calculateTolerance(-24.2));
|
||||
|
||||
// Match the stick to gyro to stop error
|
||||
simulatedSetpointRate[FD_ROLL] = 100;
|
||||
simulatedSetpointRate[FD_PITCH] = -100;
|
||||
simulatedSetpointRate[FD_YAW] = 100;
|
||||
simulatedSetpointRate[FD_YAW] = 10; // error
|
||||
|
||||
pidController(pidProfile, currentTestTime());
|
||||
// Iterm is stalled as it is not accumulating anymore
|
||||
EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].P);
|
||||
EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].P);
|
||||
EXPECT_FLOAT_EQ(0, pidData[FD_YAW].P);
|
||||
EXPECT_NEAR(-31.3, pidData[FD_ROLL].I, calculateTolerance(-31.3));
|
||||
EXPECT_NEAR(29.3, pidData[FD_PITCH].I, calculateTolerance(29.3));
|
||||
EXPECT_NEAR(-1.76, pidData[FD_YAW].I, calculateTolerance(-1.76));
|
||||
EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].D);
|
||||
EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].D);
|
||||
EXPECT_FLOAT_EQ(0, pidData[FD_YAW].D);
|
||||
|
||||
|
||||
for(int loop = 0; loop < 5; loop++) {
|
||||
pidController(pidProfile, currentTestTime());
|
||||
|
|
@ -350,9 +361,9 @@ TEST(pidControllerTest, testPidLoop)
|
|||
EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].P);
|
||||
EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].P);
|
||||
EXPECT_FLOAT_EQ(0, pidData[FD_YAW].P);
|
||||
EXPECT_NEAR(-23.5, pidData[FD_ROLL].I, calculateTolerance(-23.5));
|
||||
EXPECT_NEAR(19.6, pidData[FD_PITCH].I, calculateTolerance(19.6));
|
||||
EXPECT_NEAR(-10.6, pidData[FD_YAW].I, calculateTolerance(-10.6));
|
||||
EXPECT_NEAR(-31.3, pidData[FD_ROLL].I, calculateTolerance(-31.3));
|
||||
EXPECT_NEAR(29.3, pidData[FD_PITCH].I, calculateTolerance(29.3));
|
||||
EXPECT_NEAR(-1.76, pidData[FD_YAW].I, calculateTolerance(-1.76));
|
||||
EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].D);
|
||||
EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].D);
|
||||
EXPECT_FLOAT_EQ(0, pidData[FD_YAW].D);
|
||||
|
|
@ -516,73 +527,153 @@ TEST(pidControllerTest, testPidHorizon)
|
|||
|
||||
}
|
||||
|
||||
// trying to fix
|
||||
|
||||
|
||||
|
||||
TEST(pidControllerTest, testMixerSaturation)
|
||||
{
|
||||
resetTest();
|
||||
|
||||
ENABLE_ARMING_FLAG(ARMED);
|
||||
pidStabilisationState(PID_STABILISATION_ON);
|
||||
|
||||
pidRuntime.itermLimit = 400;
|
||||
pidRuntime.itermLimitYaw = 320;
|
||||
|
||||
// Test full stick response
|
||||
setStickPosition(FD_ROLL, 1.0f);
|
||||
setStickPosition(FD_PITCH, -1.0f);
|
||||
setStickPosition(FD_YAW, 1.0f);
|
||||
simulatedMotorMixRange = 2.0f;
|
||||
pidController(pidProfile, currentTestTime());
|
||||
|
||||
// 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);
|
||||
// Expect iterm accumulation for all axes because at this point, pidSum is not at limit
|
||||
EXPECT_NEAR(156.2f, pidData[FD_ROLL].I, calculateTolerance(156.2f));
|
||||
EXPECT_NEAR(-195.3f, pidData[FD_PITCH].I, calculateTolerance(-195.3f));
|
||||
EXPECT_NEAR(7.0f, pidData[FD_YAW].I, calculateTolerance(7.0f));
|
||||
|
||||
// Test itermWindup limit
|
||||
// First store values without exceeding iterm windup limit
|
||||
resetTest();
|
||||
ENABLE_ARMING_FLAG(ARMED);
|
||||
pidStabilisationState(PID_STABILISATION_ON);
|
||||
setStickPosition(FD_ROLL, 0.1f);
|
||||
setStickPosition(FD_PITCH, -0.1f);
|
||||
setStickPosition(FD_YAW, 0.1f);
|
||||
simulatedMotorMixRange = 0.0f;
|
||||
pidController(pidProfile, currentTestTime());
|
||||
float rollTestIterm = pidData[FD_ROLL].I;
|
||||
float pitchTestIterm = pidData[FD_PITCH].I;
|
||||
float yawTestIterm = pidData[FD_YAW].I;
|
||||
// ????? why such slow yaw iTerm growth ?? this is not what I see in the real logs == strange
|
||||
|
||||
// Now compare values when exceeding the limit
|
||||
resetTest();
|
||||
ENABLE_ARMING_FLAG(ARMED);
|
||||
pidStabilisationState(PID_STABILISATION_ON);
|
||||
setStickPosition(FD_ROLL, 0.1f);
|
||||
setStickPosition(FD_PITCH, -0.1f);
|
||||
setStickPosition(FD_YAW, 0.1f);
|
||||
simulatedMotorMixRange = (pidProfile->itermWindupPointPercent + 2) / 100.0f;
|
||||
// Check for iterm growth, should not reach limits yet
|
||||
pidController(pidProfile, currentTestTime());
|
||||
EXPECT_LT(pidData[FD_ROLL].I, rollTestIterm);
|
||||
EXPECT_GE(pidData[FD_PITCH].I, pitchTestIterm);
|
||||
EXPECT_LT(pidData[FD_YAW].I, yawTestIterm);
|
||||
EXPECT_NEAR(312.4f, pidData[FD_ROLL].I, calculateTolerance(312.4f));
|
||||
EXPECT_NEAR(-390.6f, pidData[FD_PITCH].I, calculateTolerance(-390.6));
|
||||
EXPECT_NEAR(21.1f, pidData[FD_YAW].I, calculateTolerance(21.1));
|
||||
|
||||
// Expect iterm roll + pitch to stop at limit of 400
|
||||
// yaw is still growing,
|
||||
pidController(pidProfile, currentTestTime());
|
||||
EXPECT_NEAR(400.0f, pidData[FD_ROLL].I, calculateTolerance(400.0f));
|
||||
EXPECT_NEAR(-400.0f, pidData[FD_PITCH].I, calculateTolerance(-400.0f));
|
||||
EXPECT_NEAR(42.2f, pidData[FD_YAW].I, calculateTolerance(42.2f));
|
||||
|
||||
// run some more loops, check all iTerm values are at their limit
|
||||
for (int loop = 0; loop < 7; loop++) {
|
||||
pidController(pidProfile, currentTestTime());
|
||||
}
|
||||
EXPECT_NEAR(400, pidData[FD_ROLL].I, calculateTolerance(400));
|
||||
EXPECT_NEAR(-400, pidData[FD_PITCH].I, calculateTolerance(-400));
|
||||
EXPECT_NEAR(320, pidData[FD_YAW].I, calculateTolerance(320));
|
||||
|
||||
// Test that the added i term gain from Anti Gravity
|
||||
// is also affected by itermWindup
|
||||
// is also limited
|
||||
resetTest();
|
||||
ENABLE_ARMING_FLAG(ARMED);
|
||||
pidStabilisationState(PID_STABILISATION_ON);
|
||||
pidRuntime.itermLimit = 400;
|
||||
pidRuntime.itermLimitYaw = 320;
|
||||
|
||||
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());
|
||||
|
||||
// Expect more iterm accumulation than before on pitch and roll, no change on yaw
|
||||
// without antigravity values were 156, 195, 7
|
||||
EXPECT_NEAR(210.6, pidData[FD_ROLL].I, calculateTolerance(210.6f));
|
||||
EXPECT_NEAR(-249.6f, pidData[FD_PITCH].I, calculateTolerance(-249.6f));
|
||||
EXPECT_NEAR(7.0f, pidData[FD_YAW].I, calculateTolerance(7.0f));
|
||||
|
||||
// run again and should expect to hit the limit on pitch and roll but yaw unaffected
|
||||
pidController(pidProfile, currentTestTime());
|
||||
EXPECT_NEAR(400, pidData[FD_ROLL].I, calculateTolerance(400));
|
||||
EXPECT_NEAR(-400, pidData[FD_PITCH].I, calculateTolerance(-400));
|
||||
EXPECT_NEAR(21.0f, pidData[FD_YAW].I, calculateTolerance(21.0f));
|
||||
|
||||
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);
|
||||
// Test that i term is limited on yaw at 320 when only yaw is saturated
|
||||
resetTest();
|
||||
ENABLE_ARMING_FLAG(ARMED);
|
||||
pidRuntime.itermLimit = 400;
|
||||
pidRuntime.itermLimitYaw = 320;
|
||||
|
||||
pidStabilisationState(PID_STABILISATION_ON);
|
||||
setStickPosition(FD_ROLL, 0.0f);
|
||||
setStickPosition(FD_PITCH, 0.0f);
|
||||
setStickPosition(FD_YAW, 0.5f);
|
||||
|
||||
for (int loop = 0; loop < 7; loop++) {
|
||||
pidController(pidProfile, currentTestTime());
|
||||
}
|
||||
|
||||
EXPECT_NEAR(0, pidData[FD_ROLL].I, calculateTolerance(0));
|
||||
EXPECT_NEAR(0, pidData[FD_PITCH].I, calculateTolerance(0));
|
||||
EXPECT_NEAR(197, pidData[FD_YAW].I, calculateTolerance(197));
|
||||
|
||||
pidController(pidProfile, currentTestTime());
|
||||
EXPECT_NEAR(253, pidData[FD_YAW].I, calculateTolerance(320));
|
||||
|
||||
pidController(pidProfile, currentTestTime());
|
||||
EXPECT_NEAR(320, pidData[FD_YAW].I, calculateTolerance(320));
|
||||
|
||||
pidController(pidProfile, currentTestTime());
|
||||
EXPECT_NEAR(0, pidData[FD_ROLL].I, calculateTolerance(0));
|
||||
EXPECT_NEAR(0, pidData[FD_PITCH].I, calculateTolerance(0));
|
||||
EXPECT_NEAR(320, pidData[FD_YAW].I, calculateTolerance(320));
|
||||
}
|
||||
|
||||
TEST(pidControllerTest, testiTermWindup)
|
||||
{
|
||||
resetTest();
|
||||
ENABLE_ARMING_FLAG(ARMED);
|
||||
// simulate the outcome with iterm_windup of 50
|
||||
pidRuntime.itermLimit = 200;
|
||||
pidRuntime.itermLimitYaw = 160;
|
||||
|
||||
pidStabilisationState(PID_STABILISATION_ON);
|
||||
setStickPosition(FD_ROLL, 0.12f);
|
||||
setStickPosition(FD_PITCH, 0.12f);
|
||||
setStickPosition(FD_YAW, 0.12f);
|
||||
|
||||
for (int loop = 0; loop < 7; loop++) {
|
||||
pidController(pidProfile, currentTestTime());
|
||||
}
|
||||
|
||||
EXPECT_NEAR(131, pidData[FD_ROLL].I, calculateTolerance(131));
|
||||
EXPECT_NEAR(164, pidData[FD_PITCH].I, calculateTolerance(164));
|
||||
EXPECT_NEAR(126, pidData[FD_YAW].I, calculateTolerance(126));
|
||||
|
||||
pidController(pidProfile, currentTestTime());
|
||||
EXPECT_NEAR(150, pidData[FD_ROLL].I, calculateTolerance(150));
|
||||
EXPECT_NEAR(200, pidData[FD_PITCH].I, calculateTolerance(200));
|
||||
EXPECT_NEAR(137, pidData[FD_YAW].I, calculateTolerance(137));
|
||||
|
||||
pidController(pidProfile, currentTestTime());
|
||||
EXPECT_NEAR(169, pidData[FD_ROLL].I, calculateTolerance(200));
|
||||
EXPECT_NEAR(200, pidData[FD_PITCH].I, calculateTolerance(200));
|
||||
EXPECT_NEAR(160, pidData[FD_YAW].I, calculateTolerance(160));
|
||||
|
||||
pidController(pidProfile, currentTestTime());
|
||||
EXPECT_NEAR(200, pidData[FD_ROLL].I, calculateTolerance(200));
|
||||
EXPECT_NEAR(200, pidData[FD_PITCH].I, calculateTolerance(200));
|
||||
EXPECT_NEAR(160, pidData[FD_YAW].I, calculateTolerance(320));
|
||||
}
|
||||
|
||||
// TODO - Add more scenarios
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue