From 00dbaf9fa7325133265b694055a1155672ed5eda Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Mon, 10 Sep 2018 12:27:53 +0200 Subject: [PATCH] Cleanup code // add unittestst for iterm windup --- src/main/flight/mixer.c | 9 --------- src/main/flight/mixer.h | 1 - src/main/flight/pid.c | 9 ++------- src/main/interface/settings.c | 2 +- src/test/unit/pid_unittest.cc | 32 +++++++++++++++++++++++++++++--- 5 files changed, 32 insertions(+), 21 deletions(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index a84422cc3..2bb28aa05 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -361,15 +361,6 @@ bool mixerIsTricopter(void) #endif } -bool mixerIsOutputSaturated(int axis, float errorRate) -{ - if (axis == FD_YAW && mixerIsTricopter()) { - return mixerTricopterIsServoSaturated(errorRate); - } - - return motorMixRange >= 1.0f; -} - // All PWM motor scaling is done to standard PWM range of 1000-2000 for easier tick conversion with legacy code / configurator // DSHOT scaling is done to the actual dshot range void initEscEndpoints(void) diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 1883881ac..e235a9d6b 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -117,7 +117,6 @@ struct rxConfig_s; uint8_t getMotorCount(void); float getMotorMixRange(void); bool areMotorsRunning(void); -bool mixerIsOutputSaturated(int axis, float errorRate); void mixerLoadMix(int index, motorMixer_t *customMixers); void mixerInit(mixerMode_e mixerMode); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 67d398fee..e4b6b0d48 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -445,7 +445,7 @@ void pidInitConfig(const pidProfile_t *pidProfile) horizonFactorRatio = (100 - pidProfile->horizon_tilt_effect) * 0.01f; maxVelocity[FD_ROLL] = maxVelocity[FD_PITCH] = pidProfile->rateAccelLimit * 100 * dT; maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 100 * dT; - const float ITermWindupPoint = (float)pidProfile->itermWindupPointPercent / 100.0f; + const float ITermWindupPoint = ((float)pidProfile->itermWindupPointPercent - 0.001f) / 100.0f; ITermWindupPointInv = 1.0f / (1.0f - ITermWindupPoint); itermAcceleratorGain = pidProfile->itermAcceleratorGain; crashTimeLimitUs = pidProfile->crash_time * 1000; @@ -985,12 +985,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT } // -----calculate I component - const float ITermNew = constrainf(ITerm + pidCoefficient[axis].Ki * itermErrorRate * dynCi, -itermLimit, itermLimit); - const bool outputSaturated = mixerIsOutputSaturated(axis, errorRate); - if (outputSaturated == false || ABS(ITermNew) < ABS(ITerm)) { - // Only increase ITerm if output is not saturated - pidData[axis].I = ITermNew; - } + pidData[axis].I = constrainf(ITerm + pidCoefficient[axis].Ki * itermErrorRate * dynCi, -itermLimit, itermLimit); // -----calculate D component if (pidCoefficient[axis].Kd > 0) { diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 49eb1b356..e5cf7e90a 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -846,7 +846,7 @@ const clivalue_t valueTable[] = { { "iterm_relax_type", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ITERM_RELAX_TYPE }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax_type) }, { "iterm_relax_cutoff", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 1, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax_cutoff) }, #endif - { "iterm_windup", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 30, 99 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermWindupPointPercent) }, + { "iterm_windup", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 30, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermWindupPointPercent) }, { "iterm_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermLimit) }, { "pidsum_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { PIDSUM_LIMIT_MIN, PIDSUM_LIMIT_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, pidSumLimit) }, { "pidsum_limit_yaw", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { PIDSUM_LIMIT_MIN, PIDSUM_LIMIT_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, pidSumLimitYaw) }, diff --git a/src/test/unit/pid_unittest.cc b/src/test/unit/pid_unittest.cc index 1820ef5e2..a18421e34 100644 --- a/src/test/unit/pid_unittest.cc +++ b/src/test/unit/pid_unittest.cc @@ -67,7 +67,6 @@ extern "C" { float getThrottlePIDAttenuation(void) { return simulatedThrottlePIDAttenuation; } float getMotorMixRange(void) { return simulatedMotorMixRange; } float getSetpointRate(int axis) { return simulatedSetpointRate[axis]; } - bool mixerIsOutputSaturated(int, float) { return simulateMixerSaturated; } float getRcDeflectionAbs(int axis) { return ABS(simulatedRcDeflection[axis]); } void systemBeep(bool) { } bool gyroOverflowDetected(void) { return false; } @@ -431,14 +430,41 @@ TEST(pidControllerTest, testMixerSaturation) { // Test full stick response setStickPosition(FD_ROLL, 1.0f); setStickPosition(FD_PITCH, -1.0f); - simulateMixerSaturated = true; + setStickPosition(FD_YAW, 1.0f); + simulatedMotorMixRange = 2.0f; pidController(pidProfile, &rollAndPitchTrims, currentTestTime()); // Expect no iterm accumulation - EXPECT_FLOAT_EQ(0, pidData[FD_YAW].P); 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 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, &rollAndPitchTrims, 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 + 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 + 1 / 100.0f); + pidController(pidProfile, &rollAndPitchTrims, currentTestTime()); + ASSERT_NE(pidData[FD_ROLL].I, rollTestIterm); + ASSERT_NE(pidData[FD_PITCH].I, pitchTestIterm); + ASSERT_NE(pidData[FD_YAW].I, yawTestIterm); } // TODO - Add more scenarios