Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-14 03:50:02 +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

12
src/main/blackbox/blackbox.c
View file

@ -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

21
src/main/cli/settings.c
View file

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

3
src/main/cli/settings.h
View file

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

5
src/main/fc/parameter_names.h
View file

@ -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"

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

@ -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

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

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

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

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

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

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

4
src/test/Makefile
View file

@ -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=

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