Revert "latcontrol_torque: retune torque controller (#36392)"

This reverts commit 76c5cb6.

Author: felsager

selfdrive/controls/lib/latcontrol_torque.py

@@ -21,16 +21,16 @@
 # to be overcome to move it at all, this is compensated for too.
 
 KP = 1.0
-KI = 0.1
-KD = 0.3
+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]
 
 LP_FILTER_CUTOFF_HZ = 1.2
 JERK_LOOKAHEAD_SECONDS = 0.19
 JERK_GAIN = 0.3
 LAT_ACCEL_REQUEST_BUFFER_SECONDS = 1.0
-VERSION = 1  # bump this when changing controller
+VERSION = 0 # bump this when changing controller
 
 class LatControlTorque(LatControl):
   def __init__(self, CP, CI, dt):
@@ -84,17 +84,22 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat
       measurement_rate = self.measurement_rate_filter.update((measurement - self.previous_measurement) / self.dt)
       self.previous_measurement = measurement
 
-      error = setpoint - measurement
+      error = setpoint - measurement + JERK_GAIN * desired_lateral_jerk
 
       # 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
-      ff += get_friction(error + JERK_GAIN * desired_lateral_jerk, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params)
+      # 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)
 
       freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5
-      output_lataccel = self.pid.update(pid_log.error, -measurement_rate, CS.vEgo, ff, freeze_integrator)
+      output_lataccel = self.pid.update(pid_log.error,
+                                       -measurement_rate,
+                                        feedforward=ff,
+                                        speed=CS.vEgo,
+                                        freeze_integrator=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 @@
-d74c33b02938cafb3422f3500cfd083bd965e637
+9a7d8dd0660ba6a344ac61be89b2e832e6756184