latcontrol_torque: retune torque controller (#36392)

selfdrive/controls/lib/latcontrol_torque.py

@@ -21,8 +21,8 @@ from openpilot.common.pid import PIDController
 # to be overcome to move it at all, this is compensated for too.
 
 KP = 1.0
-KI = 0.3
-KD = 0.0
+KI = 0.1
+KD = 0.3
 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]
 
@@ -30,7 +30,7 @@ LP_FILTER_CUTOFF_HZ = 1.2
 JERK_LOOKAHEAD_SECONDS = 0.19
 JERK_GAIN = 0.3
 LAT_ACCEL_REQUEST_BUFFER_SECONDS = 1.0
-VERSION = 0 # bump this when changing controller
+VERSION = 1  # bump this when changing controller
 
 class LatControlTorque(LatControl):
   def __init__(self, CP, CI, dt):
@@ -84,22 +84,17 @@ class LatControlTorque(LatControl):
       measurement_rate = self.measurement_rate_filter.update((measurement - self.previous_measurement) / self.dt)
       self.previous_measurement = measurement
 
-      error = setpoint - measurement + JERK_GAIN * desired_lateral_jerk
+      error = setpoint - measurement
 
       # do error correction in lateral acceleration space, convert at end to handle non-linear torque responses correctly
       pid_log.error = float(error)
       ff = gravity_adjusted_future_lateral_accel
       # latAccelOffset corrects roll compensation bias from device roll misalignment relative to car roll
       ff -= self.torque_params.latAccelOffset
-      # TODO remove lateral jerk from feed forward - moving it from error means jerk is not scaled by low speed factor
-      ff += get_friction(error, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params)
+      ff += get_friction(error + JERK_GAIN * desired_lateral_jerk, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params)
 
       freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5
-      output_lataccel = self.pid.update(pid_log.error,
-                                       -measurement_rate,
-                                        feedforward=ff,
-                                        speed=CS.vEgo,
-                                        freeze_integrator=freeze_integrator)
+      output_lataccel = self.pid.update(pid_log.error, -measurement_rate, CS.vEgo, ff, freeze_integrator)
       output_torque = self.torque_from_lateral_accel(output_lataccel, self.torque_params)
 
       pid_log.active = True

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-9a7d8dd0660ba6a344ac61be89b2e832e6756184
+d74c33b02938cafb3422f3500cfd083bd965e637