From aa44fd6bbd8866c600a6fc373f9cc478c8ddc986 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Mon, 10 Sep 2018 12:31:43 +0200 Subject: [PATCH] Change default iterm_windup --- src/main/flight/pid.c | 17 ++++++++++++----- src/test/unit/pid_unittest.cc | 8 ++++---- 2 files changed, 16 insertions(+), 9 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index e4b6b0d48..707fae69e 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -132,7 +132,7 @@ void resetPidProfile(pidProfile_t *pidProfile) .dterm_notch_hz = 0, .dterm_notch_cutoff = 0, .dterm_filter_type = FILTER_PT1, - .itermWindupPointPercent = 40, + .itermWindupPointPercent = 100, .vbatPidCompensation = 0, .pidAtMinThrottle = PID_STABILISATION_ON, .levelAngleLimit = 55, @@ -445,8 +445,13 @@ 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 - 0.001f) / 100.0f; - ITermWindupPointInv = 1.0f / (1.0f - ITermWindupPoint); + ITermWindupPointInv = 0.0f; + if (pidProfile->itermWindupPointPercent < 100) { + float ITermWindupPoint = (float)pidProfile->itermWindupPointPercent / 100.0f; + ITermWindupPointInv = 1.0f / (1.0f - ITermWindupPoint); + } else { + ITermWindupPointInv = 0.0f; + } itermAcceleratorGain = pidProfile->itermAcceleratorGain; crashTimeLimitUs = pidProfile->crash_time * 1000; crashTimeDelayUs = pidProfile->crash_delay * 1000; @@ -859,8 +864,10 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT DEBUG_SET(DEBUG_ANTI_GRAVITY, 0, lrintf(itermAccelerator * 1000)); // gradually scale back integration when above windup point - const float dynCi = constrainf((1.0f - motorMixRange) * ITermWindupPointInv, 0.0f, 1.0f) - * dT * itermAccelerator; + float dynCi = dT * itermAccelerator; + if (ITermWindupPointInv > 0) { + dynCi *= constrainf((1.0f - motorMixRange) * ITermWindupPointInv, 0.0f, 1.0f); + } // 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 index a18421e34..ccb27fc47 100644 --- a/src/test/unit/pid_unittest.cc +++ b/src/test/unit/pid_unittest.cc @@ -460,11 +460,11 @@ TEST(pidControllerTest, testMixerSaturation) { setStickPosition(FD_ROLL, 0.1f); setStickPosition(FD_PITCH, -0.1f); setStickPosition(FD_YAW, 0.1f); - simulatedMotorMixRange = (pidProfile->itermWindupPointPercent + 1 / 100.0f); + 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); + ASSERT_LT(pidData[FD_ROLL].I, rollTestIterm); + ASSERT_GE(pidData[FD_PITCH].I, pitchTestIterm); + ASSERT_LT(pidData[FD_YAW].I, yawTestIterm); } // TODO - Add more scenarios