From 26701f0638bae39d31cb787504199ebbea5a8737 Mon Sep 17 00:00:00 2001 From: Tanner Beard Date: Sun, 28 May 2023 16:18:16 -0400 Subject: [PATCH] RPM Limiter (#12054) --- src/main/build/debug.c | 1 + src/main/build/debug.h | 1 + src/main/cli/settings.c | 10 +++++- src/main/flight/mixer.c | 62 ++++++++++++++++++++++++++++++++---- src/main/flight/mixer.h | 15 ++++++++- src/main/flight/mixer_init.c | 55 ++++++++++++++++++++++++++------ src/main/flight/mixer_init.h | 9 +++++- src/main/pg/motor.c | 5 +-- src/main/pg/motor.h | 5 +-- src/main/sensors/battery.c | 7 ++-- src/main/sensors/voltage.h | 2 +- 11 files changed, 146 insertions(+), 26 deletions(-) diff --git a/src/main/build/debug.c b/src/main/build/debug.c index f6aba1db236..9fa64b37660 100644 --- a/src/main/build/debug.c +++ b/src/main/build/debug.c @@ -113,4 +113,5 @@ const char * const debugModeNames[DEBUG_COUNT] = { "ANGLE_TARGET", "CURRENT_ANGLE", "DSHOT_TELEMETRY_COUNTS", + "RPM_LIMIT", }; diff --git a/src/main/build/debug.h b/src/main/build/debug.h index e01dd1366ac..e6813175503 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -111,6 +111,7 @@ typedef enum { DEBUG_ANGLE_TARGET, DEBUG_CURRENT_ANGLE, DEBUG_DSHOT_TELEMETRY_COUNTS, + DEBUG_RPM_LIMIT, DEBUG_COUNT } debugType_e; diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index b0b95c7b8c1..d0595429175 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -837,6 +837,7 @@ const clivalue_t valueTable[] = { { "min_throttle", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { PWM_PULSE_MIN, PWM_PULSE_MAX }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, minthrottle) }, { "max_throttle", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { PWM_PULSE_MIN, PWM_PULSE_MAX }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, maxthrottle) }, { "min_command", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { PWM_PULSE_MIN, PWM_PULSE_MAX }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, mincommand) }, + { "motor_kv", VAR_UINT16 | HARDWARE_VALUE, .config.minmaxUnsigned = { 1, 40000 }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, kv) }, #ifdef USE_DSHOT { PARAM_NAME_DSHOT_IDLE_VALUE, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 2000 }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, digitalIdleOffsetValue) }, #ifdef USE_DSHOT_DMAR @@ -938,7 +939,14 @@ const clivalue_t valueTable[] = { { "yaw_motors_reversed", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, yaw_motors_reversed) }, { "mixer_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_MIXER_TYPE }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, mixer_type) }, { "crashflip_motor_percent", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, crashflip_motor_percent) }, - { "crashflip_expo", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, crashflip_expo) }, + { "crashflip_expo", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, crashflip_expo) }, +#ifdef USE_RPM_LIMIT + { "rpm_limit", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, rpm_limit) }, + { "rpm_limit_p", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, rpm_limit_p) }, + { "rpm_limit_i", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, rpm_limit_i) }, + { "rpm_limit_d", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, rpm_limit_d) }, + { "rpm_limit_value", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1, UINT16_MAX }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, rpm_limit_value) }, +#endif // PG_MOTOR_3D_CONFIG { "3d_deadband_low", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { PWM_PULSE_MIN, PWM_RANGE_MIDDLE }, PG_MOTOR_3D_CONFIG, offsetof(flight3DConfig_t, deadband3d_low) }, diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 2c0df52dce0..c27c790a204 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -18,8 +18,6 @@ * If not, see . */ -#include -#include #include #include #include @@ -67,6 +65,12 @@ #define DYN_LPF_THROTTLE_STEPS 100 #define DYN_LPF_THROTTLE_UPDATE_DELAY_US 5000 // minimum of 5ms between updates +#ifdef USE_RPM_LIMIT +#define RPM_LIMIT_ACTIVE mixerConfig()->rpm_limit +#else +#define RPM_LIMIT_ACTIVE false +#endif + static FAST_DATA_ZERO_INIT float motorMixRange; float FAST_DATA_ZERO_INIT motor[MAX_SUPPORTED_MOTORS]; @@ -218,7 +222,6 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs) } else { throttle = rcCommand[THROTTLE] - PWM_RANGE_MIN + throttleAngleCorrection; currentThrottleInputRange = PWM_RANGE; - #ifdef USE_DYN_IDLE if (mixerRuntime.dynIdleMinRps > 0.0f) { const float maxIncrease = isAirmodeActivated() @@ -347,6 +350,47 @@ static void applyFlipOverAfterCrashModeToMotors(void) } } +#ifdef USE_RPM_LIMIT +static void applyRpmLimiter(mixerRuntime_t *mixer) +{ + static float prevError = 0.0f; + static float i = 0.0f; + const float unsmoothedAverageRpm = getDshotAverageRpm(); + const float averageRpm = pt1FilterApply(&mixer->averageRpmFilter, unsmoothedAverageRpm); + const float error = averageRpm - mixer->rpmLimiterRpmLimit; + + // PID + const float p = error * mixer->rpmLimiterPGain; + const float d = (error - prevError) * mixer->rpmLimiterDGain; // rpmLimiterDGain already adjusted for looprate (see mixer_init.c) + i += error * mixer->rpmLimiterIGain; // rpmLimiterIGain already adjusted for looprate (see mixer_init.c) + i = MAX(0.0f, i); + float pidOutput = p + i + d; + + // Throttle limit learning + if (error > 0.0f && rcCommand[THROTTLE] < rxConfig()->maxcheck) { + mixer->rpmLimiterThrottleScale *= 1.0f - 4.8f * pidGetDT(); + } else if (pidOutput < -400 * pidGetDT() && rcCommand[THROTTLE] >= rxConfig()->maxcheck && !areMotorsSaturated()) { // Throttle accel corresponds with motor accel + mixer->rpmLimiterThrottleScale *= 1.0f + 3.2f * pidGetDT(); + } + mixer->rpmLimiterThrottleScale = constrainf(mixer->rpmLimiterThrottleScale, 0.01f, 1.0f); + throttle *= mixer->rpmLimiterThrottleScale; + + // Output + pidOutput = MAX(0.0f, pidOutput); + throttle = constrainf(throttle - pidOutput, 0.0f, 1.0f); + prevError = error; + + DEBUG_SET(DEBUG_RPM_LIMIT, 0, lrintf(averageRpm)); + DEBUG_SET(DEBUG_RPM_LIMIT, 1, lrintf(unsmoothedAverageRpm)); + DEBUG_SET(DEBUG_RPM_LIMIT, 2, lrintf(mixer->rpmLimiterThrottleScale * 100.0f)); + DEBUG_SET(DEBUG_RPM_LIMIT, 3, lrintf(throttle * 100.0f)); + DEBUG_SET(DEBUG_RPM_LIMIT, 4, lrintf(error)); + DEBUG_SET(DEBUG_RPM_LIMIT, 5, lrintf(p * 100.0f)); + DEBUG_SET(DEBUG_RPM_LIMIT, 6, lrintf(i * 100.0f)); + DEBUG_SET(DEBUG_RPM_LIMIT, 7, lrintf(d * 100.0f)); +} +#endif // USE_RPM_LIMIT + static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS], motorMixer_t *activeMixer) { // Now add in the desired throttle, but keep in a range that doesn't clip adjusted @@ -354,7 +398,9 @@ static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS], motorMixer_t for (int i = 0; i < mixerRuntime.motorCount; i++) { float motorOutput = motorOutputMixSign * motorMix[i] + throttle * activeMixer[i].throttle; #ifdef USE_THRUST_LINEARIZATION + if (!RPM_LIMIT_ACTIVE) { motorOutput = pidApplyThrustLinearization(motorOutput); + } #endif motorOutput = motorOutputMin + motorOutputRange * motorOutput; @@ -386,7 +432,7 @@ static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS], motorMixer_t static float applyThrottleLimit(float throttle) { - if (currentControlRateProfile->throttle_limit_percent < 100) { + if (currentControlRateProfile->throttle_limit_percent < 100 && !RPM_LIMIT_ACTIVE) { const float throttleLimitFactor = currentControlRateProfile->throttle_limit_percent / 100.0f; switch (currentControlRateProfile->throttle_limit_type) { case THROTTLE_LIMIT_TYPE_SCALE: @@ -499,7 +545,6 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs) if (isFlipOverAfterCrashActive()) { applyFlipOverAfterCrashModeToMotors(); - return; } @@ -573,12 +618,17 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs) throttle = pidCompensateThrustLinearization(throttle); #endif +#ifdef USE_RPM_LIMIT + if (RPM_LIMIT_ACTIVE && motorConfig()->dev.useDshotTelemetry && ARMING_FLAG(ARMED)) { + applyRpmLimiter(&mixerRuntime); + } +#endif + // Find roll/pitch/yaw desired output // ??? Where is the optimal location for this code? float motorMix[MAX_SUPPORTED_MOTORS]; float motorMixMax = 0, motorMixMin = 0; for (int i = 0; i < mixerRuntime.motorCount; i++) { - float mix = scaledAxisPidRoll * activeMixer[i].roll + scaledAxisPidPitch * activeMixer[i].pitch + diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 11a6befad86..b3e1a3cb056 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -20,10 +20,15 @@ #pragma once +#include +#include + #include "platform.h" #include "common/time.h" + #include "pg/pg.h" + #include "drivers/io_types.h" #include "drivers/pwm_output.h" @@ -91,6 +96,13 @@ typedef struct mixerConfig_s { uint8_t crashflip_motor_percent; uint8_t crashflip_expo; uint8_t mixer_type; +#ifdef USE_RPM_LIMIT + bool rpm_limit; + uint16_t rpm_limit_p; + uint16_t rpm_limit_i; + uint16_t rpm_limit_d; + uint16_t rpm_limit_value; +#endif } mixerConfig_t; PG_DECLARE(mixerConfig_t, mixerConfig); @@ -105,12 +117,13 @@ struct rxConfig_s; uint8_t getMotorCount(void); float getMotorMixRange(void); bool areMotorsRunning(void); +bool areMotorsSaturated(void); void mixerLoadMix(int index, motorMixer_t *customMixers); void initEscEndpoints(void); void mixerInit(mixerMode_e mixerMode); void mixerInitProfile(void); - +void mixerResetRpmLimiter(void); void mixerResetDisarmedMotors(void); void mixTable(timeUs_t currentTimeUs); void stopMotors(void); diff --git a/src/main/flight/mixer_init.c b/src/main/flight/mixer_init.c index ec2d4f8948d..40eb5623289 100644 --- a/src/main/flight/mixer_init.c +++ b/src/main/flight/mixer_init.c @@ -27,6 +27,8 @@ #include "build/build_config.h" #include "build/debug.h" +#include "common/maths.h" + #include "config/config.h" #include "config/feature.h" @@ -44,15 +46,23 @@ #include "mixer_init.h" -PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 0); +PG_REGISTER_WITH_RESET_FN(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 1); -PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig, - .mixerMode = DEFAULT_MIXER, - .yaw_motors_reversed = false, - .crashflip_motor_percent = 0, - .crashflip_expo = 35, - .mixer_type = MIXER_LEGACY, -); +void pgResetFn_mixerConfig(mixerConfig_t *mixerConfig) +{ + mixerConfig->mixerMode = DEFAULT_MIXER; + mixerConfig->yaw_motors_reversed = false; + mixerConfig->crashflip_motor_percent = 0; + mixerConfig->crashflip_expo = 35; + mixerConfig->mixer_type = MIXER_LEGACY; +#ifdef USE_RPM_LIMIT + mixerConfig->rpm_limit = false; + mixerConfig->rpm_limit_p = 25; + mixerConfig->rpm_limit_i = 10; + mixerConfig->rpm_limit_d = 8; + mixerConfig->rpm_limit_value = 18000; +#endif +} PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, customMotorMixer, PG_MOTOR_MIXER, 0); @@ -277,15 +287,23 @@ bool areMotorsRunning(void) for (int i = 0; i < mixerRuntime.motorCount; i++) { if (motor_disarmed[i] != mixerRuntime.disarmMotorOutput) { motorsRunning = true; - break; } } } - return motorsRunning; } +bool areMotorsSaturated(void) +{ + for (int i = 0; i < getMotorCount(); i++) { + if (motor[i] >= motorConfig()->maxthrottle) { + return true; + } + } + return false; +} + #ifdef USE_SERVOS bool mixerIsTricopter(void) { @@ -336,7 +354,24 @@ void mixerInitProfile(void) } } #endif + +#ifdef USE_RPM_LIMIT + mixerRuntime.rpmLimiterRpmLimit = mixerConfig()->rpm_limit_value; + mixerRuntime.rpmLimiterPGain = mixerConfig()->rpm_limit_p * 15e-6f; + mixerRuntime.rpmLimiterIGain = mixerConfig()->rpm_limit_i * 1e-3f * pidGetDT(); + mixerRuntime.rpmLimiterDGain = mixerConfig()->rpm_limit_d * 3e-7f * pidGetPidFrequency(); + pt1FilterInit(&mixerRuntime.averageRpmFilter, pt1FilterGain(6.0f, pidGetDT())); + mixerResetRpmLimiter(); +#endif +} + +#ifdef USE_RPM_LIMIT +void mixerResetRpmLimiter(void) +{ + const float maxExpectedRpm = MAX(1.0f, motorConfig()->kv * getBatteryVoltage() * 0.01f); + mixerRuntime.rpmLimiterThrottleScale = constrainf(mixerRuntime.rpmLimiterRpmLimit / maxExpectedRpm, 0.0f, 1.0f); } +#endif // USE_RPM_LIMIT #ifdef USE_LAUNCH_CONTROL // Create a custom mixer for launch control based on the current settings diff --git a/src/main/flight/mixer_init.h b/src/main/flight/mixer_init.h index 3cf4a331ded..4252bfac5e3 100644 --- a/src/main/flight/mixer_init.h +++ b/src/main/flight/mixer_init.h @@ -24,7 +24,6 @@ #include "flight/mixer.h" - typedef struct mixerRuntime_s { uint8_t motorCount; motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS]; @@ -54,6 +53,14 @@ typedef struct mixerRuntime_s { float vbatFull; float vbatRangeToCompensate; #endif +#if defined(USE_RPM_LIMIT) + float rpmLimiterRpmLimit; + float rpmLimiterThrottleScale; + float rpmLimiterPGain; + float rpmLimiterIGain; + float rpmLimiterDGain; + pt1Filter_t averageRpmFilter; +#endif } mixerRuntime_t; extern mixerRuntime_t mixerRuntime; diff --git a/src/main/pg/motor.c b/src/main/pg/motor.c index 611ef73cc10..10d341c7394 100644 --- a/src/main/pg/motor.c +++ b/src/main/pg/motor.c @@ -44,7 +44,7 @@ #define DEFAULT_DSHOT_BURST DSHOT_DMAR_OFF #endif -PG_REGISTER_WITH_RESET_FN(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 1); +PG_REGISTER_WITH_RESET_FN(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 2); void pgResetFn_motorConfig(motorConfig_t *motorConfig) { @@ -68,6 +68,7 @@ void pgResetFn_motorConfig(motorConfig_t *motorConfig) motorConfig->maxthrottle = 2000; motorConfig->mincommand = 1000; motorConfig->digitalIdleOffsetValue = 550; + motorConfig->kv = 1960; #ifdef USE_TIMER #ifdef MOTOR1_PIN @@ -96,7 +97,7 @@ void pgResetFn_motorConfig(motorConfig_t *motorConfig) #endif #endif - motorConfig->motorPoleCount = 14; // Most brushes motors that we use are 14 poles + motorConfig->motorPoleCount = 14; // Most brushless motors that we use are 14 poles for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) { motorConfig->dev.motorOutputReordering[i] = i; diff --git a/src/main/pg/motor.h b/src/main/pg/motor.h index a2ea0fb5515..8c55a87b848 100644 --- a/src/main/pg/motor.h +++ b/src/main/pg/motor.h @@ -56,9 +56,10 @@ typedef struct motorConfig_s { motorDevConfig_t dev; uint16_t digitalIdleOffsetValue; // Idle value for DShot protocol, full motor output = 10000 uint16_t minthrottle; // Set the minimum throttle command sent to the ESC (Electronic Speed Controller). This is the minimum value that allow motors to run at a idle speed. - uint16_t maxthrottle; // This is the maximum value for the ESCs at full power this value can be increased up to 2000 + uint16_t maxthrottle; // This is the maximum value for the ESCs at full power. This value can be increased up to 2000 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 - uint8_t motorPoleCount; // Magnetic poles in the motors for calculating actual RPM from eRPM provided by ESC telemetry + uint16_t kv; // Motor velocity constant (Kv) to estimate RPM under no load (unloadedRpm = Kv * batteryVoltage) + uint8_t motorPoleCount; // Number of magnetic poles in the motor bell for calculating actual RPM from eRPM provided by ESC telemetry } motorConfig_t; PG_DECLARE(motorConfig_t, motorConfig); diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 15d5f2465de..211210f3e50 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -40,6 +40,7 @@ #include "fc/rc_controls.h" #include "flight/mixer.h" +#include "flight/mixer_init.h" #include "io/beeper.h" @@ -198,7 +199,6 @@ static bool isVoltageFromBat(void) void batteryUpdatePresence(void) { - if ((voltageState == BATTERY_NOT_PRESENT || voltageState == BATTERY_INIT) && isVoltageFromBat() && isVoltageStable()) { // Battery has just been connected - calculate cells, warning voltages and reset state @@ -217,6 +217,9 @@ void batteryUpdatePresence(void) changePidProfileFromCellCount(batteryCellCount); } } +#ifdef USE_RPM_LIMIT + mixerResetRpmLimiter(); +#endif batteryWarningVoltage = batteryCellCount * batteryConfig()->vbatwarningcellvoltage; batteryCriticalVoltage = batteryCellCount * batteryConfig()->vbatmincellvoltage; batteryWarningHysteresisVoltage = (batteryWarningVoltage > batteryConfig()->vbathysteresis) ? batteryWarningVoltage - batteryConfig()->vbathysteresis : 0; @@ -237,7 +240,7 @@ void batteryUpdatePresence(void) } } -void batteryUpdateWhDrawn(void) +void batteryUpdateWhDrawn(void) { static int32_t mAhDrawnPrev = 0; const int32_t mAhDrawnCurrent = getMAhDrawn(); diff --git a/src/main/sensors/voltage.h b/src/main/sensors/voltage.h index d0edd0b56d0..b1f1f6c4c2c 100644 --- a/src/main/sensors/voltage.h +++ b/src/main/sensors/voltage.h @@ -41,7 +41,7 @@ extern const char * const voltageMeterSourceNames[VOLTAGE_METER_COUNT]; // WARNING - do not mix usage of VOLTAGE_METER_* and VOLTAGE_SENSOR_*, they are separate concerns. typedef struct voltageMeter_s { - uint16_t displayFiltered; // voltage in 0.01V steps + uint16_t displayFiltered; // voltage in 0.01V steps uint16_t unfiltered; // voltage in 0.01V steps #if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION) uint16_t sagFiltered; // voltage in 0.01V steps