RPM Limiter (#12054)

src/main/build/debug.c

@@ -113,4 +113,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
     "ANGLE_TARGET",
     "CURRENT_ANGLE",
     "DSHOT_TELEMETRY_COUNTS",
+    "RPM_LIMIT",
 };

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;
 

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) },

src/main/flight/mixer.c

@@ -18,8 +18,6 @@
  * If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include <stdbool.h>
-#include <stdint.h>
 #include <stdlib.h>
 #include <math.h>
 #include <float.h>
@@ -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,14 +350,57 @@ 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
     // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips.
     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 +

src/main/flight/mixer.h

@@ -20,10 +20,15 @@
 
 #pragma once
 
+#include <stdbool.h>
+#include <stdint.h>
+
 #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);

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

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;

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;

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

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