From 190091f314a256d246fac4820cc60e1f3c1b31af Mon Sep 17 00:00:00 2001 From: "Konstantin Sharlaimov (DigitalEntity)" Date: Sat, 3 Mar 2018 20:37:25 +1000 Subject: [PATCH 1/2] Initial cut on throttle rate of change limiting --- src/main/fc/fc_core.c | 2 +- src/main/flight/mixer.c | 47 +++++++++++++++++++++++++++++++++++++++-- src/main/flight/mixer.h | 4 +++- 3 files changed, 49 insertions(+), 4 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 9723e515091..52f0805c8e2 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -803,7 +803,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs) } #endif - mixTable(); + mixTable(dT); #ifdef USE_SERVOS if (isMixerUsingServos()) { diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 45ed644abf3..1d0404fd0ab 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -94,7 +94,9 @@ PG_RESET_TEMPLATE(motorConfig_t, motorConfig, .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); } diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index d8daae74f8a..31a328d2747 100644 --- a/src/main/flight/mixer.h +++ b/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); From 6d8f231bfd7750729506cffe289557b80f96cd24 Mon Sep 17 00:00:00 2001 From: "Konstantin Sharlaimov (DigitalEntity)" Date: Tue, 22 May 2018 18:35:31 +1000 Subject: [PATCH 2/2] Settings --- src/main/fc/settings.yaml | 8 ++++++++ src/main/flight/mixer.c | 2 +- 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index d81aa2226ba..789508e5326 100644 --- a/src/main/fc/settings.yaml +++ b/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 diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 1d0404fd0ab..3625bb323e5 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -87,7 +87,7 @@ 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,