diff --git a/opendbc_repo/opendbc/safety/modes/subaru.h b/opendbc_repo/opendbc/safety/modes/subaru.h index 2f50f3381d..a0c17de48b 100644 --- a/opendbc_repo/opendbc/safety/modes/subaru.h +++ b/opendbc_repo/opendbc/safety/modes/subaru.h @@ -132,10 +132,12 @@ static void subaru_rx_hook(const CANPacket_t *msg) { } if (subaru_lkas_angle && (msg->addr == MSG_SUBARU_ES_DashStatus) && (msg->bus == SUBARU_CAM_BUS)) { acc_main_on = GET_BIT(msg, 49U); - subaru_aol_main = acc_main_on; - subaru_aol_main_seen = true; - subaru_aol_main_ts = microsecond_timer_get(); - if (!acc_main_on) { + subaru_aol_observe_main(acc_main_on); + } + + if (subaru_aol_main_arm && msg_matches(msg, MSG_SUBARU_ES_Distance, SUBARU_ALT_BUS)) { + subaru_aol_acc_fault = GET_BIT(msg, 15U); + if (subaru_aol_acc_fault) { subaru_aol_clear(); } } @@ -401,6 +403,7 @@ static safety_config subaru_init(uint16_t param) { subaru_aol_enabled = ((unsigned int)alternative_experience == 32U) && ((param == 209U) || (param == 465U) || (param == 16U) || (param == 0U) || (param == 1U) || (param == 17U) || (param == 145U) || avh || (param == 177U) || (param == 401U) || (param == 8U) || (param == 520U) || (param == 512U) || (param == 2225U)); + subaru_aol_main_arm = subaru_aol_enabled && (param == 17U); if (subaru_aol_enabled) { static const AolSafetyPolicy policy = { .reset = subaru_aol_reset, @@ -413,39 +416,60 @@ static safety_config subaru_init(uint16_t param) { } safety_config ret; + // Only one Subaru profile is active; share its mutable RX workspace. + static RxCheck subaru_rx_workspace[8]; + int rx_count; if (subaru_lkas_angle) { - static RxCheck subaru_gen2_angle_rx_checks[] = { + static const RxCheck subaru_gen2_angle_rx_checks[] = { SUBARU_GEN2_ANGLE_RX_CHECKS() + {.msg = {{MSG_SUBARU_ES_Distance, SUBARU_ALT_BUS, 8, 20U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, }; + rx_count = sizeof(subaru_gen2_angle_rx_checks) / sizeof(subaru_gen2_angle_rx_checks[0]); + for (int i = 0; i < rx_count; i++) { + subaru_rx_workspace[i] = subaru_gen2_angle_rx_checks[i]; + } // Only one Subaru angle profile is active. Rebind the complete topology - // on every init; the safety selector resets all seven RX statuses afterward. - subaru_gen2_angle_rx_checks[0].msg[0].bus = subaru_d_platform ? SUBARU_ALT_BUS : SUBARU_MAIN_BUS; - subaru_gen2_angle_rx_checks[3].msg[0].bus = subaru_forester_angle ? SUBARU_MAIN_BUS : SUBARU_ALT_BUS; - subaru_gen2_angle_rx_checks[4].msg[0].bus = subaru_forester_angle ? SUBARU_MAIN_BUS : SUBARU_ALT_BUS; - subaru_gen2_angle_rx_checks[5].msg[0].bus = subaru_forester_angle ? SUBARU_CAM_BUS : SUBARU_ALT_BUS; - ret = subaru_d_platform ? (stop_start ? BUILD_SAFETY_CFG(subaru_gen2_angle_rx_checks, SUBARU_OUTBACK_STOP_START_TX_MSGS) : - BUILD_SAFETY_CFG(subaru_gen2_angle_rx_checks, SUBARU_ASCENT_ANGLE_TX_MSGS_LIST)) : - subaru_forester_angle ? BUILD_SAFETY_CFG(subaru_gen2_angle_rx_checks, SUBARU_FORESTER_ANGLE_TX_MSGS) : - avh ? (stop_start ? BUILD_SAFETY_CFG(subaru_gen2_angle_rx_checks, SUBARU_AVH_STOP_START_TX_MSGS) : - BUILD_SAFETY_CFG(subaru_gen2_angle_rx_checks, SUBARU_AVH_TX_MSGS)) : - stop_start ? BUILD_SAFETY_CFG(subaru_gen2_angle_rx_checks, SUBARU_STOP_START_TX_MSGS) : - ascent_alert ? BUILD_SAFETY_CFG(subaru_gen2_angle_rx_checks, SUBARU_ASCENT_ALERT_TX_MSGS) : - subaru_angle_main_bus ? BUILD_SAFETY_CFG(subaru_gen2_angle_rx_checks, SUBARU_ASCENT_ANGLE_TX_MSGS_LIST) : - BUILD_SAFETY_CFG(subaru_gen2_angle_rx_checks, SUBARU_GEN2_ANGLE_TX_MSGS_LIST); + // on every init; the safety selector resets every selected RX status afterward. + subaru_rx_workspace[0].msg[0].bus = subaru_d_platform ? SUBARU_ALT_BUS : SUBARU_MAIN_BUS; + subaru_rx_workspace[3].msg[0].bus = subaru_forester_angle ? SUBARU_MAIN_BUS : SUBARU_ALT_BUS; + subaru_rx_workspace[4].msg[0].bus = subaru_forester_angle ? SUBARU_MAIN_BUS : SUBARU_ALT_BUS; + subaru_rx_workspace[5].msg[0].bus = subaru_forester_angle ? SUBARU_CAM_BUS : SUBARU_ALT_BUS; + ret = subaru_d_platform ? (stop_start ? BUILD_SAFETY_CFG(subaru_rx_workspace, SUBARU_OUTBACK_STOP_START_TX_MSGS) : + BUILD_SAFETY_CFG(subaru_rx_workspace, SUBARU_ASCENT_ANGLE_TX_MSGS_LIST)) : + subaru_forester_angle ? BUILD_SAFETY_CFG(subaru_rx_workspace, SUBARU_FORESTER_ANGLE_TX_MSGS) : + avh ? (stop_start ? BUILD_SAFETY_CFG(subaru_rx_workspace, SUBARU_AVH_STOP_START_TX_MSGS) : + BUILD_SAFETY_CFG(subaru_rx_workspace, SUBARU_AVH_TX_MSGS)) : + stop_start ? BUILD_SAFETY_CFG(subaru_rx_workspace, SUBARU_STOP_START_TX_MSGS) : + ascent_alert ? BUILD_SAFETY_CFG(subaru_rx_workspace, SUBARU_ASCENT_ALERT_TX_MSGS) : + subaru_angle_main_bus ? BUILD_SAFETY_CFG(subaru_rx_workspace, SUBARU_ASCENT_ANGLE_TX_MSGS_LIST) : + BUILD_SAFETY_CFG(subaru_rx_workspace, SUBARU_GEN2_ANGLE_TX_MSGS_LIST); + // Keep the original seven checks on other profiles; only MAIN arming needs ACC health. + if (!subaru_aol_main_arm) { + rx_count--; + } } else if (subaru_gen2) { - static RxCheck subaru_gen2_rx_checks[] = { + static const RxCheck subaru_gen2_rx_checks[] = { SUBARU_COMMON_RX_CHECKS(SUBARU_ALT_BUS) }; - ret = BUILD_SAFETY_CFG(subaru_gen2_rx_checks, SUBARU_GEN2_TX_MSGS); + rx_count = sizeof(subaru_gen2_rx_checks) / sizeof(subaru_gen2_rx_checks[0]); + for (int i = 0; i < rx_count; i++) { + subaru_rx_workspace[i] = subaru_gen2_rx_checks[i]; + } + ret = BUILD_SAFETY_CFG(subaru_rx_workspace, SUBARU_GEN2_TX_MSGS); } else { - static RxCheck subaru_rx_checks[] = { + static const RxCheck subaru_rx_checks[] = { SUBARU_COMMON_RX_CHECKS(SUBARU_MAIN_BUS) }; + rx_count = sizeof(subaru_rx_checks) / sizeof(subaru_rx_checks[0]); + for (int i = 0; i < rx_count; i++) { + subaru_rx_workspace[i] = subaru_rx_checks[i]; + } ret = subaru_sng_enabled ? (subaru_software_enabled ? - BUILD_SAFETY_CFG(subaru_rx_checks, SUBARU_SNG_SOFTWARE_TX_MSGS) : BUILD_SAFETY_CFG(subaru_rx_checks, SUBARU_SNG_TX_MSGS)) : - subaru_software_enabled ? BUILD_SAFETY_CFG(subaru_rx_checks, SUBARU_SOFTWARE_TX_MSGS) : - BUILD_SAFETY_CFG(subaru_rx_checks, SUBARU_TX_MSGS); + BUILD_SAFETY_CFG(subaru_rx_workspace, SUBARU_SNG_SOFTWARE_TX_MSGS) : BUILD_SAFETY_CFG(subaru_rx_workspace, SUBARU_SNG_TX_MSGS)) : + subaru_software_enabled ? BUILD_SAFETY_CFG(subaru_rx_workspace, SUBARU_SOFTWARE_TX_MSGS) : + BUILD_SAFETY_CFG(subaru_rx_workspace, SUBARU_TX_MSGS); } + ret.rx_checks_len = rx_count; if (((unsigned int)alternative_experience == 32U) && !subaru_aol_enabled) { ret.tx_msgs = NULL; ret.tx_msgs_len = 0; diff --git a/opendbc_repo/opendbc/safety/modes/subaru_stock_aol.h b/opendbc_repo/opendbc/safety/modes/subaru_stock_aol.h index f63d837f1f..566feb8b38 100644 --- a/opendbc_repo/opendbc/safety/modes/subaru_stock_aol.h +++ b/opendbc_repo/opendbc/safety/modes/subaru_stock_aol.h @@ -1,7 +1,7 @@ #pragma once // The existing axis transport owns host request/heartbeat leases. This owner -// grants lateral only after ordinary engagement, with current Subaru sources. +// grants lateral after physical intent, with current Subaru sources. static bool subaru_aol_enabled = false; static uint8_t subaru_aol_gear_bus = 0U; static bool subaru_aol_armed = false; @@ -21,8 +21,13 @@ static uint8_t subaru_aol_gear_counter = 0U; static uint32_t subaru_aol_gear_ts = 0U; static uint32_t subaru_aol_main_ts = 0U; static bool subaru_aol_main_seen = false; +static bool subaru_aol_main_arm = false; +static bool subaru_aol_main_epoch = false; +static bool subaru_aol_main_low_seen = false; +static bool subaru_aol_acc_fault = false; static void subaru_aol_clear(void) { + subaru_aol_main_epoch = false; subaru_aol_armed = false; subaru_aol_token = false; subaru_aol_pending = false; @@ -43,6 +48,9 @@ static void subaru_aol_reset(void) { subaru_aol_gear_seen = false; subaru_aol_gear_qualified = false; subaru_aol_main_seen = false; + subaru_aol_main_arm = false; + subaru_aol_main_low_seen = false; + subaru_aol_acc_fault = false; subaru_aol_gear_counter = 0U; subaru_aol_gear_ts = 0U; subaru_aol_main_ts = 0U; @@ -55,6 +63,7 @@ static void subaru_aol_grant(bool cruise) { if (subaru_aol_enabled && cruise && !subaru_aol_cruise && controls_allowed && subaru_aol_main && subaru_aol_gear_qualified && subaru_aol_drive && (safety_get_ts_elapsed(microsecond_timer_get(), subaru_aol_gear_ts) <= 100000U) && + (!subaru_aol_main_arm || !subaru_aol_acc_fault) && !subaru_aol_permanent && !subaru_aol_temporary && !brake_pressed && !relay_malfunction && !safety_rx_checks_invalid && aol_rx_healthy()) { subaru_aol_token = true; @@ -65,7 +74,35 @@ static void subaru_aol_grant(bool cruise) { subaru_aol_cruise = cruise; } +// MAIN authorizes intent, never output. Joining still needs a new bounded host +// request, heartbeat, healthy physical ACC/steering sources and moving Drive. +static void subaru_aol_observe_main(bool main) { + if (!main) { + subaru_aol_clear(); + subaru_aol_main_low_seen = true; + } else if (subaru_aol_main_arm && subaru_aol_main_low_seen && !subaru_aol_main && + subaru_aol_gear_qualified && subaru_aol_drive && + (safety_get_ts_elapsed(microsecond_timer_get(), subaru_aol_gear_ts) <= 100000U) && + !subaru_aol_permanent && !subaru_aol_temporary && !subaru_aol_acc_fault && + !relay_malfunction && !safety_rx_checks_invalid && aol_rx_healthy()) { + subaru_aol_main_epoch = true; + } + subaru_aol_main = main; + subaru_aol_main_seen = true; + subaru_aol_main_ts = microsecond_timer_get(); +} + static void subaru_aol_host_request(uint8_t mask) { + if (subaru_aol_main_arm && subaru_aol_main_epoch && !subaru_aol_token && (mask != 0U) && + subaru_aol_main && subaru_aol_gear_qualified && subaru_aol_drive && vehicle_moving && + (safety_get_ts_elapsed(microsecond_timer_get(), subaru_aol_gear_ts) <= 100000U) && + !subaru_aol_acc_fault && !subaru_aol_permanent && !subaru_aol_temporary && !brake_pressed && + !relay_malfunction && !safety_rx_checks_invalid && aol_rx_healthy()) { + subaru_aol_token = true; + subaru_aol_pending = !heartbeat_engaged; + subaru_aol_session = heartbeat_engaged; + subaru_aol_grant_ts = microsecond_timer_get(); + } subaru_aol_requested |= mask != 0U; } @@ -106,8 +143,10 @@ static uint8_t subaru_aol_permission(void) { } uint8_t permission = 0U; if (current && subaru_aol_main && subaru_aol_drive && vehicle_moving && + (!subaru_aol_main_arm || !subaru_aol_acc_fault) && !subaru_aol_permanent && !subaru_aol_temporary) { - if ((request != 0U) && controls_allowed && subaru_aol_cruise && subaru_aol_token) { + if ((request != 0U) && subaru_aol_token && + ((controls_allowed && subaru_aol_cruise) || subaru_aol_main_epoch)) { subaru_aol_armed = true; } if (subaru_aol_armed) { diff --git a/opendbc_repo/opendbc/safety/tests/test_subaru_main_aol.py b/opendbc_repo/opendbc/safety/tests/test_subaru_main_aol.py new file mode 100644 index 0000000000..da336b5619 --- /dev/null +++ b/opendbc_repo/opendbc/safety/tests/test_subaru_main_aol.py @@ -0,0 +1,101 @@ +"""Physical MAIN intent for exactly Crosstrek AOL; no ordinary controls grant.""" +import pytest + +from opendbc.safety.tests.libsafety import libsafety_py +from opendbc.safety.tests import test_subaru_stock_aol as stock_module + + +@pytest.fixture +def owner(): + item = stock_module.TestSubaruStockAol(methodName='runTest') + item.setUp() + item.configure(17) + try: + yield item + finally: + item.tearDown() + + +def neutral(item, **kwargs): + for _ in range(12): + assert item.feed(main=False, **kwargs) == 0 + + +def test_physical_main_without_set_grants_only_fresh_lateral(owner): + neutral(owner) + assert owner.feed() == 1 + assert not owner.safety.get_controls_allowed() + owner.safety.aol_set_host_request(3) + assert owner.safety.aol_get_permission_mask() == 1 + assert owner.feed(brake=True) == 1 + assert owner.feed(gear=3) == 0 + assert owner.feed(gear=121) == 1 + assert owner.feed(main=False) == 0 + + +def test_main_held_at_startup_cannot_invent_a_driver_gesture(owner): + for _ in range(20): + assert owner.feed() == 0 + assert not owner.safety.get_controls_allowed() + + +def test_main_can_record_stopped_intent_but_waits_for_moving_request(owner): + neutral(owner, speed=0, host_mask=0) + assert owner.feed(speed=0, host_mask=0) == 0 + for _ in range(30): + assert owner.feed(speed=0, host_mask=0) == 0 + assert owner.feed(speed=54, host_mask=1) == 1 + assert not owner.safety.get_controls_allowed() + + +@pytest.mark.parametrize('fault', ('permanent', 'acc_fault')) +def test_fault_clear_with_main_held_cannot_rearm(owner, fault): + neutral(owner) + assert owner.feed() == 1 + assert owner.feed(**{fault: True}) == 0 + for _ in range(10): + assert owner.feed() == 0 + assert owner.feed(main=False) == 0 + assert owner.feed() == 1 + + +def test_new_physical_acc_source_is_required_before_main_edge(owner): + neutral(owner, omit_distance=True) + assert owner.feed(omit_distance=True) == 0 + for _ in range(20): + assert owner.feed() == 0 + assert owner.feed(main=False) == 0 + assert owner.feed() == 1 + + +def test_acc_wire_checksum_and_counter_cannot_be_faked_by_host(owner): + neutral(owner) + assert owner.feed() == 1 + bad = owner.frame('ES_Distance', {}, 1) + raw = bytearray(bytes(bad.data)) + raw[0] ^= 1 + assert not owner.safety.safety_rx_hook(libsafety_py.make_CANPacket(0x221, 1, bytes(raw[:8]))) + assert owner.safety.aol_get_permission_mask() == 0 + + +def test_heartbeat_and_host_lease_still_withdraw(owner): + neutral(owner) + assert owner.feed() == 1 + owner.safety.set_aol_test_heartbeat(False) + assert owner.safety.aol_get_permission_mask() == 0 + owner.safety.set_aol_test_heartbeat(True) + assert owner.feed() == 0 + assert owner.feed(main=False) == 0 + assert owner.feed() == 1 + owner.safety.set_timer(owner.now + 301_000) + assert owner.safety.aol_get_permission_mask() == 0 + + +def test_acc_fault_cannot_be_bypassed_by_stock_enable_packets(owner): + neutral(owner) + assert owner.feed() == 1 + assert owner.feed(acc_fault=True) == 0 + for enabled in (0, 1): + assert owner.safety.safety_rx_hook(owner.frame('ES_Status', {'Cruise_Activated': enabled}, 1)) + owner.safety.aol_set_host_request(1) + assert owner.safety.aol_get_permission_mask() == 0 diff --git a/opendbc_repo/opendbc/safety/tests/test_subaru_stock_aol.py b/opendbc_repo/opendbc/safety/tests/test_subaru_stock_aol.py index 58b3aa8ed2..9549641ab0 100644 --- a/opendbc_repo/opendbc/safety/tests/test_subaru_stock_aol.py +++ b/opendbc_repo/opendbc/safety/tests/test_subaru_stock_aol.py @@ -30,7 +30,7 @@ class TestSubaruStockAol(unittest.TestCase): return self.packer.make_can_msg_safety(name, bus, values) def feed(self, *, main=True, cruise=False, brake=False, gear=121, temporary=False, permanent=False, - gear_bus=None, omit_gear=False, host_mask=1, omit_main=False): + gear_bus=None, omit_gear=False, host_mask=1, omit_main=False, speed=54, acc_fault=False, omit_distance=False): self.now += 10_000 self.safety.set_timer(self.now) alt = 1 if self.param & 1 else 0 @@ -41,12 +41,14 @@ class TestSubaruStockAol(unittest.TestCase): ('Throttle', {}, main_bus), ('Steering_Torque', {'Steer_Error_1': int(permanent), 'Steer_Warning': int(temporary)}, 0), ('Steering_2', {}, 0), - ('Wheel_Speeds', dict.fromkeys(('FL', 'FR', 'RL', 'RR'), 54), alt), + ('Wheel_Speeds', dict.fromkeys(('FL', 'FR', 'RL', 'RR'), speed), alt), ('Brake_Status', {'Brake': int(brake)}, alt), ('CruiseControl', {'Cruise_On': int(main), 'Cruise_Activated': int(cruise)}, alt), ('ES_Status', {'Cruise_Activated': int(cruise)}, 2 if self.param == 16 else 1), ('ES_DashStatus', {'Cruise_On': int(main)}, 2), ] + if not omit_distance and self.param == 17: + frames.append(('ES_Distance', {'Cruise_Fault': int(acc_fault)}, 1)) if not omit_gear: frames.append(('Transmission', {'Gear': gear}, gear_bus)) for name, fields, bus in frames: @@ -77,7 +79,7 @@ class TestSubaruStockAol(unittest.TestCase): self.assertEqual(self.feed(temporary=True), 0) self.assertEqual(self.feed(), 1) self.assertEqual(self.feed(main=False), 0) - self.assertEqual(self.feed(main=True), 0) + self.assertEqual(self.feed(main=True), int(param == 17)) self.assertEqual(self.feed(cruise=True), 1) def test_permanent_fault_and_rx_fault_require_fresh_ordinary_engagement(self): diff --git a/openpilot/starpilot/car/subaru/aol.py b/openpilot/starpilot/car/subaru/aol.py index 6cfc6b8841..0d2eec0afd 100644 --- a/openpilot/starpilot/car/subaru/aol.py +++ b/openpilot/starpilot/car/subaru/aol.py @@ -1,6 +1,7 @@ -"""Subaru stock ACC retains independent lateral intent after full engagement.""" +"""Subaru physical cruise intent with independent lateral authority.""" from opendbc.car import structs from opendbc.car.subaru.stock_aol import qualified, temporary_restriction +from opendbc.car.subaru.values import CAR from openpilot.starpilot.aol.intent import AolCardIntent from openpilot.starpilot.aol.policy import AolVehiclePolicy @@ -8,12 +9,47 @@ from openpilot.starpilot.aol.policy import AolVehiclePolicy class SubaruStockCardIntent(AolCardIntent): observe_stock_engagement = True - def __init__(self, settings): + def __init__(self, settings, *, main_arm=False): super().__init__(settings, explicit_latch=True, lkas_button_supported=False) + self.main_arm = main_arm + self._previous_main = None + self._main_observed_ns = 0 + self._main_event_ns = 0 + self._pending_main_ns = 0 + self._pending_main_after_ns = 0 + + def observe_startup_readiness(self, sm, CS, *, now_ns, **kwargs): + if self.main_arm: + stamp = int(sm.logMonoTime['onroadEvents']) + self._main_event_ns = stamp if (sm.valid['onroadEvents'] and 0 < stamp <= now_ns and + now_ns - stamp <= 200_000_000) else 0 def update(self, CS, **kwargs): super().update(CS, **kwargs) - if not CS.cruiseState.available: + now_ns = kwargs.get('now_ns', 0) + main = bool(CS.cruiseState.available) + fresh_main = self._previous_main is False and main + physical_safe = (self.settings.enabled and CS.canValid and not CS.canTimeout and + not self._calibration_inhibit and not self._calibration_rearm_required and + not CS.steerFaultPermanent and not CS.steerFaultTemporary and not CS.accFaulted and + CS.gearShifter == structs.CarState.GearShifter.drive) + if self.main_arm and fresh_main and physical_safe: + self._pending_main_ns = now_ns + self._pending_main_after_ns = self._main_observed_ns + self._previous_main = main if CS.canValid and not CS.canTimeout else None + self._main_observed_ns = now_ns + if self._pending_main_ns: + if not main or not physical_safe or not 0 <= now_ns - self._pending_main_ns <= 200_000_000: + self._pending_main_ns = 0 + elif self._main_event_ns > self._pending_main_after_ns and kwargs.get('fault_active') is not None: + # Wait for the event observation after MAIN; an earlier MAIN-off fault + # cannot decide this edge. A new fault still consumes it without arming. + if not kwargs['fault_active'] and not self._fault_inhibit: + self.allowed_latch = True + self.pause_lateral = False + self._last_latch_edge_ns = self._pending_main_ns + self._pending_main_ns = 0 + if not main: self._record_latch_clear('subaru_main_off') self.allowed_latch = False self._withdraw_full_engagement() @@ -50,7 +86,7 @@ def ordinary_axis_request_allowed(cp, state): def create_intent(cp, settings): - return SubaruStockCardIntent(settings) + return SubaruStockCardIntent(settings, main_arm=cp.carFingerprint == CAR.SUBARU_CROSSTREK_2025) def sng_standstill_lateral_inhibit(cp, state): diff --git a/openpilot/starpilot/car/subaru/tests/test_main_aol.py b/openpilot/starpilot/car/subaru/tests/test_main_aol.py new file mode 100644 index 0000000000..ae0af2966b --- /dev/null +++ b/openpilot/starpilot/car/subaru/tests/test_main_aol.py @@ -0,0 +1,70 @@ +"""A fresh physical MAIN waits for its own current fault observation.""" +from types import SimpleNamespace + +from opendbc.car import structs +from openpilot.starpilot.aol.intent import AolSettings +from openpilot.starpilot.car.subaru.aol import SubaruStockCardIntent + + +def state(): + cs = structs.CarState(canValid=True, gearShifter=structs.CarState.GearShifter.drive) + cs.cruiseState.available = False + return cs + + +def observe(owner, cs, now, event, fault): + sm = SimpleNamespace(logMonoTime={'onroadEvents': event}, valid={'onroadEvents': True}) + owner.observe_startup_readiness(sm, cs, now_ns=now) + owner.update(cs, now_ns=now, fault_active=fault) + + +def owner(): + return SubaruStockCardIntent(AolSettings(True, 0., 0, 0, (0, 0, 0), (0, 0, 0)), main_arm=True) + + +def test_main_waits_for_post_edge_observation_and_allows_stopped_intent(): + intent, cs = owner(), state() + cs.standstill = cs.brakePressed = True + observe(intent, cs, 1_000_000, 1_000_000, True) + cs.cruiseState.available = True + observe(intent, cs, 10_000_000, 1_000_000, True) + assert not intent.allowed_latch + observe(intent, cs, 20_000_000, 10_000_000, False) + assert intent.allowed_latch + assert not cs.cruiseState.enabled + + +def test_new_post_main_fault_consumes_gesture_without_rearming_when_it_clears(): + intent, cs = owner(), state() + observe(intent, cs, 1_000_000, 1_000_000, False) + cs.cruiseState.available = True + observe(intent, cs, 10_000_000, 1_000_000, False) + observe(intent, cs, 20_000_000, 10_000_000, True) + observe(intent, cs, 30_000_000, 20_000_000, False) + assert not intent.allowed_latch + + +def test_expired_main_edge_and_startup_held_main_cannot_arm(): + intent, cs = owner(), state() + cs.cruiseState.available = True + observe(intent, cs, 1_000_000, 1_000_000, False) + assert not intent.allowed_latch + cs.cruiseState.available = False + observe(intent, cs, 2_000_000, 2_000_000, False) + cs.cruiseState.available = True + observe(intent, cs, 10_000_000, 2_000_000, False) + observe(intent, cs, 211_000_000, 2_000_000, False) + observe(intent, cs, 220_000_000, 220_000_000, False) + assert not intent.allowed_latch + + +def test_physical_acc_fault_cancels_pending_main_edge(): + intent, cs = owner(), state() + observe(intent, cs, 1_000_000, 1_000_000, False) + cs.cruiseState.available = True + observe(intent, cs, 10_000_000, 1_000_000, False) + cs.accFaulted = True + observe(intent, cs, 20_000_000, 10_000_000, False) + cs.accFaulted = False + observe(intent, cs, 30_000_000, 20_000_000, False) + assert not intent.allowed_latch 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 0c1c7f170d..548d8bb739 100644 --- a/openpilot/starpilot/car/subaru/tests/test_stock_aol_callers.py +++ b/openpilot/starpilot/car/subaru/tests/test_stock_aol_callers.py @@ -23,7 +23,7 @@ from openpilot.starpilot.car.subaru.tests.native_fixture import subaru_native_pa @pytest.mark.parametrize('candidate', [CAR.SUBARU_ASCENT_2023, CAR.SUBARU_LEGACY_2025, CAR.SUBARU_CROSSTREK_2025, CAR.SUBARU_FORESTER, CAR.SUBARU_OUTBACK]) 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): + hard_stop_native_first=False, main_only=False): fw = firmware_records(candidate, FW_VERSIONS, 'subaru') rows = [] with actual_factory(candidate, gen_empty_fingerprint(), fw, '0' * 17, manual=manual, aol=aol) as (card, params, matches): @@ -59,11 +59,11 @@ def test_actual_full_engage_brake_reverse_temporary_recovery_mainoff(candidate, if hard_stop_native_first else range(460)) for elapsed, tick in enumerate(ticks): now = base + elapsed * 10_000_000 - cruise = 30 <= tick < 110 or tick >= 410 + cruise = not main_only and (30 <= tick < 110 or tick >= 410) brake = 90 <= tick < 110 reverse = 150 <= tick < 185 temporary = 225 <= tick < 260 - main = not 310 <= tick < 350 + main = not 310 <= tick < 350 and (tick >= 30 or not main_only) stopped = stop_resume and (125 <= tick < 135 if hard_stop_native_first else 185 <= tick < 195) first_resume = 145 <= tick < 150 if hard_stop_native_first else 125 <= tick < 145 values = { @@ -227,8 +227,11 @@ def test_actual_full_engage_brake_reverse_temporary_recovery_mainoff(candidate, bool(intent and intent.lateralArmed) == aol and row['cardLatch'] == aol and row['permission'] == 0), row if 240 <= tick < 255: assert not command.latActive and bool(intent and intent.lateralArmed) == aol and row['cardLatch'] == aol and row['permission'] == 0, row - if 330 <= tick < 400: + main_rearm = aol and candidate == CAR.SUBARU_CROSSTREK_2025 + if 330 <= tick < (350 if main_rearm else 400): assert not command.latActive and not bool(intent and intent.allowedLatch), row + if main_rearm and 375 <= tick < 400: + assert command.latActive and row['cardLatch'] and row['permission'] == 1 and not row['controls'], row if tick >= 440: assert command.latActive and bool(intent and intent.allowedLatch) == aol and row['permission'] == int(aol), row assert len(active_emissions) >= 4, rows[65:90] @@ -313,3 +316,13 @@ def test_actual_native_first_hard_stop_retains_intent_and_resumes(candidate, sub assert all(not command['request'] or not command['nativeAccepted'] for command in stopped[0]['steering']) settled = [command for row in stopped if row['elapsed'] >= 127 for command in row['steering']] assert settled and all(not command['request'] and command['nativeAccepted'] for command in settled) + + +def test_actual_main_without_set_retains_native_first_stop_intent(subaru_native_pair, tmp_path): + test_actual_full_engage_brake_reverse_temporary_recovery_mainoff( + CAR.SUBARU_CROSSTREK_2025, subaru_native_pair, tmp_path, + stop_resume=True, hard_stop_native_first=True, main_only=True) + rows = json.loads((tmp_path / (str(CAR.SUBARU_CROSSTREK_2025) + '-True-actual.json')).read_text()) + assert all(not row['controls'] and not row['enabled'] and not row['long'] for row in rows) + assert all(row['lat'] and row['permission'] == 1 for row in rows if 65 <= row['tick'] < 90) + assert all(not row['lat'] and row['cardLatch'] and row['permission'] == 0 for row in rows if 125 <= row['tick'] < 135) diff --git a/openpilot/starpilot/ui/onroad.py b/openpilot/starpilot/ui/onroad.py index 9e1d88f94a..9cad007319 100644 --- a/openpilot/starpilot/ui/onroad.py +++ b/openpilot/starpilot/ui/onroad.py @@ -102,7 +102,7 @@ def axis_status_color(state: OnroadState) -> rl.Color: return rl.Color(218, 111, 37, 255) return rl.Color(22, 127, 64, 255) if state.lateral_active: - return rl.Color(10, 186, 181, 255) + return rl.Color(22, 127, 64, 255) if state.stock_cruise_active else rl.Color(10, 186, 181, 255) if state.longitudinal_active: return rl.Color(255, 105, 180, 255) return rl.Color(18, 40, 57, 255) diff --git a/openpilot/starpilot/ui/runtime_snapshot.py b/openpilot/starpilot/ui/runtime_snapshot.py index 126136536c..c6c2e25941 100644 --- a/openpilot/starpilot/ui/runtime_snapshot.py +++ b/openpilot/starpilot/ui/runtime_snapshot.py @@ -684,7 +684,7 @@ class RuntimeSnapshotAdapter: longitudinal_overridden=longitudinal_overridden, driver_override_active=bool(display_car_valid and ( display_lat_active and getattr(display_car, 'steeringPressed', False) or - (display_long_active or longitudinal_overridden) and getattr(display_car, 'gasPressed', False))), + (display_long_active or longitudinal_overridden or stock_cruise) and getattr(display_car, 'gasPressed', False))), stock_cruise_active=stock_cruise, slc_system_long_available=system_long, slc_wheel_confirmation_available=wheel_confirmation, diff --git a/openpilot/starpilot/ui/tests/test_onroad_axes.py b/openpilot/starpilot/ui/tests/test_onroad_axes.py index 619a71912e..25d62901be 100644 --- a/openpilot/starpilot/ui/tests/test_onroad_axes.py +++ b/openpilot/starpilot/ui/tests/test_onroad_axes.py @@ -393,6 +393,27 @@ class TestOnroadAxes(unittest.TestCase): self.assertEqual(clock_text(midnight, use_24_hour=True), "00:00") + + def test_stock_acc_green_keeps_system_long_inactive_and_aol_cyan(self): + from opendbc.car.subaru.interface import CarInterface + from opendbc.car.subaru.values import CAR + + self.ui.CP = CarInterface.get_non_essential_params(CAR.SUBARU_CROSSTREK_2025) + self.ui.sm['carState'].cruiseState.enabled = True + state = self.state(True, False) + self.assertTrue(state.stock_cruise_active) + self.assertFalse(state.longitudinal_active) + self.assertEqual(rgba(axis_status_color(state)), (22, 127, 64, 255)) + self.assertEqual(rgba(axis_status_color(replace(state, experimental_enabled=True))), (22, 127, 64, 255)) + self.ui.sm['carState'].gasPressed = True + self.assertEqual(rgba(axis_status_color(self.state(True, False))), (145, 155, 149, 255)) + self.ui.sm['carState'].gasPressed = False + self.ui.sm['carState'].cruiseState.enabled = False + self.assertEqual(rgba(axis_status_color(self.state(True, False))), (10, 186, 181, 255)) + self.ui.sm['carState'].cruiseState.enabled = True + self.ui.sm.logMonoTime['carState'] = NOW - 300_000_000 + self.assertFalse(self.state(True, False).stock_cruise_active) + + if __name__ == "__main__": unittest.main() - diff --git a/prebuilt.json b/prebuilt.json index 21bdf68a78..b4953f09c4 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "f5d223b542ea53e06d33ae95394033d6a648ac123ae3aca3c7c84bc46b171439", "schema_version": 1} +{"protected_inventory_sha256": "2784b72fe7fe41a0091479265ac775a78feb7b2458ada3cef55a9a9a2796b123", "schema_version": 1}