Rpm limiter initial throttle cap prediction fix (betaflight#13039)

* initial commit

* initial commit

* Update src/main/flight/mixer_init.c

Co-authored-by: Jan Post <[email protected]>

* rpm limiter vbat compensation

* remove forced define

* change to multiply

* rpm limiter vbat compensation method 2

* fixes

* remove def

* test

* test

* test

* cleanup

* fix for maxcheck condition not satisfied if set to 2000

* Refactoring / cleanup

* Rename averageRpmFilter

* missing paren

* Move RPM estimation to motor.h

* Add macro for deadband

---------

Co-authored-by: Jan Post <[email protected]>

src/main/drivers/motor.c

@@ -33,13 +33,15 @@
 #include "config/feature.h"
 
 #include "drivers/dshot.h" // for DSHOT_ constants in initEscEndpoints; may be gone in the future
-#include "drivers/pwm_output.h" // for PWM_TYPE_* and others
-#include "drivers/time.h"
 #include "drivers/dshot_bitbang.h"
 #include "drivers/dshot_dpwm.h"
+#include "drivers/pwm_output.h" // for PWM_TYPE_* and others
+#include "drivers/time.h"
 
 #include "fc/rc_controls.h" // for flight3DConfig_t
 
+#include "sensors/battery.h"
+
 #include "motor.h"
 
 static FAST_DATA_ZERO_INIT motorDevice_t *motorDevice;
@@ -343,6 +345,16 @@ void motorEnable(void)
     }
 }
 
+float motorEstimateMaxRpm(void)
+{
+    // Empirical testing found this relationship between estimated max RPM without props attached
+    // (unloaded) and measured max RPM with props attached (loaded), independent from prop size
+    float unloadedMaxRpm = 0.01f * getBatteryVoltage() * motorConfig()->kv;
+    float loadDerating = -5.44e-6f * unloadedMaxRpm + 0.944f;
+
+    return unloadedMaxRpm * loadDerating;
+}
+
 bool motorIsEnabled(void)
 {
     return motorDevice->enabled;

src/main/drivers/motor.h

@@ -93,6 +93,7 @@ bool isMotorProtocolEnabled(void);
 
 void motorDisable(void);
 void motorEnable(void);
+float motorEstimateMaxRpm(void);
 bool motorIsEnabled(void);
 bool motorIsMotorEnabled(uint8_t index);
 timeMs_t motorGetMotorEnableTimeMs(void);

src/main/flight/mixer.c

@@ -345,12 +345,13 @@ static void applyFlipOverAfterCrashModeToMotors(void)
 }
 
 #ifdef USE_RPM_LIMIT
+#define STICK_HIGH_DEADBAND 5    // deadband to make sure throttle cap can raise, even with maxcheck set around 2000
 static void applyRpmLimiter(mixerRuntime_t *mixer)
 {
     static float prevError = 0.0f;
     static float i = 0.0f;
     const float unsmoothedAverageRpm = getDshotRpmAverage();
-    const float averageRpm = pt1FilterApply(&mixer->averageRpmFilter, unsmoothedAverageRpm);
+    const float averageRpm = pt1FilterApply(&mixer->rpmLimiterAverageRpmFilter, unsmoothedAverageRpm);
     const float error = averageRpm - mixer->rpmLimiterRpmLimit;
 
     // PID
@@ -363,19 +364,21 @@ static void applyRpmLimiter(mixerRuntime_t *mixer)
     // 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
+    } else if (pidOutput < -400.0f * pidGetDT() && lrintf(rcCommand[THROTTLE]) >= rxConfig()->maxcheck - STICK_HIGH_DEADBAND && !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;
+
+    float rpmLimiterThrottleScaleOffset = pt1FilterApply(&mixer->rpmLimiterThrottleScaleOffsetFilter, constrainf(mixer->rpmLimiterRpmLimit / motorEstimateMaxRpm(), 0.0f, 1.0f) - mixer->rpmLimiterInitialThrottleScale);
+    throttle *= constrainf(mixer->rpmLimiterThrottleScale + rpmLimiterThrottleScaleOffset, 0.0f, 1.0f);
 
     // 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, 1, lrintf(rpmLimiterThrottleScaleOffset * 100.0f));
     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));

src/main/flight/mixer_init.c

@@ -361,17 +361,19 @@ void mixerInitProfile(void)
     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()));
+    pt1FilterInit(&mixerRuntime.rpmLimiterAverageRpmFilter, pt1FilterGain(6.0f, pidGetDT()));
+    pt1FilterInit(&mixerRuntime.rpmLimiterThrottleScaleOffsetFilter, pt1FilterGain(2.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);
+    mixerRuntime.rpmLimiterThrottleScale = constrainf(mixerRuntime.rpmLimiterRpmLimit / motorEstimateMaxRpm(), 0.0f, 1.0f);
+    mixerRuntime.rpmLimiterInitialThrottleScale = mixerRuntime.rpmLimiterThrottleScale;
 }
+
 #endif // USE_RPM_LIMIT
 
 #ifdef USE_LAUNCH_CONTROL

src/main/flight/mixer_init.h

@@ -56,10 +56,12 @@ typedef struct mixerRuntime_s {
 #if defined(USE_RPM_LIMIT)
     float rpmLimiterRpmLimit;
     float rpmLimiterThrottleScale;
+    float rpmLimiterInitialThrottleScale;
     float rpmLimiterPGain;
     float rpmLimiterIGain;
     float rpmLimiterDGain;
-    pt1Filter_t averageRpmFilter;
+    pt1Filter_t rpmLimiterAverageRpmFilter;
+    pt1Filter_t rpmLimiterThrottleScaleOffsetFilter;
 #endif
 } mixerRuntime_t;