diff --git a/src/main/fc/core.c b/src/main/fc/core.c index 2e7719fcd3..e46e1d3666 100644 --- a/src/main/fc/core.c +++ b/src/main/fc/core.c @@ -550,6 +550,9 @@ void tryArm(void) #ifdef USE_OSD osdSuppressStats(false); +#endif +#ifdef USE_RPM_LIMIT + mixerResetRpmLimiter(); #endif ENABLE_ARMING_FLAG(ARMED); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 524b2de081..fe358c1960 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -348,7 +348,6 @@ static void applyFlipOverAfterCrashModeToMotors(void) 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->rpmLimiterAverageRpmFilter, unsmoothedAverageRpm); const float error = averageRpm - mixer->rpmLimiterRpmLimit; @@ -356,9 +355,9 @@ static void applyRpmLimiter(mixerRuntime_t *mixer) // 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; + mixer->rpmLimiterI += error * mixer->rpmLimiterIGain; // rpmLimiterIGain already adjusted for looprate (see mixer_init.c) + mixer->rpmLimiterI = MAX(0.0f, mixer->rpmLimiterI); + float pidOutput = p + mixer->rpmLimiterI + d; // Throttle limit learning if (error > 0.0f && rcCommand[THROTTLE] < rxConfig()->maxcheck) { @@ -382,7 +381,7 @@ static void applyRpmLimiter(mixerRuntime_t *mixer) 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, 6, lrintf(mixer->rpmLimiterI * 100.0f)); DEBUG_SET(DEBUG_RPM_LIMIT, 7, lrintf(d * 100.0f)); } #endif // USE_RPM_LIMIT diff --git a/src/main/flight/mixer_init.c b/src/main/flight/mixer_init.c index a1edc86eea..328b0eadc8 100644 --- a/src/main/flight/mixer_init.c +++ b/src/main/flight/mixer_init.c @@ -361,6 +361,7 @@ 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(); + mixerRuntime.rpmLimiterI = 0.0; pt1FilterInit(&mixerRuntime.rpmLimiterAverageRpmFilter, pt1FilterGain(6.0f, pidGetDT())); pt1FilterInit(&mixerRuntime.rpmLimiterThrottleScaleOffsetFilter, pt1FilterGain(2.0f, pidGetDT())); mixerResetRpmLimiter(); @@ -373,6 +374,7 @@ void mixerInitProfile(void) #ifdef USE_RPM_LIMIT void mixerResetRpmLimiter(void) { + mixerRuntime.rpmLimiterI = 0.0; mixerRuntime.rpmLimiterThrottleScale = constrainf(mixerRuntime.rpmLimiterRpmLimit / motorEstimateMaxRpm(), 0.0f, 1.0f); mixerRuntime.rpmLimiterInitialThrottleScale = mixerRuntime.rpmLimiterThrottleScale; } diff --git a/src/main/flight/mixer_init.h b/src/main/flight/mixer_init.h index 1d2c557772..5b6303d0ab 100644 --- a/src/main/flight/mixer_init.h +++ b/src/main/flight/mixer_init.h @@ -60,6 +60,7 @@ typedef struct mixerRuntime_s { float rpmLimiterPGain; float rpmLimiterIGain; float rpmLimiterDGain; + float rpmLimiterI; pt1Filter_t rpmLimiterAverageRpmFilter; pt1Filter_t rpmLimiterThrottleScaleOffsetFilter; #endif