diff --git a/docs/Settings.md b/docs/Settings.md index cbc526adb33..b22806d6843 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 + +Limits throttle output rate of change. Setting defines minimum time in milliseconds for throttle to change by 1000us (min to max throttle range). Set to 0 to disable. Fixed wing only. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 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..e075ff0dcc2 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: "Limits throttle output rate of change. Setting defines minimum time in milliseconds for throttle to change by 1000us (min to max throttle range). Set to 0 to disable. Fixed wing only." + default_value: 0 + field: motor.throttle_rate_limiter + min: 0 + 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..f0a8c4943eb 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.throttle_rate_limiter > 0) { + throttleRateLimit = (PWM_RANGE_MAX - PWM_RANGE_MIN) / MS2S(currentBatteryProfile->motor.throttle_rate_limiter); + } } void mixerResetDisarmedMotors(void) @@ -486,8 +491,10 @@ 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 +512,7 @@ void FAST_CODE mixTable(void) motor[i] = isDisarmed ? motor_disarmed[i] : motorValueWhenStopped; } mixerThrottleCommand = motor[0]; + lastMixerThrottleCommand = mixerThrottleCommand; return; } @@ -607,6 +615,17 @@ 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; + + if (fabsf(throttleRate) > throttleRateLimit) { + lastMixerThrottleCommand = lastMixerThrottleCommand + SIGN(throttleRate) * throttleRateLimit * dT; + mixerThrottleCommand = lastMixerThrottleCommand; + } + } + #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..f35da2e57f3 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, + .throttle_rate_limiter = 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..b66c2f4329b 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. + uint16_t throttle_rate_limiter; // 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 {