Throttle rate limiting

src/main/fc/fc_core.c

@@ -803,7 +803,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
  }
 #endif
 
- mixTable();
+ mixTable(dT);
 
 #ifdef USE_SERVOS
  if (isMixerUsingServos()) {

src/main/fc/settings.yaml

@@ -403,6 +403,14 @@ groups:
  field: motorPwmRate
  min: 50
  max: 32000
+ - name: motor_accel_time
+ field: motorAccelTimeMs
+ min: 0
+ max: 1000
+ - name: motor_decel_time
+ field: motorDecelTimeMs
+ min: 0
+ max: 1000
  - name: motor_pwm_protocol
  field: motorPwmProtocol
  table: motor_pwm_protocol

src/main/flight/mixer.c

@@ -87,14 +87,16 @@ PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig,
 #define DEFAULT_MIN_THROTTLE 1150
 #endif
 
-PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 0);
+PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 1);
 
 PG_RESET_TEMPLATE(motorConfig_t, motorConfig,
  .minthrottle = DEFAULT_MIN_THROTTLE,
  .motorPwmProtocol = DEFAULT_PWM_PROTOCOL,
  .motorPwmRate = DEFAULT_PWM_RATE,
  .maxthrottle = 1850,
- .mincommand = 1000
+ .mincommand = 1000,
+ .motorAccelTimeMs = 0,
+ .motorDecelTimeMs = 0
 );
 
 static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
@@ -457,7 +459,45 @@ void stopPwmAllMotors(void)
  pwmShutdownPulsesForAllMotors(motorCount);
 }
 
-void mixTable(void)
+static void applyMotorRateLimiting(const float dT)
+{
+ static float motorPrevious[MAX_SUPPORTED_MOTORS] = { 0 };
+
+ if (feature(FEATURE_3D)) {
+ // FIXME: Don't apply rate limiting in 3D mode
+ for (int i = 0; i < motorCount; i++) {
+ motorPrevious[i] = motor[i];
+ }
+ }
+ else {
+ // Calculate max motor step
+ const uint16_t motorRange = motorConfig()->maxthrottle - motorConfig()->minthrottle;
+ const float motorMaxInc = (motorConfig()->motorAccelTimeMs == 0) ? 2000 : motorRange * dT / (motorConfig()->motorAccelTimeMs * 1e-3f);
+ const float motorMaxDec = (motorConfig()->motorDecelTimeMs == 0) ? 2000 : motorRange * dT / (motorConfig()->motorDecelTimeMs * 1e-3f);
+
+ for (int i = 0; i < motorCount; i++) {
+ // Apply motor rate limiting
+ motorPrevious[i] = constrainf(motor[i], motorPrevious[i] - motorMaxDec, motorPrevious[i] + motorMaxInc);
+
+ // Handle throttle below min_throttle (motor start/stop)
+ if (motorPrevious[i] < motorConfig()->minthrottle) {
+ if (motor[i] < motorConfig()->minthrottle) {
+ motorPrevious[i] = motor[i];
+ }
+ else {
+ motorPrevious[i] = motorConfig()->minthrottle;
+ }
+ }
+ }
+ }
+
+ // Update motor values
+ for (int i = 0; i < motorCount; i++) {
+ motor[i] = motorPrevious[i];
+ }
+}
+
+void mixTable(const float dT)
 {
  int16_t input[3]; // RPY, range [-500:+500]
  // Allow direct stick input to motors in passthrough mode on airplanes
@@ -582,4 +622,7 @@ void mixTable(void)
  motor[i] = motor_disarmed[i];
  }
  }
+
+ /* Apply motor acceleration/deceleration limit */
+ applyMotorRateLimiting(dT);
 }

src/main/flight/mixer.h

@@ -111,6 +111,8 @@ typedef struct motorConfig_s {
  uint16_t mincommand; // This is the value for the ESCs when they are not armed. In some cases, this value must be lowered down to 900 for some specific ESCs
  uint16_t motorPwmRate; // The update rate of motor outputs (50-498Hz)
  uint8_t motorPwmProtocol;
+ uint16_t motorAccelTimeMs; // Time limit for motor to accelerate from 0 to 100% throttle [ms]
+ uint16_t motorDecelTimeMs; // Time limit for motor to decelerate from 0 to 100% throttle [ms]
 } motorConfig_t;
 
 PG_DECLARE(motorConfig_t, motorConfig);
@@ -128,7 +130,7 @@ void mixerLoadMix(int index, motorMixer_t *customMixers);
 void mixerUsePWMIOConfiguration(void);
 void mixerUpdateStateFlags(void);
 void mixerResetDisarmedMotors(void);
-void mixTable(void);
+void mixTable(const float dT);
 void writeMotors(void);
 void processServoTilt(void);
 void processServoAutotrim(void);