From 983b510184f377214ecc64535be8fc2511b3125b Mon Sep 17 00:00:00 2001 From: Ivan Efimov Date: Fri, 27 Sep 2024 13:24:34 -0500 Subject: [PATCH] Fix: Hyperbolic TPA curve initialization (for wings) (#13932) --- src/main/flight/mixer.c | 2 +- src/main/flight/pid.c | 5 ++- src/main/flight/pid.h | 3 +- src/main/flight/pid_init.c | 3 +- src/test/unit/pid_unittest.cc | 64 +++++++++++++++++------------------ 5 files changed, 39 insertions(+), 38 deletions(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 831d895990..c2623ae09b 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -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 diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 83350d17ee..0d4e7b1fd4 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -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 diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 4d1b4a9129..9d6e999e1a 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -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); diff --git a/src/main/flight/pid_init.c b/src/main/flight/pid_init.c index d405476060..74aaf0c1ad 100644 --- a/src/main/flight/pid_init.c +++ b/src/main/flight/pid_init.c @@ -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; diff --git a/src/test/unit/pid_unittest.cc b/src/test/unit/pid_unittest.cc index 97f6c06154..52a675e392 100644 --- a/src/test/unit/pid_unittest.cc +++ b/src/test/unit/pid_unittest.cc @@ -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); }