Subaru: the high-angle guard hands back on the angle alone

2026-11-12 07:23:45 +08:00 · 2026-11-11 16:13:22 -06:00
parent 9dadd4e4a8
commit bf0880f064
7 changed files with 133 additions and 6 deletions
@@ -160,7 +160,8 @@ class CarController(CarControllerBase):
          _projection.input(new_torque, self.p.STEER_MAX, self.apply_torque_last, CS.out.steeringTorque, self.p, limiter='driver')
        apply_torque = apply_driver_steer_torque_limits(new_torque, self.apply_torque_last, CS.out.steeringTorque, self.p)

-        if not CC.latActive:
+        torque_active = CC.latActive and not getattr(CS, "high_angle_cut", False)
+        if not torque_active:
          apply_torque = 0

        if self.CP.flags & SubaruFlags.PREGLOBAL:
@@ -168,7 +169,7 @@ class CarController(CarControllerBase):
          if _projection is not None:
            _projection.emit('subaru.PREGLOBAL_ES_LKAS', apply_torque, CC.latActive, can_sends[-1])
        else:
-          apply_steer_req = CC.latActive
+          apply_steer_req = torque_active

          if self.CP.flags & SubaruFlags.STEER_RATE_LIMITED:
            # Steering rate fault prevention
@@ -30,6 +30,9 @@ class CarState(CarStateBase):
    self.software_buttons_msg = {}
    self.es_lkas_alert_dat = b""
    self.angle_rate_calulator = CanSignalRateCalculator(50)
+    from opendbc.car.subaru.high_angle_guard import HighAngleGuard, selected
+    self.high_angle_guard = HighAngleGuard() if selected(CP) else None
+    self.high_angle_cut = False

  def update(self, can_parsers) -> structs.CarState:
    cp = can_parsers[Bus.pt]
@@ -132,7 +135,10 @@ class CarState(CarStateBase):
      self.cruise_button = cp_cam.vl["ES_Distance"]["Cruise_Button"]
      self.ready = not cp_cam.vl["ES_DashStatus"]["Not_Ready_Startup"]
    else:
-      ret.steerFaultTemporary = cp_angle.vl["Steering_Torque"]["Steer_Warning"] == 1
+      if self.high_angle_guard is not None:
+        self.high_angle_cut = self.high_angle_guard.update(
+          cp.vl["Brake_Pressure_L_R"]["Steering_Angle"], cp.vl["Brake_Pedal"]["Speed"], ret.steeringTorque)
+      ret.steerFaultTemporary = cp_angle.vl["Steering_Torque"]["Steer_Warning"] == 1 or self.high_angle_cut
      ret.cruiseState.nonAdaptive = cp_cam.vl["ES_DashStatus"]["Conventional_Cruise"] == 1
      ret.cruiseState.standstill = cp_cam.vl["ES_DashStatus"]["Cruise_State"] == 3
      ret.stockFcw = (cp_cam.vl["ES_LKAS_State"]["LKAS_Alert"] == 1) or \
@@ -192,8 +198,10 @@ class CarState(CarStateBase):

  @staticmethod
  def get_can_parsers(CP):
+    from opendbc.car.subaru.high_angle_guard import selected
+    guard_checks = [("Brake_Pressure_L_R", 50), ("Brake_Pedal", 50)] if selected(CP) else []
    parsers = {
-      Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus.alt if CP.flags & SubaruFlags.D_PLATFORM else CanBus.main),
+      Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], guard_checks, CanBus.alt if CP.flags & SubaruFlags.D_PLATFORM else CanBus.main),
      Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], [("ES_LKAS_Alert", 0)] if CP.flags & SubaruFlags.ASCENT_LKAS_ALERT else [], CanBus.camera),
      Bus.alt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus.alt)
    }
