Fix d adjustment (betaflight#13822)

* fix D adjustments change dmax instead of D * fix unit test * add yaw and other adjustment cases * fix tests
demvlad · Aug 21, 2024 · a232655 · a232655
1 parent dd73ce4
commit a232655
Show file tree

Hide file tree

Showing 2 changed files with 20 additions and 12 deletions.
diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c
@@ -364,17 +364,17 @@ static int applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t a
         break;
     case ADJUSTMENT_PITCH_ROLL_D:
     case ADJUSTMENT_PITCH_D:
-        newValue = constrain((int)currentPidProfile->pid[PID_PITCH].D + delta, 0, 200); // FIXME magic numbers repeated in cli.c
-        currentPidProfile->pid[PID_PITCH].D = newValue;
+        newValue = constrain((int)currentPidProfile->d_min[FD_PITCH] + delta, 0, 200); // FIXME magic numbers repeated in cli.c
+        currentPidProfile->d_min[FD_PITCH] = newValue;
         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_D, newValue);
         if (adjustmentFunction == ADJUSTMENT_PITCH_D) {
             break;
         }
         // fall through for combined ADJUSTMENT_PITCH_ROLL_D
         FALLTHROUGH;
     case ADJUSTMENT_ROLL_D:
-        newValue = constrain((int)currentPidProfile->pid[PID_ROLL].D + delta, 0, 200); // FIXME magic numbers repeated in cli.c
-        currentPidProfile->pid[PID_ROLL].D = newValue;
+        newValue = constrain((int)currentPidProfile->d_min[FD_ROLL] + delta, 0, 200); // FIXME magic numbers repeated in cli.c
+        currentPidProfile->d_min[FD_ROLL] = newValue;
         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_D, newValue);
         break;
     case ADJUSTMENT_YAW_P:
@@ -388,8 +388,8 @@ static int applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t a
         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_I, newValue);
         break;
     case ADJUSTMENT_YAW_D:
-        newValue = constrain((int)currentPidProfile->pid[PID_YAW].D + delta, 0, 200); // FIXME magic numbers repeated in cli.c
-        currentPidProfile->pid[PID_YAW].D = newValue;
+        newValue = constrain((int)currentPidProfile->d_min[FD_YAW] + delta, 0, 200); // FIXME magic numbers repeated in cli.c
+        currentPidProfile->d_min[FD_YAW] = newValue;
         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_D, newValue);
         break;
     case ADJUSTMENT_RC_RATE_YAW:
@@ -528,7 +528,7 @@ static int applyAbsoluteAdjustment(controlRateConfig_t *controlRateConfig, adjus
     case ADJUSTMENT_PITCH_ROLL_D:
     case ADJUSTMENT_PITCH_D:
         newValue = constrain(value, 0, 200); // FIXME magic numbers repeated in cli.c
-        currentPidProfile->pid[PID_PITCH].D = newValue;
+        currentPidProfile->d_min[FD_PITCH] = newValue;
         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_D, newValue);
         if (adjustmentFunction == ADJUSTMENT_PITCH_D) {
             break;
@@ -537,7 +537,7 @@ static int applyAbsoluteAdjustment(controlRateConfig_t *controlRateConfig, adjus
         FALLTHROUGH;
     case ADJUSTMENT_ROLL_D:
         newValue = constrain(value, 0, 200); // FIXME magic numbers repeated in cli.c
-        currentPidProfile->pid[PID_ROLL].D = newValue;
+        currentPidProfile->d_min[FD_ROLL] = newValue;
         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_D, newValue);
         break;
     case ADJUSTMENT_YAW_P:
@@ -552,7 +552,7 @@ static int applyAbsoluteAdjustment(controlRateConfig_t *controlRateConfig, adjus
         break;
     case ADJUSTMENT_YAW_D:
         newValue = constrain(value, 0, 200); // FIXME magic numbers repeated in cli.c
-        currentPidProfile->pid[PID_YAW].D = newValue;
+        currentPidProfile->d_min[FD_YAW] = newValue;
         blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_D, newValue);
         break;
     case ADJUSTMENT_RC_RATE_YAW:

diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc
@@ -549,6 +549,11 @@ TEST_F(RcControlsAdjustmentsTest, processPIDIncreasePidController0)
     pidProfile.pid[PID_YAW].P = 7;
     pidProfile.pid[PID_YAW].I = 17;
     pidProfile.pid[PID_YAW].D = 27;
+
+    pidProfile.d_min[FD_PITCH] = 19;
+    pidProfile.d_min[FD_ROLL] = 19;
+    pidProfile.d_min[FD_YAW] = 19;
+
     // and
     controlRateConfig_t controlRateConfig;
     memset(&controlRateConfig, 0, sizeof(controlRateConfig));
@@ -591,13 +596,16 @@ TEST_F(RcControlsAdjustmentsTest, processPIDIncreasePidController0)
     // and
     EXPECT_EQ(1,  pidProfile.pid[PID_PITCH].P);
     EXPECT_EQ(11, pidProfile.pid[PID_PITCH].I);
-    EXPECT_EQ(21, pidProfile.pid[PID_PITCH].D);
+    EXPECT_EQ(20, pidProfile.pid[PID_PITCH].D);
+    EXPECT_EQ(20, pidProfile.d_min[FD_PITCH]);
     EXPECT_EQ(6,  pidProfile.pid[PID_ROLL].P);
     EXPECT_EQ(16, pidProfile.pid[PID_ROLL].I);
-    EXPECT_EQ(26, pidProfile.pid[PID_ROLL].D);
+    EXPECT_EQ(25, pidProfile.pid[PID_ROLL].D);
+    EXPECT_EQ(20, pidProfile.d_min[FD_ROLL]);
     EXPECT_EQ(8,  pidProfile.pid[PID_YAW].P);
     EXPECT_EQ(18, pidProfile.pid[PID_YAW].I);
-    EXPECT_EQ(28, pidProfile.pid[PID_YAW].D);
+    EXPECT_EQ(27, pidProfile.pid[PID_YAW].D);
+    EXPECT_EQ(20, pidProfile.d_min[FD_YAW]);
 }
 
 extern "C" {