diff --git a/docs/Settings.md b/docs/Settings.md index a0bdc187935..daa39da2053 100644 --- a/docs/Settings.md +++ b/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. diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index fc6b8c11d3b..a8f051f2de6 100644 --- a/src/main/blackbox/blackbox.c +++ b/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()); diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 142f5f06c9b..1ed349ccc1d 100644 --- a/src/main/fc/settings.yaml +++ b/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 diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 2dccb22e3b2..40a64b1430c 100644 --- a/src/main/flight/mixer.c +++ b/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; diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 43c31c9132d..450174eec7e 100644 --- a/src/main/flight/mixer.h +++ b/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);