From 44c889fa41bdddfb8dd549e80655c129bf91d9b6 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Fri, 8 Nov 2024 20:15:36 -0600 Subject: [PATCH] longcontrol: prevent integral windup when feedforward rises gradually (#33970) * try * messy * simplify that * clip instead * clean up * clean up * update refs * this is better for latcontrol * Revert "update refs" This reverts commit 1d2508237f76121248060614b0c5e6ecdfdc4daf. * ref commit --- common/pid.py | 16 +++++++--------- selfdrive/test/process_replay/ref_commit | 2 +- 2 files changed, 8 insertions(+), 10 deletions(-) diff --git a/common/pid.py b/common/pid.py index 29c4d8bd46..36cbf9c4e9 100644 --- a/common/pid.py +++ b/common/pid.py @@ -59,15 +59,13 @@ class PIDController: if override: self.i -= self.i_unwind_rate * float(np.sign(self.i)) else: - i = self.i + error * self.k_i * self.i_rate - control = self.p + i + self.d + self.f - - # Update when changing i will move the control away from the limits - # or when i will move towards the sign of the error - if ((error >= 0 and (control <= self.pos_limit or i < 0.0)) or - (error <= 0 and (control >= self.neg_limit or i > 0.0))) and \ - not freeze_integrator: - self.i = i + if not freeze_integrator: + self.i = self.i + error * self.k_i * self.i_rate + + # Clip i to prevent exceeding control limits + control_no_i = self.p + self.d + self.f + control_no_i = clip(control_no_i, self.neg_limit, self.pos_limit) + self.i = clip(self.i, self.neg_limit - control_no_i, self.pos_limit - control_no_i) control = self.p + self.i + self.d + self.f diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 458dc90414..40bf1b836f 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -bde3a4e45bcb4c1c1952421a669b9dc3a705e31a +48ac1ed94c5f1aa3bd21ac609fed8ce3eb0bab8f \ No newline at end of file