From 66f0ec13716f64dfbb418edbe2bc02c7ac345c74 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Thu, 22 Oct 2026 15:28:09 -0500 Subject: [PATCH] Restore Mazda MAIN-on lateral startup --- .../tests/test_native_engagement_callers.py | 151 ++++++++++++++---- .../opendbc/safety/modes/mazda_stock_aol.h | 18 ++- .../safety/tests/test_mazda_engagement_aol.py | 2 + .../safety/tests/test_mazda_stock_aol.py | 73 +++++++++ openpilot/selfdrive/car/card.py | 4 + openpilot/starpilot/car/mazda/stock_intent.py | 73 +++++++++ .../car/mazda/tests/test_main_startup.py | 83 ++++++++++ prebuilt.json | 2 +- tools/ci/run_vehicle_tests.py | 1 + 9 files changed, 374 insertions(+), 33 deletions(-) create mode 100644 openpilot/starpilot/car/mazda/tests/test_main_startup.py diff --git a/opendbc_repo/opendbc/car/mazda/tests/test_native_engagement_callers.py b/opendbc_repo/opendbc/car/mazda/tests/test_native_engagement_callers.py index 0e6128ee11..19c854c5bc 100644 --- a/opendbc_repo/opendbc/car/mazda/tests/test_native_engagement_callers.py +++ b/opendbc_repo/opendbc/car/mazda/tests/test_native_engagement_callers.py @@ -90,7 +90,7 @@ def test_eps_recovery_actual_callers(identity, lateral_only, monkeypatch, armed_ def _exercise_actual_family_callers(identity, monkeypatch, *, scenario='brake', cancel_action=3, armed_intent=None, - lateral_only=False, unbelted=False): + lateral_only=False): from openpilot.cereal import messaging from openpilot.selfdrive.car import card as card_module from openpilot.starpilot.aol.wire import PandaSlot, SafetyState, encode_safety @@ -107,17 +107,16 @@ def _exercise_actual_family_callers(identity, monkeypatch, *, scenario='brake', now, tick, queued = [10_000_000_000], 0, [] received, previous_sd = {}, {} first_engagement_ns = None - belt_off = [False] try: sd, controls, drive = case._authority_callers(False, cp=card.CP.as_reader().as_builder(), genuine_events=True) - if scenario in ('cancel', 'long_cancel', 'mapped_cancel', 'temporary'): + if scenario in ('cancel', 'long_cancel', 'mapped_cancel', 'temporary', 'startup', 'startup_stopped'): from openpilot.starpilot.aol.intent import read_settings sd.aol_settings = read_settings(saved) assert sd.aol_settings.cancel_actions == (cancel_action, 0, 0) assert card.aol_card_intent.settings == sd.aol_settings assert sd.CP.to_dict() == card.CP.to_dict() == ci.CP.to_dict() == controls.CP.to_dict() safety.init_tests() - safety.set_alternative_experience(32) + safety.set_alternative_experience(int(card.CP.alternativeExperience)) safety.set_safety_hooks(structs.CarParams.SafetyModel.mazda, 0) safety.set_aol_test_heartbeat(False) local_patch.setattr(card_module.time, 'monotonic_ns', lambda: now[0]) @@ -147,9 +146,6 @@ def _exercise_actual_family_callers(identity, monkeypatch, *, scenario='brake', frames = complete_frames(packer, card.CP, tick, main=main, engaged=engaged, brake=brake, cancel=cancel, reverse=reverse, speed_kph=speed_kph, permanent=permanent, temporary=temporary) frames.append(packer.make_can_msg('STEER', 0, {'STEER_ANGLE': 1.0})) - if belt_off[0]: - belt = packer.make_can_msg('SEATBELT', 0, {'DRIVER_SEATBELT': 0}) - frames = [row for row in frames if not (row[0] == belt[0] and row[2] == 0)] + [belt] safety.set_timer(now[0] // 1000) prior_state = previous_sd.get('selfdriveState') prior_axis = previous_sd.get('aolAxisState') @@ -186,14 +182,15 @@ def _exercise_actual_family_callers(identity, monkeypatch, *, scenario='brake', mask = safety.aol_get_permission_mask() reported_request_lat = bool(reported_request_mask & 1) reported_request_long = bool(reported_request_mask & 2) - if first_engagement_ns is None: + if first_engagement_ns is None and not scenario.startswith('startup'): assert request_mask == reported_request_mask == mask == 0, 'No axis request before actual SD desire' if engaged: assert not heartbeat and safety.get_controls_allowed(), 'Cold physical engagement must precede host acknowledgment' first_engagement_ns = now[0] if mask & 1: assert heartbeat and request_lat and reported_request_lat, 'Native lateral permission needs actual request and heartbeat' - assert first_engagement_ns is not None and now[0] > first_engagement_ns, 'Native acknowledgment must follow physical grant' + assert (scenario.startswith('startup') or first_engagement_ns is not None and now[0] > first_engagement_ns), ( + 'Native acknowledgment must follow physical grant') source = messaging.log_from_bytes(card_module.can_list_to_can_capnp(frames)).as_builder() source.logMonoTime = now[0] queued[:] = [source.to_bytes()] @@ -205,7 +202,7 @@ def _exercise_actual_family_callers(identity, monkeypatch, *, scenario='brake', panda = events[-1].pandaStates[0] panda.safetyModel = card.CP.safetyConfigs[0].safetyModel panda.safetyParam = 0 - panda.alternativeExperience = 32 + panda.alternativeExperience = int(card.CP.alternativeExperience) panda.controlsAllowed = safety.get_controls_allowed() if 'onroadEvents' not in previous_sd: events.append(messaging.new_message('onroadEvents', 0, valid=True, logMonoTime=now[0])) @@ -216,7 +213,7 @@ def _exercise_actual_family_callers(identity, monkeypatch, *, scenario='brake', wire.aolSafetyWire = encode_safety(SafetyState(1, True, now[0], now[0] + 30_000_000, int(card.CP.safetyConfigs[0].safetyModel.raw), 0, bool(mask & 1), bool(mask & 2), reported_request_lat, reported_request_long, 'native-mazda-test', card.aol_axis_session(), now[0], - (PandaSlot(0, 'native-mazda-test', int(card.CP.safetyConfigs[0].safetyModel.raw), 0, 32, + (PandaSlot(0, 'native-mazda-test', int(card.CP.safetyConfigs[0].safetyModel.raw), 0, int(card.CP.alternativeExperience), safety.get_controls_allowed(), bool(not safety.safety_config_valid()), False, int(bool(safety.get_relay_malfunction()))),))) events.append(wire) @@ -245,29 +242,49 @@ def _exercise_actual_family_callers(identity, monkeypatch, *, scenario='brake', assert any(frame.address == 0x243 for frame in outgoing.sendcan) for frame in outgoing.sendcan: assert safety.safety_tx_hook(make_msg(frame.src, frame.address, dat=frame.dat)) - if unbelted and belt_off[0]: - assert state.seatbeltUnlatched - assert not axis.desiredLongitudinal and not axis.longitudinalActive and not command.longActive - assert not safety.aol_get_permission_mask() & 2 return state, axis, host_state, command for _ in range(120): - step() + state, axis, host_state, command = step(speed_kph=0. if scenario == 'startup_stopped' else 80.) + if scenario == 'startup_stopped': + assert not command.latActive and not command.longActive and safety.aol_get_permission_mask() == 0 + if scenario == 'startup_stopped': + for _ in range(30): + state, axis, host_state, command = step() + if scenario == 'startup_managed': + state, axis, host_state, command = step() + assert state.canValid and state.cruiseState.available and not state.cruiseState.enabled + assert not card.aol_card_intent.allowed_latch and not command.latActive and not command.longActive + assert safety.aol_get_permission_mask() == 0 and not safety.get_controls_allowed() + return + if scenario in ('startup', 'startup_stopped'): + state, axis, host_state, command = step() + assert state.canValid and state.cruiseState.available and not state.cruiseState.enabled + assert card.aol_card_intent.allowed_latch, 'MAIN already ON at boot never armed Mazda lateral intent' + assert axis.lateralActive and command.latActive and safety.aol_get_permission_mask() == 1 + assert not host_state.enabled and not command.longActive and not safety.get_controls_allowed() + for _ in range(5): + step(cancel=True) + for off_tick in range(120): + state, axis, host_state, command = step() + assert not card.aol_card_intent.allowed_latch and not command.latActive + for frame in received['sendcan'].sendcan: + if frame.address == 0x243 and frame.src == 0: + assert (((frame.dat[0] & 15) << 8) | frame.dat[1]) - 2048 == 0 + # The first native poll precedes this cycle's published OFF decision. + # Its next real request must withdraw the previous grant completely. + if off_tick: + assert safety.aol_get_permission_mask() == 0 + assert not safety.get_controls_allowed() + for _ in range(10): + state, axis, host_state, command = step(main=False) + assert not card.aol_card_intent.allowed_latch and not command.latActive + return assert not card.aol_card_intent.allowed_latch for _ in range(30): state, axis, host_state, command = step(engaged=True) assert state.canValid and host_state.enabled and command.latActive assert card.aol_card_intent.allowed_latch - if unbelted: - for _ in range(30): - state, axis, host_state, command = step(brake=True) - assert card.aol_card_intent.allowed_latch and axis.lateralActive and not axis.longitudinalActive - belt_off[0] = True - for _ in range(5): - state, axis, host_state, command = step() - assert state.seatbeltUnlatched and card.aol_card_intent.allowed_latch - assert axis.lateralActive and command.latActive and not command.longActive - assert safety.aol_get_permission_mask() == 1 if lateral_only: for _ in range(30): state, axis, host_state, command = step(brake=True) @@ -387,10 +404,82 @@ def _exercise_actual_family_callers(identity, monkeypatch, *, scenario='brake', patch_scope.__exit__(None, None, None) case.doCleanups() - exercise_startup(identity, False, False, True, monkeypatch, probe=probe, cancel_action=cancel_action) + exercise_startup(identity, False, False, True, monkeypatch, probe=probe, cancel_action=cancel_action, + mapping=(0, 0) if scenario in ('startup', 'startup_stopped') else (9, 9)) + @pytest.mark.parametrize('identity', (CAR.MAZDA_CX5_2022, CAR.MAZDA_CX9_2021)) -@pytest.mark.parametrize('scenario', ('mapped_cancel', 'temporary')) -def test_unbelted_lateral_pause_recovery_actual_callers(identity, scenario, monkeypatch, armed_intent_decoder): - _exercise_actual_family_callers(identity, monkeypatch, scenario=scenario, cancel_action=3, - armed_intent=armed_intent_decoder, unbelted=True) +def test_main_already_on_startup_actual_callers(identity, monkeypatch, armed_intent_decoder): + _exercise_actual_family_callers(identity, monkeypatch, scenario='startup', cancel_action=9, armed_intent=armed_intent_decoder) + + +@pytest.mark.parametrize('identity', (CAR.MAZDA_CX5_2022, CAR.MAZDA_CX9_2021)) +def test_main_on_startup_respects_button_managed_aol(identity, monkeypatch, armed_intent_decoder): + _exercise_actual_family_callers(identity, monkeypatch, scenario='startup_managed', armed_intent=armed_intent_decoder) + + +@pytest.mark.parametrize('identity', (CAR.MAZDA_CX5_2022, CAR.MAZDA_CX9_2021)) +def test_main_on_startup_stopped_then_drive_actual_callers(identity, monkeypatch, armed_intent_decoder): + _exercise_actual_family_callers(identity, monkeypatch, scenario='startup_stopped', cancel_action=9, + armed_intent=armed_intent_decoder) + + +@pytest.mark.parametrize('identity', (CAR.MAZDA_CX5_2022, CAR.MAZDA_CX9_2021)) +def test_main_on_startup_disabled_aol_actual_card_controls_native(identity, monkeypatch): + from openpilot.cereal import messaging + from openpilot.selfdrive.car import card as card_module + from openpilot.starpilot.lateral.tests.test_lane_runtime import feed + from opendbc.safety.tests.common import make_msg + from opendbc.safety.tests.libsafety import libsafety_py + + def probe(card, ci, controls, saved): + now, queued = [10_000_000_000], [] + monkeypatch.setattr(card_module.time, 'monotonic_ns', lambda: now[0]) + monkeypatch.setattr(card_module.time, 'clock_gettime_ns', lambda clock: now[0]) + monkeypatch.setattr(card_module.messaging, 'drain_sock_raw', lambda *args, **kwargs: list(queued)) + monkeypatch.setattr(card.sm, 'update', lambda *args: None) + packer = CANPacker(DBC[identity][Bus.pt]) + safety = libsafety_py.libsafety + safety.init_tests() + safety.set_alternative_experience(int(card.CP.alternativeExperience)) + safety.set_safety_hooks(structs.CarParams.SafetyModel.mazda, 0) + try: + assert card.CP.alternativeExperience == 0 + for tick in range(1, 131): + now[0] += 10_000_000 + safety.set_timer(now[0] // 1000) + frames = complete_frames(packer, card.CP, tick, main=True) + for address, data, bus in frames: + safety.safety_rx_hook(make_msg(bus, address, dat=data)) + safety.safety_tick() + safety.aol_set_host_request(0) + source = messaging.log_from_bytes(card_module.can_list_to_can_capnp(frames)).as_builder() + source.logMonoTime = now[0] + queued[:] = [source.to_bytes()] + events = [messaging.new_message(name, size, valid=True, logMonoTime=now[0]) + for name, size in (('deviceState', None), ('onroadEvents', 0), ('pandaStates', 1))] + events[0].deviceState.started = True + events[0].deviceState.startedMonoTime = 9_000_000_000 + panda = events[-1].pandaStates[0] + panda.safetyModel = card.CP.safetyConfigs[0].safetyModel + panda.safetyParam = 0 + panda.alternativeExperience = 0 + card.sm.update_msgs(now[0] / 1e9, [e.as_reader() for e in events]) + state, _ = card.state_update() + card.state_publish(state, None) + feed(controls, now[0], tick, active=False, enabled=False, + can_valid=state.canValid, can_timeout=state.canTimeout) + observed = messaging.new_message('carState', valid=bool(state.canValid), logMonoTime=now[0]) + observed.carState = state + controls.sm.update_msgs(now[0] / 1e9, [observed.as_reader()]) + command, _ = controls.state_control() + assert card.aol_card_intent is None and not command.latActive and not command.longActive + assert safety.aol_get_permission_mask() == 0 and not safety.get_controls_allowed() + assert state.canValid and state.cruiseState.available and not state.cruiseState.enabled + assert card.CP.to_dict() == controls.CP.to_dict() + finally: + safety.init_tests() + safety.set_alternative_experience(0) + safety.set_safety_hooks(structs.CarParams.SafetyModel.noOutput, 0) + + exercise_startup(identity, False, False, False, monkeypatch, probe=probe) diff --git a/opendbc_repo/opendbc/safety/modes/mazda_stock_aol.h b/opendbc_repo/opendbc/safety/modes/mazda_stock_aol.h index 1339452e2c..f3339eaad3 100644 --- a/opendbc_repo/opendbc/safety/modes/mazda_stock_aol.h +++ b/opendbc_repo/opendbc/safety/modes/mazda_stock_aol.h @@ -1,6 +1,7 @@ #pragma once static bool mazda_aol_enabled = false; +static bool mazda_aol_startup_pending = true; static bool mazda_aol_token = false; static bool mazda_aol_neutral = false; static bool mazda_aol_buttons_neutral = false; @@ -31,6 +32,7 @@ static void mazda_aol_clear(void) { static void mazda_aol_reset(void) { mazda_aol_clear(); mazda_aol_enabled = false; + mazda_aol_startup_pending = true; mazda_aol_requested = false; mazda_aol_seen = 0U; mazda_aol_sources = 0U; @@ -58,6 +60,7 @@ static void mazda_aol_request(uint8_t mask) { // A physical grant may arrive before pandad's engaged heartbeat. It remains // output-ineligible until a current host request from after this grant joins it. static void mazda_aol_grant(void) { + mazda_aol_startup_pending = false; mazda_aol_token = true; mazda_aol_pending = !heartbeat_engaged; mazda_aol_session = heartbeat_engaged; @@ -74,7 +77,7 @@ static uint8_t mazda_aol_request_mask(void) { if (relay_malfunction || expired || pending_expired || safety_rx_checks_invalid || (mazda_aol_session && !heartbeat_engaged) || !aol_rx_healthy() || !mazda_aol_current()) { mazda_aol_clear(); - } else if (heartbeat_engaged && aol_rx_healthy() && mazda_aol_current()) { + } else if (!mazda_aol_startup_pending && heartbeat_engaged && aol_rx_healthy() && mazda_aol_current()) { // Equal microsecond stamps cannot prove request-after-grant ordering. // Wait for a strictly later request; the original grant deadline stays fixed. const bool pending_ack = !mazda_aol_pending || ((aol_host_axis_mask != 0U) && @@ -120,6 +123,7 @@ static void mazda_aol_optional_rx(const CANPacket_t *msg) { mazda_aol_sources = (mazda_aol_sources & 3U) | (GET_BIT(msg, 16U) ? 4U : 0U); if ((mazda_aol_sources & 4U) != 0U) { mazda_aol_clear(); + mazda_aol_startup_pending = false; } } else { } @@ -136,7 +140,19 @@ static void mazda_aol_rx(const CANPacket_t *msg) { mazda_aol_clear(); } else if (!acc_main_on) { mazda_aol_clear(); + mazda_aol_startup_pending = false; mazda_aol_neutral = true; + } else if (mazda_aol_startup_pending && GET_BIT(msg, 3U)) { + mazda_aol_startup_pending = false; + } else if (mazda_aol_startup_pending && vehicle_moving && (mazda_aol_sources == 1U) && !brake_pressed && !gas_pressed && + ((aol_host_axis_mask & 1U) != 0U) && + (safety_get_ts_elapsed(microsecond_timer_get(), aol_host_request_ts) <= AOL_HOST_REQUEST_TIMEOUT_US)) { + // Dom follows fresh MAIN at boot. Wait through physical restrictions, + // then require a strictly later host ACK within the original deadline. + mazda_aol_startup_pending = false; + mazda_aol_grant(); + mazda_aol_pending = true; + mazda_aol_session = false; } else if (mazda_aol_neutral) { mazda_aol_grant(); mazda_aol_neutral = false; diff --git a/opendbc_repo/opendbc/safety/tests/test_mazda_engagement_aol.py b/opendbc_repo/opendbc/safety/tests/test_mazda_engagement_aol.py index 78076c6953..ac5b1d5441 100644 --- a/opendbc_repo/opendbc/safety/tests/test_mazda_engagement_aol.py +++ b/opendbc_repo/opendbc/safety/tests/test_mazda_engagement_aol.py @@ -30,6 +30,8 @@ class TestMazdaEngagementAol(unittest.TestCase): self.safety.aol_set_host_request(request) def warm(self, **kwargs): + # These tests isolate ordinary cruise engagement; no bootstrap LAT desire. + kwargs.setdefault('request', 0) for _ in range(20): self.frame(**kwargs) diff --git a/opendbc_repo/opendbc/safety/tests/test_mazda_stock_aol.py b/opendbc_repo/opendbc/safety/tests/test_mazda_stock_aol.py index 43239cb285..f049169c3c 100644 --- a/opendbc_repo/opendbc/safety/tests/test_mazda_stock_aol.py +++ b/opendbc_repo/opendbc/safety/tests/test_mazda_stock_aol.py @@ -175,6 +175,8 @@ class TestMazdaStockAol(unittest.TestCase): def test_cancel_requires_neutral_and_current_host_ack(self): self.configure() + self.arm() + self.feed(buttons=3, permanent=True) # Consume startup eligibility; isolate fresh Cancel rearming. self.safety.set_aol_test_heartbeat(False) for _ in range(12): self.feed(buttons=3) @@ -205,3 +207,74 @@ class TestMazdaStockAol(unittest.TestCase): self.feed(buttons=0, main=False) self.feed(buttons=3, main=False) self.assertEqual(self.safety.aol_get_permission_mask(), 0) + + def test_boot_main_on_requires_strictly_later_ack_and_never_rearms_held_main(self): + self.configure() + self.safety.set_aol_test_heartbeat(False) + for _ in range(12): + self.feed() + self.assertEqual(self.safety.aol_get_permission_mask(), 0) + self.safety.set_aol_test_heartbeat(True) + self.safety.aol_set_host_request(1) + self.assertEqual(self.safety.aol_get_permission_mask(), 0) + self.time += 1 + self.safety.set_timer(self.time) + self.safety.aol_set_host_request(1) + self.assertEqual(self.safety.aol_get_permission_mask(), 1) + self.assertFalse(self.safety.get_controls_allowed()) + self.assertTrue(self.safety.safety_tx_hook(self.packet('CAM_LKAS', {'LKAS_REQUEST': 10}))) + self.assertEqual(self.safety.safety_fwd_hook(2, 0x243), -1) + self.safety.aol_set_host_request(3) + self.assertEqual(self.safety.aol_get_permission_mask(), 1) + self.safety.set_aol_test_heartbeat(False) + self.assertEqual(self.safety.aol_get_permission_mask(), 0) + self.safety.set_aol_test_heartbeat(True) + for _ in range(12): + self.feed() + self.assertEqual(self.safety.aol_get_permission_mask(), 0) + + def test_boot_permanent_camera_fault_consumes_main_on_eligibility(self): + self.configure() + self.feed(permanent=True) + for _ in range(12): + self.feed() + self.time += 1 + self.safety.set_timer(self.time) + self.safety.aol_set_host_request(1) + self.assertEqual(self.safety.aol_get_permission_mask(), 0) + + def test_boot_main_on_pending_ack_keeps_original_deadline(self): + self.configure() + self.safety.set_aol_test_heartbeat(False) + for _ in range(12): + self.feed() + for _ in range(31): + self.time += 10000 + self.safety.set_timer(self.time) + self.feed() # Fresh CAN and requests cannot renew the startup grant. + self.assertEqual(self.safety.aol_get_permission_mask(), 0) + self.safety.set_aol_test_heartbeat(True) + self.time += 1 + self.safety.set_timer(self.time) + self.feed() + self.assertEqual(self.safety.aol_get_permission_mask(), 0) + + def test_boot_main_waits_for_fresh_host_readiness_without_starting_grant_deadline(self): + self.configure() + self.safety.set_aol_test_heartbeat(False) + self.safety.aol_set_host_request(0) + for _ in range(150): + self.time += 10000 + self.safety.set_timer(self.time) + self.feed() + self.safety.aol_set_host_request(0) + self.assertEqual(self.safety.aol_get_permission_mask(), 0) + self.safety.aol_set_host_request(1) + self.feed() + self.assertEqual(self.safety.aol_get_permission_mask(), 0) + self.safety.set_aol_test_heartbeat(True) + self.time += 1 + self.safety.set_timer(self.time) + self.safety.aol_set_host_request(1) + self.assertEqual(self.safety.aol_get_permission_mask(), 1) + self.assertFalse(self.safety.get_controls_allowed()) diff --git a/openpilot/selfdrive/car/card.py b/openpilot/selfdrive/car/card.py index 357ddc28a5..da684e22de 100644 --- a/openpilot/selfdrive/car/card.py +++ b/openpilot/selfdrive/car/card.py @@ -674,6 +674,10 @@ class Car: observe_physical = getattr(self.aol_card_intent, 'observe_physical_samples', None) if observe_physical is not None: observe_physical(self.CI.CS, now_ns=now_ns) + startup_readiness = getattr(self.aol_card_intent, 'observe_startup_readiness', None) + if startup_readiness is not None: + startup_readiness(self.sm, CS, native=native, panda_ready=angle_panda_ready, + now_ns=now_ns, producer=self.slc_producer_session) self.aol_card_intent.update(CS, fault_active=fault_active, now_ns=now_ns, native_rejection_ns=rejection_ns, standard_enabled=(host_enabled if getattr(self.aol_card_intent, 'observe_stock_engagement', False) else host_control_enabled)) diff --git a/openpilot/starpilot/car/mazda/stock_intent.py b/openpilot/starpilot/car/mazda/stock_intent.py index 1a08fdef35..b4ea9dd4b4 100644 --- a/openpilot/starpilot/car/mazda/stock_intent.py +++ b/openpilot/starpilot/car/mazda/stock_intent.py @@ -1,4 +1,5 @@ from opendbc.car import structs +from opendbc.car.mazda.stock_aol import qualified from openpilot.selfdrive.car.cruise import CRUISE_LONG_PRESS from openpilot.starpilot.aol.intent import AolCardIntent, AOL_TOGGLE, ButtonType @@ -8,13 +9,64 @@ class MazdaStockCardIntent(AolCardIntent): def __init__(self, cp, settings): super().__init__(settings, explicit_latch=True) + self.cp = cp self.main_seen = False + self.startup_main_pending = True + self.startup_main_source_ns = 0 + self.startup_native_ready = False + self.startup_binding = None self.main_available = False self.main_neutral = False self.main_pulse = False self.cancel_held = False self.cancel_ticks = 0 + def observe_startup_readiness(self, sm, CS, *, native, panda_ready, now_ns, producer): + from openpilot.starpilot.aol.intent import disarming_fault + from openpilot.starpilot.aol.runtime import AXIS_MAX_AGE_NS + from openpilot.starpilot.aol.transport_pause import _healthy_transport_native, availability_only + + self.startup_native_ready = False + if not self.startup_main_pending or not qualified(self.cp, marked_only=True): + return + if CS.steerFaultPermanent or CS.accFaulted: + self.startup_main_pending = False + return + try: + event_ns = int(sm.logMonoTime['onroadEvents']) + events_current = bool(sm.seen['onroadEvents'] and sm.valid['onroadEvents'] and sm.alive['onroadEvents'] and + 0 < event_ns <= now_ns and now_ns - event_ns <= 1_500_000_000) + events = sm['onroadEvents'] + faults = [event for event in events if disarming_fault([event], CS)] + if events_current and faults and not availability_only(faults): + self.startup_main_pending = False + return + axis = sm['aolAxisState'] + native = (_healthy_transport_native(sm, self.cp, CS, str(axis.sessionId), now_ns, native) + if native is not None else None) + self.startup_native_ready = bool(events_current and panda_ready and native is not None and native.pandaInventory and + not native.requestedLateral and not native.lateralAllowed and not native.requestedLongitudinal and not native.longitudinalAllowed and + all(not slot.controlsAllowed for slot in native.pandaInventory) and + sm.seen['aolAxisState'] and sm.valid['aolAxisState'] and sm.alive['aolAxisState'] and + int(sm.logMonoTime['aolAxisState']) == axis.observedMonoTime and + axis.validUntilMonoTime - axis.observedMonoTime <= AXIS_MAX_AGE_NS and + axis.qualified and axis.nativeAcknowledged and str(axis.faultReason) == 'none' and + str(axis.sessionId) == native.axisSessionId and producer and axis.intentProducerSessionId == producer and + not axis.desiredLateral and not axis.desiredLongitudinal and not axis.lateralActive and not axis.longitudinalActive and + 0 < axis.sourceCarStateMonoTime <= axis.observedMonoTime <= now_ns <= axis.validUntilMonoTime and + now_ns - axis.sourceCarStateMonoTime <= AXIS_MAX_AGE_NS) + except (AttributeError, KeyError, TypeError, ValueError, OverflowError): + self.startup_native_ready = False + if self.startup_native_ready: + binding = (str(axis.sessionId), producer, int(axis.intentIntegrityEpoch), native.pandaSerial, + tuple(slot.hardwareSerial for slot in native.pandaInventory)) + if binding != self.startup_binding: + self.startup_main_source_ns = 0 + self.startup_binding = binding + else: + self.startup_binding = None + self.startup_main_source_ns = 0 + def cancel_action(self, action): if action == AOL_TOGGLE and self.settings.enabled: self.allowed_latch = not self.allowed_latch @@ -62,8 +114,29 @@ class MazdaStockCardIntent(AolCardIntent): self.cancel_held = False self.cancel_ticks = 0 self.main_neutral = False + startup_main = False + if self.startup_main_pending and healthy: + eligible = bool(available and not state.cruiseState.enabled and self.settings.enabled and + self.main_action != AOL_TOGGLE and self.lkas_action != AOL_TOGGLE and + not state.steerFaultPermanent and not state.accFaulted and + not (self.cancel_held and any(self.settings.cancel_actions))) + source_ns = kwargs.get('now_ns', 0) + if not eligible: + self.startup_main_pending = False + elif self.startup_native_ready and kwargs.get('fault_active'): + self.startup_main_pending = False + elif self.startup_native_ready and self.startup_main_source_ns and source_ns > self.startup_main_source_ns: + # The first valid sample initializes the transport feedback pipeline. + # Confirm MAIN on a subsequent fresh sample before admitting startup intent. + self.startup_main_pending = False + startup_main = True + elif self.startup_native_ready and source_ns > 0: + self.startup_main_source_ns = source_ns state.buttonEvents = events super().update(state.as_reader(), **kwargs) + if startup_main and not self._fault_inhibit: + self.allowed_latch = True + self._last_latch_edge_ns = kwargs.get('now_ns', 0) if not available: self.allowed_latch = False if self.cancel_held: diff --git a/openpilot/starpilot/car/mazda/tests/test_main_startup.py b/openpilot/starpilot/car/mazda/tests/test_main_startup.py new file mode 100644 index 0000000000..ea3302881f --- /dev/null +++ b/openpilot/starpilot/car/mazda/tests/test_main_startup.py @@ -0,0 +1,83 @@ +from dataclasses import replace + +import pytest +from opendbc.car import gen_empty_fingerprint, structs +from opendbc.car.mazda.interface import CarInterface +from opendbc.car.mazda.values import CAR +from openpilot.starpilot.aol.intent import AolSettings +from openpilot.starpilot.car.mazda.stock_intent import MazdaStockCardIntent + + +@pytest.mark.parametrize('identity', (CAR.MAZDA_CX5_2022, CAR.MAZDA_CX9_2021)) +@pytest.mark.parametrize('blocker', ('disabled', 'main_toggle', 'lkas_toggle', 'active', 'permanent', 'fault')) +def test_startup_main_never_overrides_saved_off_or_disarming_fault(identity, blocker): + cp = CarInterface.get_params(identity, gen_empty_fingerprint(), [], False, False, False) + settings = AolSettings(True, 0., 0, 0, (0, 0, 0), (0, 0, 0)) + if blocker == 'disabled': + settings = replace(settings, enabled=False) + elif blocker == 'main_toggle': + settings = replace(settings, main_action=9) + elif blocker == 'lkas_toggle': + settings = replace(settings, lkas_action=9) + intent = MazdaStockCardIntent(cp, settings) + cs = structs.CarState(canValid=True, gearShifter='drive') + cs.cruiseState.available = True + cs.cruiseState.enabled = blocker == 'active' + cs.steerFaultPermanent = blocker == 'permanent' + intent.update(cs.as_reader(), fault_active=blocker == 'fault', now_ns=1_000_000_000) + assert not intent.allowed_latch + cs.cruiseState.enabled = False + cs.steerFaultPermanent = False + for tick in range(10): + intent.update(cs.as_reader(), fault_active=False, now_ns=1_010_000_000 + tick * 10_000_000) + assert not intent.allowed_latch + + +@pytest.mark.parametrize('identity', (CAR.MAZDA_CX5_2022, CAR.MAZDA_CX9_2021)) +def test_invalid_can_cannot_supply_startup_main(identity): + cp = CarInterface.get_params(identity, gen_empty_fingerprint(), [], False, False, False) + intent = MazdaStockCardIntent(cp, AolSettings(True, 0., 0, 0, (0, 0, 0), (0, 0, 0))) + cs = structs.CarState(canValid=False, canTimeout=True) + cs.cruiseState.available = True + intent.update(cs.as_reader(), fault_active=False, now_ns=1_000_000_000) + assert not intent.allowed_latch and intent.startup_main_pending + + +@pytest.mark.parametrize('bad', ('none', 'producer', 'session', 'future', 'expired', 'unacknowledged', + 'active', 'native_request', 'panda_fault', 'axis_invalid', 'events_stale')) +def test_startup_admission_requires_bound_current_idle_proof(bad): + from openpilot.starpilot.tests.test_ford_reverse_intent import NOW, PRODUCER, change, native_change, sample + + cp = CarInterface.get_params(CAR.MAZDA_CX5_2022, gen_empty_fingerprint(), [], False, False, False) + cp.alternativeExperience = 32 + intent = MazdaStockCardIntent(cp, AolSettings(True, 0., 0, 0, (0, 0, 0), (0, 0, 0))) + cs = structs.CarState(canValid=True, gearShifter='drive') + cs.cruiseState.available = True + for tick in range(1, 4): + now = NOW + tick * 10_000_000 + sm, native = sample(cp, now, now, tick, active=False, requested=False, allowed=False, event_stamp=now) + producer = PRODUCER + if bad == 'producer': + producer = 'other-card' + elif bad == 'session': + change(sm, 'aolAxisState', lambda axis: setattr(axis, 'sessionId', 'other-axis')) + elif bad == 'future': + change(sm, 'aolAxisState', lambda axis, stamp=now: setattr(axis, 'sourceCarStateMonoTime', stamp + 1)) + elif bad == 'expired': + change(sm, 'aolAxisState', lambda axis, stamp=now: setattr(axis, 'validUntilMonoTime', stamp - 1)) + elif bad == 'unacknowledged': + change(sm, 'aolAxisState', lambda axis: setattr(axis, 'nativeAcknowledged', False)) + elif bad == 'active': + change(sm, 'aolAxisState', lambda axis: setattr(axis, 'lateralActive', True)) + elif bad == 'native_request': + native = native_change(sm, native, requestedLateral=True) + elif bad == 'panda_fault': + change(sm, 'pandaStates', lambda pandas: setattr(pandas[0], 'safetyRxChecksInvalid', True)) + elif bad == 'axis_invalid': + sm.valid['aolAxisState'] = False + elif bad == 'events_stale': + sm.logMonoTime['onroadEvents'] = now - 1_500_000_001 + intent.observe_startup_readiness(sm, cs, native=native, panda_ready=True, now_ns=now, producer=producer) + intent.update(cs.as_reader(), fault_active=False, now_ns=now) + assert intent.allowed_latch is (bad == 'none') + assert not native.lateralAllowed and not native.longitudinalAllowed diff --git a/prebuilt.json b/prebuilt.json index 39f4bdec65..b3d0c302f3 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "b432c56c155a47a011e93e7a526694064716e7cf5b9afcb1b88b8dd4e8f3c933", "schema_version": 1} +{"protected_inventory_sha256": "fcc070ecccb358ea541e59f16cd29983b7a71cf11783b012fc1cf30dd5ecf156", "schema_version": 1} diff --git a/tools/ci/run_vehicle_tests.py b/tools/ci/run_vehicle_tests.py index 1094940abb..43e306698e 100644 --- a/tools/ci/run_vehicle_tests.py +++ b/tools/ci/run_vehicle_tests.py @@ -47,6 +47,7 @@ AOL_LIFECYCLE_TARGETS = ( "openpilot/starpilot/aol/tests/test_honda_alt_radar_lifecycle.py", "openpilot/starpilot/aol/tests/test_fresh_inactive_transport.py", "opendbc_repo/opendbc/car/mazda/tests/test_native_engagement_callers.py", + "openpilot/starpilot/car/mazda/tests/test_main_startup.py", "opendbc_repo/opendbc/car/mazda/tests/test_dashboard_stop_sign.py", "opendbc_repo/opendbc/safety/tests/test_mazda_stock_aol.py", "openpilot/starpilot/aol/tests/test_temporary_lag_intent.py",