mixer: thrust linearization, ported from Betaflight

docs/Settings.md

@@ -7243,6 +7243,26 @@ Throttle value when the stick is set to mid-position. Used in the throttle curve
 
 ---
 
+### throttle_boost
+
+Multirotor only, ported from Betaflight. Adds a high-pass filtered copy of the throttle to it, so the motors follow quick throttle moves sooner. The boost is this value divided by 10 times the difference between the throttle and its low-pass filtered value. 0 disables it; Betaflight uses 5 by default. Only the motors get the boost: OSD, telemetry and servos show the throttle that was asked for. Not applied while navigation controls the throttle, during a VTOL transition, with reversible motors or with the logic-condition throttle override.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0 | 0 | 100 |
+
+---
+
+### throttle_boost_cutoff
+
+Cutoff of the low-pass filter used by `throttle_boost` [Hz]. A higher cutoff makes each boost shorter.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 15 | 5 | 50 |
+
+---
+
 ### throttle_idle
 
 The percentage of the throttle range (`max_throttle` - `min_command`) above `min_command` used for minimum / idle throttle.
@@ -7273,6 +7293,16 @@ Can be used in ANGLE and HORIZON mode and will automatically boost throttle when
 
 ---
 
+### thrust_linear
+
+Multirotor only, ported from Betaflight 2026.6. A propeller's thrust grows faster than linearly with the motor command, so the quad responds less to the same correction at low throttle. This raises low motor outputs and lowers the throttle before the mix by the inverse amount, so hover stays where it was (within 8 us up to 50, about 23 us at 70 and 80 us at 100, at worst near 15 % throttle). The value is about ArduPilot's `MOT_THST_EXPO` times 100. Only the motors get the change: OSD, telemetry and servos show the throttle that was asked for. 0 disables it. Above about 70 check `nav_mc_hover_thr` again. Betaflight allows up to 150, but its formulas drive the throttle negative above 100 and stop being monotonic from about 130. Off on VTOL setups (any mixer profile that is not a multirotor), when any motor's throttle weight in the mixer is not 1, with reversible motors and with the logic-condition throttle override.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0 | 0 | 100 |
+
+---
+
 ### tpa_breakpoint
 
 See tpa_rate.

src/main/blackbox/blackbox.c

@@ -2153,6 +2153,9 @@ static bool blackboxWriteSysinfo(void)
         BLACKBOX_PRINT_HEADER_LINE("serialrx_provider", "%d",               rxConfig()->serialrx_provider);
         BLACKBOX_PRINT_HEADER_LINE("motor_pwm_protocol", "%d",              motorConfig()->motorPwmProtocol);
         BLACKBOX_PRINT_HEADER_LINE("motor_pwm_rate", "%d",                  getEscUpdateFrequency());
+        BLACKBOX_PRINT_HEADER_LINE("throttle_boost", "%d",                  motorConfig()->throttleBoost);
+        BLACKBOX_PRINT_HEADER_LINE("throttle_boost_cutoff", "%d",           motorConfig()->throttleBoostCutoff);
+        BLACKBOX_PRINT_HEADER_LINE("thrust_linear", "%d",                   motorConfig()->thrustLinear);
         BLACKBOX_PRINT_HEADER_LINE("debug_mode", "%d",                      systemConfig()->debug_mode);
         BLACKBOX_PRINT_HEADER_LINE("features", "%d",                        featureConfig()->enabledFeatures);
         BLACKBOX_PRINT_HEADER_LINE("waypoints", "%d,%d",                    getWaypointCount(),isWaypointListValid());

src/main/fc/settings.yaml

@@ -902,6 +902,24 @@ groups:
         min: 4
         max: 255
         default_value: 14
