From 2c7e6e72c5bfad3384ddd95fd99c2f31232c3273 Mon Sep 17 00:00:00 2001
From: firestar5683 <168790843+firestar5683@users.noreply.github.com>
Date: Tue, 21 Jul 2026 11:45:22 -0500
Subject: [PATCH] PID Alerts

---
 selfdrive/selfdrived/selfdrived.py            |  8 +++++++-
 selfdrive/selfdrived/tests/test_selfdrived.py | 19 +++++++++++++++++++
 2 files changed, 26 insertions(+), 1 deletion(-)
 create mode 100644 selfdrive/selfdrived/tests/test_selfdrived.py

diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py
index d0159e44c..6324018b6 100644
--- a/selfdrive/selfdrived/selfdrived.py
+++ b/selfdrive/selfdrived/selfdrived.py
@@ -55,6 +55,12 @@ StarPilotEventName = custom.StarPilotOnroadEvent.EventName
 IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput)
 
 
+def commanded_torque_at_max_for_saturation(CP, output: float) -> bool:
+  torque_controller = (CP.steerControlType == car.CarParams.SteerControlType.torque and
+                       CP.lateralTuning.which() == "torque")
+  return torque_controller and abs(output) > 0.99
+
+
 def should_loud_blindspot_alert_without_lateral(CS, sm, starpilot_toggles) -> bool:
   if not (getattr(starpilot_toggles, "loud_blindspot_alert", False) and
           getattr(starpilot_toggles, "loud_blindspot_alert_when_disengaged", False)):
@@ -643,7 +649,7 @@ class SelfdriveD:
       desired_lateral_accel = self.sm['modelV2'].action.desiredCurvature * (clipped_speed**2)
       undershooting = abs(desired_lateral_accel) / abs(1e-3 + actual_lateral_accel) > 1.2
       turning = abs(desired_lateral_accel) > 1.0
-      commanded_torque_at_max = abs(lac.output) > 0.99
+      commanded_torque_at_max = commanded_torque_at_max_for_saturation(self.CP, lac.output)
       # TODO: lac.saturated includes speed and other checks, should be pulled out
       if undershooting and turning and (lac.saturated or commanded_torque_at_max):
         now = time.monotonic()
diff --git a/selfdrive/selfdrived/tests/test_selfdrived.py b/selfdrive/selfdrived/tests/test_selfdrived.py
new file mode 100644
index 000000000..eecdbfa10
--- /dev/null
+++ b/selfdrive/selfdrived/tests/test_selfdrived.py
@@ -0,0 +1,19 @@
+from cereal import car
+
+from openpilot.selfdrive.selfdrived.selfdrived import commanded_torque_at_max_for_saturation
+
+
+def test_immediate_max_output_saturation_is_torque_controller_only():
+  CP = car.CarParams.new_message()
+  CP.steerControlType = car.CarParams.SteerControlType.torque
+  CP.lateralTuning.init("torque")
+
+  assert commanded_torque_at_max_for_saturation(CP, 1.0)
+  assert not commanded_torque_at_max_for_saturation(CP, 0.99)
+
+  CP.lateralTuning.init("pid")
+  assert not commanded_torque_at_max_for_saturation(CP, 1.0)
+
+  CP.lateralTuning.init("torque")
+  CP.steerControlType = car.CarParams.SteerControlType.angle
+  assert not commanded_torque_at_max_for_saturation(CP, 1.0)