diff --git a/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py b/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py index 609d2f78b6..31c2182d41 100644 --- a/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py +++ b/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