From 5b7209329b78109681b2d14967b633d6647be1e6 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 28 Aug 2018 08:44:06 +0200 Subject: [PATCH 1/2] Add tolerance for feedforward --- src/test/unit/pid_unittest.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/test/unit/pid_unittest.cc b/src/test/unit/pid_unittest.cc index 0de0172edd..e687451834 100644 --- a/src/test/unit/pid_unittest.cc +++ b/src/test/unit/pid_unittest.cc @@ -266,7 +266,7 @@ TEST(pidControllerTest, testPidLoop) { ASSERT_NEAR(-8.7, pidData[FD_YAW].I, calculateTolerance(-8.7)); EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].D); EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].D); - EXPECT_FLOAT_EQ(-132.25, pidData[FD_YAW].D); + ASSERT_NEAR(-132.25, pidData[FD_YAW].D, calculateTolerance(-132.25)); // Match the stick to gyro to stop error simulatedSetpointRate[FD_ROLL] = 100; @@ -458,7 +458,7 @@ TEST(pidControllerTest, testCrashRecoveryMode) { // Add additional verifications } -TEST(pidControllerTest, pidSetpointTransition) { +TEST(pidControllerTest, testFeedForward) { // TODO } From a85eecf42d667405029f4627d2e5427c42164d2d Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 28 Aug 2018 10:06:48 +0200 Subject: [PATCH 2/2] Add feedforward tests --- src/test/unit/pid_unittest.cc | 41 ++++++++++++++++++++++++++++++++++- 1 file changed, 40 insertions(+), 1 deletion(-) diff --git a/src/test/unit/pid_unittest.cc b/src/test/unit/pid_unittest.cc index e687451834..f0c353dfee 100644 --- a/src/test/unit/pid_unittest.cc +++ b/src/test/unit/pid_unittest.cc @@ -144,6 +144,7 @@ void resetTest(void) { pidData[axis].P = 0; pidData[axis].I = 0; pidData[axis].D = 0; + pidData[axis].F = 0; pidData[axis].Sum = 0; simulatedSetpointRate[axis] = 0; simulatedRcDeflection[axis] = 0; @@ -459,7 +460,45 @@ TEST(pidControllerTest, testCrashRecoveryMode) { } TEST(pidControllerTest, testFeedForward) { -// TODO + resetTest(); + ENABLE_ARMING_FLAG(ARMED); + pidStabilisationState(PID_STABILISATION_ON); + + EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].F); + EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].F); + EXPECT_FLOAT_EQ(0, pidData[FD_YAW].F); + + // Match the stick to gyro to stop error + setStickPosition(FD_ROLL, 1.0f); + setStickPosition(FD_PITCH, -1.0f); + setStickPosition(FD_YAW, -1.0f); + + pidController(pidProfile, &rollAndPitchTrims, currentTestTime()); + + ASSERT_NEAR(2232.78, pidData[FD_ROLL].F, calculateTolerance(2232.78)); + ASSERT_NEAR(-2061.03, pidData[FD_PITCH].F, calculateTolerance(-2061.03)); + ASSERT_NEAR(-82.52, pidData[FD_YAW].F, calculateTolerance(-82.5)); + + // Match the stick to gyro to stop error + setStickPosition(FD_ROLL, 0.5f); + setStickPosition(FD_PITCH, -0.5f); + setStickPosition(FD_YAW, -0.5f); + + pidController(pidProfile, &rollAndPitchTrims, currentTestTime()); + + ASSERT_NEAR(-558.20, pidData[FD_ROLL].F, calculateTolerance(-558.20)); + ASSERT_NEAR(515.26, pidData[FD_PITCH].F, calculateTolerance(515.26)); + ASSERT_NEAR(-41.26, pidData[FD_YAW].F, calculateTolerance(-41.26)); + + for (int loop =0; loop <= 15; loop++) { + gyro.gyroADCf[FD_ROLL] += gyro.gyroADCf[FD_ROLL]; + pidController(pidProfile, &rollAndPitchTrims, currentTestTime()); + } + + EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].F); + EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].F); + EXPECT_FLOAT_EQ(0, pidData[FD_YAW].F); + } TEST(pidControllerTest, testDtermFiltering) {