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

* 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

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
 

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

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;

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"

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

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

src/main/flight/pid.h

@@ -21,9 +21,11 @@
 #pragma once
 
 #include <stdbool.h>
-#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_thr0;              // For wings: gravity force 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_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);

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)

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=

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