Apply iterm_windup per-axis by limiting iTerm based on pidSum (#13543)

src/main/blackbox/blackbox.c

@@ -1483,7 +1483,7 @@ static bool blackboxWriteSysinfo(void)
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_YAW_LOWPASS_HZ, "%d",         currentPidProfile->yaw_lowpass_hz);
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DTERM_NOTCH_HZ, "%d",         currentPidProfile->dterm_notch_hz);
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DTERM_NOTCH_CUTOFF, "%d",     currentPidProfile->dterm_notch_cutoff);
-        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ITERM_WINDUP, "%d",           currentPidProfile->itermWindupPointPercent);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ITERM_WINDUP, "%d",           currentPidProfile->itermWindup);
 #if defined(USE_ITERM_RELAX)
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ITERM_RELAX, "%d",            currentPidProfile->iterm_relax);
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ITERM_RELAX_TYPE, "%d",       currentPidProfile->iterm_relax_type);

src/main/cli/settings.c

@@ -1145,8 +1145,7 @@ const clivalue_t valueTable[] = {
     { PARAM_NAME_ITERM_RELAX_TYPE,   VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ITERM_RELAX_TYPE }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax_type) },
     { PARAM_NAME_ITERM_RELAX_CUTOFF, VAR_UINT8  | PROFILE_VALUE, .config.minmaxUnsigned = { 1, 50 }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax_cutoff) },
 #endif
-    { PARAM_NAME_ITERM_WINDUP,      VAR_UINT8  | PROFILE_VALUE, .config.minmaxUnsigned = { 30, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermWindupPointPercent) },
+    { PARAM_NAME_ITERM_WINDUP,      VAR_UINT8  | PROFILE_VALUE, .config.minmaxUnsigned = { 20, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermWindup) },
-    { "iterm_limit",                VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermLimit) },
     { PARAM_NAME_PIDSUM_LIMIT,      VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { PIDSUM_LIMIT_MIN, PIDSUM_LIMIT_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, pidSumLimit) },
     { PARAM_NAME_PIDSUM_LIMIT_YAW,  VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { PIDSUM_LIMIT_MIN, PIDSUM_LIMIT_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, pidSumLimitYaw) },
     { PARAM_NAME_YAW_LOWPASS_HZ,    VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, yaw_lowpass_hz) },

src/main/flight/pid.c

@@ -133,7 +133,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
         .yaw_lowpass_hz = 100,
         .dterm_notch_hz = 0,
         .dterm_notch_cutoff = 0,
-        .itermWindupPointPercent = 85,
+        .itermWindup = 80,         // sets iTerm limit to this percentage below pidSumLimit
         .pidAtMinThrottle = PID_STABILISATION_ON,
         .angle_limit = 60,
         .feedforward_transition = 0,
@@ -994,12 +994,6 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
     // amount of antigravity added relative to user's pitch iTerm coefficient
     // used later to increase iTerm
 
-    // iTerm windup (attenuation of iTerm if motorMix range is large)
-    float dynCi = 1.0;
-    if (pidRuntime.itermWindupPointInv > 1.0f) {
-        dynCi = constrainf((1.0f - getMotorMixRange()) * pidRuntime.itermWindupPointInv, 0.0f, 1.0f);
-    }
-
     // Precalculate gyro delta for D-term here, this allows loop unrolling
     float gyroRateDterm[XYZ_AXIS_COUNT];
     for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
@@ -1119,9 +1113,10 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
             pidData[axis].P = pidRuntime.ptermYawLowpassApplyFn((filter_t *) &pidRuntime.ptermYawLowpass, pidData[axis].P);
         }
 
-
         // -----calculate I component
         float Ki = pidRuntime.pidCoefficient[axis].Ki;
