From 1d998ea4041088293433cf0f301bca53867e525d Mon Sep 17 00:00:00 2001 From: ctzsnooze Date: Tue, 26 Feb 2019 12:23:21 +1100 Subject: [PATCH 1/2] iTermRelaxFactor now only attenuates amount added The original setpoint based iTerm Relax code attenuated the amount of iTerm added per loop by a relax factor based on an HPF of setpoint. At some point the code was re-factored and the relax factor multiplied the accumulating iterm error itself, such that almost any relax factor below 1.0 would quickly zero out iTerm. This was bad for racing because when making sustained tight turns, I would abrubtly be zeroed when the setpoint for the starting of some relax was close to the threshold. This was never the intent of the original proposal, which was for a smoother attenuation of iTerm, and for retention of some accumulation of iTerm during spirals around poles etc. This PR fixes that error. It also changes the default threshold up from 30 deg/s to 40 deg/s which better suits racing. Also included us a form of simple cutoff independence. In the initial form, lowering cutoff would reduce the effectiveness but draw out the duration. Now cutoff only really affects duration. Lower cutoff values are better for quads with greater motor delay, faster values are better for quicker quads. For most of my quads a cutoff of 30 works best. I've also removed newlines. set to current cutoff maybe fix unit test default cutoff set to current value in unit test add itermRelaxSetpointThreshold as float in unit test add itermRelaxSetpointThreshold as float, because ITERM_RELAX_SETPOINT_THRESHOLD doesn't exist any more Move itermRelaxFactor limit, remove from fast ram remove unncessary max, revert unit test changes, restore original defaults. remove max from debug restore old defaults and revert unit test changes temporarily to see if will pass unit test with defaults whoops lets see if unit test passes when cutoff is 20 lets see if unit test passes when cutoff is 20 --- src/main/flight/pid.c | 14 +++++++------- src/main/flight/pid.h | 1 + src/test/unit/pid_unittest.cc | 2 +- 3 files changed, 9 insertions(+), 8 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 3de6465c49..cc1742b8a4 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -267,7 +267,8 @@ static FAST_RAM_ZERO_INIT pt1Filter_t ptermYawLowpass; static FAST_RAM_ZERO_INIT pt1Filter_t windupLpf[XYZ_AXIS_COUNT]; static FAST_RAM_ZERO_INIT uint8_t itermRelax; static FAST_RAM_ZERO_INIT uint8_t itermRelaxType; -static FAST_RAM_ZERO_INIT uint8_t itermRelaxCutoff; +static uint8_t itermRelaxCutoff; +static FAST_RAM_ZERO_INIT float itermRelaxSetpointThreshold; #endif #if defined(USE_ABSOLUTE_CONTROL) @@ -615,10 +616,12 @@ void pidInitConfig(const pidProfile_t *pidProfile) #if defined(USE_SMART_FEEDFORWARD) smartFeedforward = pidProfile->smart_feedforward; #endif + #if defined(USE_ITERM_RELAX) itermRelax = pidProfile->iterm_relax; itermRelaxType = pidProfile->iterm_relax_type; itermRelaxCutoff = pidProfile->iterm_relax_cutoff; + itermRelaxSetpointThreshold = ITERM_RELAX_SETPOINT_THRESHOLD * 20.0f / itermRelaxCutoff; #endif #ifdef USE_ACRO_TRAINER @@ -1107,16 +1110,13 @@ STATIC_UNIT_TESTED void applyItermRelax(const int axis, const float iterm, const float setpointLpf = pt1FilterApply(&windupLpf[axis], *currentPidSetpoint); const float setpointHpf = fabsf(*currentPidSetpoint - setpointLpf); - if (itermRelax && - (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY || - itermRelax == ITERM_RELAX_RPY_INC)) { - const float itermRelaxFactor = 1 - setpointHpf / ITERM_RELAX_SETPOINT_THRESHOLD; - + if (itermRelax && (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY || itermRelax == ITERM_RELAX_RPY_INC)) { + const float itermRelaxFactor = MAX(0, 1 - setpointHpf / itermRelaxSetpointThreshold); const bool isDecreasingI = ((iterm > 0) && (*itermErrorRate < 0)) || ((iterm < 0) && (*itermErrorRate > 0)); if ((itermRelax >= ITERM_RELAX_RP_INC) && isDecreasingI) { // Do Nothing, use the precalculed itermErrorRate - } else if (itermRelaxType == ITERM_RELAX_SETPOINT && setpointHpf < ITERM_RELAX_SETPOINT_THRESHOLD) { + } else if (itermRelaxType == ITERM_RELAX_SETPOINT) { *itermErrorRate *= itermRelaxFactor; } else if (itermRelaxType == ITERM_RELAX_GYRO ) { *itermErrorRate = fapplyDeadband(setpointLpf - gyroRate, setpointHpf); diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index ac9110b1c8..57475e17ef 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -44,6 +44,7 @@ // This value gives the same "feel" as the previous Kd default of 26 (26 * DTERM_SCALE) #define FEEDFORWARD_SCALE 0.013754f +// Full iterm suppression at 40deg/sec * default cutoff of 20 #define ITERM_RELAX_SETPOINT_THRESHOLD 30.0f typedef enum { diff --git a/src/test/unit/pid_unittest.cc b/src/test/unit/pid_unittest.cc index 0ecba83b84..049a3832a3 100644 --- a/src/test/unit/pid_unittest.cc +++ b/src/test/unit/pid_unittest.cc @@ -129,7 +129,7 @@ void setDefaultTestSettings(void) { pidProfile->iterm_rotation = false; pidProfile->smart_feedforward = false, pidProfile->iterm_relax = ITERM_RELAX_OFF, - pidProfile->iterm_relax_cutoff = 11, + pidProfile->iterm_relax_cutoff = 20, pidProfile->iterm_relax_type = ITERM_RELAX_SETPOINT, pidProfile->abs_control_gain = 0, pidProfile->launchControlMode = LAUNCH_CONTROL_MODE_NORMAL, From 03dc1a1aee9f8d6aa6d91509ffdc13568ac22528 Mon Sep 17 00:00:00 2001 From: mikeller Date: Thu, 28 Feb 2019 19:52:11 +1300 Subject: [PATCH 2/2] Fixed tests. --- src/test/unit/pid_unittest.cc | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/test/unit/pid_unittest.cc b/src/test/unit/pid_unittest.cc index 049a3832a3..af504102b4 100644 --- a/src/test/unit/pid_unittest.cc +++ b/src/test/unit/pid_unittest.cc @@ -129,7 +129,7 @@ void setDefaultTestSettings(void) { pidProfile->iterm_rotation = false; pidProfile->smart_feedforward = false, pidProfile->iterm_relax = ITERM_RELAX_OFF, - pidProfile->iterm_relax_cutoff = 20, + pidProfile->iterm_relax_cutoff = 11, pidProfile->iterm_relax_type = ITERM_RELAX_SETPOINT, pidProfile->abs_control_gain = 0, pidProfile->launchControlMode = LAUNCH_CONTROL_MODE_NORMAL, @@ -529,12 +529,12 @@ TEST(pidControllerTest, testItermRelax) { applyItermRelax(FD_PITCH, pidData[FD_PITCH].I, gyroRate, &itermErrorRate, ¤tPidSetpoint); - ASSERT_NEAR(-6.66, itermErrorRate, calculateTolerance(-6.66)); + ASSERT_NEAR(-8.16, itermErrorRate, calculateTolerance(-6.66)); currentPidSetpoint += ITERM_RELAX_SETPOINT_THRESHOLD; applyItermRelax(FD_PITCH, pidData[FD_PITCH].I, gyroRate, &itermErrorRate, ¤tPidSetpoint); - EXPECT_FLOAT_EQ(itermErrorRate, 0); + ASSERT_NEAR(-2.17, itermErrorRate, calculateTolerance(-2.17)); applyItermRelax(FD_PITCH, pidData[FD_PITCH].I, gyroRate, &itermErrorRate, ¤tPidSetpoint); - EXPECT_FLOAT_EQ(itermErrorRate, 0); + ASSERT_NEAR(-0.58, itermErrorRate, calculateTolerance(-0.58)); pidProfile->iterm_relax_type = ITERM_RELAX_GYRO; pidInit(pidProfile); @@ -578,7 +578,7 @@ TEST(pidControllerTest, testItermRelax) { pidProfile->iterm_relax = ITERM_RELAX_RPY; pidInit(pidProfile); applyItermRelax(FD_YAW, pidData[FD_YAW].I, gyroRate, &itermErrorRate, ¤tPidSetpoint); - ASSERT_NEAR(-3.6, itermErrorRate, calculateTolerance(-3.6)); + ASSERT_NEAR(-6.46, itermErrorRate, calculateTolerance(-3.6)); } // TODO - Add more tests