diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index ebd93279af..65ee102551 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -389,7 +389,7 @@ static FAST_RAM_ZERO_INIT bool itermRotation; static FAST_RAM_ZERO_INIT bool smartFeedforward; #endif #if defined(USE_ABSOLUTE_CONTROL) -static FAST_RAM_ZERO_INIT float axisError[XYZ_AXIS_COUNT]; +STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT float axisError[XYZ_AXIS_COUNT]; static FAST_RAM_ZERO_INIT float acGain; static FAST_RAM_ZERO_INIT float acLimit; static FAST_RAM_ZERO_INIT float acErrorLimit; @@ -522,7 +522,7 @@ void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex) } // calculates strength of horizon leveling; 0 = none, 1.0 = most leveling -static float calcHorizonLevelStrength(void) +STATIC_UNIT_TESTED float calcHorizonLevelStrength(void) { // start with 1.0 at center stick, 0.0 at max stick deflection: float horizonLevelStrength = 1.0f - MAX(getRcDeflectionAbs(FD_ROLL), getRcDeflectionAbs(FD_PITCH)); @@ -577,7 +577,7 @@ static float calcHorizonLevelStrength(void) return constrainf(horizonLevelStrength, 0, 1); } -static float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint) { +STATIC_UNIT_TESTED float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint) { // calculate error angle and limit the angle to the max inclination // rcDeflection is in range [-1.0, 1.0] float angle = pidProfile->levelAngleLimit * getRcDeflection(axis); @@ -682,7 +682,7 @@ static void detectAndSetCrashRecovery( } } -static void rotateVector(float v[XYZ_AXIS_COUNT], float rotation[XYZ_AXIS_COUNT]) +static void rotateVector(float v[XYZ_AXIS_COUNT], float rotation[XYZ_AXIS_COUNT]) { // rotate v around rotation vector rotation // rotation in radians, all elements must be small @@ -695,7 +695,7 @@ static void rotateVector(float v[XYZ_AXIS_COUNT], float rotation[XYZ_AXIS_COUNT] } } -static void rotateItermAndAxisError() +STATIC_UNIT_TESTED void rotateItermAndAxisError() { if (itermRotation #if defined(USE_ABSOLUTE_CONTROL) @@ -839,7 +839,7 @@ 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, +STATIC_UNIT_TESTED void applyAbsoluteControl(const int axis, const float gyroRate, const bool itermRelaxIsEnabled, const float setpointLpf, const float setpointHpf, float *currentPidSetpoint, float *itermErrorRate) { @@ -881,7 +881,7 @@ static void applyAbsoluteControl(const int axis, const float gyroRate, const boo #endif #if defined(USE_ITERM_RELAX) -static void applyItermRelax(const int axis, const float iterm, +STATIC_UNIT_TESTED void applyItermRelax(const int axis, const float iterm, const float gyroRate, float *itermErrorRate, float *currentPidSetpoint) { const float setpointLpf = pt1FilterApply(&windupLpf[axis], *currentPidSetpoint); diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 365f177649..581ab7d869 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -23,6 +23,7 @@ #include #include "common/time.h" #include "common/filter.h" +#include "common/axis.h" #include "pg/pg.h" #define MAX_PID_PROCESS_DENOM 16 @@ -198,3 +199,17 @@ bool pidOsdAntiGravityActive(void); bool pidOsdAntiGravityMode(void); void pidSetAntiGravityState(bool newState); bool pidAntiGravityEnabled(void); + +#ifdef UNIT_TEST +#include "sensors/acceleration.h" +extern float axisError[XYZ_AXIS_COUNT]; +void applyItermRelax(const int axis, const float iterm, + const float gyroRate, float *itermErrorRate, float *currentPidSetpoint); +void applyAbsoluteControl(const int axis, const float gyroRate, const bool itermRelaxIsEnabled, + const float setpointLpf, const float setpointHpf, + float *currentPidSetpoint, float *itermErrorRate); +void rotateItermAndAxisError(); +float pidLevel(int axis, const pidProfile_t *pidProfile, + const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint); +float calcHorizonLevelStrength(void); +#endif diff --git a/src/test/unit/pid_unittest.cc b/src/test/unit/pid_unittest.cc index 6cc9297b11..6804ec663f 100644 --- a/src/test/unit/pid_unittest.cc +++ b/src/test/unit/pid_unittest.cc @@ -324,53 +324,42 @@ TEST(pidControllerTest, testPidLevel) { // Test Angle mode response enableFlightMode(ANGLE_MODE); - pidController(pidProfile, &rollAndPitchTrims, currentTestTime()); + float currentPidSetpoint = 30; + rollAndPitchTrims_t angleTrim = { { 0, 0 } }; - // Loop 1 - 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); + currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint); + EXPECT_FLOAT_EQ(0, currentPidSetpoint); + currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint); + EXPECT_FLOAT_EQ(0, currentPidSetpoint); // Test attitude response setStickPosition(FD_ROLL, 1.0f); setStickPosition(FD_PITCH, -1.0f); - attitude.values.roll = 550; - attitude.values.pitch = -550; - pidController(pidProfile, &rollAndPitchTrims, currentTestTime()); - pidController(pidProfile, &rollAndPitchTrims, currentTestTime()); - pidController(pidProfile, &rollAndPitchTrims, currentTestTime()); + currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint); + EXPECT_FLOAT_EQ(275, currentPidSetpoint); + currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint); + EXPECT_FLOAT_EQ(-275, currentPidSetpoint); - // Loop 2 - 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); + setStickPosition(FD_ROLL, -0.5f); + setStickPosition(FD_PITCH, 0.5f); + currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint); + EXPECT_FLOAT_EQ(-137.5, currentPidSetpoint); + currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint); + EXPECT_FLOAT_EQ(137.5, currentPidSetpoint); - // Disable ANGLE_MODE on full stick inputs + attitude.values.roll = -275; + attitude.values.pitch = 275; + currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint); + EXPECT_FLOAT_EQ(0, currentPidSetpoint); + currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint); + EXPECT_FLOAT_EQ(0, currentPidSetpoint); + + // Disable ANGLE_MODE disableFlightMode(ANGLE_MODE); - pidController(pidProfile, &rollAndPitchTrims, currentTestTime()); - - // Expect full rate output - ASSERT_NEAR(2559.8, pidData[FD_ROLL].P, calculateTolerance(2559.8)); - ASSERT_NEAR(-3711.6, pidData[FD_PITCH].P, calculateTolerance(-3711.6)); - EXPECT_FLOAT_EQ(0, pidData[FD_YAW].P); - ASSERT_NEAR(150, pidData[FD_ROLL].I, calculateTolerance(150)); - ASSERT_NEAR(-150, pidData[FD_PITCH].I, calculateTolerance(-150)); - 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); + currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint); + EXPECT_FLOAT_EQ(0, currentPidSetpoint); + currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint); + EXPECT_FLOAT_EQ(0, currentPidSetpoint); } @@ -380,51 +369,21 @@ TEST(pidControllerTest, testPidHorizon) { pidStabilisationState(PID_STABILISATION_ON); enableFlightMode(HORIZON_MODE); - // Loop 1 - 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); - // Test full stick response setStickPosition(FD_ROLL, 1.0f); setStickPosition(FD_PITCH, -1.0f); - attitude.values.roll = 550; - attitude.values.pitch = -550; - pidController(pidProfile, &rollAndPitchTrims, currentTestTime()); + EXPECT_FLOAT_EQ(0, calcHorizonLevelStrength()); // Expect full rate output on full stick - ASSERT_NEAR(2559.8, pidData[FD_ROLL].P, calculateTolerance(2559.8)); - ASSERT_NEAR(-3711.6, pidData[FD_PITCH].P, calculateTolerance(-3711.6)); - EXPECT_FLOAT_EQ(0, pidData[FD_YAW].P); - ASSERT_NEAR(150, pidData[FD_ROLL].I, calculateTolerance(150)); - ASSERT_NEAR(-150, pidData[FD_PITCH].I, calculateTolerance(-150)); - 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); + // Test zero stick response + setStickPosition(FD_ROLL, 0); + setStickPosition(FD_PITCH, 0); + EXPECT_FLOAT_EQ(1, calcHorizonLevelStrength()); - // Test full stick response + // Test small stick response setStickPosition(FD_ROLL, 0.1f); setStickPosition(FD_PITCH, -0.1f); - attitude.values.roll = 536; - attitude.values.pitch = -536; - pidController(pidProfile, &rollAndPitchTrims, currentTestTime()); - - ASSERT_NEAR(0.75, pidData[FD_ROLL].P, calculateTolerance(0.75)); - ASSERT_NEAR(-1.09, pidData[FD_PITCH].P, calculateTolerance(-1.09)); - EXPECT_FLOAT_EQ(0, pidData[FD_YAW].P); - ASSERT_NEAR(150, pidData[FD_ROLL].I, calculateTolerance(150)); - ASSERT_NEAR(-150, pidData[FD_PITCH].I, calculateTolerance(-150)); - 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); + ASSERT_NEAR(0.82, calcHorizonLevelStrength(), calculateTolerance(0.82)); } TEST(pidControllerTest, testMixerSaturation) { @@ -541,56 +500,75 @@ TEST(pidControllerTest, testFeedForward) { TEST(pidControllerTest, testItermRelax) { resetTest(); - pidProfile->iterm_relax = ITERM_RELAX_RPY; - pidInit(pidProfile); + pidProfile->iterm_relax = ITERM_RELAX_RP; 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, + pidProfile->iterm_relax_type = ITERM_RELAX_SETPOINT; 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)); + float itermErrorRate = 0; + float currentPidSetpoint = 0; + float gyroRate = 0; + + applyItermRelax(FD_PITCH, 0, gyroRate, &itermErrorRate, ¤tPidSetpoint); + EXPECT_FLOAT_EQ(itermErrorRate, 0); + itermErrorRate = -10; + currentPidSetpoint = 10; + pidData[FD_PITCH].I = 10; + + applyItermRelax(FD_PITCH, pidData[FD_PITCH].I, gyroRate, &itermErrorRate, ¤tPidSetpoint); + + ASSERT_NEAR(-6.66, itermErrorRate, calculateTolerance(-6.66)); + currentPidSetpoint += ITERM_RELAX_SETPOINT_THRESHOLD; + applyItermRelax(FD_PITCH, pidData[FD_PITCH].I, gyroRate, &itermErrorRate, ¤tPidSetpoint); + EXPECT_FLOAT_EQ(itermErrorRate, 0); + applyItermRelax(FD_PITCH, pidData[FD_PITCH].I, gyroRate, &itermErrorRate, ¤tPidSetpoint); + EXPECT_FLOAT_EQ(itermErrorRate, 0); + + pidProfile->iterm_relax_type = ITERM_RELAX_GYRO; + pidInit(pidProfile); + + currentPidSetpoint = 100; + applyItermRelax(FD_PITCH, pidData[FD_PITCH].I, gyroRate, &itermErrorRate, ¤tPidSetpoint); + EXPECT_FLOAT_EQ(itermErrorRate, 0); + gyroRate = 10; + itermErrorRate = -10; + applyItermRelax(FD_PITCH, pidData[FD_PITCH].I, gyroRate, &itermErrorRate, ¤tPidSetpoint); + ASSERT_NEAR(7, itermErrorRate, calculateTolerance(7)); + gyroRate += 100; + applyItermRelax(FD_PITCH, pidData[FD_PITCH].I, gyroRate, &itermErrorRate, ¤tPidSetpoint); + ASSERT_NEAR(-10, itermErrorRate, calculateTolerance(-10)); + + pidProfile->iterm_relax = ITERM_RELAX_RP_INC; + pidInit(pidProfile); + + itermErrorRate = -10; + pidData[FD_PITCH].I = 10; + currentPidSetpoint = 10; + applyItermRelax(FD_PITCH, pidData[FD_PITCH].I, gyroRate, &itermErrorRate, ¤tPidSetpoint); + EXPECT_FLOAT_EQ(itermErrorRate, -10); + itermErrorRate = 10; + pidData[FD_PITCH].I = -10; + applyItermRelax(FD_PITCH, pidData[FD_PITCH].I, gyroRate, &itermErrorRate, ¤tPidSetpoint); + EXPECT_FLOAT_EQ(itermErrorRate, 10); + itermErrorRate = -10; + currentPidSetpoint = 10; + applyItermRelax(FD_PITCH, pidData[FD_PITCH].I, gyroRate, &itermErrorRate, ¤tPidSetpoint); + EXPECT_FLOAT_EQ(itermErrorRate, -100); + + pidProfile->iterm_relax_type = ITERM_RELAX_SETPOINT; + pidInit(pidProfile); + + itermErrorRate = -10; + currentPidSetpoint = ITERM_RELAX_SETPOINT_THRESHOLD; + applyItermRelax(FD_YAW, pidData[FD_YAW].I, gyroRate, &itermErrorRate, ¤tPidSetpoint); + EXPECT_FLOAT_EQ(itermErrorRate, -10); + + 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)); } // TODO - Add more tests @@ -601,29 +579,32 @@ TEST(pidControllerTest, testAbsoluteControl) { 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()); + float gyroRate = 0; + bool itermRelaxIsEnabled = false; + float setpointLpf = 6; + float setpointHpf = 30; - // 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)); + float itermErrorRate = 10; + float currentPidSetpoint = 10; + + applyAbsoluteControl(FD_PITCH, gyroRate, itermRelaxIsEnabled, setpointLpf, setpointHpf, + ¤tPidSetpoint, &itermErrorRate); + + ASSERT_NEAR(10.8, itermErrorRate, calculateTolerance(10.8)); + ASSERT_NEAR(10.8, currentPidSetpoint, calculateTolerance(10.8)); + + itermRelaxIsEnabled = true; + applyAbsoluteControl(FD_PITCH, gyroRate, itermRelaxIsEnabled, setpointLpf, setpointHpf, + ¤tPidSetpoint, &itermErrorRate); + ASSERT_NEAR(10.8, itermErrorRate, calculateTolerance(10.8)); + ASSERT_NEAR(10.8, currentPidSetpoint, calculateTolerance(10.8)); + + gyroRate = -53; + axisError[FD_PITCH] = -60; + applyAbsoluteControl(FD_PITCH, gyroRate, itermRelaxIsEnabled, setpointLpf, setpointHpf, + ¤tPidSetpoint, &itermErrorRate); + ASSERT_NEAR(-79.2, itermErrorRate, calculateTolerance(-79.2)); + ASSERT_NEAR(-79.2, currentPidSetpoint, calculateTolerance(-79.2)); } TEST(pidControllerTest, testDtermFiltering) { @@ -631,5 +612,43 @@ TEST(pidControllerTest, testDtermFiltering) { } TEST(pidControllerTest, testItermRotationHandling) { -// TODO + resetTest(); + pidInit(pidProfile); + + rotateItermAndAxisError(); + EXPECT_FLOAT_EQ(pidData[FD_ROLL].I, 0); + EXPECT_FLOAT_EQ(pidData[FD_PITCH].I, 0); + EXPECT_FLOAT_EQ(pidData[FD_YAW].I, 0); + + pidProfile->iterm_rotation = true; + pidInit(pidProfile); + + rotateItermAndAxisError(); + EXPECT_FLOAT_EQ(pidData[FD_ROLL].I, 0); + EXPECT_FLOAT_EQ(pidData[FD_PITCH].I, 0); + EXPECT_FLOAT_EQ(pidData[FD_YAW].I, 0); + + pidData[FD_ROLL].I = 10; + pidData[FD_PITCH].I = 1000; + pidData[FD_YAW].I = 1000; + gyro.gyroADCf[FD_ROLL] = -1000; + rotateItermAndAxisError(); + EXPECT_FLOAT_EQ(pidData[FD_ROLL].I, 10); + ASSERT_NEAR(860.37, pidData[FD_PITCH].I, calculateTolerance(860.37)); + ASSERT_NEAR(1139.6, pidData[FD_YAW].I, calculateTolerance(1139.6)); + + pidProfile->abs_control_gain = 10; + pidInit(pidProfile); + pidData[FD_ROLL].I = 10; + pidData[FD_PITCH].I = 1000; + pidData[FD_YAW].I = 1000; + + gyro.gyroADCf[FD_ROLL] = -1000; + // FIXME - axisError changes don't affect the system. This is a potential bug or intendend behaviour? + axisError[FD_PITCH] = 1000; + axisError[FD_YAW] = 1000; + rotateItermAndAxisError(); + EXPECT_FLOAT_EQ(pidData[FD_ROLL].I, 10); + ASSERT_NEAR(860.37, pidData[FD_PITCH].I, calculateTolerance(860.37)); + ASSERT_NEAR(1139.6, pidData[FD_YAW].I, calculateTolerance(1139.6)); }