From d867aeced3ad97212cf78960930b15c82fe45c46 Mon Sep 17 00:00:00 2001
From: Bruce Luckcuck <github@etracer.net>
Date: Sat, 19 Jan 2019 10:13:34 -0500
Subject: [PATCH] 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                          | 3 ++-
 src/main/flight/pid.c                       | 8 ++++++++
 src/main/flight/pid.h                       | 2 +-
 src/test/unit/arming_prevention_unittest.cc | 1 +
 src/test/unit/vtx_unittest.cc               | 1 +
 5 files changed, 13 insertions(+), 2 deletions(-)

diff --git a/src/main/fc/core.c b/src/main/fc/core.c
index 999f03287..6e5015b04 100644
--- a/src/main/fc/core.c
+++ b/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);
     }
 
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 73724fec5..efbc73f60 100644
--- a/src/main/flight/pid.c
+++ b/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;
+}
diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
index 4c7286216..c9881fad2 100644
--- a/src/main/flight/pid.h
+++ b/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);
diff --git a/src/test/unit/arming_prevention_unittest.cc b/src/test/unit/arming_prevention_unittest.cc
index fee5772f3..39c1f5716 100644
--- a/src/test/unit/arming_prevention_unittest.cc
+++ b/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) {}
 }
diff --git a/src/test/unit/vtx_unittest.cc b/src/test/unit/vtx_unittest.cc
index 4a90be274..5e86cd0ed 100644
--- a/src/test/unit/vtx_unittest.cc
+++ b/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) {}
 }