From 7cd83464d6ae3eb22bc58010f82863c86ee28c16 Mon Sep 17 00:00:00 2001
From: ctzsnooze <chris.thompson@sydney.edu.au>
Date: Sat, 25 Apr 2020 23:40:00 +1000
Subject: [PATCH 1/2] Power 2 thrust compensation with 50% throttle
 compensation

^2 gain curve after discussions with Markus
Variable throttle compensation, more with higher TL for whoops
Calculator https://www.desmos.com/calculator/1rhq0pqoug
---
 src/main/cli/settings.c    | 2 +-
 src/main/flight/pid.c      | 9 ++++++---
 src/main/flight/pid.h      | 2 --
 src/main/flight/pid_init.c | 4 ----
 4 files changed, 7 insertions(+), 10 deletions(-)

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..1995fa5e5a 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 * thrustLinearization);
+        const float throttleReversed = (1.0f - throttle);
+        throttle /= 1.0f + throttleCompensateAmount * throttleReversed  * throttleReversed * 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 + motorOutputReversed  * motorOutputReversed * 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) {

From fb605aed019ce56dff9e63195d332ec1f59d40c2 Mon Sep 17 00:00:00 2001
From: mikeller <github@ike.ch>
Date: Sun, 5 Jul 2020 14:05:10 +1200
Subject: [PATCH 2/2] Fixed throttle calculation.

---
 src/main/flight/pid.c | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 1995fa5e5a..c3fc4d9888 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -291,9 +291,9 @@ float pidCompensateThrustLinearization(float throttle)
 {
     if (pidRuntime.thrustLinearization != 0.0f) {
         // for whoops where a lot of TL is needed, allow more throttle boost
-        const float throttleCompensateAmount = (1.0f - 0.5f * thrustLinearization);
+        const float throttleCompensateAmount = (1.0f - 0.5f * pidRuntime.thrustLinearization);
         const float throttleReversed = (1.0f - throttle);
-        throttle /= 1.0f + throttleCompensateAmount * throttleReversed  * throttleReversed * thrustLinearization;
+        throttle /= 1.0f + throttleCompensateAmount * powerf(throttleReversed, 2) * pidRuntime.thrustLinearization;
     }
     return throttle;
 }
@@ -303,7 +303,7 @@ float pidApplyThrustLinearization(float motorOutput)
     if (pidRuntime.thrustLinearization != 0.0f) {
         if (motorOutput > 0.0f) {
             const float motorOutputReversed = (1.0f - motorOutput);
-            motorOutput *= 1.0f + motorOutputReversed  * motorOutputReversed * thrustLinearization;
+            motorOutput *= 1.0f + powerf(motorOutputReversed, 2) * pidRuntime.thrustLinearization;
         }
     }
     return motorOutput;