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
+
+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.

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: "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

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

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,
+                .throttle_rate_limiter = 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.
+        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 {