From 6c8af1da8fa5eddeb8e69906d4b5f4e8ce3aba37 Mon Sep 17 00:00:00 2001
From: bkleiner <b.kleiner@str8labs.com>
Date: Sat, 16 Sep 2023 01:06:59 +0200
Subject: [PATCH] pid: use flag to not wind down intergral once we detected
 sticks

---
 src/flight/pid.c | 31 ++++++++++++++++++++++++++-----
 1 file changed, 26 insertions(+), 5 deletions(-)

diff --git a/src/flight/pid.c b/src/flight/pid.c
index 32486525b..4d51d2fc3 100644
--- a/src/flight/pid.c
+++ b/src/flight/pid.c
@@ -168,6 +168,28 @@ static inline float pid_filter_dterm(uint8_t x, float dterm) {
   return dterm;
 }
 
+static inline bool pid_should_enable_iterm(uint8_t x) {
+  static bool stick_movement = false;
+  if (!flags.arm_state) {
+    // disarmed, disable, flag no movement
+    stick_movement = false;
+    return false;
+  }
+  if (flags.in_air) {
+    // in-air, enable, flag no movement
+    stick_movement = false;
+    return true;
+  }
+
+  if (fabsf(state.setpoint.axis[x]) > 0.1f) {
+    // record first stick crossing
+    stick_movement = true;
+  }
+
+  // enable if we recored stick movement previously
+  return stick_movement;
+}
+
 // pid calculation for acro ( rate ) mode
 // input: error[x] = setpoint - gyro
 // output: state.pidoutput.axis[x] = change required from motors
@@ -181,17 +203,16 @@ static inline void pid(uint8_t x) {
   state.pid_p_term.axis[x] *= v_compensation;
 
   // I term
-  if (flags.on_ground || flags.in_air == 0) {
-    // wind down integral while we are still on ground
-    ierror[x] *= 0.98f;
-  }
-
   static vec3_t pid_output = {.roll = 0, .pitch = 0, .yaw = 0};
 
   // SIMPSON_RULE_INTEGRAL
   // assuming similar time intervals
   const float *integral_limit = target.brushless ? integral_limit_brushless : integral_limit_brushed;
   const float iterm_windup = pid_compute_iterm_windup(x, pid_output.axis[x]);
+  if (!pid_should_enable_iterm(x)) {
+    // wind down integral while we are still on ground and we do not get any input from the sticks
+    ierror[x] *= 0.98f;
+  }
   ierror[x] = ierror[x] + 0.166666f * (lasterror2[x] + 4 * lasterror[x] + state.error.axis[x]) * current_ki[x] * iterm_windup * state.looptime;
   ierror[x] = constrain(ierror[x], -integral_limit[x], integral_limit[x]);
   lasterror2[x] = lasterror[x];