Revert "Merge remote-tracking branch 'sunnypilot/master' into SunnyPilot"

This reverts commit c07eb75f55, reversing
changes made to f56b049010.

common/pid.py

@@ -17,6 +17,7 @@ class PIDController:
     self.pos_limit = pos_limit
     self.neg_limit = neg_limit
 
+    self.i_unwind_rate = 0.3 / rate
     self.i_rate = 1.0 / rate
     self.speed = 0.0
 
@@ -34,6 +35,10 @@ class PIDController:
   def k_d(self):
     return np.interp(self.speed, self._k_d[0], self._k_d[1])
 
+  @property
+  def error_integral(self):
+    return self.i/self.k_i
+
   def reset(self):
     self.p = 0.0
     self.i = 0.0
@@ -41,21 +46,25 @@ class PIDController:
     self.f = 0.0
     self.control = 0
 
-  def update(self, error, error_rate=0.0, speed=0.0, feedforward=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
 
-    if not freeze_integrator:
-      i = self.i + error * self.k_i * self.i_rate
+    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
 
-      # 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)
+        # Clip i to prevent exceeding control limits
+        control_no_i = self.p + self.d + self.f
+        control_no_i = np.clip(control_no_i, self.neg_limit, self.pos_limit)
+        self.i = np.clip(self.i, self.neg_limit - control_no_i, self.pos_limit - control_no_i)
 
     control = self.p + self.i + self.d + self.f
+
     self.control = np.clip(control, self.neg_limit, self.pos_limit)
     return self.control