add Thrust Linearization feature

src/main/flight/mixer.c

@@ -726,7 +726,12 @@ static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS], motorMixer_t
     // Now add in the desired throttle, but keep in a range that doesn't clip adjusted
     // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips.
     for (int i = 0; i < motorCount; i++) {
-        float motorOutput = motorOutputMin + (motorOutputRange * (motorOutputMixSign * motorMix[i] + throttle * activeMixer[i].throttle));
+        float motorOutput = motorOutputMixSign * motorMix[i] + throttle * activeMixer[i].throttle;
+#ifdef USE_THRUST_LINEARIZATION
+        motorOutput = pidApplyThrustLinearization(motorOutput);
+#endif
+        motorOutput = motorOutputMin + motorOutputRange * motorOutput;
+
 #ifdef USE_SERVOS
         if (mixerIsTricopter()) {
             motorOutput += mixerTricopterMotorCorrection(i);
@@ -887,6 +892,11 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensa
     updateDynLpfCutoffs(currentTimeUs, throttle);
 #endif
 
+#ifdef USE_THRUST_LINEARIZATION
+    // reestablish old throttle stick feel by counter compensating thrust linearization
+    throttle = pidCompensateThrustLinearization(throttle);
+#endif
+
 #if defined(USE_THROTTLE_BOOST)
     if (throttleBoost > 0.0f) {
         const float throttleHpf = throttle - pt1FilterApply(&throttleLpf, throttle);

src/main/flight/pid.c

@@ -114,7 +114,7 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
 
 #define LAUNCH_CONTROL_YAW_ITERM_LIMIT 50 // yaw iterm windup limit when launch mode is "FULL" (all axes)
 
-PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 6);
+PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 7);
 
 void resetPidProfile(pidProfile_t *pidProfile)
 {
@@ -181,6 +181,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
         .launchControlAllowTriggerReset = true,
         .use_integrated_yaw = false,
         .integrated_yaw_relax = 200,
+        .thrustLinearization = 0,
     );
 #ifdef USE_DYN_LPF
     pidProfile->dterm_lowpass_hz = 150;
@@ -474,6 +475,12 @@ static FAST_RAM_ZERO_INIT int acroTrainerAxisState[2];  // only need roll and pi
 static FAST_RAM_ZERO_INIT float acroTrainerGain;
 #endif // USE_ACRO_TRAINER
 
+#ifdef USE_THRUST_LINEARIZATION
+FAST_RAM_ZERO_INIT float thrustLinearization;
+FAST_RAM_ZERO_INIT float thrustLinearizationReciprocal;
+FAST_RAM_ZERO_INIT float thrustLinearizationB;
+#endif
+
 void pidUpdateAntiGravityThrottleFilter(float throttle)
 {
     if (antiGravityMode == ANTI_GRAVITY_SMOOTH) {
@@ -597,6 +604,12 @@ void pidInitConfig(const pidProfile_t *pidProfile)
     useIntegratedYaw = pidProfile->use_integrated_yaw;
     integratedYawRelax = pidProfile->integrated_yaw_relax;
 #endif
+
+#ifdef USE_THRUST_LINEARIZATION
+    thrustLinearization = pidProfile->thrustLinearization / 100.0f;
+    thrustLinearizationReciprocal = 1.0f / thrustLinearization;
+    thrustLinearizationB = (1.0f - thrustLinearization) / (2.0f * thrustLinearization);
+#endif    
 }
 
 void pidInit(const pidProfile_t *pidProfile)
@@ -614,6 +627,27 @@ void pidAcroTrainerInit(void)
 }
 #endif // USE_ACRO_TRAINER
 
+#ifdef USE_THRUST_LINEARIZATION
+float pidCompensateThrustLinearization(float throttle)
+{
+    if (thrustLinearization != 0.0f) {
+        throttle = throttle * (throttle * thrustLinearization + 1.0f - thrustLinearization);
+    }
+    return throttle;
+}
+
+float pidApplyThrustLinearization(float motorOutput)
+{
+    if (thrustLinearization != 0.0f) {
+        if (motorOutput > 0.0f) {
+            motorOutput = sqrtf(motorOutput * thrustLinearizationReciprocal +
+                                thrustLinearizationB * thrustLinearizationB) - thrustLinearizationB;
+        }
+    }
+    return motorOutput;
+}
+#endif
+
 void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex)
 {
     if ((dstPidProfileIndex < MAX_PROFILE_COUNT-1 && srcPidProfileIndex < MAX_PROFILE_COUNT-1)

src/main/flight/pid.h

@@ -159,6 +159,7 @@ typedef struct pidProfile_s {
     uint8_t launchControlAllowTriggerReset; // Controls trigger behavior and whether the trigger can be reset
     uint8_t use_integrated_yaw;             // Selects whether the yaw pidsum should integrated
     uint8_t integrated_yaw_relax;           // Specifies how much integrated yaw should be reduced to offset the drag based yaw component
+    uint8_t thrustLinearization;            // Compensation factor for pid linearization
 } pidProfile_t;
 
 PG_DECLARE_ARRAY(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles);
@@ -210,6 +211,10 @@ bool pidOsdAntiGravityActive(void);
 bool pidOsdAntiGravityMode(void);
 void pidSetAntiGravityState(bool newState);
 bool pidAntiGravityEnabled(void);
+#ifdef USE_THRUST_LINEARIZATION
+float pidApplyThrustLinearization(float motorValue);
+float pidCompensateThrustLinearization(float throttle);
+#endif
 
 #ifdef UNIT_TEST
 #include "sensors/acceleration.h"

src/main/interface/settings.c

@@ -954,6 +954,10 @@ const clivalue_t valueTable[] = {
     { "launch_control_gain",        VAR_UINT8 | PROFILE_VALUE,  .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, launchControlGain) },
 #endif
 
+#ifdef USE_THRUST_LINEARIZATION
+    { "thrust_linear",              VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, thrustLinearization) },
+#endif
+
 // PG_TELEMETRY_CONFIG
 #ifdef USE_TELEMETRY
     { "tlm_inverted",               VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, telemetry_inverted) },