+        float itermLimit = pidRuntime.itermLimit; // windup fraction of pidSumLimit
+
 #ifdef USE_LAUNCH_CONTROL
         // if launch control is active override the iterm gains and apply iterm windup protection to all axes
         if (launchControlActive) {
@@ -1129,13 +1124,15 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
         } else
 #endif
         {
+            // yaw iTerm has it's own limit based on pidSumLimitYaw
             if (axis == FD_YAW) {
+                itermLimit = pidRuntime.itermLimitYaw; // windup fraction of pidSumLimitYaw
+                // note that this is a stronger limit than previously
                 pidRuntime.itermAccelerator = 0.0f; // no antigravity on yaw iTerm
             }
         }
 
-        float iTermChange = (Ki + pidRuntime.itermAccelerator) * dynCi * pidRuntime.dT * itermErrorRate;
+        float iTermChange = (Ki + pidRuntime.itermAccelerator) * pidRuntime.dT * itermErrorRate;
-
 #ifdef USE_WING
         if (pidProfile->spa_mode[axis] != SPA_MODE_OFF) {
             // slowing down I-term change, or even making it zero if setpoint is high enough
@@ -1143,7 +1140,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
         }
 #endif // USE_WING
 
-        pidData[axis].I = constrainf(previousIterm + iTermChange, -pidRuntime.itermLimit, pidRuntime.itermLimit);
+        pidData[axis].I = constrainf(previousIterm + iTermChange, -itermLimit, itermLimit);
 
         // -----calculate D component
 

src/main/flight/pid.h

