Fix: RPM Limit - reset I-term at arm (betaflight#13214)

* RPM Limiter - reset I-term at arm * Update src/main/flight/mixer.c Co-authored-by: Jan Post <[email protected]> --------- Co-authored-by: Jan Post <[email protected]>
SupaflyFPV · Dec 10, 2023 · 6ac08cd · 6ac08cd
1 parent 4f5bb1a
commit 6ac08cd
Show file tree

Hide file tree

Showing 4 changed files with 10 additions and 5 deletions.
diff --git 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
@@ -348,17 +348,16 @@ 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;
 
     // 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
@@ -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
@@ -60,6 +60,7 @@ typedef struct mixerRuntime_s {
     float rpmLimiterPGain;
     float rpmLimiterIGain;
     float rpmLimiterDGain;
+    float rpmLimiterI;
     pt1Filter_t rpmLimiterAverageRpmFilter;
     pt1Filter_t rpmLimiterThrottleScaleOffsetFilter;
 #endif