Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-14 11:59:58 +03:00
Code Issues Wiki Activity

Fix: Hyperbolic TPA curve initialization (for wings) (#13932)

Browse source
This commit is contained in:
Ivan Efimov 2024-09-27 13:24:34 -05:00 • committed by GitHub
parent 331801c7a2
commit 983b510184
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
5 changed files with 39 additions and 38 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1068,22 +1068,22 @@ TEST(pidControllerTest, testTpaClassic)
pidInit(pidProfile);
pidUpdateTpaFactor(0.0f, pidProfile);
pidUpdateTpaFactor(0.0f);
EXPECT_FLOAT_EQ(1.5f, pidRuntime.tpaFactor);
pidUpdateTpaFactor(0.1f, pidProfile);
pidUpdateTpaFactor(0.1f);
EXPECT_FLOAT_EQ(1.25f, pidRuntime.tpaFactor);
pidUpdateTpaFactor(0.2f, pidProfile);
pidUpdateTpaFactor(0.2f);
EXPECT_FLOAT_EQ(1.0f, pidRuntime.tpaFactor);
pidUpdateTpaFactor(0.6f, pidProfile);
pidUpdateTpaFactor(0.6f);
EXPECT_FLOAT_EQ(1.0f, pidRuntime.tpaFactor);
pidUpdateTpaFactor(0.8f, pidProfile);
pidUpdateTpaFactor(0.8f);
EXPECT_FLOAT_EQ(0.85f, pidRuntime.tpaFactor);
pidUpdateTpaFactor(1.0f, pidProfile);
pidUpdateTpaFactor(1.0f);
EXPECT_FLOAT_EQ(0.7f, pidRuntime.tpaFactor);
@ -1096,22 +1096,22 @@ TEST(pidControllerTest, testTpaClassic)
pidInit(pidProfile);
pidUpdateTpaFactor(0.0f, pidProfile);
pidUpdateTpaFactor(0.0f);
EXPECT_FLOAT_EQ(1.0f, pidRuntime.tpaFactor);
pidUpdateTpaFactor(0.1f, pidProfile);
pidUpdateTpaFactor(0.1f);
EXPECT_FLOAT_EQ(1.0f, pidRuntime.tpaFactor);
pidUpdateTpaFactor(0.2f, pidProfile);
pidUpdateTpaFactor(0.2f);
EXPECT_FLOAT_EQ(1.0f, pidRuntime.tpaFactor);
pidUpdateTpaFactor(0.6f, pidProfile);
pidUpdateTpaFactor(0.6f);
EXPECT_FLOAT_EQ(1.0f, pidRuntime.tpaFactor);
pidUpdateTpaFactor(0.8f, pidProfile);
pidUpdateTpaFactor(0.8f);
EXPECT_FLOAT_EQ(0.85f, pidRuntime.tpaFactor);
pidUpdateTpaFactor(1.0f, pidProfile);
pidUpdateTpaFactor(1.0f);
EXPECT_FLOAT_EQ(0.7f, pidRuntime.tpaFactor);
}
@ -1128,19 +1128,19 @@ TEST(pidControllerTest, testTpaHyperbolic)
pidInit(pidProfile);
pidUpdateTpaFactor(0.0f, pidProfile);
pidUpdateTpaFactor(0.0f);
EXPECT_FLOAT_EQ(5.0f, pidRuntime.tpaFactor);
pidUpdateTpaFactor(0.15f, pidProfile);
pidUpdateTpaFactor(0.15f);
EXPECT_FLOAT_EQ(5.0f, pidRuntime.tpaFactor);
pidUpdateTpaFactor(0.5, pidProfile);
pidUpdateTpaFactor(0.5);
EXPECT_NEAR(2.588f, pidRuntime.tpaFactor, 0.01f);
pidUpdateTpaFactor(0.9, pidProfile);
pidUpdateTpaFactor(0.9);
EXPECT_NEAR(0.693f, pidRuntime.tpaFactor, 0.01f);
pidUpdateTpaFactor(1.0, pidProfile);
pidUpdateTpaFactor(1.0);
EXPECT_NEAR(0.5f, pidRuntime.tpaFactor, 0.01f);
// linear curve
@ -1152,19 +1152,19 @@ TEST(pidControllerTest, testTpaHyperbolic)
pidInit(pidProfile);
pidUpdateTpaFactor(0.0f, pidProfile);
pidUpdateTpaFactor(0.0f);
EXPECT_FLOAT_EQ(3.0f, pidRuntime.tpaFactor);
pidUpdateTpaFactor(0.15f, pidProfile);
pidUpdateTpaFactor(0.15f);
EXPECT_NEAR(2.565f, pidRuntime.tpaFactor, 0.01f);
pidUpdateTpaFactor(0.5, pidProfile);
pidUpdateTpaFactor(0.5);
EXPECT_NEAR(1.550f, pidRuntime.tpaFactor, 0.01f);
pidUpdateTpaFactor(0.9, pidProfile);
pidUpdateTpaFactor(0.9);
EXPECT_NEAR(0.390f, pidRuntime.tpaFactor, 0.01f);
pidUpdateTpaFactor(1.0, pidProfile);
pidUpdateTpaFactor(1.0);
EXPECT_NEAR(0.1f, pidRuntime.tpaFactor, 0.01f);
// curve bends up
@ -1176,19 +1176,19 @@ TEST(pidControllerTest, testTpaHyperbolic)
pidInit(pidProfile);
pidUpdateTpaFactor(0.0f, pidProfile);
pidUpdateTpaFactor(0.0f);
EXPECT_FLOAT_EQ(10.0f, pidRuntime.tpaFactor);
pidUpdateTpaFactor(0.15f, pidProfile);
pidUpdateTpaFactor(0.15f);
EXPECT_NEAR(10.0f, pidRuntime.tpaFactor, 0.01f);
pidUpdateTpaFactor(0.5, pidProfile);
pidUpdateTpaFactor(0.5);
EXPECT_NEAR(9.700f, pidRuntime.tpaFactor, 0.01f);
pidUpdateTpaFactor(0.9, pidProfile);
pidUpdateTpaFactor(0.9);
EXPECT_NEAR(7.364f, pidRuntime.tpaFactor, 0.01f);
pidUpdateTpaFactor(1.0, pidProfile);
pidUpdateTpaFactor(1.0);
EXPECT_NEAR(0.625f, pidRuntime.tpaFactor, 0.01f);
// curve bends down
@ -1200,18 +1200,18 @@ TEST(pidControllerTest, testTpaHyperbolic)
pidInit(pidProfile);
pidUpdateTpaFactor(0.0f, pidProfile);
pidUpdateTpaFactor(0.0f);
EXPECT_FLOAT_EQ(2.5f, pidRuntime.tpaFactor);
pidUpdateTpaFactor(0.15f, pidProfile);
pidUpdateTpaFactor(0.15f);
EXPECT_NEAR(2.5f, pidRuntime.tpaFactor, 0.01f);
pidUpdateTpaFactor(0.5, pidProfile);
pidUpdateTpaFactor(0.5);
EXPECT_NEAR(2.5f, pidRuntime.tpaFactor, 0.01f);
pidUpdateTpaFactor(0.9, pidProfile);
pidUpdateTpaFactor(0.9);
EXPECT_NEAR(0.954f, pidRuntime.tpaFactor, 0.01f);
pidUpdateTpaFactor(1.0, pidProfile);
pidUpdateTpaFactor(1.0);
EXPECT_NEAR(0.9f, pidRuntime.tpaFactor, 0.01f);
}