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

Hyperbolic PID multiplier curve (for wings) (#13805)

Browse source 
* TPA_CURVE_HYPERBOLIC (for wings)

* typo fix: tpa_rate_stall_throttle

* ledvinap's review

* fix tpa_curve_expo divider (10 instead of 100)

* Define fixes

* whoops, 16 instead of 1600 for PWL points

* if case for when dividing by zero for hyperbolic expo

* More ledvinap's review

* pow instead of pow_approx + unit tests for hyperbolic TPA

* basic unit tests for classic TPA

* pow was for double. pow_approx for floats is enough

* remove #else from comments

* pow_approx -> powf for hyperbolic TPA

* PWL: brigning back static assert after #13818 PR

* removed extra line per haslinghuis's review
This commit is contained in:
Ivan Efimov 2024-08-29 00:57:03 -05:00 • committed by GitHub
parent deef912d47
commit ac384cf34c
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
10 changed files with 317 additions and 26 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -958,3 +958,164 @@ TEST(pidControllerTest, testLaunchControl)
EXPECT_NEAR(44.84, pidData[FD_YAW].P, calculateTolerance(44.84));
EXPECT_NEAR(1.56, pidData[FD_YAW].I, calculateTolerance(1.56));
}
TEST(pidControllerTest, testTpaClassic)
{
resetTest();
pidProfile->tpa_curve_type = TPA_CURVE_CLASSIC;
pidProfile->tpa_rate = 30;
pidProfile->tpa_breakpoint = 1600;
pidProfile->tpa_low_rate = -50;
pidProfile->tpa_low_breakpoint = 1200;
pidProfile->tpa_low_always = 1;
pidInit(pidProfile);
pidUpdateTpaFactor(0.0f, pidProfile);
EXPECT_FLOAT_EQ(1.5f, pidRuntime.tpaFactor);
pidUpdateTpaFactor(0.1f, pidProfile);
EXPECT_FLOAT_EQ(1.25f, pidRuntime.tpaFactor);
pidUpdateTpaFactor(0.2f, pidProfile);
EXPECT_FLOAT_EQ(1.0f, pidRuntime.tpaFactor);
pidUpdateTpaFactor(0.6f, pidProfile);
EXPECT_FLOAT_EQ(1.0f, pidRuntime.tpaFactor);
pidUpdateTpaFactor(0.8f, pidProfile);
EXPECT_FLOAT_EQ(0.85f, pidRuntime.tpaFactor);
pidUpdateTpaFactor(1.0f, pidProfile);
EXPECT_FLOAT_EQ(0.7f, pidRuntime.tpaFactor);
pidProfile->tpa_curve_type = TPA_CURVE_CLASSIC;
pidProfile->tpa_rate = 30;
pidProfile->tpa_breakpoint = 1600;
pidProfile->tpa_low_rate = -50;
pidProfile->tpa_low_breakpoint = 1000;
pidProfile->tpa_low_always = 1;
pidInit(pidProfile);
pidUpdateTpaFactor(0.0f, pidProfile);
EXPECT_FLOAT_EQ(1.0f, pidRuntime.tpaFactor);
pidUpdateTpaFactor(0.1f, pidProfile);
EXPECT_FLOAT_EQ(1.0f, pidRuntime.tpaFactor);
pidUpdateTpaFactor(0.2f, pidProfile);
EXPECT_FLOAT_EQ(1.0f, pidRuntime.tpaFactor);
pidUpdateTpaFactor(0.6f, pidProfile);
EXPECT_FLOAT_EQ(1.0f, pidRuntime.tpaFactor);
pidUpdateTpaFactor(0.8f, pidProfile);
EXPECT_FLOAT_EQ(0.85f, pidRuntime.tpaFactor);
pidUpdateTpaFactor(1.0f, pidProfile);
EXPECT_FLOAT_EQ(0.7f, pidRuntime.tpaFactor);
}
TEST(pidControllerTest, testTpaHyperbolic)
{
resetTest();
// curve sligly down - edge case where internal expo -> inf
pidProfile->tpa_curve_type = TPA_CURVE_HYPERBOLIC;
pidProfile->tpa_curve_pid_thr100 = 50;
pidProfile->tpa_curve_pid_thr0 = 500;
pidProfile->tpa_curve_expo = 10;
pidProfile->tpa_curve_stall_throttle = 30;
pidInit(pidProfile);
pidUpdateTpaFactor(0.0f, pidProfile);
EXPECT_FLOAT_EQ(5.0f, pidRuntime.tpaFactor);
pidUpdateTpaFactor(0.15f, pidProfile);
EXPECT_FLOAT_EQ(5.0f, pidRuntime.tpaFactor);
pidUpdateTpaFactor(0.5, pidProfile);
EXPECT_NEAR(2.588f, pidRuntime.tpaFactor, 0.01f);
pidUpdateTpaFactor(0.9, pidProfile);
EXPECT_NEAR(0.693f, pidRuntime.tpaFactor, 0.01f);
pidUpdateTpaFactor(1.0, pidProfile);
EXPECT_NEAR(0.5f, pidRuntime.tpaFactor, 0.01f);
// linear curve
pidProfile->tpa_curve_type = TPA_CURVE_HYPERBOLIC;
pidProfile->tpa_curve_pid_thr100 = 10;
pidProfile->tpa_curve_pid_thr0 = 300;
pidProfile->tpa_curve_expo = 0;
pidProfile->tpa_curve_stall_throttle = 0;
pidInit(pidProfile);
pidUpdateTpaFactor(0.0f, pidProfile);
EXPECT_FLOAT_EQ(3.0f, pidRuntime.tpaFactor);
pidUpdateTpaFactor(0.15f, pidProfile);
EXPECT_NEAR(2.565f, pidRuntime.tpaFactor, 0.01f);
pidUpdateTpaFactor(0.5, pidProfile);
EXPECT_NEAR(1.550f, pidRuntime.tpaFactor, 0.01f);
pidUpdateTpaFactor(0.9, pidProfile);
EXPECT_NEAR(0.390f, pidRuntime.tpaFactor, 0.01f);
pidUpdateTpaFactor(1.0, pidProfile);
EXPECT_NEAR(0.1f, pidRuntime.tpaFactor, 0.01f);
// curve bends up
pidProfile->tpa_curve_type = TPA_CURVE_HYPERBOLIC;
pidProfile->tpa_curve_pid_thr100 = 60;
pidProfile->tpa_curve_pid_thr0 = 1000;
pidProfile->tpa_curve_expo = -50;
pidProfile->tpa_curve_stall_throttle = 40;
pidInit(pidProfile);
pidUpdateTpaFactor(0.0f, pidProfile);
EXPECT_FLOAT_EQ(10.0f, pidRuntime.tpaFactor);
pidUpdateTpaFactor(0.15f, pidProfile);
EXPECT_NEAR(10.0f, pidRuntime.tpaFactor, 0.01f);
pidUpdateTpaFactor(0.5, pidProfile);
EXPECT_NEAR(9.700f, pidRuntime.tpaFactor, 0.01f);
pidUpdateTpaFactor(0.9, pidProfile);
EXPECT_NEAR(7.364f, pidRuntime.tpaFactor, 0.01f);
pidUpdateTpaFactor(1.0, pidProfile);
EXPECT_NEAR(0.625f, pidRuntime.tpaFactor, 0.01f);
// curve bends down
pidProfile->tpa_curve_type = TPA_CURVE_HYPERBOLIC;
pidProfile->tpa_curve_pid_thr100 = 90;
pidProfile->tpa_curve_pid_thr0 = 250;
pidProfile->tpa_curve_expo = 60;
pidProfile->tpa_curve_stall_throttle = 60;
pidInit(pidProfile);
pidUpdateTpaFactor(0.0f, pidProfile);
EXPECT_FLOAT_EQ(2.5f, pidRuntime.tpaFactor);
pidUpdateTpaFactor(0.15f, pidProfile);
EXPECT_NEAR(2.5f, pidRuntime.tpaFactor, 0.01f);
pidUpdateTpaFactor(0.5, pidProfile);
EXPECT_NEAR(2.5f, pidRuntime.tpaFactor, 0.01f);
pidUpdateTpaFactor(0.9, pidProfile);
EXPECT_NEAR(0.954f, pidRuntime.tpaFactor, 0.01f);
pidUpdateTpaFactor(1.0, pidProfile);
EXPECT_NEAR(0.9f, pidRuntime.tpaFactor, 0.01f);
}