diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 2c67e844c1..8180cd5f0d 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1429,7 +1429,15 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SPA_YAW_CENTER, "%d", currentPidProfile->spa_center[FD_YAW]); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SPA_YAW_WIDTH, "%d", currentPidProfile->spa_width[FD_YAW]); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SPA_YAW_MODE, "%d", currentPidProfile->spa_mode[FD_YAW]); -#endif +#endif // USE_WING + +#ifdef USE_ADVANCED_TPA + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_CURVE_TYPE, "%d", currentPidProfile->tpa_curve_type); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_CURVE_STALL_THROTTLE, "%d", currentPidProfile->tpa_curve_stall_throttle); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_CURVE_PID_THR0, "%d", currentPidProfile->tpa_curve_pid_thr0); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_CURVE_PID_THR100, "%d", currentPidProfile->tpa_curve_pid_thr100); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_CURVE_EXPO, "%d", currentPidProfile->tpa_curve_expo); +#endif // USE_ADVANCED_TPA BLACKBOX_PRINT_HEADER_LINE("rc_rates", "%d,%d,%d", currentControlRateProfile->rcRates[ROLL], currentControlRateProfile->rcRates[PITCH], @@ -1523,7 +1531,7 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_S_ROLL, "%d", currentPidProfile->pid[PID_ROLL].S); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_S_PITCH, "%d", currentPidProfile->pid[PID_PITCH].S); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_S_YAW, "%d", currentPidProfile->pid[PID_YAW].S); -#endif // #ifdef USE_WING +#endif // USE_WING // End of Betaflight controller parameters diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index e0e92e8c9a..43a91dfedf 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -533,6 +533,12 @@ static const char* const lookupTableFreqDomain[] = { }; #endif +#ifdef USE_ADVANCED_TPA +const char* const lookupTableTpaCurveType[] = { + "CLASSIC", "HYPERBOLIC", +}; +#endif + #define LOOKUP_TABLE_ENTRY(name) { name, ARRAYLEN(name) } const lookupTableEntry_t lookupTables[] = { @@ -658,6 +664,9 @@ const lookupTableEntry_t lookupTables[] = { #ifdef USE_RX_EXPRESSLRS LOOKUP_TABLE_ENTRY(lookupTableFreqDomain), #endif +#ifdef USE_ADVANCED_TPA + LOOKUP_TABLE_ENTRY(lookupTableTpaCurveType), +#endif }; #undef LOOKUP_TABLE_ENTRY @@ -1171,7 +1180,7 @@ const clivalue_t valueTable[] = { { PARAM_NAME_S_PITCH, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, PID_GAIN_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_PITCH].S) }, { PARAM_NAME_S_ROLL, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, PID_GAIN_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_ROLL].S) }, { PARAM_NAME_S_YAW, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, PID_GAIN_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_YAW].S) }, -#endif // #ifdef USE_WING +#endif // USE_WING { PARAM_NAME_ANGLE_P_GAIN, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].P) }, { PARAM_NAME_ANGLE_FEEDFORWARD, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].F) }, @@ -1275,7 +1284,15 @@ const clivalue_t valueTable[] = { { PARAM_NAME_TPA_DELAY_MS, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, UINT16_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_delay_ms) }, { PARAM_NAME_TPA_GRAVITY_THR0, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, TPA_GRAVITY_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_gravity_thr0) }, { PARAM_NAME_TPA_GRAVITY_THR100, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, TPA_GRAVITY_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_gravity_thr100) }, -#endif +#endif // USE_WING + +#ifdef USE_ADVANCED_TPA + { PARAM_NAME_TPA_CURVE_TYPE, VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_TPA_CURVE_TYPE }, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_curve_type) }, + { PARAM_NAME_TPA_CURVE_STALL_THROTTLE, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, TPA_CURVE_STALL_THROTTLE_MAX}, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_curve_stall_throttle) }, + { PARAM_NAME_TPA_CURVE_PID_THR0, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, TPA_CURVE_PID_MAX}, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_curve_pid_thr0) }, + { PARAM_NAME_TPA_CURVE_PID_THR100, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, TPA_CURVE_PID_MAX}, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_curve_pid_thr100) }, + { PARAM_NAME_TPA_CURVE_EXPO, VAR_INT8 | PROFILE_VALUE, .config.minmaxUnsigned = { TPA_CURVE_EXPO_MIN, TPA_CURVE_EXPO_MAX}, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_curve_expo) }, +#endif // USE_ADVANCED_TPA { PARAM_NAME_EZ_LANDING_THRESHOLD, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, ez_landing_threshold) }, { PARAM_NAME_EZ_LANDING_LIMIT, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 75 }, PG_PID_PROFILE, offsetof(pidProfile_t, ez_landing_limit) }, diff --git a/src/main/cli/settings.h b/src/main/cli/settings.h index 8abe7f9064..2163ad0e29 100644 --- a/src/main/cli/settings.h +++ b/src/main/cli/settings.h @@ -144,6 +144,9 @@ typedef enum { #endif #ifdef USE_RX_EXPRESSLRS TABLE_FREQ_DOMAIN, +#endif +#ifdef USE_ADVANCED_TPA + TABLE_TPA_CURVE_TYPE, #endif LOOKUP_TABLE_COUNT } lookupTableIndex_e; diff --git a/src/main/fc/parameter_names.h b/src/main/fc/parameter_names.h index 9a9a4a1289..8d57111743 100644 --- a/src/main/fc/parameter_names.h +++ b/src/main/fc/parameter_names.h @@ -61,6 +61,11 @@ #define PARAM_NAME_TPA_DELAY_MS "tpa_delay_ms" #define PARAM_NAME_TPA_GRAVITY_THR0 "tpa_gravity_thr0" #define PARAM_NAME_TPA_GRAVITY_THR100 "tpa_gravity_thr100" +#define PARAM_NAME_TPA_CURVE_TYPE "tpa_curve_type" +#define PARAM_NAME_TPA_CURVE_STALL_THROTTLE "tpa_curve_stall_throttle" +#define PARAM_NAME_TPA_CURVE_PID_THR0 "tpa_curve_pid_thr0" +#define PARAM_NAME_TPA_CURVE_PID_THR100 "tpa_curve_pid_thr100" +#define PARAM_NAME_TPA_CURVE_EXPO "tpa_curve_expo" #define PARAM_NAME_MIXER_TYPE "mixer_type" #define PARAM_NAME_EZ_LANDING_THRESHOLD "ez_landing_threshold" #define PARAM_NAME_EZ_LANDING_LIMIT "ez_landing_limit" diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index ae2226f68e..3a6cf1db77 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -651,7 +651,7 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs) pidUpdateAntiGravityThrottleFilter(throttle); // and for TPA - pidUpdateTpaFactor(throttle); + pidUpdateTpaFactor(throttle, currentPidProfile); #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 29df76cfbb..4aba7dc3d9 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -239,6 +239,11 @@ void resetPidProfile(pidProfile_t *pidProfile) .landing_disarm_threshold = 0, // relatively safe values are around 100 .feedforward_yaw_hold_gain = 15, // zero disables; 15-20 is OK for 5in .feedforward_yaw_hold_time = 100, // a value of 100 is a time constant of about 100ms, and is OK for a 5in; smaller values decay faster, eg for smaller props + .tpa_curve_type = TPA_CURVE_CLASSIC, + .tpa_curve_stall_throttle = 30, + .tpa_curve_pid_thr0 = 200, + .tpa_curve_pid_thr100 = 70, + .tpa_curve_expo = 20, ); #ifndef USE_D_MIN @@ -303,18 +308,9 @@ static float getWingTpaArgument(float throttle) } #endif // #ifndef USE_WING -void pidUpdateTpaFactor(float throttle) +float getTpaFactorClassic(float tpaArgument) { static bool isTpaLowFaded = false; - // 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); - -#ifdef USE_WING - const float tpaArgument = isFixedWing() ? getWingTpaArgument(throttle) : throttle; -#else - const float tpaArgument = throttle; -#endif - bool isThrottlePastTpaLowBreakpoint = (tpaArgument >= pidRuntime.tpaLowBreakpoint || pidRuntime.tpaLowBreakpoint <= 0.01f); float tpaRate = 0.0f; if (isThrottlePastTpaLowBreakpoint || isTpaLowFaded) { @@ -326,7 +322,35 @@ void pidUpdateTpaFactor(float throttle) tpaRate = pidRuntime.tpaLowMultiplier * (pidRuntime.tpaLowBreakpoint - tpaArgument); } - float tpaFactor = 1.0f - tpaRate; + return 1.0f - tpaRate; +} + +void pidUpdateTpaFactor(float throttle, const pidProfile_t *pidProfile) +{ + // 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); + float tpaFactor; + +#ifdef USE_WING + const float tpaArgument = isFixedWing() ? getWingTpaArgument(throttle) : throttle; +#else + const float tpaArgument = throttle; +#endif + +#ifdef USE_ADVANCED_TPA + switch (pidProfile->tpa_curve_type) { + case TPA_CURVE_HYPERBOLIC: + tpaFactor = pwlInterpolate(&pidRuntime.tpaCurvePwl, tpaArgument); + break; + case TPA_CURVE_CLASSIC: + default: + tpaFactor = getTpaFactorClassic(tpaArgument); + } +#else + UNUSED(pidProfile); + tpaFactor = getTpaFactorClassic(tpaArgument); +#endif + DEBUG_SET(DEBUG_TPA, 0, lrintf(tpaFactor * 1000)); pidRuntime.tpaFactor = tpaFactor; } @@ -861,7 +885,7 @@ NOINLINE static void calculateSpaValues(const pidProfile_t *pidProfile) } #else UNUSED(pidProfile); -#endif // #ifdef USE_WING ... #else +#endif // USE_WING } NOINLINE static void applySpa(int axis, const pidProfile_t *pidProfile) @@ -894,7 +918,7 @@ NOINLINE static void applySpa(int axis, const pidProfile_t *pidProfile) #else UNUSED(axis); UNUSED(pidProfile); -#endif // #ifdef USE_WING ... #else +#endif // USE_WING } // Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage. @@ -1115,7 +1139,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim // slowing down I-term change, or even making it zero if setpoint is high enough iTermChange *= pidRuntime.spa[axis]; } -#endif // #ifdef USE_WING +#endif // USE_WING pidData[axis].I = constrainf(previousIterm + iTermChange, -pidRuntime.itermLimit, pidRuntime.itermLimit); diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 613978026c..1159d9902a 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -21,9 +21,11 @@ #pragma once #include -#include "common/time.h" -#include "common/filter.h" + #include "common/axis.h" +#include "common/filter.h" +#include "common/pwl.h" +#include "common/time.h" #include "pg/pg.h" @@ -73,10 +75,18 @@ #ifdef USE_WING #define TPA_LOW_RATE_MIN INT8_MIN #define TPA_GRAVITY_MAX 5000 +#define TPA_CURVE_STALL_THROTTLE_MAX 100 #else #define TPA_LOW_RATE_MIN 0 #endif +#ifdef USE_ADVANCED_TPA +#define TPA_CURVE_PID_MAX 1000 +#define TPA_CURVE_EXPO_MIN -100 +#define TPA_CURVE_EXPO_MAX 100 +#define TPA_CURVE_PWL_SIZE 17 +#endif // USE_ADVANCED_TPA + typedef enum { TPA_MODE_PD, TPA_MODE_D @@ -147,6 +157,11 @@ typedef enum feedforwardAveraging_e { FEEDFORWARD_AVERAGING_4_POINT, } feedforwardAveraging_t; +typedef enum tpaCurveType_e { + TPA_CURVE_CLASSIC, + TPA_CURVE_HYPERBOLIC, +} tpaCurveType_t; + #define MAX_PROFILE_NAME_LENGTH 8u typedef struct pidProfile_s { @@ -269,8 +284,13 @@ typedef struct pidProfile_s { uint16_t spa_center[XYZ_AXIS_COUNT]; // RPY setpoint at which PIDs are reduced to 50% (setpoint PID attenuation) uint16_t spa_width[XYZ_AXIS_COUNT]; // Width of smooth transition around spa_center uint8_t spa_mode[XYZ_AXIS_COUNT]; // SPA mode for each axis - uint16_t tpa_gravity_thr0; // For wings: addition to tpa argument in % when zero throttle - uint16_t tpa_gravity_thr100; // For wings: addition to tpa argument in % when full throttle + uint16_t tpa_gravity_thr0; // For wings: gravity force addition to tpa argument in % when zero throttle + uint16_t tpa_gravity_thr100; // For wings: gravity force addition to tpa argument in % when full throttle + uint8_t tpa_curve_type; // Classic type - for multirotor, hyperbolic - usually for wings + uint8_t tpa_curve_stall_throttle; // For wings: speed at which PIDs should be maxed out (stall speed) + uint16_t tpa_curve_pid_thr0; // For wings: PIDs multiplier at stall speed + uint16_t tpa_curve_pid_thr100; // For wings: PIDs multiplier at full speed + int8_t tpa_curve_expo; // For wings: how fast PIDs do transition as speed grows } pidProfile_t; PG_DECLARE_ARRAY(pidProfile_t, PID_PROFILE_COUNT, pidProfiles); @@ -461,7 +481,12 @@ typedef struct pidRuntime_s { float spa[XYZ_AXIS_COUNT]; // setpoint pid attenuation (0.0 to 1.0). 0 - full attenuation, 1 - no attenuation float tpaGravityThr0; float tpaGravityThr100; -#endif +#endif // USE_WING + +#ifdef USE_ADVANCED_TPA + pwl_t tpaCurvePwl; + float tpaCurvePwl_yValues[TPA_CURVE_PWL_SIZE]; +#endif // USE_ADVANCED_TPA } pidRuntime_t; extern pidRuntime_t pidRuntime; @@ -483,7 +508,7 @@ void pidSetItermAccelerator(float newItermAccelerator); bool crashRecoveryModeActive(void); void pidAcroTrainerInit(void); void pidSetAcroTrainerState(bool newState); -void pidUpdateTpaFactor(float throttle); +void pidUpdateTpaFactor(float throttle, const pidProfile_t *pidProfile); 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 1cb2ca278c..158a65535d 100644 --- a/src/main/flight/pid_init.c +++ b/src/main/flight/pid_init.c @@ -267,6 +267,49 @@ void pidInitFilters(const pidProfile_t *pidProfile) #endif } + +#ifdef USE_ADVANCED_TPA +float tpaCurveHyperbolicFunction(float x, void *args) +{ + const pidProfile_t *pidProfile = (const pidProfile_t*)args; + + const float thrStall = pidProfile->tpa_curve_stall_throttle / 100.0f; + const float pidThr0 = pidProfile->tpa_curve_pid_thr0 / 100.0f; + + if (x <= thrStall) { + return pidThr0; + } + + const float expoDivider = pidProfile->tpa_curve_expo / 10.0f - 1.0f; + const float expo = (fabsf(expoDivider) > 1e-3f) ? 1.0f / expoDivider : 1e3f; // avoiding division by zero for const float base = ... + + const float pidThr100 = pidProfile->tpa_curve_pid_thr100 / 100.0f; + const float xShifted = scaleRangef(x, thrStall, 1.0f, 0.0f, 1.0f); + const float base = (1 + (powf(pidThr0 / pidThr100, 1.0f / expo) - 1) * xShifted); + const float divisor = powf(base, expo); + + return pidThr0 / divisor; +} + +void tpaCurveHyperbolicInit(const pidProfile_t *pidProfile) +{ + pwlInitialize(&pidRuntime.tpaCurvePwl, pidRuntime.tpaCurvePwl_yValues, TPA_CURVE_PWL_SIZE, 0.0f, 1.0f); + pwlFill(&pidRuntime.tpaCurvePwl, tpaCurveHyperbolicFunction, (void*)pidProfile); +} + +void tpaCurveInit(const pidProfile_t *pidProfile) +{ + switch (pidProfile->tpa_curve_type) { + case TPA_CURVE_HYPERBOLIC: + tpaCurveHyperbolicInit(pidProfile); + return; + case TPA_CURVE_CLASSIC: + default: + return; + } +} +#endif // USE_ADVANCED_TPA + void pidInit(const pidProfile_t *pidProfile) { pidSetTargetLooptime(gyro.targetLooptime); // Initialize pid looptime @@ -275,6 +318,9 @@ void pidInit(const pidProfile_t *pidProfile) #ifdef USE_RPM_FILTER rpmFilterInit(rpmFilterConfig(), gyro.targetLooptime); #endif +#ifdef USE_ADVANCED_TPA + tpaCurveInit(pidProfile); +#endif } void pidInitConfig(const pidProfile_t *pidProfile) diff --git a/src/test/Makefile b/src/test/Makefile index 9c8ead24c3..0ec3a16f74 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -384,6 +384,7 @@ pid_unittest_SRC := \ $(USER_DIR)/common/crc.c \ $(USER_DIR)/common/filter.c \ $(USER_DIR)/common/maths.c \ + $(USER_DIR)/common/pwl.c \ $(USER_DIR)/common/streambuf.c \ $(USER_DIR)/drivers/accgyro/gyro_sync.c \ $(USER_DIR)/fc/controlrate_profile.c \ @@ -397,7 +398,8 @@ pid_unittest_DEFINES := \ USE_RC_SMOOTHING_FILTER= \ USE_ABSOLUTE_CONTROL= \ USE_LAUNCH_CONTROL= \ - USE_FEEDFORWARD= + USE_FEEDFORWARD= \ + USE_ADVANCED_TPA= \ rcdevice_unittest_DEFINES := \ USE_RCDEVICE= diff --git a/src/test/unit/pid_unittest.cc b/src/test/unit/pid_unittest.cc index 8972f3c1da..2b72d33a39 100644 --- a/src/test/unit/pid_unittest.cc +++ b/src/test/unit/pid_unittest.cc @@ -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); +}