Fix bug with antiwindup inversion

betaflight · Sep 10, 2018 · 864a2d6 · 864a2d6
1 parent 39ced6b
commit 864a2d6
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 3 deletions.
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
@@ -857,8 +857,10 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
         DEBUG_SET(DEBUG_ANTI_GRAVITY, 1, lrintf(antiGravityThrottleHpf * 1000));
     }
     DEBUG_SET(DEBUG_ANTI_GRAVITY, 0, lrintf(itermAccelerator * 1000));
+
     // gradually scale back integration when above windup point
-    const float dynCi = MIN((1.0f - motorMixRange) * ITermWindupPointInv, 1.0f) * dT * itermAccelerator;
+    const float dynCi = constrainf((1.0f - motorMixRange) * ITermWindupPointInv, 0.0f, 1.0f)
+        * dT * itermAccelerator;
 
     // Precalculate gyro deta for D-term here, this allows loop unrolling
     float gyroRateDterm[XYZ_AXIS_COUNT];

diff --git a/src/test/unit/pid_unittest.cc b/src/test/unit/pid_unittest.cc
@@ -269,6 +269,14 @@ TEST(pidControllerTest, testPidLoop) {
     EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].D);
     ASSERT_NEAR(-132.25, pidData[FD_YAW].D, calculateTolerance(-132.25));
 
+    // Simulate Iterm behaviour during mixer saturation
+    simulatedMotorMixRange = 1.2f;
+    pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
+    ASSERT_NEAR(-23.5, pidData[FD_ROLL].I, calculateTolerance(-23.5));
+    ASSERT_NEAR(19.6, pidData[FD_PITCH].I, calculateTolerance(19.6));
+    ASSERT_NEAR(-8.8, pidData[FD_YAW].I, calculateTolerance(-8.8));
+    simulatedMotorMixRange = 0;
+
     // Match the stick to gyro to stop error
     simulatedSetpointRate[FD_ROLL] = 100;
     simulatedSetpointRate[FD_PITCH] = -100;
@@ -277,14 +285,13 @@ TEST(pidControllerTest, testPidLoop) {
     for(int loop = 0; loop < 5; loop++) {
         pidController(pidProfile, &rollAndPitchTrims, 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);
     ASSERT_NEAR(-23.5, pidData[FD_ROLL].I, calculateTolerance(-23.5));
     ASSERT_NEAR(19.6, pidData[FD_PITCH].I, calculateTolerance(19.6));
-    ASSERT_NEAR(-10.6, pidData[FD_YAW].I, calculateTolerance(-10.5));
+    ASSERT_NEAR(-10.6, pidData[FD_YAW].I, calculateTolerance(-10.6));
     EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].D);
     EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].D);
     EXPECT_FLOAT_EQ(0, pidData[FD_YAW].D);