diff --git a/docs/Settings.md b/docs/Settings.md index ded7825f65c..c8eb8bb0716 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -1670,6 +1670,16 @@ Reference airspeed. Set this to airspeed at which PIDs were tuned. Usually shoul --- +### fw_throttle_rate_limiter + +Minimum time allowed for throttle to increase from minimum to maximum throttle (1000 to 2000) in milliseconds. Negative values limit decreasing as well as increasing throttle. Positive values only limit increasing throttle. Set to 0 to disable. Fixed wing only. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | -5000 | 5000 | + +--- + ### fw_tpa_time_constant TPA smoothing and delay time constant to reflect non-instant speed/throttle response of the plane. See **PID Attenuation and scaling** Wiki for full details. diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 3223aca497e..d25e04fce3d 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -925,7 +925,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs) { cycleTime = getTaskDeltaTime(TASK_SELF); - dT = (float)cycleTime * 0.000001f; + dT = US2S(cycleTime); bool fwLaunchIsActive = STATE(AIRPLANE) && isNavLaunchEnabled() && armTime == 0; @@ -992,7 +992,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs) // Calculate stabilisation pidController(dT); - mixTable(); + mixTable(dT); if (isMixerUsingServos()) { servoMixer(dT); diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 7d2e9c01385..74ad48e8d58 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1177,6 +1177,12 @@ groups: field: nav.fw.launch_idle_throttle min: 1000 max: 2000 + - name: fw_throttle_rate_limiter + description: "Minimum time allowed for throttle to increase from minimum to maximum throttle (1000 to 2000) in milliseconds. Negative values limit decreasing as well as increasing throttle. Positive values only limit increasing throttle. Set to 0 to disable. Fixed wing only." + default_value: 0 + field: motor.throttleRateLimiter + min: -5000 + max: 5000 - name: fw_auto_speed_level_min_thr description: "Minimum throttle for LEVEL flight in auto speed mode when using ground speed as speed reference [us]. This should be set with just enough margin to avoid stalling. Throttle can go lower than this setting when pitching down to a minimum of nav_fw_min_thr." default_value: 1300 diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 27f1ec7ffe2..8cd06682c9a 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -75,6 +75,7 @@ static EXTENDED_FASTRAM int throttleDeadbandHigh = 0; static EXTENDED_FASTRAM int throttleRangeMin = 0; static EXTENDED_FASTRAM int throttleRangeMax = 0; static EXTENDED_FASTRAM int8_t motorYawMultiplier = 1; +static EXTENDED_FASTRAM float throttleRateLimit = 0.0f; int motorZeroCommand = 0; @@ -235,6 +236,10 @@ void mixerInit(void) } else { motorYawMultiplier = 1; } + + if (currentBatteryProfile->motor.throttleRateLimiter) { + throttleRateLimit = (PWM_RANGE_MAX - PWM_RANGE_MIN) / MS2S(currentBatteryProfile->motor.throttleRateLimiter); + } } void mixerResetDisarmedMotors(void) @@ -486,8 +491,9 @@ static int getReversibleMotorsThrottleDeadband(void) return ifMotorstopFeatureEnabled() ? reversibleMotorsConfig()->neutral : directionValue; } -void FAST_CODE mixTable(void) +void FAST_CODE mixTable(float dT) { + static float lastMixerThrottleCommand = 1000.0f; #ifdef USE_DSHOT if (FLIGHT_MODE(TURTLE_MODE)) { applyTurtleModeToMotors(); @@ -505,6 +511,7 @@ void FAST_CODE mixTable(void) motor[i] = isDisarmed ? motor_disarmed[i] : motorValueWhenStopped; } mixerThrottleCommand = motor[0]; + lastMixerThrottleCommand = mixerThrottleCommand; return; } @@ -607,6 +614,26 @@ void FAST_CODE mixTable(void) throttleMax = throttleRangeMax; throttleRange = throttleMax - throttleMin; + // FW throttle rate limiter + if (STATE(AIRPLANE) && throttleRateLimit) { + const float deltaThrottle = mixerThrottleCommand - lastMixerThrottleCommand; + const float throttleRate = deltaThrottle / dT; + bool limitOutput = false; + + if (throttleRateLimit < 0.0f) { + limitOutput = fabsf(throttleRate) > -throttleRateLimit; + } else if (throttleRate > throttleRateLimit) { + limitOutput = true; + } + + if (limitOutput) { + lastMixerThrottleCommand += SIGN(throttleRate) * fabsf(throttleRateLimit) * dT; + mixerThrottleCommand = lastMixerThrottleCommand; + } else { + lastMixerThrottleCommand = mixerThrottleCommand; + } + } + #define THROTTLE_CLIPPING_FACTOR 0.33f motorMixRange = (float)rpyMixRange / (float)throttleRange; if (motorMixRange > 1.0f) { diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 5f9caf165e6..cc26e45a623 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -121,7 +121,7 @@ void writeAllMotors(int16_t mc); void mixerInit(void); void mixerUpdateStateFlags(void); void mixerResetDisarmedMotors(void); -void mixTable(void); +void mixTable(float dT); void writeMotors(void); void processServoAutotrim(const float dT); void processServoAutotrimMode(void); diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index dc9d0ec270a..eb8eb1f5852 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -108,7 +108,7 @@ static pt1Filter_t amperageFilterState; batteryState_e batteryState; const batteryProfile_t *currentBatteryProfile; -PG_REGISTER_ARRAY_WITH_RESET_FN(batteryProfile_t, MAX_BATTERY_PROFILE_COUNT, batteryProfiles, PG_BATTERY_PROFILES, 3); +PG_REGISTER_ARRAY_WITH_RESET_FN(batteryProfile_t, MAX_BATTERY_PROFILE_COUNT, batteryProfiles, PG_BATTERY_PROFILES, 4); void pgResetFn_batteryProfiles(batteryProfile_t *instance) { @@ -136,6 +136,7 @@ void pgResetFn_batteryProfiles(batteryProfile_t *instance) .motor = { .throttleIdle = SETTING_THROTTLE_IDLE_DEFAULT, .throttleScale = SETTING_THROTTLE_SCALE_DEFAULT, + .throttleRateLimiter = SETTING_FW_THROTTLE_RATE_LIMITER_DEFAULT, // 100 millis #ifdef USE_DSHOT .turtleModePowerFactor = SETTING_TURTLE_MODE_POWER_FACTOR_DEFAULT, #endif diff --git a/src/main/sensors/battery_config_structs.h b/src/main/sensors/battery_config_structs.h index 07b549a02eb..42e3c540fd9 100644 --- a/src/main/sensors/battery_config_structs.h +++ b/src/main/sensors/battery_config_structs.h @@ -109,12 +109,13 @@ typedef struct batteryProfile_s { struct { float throttleIdle; // Throttle IDLE value based on min_command, max_throttle, in percent float throttleScale; // Scaling factor for throttle. + int16_t throttleRateLimiter; // Min time in millis for fixed wing throttle to go from min to max #ifdef USE_DSHOT uint8_t turtleModePowerFactor; // Power factor from 0 to 100% of flip over after crash #endif } motor; - uint16_t failsafe_throttle; // Throttle level used for landing - specify value between 1000..2000 (pwm pulse width for slightly below hover). center throttle = 1500. + uint16_t failsafe_throttle; // Throttle level used for landing - slightly below hover for MC, probably motor off for FW. struct { struct {