From e47a41cc09d4b37a74dbdf8e294b172dfa6e8130 Mon Sep 17 00:00:00 2001 From: MoreTore Date: Tue, 11 Nov 2025 12:11:36 -0600 Subject: [PATCH] fix sticky integrator --- selfdrive/controls/lib/pid.py | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/selfdrive/controls/lib/pid.py b/selfdrive/controls/lib/pid.py index 7cf25ed5a..8e5051dd7 100644 --- a/selfdrive/controls/lib/pid.py +++ b/selfdrive/controls/lib/pid.py @@ -50,14 +50,17 @@ class PIDController: self.d = self.k_d * error_rate self.f = self.k_f * feedforward - if not freeze_integrator: - i = self.i + self.k_i * self.i_dt * error + i_candidate = self.i if freeze_integrator else self.i + self.k_i * self.i_dt * error + u = self.p + i_candidate + self.d + self.f + u_sat = np.clip(u, self.neg_limit, self.pos_limit) - # 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) + if u == u_sat: + self.i = i_candidate + else: + if u > self.pos_limit and error < 0: + self.i = i_candidate + elif u < self.neg_limit and error > 0: + self.i = i_candidate control = self.p + self.i + self.d + self.f self.control = np.clip(control, self.neg_limit, self.pos_limit)