Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-13 19:40:31 +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

2
src/main/flight/mixer.c
View file

@ -652,7 +652,7 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs)
pidUpdateAntiGravityThrottleFilter(throttle);
// and for TPA
pidUpdateTpaFactor(throttle, currentPidProfile);
pidUpdateTpaFactor(throttle);
#ifdef USE_DYN_LPF
// keep the changes to dynamic lowpass clean, without unnecessary dynamic changes

5
src/main/flight/pid.c
View file

@ -321,7 +321,7 @@ float getTpaFactorClassic(float tpaArgument)
return 1.0f - tpaRate;
}
void pidUpdateTpaFactor(float throttle, const pidProfile_t *pidProfile)
void pidUpdateTpaFactor(float throttle)
{
// don't permit throttle > 1 & throttle < 0 ? is this needed ? can throttle be > 1 or < 0 at this point
throttle = constrainf(throttle, 0.0f, 1.0f);
@ -334,7 +334,7 @@ void pidUpdateTpaFactor(float throttle, const pidProfile_t *pidProfile)
#endif
#ifdef USE_ADVANCED_TPA
switch (pidProfile->tpa_curve_type) {
switch (pidRuntime.tpaCurveType) {
case TPA_CURVE_HYPERBOLIC:
tpaFactor = pwlInterpolate(&pidRuntime.tpaCurvePwl, tpaArgument);
break;
@ -343,7 +343,6 @@ void pidUpdateTpaFactor(float throttle, const pidProfile_t *pidProfile)
tpaFactor = getTpaFactorClassic(tpaArgument);
}
#else
UNUSED(pidProfile);
tpaFactor = getTpaFactorClassic(tpaArgument);
#endif

3
src/main/flight/pid.h
View file

@ -484,6 +484,7 @@ typedef struct pidRuntime_s {
#ifdef USE_ADVANCED_TPA
pwl_t tpaCurvePwl;
float tpaCurvePwl_yValues[TPA_CURVE_PWL_SIZE];
tpaCurveType_t tpaCurveType;
#endif // USE_ADVANCED_TPA
} pidRuntime_t;
@ -506,7 +507,7 @@ void pidSetItermAccelerator(float newItermAccelerator);
bool crashRecoveryModeActive(void);
void pidAcroTrainerInit(void);
void pidSetAcroTrainerState(bool newState);
void pidUpdateTpaFactor(float throttle, const pidProfile_t *pidProfile);
void pidUpdateTpaFactor(float throttle);
void pidUpdateAntiGravityThrottleFilter(float throttle);
bool pidOsdAntiGravityActive(void);
void pidSetAntiGravityState(bool newState);

3
src/main/flight/pid_init.c
View file

@ -298,7 +298,8 @@ void tpaCurveHyperbolicInit(const pidProfile_t *pidProfile)
void tpaCurveInit(const pidProfile_t *pidProfile)
{
switch (pidProfile->tpa_curve_type) {
pidRuntime.tpaCurveType = pidProfile->tpa_curve_type;
switch (pidRuntime.tpaCurveType) {
case TPA_CURVE_HYPERBOLIC:
tpaCurveHyperbolicInit(pidProfile);
return;

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);
}