From 177c7f1cf327a54a005f30e0d12d1b6954dc2648 Mon Sep 17 00:00:00 2001
From: felsager <d.felsager@gmail.com>
Date: Mon, 3 Nov 2025 10:31:22 -0800
Subject: [PATCH] Revert "latcontrol_torque: retune torque controller (#36392)"

This reverts commit 76c5cb6d8737c11235b8d4843a3a921645faea8b.
---
 selfdrive/controls/lib/latcontrol_torque.py | 17 +++++++++++------
 selfdrive/test/process_replay/ref_commit    |  2 +-
 2 files changed, 12 insertions(+), 7 deletions(-)

diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py
index f36aabc1d2..1e3d80c2d5 100644
--- a/selfdrive/controls/lib/latcontrol_torque.py
+++ b/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.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]
 
@@ -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 = 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 @@ class LatControlTorque(LatControl):
       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
diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit
index f313edbc62..37d1e8f8d3 100644
--- a/selfdrive/test/process_replay/ref_commit
+++ b/selfdrive/test/process_replay/ref_commit
@@ -1 +1 @@
-d74c33b02938cafb3422f3500cfd083bd965e637
+9a7d8dd0660ba6a344ac61be89b2e832e6756184
\ No newline at end of file