commaai/openpilot:7534b2a160faa683412c04c1254440e338931c5e

sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py

@@ -14,6 +14,13 @@ from openpilot.selfdrive.modeld.constants import ModelConstants
 LAT_PLAN_MIN_IDX = 5
 LATERAL_LAG_MOD = 0.0  # seconds, modifies how far in the future we look ahead for the lateral plan
 
+# from selfdrive/controls/lib/latcontrol_torque.py
+KP = 1.0
+KI = 0.3
+KD = 0.0
+INTERP_SPEEDS = [1, 1.5, 2.0, 3.0, 5, 7.5, 10, 15, 30]
+KP_INTERP = [250, 120, 65, 30, 11.5, 5.5, 3.5, 2.0, KP]
+
 
 def get_predicted_lateral_jerk(lat_accels, t_diffs):
   # compute finite difference between subsequent model_v2.acceleration.y values
@@ -48,7 +55,6 @@ class LatControlTorqueExtBase:
     self.model_v2 = None
     self.model_valid = False
     self.lac_torque = lac_torque
-    self.torque_params = lac_torque.torque_params
 
     self.actual_lateral_jerk: float = 0.0
     self.lateral_jerk_setpoint: float = 0.0
@@ -58,8 +64,7 @@ class LatControlTorqueExtBase:
     self.torque_from_lateral_accel_in_torque_space = CI.torque_from_lateral_accel_in_torque_space()
 
     self._ff = 0.0
-    self._pid = PIDController(self.torque_params.kp, self.torque_params.ki,
-                              k_f=self.torque_params.kf)
+    self._pid = PIDController([INTERP_SPEEDS, KP_INTERP], KI, KD)
     self._pid_log = None
     self._setpoint = 0.0
     self._measurement = 0.0