From a232655b425b1d18cc9bf91f1dc38c2083993b12 Mon Sep 17 00:00:00 2001 From: MikeNomatter Date: Wed, 21 Aug 2024 15:08:01 +0200 Subject: [PATCH] Fix d adjustment (#13822) * fix D adjustments change dmax instead of D * fix unit test * add yaw and other adjustment cases * fix tests --- src/main/fc/rc_adjustments.c | 18 +++++++++--------- src/test/unit/rc_controls_unittest.cc | 14 +++++++++++--- 2 files changed, 20 insertions(+), 12 deletions(-) diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index ad4cbf0529..6cf5760ca7 100644 --- a/src/main/fc/rc_adjustments.c +++ b/src/main/fc/rc_adjustments.c @@ -364,8 +364,8 @@ 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; @@ -373,8 +373,8 @@ static int applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t a // 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 index dac615ac02..d419ab4e2d 100644 --- 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" {