@@ -0,0 +1,26 @@
+"""Conservative Impreza EPS guard adapted from Matt Purnell's Subaru work.
+
+Source: mpurnell1/opendbc c08d4d86d81048447ce0e2dbe1f50745adce096e,
+opendbc/car/subaru/carstate.py (high-angle refusal guard).
+"""
+from opendbc.car.subaru.values import CAR, SubaruFlags
+
+
+def selected(cp):
+  return bool(cp.carFingerprint == CAR.SUBARU_IMPREZA_2020 and
+              cp.flags & SubaruFlags.STEER_RATE_LIMITED and
+              not cp.flags & (SubaruFlags.LKAS_ANGLE | SubaruFlags.PREGLOBAL) and
+              not cp.openpilotLongitudinalControl)
+
+
+class HighAngleGuard:
+  def __init__(self):
+    self.cut = False
+
+  def update(self, vdc_angle, speed_kph, driver_torque):
+    # EPS reads VDC angle and Brake_Pedal speed; its own angle can differ substantially.
+    if speed_kph < 50 and abs(vdc_angle) >= 88 and abs(driver_torque) < 120:
+      self.cut = True
+    elif abs(vdc_angle) < 84 or speed_kph >= 50:
+      self.cut = False
+    return self.cut
@@ -0,0 +1,71 @@
+import pytest
+
+from opendbc.car import Bus, structs
+from opendbc.can import CANParser
+from opendbc.car.subaru.high_angle_guard import HighAngleGuard, selected
+from opendbc.car.subaru.interface import CarInterface
+from opendbc.car.subaru.values import CAR, DBC
+from openpilot.starpilot.car.subaru.tests.caller_helpers import CompleteCan
+
+
+@pytest.mark.parametrize('sign', [-1, 1])
+def test_guard_holds_until_vdc_angle_unwinds(sign):
+  guard = HighAngleGuard()
+  assert not guard.update(sign * 87.9, 8, 59)
+  assert guard.update(sign * 88, 8, 59)
+  assert guard.update(sign * 85, 8, 200)
+  assert guard.update(sign * 84, 8, 0)
+  assert not guard.update(sign * 83.9, 8, 59)
+  assert not guard.update(sign * 100, 8, 120)
+  assert guard.update(sign * 100, 8, 119)
+  assert not guard.update(sign * 100, 50, 59)
+
+
+@pytest.mark.parametrize('candidate', [CAR.SUBARU_IMPREZA_2020, CAR.SUBARU_IMPREZA, CAR.SUBARU_FORESTER, CAR.SUBARU_CROSSTREK_2025])
+def test_guard_scope(candidate):
+  cp = CarInterface.get_non_essential_params(candidate)
+  assert selected(cp) == (candidate == CAR.SUBARU_IMPREZA_2020)
+  cp.openpilotLongitudinalControl = True
+  assert not selected(cp)
+
+
+def _interface():
+  ci = CarInterface(CarInterface.get_non_essential_params(CAR.SUBARU_IMPREZA_2020))
+  stream = CompleteCan(ci)
+  return ci, stream
+
+
+def _values(vdc=0, warning=False):
+  return {'Steering_Torque': {'Steering_Angle': 70, 'Steer_Torque_Sensor': 59, 'Steer_Warning': int(warning)},
+          'Brake_Pressure_L_R': {'Steering_Angle': vdc}, 'Brake_Pedal': {'Speed': 8},
+          'Wheel_Speeds': dict.fromkeys(('FL', 'FR', 'RL', 'RR'), 8), 'Transmission': {'Gear': 121}}
+
+
+def test_real_parser_uses_vdc_angle_preserves_hardware_warning_and_zeroes_stale_command():
+  ci, stream = _interface()
+  for tick in range(100):
+    cs = ci.update([(5_000_000_000 + tick * 10_000_000, stream.frames(_values(101), tick=tick))])
+  assert cs.canValid and cs.steeringAngleDeg < 88 and cs.steerFaultTemporary
+  command = structs.CarControl(latActive=True, actuators=structs.CarControl.Actuators(torque=1))
+  decoder = CANParser(DBC[CAR.SUBARU_IMPREZA_2020][Bus.pt], [('ES_LKAS', 0)], 0)
+  for tick in range(100, 110):
+    now = 5_000_000_000 + tick * 10_000_000
+    output, outgoing = ci.apply(command.as_reader(), now)
+    decoder.update([(now, outgoing)])
+    assert output.torque == 0
+    if any(address == 0x122 for address, _, _ in outgoing):
+      assert decoder.vl['ES_LKAS']['LKAS_Request'] == 0
+      assert decoder.vl['ES_LKAS']['LKAS_Output'] == 0
+  cs = ci.update([(6_100_000_000, stream.frames(_values(0, warning=True), tick=110))])
+  assert not ci.CS.high_angle_cut and cs.steerFaultTemporary
+
+
+@pytest.mark.parametrize('missing', ['Brake_Pressure_L_R', 'Brake_Pedal'])
+def test_missing_guard_source_invalidates_actual_parser(missing):
+  ci, stream = _interface()
+  for tick in range(100):
+    cs = ci.update([(5_000_000_000 + tick * 10_000_000, stream.frames(_values(), tick=tick))])
+  assert cs.canValid
+  for tick in range(100, 250):
+    cs = ci.update([(5_000_000_000 + tick * 10_000_000, stream.frames(_values(), tick=tick, omit=((Bus.pt, missing),)))])
+  assert not cs.canValid
@@ -25,7 +25,8 @@ from openpilot.starpilot.car.subaru.tests.native_fixture import subaru_native_pa
 def test_actual_full_engage_brake_reverse_temporary_recovery_mainoff(candidate, subaru_native_pair, tmp_path, aol=True, manual=False, stop_resume=False,
                                                                      hard_stop_native_first=False, main_only=False, preferences=None, firmware=None,
                                                                      deadline_gap=False, fixed_angle_handoff=False, host_first_stop=False,
-                                                                      source_outage=False, vm_angle_handoff=0, crawl=False, eps_brake_join=False):
+                                                                      source_outage=False, vm_angle_handoff=0, crawl=False, eps_brake_join=False,
+                                                                      high_angle_guard=False):
  angle_handoff = fixed_angle_handoff or vm_angle_handoff
  fw = firmware_records(candidate, FW_VERSIONS, 'subaru') if firmware is None else firmware
  rows = []
