diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 707fae69ee..a6bd62e275 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -56,8 +56,6 @@ #include "sensors/acceleration.h" -#define ITERM_RELAX_SETPOINT_THRESHOLD 30.0f - const char pidNames[] = "ROLL;" "PITCH;" @@ -842,6 +840,89 @@ void FAST_CODE applySmartFeedforward(int axis) } #endif // USE_SMART_FEEDFORWARD +#if defined(USE_ABSOLUTE_CONTROL) +static void applyAbsoluteControl(const int axis, const float gyroRate, const bool itermRelaxIsEnabled, + const float setpointLpf, const float setpointHpf, + float *currentPidSetpoint, float *itermErrorRate) +{ + if (acGain > 0) { + float acErrorRate = 0; + if (itermRelaxIsEnabled) { + const float gmaxac = setpointLpf + 2 * setpointHpf; + const float gminac = setpointLpf - 2 * setpointHpf; + if (gyroRate >= gminac && gyroRate <= gmaxac) { + const float acErrorRate1 = gmaxac - gyroRate; + const float acErrorRate2 = gminac - gyroRate; + if (acErrorRate1 * axisError[axis] < 0) { + acErrorRate = acErrorRate1; + } else { + acErrorRate = acErrorRate2; + } + if (fabsf(acErrorRate * dT) > fabsf(axisError[axis]) ) { + acErrorRate = -axisError[axis] * pidFrequency; + } + } else { + acErrorRate = (gyroRate > gmaxac ? gmaxac : gminac ) - gyroRate; + } + } else { + acErrorRate = *itermErrorRate; + } + + if (isAirmodeActivated()) { + axisError[axis] = constrainf(axisError[axis] + acErrorRate * dT, + -acErrorLimit, acErrorLimit); + const float acCorrection = constrainf(axisError[axis] * acGain, -acLimit, acLimit); + *currentPidSetpoint += acCorrection; + *itermErrorRate += acCorrection; + if (axis == FD_ROLL) { + DEBUG_SET(DEBUG_ITERM_RELAX, 3, lrintf(axisError[axis] * 10)); + } + } + } +} +#endif + +#if defined(USE_ITERM_RELAX) +static void applyItermRelax(const int axis, const float ITerm, + const float gyroRate, float *itermErrorRate, float *currentPidSetpoint) +{ + const float setpointLpf = pt1FilterApply(&windupLpf[axis], *currentPidSetpoint); + const float setpointHpf = fabsf(*currentPidSetpoint - setpointLpf); + bool itermRelaxIsEnabled = false; + + if (itermRelax && + (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY || + itermRelax == ITERM_RELAX_RPY_INC)) { + itermRelaxIsEnabled = true; + const float itermRelaxFactor = 1 - setpointHpf / ITERM_RELAX_SETPOINT_THRESHOLD; + + 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) { + *itermErrorRate *= itermRelaxFactor; + } else if (itermRelaxType == ITERM_RELAX_GYRO ) { + *itermErrorRate = fapplyDeadband(setpointLpf - gyroRate, setpointHpf); + } else { + *itermErrorRate = 0.0f; + } + + if (axis == FD_ROLL) { + DEBUG_SET(DEBUG_ITERM_RELAX, 0, lrintf(setpointHpf)); + DEBUG_SET(DEBUG_ITERM_RELAX, 1, lrintf(itermRelaxFactor * 100.0f)); + DEBUG_SET(DEBUG_ITERM_RELAX, 2, lrintf(*itermErrorRate)); + } + } else { +#if defined(USE_ABSOLUTE_CONTROL) + applyAbsoluteControl(axis, gyroRate, setpointLpf, setpointHpf, itermRelaxIsEnabled, currentPidSetpoint, itermErrorRate); +#else + UNUSED(itermRelaxIsEnabled); +#endif + } +} +#endif + // Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage. // Based on 2DOF reference design (matlab) void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, timeUs_t currentTimeUs) @@ -912,73 +993,11 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT pidProfile->crash_recovery, angleTrim, axis, currentTimeUs, gyroRate, ¤tPidSetpoint, &errorRate); -#ifdef USE_ABSOLUTE_CONTROL - float acCorrection = 0; - float acErrorRate; -#endif - const float ITerm = pidData[axis].I; float itermErrorRate = errorRate; #if defined(USE_ITERM_RELAX) - if (itermRelax && (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY || itermRelax == ITERM_RELAX_RPY_INC)) { - const float setpointLpf = pt1FilterApply(&windupLpf[axis], currentPidSetpoint); - const float setpointHpf = fabsf(currentPidSetpoint - setpointLpf); - const float itermRelaxFactor = 1 - setpointHpf / ITERM_RELAX_SETPOINT_THRESHOLD; - - 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) { - itermErrorRate *= itermRelaxFactor; - } else if (itermRelaxType == ITERM_RELAX_GYRO ) { - itermErrorRate = fapplyDeadband(setpointLpf - gyroRate, setpointHpf); - } else { - itermErrorRate = 0.0f; - } - - if (axis == FD_ROLL) { - DEBUG_SET(DEBUG_ITERM_RELAX, 0, lrintf(setpointHpf)); - DEBUG_SET(DEBUG_ITERM_RELAX, 1, lrintf(itermRelaxFactor * 100.0f)); - DEBUG_SET(DEBUG_ITERM_RELAX, 2, lrintf(itermErrorRate)); - } - -#if defined(USE_ABSOLUTE_CONTROL) - const float gmaxac = setpointLpf + 2 * setpointHpf; - const float gminac = setpointLpf - 2 * setpointHpf; - if (gyroRate >= gminac && gyroRate <= gmaxac) { - float acErrorRate1 = gmaxac - gyroRate; - float acErrorRate2 = gminac - gyroRate; - if (acErrorRate1 * axisError[axis] < 0) { - acErrorRate = acErrorRate1; - } else { - acErrorRate = acErrorRate2; - } - if (fabsf(acErrorRate * dT) > fabsf(axisError[axis]) ) { - acErrorRate = -axisError[axis] / dT; - } - } else { - acErrorRate = (gyroRate > gmaxac ? gmaxac : gminac ) - gyroRate; - } -#endif // USE_ABSOLUTE_CONTROL - } else -#endif // USE_ITERM_RELAX - { -#if defined(USE_ABSOLUTE_CONTROL) - acErrorRate = itermErrorRate; -#endif // USE_ABSOLUTE_CONTROL - } - -#if defined(USE_ABSOLUTE_CONTROL) - if (acGain > 0 && isAirmodeActivated()) { - axisError[axis] = constrainf(axisError[axis] + acErrorRate * dT, -acErrorLimit, acErrorLimit); - acCorrection = constrainf(axisError[axis] * acGain, -acLimit, acLimit); - currentPidSetpoint += acCorrection; - itermErrorRate += acCorrection; - if (axis == FD_ROLL) { - DEBUG_SET(DEBUG_ITERM_RELAX, 3, lrintf(axisError[axis] * 10)); - } - } + applyItermRelax(axis, ITerm, gyroRate, &itermErrorRate, ¤tPidSetpoint); #endif // --------low-level gyro-based PID based on 2DOF PID controller. ---------- diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index fb3444d839..9493ce41d2 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -43,6 +43,8 @@ // This value gives the same "feel" as the previous Kd default of 26 (26 * DTERM_SCALE) #define FEEDFORWARD_SCALE 0.013754f +#define ITERM_RELAX_SETPOINT_THRESHOLD 30.0f + typedef enum { PID_ROLL, PID_PITCH, diff --git a/src/test/Makefile b/src/test/Makefile index 565b05887b..2e270dc0e1 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -300,6 +300,11 @@ pid_unittest_SRC := \ $(USER_DIR)/pg/pg.c \ $(USER_DIR)/fc/runtime_config.c +pid_unittest_DEFINES := \ + USE_ITERM_RELAX \ + USE_RC_SMOOTHING_FILTER \ + USE_ABSOLUTE_CONTROL + rcdevice_unittest_DEFINES := \ USE_RCDEVICE diff --git a/src/test/unit/pid_unittest.cc b/src/test/unit/pid_unittest.cc index ccb27fc47e..6cc9297b11 100644 --- a/src/test/unit/pid_unittest.cc +++ b/src/test/unit/pid_unittest.cc @@ -24,7 +24,7 @@ #include "gtest/gtest.h" #include "build/debug.h" -bool simulateMixerSaturated = false; +bool simulatedAirmodeEnabled = true; float simulatedSetpointRate[3] = { 0,0,0 }; float simulatedRcDeflection[3] = { 0,0,0 }; float simulatedThrottlePIDAttenuation = 1.0f; @@ -67,6 +67,7 @@ extern "C" { float getThrottlePIDAttenuation(void) { return simulatedThrottlePIDAttenuation; } float getMotorMixRange(void) { return simulatedMotorMixRange; } float getSetpointRate(int axis) { return simulatedSetpointRate[axis]; } + bool isAirmodeActivated() { return simulatedAirmodeEnabled; } float getRcDeflectionAbs(int axis) { return ABS(simulatedRcDeflection[axis]); } void systemBeep(bool) { } bool gyroOverflowDetected(void) { return false; } @@ -121,6 +122,11 @@ void setDefaultTestSettings(void) { pidProfile->throttle_boost = 0; pidProfile->throttle_boost_cutoff = 15; pidProfile->iterm_rotation = false; + pidProfile->smart_feedforward = false, + pidProfile->iterm_relax = ITERM_RELAX_OFF, + pidProfile->iterm_relax_cutoff = 11, + pidProfile->iterm_relax_type = ITERM_RELAX_SETPOINT, + pidProfile->abs_control_gain = 0, gyro.targetLooptime = 4000; } @@ -131,7 +137,6 @@ timeUs_t currentTestTime(void) { void resetTest(void) { loopIter = 0; - simulateMixerSaturated = false; simulatedThrottlePIDAttenuation = 1.0f; simulatedMotorMixRange = 0.0f; @@ -534,6 +539,93 @@ TEST(pidControllerTest, testFeedForward) { } +TEST(pidControllerTest, testItermRelax) { + resetTest(); + pidProfile->iterm_relax = ITERM_RELAX_RPY; + pidInit(pidProfile); + ENABLE_ARMING_FLAG(ARMED); + pidStabilisationState(PID_STABILISATION_ON); + + pidController(pidProfile, &rollAndPitchTrims, currentTestTime()); + + // Loop 1 - Expecting zero since there is no error + EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].P); + EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].P); + 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); + EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].D); + EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].D); + EXPECT_FLOAT_EQ(0, pidData[FD_YAW].D); + + simulatedSetpointRate[FD_ROLL] = 10; + simulatedSetpointRate[FD_PITCH] = -10; + simulatedSetpointRate[FD_YAW] = 10; + pidController(pidProfile, &rollAndPitchTrims, currentTestTime()); + ASSERT_NEAR(0.52f, pidData[FD_ROLL].I, calculateTolerance(0.52f)); + ASSERT_NEAR(-0.65f, pidData[FD_PITCH].I, calculateTolerance(-0.65f)); + ASSERT_NEAR(0.59f, pidData[FD_YAW].I, calculateTolerance(0.59f)); + + // Should stay same when ITERM_RELAX_SETPOINT_THRESHOLD reached + simulatedSetpointRate[FD_ROLL] = ITERM_RELAX_SETPOINT_THRESHOLD; + simulatedSetpointRate[FD_PITCH] = -ITERM_RELAX_SETPOINT_THRESHOLD; + simulatedSetpointRate[FD_YAW] = ITERM_RELAX_SETPOINT_THRESHOLD; + pidController(pidProfile, &rollAndPitchTrims, currentTestTime()); + ASSERT_NEAR(0.52f, pidData[FD_ROLL].I, calculateTolerance(0.52f)); + ASSERT_NEAR(-0.65f, pidData[FD_PITCH].I, calculateTolerance(-0.65f)); + ASSERT_NEAR(0.59f, pidData[FD_YAW].I, calculateTolerance(0.59f)); + + simulatedSetpointRate[FD_ROLL] = 20; + simulatedSetpointRate[FD_PITCH] = -20; + simulatedSetpointRate[FD_YAW] = 20; + + pidProfile->iterm_relax_type = ITERM_RELAX_GYRO, + pidInit(pidProfile); + pidController(pidProfile, &rollAndPitchTrims, currentTestTime()); + ASSERT_NEAR(0.52f, pidData[FD_ROLL].I, calculateTolerance(0.52f)); + ASSERT_NEAR(-0.65f, pidData[FD_PITCH].I, calculateTolerance(-0.65f)); + ASSERT_NEAR(0.59f, pidData[FD_YAW].I, calculateTolerance(0.59f)); + + pidController(pidProfile, &rollAndPitchTrims, currentTestTime()); + ASSERT_NEAR(0.79f, pidData[FD_ROLL].I, calculateTolerance(0.79f)); + ASSERT_NEAR(-0.98f, pidData[FD_PITCH].I, calculateTolerance(-0.98f)); + ASSERT_NEAR(0.88f, pidData[FD_YAW].I, calculateTolerance(0.88)); +} + +// TODO - Add more tests +TEST(pidControllerTest, testAbsoluteControl) { + resetTest(); + pidProfile->abs_control_gain = 10; + pidInit(pidProfile); + ENABLE_ARMING_FLAG(ARMED); + pidStabilisationState(PID_STABILISATION_ON); + + // Loop 1 - Expecting zero since there is no error + EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].P); + EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].P); + 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); + EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].D); + EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].D); + EXPECT_FLOAT_EQ(0, pidData[FD_YAW].D); + pidController(pidProfile, &rollAndPitchTrims, currentTestTime()); + + // Add some rotation on ROLL to generate error + simulatedSetpointRate[FD_ROLL] = 10; + simulatedSetpointRate[FD_PITCH] = -10; + simulatedSetpointRate[FD_YAW] = 10; + pidController(pidProfile, &rollAndPitchTrims, currentTestTime()); + ASSERT_NEAR(12.8, pidData[FD_ROLL].P, calculateTolerance(12.8)); + ASSERT_NEAR(-18.57, pidData[FD_PITCH].P, calculateTolerance(-18.57)); + ASSERT_NEAR(22.4, pidData[FD_YAW].P, calculateTolerance(22.4)); + ASSERT_NEAR(0.84, pidData[FD_ROLL].I, calculateTolerance(0.84)); + ASSERT_NEAR(-0.92, pidData[FD_PITCH].I, calculateTolerance(-0.92)); + ASSERT_NEAR(0.95, pidData[FD_YAW].I, calculateTolerance(-0.95)); +} + TEST(pidControllerTest, testDtermFiltering) { // TODO }