Move anti-windup iterm reset from rx loop to pid loop

Having the iterm resetting happening in the rx loop causes a sawtooth PID/motor effect while idling since the PID loop is running at a much higher rate and iterm is allowed to grow during this, and then only reset at a much lower rate in the rx loop. This can potentially lead to some oscillation and/or resonance while idling before takeoff as the sawtooth signal can make it through to the motor outputs.

src/main/fc/core.c

@@ -676,12 +676,13 @@ bool processRx(timeUs_t currentTimeUs)
     /* In airmode iterm should be prevented to grow when Low thottle and Roll + Pitch Centered.
      This is needed to prevent iterm winding on the ground, but keep full stabilisation on 0 throttle while in air */
     if (throttleStatus == THROTTLE_LOW && !airmodeIsActivated && !launchControlActive) {
-        pidResetIterm();
+        pidSetItermReset(true);
         if (currentPidProfile->pidAtMinThrottle)
             pidStabilisationState(PID_STABILISATION_ON);
         else
             pidStabilisationState(PID_STABILISATION_OFF);
     } else {
+        pidSetItermReset(false);
         pidStabilisationState(PID_STABILISATION_ON);
     }
 

src/main/flight/pid.c

@@ -80,6 +80,7 @@ static FAST_RAM_ZERO_INIT float antiGravityThrottleHpf;
 static FAST_RAM_ZERO_INIT uint16_t itermAcceleratorGain;
 static FAST_RAM float antiGravityOsdCutoff = 1.0f;
 static FAST_RAM_ZERO_INIT bool antiGravityEnabled;
+static FAST_RAM_ZERO_INIT bool zeroThrottleItermReset;
 
 PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 2);
 
@@ -1392,6 +1393,8 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
 
             pidData[axis].Sum = 0;
         }
+    } else if (zeroThrottleItermReset) {
+        pidResetIterm();
     }
 }
 
@@ -1444,3 +1447,8 @@ void dynLpfDTermUpdate(float throttle)
     }
 }
 #endif
+
+void pidSetItermReset(bool enabled)
+{
+    zeroThrottleItermReset = enabled;
+}

src/main/flight/pid.h

@@ -232,4 +232,4 @@ float pidLevel(int axis, const pidProfile_t *pidProfile,
 float calcHorizonLevelStrength(void);
 #endif
 void dynLpfDTermUpdate(float throttle);
-
+void pidSetItermReset(bool enabled);

src/test/unit/arming_prevention_unittest.cc

@@ -1088,4 +1088,5 @@ extern "C" {
     bool gpsIsHealthy() { return false; }
     bool isAltitudeOffset(void) { return false; }
     float getCosTiltAngle(void) { return 0.0f; }
+    void pidSetItermReset(bool) {}
 }

src/test/unit/vtx_unittest.cc

@@ -174,4 +174,5 @@ extern "C" {
     bool usbVcpIsConnected(void) { return false; }
     void pidSetAntiGravityState(bool newState) { UNUSED(newState); }
     void osdSuppressStats(bool) {}
+    void pidSetItermReset(bool) {}
 }