diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index f747b9d33d..7511521c52 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1112,7 +1112,7 @@ const clivalue_t valueTable[] = { #endif #ifdef USE_THRUST_LINEARIZATION - { "thrust_linear", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, thrustLinearization) }, + { "thrust_linear", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 150 }, PG_PID_PROFILE, offsetof(pidProfile_t, thrustLinearization) }, #endif #ifdef USE_AIRMODE_LPF { "transient_throttle_limit", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 30 }, PG_PID_PROFILE, offsetof(pidProfile_t, transient_throttle_limit) }, diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 03034767f7..c3fc4d9888 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -290,7 +290,10 @@ void pidAcroTrainerInit(void) float pidCompensateThrustLinearization(float throttle) { if (pidRuntime.thrustLinearization != 0.0f) { - throttle = throttle * (throttle * pidRuntime.thrustLinearization + 1.0f - pidRuntime.thrustLinearization); + // for whoops where a lot of TL is needed, allow more throttle boost + const float throttleCompensateAmount = (1.0f - 0.5f * pidRuntime.thrustLinearization); + const float throttleReversed = (1.0f - throttle); + throttle /= 1.0f + throttleCompensateAmount * powerf(throttleReversed, 2) * pidRuntime.thrustLinearization; } return throttle; } @@ -299,8 +302,8 @@ float pidApplyThrustLinearization(float motorOutput) { if (pidRuntime.thrustLinearization != 0.0f) { if (motorOutput > 0.0f) { - motorOutput = sqrtf(motorOutput * pidRuntime.thrustLinearizationReciprocal + - pidRuntime.thrustLinearizationB * pidRuntime.thrustLinearizationB) - pidRuntime.thrustLinearizationB; + const float motorOutputReversed = (1.0f - motorOutput); + motorOutput *= 1.0f + powerf(motorOutputReversed, 2) * pidRuntime.thrustLinearization; } } return motorOutput; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 95e65ccb01..b9878cf77a 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -352,8 +352,6 @@ typedef struct pidRuntime_s { #ifdef USE_THRUST_LINEARIZATION float thrustLinearization; - float thrustLinearizationReciprocal; - float thrustLinearizationB; #endif #ifdef USE_AIRMODE_LPF diff --git a/src/main/flight/pid_init.c b/src/main/flight/pid_init.c index 540a7b1647..4b7a7c94f3 100644 --- a/src/main/flight/pid_init.c +++ b/src/main/flight/pid_init.c @@ -375,10 +375,6 @@ void pidInitConfig(const pidProfile_t *pidProfile) #ifdef USE_THRUST_LINEARIZATION pidRuntime.thrustLinearization = pidProfile->thrustLinearization / 100.0f; - if (pidRuntime.thrustLinearization != 0.0f) { - pidRuntime.thrustLinearizationReciprocal = 1.0f / pidRuntime.thrustLinearization; - pidRuntime.thrustLinearizationB = (1.0f - pidRuntime.thrustLinearization) / (2.0f * pidRuntime.thrustLinearization); - } #endif #if defined(USE_D_MIN) for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {