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