Fix Tesla cooperative steering saturation warnings

opendbc_repo/opendbc/car/car.capnp

@@ -375,6 +375,17 @@ struct CarControl {
     torqueOutputCan @8: Float32;   # value sent over can to the car
     speed @6: Float32;  # m/s
     lateralControlMode @9: LateralControlMode;
+    steeringLimitInfo @10 :SteeringLimitInfo;
+
+    struct SteeringLimitInfo {
+      valid @0 :Bool;
+      modelLimitErrorDeg @1 :Float32;
+      resumeLimitErrorDeg @2 :Float32;
+      cooperativeLimitErrorDeg @3 :Float32;
+      cooperativeOffsetDeg @4 :Float32;
+      monoTime @5 :UInt64;
+      combinedLimitErrorDeg @6 :Float32;
+    }
 
     enum LongControlState @0xe40f3a917d908282{
       off @0;

opendbc_repo/opendbc/car/tesla/carcontroller.py

@@ -24,6 +24,7 @@ class CarController(CarControllerBase):
     self.coop_enabled = CP.carFingerprint == CAR.TESLA_MODEL_3 and any(
       config.safetyParam & TeslaSafetyFlags.COOP_STEERING.value for config in CP.safetyConfigs
     )
+    self._clear_steering_limit_info()
     self.packer = CANPacker(dbc_names[Bus.party])
     self.tesla_can = TeslaCAN(self.packer)
     self.preap_long = None
@@ -39,8 +40,28 @@ class CarController(CarControllerBase):
       from opendbc.car.tesla.interface import CarInterface
       self.VM = VehicleModel(CarInterface.get_non_essential_params(CAR.TESLA_MODEL_S_PREAP))
 
+  def _clear_steering_limit_info(self):
+    self.steering_limit_info_valid = False
+    self.model_limit_error_deg = 0.0
+    self.resume_limit_error_deg = 0.0
+    self.cooperative_limit_error_deg = 0.0
+    self.cooperative_offset_deg = 0.0
+    self.steering_limit_mono_time = 0
+    self.combined_limit_error_deg = 0.0
+
+  def _write_steering_limit_info(self, actuators):
+    info = actuators.steeringLimitInfo
+    info.valid = self.steering_limit_info_valid
+    info.modelLimitErrorDeg = self.model_limit_error_deg
+    info.resumeLimitErrorDeg = self.resume_limit_error_deg
+    info.cooperativeLimitErrorDeg = self.cooperative_limit_error_deg
+    info.cooperativeOffsetDeg = self.cooperative_offset_deg
+    info.monoTime = self.steering_limit_mono_time
+    info.combinedLimitErrorDeg = self.combined_limit_error_deg
+
   def update(self, CC, CS, now_nanos, starpilot_toggles):
     if self.CP.carFingerprint == CAR.TESLA_MODEL_S_PREAP:
+      self._clear_steering_limit_info()
       return self._update_preap(CC, CS)
 
     actuators = CC.actuators
@@ -48,8 +69,12 @@ class CarController(CarControllerBase):
 
     # Preserve the stock controller path unless cooperative steering is explicitly enabled.
     lat_active = CC.latActive and (not CS.out.steeringDisengage if self.coop_enabled else CS.hands_on_level < 3)
+    if not (self.coop_enabled and lat_active):
+      self._clear_steering_limit_info()
 
     if self.frame % 2 == 0:
+      requested_angle = actuators.steeringAngleDeg
+
       # Angular rate limit based on speed
       self.apply_angle_last = apply_steer_angle_limits_vm(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgoRaw, CS.out.steeringAngleDeg,
                                                           lat_active, CarControllerParams, self.VM)
@@ -57,6 +82,27 @@ class CarController(CarControllerBase):
       self.apply_angle_command_last, lat_active = self.coop_steer.update(
         self.apply_angle_last, lat_active, self.coop_enabled, CS, self.VM,
       )
+
+      if self.coop_enabled and lat_active:
+        model_limit_error_deg = abs(requested_angle - self.apply_angle_last)
+        resume_limit_error_deg = self.coop_steer.resume_limit_error_deg
+        cooperative_limit_error_deg = self.coop_steer.cooperative_limit_error_deg
+        cooperative_offset_deg = self.coop_steer.cooperative_offset_deg
+        combined_limit_error_deg = abs(requested_angle + cooperative_offset_deg - self.apply_angle_command_last)
+        limit_values = (model_limit_error_deg, resume_limit_error_deg, cooperative_limit_error_deg,
+                        cooperative_offset_deg, combined_limit_error_deg)
+
+        if all(np.isfinite(value) for value in limit_values):
+          self.steering_limit_info_valid = True
+          self.model_limit_error_deg = model_limit_error_deg
+          self.resume_limit_error_deg = resume_limit_error_deg
+          self.cooperative_limit_error_deg = cooperative_limit_error_deg
+          self.cooperative_offset_deg = cooperative_offset_deg
+          self.steering_limit_mono_time = now_nanos
+          self.combined_limit_error_deg = combined_limit_error_deg
+        else:
+          self._clear_steering_limit_info()
+
       can_sends.append(self.tesla_can.create_steering_control(self.apply_angle_command_last, lat_active))
 
     if self.frame % 10 == 0:
@@ -79,6 +125,7 @@ class CarController(CarControllerBase):
     # TODO: HUD control
     new_actuators = actuators.as_builder()
     new_actuators.steeringAngleDeg = self.apply_angle_command_last
+    self._write_steering_limit_info(new_actuators)
 
     self.frame += 1
     return new_actuators, can_sends
@@ -121,6 +168,7 @@ class CarController(CarControllerBase):
 
     new_actuators = actuators.as_builder()
     new_actuators.steeringAngleDeg = self.apply_angle_last
+    self._write_steering_limit_info(new_actuators)
 
     self.frame += 1
     return new_actuators, can_sends

opendbc_repo/opendbc/car/tesla/coop_steering.py

@@ -62,6 +62,9 @@ class CooperativeSteeringController:
     self.angle_override = 0.0
     self.resume_rate_limiter_delta = SteerRateLimiter()
     self.resume_rate_limiter = SteerRateLimiter()
+    self.resume_limit_error_deg = 0.0
+    self.cooperative_limit_error_deg = 0.0
+    self.cooperative_offset_deg = 0.0
 
   def reset_override_state(self, apply_angle: float) -> None:
     self.apply_angle_last = apply_angle
@@ -105,19 +108,25 @@ class CooperativeSteeringController:
     return self.resume_rate_limiter.update(apply_angle, angle_rate_delta)
 
   def update(self, apply_angle: float, lat_active: bool, enabled: bool, CS, VM: VehicleModel) -> tuple[float, bool]:
+    self.resume_limit_error_deg = 0.0
+    self.cooperative_limit_error_deg = 0.0
+    self.cooperative_offset_deg = 0.0
     if not enabled:
       self.reset_resume_state(apply_angle)
       self.reset_override_state(apply_angle)
       return apply_angle, lat_active
 
+    requested_angle = apply_angle
     apply_angle = self.apply_resume_rate_limit(lat_active, apply_angle)
+    self.resume_limit_error_deg = abs(requested_angle - apply_angle)
     if not lat_active:
       self.reset_override_state(apply_angle)
       return apply_angle, False
 
     apply_angle_delta = apply_angle - self.apply_angle_last
     self.apply_angle_last = apply_angle
-    apply_angle += self.update_override_angle(apply_angle_delta, CS.out.steeringTorque, CS.out.vEgo, VM)
+    self.cooperative_offset_deg = self.update_override_angle(apply_angle_delta, CS.out.steeringTorque, CS.out.vEgo, VM)
+    apply_angle += self.cooperative_offset_deg
 
     limited_angle = apply_steer_angle_limits_vm(
       apply_angle,
@@ -129,5 +138,6 @@ class CooperativeSteeringController:
       VM,
     )
     self.coop_apply_angle_last = limited_angle
+    self.cooperative_limit_error_deg = abs(apply_angle - limited_angle)
     self.unwind_override_angle(apply_angle - limited_angle)
     return limited_angle, True

opendbc_repo/opendbc/car/tesla/tests/test_coop_steering.py

@@ -1,3 +1,4 @@
+import math
 from types import SimpleNamespace
 
 import pytest
@@ -73,3 +74,110 @@ def test_safety_flag_is_model_3_only(candidate, enabled, expected):
 
   if candidate != CAR.TESLA_MODEL_S_PREAP:
     assert CarController(DBC[candidate], params).coop_enabled is expected
+
+
+def assert_finite_nonnegative_limit_errors(controller):
+  errors = (controller.resume_limit_error_deg, controller.cooperative_limit_error_deg)
+  assert all(math.isfinite(error) and error >= 0.0 for error in errors)
+  assert math.isfinite(controller.cooperative_offset_deg)
+
+
+def test_zero_torque_has_zero_cooperative_diagnostics(vehicle_model):
+  controller = CooperativeSteeringController()
+
+  angle, lat_active = controller.update(0.0, True, True, make_car_state(), vehicle_model)
+
+  assert angle == 0.0
+  assert lat_active
+  assert controller.resume_limit_error_deg == 0.0
+  assert controller.cooperative_limit_error_deg == 0.0
+  assert controller.cooperative_offset_deg == 0.0
+  assert_finite_nonnegative_limit_errors(controller)
+
+
+def test_steady_light_torque_reports_offset_without_real_limiting(vehicle_model):
+  controller = CooperativeSteeringController()
+
+  for _ in range(100):
+    controller.update(0.0, True, True, make_car_state(torque=0.9), vehicle_model)
+
+  assert controller.cooperative_offset_deg > 2.5
+  assert controller.resume_limit_error_deg < 2.5
+  assert controller.cooperative_limit_error_deg < 2.5
+  assert_finite_nonnegative_limit_errors(controller)
+
+
+def test_torque_reversal_updates_signed_offset_without_negative_errors(vehicle_model):
+  controller = CooperativeSteeringController()
+  for _ in range(100):
+    controller.update(0.0, True, True, make_car_state(torque=0.9), vehicle_model)
+
+  for _ in range(200):
+    controller.update(0.0, True, True, make_car_state(torque=-0.9), vehicle_model)
+
+  assert controller.cooperative_offset_deg < -2.5
+  assert_finite_nonnegative_limit_errors(controller)
+
+
+def test_release_reports_gradual_offset_unwind(vehicle_model):
+  controller = CooperativeSteeringController()
+  for _ in range(100):
+    controller.update(0.0, True, True, make_car_state(torque=1.5), vehicle_model)
+
+  offsets = []
+  for _ in range(100):
+    controller.update(0.0, True, True, make_car_state(), vehicle_model)
+    offsets.append(controller.cooperative_offset_deg)
+
+  assert offsets[0] > offsets[-1] >= 0.0
+  assert all(next_offset <= offset for offset, next_offset in zip(offsets, offsets[1:]))
+  assert offsets[-1] == pytest.approx(0.0, abs=1e-6)
+  assert_finite_nonnegative_limit_errors(controller)
+
+
+def test_resume_ramp_reports_resume_limiting(vehicle_model):
+  controller = CooperativeSteeringController()
+  controller.reset_resume_state(0.0)
+  controller.reset_override_state(0.0)
+
+  controller.update(20.0, True, True, make_car_state(), vehicle_model)
+
+  assert controller.resume_limit_error_deg > 2.5
+  assert controller.cooperative_limit_error_deg < 2.5
+  assert controller.cooperative_offset_deg == 0.0
+
+
+def test_final_limiter_reports_cooperative_target_clipping(vehicle_model):
+  controller = CooperativeSteeringController()
+  controller.reset_resume_state(20.0)
+  controller.reset_override_state(0.0)
+
+  controller.update(20.0, True, True, make_car_state(), vehicle_model)
+
+  assert controller.resume_limit_error_deg == 0.0
+  assert controller.cooperative_limit_error_deg > 2.5
+  assert controller.cooperative_offset_deg == 0.0
+
+
+def test_final_limiter_remains_visible_with_light_torque(vehicle_model):
+  controller = CooperativeSteeringController()
+  controller.reset_resume_state(20.0)
+  controller.reset_override_state(0.0)
+
+  controller.update(20.0, True, True, make_car_state(torque=-0.9), vehicle_model)
+
+  assert abs(controller.cooperative_offset_deg) > 0.0
+  assert controller.cooperative_limit_error_deg > 2.5
+
+
+def test_diagnostics_reset_on_disabled_update(vehicle_model):
+  controller = CooperativeSteeringController()
+  controller.reset_resume_state(20.0)
+  controller.update(20.0, True, True, make_car_state(), vehicle_model)
+  assert controller.cooperative_limit_error_deg > 2.5
+
+  controller.update(4.0, True, False, make_car_state(torque=2.0), vehicle_model)
+
+  assert controller.resume_limit_error_deg == 0.0
+  assert controller.cooperative_limit_error_deg == 0.0
+  assert controller.cooperative_offset_deg == 0.0

opendbc_repo/opendbc/car/tesla/tests/test_coop_steering_limit_info.py

@@ -0,0 +1,221 @@
+import json
+import math
+from pathlib import Path
+from types import SimpleNamespace
+
+import pytest
+
+from cereal import car
+from opendbc.car import gen_empty_fingerprint
+from opendbc.car.tesla.carcontroller import CarController
+from opendbc.car.tesla.interface import CarInterface
+from opendbc.car.tesla.values import CAR, DBC, TeslaSafetyFlags
+
+
+BASELINE_SHA = "a80064be4fdf4b8765a5e9dec44d8a5c266f48a8"
+BASELINE_SOURCE_SHA256 = {
+  "opendbc_repo/opendbc/car/tesla/coop_steering.py": "9c9d60bbfae2aaa0d8c1fca203a9fa19a14a19feef5aa89e85ecaa6f6502a9f0",
+  "opendbc_repo/opendbc/car/tesla/carcontroller.py": "1ef3cf646bc4b3c398bd12010e624b90e083426152d632fafb5b2e93fd3d8661",
+}
+BASELINE_FIXTURE = Path(__file__).parent / "fixtures" / "coop_steering_baseline_a80064be.json"
+
+
+def make_params(candidate=CAR.TESLA_MODEL_3, cooperative=True):
+  toggles = SimpleNamespace(tesla_cooperative_steering=cooperative, trailer_load_kg=0.0)
+  return CarInterface.get_params(candidate, gen_empty_fingerprint(), [], False, False, False, toggles)
+
+
+def make_car_state(torque=0.0, speed=15.0, angle=0.0, steering_disengage=False):
+  return SimpleNamespace(
+    out=SimpleNamespace(
+      steeringTorque=torque,
+      steeringAngleDeg=angle,
+      steeringDisengage=steering_disengage,
+      vEgo=speed,
+      vEgoRaw=speed,
+      gasPressed=False,
+    ),
+    hands_on_level=0,
+    das_control={"DAS_controlCounter": 0},
+  )
+
+
+def make_control(requested_angle=0.0, lat_active=True):
+  control = car.CarControl.new_message()
+  control.latActive = lat_active
+  control.actuators.steeringAngleDeg = requested_angle
+  return control.as_reader()
+
+
+def make_controller(candidate=CAR.TESLA_MODEL_3, cooperative=True):
+  params = make_params(candidate, cooperative)
+  return CarController(DBC[candidate], params)
+
+
+def run_frame(controller, requested_angle=0.0, torque=0.0, speed=15.0, measured_angle=0.0,
+              lat_active=True, steering_disengage=False, now_nanos=1_000_000_000):
+  return controller.update(
+    make_control(requested_angle, lat_active),
+    make_car_state(torque, speed, measured_angle, steering_disengage),
+    now_nanos,
+    SimpleNamespace(),
+  )
+
+
+def legacy_actuator_dict(actuators):
+  values = actuators.to_dict()
+  values.pop("steeringLimitInfo", None)
+  return values
+
+
+def test_steering_limit_info_defaults_to_invalid():
+  actuators = car.CarControl.Actuators.new_message()
+
+  assert not actuators.steeringLimitInfo.valid
+
+
+def test_steering_limit_info_round_trips_through_car_output():
+  output = car.CarOutput.new_message()
+  info = output.actuatorsOutput.steeringLimitInfo
+  info.valid = True
+  info.modelLimitErrorDeg = 1.25
+  info.resumeLimitErrorDeg = 0.5
+  info.cooperativeLimitErrorDeg = 2.0
+  info.cooperativeOffsetDeg = -4.5
+  info.monoTime = 1_234_567_890
+  info.combinedLimitErrorDeg = 3.75
+
+  with car.CarOutput.from_bytes(output.to_bytes()) as restored:
+    restored_info = restored.actuatorsOutput.steeringLimitInfo
+    assert restored_info.valid
+    assert restored_info.modelLimitErrorDeg == 1.25
+    assert restored_info.resumeLimitErrorDeg == 0.5
+    assert restored_info.cooperativeLimitErrorDeg == 2.0
+    assert restored_info.cooperativeOffsetDeg == -4.5
+    assert restored_info.monoTime == 1_234_567_890
+    assert restored_info.combinedLimitErrorDeg == 3.75
+
+
+def test_active_cooperative_controller_publishes_r_n_a_t_f_diagnostics():
+  controller = make_controller()
+  requested_angle = 20.0
+  now_nanos = 1_234_567_890
+
+  actuators, _ = run_frame(controller, requested_angle, torque=0.9, measured_angle=0.0, now_nanos=now_nanos)
+
+  info = actuators.steeringLimitInfo
+  assert info.valid
+  assert info.monoTime == now_nanos
+  assert info.modelLimitErrorDeg == pytest.approx(abs(requested_angle - controller.apply_angle_last), abs=1e-5)
+  assert info.resumeLimitErrorDeg == pytest.approx(controller.coop_steer.resume_limit_error_deg, abs=1e-5)
+  assert info.cooperativeLimitErrorDeg == pytest.approx(controller.coop_steer.cooperative_limit_error_deg, abs=1e-5)
+  assert info.cooperativeOffsetDeg == pytest.approx(controller.coop_steer.cooperative_offset_deg, abs=1e-5)
+  assert info.combinedLimitErrorDeg == pytest.approx(
+    abs(requested_angle + info.cooperativeOffsetDeg - actuators.steeringAngleDeg), abs=1e-5,
+  )
+  assert info.modelLimitErrorDeg > 2.5
+  assert info.cooperativeOffsetDeg > 0.0
+  assert info.combinedLimitErrorDeg > 2.5
+  errors = (info.modelLimitErrorDeg, info.resumeLimitErrorDeg,
+            info.cooperativeLimitErrorDeg, info.combinedLimitErrorDeg)
+  assert all(math.isfinite(error) and error >= 0.0 for error in errors)
+
+
+def test_cooperative_offset_alone_does_not_become_limiter_error():
+  controller = make_controller()
+  actuators = None
+
+  for frame in range(200):
+    actuators, _ = run_frame(controller, torque=0.9, now_nanos=1_000_000_000 + frame * 10_000_000)
+
+  assert actuators is not None
+  info = actuators.steeringLimitInfo
+  assert info.valid
+  assert info.cooperativeOffsetDeg > 2.5
+  assert info.modelLimitErrorDeg < 2.5
+  assert info.resumeLimitErrorDeg < 2.5
+  assert info.cooperativeLimitErrorDeg < 2.5
+  assert info.combinedLimitErrorDeg < 2.5
+
+
+def test_combined_error_keeps_two_same_direction_small_limits_visible():
+  controller = make_controller()
+  # Prime the resume limiter to the first-stage output for this literal input.
+  controller.coop_steer.reset_resume_state(-0.9954867959022522)
+
+  actuators, _ = run_frame(controller, -2.5, torque=-1.5, speed=12.5, measured_angle=0.0)
+
+  info = actuators.steeringLimitInfo
+  assert info.modelLimitErrorDeg == pytest.approx(1.5045133, abs=1e-5)
+  assert info.resumeLimitErrorDeg == pytest.approx(0.0, abs=1e-5)
+  assert info.cooperativeLimitErrorDeg == pytest.approx(1.5045133, abs=1e-5)
+  assert info.modelLimitErrorDeg < 2.5
+  assert info.cooperativeLimitErrorDeg < 2.5
+  assert info.combinedLimitErrorDeg == pytest.approx(3.0090265, abs=1e-5)
+  assert info.combinedLimitErrorDeg > 2.5
+
+
+def test_intervening_100hz_frame_retains_matching_50hz_sample():
+  controller = make_controller()
+  first, _ = run_frame(controller, 8.0, torque=0.9, now_nanos=1_000_000_000)
+  first_info = first.steeringLimitInfo.to_dict()
+
+  second, _ = run_frame(controller, -40.0, torque=-1.5, now_nanos=1_010_000_000)
+
+  assert second.steeringLimitInfo.to_dict() == first_info
+  assert second.steeringLimitInfo.monoTime == 1_000_000_000
+
+
+def test_inactive_interval_clears_sample_until_next_steering_update():
+  controller = make_controller()
+  active, _ = run_frame(controller, 8.0, torque=0.9, now_nanos=1_000_000_000)
+  assert active.steeringLimitInfo.valid
+
+  inactive, _ = run_frame(controller, 8.0, torque=0.9, lat_active=False, now_nanos=1_010_000_000)
+  assert not inactive.steeringLimitInfo.valid
+  assert inactive.steeringLimitInfo.monoTime == 0
+
+  resumed, _ = run_frame(controller, 8.0, torque=0.9, now_nanos=1_020_000_000)
+  assert resumed.steeringLimitInfo.valid
+  assert resumed.steeringLimitInfo.monoTime == 1_020_000_000
+
+
+@pytest.mark.parametrize(("candidate", "cooperative", "steering_disengage"), (
+  (CAR.TESLA_MODEL_3, False, False),
+  (CAR.TESLA_MODEL_Y, True, False),
+  (CAR.TESLA_MODEL_3, True, True),
+))
+def test_diagnostics_invalid_when_not_in_supported_active_path(candidate, cooperative, steering_disengage):
+  controller = make_controller(candidate, cooperative)
+
+  actuators, _ = run_frame(controller, torque=1.5, steering_disengage=steering_disengage)
+
+  assert not actuators.steeringLimitInfo.valid
+  assert actuators.steeringLimitInfo.monoTime == 0
+
+
+def test_actual_actuators_and_steering_can_match_pinned_baseline_fixture():
+  fixture = json.loads(BASELINE_FIXTURE.read_text())
+  assert fixture["metadata"] == {
+    "schemaVersion": 1,
+    "baselineSha": BASELINE_SHA,
+    "baselineSourceSha256": BASELINE_SOURCE_SHA256,
+    "frameCount": 386,
+  }
+
+  candidate = make_controller()
+  for expected in fixture["frames"]:
+    inputs = expected["input"]
+    candidate_actuators, candidate_can = run_frame(
+      candidate,
+      inputs["requestedAngleDeg"],
+      inputs["torqueNm"],
+      inputs["speedMps"],
+      inputs["measuredAngleDeg"],
+      inputs["latActive"],
+      inputs["steeringDisengage"],
+      inputs["nowNanos"],
+    )
+
+    assert legacy_actuator_dict(candidate_actuators) == expected["actuators"]
+    assert [[address, data.hex(), bus] for address, data, bus in candidate_can] == expected["can"]

selfdrive/controls/controlsd.py

@@ -1,6 +1,8 @@
 #!/usr/bin/env python3
 import math
 from numbers import Number
+import os
+import time
 
 from cereal import car, custom, log
 import cereal.messaging as messaging
@@ -26,7 +28,8 @@ from openpilot.selfdrive.controls.lib.drive_helpers import (
 from openpilot.selfdrive.controls.lib.lane_centering import LaneCenteringController
 from openpilot.selfdrive.controls.lib.latcontrol import LatControl
 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_angle import LatControlAngle
+from openpilot.selfdrive.controls.lib.steering_saturation import is_angle_steering_limited
 from openpilot.selfdrive.controls.lib.latcontrol_curvature import LatControlCurvature
 from openpilot.selfdrive.controls.lib.latcontrol_torque import (
   BOLT_2018_2021_STEER_RATIO_TEST_SCALE,
@@ -48,6 +51,7 @@ LaneChangeDirection = log.LaneChangeDirection
 LateralControlMode = car.CarControl.Actuators.LateralControlMode
 
 ACTUATOR_FIELDS = tuple(car.CarControl.Actuators.schema.fields.keys())
+REPLAY = "REPLAY" in os.environ
 
 # After a smoothed lane change ends, ramp the curvature limits back to stock over this
 # time so the final recenter correction is shaped instead of stepping through unclamped.
@@ -873,8 +877,15 @@ class Controls:
     if self.sm['selfdriveState'].active:
       CO = self.sm['carOutput']
       if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
-        self.steer_limited_by_safety = abs(CC.actuators.steeringAngleDeg - CO.actuatorsOutput.steeringAngleDeg) > \
-                                              STEER_ANGLE_SATURATION_THRESHOLD
+        output_healthy = (
+          self.sm.valid['carOutput'] and
+          self.sm.alive['carOutput'] and
+          self.sm.freq_ok['carOutput']
+        )
+        now_nanos = self.sm.logMonoTime['selfdriveState'] if REPLAY else time.monotonic_ns()
+        self.steer_limited_by_safety = is_angle_steering_limited(
+          self.CP, CC.actuators.steeringAngleDeg, CO, output_healthy, now_nanos,
+        )
       else:
         self.steer_limited_by_safety = abs(CC.actuators.torque - CO.actuatorsOutput.torque) > 1e-2
 

selfdrive/controls/lib/latcontrol_angle.py

@@ -3,9 +3,7 @@ import math
 from cereal import log
 from opendbc.car.subaru.values import CAR as SUBARU_CAR
 from openpilot.selfdrive.controls.lib.latcontrol import LatControl
-
-# TODO This is speed dependent
-STEER_ANGLE_SATURATION_THRESHOLD = 2.5  # Degrees
+from openpilot.selfdrive.controls.lib.steering_saturation import STEER_ANGLE_SATURATION_THRESHOLD
 
 _ASCENT_ANGLE_TRACKING_GAIN = 0.25
 _ASCENT_ANGLE_TRACKING_MAX_CORRECTION = 8.0

selfdrive/controls/lib/steering_saturation.py

@@ -0,0 +1,49 @@
+import math
+
+from opendbc.car.tesla.values import CAR, TeslaSafetyFlags
+
+
+# TODO This is speed dependent
+STEER_ANGLE_SATURATION_THRESHOLD = 2.5  # Degrees
+_MAX_STEERING_LIMIT_INFO_AGE_NANOS = 100_000_000
+
+
+def _legacy_angle_steering_limited(requested_angle: float, car_output) -> bool:
+  return abs(requested_angle - car_output.actuatorsOutput.steeringAngleDeg) > \
+    STEER_ANGLE_SATURATION_THRESHOLD
+
+
+def is_angle_steering_limited(CP, requested_angle: float, car_output, output_healthy: bool, now_nanos: int) -> bool:
+  legacy_limited = _legacy_angle_steering_limited(requested_angle, car_output)
+
+  try:
+    cooperative_enabled = any(
+      safety_config.safetyParam & TeslaSafetyFlags.COOP_STEERING.value
+      for safety_config in CP.safetyConfigs
+    )
+    if CP.carFingerprint != CAR.TESLA_MODEL_3 or not cooperative_enabled or not output_healthy:
+      return legacy_limited
+
+    info = car_output.actuatorsOutput.steeringLimitInfo
+    errors = (
+      info.modelLimitErrorDeg,
+      info.resumeLimitErrorDeg,
+      info.cooperativeLimitErrorDeg,
+      info.combinedLimitErrorDeg,
+    )
+    numeric_values = errors + (info.cooperativeOffsetDeg,)
+    mono_time = info.monoTime
+
+    if not info.valid or mono_time <= 0:
+      return legacy_limited
+
+    age_nanos = now_nanos - mono_time
+    if age_nanos < 0 or age_nanos > _MAX_STEERING_LIMIT_INFO_AGE_NANOS:
+      return legacy_limited
+
+    if not all(math.isfinite(value) for value in numeric_values) or any(error < 0 for error in errors):
+      return legacy_limited
+
+    return max(errors) > STEER_ANGLE_SATURATION_THRESHOLD
+  except (AttributeError, TypeError, ValueError, OverflowError):
+    return legacy_limited

selfdrive/controls/tests/test_steering_saturation.py

@@ -0,0 +1,161 @@
+import math
+
+import pytest
+
+from cereal import car
+from opendbc.car.tesla.interface import CarInterface
+from opendbc.car.tesla.values import CAR, TeslaSafetyFlags
+from openpilot.selfdrive.controls.lib.steering_saturation import is_angle_steering_limited
+
+
+REQUESTED_ANGLE = -14.5
+OUTPUT_ANGLE = -9.0
+SAMPLE_TIME_NANOS = 1_000_000_000
+FRESH_TIME_NANOS = SAMPLE_TIME_NANOS + 20_000_000
+MAX_DIAGNOSTIC_AGE_NANOS = 100_000_000
+NEXT_FLOAT32_AFTER_2_5 = 2.500000238418579
+ERROR_FIELDS = (
+  "modelLimitErrorDeg",
+  "resumeLimitErrorDeg",
+  "cooperativeLimitErrorDeg",
+  "combinedLimitErrorDeg",
+)
+NUMERIC_FIELDS = ERROR_FIELDS + ("cooperativeOffsetDeg",)
+
+
+def make_case(candidate=CAR.TESLA_MODEL_3, cooperative_enabled=True):
+  cp = CarInterface.get_non_essential_params(candidate)
+  for safety_config in cp.safetyConfigs:
+    if cooperative_enabled:
+      safety_config.safetyParam |= TeslaSafetyFlags.COOP_STEERING.value
+    else:
+      safety_config.safetyParam &= ~TeslaSafetyFlags.COOP_STEERING.value
+
+  output = car.CarOutput.new_message()
+  output.actuatorsOutput.steeringAngleDeg = OUTPUT_ANGLE
+  info = output.actuatorsOutput.steeringLimitInfo
+  info.valid = True
+  info.monoTime = SAMPLE_TIME_NANOS
+  info.cooperativeOffsetDeg = 5.5
+  return cp, output
+
+
+def detect(cp, output, requested_angle=REQUESTED_ANGLE, output_healthy=True, now_nanos=FRESH_TIME_NANOS):
+  return is_angle_steering_limited(cp.as_reader(), requested_angle, output.as_reader(), output_healthy, now_nanos)
+
+
+def test_light_offset_does_not_count_as_limiting():
+  cp, output = make_case()
+
+  assert not detect(cp, output)
+
+
+@pytest.mark.parametrize("field", ERROR_FIELDS)
+def test_each_genuine_limit_is_visible_during_cooperation(field):
+  cp, output = make_case()
+  setattr(output.actuatorsOutput.steeringLimitInfo, field, 3.0)
+
+  assert detect(cp, output)
+
+
+def test_combined_small_limits_remain_visible():
+  cp, output = make_case()
+  info = output.actuatorsOutput.steeringLimitInfo
+  info.modelLimitErrorDeg = 1.5
+  info.cooperativeLimitErrorDeg = 1.5
+  info.combinedLimitErrorDeg = 3.0
+
+  assert detect(cp, output)
+
+
+@pytest.mark.parametrize(("error", "expected"), (
+  (2.5, False),
+  (NEXT_FLOAT32_AFTER_2_5, True),
+))
+def test_diagnostic_threshold_is_strictly_greater(error, expected):
+  cp, output = make_case()
+  output.actuatorsOutput.steeringLimitInfo.modelLimitErrorDeg = error
+
+  assert detect(cp, output) is expected
+
+
+@pytest.mark.parametrize("age_nanos", (0, MAX_DIAGNOSTIC_AGE_NANOS))
+def test_diagnostic_age_bounds_are_inclusive(age_nanos):
+  cp, output = make_case()
+
+  assert not detect(cp, output, now_nanos=SAMPLE_TIME_NANOS + age_nanos)
+
+
+def test_signed_cooperative_offset_is_valid_data():
+  cp, output = make_case()
+  output.actuatorsOutput.steeringLimitInfo.cooperativeOffsetDeg = -5.5
+
+  assert not detect(cp, output)
+
+
+@pytest.mark.parametrize("scenario", (
+  "default_message",
+  "invalid_flag",
+  "zero_timestamp",
+  "future_timestamp",
+  "stale_timestamp",
+  "unhealthy_output",
+  "unsupported_model",
+  "cooperative_mode_disabled",
+))
+def test_unusable_diagnostics_keep_legacy_warning(scenario):
+  cp, output = make_case()
+  output_healthy = True
+  now_nanos = FRESH_TIME_NANOS
+
+  if scenario == "default_message":
+    output = car.CarOutput.new_message()
+    output.actuatorsOutput.steeringAngleDeg = OUTPUT_ANGLE
+  elif scenario == "invalid_flag":
+    output.actuatorsOutput.steeringLimitInfo.valid = False
+  elif scenario == "zero_timestamp":
+    output.actuatorsOutput.steeringLimitInfo.monoTime = 0
+  elif scenario == "future_timestamp":
+    output.actuatorsOutput.steeringLimitInfo.monoTime = now_nanos + 1
+  elif scenario == "stale_timestamp":
+    output.actuatorsOutput.steeringLimitInfo.monoTime = now_nanos - MAX_DIAGNOSTIC_AGE_NANOS - 1
+  elif scenario == "unhealthy_output":
+    output_healthy = False
+  elif scenario == "unsupported_model":
+    cp, output = make_case(CAR.TESLA_MODEL_Y)
+  elif scenario == "cooperative_mode_disabled":
+    cp, output = make_case(cooperative_enabled=False)
+
+  assert detect(cp, output, output_healthy=output_healthy, now_nanos=now_nanos)
+
+
+@pytest.mark.parametrize(("requested_angle", "expected"), (
+  (OUTPUT_ANGLE - 2.5, False),
+  (OUTPUT_ANGLE - NEXT_FLOAT32_AFTER_2_5, True),
+))
+def test_fallback_preserves_legacy_strict_threshold(requested_angle, expected):
+  cp, output = make_case()
+  output.actuatorsOutput.steeringLimitInfo.valid = False
+
+  assert detect(cp, output, requested_angle=requested_angle) is expected
+
+
+@pytest.mark.parametrize("field", NUMERIC_FIELDS)
+@pytest.mark.parametrize(("bad_value", "requested_angle", "legacy_result"), (
+  (math.nan, REQUESTED_ANGLE, True),
+  (math.inf, OUTPUT_ANGLE, False),
+  (-math.inf, OUTPUT_ANGLE, False),
+))
+def test_nonfinite_diagnostic_values_use_legacy_fallback(field, bad_value, requested_angle, legacy_result):
+  cp, output = make_case()
+  setattr(output.actuatorsOutput.steeringLimitInfo, field, bad_value)
+
+  assert detect(cp, output, requested_angle=requested_angle) is legacy_result
+
+
+@pytest.mark.parametrize("field", ERROR_FIELDS)
+def test_negative_error_values_use_legacy_fallback(field):
+  cp, output = make_case()
+  setattr(output.actuatorsOutput.steeringLimitInfo, field, -0.1)
+
+  assert detect(cp, output)

selfdrive/controls/tests/test_steering_saturation_publish.py

@@ -0,0 +1,153 @@
+from types import SimpleNamespace
+
+import pytest
+
+from cereal import car, custom, log
+from opendbc.car.tesla.interface import CarInterface
+from opendbc.car.tesla.values import CAR, TeslaSafetyFlags
+from openpilot.selfdrive.controls import controlsd
+from openpilot.selfdrive.controls.controlsd import Controls
+
+
+REQUESTED_ANGLE = -14.5
+OUTPUT_ANGLE = -9.0
+SAMPLE_TIME_NANOS = 1_000_000_000
+FRESH_TIME_NANOS = SAMPLE_TIME_NANOS + 20_000_000
+STALE_TIME_NANOS = SAMPLE_TIME_NANOS + 100_000_001
+HOST_TIME_NANOS = 9_000_000_000
+CAR_STATE_TIME_NANOS = 7_000_000_000
+
+
+class CapturePubMaster:
+  def __init__(self):
+    self.sent = {}
+
+  def send(self, service, message):
+    self.sent[service] = message
+
+
+class PublishSubMaster:
+  def __init__(self, car_output, selfdrive_time_nanos, output_healthy=True):
+    car_state = car.CarState.new_message()
+    car_state.canValid = True
+
+    long_plan = log.LongitudinalPlan.new_message()
+    long_plan.speeds = []
+
+    selfdrive_state = log.SelfdriveState.new_message()
+    selfdrive_state.active = True
+
+    self.messages = {
+      "carState": car_state.as_reader(),
+      "longitudinalPlan": long_plan.as_reader(),
+      "starpilotCarState": custom.StarPilotCarState.new_message().as_reader(),
+      "selfdriveState": selfdrive_state.as_reader(),
+      "carOutput": car_output.as_reader(),
+      "driverAssistance": log.DriverAssistance.new_message().as_reader(),
+      "driverMonitoringState": log.DriverMonitoringState.new_message().as_reader(),
+    }
+    self.valid = {"carOutput": output_healthy, "driverAssistance": False}
+    self.alive = {"carOutput": output_healthy}
+    self.freq_ok = {"carOutput": output_healthy}
+    self.logMonoTime = {
+      "selfdriveState": selfdrive_time_nanos,
+      "carState": CAR_STATE_TIME_NANOS,
+      "longitudinalPlan": 0,
+      "modelV2": 0,
+    }
+
+  def __getitem__(self, service):
+    return self.messages[service]
+
+
+def make_car_params():
+  cp = CarInterface.get_non_essential_params(CAR.TESLA_MODEL_3)
+  cp.safetyConfigs[0].safetyParam |= TeslaSafetyFlags.COOP_STEERING.value
+  return cp.as_reader()
+
+
+def make_car_output(real_limit_error=0.0):
+  output = car.CarOutput.new_message()
+  output.actuatorsOutput.steeringAngleDeg = OUTPUT_ANGLE
+  info = output.actuatorsOutput.steeringLimitInfo
+  info.valid = True
+  info.monoTime = SAMPLE_TIME_NANOS
+  info.cooperativeOffsetDeg = 5.5
+  info.modelLimitErrorDeg = real_limit_error
+  info.combinedLimitErrorDeg = real_limit_error
+  return output
+
+
+def make_controls(car_output, selfdrive_time_nanos, output_healthy=True):
+  controls = Controls.__new__(Controls)
+  controls.CP = make_car_params()
+  controls.sm = PublishSubMaster(car_output, selfdrive_time_nanos, output_healthy)
+  controls.pm = CapturePubMaster()
+  controls.curvature = 0.0
+  controls.calibrated_pose = None
+  controls.desired_curvature = 0.0
+  controls.LoC = SimpleNamespace(
+    long_control_state=car.CarControl.Actuators.LongControlState.off,
+    pid=SimpleNamespace(p=0.0, i=0.0, f=0.0),
+  )
+  controls.LaC = SimpleNamespace()
+  controls.starpilot_toggles = SimpleNamespace()
+  controls.steer_limited_by_safety = False
+  return controls
+
+
+def run_publish(monkeypatch, replay, selfdrive_time_nanos, host_time_nanos=HOST_TIME_NANOS,
+                output_healthy=True, real_limit_error=0.0):
+  monkeypatch.setattr(controlsd, "REPLAY", replay, raising=False)
+  monkeypatch.setattr(controlsd.time, "monotonic_ns", lambda: host_time_nanos)
+  controls = make_controls(make_car_output(real_limit_error), selfdrive_time_nanos, output_healthy)
+  cc = car.CarControl.new_message()
+  cc.enabled = True
+  cc.latActive = True
+  cc.actuators.steeringAngleDeg = REQUESTED_ANGLE
+  lac_log = log.ControlsState.LateralAngleState.new_message()
+
+  controls.publish(cc, lac_log)
+  return controls
+
+
+def test_replay_uses_current_poll_timestamp_for_fresh_diagnostics(monkeypatch):
+  controls = run_publish(monkeypatch, True, FRESH_TIME_NANOS)
+
+  assert not controls.steer_limited_by_safety
+
+
+def test_replay_stale_diagnostics_still_use_legacy_fallback(monkeypatch):
+  controls = run_publish(monkeypatch, True, STALE_TIME_NANOS)
+
+  assert controls.steer_limited_by_safety
+
+
+@pytest.mark.parametrize(("selfdrive_time_nanos", "expected_limited"), (
+  (SAMPLE_TIME_NANOS + 100_000_000, False),
+  (SAMPLE_TIME_NANOS + 100_000_001, True),
+  (SAMPLE_TIME_NANOS - 1, True),
+  (0, True),
+))
+def test_replay_publish_preserves_diagnostic_age_boundaries(monkeypatch, selfdrive_time_nanos, expected_limited):
+  controls = run_publish(monkeypatch, True, selfdrive_time_nanos)
+
+  assert controls.steer_limited_by_safety is expected_limited
+
+
+def test_live_uses_monotonic_clock_instead_of_message_clock(monkeypatch):
+  controls = run_publish(monkeypatch, False, FRESH_TIME_NANOS, host_time_nanos=STALE_TIME_NANOS)
+
+  assert controls.steer_limited_by_safety
+
+
+def test_unhealthy_car_output_still_uses_legacy_fallback(monkeypatch):
+  controls = run_publish(monkeypatch, True, FRESH_TIME_NANOS, output_healthy=False)
+
+  assert controls.steer_limited_by_safety
+
+
+def test_replay_genuine_limiter_error_remains_visible(monkeypatch):
+  controls = run_publish(monkeypatch, True, FRESH_TIME_NANOS, real_limit_error=3.0)
+
+  assert controls.steer_limited_by_safety

selfdrive/selfdrived/tests/test_tesla_coop_saturation_warning.py

@@ -0,0 +1,331 @@
+from types import SimpleNamespace
+
+import cereal.messaging as messaging
+import pytest
+
+from cereal import car, custom, log
+from opendbc.car import DT_CTRL, gen_empty_fingerprint
+from opendbc.car.tesla.carcontroller import CarController
+from opendbc.car.tesla.interface import CarInterface
+from opendbc.car.tesla.values import CAR, DBC
+from opendbc.car.vehicle_model import VehicleModel
+from openpilot.common.params import Params
+from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle
+from openpilot.selfdrive.controls.lib.steering_saturation import is_angle_steering_limited
+from openpilot.selfdrive.selfdrived.events import ET
+from openpilot.selfdrive.selfdrived.selfdrived import SelfdriveD
+
+
+REPRO_REQUESTED_ANGLE = -14.505379676818848
+REPRO_MEASURED_ANGLE = -10.0
+REPRO_SPEED = 12.509657859802246
+REPRO_TORQUE = 0.8999999761581421
+REPRO_DESIRED_CURVATURE = 0.006728461943566799
+REPRO_ACTUAL_CURVATURE = 0.004746654070913792
+START_NANOS = 1_000_000_000
+
+
+def make_params():
+  toggles = SimpleNamespace(tesla_cooperative_steering=True, trailer_load_kg=0.0)
+  return CarInterface.get_params(CAR.TESLA_MODEL_3, gen_empty_fingerprint(), [], False, False, False, toggles)
+
+
+def make_controller_state(torque, speed, measured_angle, steering_disengage=False):
+  return SimpleNamespace(
+    out=SimpleNamespace(
+      steeringTorque=torque,
+      steeringAngleDeg=measured_angle,
+      steeringDisengage=steering_disengage,
+      vEgo=speed,
+      vEgoRaw=speed,
+      gasPressed=False,
+    ),
+    hands_on_level=0,
+    das_control={"DAS_controlCounter": 0},
+  )
+
+
+def make_car_control(requested_angle, lat_active=True):
+  control = car.CarControl.new_message()
+  control.enabled = lat_active
+  control.latActive = lat_active
+  control.actuators.steeringAngleDeg = requested_angle
+  return control
+
+
+def run_controller_frame(controller, requested_angle, torque, speed, measured_angle, frame,
+                         lat_active=True, steering_disengage=False):
+  now_nanos = START_NANOS + frame * 10_000_000
+  actuators, _ = controller.update(
+    make_car_control(requested_angle, lat_active).as_reader(),
+    make_controller_state(torque, speed, measured_angle, steering_disengage),
+    now_nanos,
+    SimpleNamespace(),
+  )
+  event = messaging.new_message("carOutput", valid=True)
+  event.carOutput.actuatorsOutput = actuators
+  restored = messaging.log_from_bytes(event.to_bytes())
+  return restored.carOutput, now_nanos
+
+
+def make_lateral_car_state(speed, measured_angle):
+  state = car.CarState.new_message()
+  state.canValid = True
+  state.vEgo = speed
+  state.vEgoRaw = speed
+  state.steeringAngleDeg = measured_angle
+  state.gearShifter = car.CarState.GearShifter.drive
+  state.cruiseState.available = True
+  state.cruiseState.enabled = True
+  return state
+
+
+def advance_angle_counter(controller, CP, state, limited, desired_curvature):
+  live_params = log.LiveParametersData.new_message()
+  live_params.steerRatio = CP.steerRatio
+  live_params.stiffnessFactor = 1.0
+  live_params.roll = 0.0
+  live_params.angleOffsetDeg = 0.0
+  _, _, angle_log = controller.update(
+    True, state.as_reader(), VehicleModel(CP), live_params.as_reader(), limited,
+    desired_curvature, False, 0.2, None, None, SimpleNamespace(),
+  )
+  return angle_log
+
+
+def controls_state_with_angle_log(angle_log, actual_curvature):
+  controls_state = log.ControlsState.new_message()
+  controls_state.curvature = actual_curvature
+  controls_state.lateralControlState.angleState = angle_log
+  return controls_state
+
+
+def configure_selfdrived(CP):
+  fpcp = custom.StarPilotCarParams.new_message()
+  Params().put("StarPilotCarParams", fpcp.to_bytes())
+  selfdrived = SelfdriveD(CP.as_reader())
+  selfdrived.initialized = True
+  selfdrived.enabled = True
+  selfdrived.active = True
+  selfdrived.startup_event = None
+  selfdrived.last_steering_pressed_frame = 0
+  selfdrived.sm.frame = 300
+
+  for service in selfdrived.sm.data:
+    selfdrived.sm.valid[service] = True
+    selfdrived.sm.alive[service] = True
+    selfdrived.sm.freq_ok[service] = True
+    selfdrived.sm.seen[service] = True
+    selfdrived.sm.updated[service] = False
+    selfdrived.sm.recv_frame[service] = selfdrived.sm.frame
+
+  device_state = log.DeviceState.new_message()
+  device_state.started = True
+  device_state.freeSpacePercent = 100.0
+  device_state.memoryUsagePercent = 0
+  selfdrived.sm.data["deviceState"] = device_state.as_reader()
+
+  live_calibration = log.LiveCalibrationData.new_message()
+  live_calibration.calStatus = log.LiveCalibrationData.Status.calibrated
+  selfdrived.sm.data["liveCalibration"] = live_calibration.as_reader()
+
+  live_parameters = log.LiveParametersData.new_message()
+  live_parameters.valid = True
+  selfdrived.sm.data["liveParameters"] = live_parameters.as_reader()
+
+  panda_states = messaging.new_message("pandaStates", 0)
+  selfdrived.sm.data["pandaStates"] = panda_states.pandaStates
+  return selfdrived
+
+
+def run_selfdrived_warning_path(selfdrived, state, angle_log, actual_curvature, desired_curvature,
+                                requested_angle=REPRO_REQUESTED_ANGLE):
+  controls_state = controls_state_with_angle_log(angle_log, actual_curvature)
+  model = log.ModelDataV2.new_message()
+  model.action.desiredCurvature = desired_curvature
+  selfdrived.sm.data["controlsState"] = controls_state.as_reader()
+  selfdrived.sm.data["modelV2"] = model.as_reader()
+  selfdrived.sm.data["carControl"] = make_car_control(requested_angle).as_reader()
+  selfdrived.CS_prev = state.as_reader()
+  selfdrived.update_events(state.as_reader())
+  alerts = selfdrived.events.create_alerts([ET.WARNING])
+  return selfdrived.events.names, [alert.alert_type for alert in alerts]
+
+
+def test_recorded_light_torque_reproduction_no_longer_reaches_warning():
+  CP = make_params()
+  tesla_controller = CarController(DBC[CAR.TESLA_MODEL_3], CP)
+  lateral_state = make_lateral_car_state(REPRO_SPEED, REPRO_MEASURED_ANGLE)
+  baseline_counter = LatControlAngle(CP.as_reader(), None, DT_CTRL)
+  corrected_counter = LatControlAngle(CP.as_reader(), None, DT_CTRL)
+
+  baseline_log = corrected_log = None
+  output = None
+  now_nanos = 0
+  for frame in range(260):
+    output, now_nanos = run_controller_frame(
+      tesla_controller, REPRO_REQUESTED_ANGLE, REPRO_TORQUE, REPRO_SPEED, REPRO_MEASURED_ANGLE, frame,
+    )
+    if frame >= 210:
+      legacy_limited = abs(REPRO_REQUESTED_ANGLE - output.actuatorsOutput.steeringAngleDeg) > 2.5
+      corrected_limited = is_angle_steering_limited(
+        CP.as_reader(), REPRO_REQUESTED_ANGLE, output, True, now_nanos,
+      )
+      baseline_log = advance_angle_counter(
+        baseline_counter, CP, lateral_state, legacy_limited, REPRO_DESIRED_CURVATURE,
+      )
+      corrected_log = advance_angle_counter(
+        corrected_counter, CP, lateral_state, corrected_limited, REPRO_DESIRED_CURVATURE,
+      )
+
+  info = output.actuatorsOutput.steeringLimitInfo
+  errors = (info.modelLimitErrorDeg, info.resumeLimitErrorDeg,
+            info.cooperativeLimitErrorDeg, info.combinedLimitErrorDeg)
+  assert info.valid
+  assert info.cooperativeOffsetDeg > 2.5
+  assert max(errors) <= 2.5
+  assert baseline_log.saturated
+  assert not corrected_log.saturated
+
+  selfdrived = configure_selfdrived(CP)
+  baseline_events, baseline_alert_types = run_selfdrived_warning_path(
+    selfdrived, lateral_state, baseline_log, REPRO_ACTUAL_CURVATURE, REPRO_DESIRED_CURVATURE,
+  )
+  assert log.OnroadEvent.EventName.steerSaturated in baseline_events
+  assert "steerSaturated/warning" in baseline_alert_types
+
+  selfdrived.sm.frame += 1
+  events, alert_types = run_selfdrived_warning_path(
+    selfdrived, lateral_state, corrected_log, REPRO_ACTUAL_CURVATURE, REPRO_DESIRED_CURVATURE,
+  )
+  assert log.OnroadEvent.EventName.steerSaturated not in events
+  assert "steerSaturated/warning" not in alert_types
+
+
+def test_persistent_real_model_limiting_with_light_torque_still_warns():
+  CP = make_params()
+  tesla_controller = CarController(DBC[CAR.TESLA_MODEL_3], CP)
+  speed = 22.0
+  requested_angle = 40.0
+  lateral_state = make_lateral_car_state(speed, 0.0)
+  angle_counter = LatControlAngle(CP.as_reader(), None, DT_CTRL)
+
+  angle_log = None
+  output = None
+  for frame in range(60):
+    output, now_nanos = run_controller_frame(
+      tesla_controller, requested_angle, REPRO_TORQUE, speed, 0.0, frame,
+    )
+    limited = is_angle_steering_limited(CP.as_reader(), requested_angle, output, True, now_nanos)
+    angle_log = advance_angle_counter(angle_counter, CP, lateral_state, limited, 0.01)
+
+  info = output.actuatorsOutput.steeringLimitInfo
+  assert info.valid
+  assert info.modelLimitErrorDeg > 2.5
+  assert info.combinedLimitErrorDeg > 2.5
+  assert angle_log.saturated
+
+  selfdrived = configure_selfdrived(CP)
+  events, alert_types = run_selfdrived_warning_path(
+    selfdrived, lateral_state, angle_log, 0.003, 0.01, requested_angle,
+  )
+  assert log.OnroadEvent.EventName.steerSaturated in events
+  assert "steerSaturated/warning" in alert_types
+
+
+def test_default_old_output_uses_legacy_warning_path():
+  CP = make_params()
+  output_event = messaging.new_message("carOutput", valid=True)
+  output_event.carOutput.actuatorsOutput.steeringAngleDeg = REPRO_MEASURED_ANGLE
+  output = messaging.log_from_bytes(output_event.to_bytes()).carOutput
+  lateral_state = make_lateral_car_state(REPRO_SPEED, REPRO_MEASURED_ANGLE)
+  angle_counter = LatControlAngle(CP.as_reader(), None, DT_CTRL)
+
+  for frame in range(50):
+    limited = is_angle_steering_limited(
+      CP.as_reader(), REPRO_REQUESTED_ANGLE, output, True, START_NANOS + frame * 10_000_000,
+    )
+    angle_log = advance_angle_counter(
+      angle_counter, CP, lateral_state, limited, REPRO_DESIRED_CURVATURE,
+    )
+
+  assert not output.actuatorsOutput.steeringLimitInfo.valid
+  assert angle_log.saturated
+  selfdrived = configure_selfdrived(CP)
+  events, _ = run_selfdrived_warning_path(
+    selfdrived, lateral_state, angle_log, REPRO_ACTUAL_CURVATURE, REPRO_DESIRED_CURVATURE,
+  )
+  assert log.OnroadEvent.EventName.steerSaturated in events
+
+
+def test_stale_real_offset_sample_uses_legacy_warning_path():
+  CP = make_params()
+  tesla_controller = CarController(DBC[CAR.TESLA_MODEL_3], CP)
+  lateral_state = make_lateral_car_state(REPRO_SPEED, REPRO_MEASURED_ANGLE)
+  angle_counter = LatControlAngle(CP.as_reader(), None, DT_CTRL)
+
+  for frame in range(260):
+    output, now_nanos = run_controller_frame(
+      tesla_controller, REPRO_REQUESTED_ANGLE, REPRO_TORQUE, REPRO_SPEED, REPRO_MEASURED_ANGLE, frame,
+    )
+
+  assert not is_angle_steering_limited(CP.as_reader(), REPRO_REQUESTED_ANGLE, output, True, now_nanos)
+  stale_now_nanos = output.actuatorsOutput.steeringLimitInfo.monoTime + 100_000_001
+  for _ in range(50):
+    limited = is_angle_steering_limited(
+      CP.as_reader(), REPRO_REQUESTED_ANGLE, output, True, stale_now_nanos,
+    )
+    angle_log = advance_angle_counter(
+      angle_counter, CP, lateral_state, limited, REPRO_DESIRED_CURVATURE,
+    )
+
+  assert angle_log.saturated
+  selfdrived = configure_selfdrived(CP)
+  events, _ = run_selfdrived_warning_path(
+    selfdrived, lateral_state, angle_log, REPRO_ACTUAL_CURVATURE, REPRO_DESIRED_CURVATURE,
+  )
+  assert log.OnroadEvent.EventName.steerSaturated in events
+
+
+def test_inactive_interval_clears_diagnostics_before_reengagement():
+  CP = make_params()
+  tesla_controller = CarController(DBC[CAR.TESLA_MODEL_3], CP)
+
+  active, _ = run_controller_frame(tesla_controller, 0.0, REPRO_TORQUE, REPRO_SPEED, 0.0, 0)
+  inactive, _ = run_controller_frame(
+    tesla_controller, 0.0, REPRO_TORQUE, REPRO_SPEED, 0.0, 1, lat_active=False,
+  )
+  resumed, resumed_now = run_controller_frame(
+    tesla_controller, 0.0, REPRO_TORQUE, REPRO_SPEED, 0.0, 2,
+  )
+  overridden, _ = run_controller_frame(
+    tesla_controller, 0.0, REPRO_TORQUE, REPRO_SPEED, 0.0, 3, steering_disengage=True,
+  )
+
+  assert active.actuatorsOutput.steeringLimitInfo.valid
+  assert not inactive.actuatorsOutput.steeringLimitInfo.valid
+  assert inactive.actuatorsOutput.steeringLimitInfo.monoTime == 0
+  assert resumed.actuatorsOutput.steeringLimitInfo.valid
+  assert resumed.actuatorsOutput.steeringLimitInfo.monoTime == resumed_now
+  assert not overridden.actuatorsOutput.steeringLimitInfo.valid
+  assert overridden.actuatorsOutput.steeringLimitInfo.monoTime == 0
+
+
+@pytest.mark.parametrize(("fault_field", "expected_event"), (
+  ("steerFaultTemporary", log.OnroadEvent.EventName.steerTempUnavailableSilent),
+  ("steerFaultPermanent", log.OnroadEvent.EventName.steerUnavailable),
+))
+def test_existing_steering_fault_events_remain_present(fault_field, expected_event):
+  CP = make_params()
+  state = make_lateral_car_state(REPRO_SPEED, REPRO_MEASURED_ANGLE)
+  setattr(state, fault_field, True)
+  angle_counter = LatControlAngle(CP.as_reader(), None, DT_CTRL)
+  angle_log = advance_angle_counter(
+    angle_counter, CP, state, False, REPRO_DESIRED_CURVATURE,
+  )
+
+  selfdrived = configure_selfdrived(CP)
+  events, _ = run_selfdrived_warning_path(
+    selfdrived, state, angle_log, REPRO_ACTUAL_CURVATURE, REPRO_DESIRED_CURVATURE,
+  )
+  assert expected_event in events