Fixed wing throttle rate limiter

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.

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);

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

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) {

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);

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

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 {