From c8bd1cdd1e80fbee06313d82f374c92fda8251c1 Mon Sep 17 00:00:00 2001
From: MoreTore <ryleymccarroll22@gmail.com>
Date: Tue, 11 Nov 2025 12:11:36 -0600
Subject: [PATCH] fix sticky integrator

---
 selfdrive/controls/lib/pid.py | 30 ++++++++++++++++++++++++------
 1 file changed, 24 insertions(+), 6 deletions(-)

diff --git a/selfdrive/controls/lib/pid.py b/selfdrive/controls/lib/pid.py
index 99142280ca..1fdf20a523 100644
--- a/selfdrive/controls/lib/pid.py
+++ b/selfdrive/controls/lib/pid.py
@@ -51,14 +51,32 @@ class PIDController:
     self.d = error_rate * self.k_d
 
     if not freeze_integrator:
-      i = self.i + error * self.k_i * self.i_rate
+      i_candidate = self.i + error * self.k_i * self.i_rate
+    else:
+      i_candidate = self.i
 
-      # Don't allow windup if already clipping
-      test_control = self.p + i + self.d + self.f
-      i_upperbound = self.i if test_control > self.pos_limit else self.pos_limit
-      i_lowerbound = self.i if test_control < self.neg_limit else self.neg_limit
-      self.i = np.clip(i, i_lowerbound, i_upperbound)
+    # Unclipped control with candidate integral
+    u = self.p + i_candidate + self.d + self.f
 
+    # Saturated output
+    u_sat = np.clip(u, self.neg_limit, self.pos_limit)
+
+    # Anti-windup: only integrate if
+    # - we're not saturating, OR
+    # - the integral change moves us OUT of saturation.
+    if u == u_sat:
+      # not saturated -> accept integral update
+      self.i = i_candidate
+    else:
+      # saturated high, only allow integral if error < 0 (pull down)
+      if u > self.pos_limit and error < 0:
+        self.i = i_candidate
+      # saturated low, only allow integral if error > 0 (push up)
+      elif u < self.neg_limit and error > 0:
+        self.i = i_candidate
+      # else: reject this integral step (hold self.i)
+
+    # Final control with updated integral
     control = self.p + self.i + self.d + self.f
     self.control = np.clip(control, self.neg_limit, self.pos_limit)
     return self.control