+      - name: throttle_boost
+        description: "Multirotor only, ported from Betaflight. Adds a high-pass filtered copy of the throttle to it, so the motors follow quick throttle moves sooner. The boost is this value divided by 10 times the difference between the throttle and its low-pass filtered value. 0 disables it; Betaflight uses 5 by default. Only the motors get the boost: OSD, telemetry and servos show the throttle that was asked for. Not applied while navigation controls the throttle, during a VTOL transition, with reversible motors or with the logic-condition throttle override."
+        default_value: 0
+        field: throttleBoost
+        min: 0
+        max: 100
+      - name: throttle_boost_cutoff
+        description: "Cutoff of the low-pass filter used by `throttle_boost` [Hz]. A higher cutoff makes each boost shorter."
+        default_value: 15
+        field: throttleBoostCutoff
+        min: 5
+        max: 50
+      - name: thrust_linear
+        description: "Multirotor only, ported from Betaflight 2026.6. A propeller's thrust grows faster than linearly with the motor command, so the quad responds less to the same correction at low throttle. This raises low motor outputs and lowers the throttle before the mix by the inverse amount, so hover stays where it was (within 8 us up to 50, about 23 us at 70 and 80 us at 100, at worst near 15 % throttle). The value is about ArduPilot's `MOT_THST_EXPO` times 100. Only the motors get the change: OSD, telemetry and servos show the throttle that was asked for. 0 disables it. Above about 70 check `nav_mc_hover_thr` again. Betaflight allows up to 150, but its formulas drive the throttle negative above 100 and stop being monotonic from about 130. Off on VTOL setups (any mixer profile that is not a multirotor), when any motor's throttle weight in the mixer is not 1, with reversible motors and with the logic-condition throttle override."
+        default_value: 0
+        field: thrustLinear
+        min: 0
+        max: 100
 
   - name: PG_FAILSAFE_CONFIG
     type: failsafeConfig_t

src/main/flight/mixer.c

@@ -52,6 +52,7 @@
 #include "flight/mixer_transition_logic.h"
 #include "flight/pid.h"
 #include "flight/servos.h"
+#include "flight/thrust_linear.h"
 
 #include "navigation/navigation.h"
 
@@ -101,6 +102,9 @@ PG_RESET_TEMPLATE(motorConfig_t, motorConfig,
     .srxl2TelemetryRate = SETTING_ESC_SRXL2_TELEMETRY_RATE_DEFAULT,
     .srxl2Telemetry = SETTING_ESC_SRXL2_TELEMETRY_DEFAULT,
 #endif
+    .throttleBoost = SETTING_THROTTLE_BOOST_DEFAULT,
+    .throttleBoostCutoff = SETTING_THROTTLE_BOOST_CUTOFF_DEFAULT,
+    .thrustLinear = SETTING_THRUST_LINEAR_DEFAULT,
 );
 PG_REGISTER_ARRAY_WITH_RESET_FN(timerOverride_t, HARDWARE_TIMER_DEFINITION_COUNT, timerOverrides, PG_TIMER_OVERRIDE_CONFIG, 0);
 
@@ -655,6 +659,95 @@ static int getReversibleMotorsThrottleDeadband(void)
     return ifMotorstopFeatureEnabled() ? reversibleMotorsConfig()->neutral : directionValue;
 }
 
