From bc25677aa8f228fc62e345e4c7b79151fd22c051 Mon Sep 17 00:00:00 2001
From: DevTekVE <devtekve@gmail.com>
Date: Thu, 19 Jun 2025 16:21:41 +0200
Subject: [PATCH] adjust latcontorl too

---
 selfdrive/controls/controlsd.py                   |  5 ++++-
 selfdrive/controls/lib/latcontrol_angle_torque.py | 13 +++++++++++++
 2 files changed, 17 insertions(+), 1 deletion(-)
 create mode 100644 selfdrive/controls/lib/latcontrol_angle_torque.py

diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index 5ecff1ccdf..9698d69c55 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -17,6 +17,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
 from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD
 from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
 from openpilot.selfdrive.controls.lib.longcontrol import LongControl
+from openpilot.selfdrive.controls.lib.latcontrol_angle_torque import LatControlAngleTorque
 from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
 
 State = log.SelfdriveState.OpenpilotState
@@ -50,7 +51,9 @@ class Controls:
     self.LoC = LongControl(self.CP)
     self.VM = VehicleModel(self.CP)
     self.LaC: LatControl
-    if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
+    if self.CP.steerControlType == car.CarParams.SteerControlType.angle and self.CP.lateralTuning.which() == 'torque':
+      self.LaC = LatControlAngleTorque(self.CP, self.CI)
+    elif self.CP.steerControlType == car.CarParams.SteerControlType.angle:
       self.LaC = LatControlAngle(self.CP, self.CI)
     elif self.CP.lateralTuning.which() == 'pid':
       self.LaC = LatControlPID(self.CP, self.CI)
diff --git a/selfdrive/controls/lib/latcontrol_angle_torque.py b/selfdrive/controls/lib/latcontrol_angle_torque.py
new file mode 100644
index 0000000000..e8d31dca14
--- /dev/null
+++ b/selfdrive/controls/lib/latcontrol_angle_torque.py
@@ -0,0 +1,13 @@
+from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
+from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle
+
+
+class LatControlAngleTorque(LatControlTorque, LatControlAngle):
+  def __init__(self, CP, CI):
+    LatControlTorque.__init__(self, CP, CI)
+    LatControlAngle.__init__(self, CP, CI)
+
+  def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose, curvature_limited):
+    torque, _, _ = LatControlTorque.update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose, curvature_limited)
+    _, angle, angle_log = LatControlAngle.update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose, curvature_limited)
+    return torque, angle, angle_log
\ No newline at end of file