mixer: throttle boost, 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.

src/main/blackbox/blackbox.c

@@ -2153,6 +2153,8 @@ 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("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,18 @@ 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: PG_FAILSAFE_CONFIG
     type: failsafeConfig_t

src/main/flight/mixer.c

@@ -101,6 +101,8 @@ 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,
 );
 PG_REGISTER_ARRAY_WITH_RESET_FN(timerOverride_t, HARDWARE_TIMER_DEFINITION_COUNT, timerOverrides, PG_TIMER_OVERRIDE_CONFIG, 0);
 
@@ -655,6 +657,49 @@ 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);
+}
+
+// Out of line: mixTable() is FAST_CODE and ITCM is nearly full on some F7 targets
+static NOINLINE void updateMotorThrottle(float dT)
+{
+    motorThrottleCommand = applyThrottleBoost(mixerThrottleCommand, dT);
+}
+
 void FAST_CODE mixTable(float dT)
 {
     static float lastMixerThrottleCommand = 1000.0f;
@@ -827,6 +872,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 +898,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;

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,8 @@ 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
 } motorConfig_t;
 
 PG_DECLARE(motorConfig_t, motorConfig);