+// The throttle the motors get; mixerThrottleCommand stays the one OSD, telemetry and servos show
+static EXTENDED_FASTRAM int motorThrottleCommand;
+
+// Longer than any mixer period, so a gap this long means the mixer skipped the boost at least once
+#define THROTTLE_BOOST_RESTART_US 20000
+
+// Betaflight's throttle boost: a high-passed copy of the throttle on top of it, so the motors follow quick moves sooner
+static int applyThrottleBoost(int throttle, float dT)
+{
+    static pt1Filter_t throttleLpf;
+    static timeUs_t lastBoostUs;
+    static uint8_t boostCutoff;
+
+    // A cutoff of 0 would freeze the low-pass and turn the boost into a constant offset
+    if (!motorConfig()->throttleBoost || !motorConfig()->throttleBoostCutoff || !STATE(MULTIROTOR) || isMixerTransitionMixing
+        || feature(FEATURE_REVERSIBLE_MOTORS) || navigationRequiresAutoThrottleMode()
+#ifdef USE_PROGRAMMING_FRAMEWORK
+        || LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE)
+#endif
+        ) {
+        lastBoostUs = 0;
+        return throttle;
+    }
+
+    // mixTable() does not get here while disarmed or with the motors stopped: restart from the current throttle
+    const timeUs_t nowUs = micros();
+    if (nowUs - lastBoostUs > THROTTLE_BOOST_RESTART_US || boostCutoff != motorConfig()->throttleBoostCutoff) {
+        boostCutoff = motorConfig()->throttleBoostCutoff;
+        pt1FilterInit(&throttleLpf, boostCutoff, dT);
+        pt1FilterReset(&throttleLpf, throttle);
+    }
+    lastBoostUs = nowUs;
+
+    const float throttleHpf = throttle - pt1FilterApply3(&throttleLpf, throttle, dT);
+    return constrain(throttle + lrintf(motorConfig()->throttleBoost * 0.1f * throttleHpf), throttleRangeMin, throttleRangeMax);
+}
+
+static EXTENDED_FASTRAM float thrustLinear;     // 0 when off
+
+// A VTOL switching it in flight would step the motors, so it stays off on any setup with a non-multirotor profile
+static bool isThrustLinearizationAllowed(void)
+{
+    for (int i = 0; i < MAX_MIXER_PROFILE_COUNT; i++) {
+        if (!isMultirotorTypePlatform(mixerConfigByIndex(i)->platformType)) {
+            return false;
+        }
+    }
+    // INAV scales the absolute command by the weight: with any other weight this is not the curve's inverse
+    for (int i = 0; i < motorCount; i++) {
+        if (currentMixer[i].throttle != 1.0f) {
+            return false;
+        }
+    }
+    return STATE(MULTIROTOR) && !feature(FEATURE_REVERSIBLE_MOTORS)
+#ifdef USE_PROGRAMMING_FRAMEWORK
+        && !LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE)
+#endif
+        ;
+}
+
+static int compensateThrustLinearization(int throttle)
+{
+    thrustLinear = 0.0f;
+    if (!motorConfig()->thrustLinear || !isThrustLinearizationAllowed()) {
+        return throttle;
+    }
+
+    thrustLinear = motorConfig()->thrustLinear / 100.0f;
+    const float range = throttleRangeMax - throttleRangeMin;
+    return throttleRangeMin + lrintf(range * thrustLinearCompensate((throttle - throttleRangeMin) / range, thrustLinear));
+}
+
+static NOINLINE int applyThrustLinearization(int motorOutput)
+{
+    // Not in Betaflight: at or below idle nothing changes, so the failsafe clamp to min_command is not pushed further
+    if (motorOutput <= throttleRangeMin) {
+        return motorOutput;
+    }
+
+    const float range = throttleRangeMax - throttleRangeMin;
+    return throttleRangeMin + lrintf(range * thrustLinearCurve((motorOutput - throttleRangeMin) / range, thrustLinear));
+}
+
+// Out of line: mixTable() is FAST_CODE and ITCM is nearly full on some F7 targets
+static NOINLINE void updateMotorThrottle(float dT)
+{
+    motorThrottleCommand = compensateThrustLinearization(applyThrottleBoost(mixerThrottleCommand, dT));
+}
+
 void FAST_CODE mixTable(float dT)
 {
     static float lastMixerThrottleCommand = 1000.0f;
@@ -827,6 +920,8 @@ void FAST_CODE mixTable(float dT)
         }
     }
 
