diff --git a/docs/Settings.md b/docs/Settings.md index cbc526adb33..98e7d719dda 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -1620,6 +1620,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 15a0c0fff4f..5df076f4f1c 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: limit_cont_current description: "Continous current limit (dA), set to 0 to disable" condition: USE_POWER_LIMITS diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index a80992b772d..c6648202ed3 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 6c4370d4176..a18796d7b59 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -120,7 +120,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 da8a6237bd0..8ca7b268afc 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 8fe49295f8e..4b634da0a89 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 {