From bf0880f06447abeab194af3ff105b43c10e38c19 Mon Sep 17 00:00:00 2001 From: Matt Purnell Date: Wed, 11 Nov 2026 16:13:22 -0600 Subject: [PATCH] Subaru: the high-angle guard hands back on the angle alone --- .../opendbc/car/subaru/carcontroller.py | 5 +- opendbc_repo/opendbc/car/subaru/carstate.py | 12 +++- .../opendbc/car/subaru/high_angle_guard.py | 26 +++++++ .../car/subaru/tests/test_high_angle_guard.py | 71 +++++++++++++++++++ .../subaru/tests/test_stock_aol_callers.py | 20 +++++- prebuilt.json | 2 +- tools/ci/run_vehicle_tests.py | 3 + 7 files changed, 133 insertions(+), 6 deletions(-) create mode 100644 opendbc_repo/opendbc/car/subaru/high_angle_guard.py create mode 100644 openpilot/starpilot/car/subaru/tests/test_high_angle_guard.py diff --git a/opendbc_repo/opendbc/car/subaru/carcontroller.py b/opendbc_repo/opendbc/car/subaru/carcontroller.py index 706fea1d9d..126d3cb9b6 100644 --- a/opendbc_repo/opendbc/car/subaru/carcontroller.py +++ b/opendbc_repo/opendbc/car/subaru/carcontroller.py @@ -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 diff --git a/opendbc_repo/opendbc/car/subaru/carstate.py b/opendbc_repo/opendbc/car/subaru/carstate.py index 6053e2923e..e415f7f36e 100644 --- a/opendbc_repo/opendbc/car/subaru/carstate.py +++ b/opendbc_repo/opendbc/car/subaru/carstate.py @@ -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) } diff --git a/opendbc_repo/opendbc/car/subaru/high_angle_guard.py b/opendbc_repo/opendbc/car/subaru/high_angle_guard.py new file mode 100644 index 0000000000..99c9516b8c --- /dev/null +++ b/opendbc_repo/opendbc/car/subaru/high_angle_guard.py @@ -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 diff --git a/openpilot/starpilot/car/subaru/tests/test_high_angle_guard.py b/openpilot/starpilot/car/subaru/tests/test_high_angle_guard.py new file mode 100644 index 0000000000..51d1d48996 --- /dev/null +++ b/openpilot/starpilot/car/subaru/tests/test_high_angle_guard.py @@ -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 diff --git a/openpilot/starpilot/car/subaru/tests/test_stock_aol_callers.py b/openpilot/starpilot/car/subaru/tests/test_stock_aol_callers.py index 85364583cc..ed230aaf96 100644 --- a/openpilot/starpilot/car/subaru/tests/test_stock_aol_callers.py +++ b/openpilot/starpilot/car/subaru/tests/test_stock_aol_callers.py @@ -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) diff --git a/prebuilt.json b/prebuilt.json index be62478134..a9a89eb19c 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "29623ab48d26c161d75502f9ddeeb5cf416cc2f4e5bdafa8243660fadf9c4e96", "schema_version": 1} +{"protected_inventory_sha256": "eb73f4fe767f8dd3ee21633f8f3cd1c055002f5870b288273306cdfbbf0393f3", "schema_version": 1} diff --git a/tools/ci/run_vehicle_tests.py b/tools/ci/run_vehicle_tests.py index 1c7b8e61a0..f8d7492fc7 100644 --- a/tools/ci/run_vehicle_tests.py +++ b/tools/ci/run_vehicle_tests.py @@ -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",