+    updateMotorThrottle(dT);
+
     #define THROTTLE_CLIPPING_FACTOR    0.33f
     motorMixRange = (float)rpyMixRange / (float)throttleRange;
     if (motorMixRange > 1.0f) {
@@ -851,7 +946,7 @@ void FAST_CODE mixTable(float dT)
     const bool fwEmergencyLanding = STATE(AIRPLANE) && !isMixerTransitionMixing && navigationIsExecutingAnEmergencyLanding();
 
     for (int i = 0; i < motorCount; i++) {
-        float motorThrottle = mixerThrottleCommand * currentMixer[i].throttle;
+        float motorThrottle = motorThrottleCommand * currentMixer[i].throttle;
 #ifdef USE_AUTO_TRANSITION
         const motorMixer_t *targetMixer = autoTransition.active ? &autoTransition.targetMotorMixer[i] : NULL;
         const bool currentMotorActive = currentMixer[i].throttle > 0.0f;
@@ -886,6 +981,9 @@ void FAST_CODE mixTable(float dT)
             motor[i] = constrain(motorThrottle, throttleRangeMin, throttleRangeMax);
         } else {
             motor[i] = rpyMix[i] + constrain(motorThrottle, throttleMin, throttleMax);
+            if (thrustLinear) {
+                motor[i] = applyThrustLinearization(motor[i]);
+            }
         }
 
         if (failsafeIsActive()) {

src/main/flight/mixer.h

@@ -95,7 +95,7 @@ typedef struct motorConfig_s {
     uint8_t  motorPwmProtocol;
     uint16_t digitalIdleOffsetValue;
     uint8_t motorPoleCount;                 // Magnetic poles in the motors for calculating actual RPM from eRPM provided by ESC telemetry
-#ifdef USE_MOTOR_SRXL2
+    // On every target, so turning USE_MOTOR_SRXL2 on or off never moves the fields stored after these
     /*
      * Appended at the end, but not immediately after motorPoleCount.
      *
@@ -120,7 +120,9 @@ typedef struct motorConfig_s {
     uint8_t srxl2ReverseChannel;            // 1-based aux channel an SRXL2 ESC uses to arm reverse; 0 disables
     uint8_t srxl2Telemetry;                 // read ESC telemetry off the SRXL2 link
     uint8_t srxl2TelemetryRate;             // how often to ask the ESC for telemetry, as srxl2TelemetryRate_e
-#endif
+    uint8_t throttleBoost;                  // must default to 0: it can land in the old tail padding, stored as zero
+    uint8_t throttleBoostCutoff;            // Hz
+    uint8_t thrustLinear;                   // must default to 0: it can land in the old tail padding, stored as zero
 } motorConfig_t;
 
 PG_DECLARE(motorConfig_t, motorConfig);

src/main/flight/thrust_linear.h

@@ -0,0 +1,33 @@
+/*
+ * This file is part of INAV.
+ *
+ * INAV is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * INAV is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with INAV.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+// Betaflight 2026.6 (#15226), on 0-1 over the throttle range with e = thrust_linear / 100; sound only for e <= 1
+
+// Raises low motor outputs, where thrust grows more slowly than the command
+static inline float thrustLinearCurve(float u, float e)
+{
+    const float inv = 1.0f - u;
+    return u * (1.0f + e * inv * (1.0f + e * (inv - u)));
+}
+
+// Inverse of the curve to second order in e, applied to the throttle so hover stays where it was
+static inline float thrustLinearCompensate(float t, float e)
+{
+    return t * (1.0f - e * (1.0f - t));
+}

src/test/unit/thrust_linear_unittest.cc

@@ -0,0 +1,88 @@
+/*
+ * This file is part of INAV.
+ *
+ * INAV is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * INAV is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with INAV.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <math.h>
+
+extern "C" {
+    #include "flight/thrust_linear.h"
+}
+
+#include "unittest_macros.h"
+#include "gtest/gtest.h"
+
+// thrust_linear 0-100 in steps of 10
+static float strength(int step)
+{
+    return step / 10.0f;
+}
+
+TEST(ThrustLinearTest, OffIsIdentity)
+{
+    for (int i = 0; i <= 100; i++) {
+        const float x = i / 100.0f;
+        EXPECT_FLOAT_EQ(thrustLinearCurve(x, 0.0f), x);
+        EXPECT_FLOAT_EQ(thrustLinearCompensate(x, 0.0f), x);
+    }
+}
+
+TEST(ThrustLinearTest, MatchesBetaflight)
+{
+    // Betaflight 2026.6 pidApplyThrustLinearization() / pidCompensateThrustLinearization(), worked by hand
+    EXPECT_NEAR(thrustLinearCurve(0.3f, 0.5f), 0.426f, 1e-5f);
+    EXPECT_NEAR(thrustLinearCompensate(0.3f, 0.5f), 0.195f, 1e-5f);
+    EXPECT_NEAR(thrustLinearCurve(0.8f, 1.0f), 0.864f, 1e-5f);
+}
+
+TEST(ThrustLinearTest, CurveKeepsEndsAndOrder)
+{
+    for (int s = 0; s <= 10; s++) {
+        const float e = strength(s);
+        EXPECT_NEAR(thrustLinearCurve(0.0f, e), 0.0f, 1e-6f);
+        EXPECT_NEAR(thrustLinearCurve(1.0f, e), 1.0f, 1e-6f);
+        float previous = 0.0f;
+        for (int i = 1; i <= 1000; i++) {
+            const float u = i / 1000.0f;
+            const float out = thrustLinearCurve(u, e);
+            EXPECT_GE(out, previous) << "e " << e << " u " << u;
+            EXPECT_GE(out, u - 1e-6f) << "raises, never lowers: e " << e << " u " << u;
+            EXPECT_LE(out, 1.0f + 1e-6f) << "e " << e << " u " << u;
+            previous = out;
+        }
+    }
+}
+
+TEST(ThrustLinearTest, CompensationNeverNegative)
+{
+    for (int s = 0; s <= 10; s++) {
+        for (int i = 0; i <= 1000; i++) {
+            EXPECT_GE(thrustLinearCompensate(i / 1000.0f, strength(s)), 0.0f);
+        }
+    }
+}
+
+TEST(ThrustLinearTest, HoverStaysWhereItWas)
+{
+    // Worst cases near 15 % throttle: 0.81 % of the range at 50, 2.5 % at 70, 8.7 % at 100
+    for (int s = 0; s <= 10; s++) {
+        const float e = strength(s);
+        const float tolerance = e <= 0.5f ? 0.0085f : (e <= 0.7f ? 0.026f : 0.088f);
+        for (int i = 100; i <= 1000; i++) {
+            const float t = i / 1000.0f;
+            EXPECT_NEAR(thrustLinearCurve(thrustLinearCompensate(t, e), e), t, tolerance) << "e " << e << " t " << t;
+        }
+    }
+}