diff --git a/src/main/drivers/motor.c b/src/main/drivers/motor.c index fc2f11731b3..dd336ca8485 100644 --- a/src/main/drivers/motor.c +++ b/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; diff --git a/src/main/drivers/motor.h b/src/main/drivers/motor.h index 9ff075fe750..6dd2de92e2d 100644 --- a/src/main/drivers/motor.h +++ b/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); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 3c051cf9f38..9f3ba386dcc 100644 --- a/src/main/flight/mixer.c +++ b/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,11 +364,13 @@ 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); @@ -375,7 +378,7 @@ static void applyRpmLimiter(mixerRuntime_t *mixer) 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)); diff --git a/src/main/flight/mixer_init.c b/src/main/flight/mixer_init.c index bc98235c6ea..50d7c9edfbb 100644 --- a/src/main/flight/mixer_init.c +++ b/src/main/flight/mixer_init.c @@ -361,7 +361,8 @@ 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 } @@ -369,9 +370,10 @@ void mixerInitProfile(void) #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 diff --git a/src/main/flight/mixer_init.h b/src/main/flight/mixer_init.h index 4252bfac5e3..098ddc2e065 100644 --- a/src/main/flight/mixer_init.h +++ b/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;