@@ -95,6 +96,12 @@ def test_actual_full_engage_brake_reverse_temporary_recovery_mainoff(candidate,
          'ES_DashStatus': {'Cruise_On': int(main), 'Cruise_Set_Speed': 100},
          'Transmission': {'Gear': 3 if reverse else 121},
        }
+        if high_angle_guard:
+          values['Brake_Pedal'] = {'Speed': 8}
+          values['Brake_Pressure_L_R'] = {'Steering_Angle': 101 if 110 <= tick < 120 else 83 if 120 <= tick < 125 else 0}
+          if 110 <= tick < 125:
+            values['Steering_Torque'].update(Steering_Angle=100, Steer_Torque_Sensor=59)
+            values['Wheel_Speeds'] = dict.fromkeys(('FL', 'FR', 'RL', 'RR'), 8)
        incoming = stream.frames(values, tick=tick)
        if source_outage:
          pending_host_can.append((now, incoming))
@@ -273,6 +280,12 @@ def test_actual_full_engage_brake_reverse_temporary_recovery_mainoff(candidate,
          'events': [str(e.name) for e in driven['onroadEvents'].onroadEvents],
        }
        rows.append(row)
+        if high_angle_guard and 115 <= tick < 120:
+          assert cs.steerFaultTemporary and ci.CS.high_angle_cut, ('HIGH_ANGLE_GUARD_NOT_WITHDRAWN', row)
+          assert not row['lat'] and row['cardLatch'] and row['armed'] and row['permission'] == 0, ('HIGH_ANGLE_GUARD_NOT_WITHDRAWN', row)
+          assert all(not command['request'] and command['output'] == 0 and command['nativeAccepted'] for command in row['steering']), row
+        if high_angle_guard and 140 <= tick < 145:
+          assert not cs.steerFaultTemporary and not ci.CS.high_angle_cut and row['lat'] and row['cardLatch'] and row['permission'] == 1, row
        if stopped:
          assert not row['lat'] and row['cardLatch'] and row['permission'] == 0 and row['fault'] == 'none', ('SUBARU_STOP_LOST_ARMED_INTENT', row)
        if source_outage and 78 <= tick < 81:
@@ -454,3 +467,8 @@ def test_actual_crawl_lateral_only_tracks_small_targets(subaru_native_pair, tmp_
 def test_actual_eps_brake_event_join_preserves_armed_intent(subaru_native_pair, tmp_path):
  test_actual_full_engage_brake_reverse_temporary_recovery_mainoff(
    CAR.SUBARU_IMPREZA_2020, subaru_native_pair, tmp_path, main_only=True, eps_brake_join=True)
+
+
+def test_actual_impreza_high_angle_refusal_guard(subaru_native_pair, tmp_path):
+  test_actual_full_engage_brake_reverse_temporary_recovery_mainoff(
+    CAR.SUBARU_IMPREZA_2020, subaru_native_pair, tmp_path, main_only=True, high_angle_guard=True)
@@ -1 +1 @@
-{"protected_inventory_sha256": "29623ab48d26c161d75502f9ddeeb5cf416cc2f4e5bdafa8243660fadf9c4e96", "schema_version": 1}
+{"protected_inventory_sha256": "eb73f4fe767f8dd3ee21633f8f3cd1c055002f5870b288273306cdfbbf0393f3", "schema_version": 1}
@@ -41,6 +41,7 @@ FORD_BLUEPILOT_NATIVE_TARGETS = tuple(str(path.relative_to(ROOT)) for path in so
 # Representative production callers plus their source/native refusal cases.
 # The emitted selected_test_ids record exact parametrized configurations.
 AOL_LIFECYCLE_TARGETS = (
+  "openpilot/starpilot/car/subaru/tests/test_high_angle_guard.py",
  "openpilot/starpilot/aol/tests/test_ford_long_only_withdrawal.py",
  'opendbc_repo/opendbc/car/tests/test_isotp_query_interference.py',
  'openpilot/starpilot/aol/tests/test_mazda_source_deadline.py',
@@ -270,6 +271,8 @@ DEBUG_SAFETY_CLASS_TARGETS = {


 RELEASE_TARGETS = (
+  "openpilot/starpilot/car/subaru/tests/test_high_angle_guard.py",
+  "openpilot/starpilot/car/subaru/tests/test_stock_aol_callers.py::test_actual_impreza_high_angle_refusal_guard",
  "openpilot/starpilot/car/toyota/tests/test_radar_filter_buttons.py",
  "openpilot/starpilot/aol/tests/test_ford_long_only_withdrawal.py",
  "opendbc_repo/opendbc/safety/tests/test_hyundai_gv60_hda1_long_aol.py",