Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 06:15:16 +03:00
Code Issues Wiki Activity

Merge pull request #6659 from betaflight/pid_unittest_fix

Browse source 
Feedforward unittest
This commit is contained in:
borisbstyle 2018-08-29 09:10:45 +02:00 • committed by GitHub
commit 9d01a5681e
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
1 changed files with 42 additions and 3 deletions
Download patch file Download diff file Expand all files Collapse all files

45
src/test/unit/pid_unittest.cc
View file

@ -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;
@ -266,7 +267,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,8 +459,46 @@ TEST(pidControllerTest, testCrashRecoveryMode) {
// Add additional verifications
}
TEST(pidControllerTest, pidSetpointTransition) {
// TODO
TEST(pidControllerTest, testFeedForward) {
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) {