src/main/target/FURYF3/target.h

@@ -23,6 +23,7 @@
 #ifdef FURYF3OSD
     #define TARGET_BOARD_IDENTIFIER "FY3O"
 //    #define USBD_PRODUCT_STRING     "FuryF3OSD"
+    #undef USE_THRUST_LINEARIZATION
 #else
     #define TARGET_BOARD_IDENTIFIER "FYF3"
 //    #define USBD_PRODUCT_STRING     "FuryF3"

src/main/target/OMNIBUS/target.h

@@ -31,6 +31,7 @@
 
 #undef USE_ITERM_RELAX
 #undef USE_RC_SMOOTHING_FILTER
+#undef USE_THRUST_LINEARIZATION
 
 #undef USE_HUFFMAN
 #undef USE_PINIO

src/main/target/SPRACINGF3/target.h

@@ -58,6 +58,7 @@
 #undef USE_EXTENDED_CMS_MENUS
 #endif
 
+#undef USE_THRUST_LINEARIZATION
 #undef USE_BOARD_INFO
 #undef USE_RTC_TIME
 

src/main/target/SPRACINGF3EVO/target.h

@@ -53,6 +53,7 @@
 #undef USE_SERIALRX_SUMH
 #undef USE_PWM
 
+#undef USE_THRUST_LINEARIZATION
 #undef USE_BOARD_INFO
 #undef USE_EXTENDED_CMS_MENUS
 #undef USE_RTC_TIME

src/main/target/common_pre.h

@@ -199,6 +199,7 @@
 #define USE_ITERM_RELAX
 #define USE_DYN_LPF
 #define USE_INTEGRATED_YAW_CONTROL
+#define USE_THRUST_LINEARIZATION
 
 #ifdef USE_SERIALRX_SPEKTRUM
 #define USE_SPEKTRUM_BIND