adapt to upstream

common/pid_mu.py

@@ -2,27 +2,31 @@ from openpilot.common.pid import PIDController
 import numpy as np
 
 class MultiplicativeUnwindPID(PIDController):
+  def __init__(self, k_p, k_i, k_f=0., k_d=0., pos_limit=1e308, neg_limit=-1e308, rate=100):
+    super().__init__(k_p, k_i, k_f=k_f, k_d=k_d, pos_limit=pos_limit, neg_limit=neg_limit, rate=rate)
+      
+    self.i_unwind_rate = 0.3 / rate
+    
   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 override:
-      # Multiplikativer Abbau von i
       self.i *= (1.0 - self.i_unwind_rate)
       if abs(self.i) < 1e-10:
         self.i = 0.0
     else:
       if not freeze_integrator:
-        self.i += error * self.k_i * self.i_rate
+        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 = 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)
+        # 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)
 
     control = self.p + self.i + self.d + self.f
     self.control = np.clip(control, self.neg_limit, self.pos_limit)
-    return self.control
+    return self.control

selfdrive/controls/lib/latcontrol_curvature.py

@@ -20,7 +20,7 @@ class LatControlCurvature(LatControl):
     super().reset()
     self.pid.reset()
   
-  def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose, curvature_limited):
+  def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, calibrated_pose, curvature_limited):
     pid_log = log.ControlsState.LateralCurvatureState.new_message()
     if not active:
       output_curvature = 0.0
@@ -37,7 +37,7 @@ class LatControlCurvature(LatControl):
       desired_curvature_corr = desired_curvature - roll_compensation
       
       pid_log.error = float(desired_curvature - actual_curvature)
-      freeze_integrator = steer_limited_by_controls or CS.vEgo < 5 or CS.steeringPressed
+      freeze_integrator = steer_limited_by_safety or CS.vEgo < 5 or CS.steeringPressed
       
       pid_curvature = self.pid.update(pid_log.error, feedforward=desired_curvature_corr, speed=CS.vEgo,
                                       freeze_integrator=freeze_integrator, override=CS.steeringPressed)
@@ -51,6 +51,6 @@ class LatControlCurvature(LatControl):
       pid_log.output = float(output_curvature)
       pid_log.actualCurvature = float(actual_curvature)
       pid_log.desiredCurvature = float(desired_curvature)
-      pid_log.saturated = bool(self._check_saturation(steer_limited_by_controls, CS, False, curvature_limited))
+      pid_log.saturated = bool(self._check_saturation(steer_limited_by_safety, CS, False, curvature_limited))
 
     return 0.0, 0.0, output_curvature, pid_log