@@ -173,9 +173,9 @@ typedef struct pidProfile_s {
     pidf_t  pid[PID_ITEM_COUNT];
 
     uint8_t dterm_lpf1_type;                // Filter type for dterm lowpass 1
-    uint8_t itermWindupPointPercent;        // iterm windup threshold, percent motor saturation
+    uint8_t itermWindup;                    // iterm windup threshold, percentage of pidSumLimit within which to limit iTerm
-    uint16_t pidSumLimit;
+    uint16_t pidSumLimit;                   // pidSum limit value for pitch and roll
-    uint16_t pidSumLimitYaw;
+    uint16_t pidSumLimitYaw;                // pidSum limit value for yaw
     uint8_t pidAtMinThrottle;               // Disable/Enable pids on zero throttle. Normally even without airmode P and D would be active.
     uint8_t angle_limit;                    // Max angle in degrees in Angle mode
 
@@ -361,7 +361,6 @@ typedef struct pidRuntime_s {
     float horizonLimitDegreesInv;
     float horizonIgnoreSticks;
     float maxVelocity[XYZ_AXIS_COUNT];
-    float itermWindupPointInv;
     bool inCrashRecoveryMode;
     timeUs_t crashDetectedAtUs;
     timeDelta_t crashTimeLimitUs;
@@ -373,6 +372,7 @@ typedef struct pidRuntime_s {
     float crashSetpointThreshold;
     float crashLimitYaw;
     float itermLimit;
+    float itermLimitYaw;
     bool itermRotation;
     bool zeroThrottleItermReset;
     bool levelRaceMode;

src/main/flight/pid_init.c

@@ -351,11 +351,6 @@ void pidInitConfig(const pidProfile_t *pidProfile)
 
     pidRuntime.maxVelocity[FD_ROLL] = pidRuntime.maxVelocity[FD_PITCH] = pidProfile->rateAccelLimit * 100 * pidRuntime.dT;
     pidRuntime.maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 100 * pidRuntime.dT;
-    pidRuntime.itermWindupPointInv = 1.0f;
-    if (pidProfile->itermWindupPointPercent < 100) {
-        const float itermWindupPoint = pidProfile->itermWindupPointPercent / 100.0f;
-        pidRuntime.itermWindupPointInv = 1.0f / (1.0f - itermWindupPoint);
-    }
     pidRuntime.antiGravityGain = pidProfile->anti_gravity_gain;
     pidRuntime.crashTimeLimitUs = pidProfile->crash_time * 1000;
     pidRuntime.crashTimeDelayUs = pidProfile->crash_delay * 1000;
@@ -365,7 +360,10 @@ void pidInitConfig(const pidProfile_t *pidProfile)
     pidRuntime.crashDtermThreshold = pidProfile->crash_dthreshold * 1000.0f; // gyro delta in deg/s/s * 1000 to match original 2017 intent
     pidRuntime.crashSetpointThreshold = pidProfile->crash_setpoint_threshold;
     pidRuntime.crashLimitYaw = pidProfile->crash_limit_yaw;
-    pidRuntime.itermLimit = pidProfile->itermLimit;
+
+    pidRuntime.itermLimit = 0.01f * pidProfile->itermWindup * pidProfile->pidSumLimit;
+    pidRuntime.itermLimitYaw = 0.01f * pidProfile->itermWindup * pidProfile->pidSumLimitYaw;
+
 #if defined(USE_THROTTLE_BOOST)
     throttleBoost = pidProfile->throttle_boost * 0.1f;
 #endif

src/test/unit/pid_unittest.cc

@@ -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->pidSumLimit = PIDSUM_LIMIT;        // 500
-    pidProfile->pidSumLimitYaw = PIDSUM_LIMIT_YAW;
+    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
+    // Add some rotation on YAW to generate error, but not enough to trigger pidSumLimitYaw
-    gyro.gyroADCf[FD_YAW] = 100;
+    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
+    // Simulate Iterm growth if not saturated
-    simulatedMotorMixRange = 1.2f;
     pidController(pidProfile, currentTestTime());
-    EXPECT_NEAR(-23.5, pidData[FD_ROLL].I, calculateTolerance(-23.5));
+    EXPECT_NEAR(-31.3, pidData[FD_ROLL].I, calculateTolerance(-31.3));
-    EXPECT_NEAR(19.6, pidData[FD_PITCH].I, calculateTolerance(19.6));
+    EXPECT_NEAR(29.3, pidData[FD_PITCH].I, calculateTolerance(29.3));
-    EXPECT_NEAR(-8.8, pidData[FD_YAW].I, calculateTolerance(-8.8));
+    EXPECT_NEAR(-1.76, pidData[FD_YAW].I, calculateTolerance(-1.76));
-    simulatedMotorMixRange = 0;
+        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(-31.3, pidData[FD_ROLL].I, calculateTolerance(-31.3));
-    EXPECT_NEAR(19.6, pidData[FD_PITCH].I, calculateTolerance(19.6));
+    EXPECT_NEAR(29.3, pidData[FD_PITCH].I, calculateTolerance(29.3));
-    EXPECT_NEAR(-10.6, pidData[FD_YAW].I, calculateTolerance(-10.6));
+    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 iterm accumulation for all axes because at this point, pidSum is not at limit
-    EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].I);
+    EXPECT_NEAR(156.2f, pidData[FD_ROLL].I, calculateTolerance(156.2f));
-    EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].I);
+    EXPECT_NEAR(-195.3f, pidData[FD_PITCH].I, calculateTolerance(-195.3f));
-    EXPECT_FLOAT_EQ(0, pidData[FD_YAW].I);
+    EXPECT_NEAR(7.0f, pidData[FD_YAW].I, calculateTolerance(7.0f));
 
-    // Test itermWindup limit
+     // ????? why such slow yaw iTerm growth ?? this is not what I see in the real logs == strange
-    // 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;
 
-    // Now compare values when exceeding the limit
+    // Check for iterm growth, should not reach limits yet
-    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;
     pidController(pidProfile, currentTestTime());
-    EXPECT_LT(pidData[FD_ROLL].I, rollTestIterm);
+    EXPECT_NEAR(312.4f, pidData[FD_ROLL].I, calculateTolerance(312.4f));
-    EXPECT_GE(pidData[FD_PITCH].I, pitchTestIterm);
+    EXPECT_NEAR(-390.6f, pidData[FD_PITCH].I, calculateTolerance(-390.6));
-    EXPECT_LT(pidData[FD_YAW].I, yawTestIterm);
+    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
+    // Test that i term is limited on yaw at 320  when only yaw is saturated
-    EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].I);
+    resetTest();
-    EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].I);
+    ENABLE_ARMING_FLAG(ARMED);
-    EXPECT_FLOAT_EQ(0, pidData[FD_YAW].I);
+    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