From b17502d65833f95b32740ba6969e1293aa0f2ce9 Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Wed, 20 Nov 2024 19:26:21 -0800
Subject: [PATCH] Match pid.py file from openpilot (#1514)

* match pid

* fix
---
 opendbc/car/common/pid.py | 37 ++++++++++++++++++++++++++-----------
 1 file changed, 26 insertions(+), 11 deletions(-)

diff --git a/opendbc/car/common/pid.py b/opendbc/car/common/pid.py
index 7a376c86..cfcc6d5c 100644
--- a/opendbc/car/common/pid.py
+++ b/opendbc/car/common/pid.py
@@ -1,15 +1,21 @@
+import numpy as np
 from numbers import Number
+
 from opendbc.car.common.numpy_fast import clip, interp
 
 
 class PIDController:
-  def __init__(self, k_p, k_i, pos_limit=1e308, neg_limit=-1e308, rate=100):
+  def __init__(self, k_p, k_i, k_f=0., k_d=0., pos_limit=1e308, neg_limit=-1e308, rate=100):
     self._k_p = k_p
     self._k_i = k_i
+    self._k_d = k_d
+    self.k_f = k_f   # feedforward gain
     if isinstance(self._k_p, Number):
       self._k_p = [[0], [self._k_p]]
     if isinstance(self._k_i, Number):
       self._k_i = [[0], [self._k_i]]
+    if isinstance(self._k_d, Number):
+      self._k_d = [[0], [self._k_d]]
 
     self.pos_limit = pos_limit
     self.neg_limit = neg_limit
@@ -28,6 +34,10 @@ class PIDController:
   def k_i(self):
     return interp(self.speed, self._k_i[0], self._k_i[1])
 
+  @property
+  def k_d(self):
+    return interp(self.speed, self._k_d[0], self._k_d[1])
+
   @property
   def error_integral(self):
     return self.i/self.k_i
@@ -35,24 +45,29 @@ class PIDController:
   def reset(self):
     self.p = 0.0
     self.i = 0.0
+    self.d = 0.0
+    self.f = 0.0
     self.control = 0
 
-  def update(self, error, speed=0.0, freeze_integrator=False):
+  def update(self, error, error_rate=0.0, speed=0.0, override=False, feedforward=0., freeze_integrator=False):
     self.speed = speed
 
     self.p = float(error) * self.k_p
+    self.f = feedforward * self.k_f
+    self.d = error_rate * self.k_d
 
-    i = self.i + error * self.k_i * self.i_rate
-    control = self.p + i
+    if override:
+      self.i -= self.i_unwind_rate * float(np.sign(self.i))
+    else:
+      if not freeze_integrator:
+        self.i = self.i + error * self.k_i * self.i_rate
 
-    # 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
+        # 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
+    control = self.p + self.i + self.d + self.f
 
     self.control = clip(control, self.neg_limit, self.pos_limit)
     return self.control