From 7f0c5673b488cd439c74f23df604b61a9151e1eb Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Tue, 22 Sep 2026 08:37:31 -0500 Subject: [PATCH] pre-ap --- .../opendbc/car/tesla/preap/engagement.py | 5 +++++ .../opendbc/car/tesla/preap/lateral.py | 2 ++ .../tests/test_preap_steering_engagement.py | 12 ++++++++-- starpilot/controls/lib/starpilot_events.py | 14 +++++++----- .../controls/tests/test_preap_aol_alerts.py | 22 +++++++++++++++++++ 5 files changed, 48 insertions(+), 7 deletions(-) create mode 100644 starpilot/controls/tests/test_preap_aol_alerts.py diff --git a/opendbc_repo/opendbc/car/tesla/preap/engagement.py b/opendbc_repo/opendbc/car/tesla/preap/engagement.py index 6f9f7e24a5..94226ce9da 100644 --- a/opendbc_repo/opendbc/car/tesla/preap/engagement.py +++ b/opendbc_repo/opendbc/car/tesla/preap/engagement.py @@ -14,6 +14,7 @@ class PreAPEngagement: self.double_pull_window_ms = double_pull_window_ms self.cruiseEnabled = False self.lateralEnabled = False + self.lateralRearmRequired = False self.enableLongControl = False self.enableJustCC = False self.pending_enable = False @@ -30,6 +31,7 @@ class PreAPEngagement: def handle_steering_disengage(self, steering_disengage: bool) -> None: if steering_disengage and not self.prev_steering_disengage: self.lateralEnabled = False + self.lateralRearmRequired = True self.cruiseEnabled = False self.enableLongControl = False self.enableJustCC = False @@ -48,6 +50,7 @@ class PreAPEngagement: if cruise_buttons == CruiseButtons.MAIN and prev_cruise_buttons != CruiseButtons.MAIN: self.lateralEnabled = True + self.lateralRearmRequired = False if self.enableDoublePull: self._handle_double_pull(curr_time_ms, v_ego, speed_units, use_pedal, pedal_long_allowed, long_control_allowed, di_cruise_state) else: @@ -79,6 +82,7 @@ class PreAPEngagement: can_engage = not door_open and gear_shifter == structs.CarState.GearShifter.drive and not seatbelt_unlatched if not can_engage: self.lateralEnabled = False + self.lateralRearmRequired = True self.cruiseEnabled = False self.enableLongControl = False self.enableJustCC = False @@ -123,6 +127,7 @@ class PreAPEngagement: be.type = ButtonType.unknown if is_echo else ButtonType.cancel if not is_echo: self.lateralEnabled = False + self.lateralRearmRequired = True self.cruiseEnabled = False self.enableLongControl = False self.enableJustCC = False diff --git a/opendbc_repo/opendbc/car/tesla/preap/lateral.py b/opendbc_repo/opendbc/car/tesla/preap/lateral.py index b504afbf15..081a89974b 100644 --- a/opendbc_repo/opendbc/car/tesla/preap/lateral.py +++ b/opendbc_repo/opendbc/car/tesla/preap/lateral.py @@ -6,6 +6,8 @@ def preap_lateral_authorized(CP, CS, panda_states, panda_states_valid: bool) -> """Match Pre-AP's existing safety authorization without treating software CC availability as ACC main.""" if not panda_states_valid or CS.out.gearShifter != structs.CarState.GearShifter.drive or CS.out.doorOpen or CS.out.steeringDisengage: return False + if CS.engagement.lateralRearmRequired: + return False config = CP.safetyConfigs[0] matching = [p for p in panda_states if p.safetyModel == config.safetyModel and p.safetyParam == config.safetyParam] if len(matching) != 1 or matching[0].safetyRxChecksInvalid: diff --git a/starpilot/common/tests/test_preap_steering_engagement.py b/starpilot/common/tests/test_preap_steering_engagement.py index 21239a5020..c6daf2e524 100644 --- a/starpilot/common/tests/test_preap_steering_engagement.py +++ b/starpilot/common/tests/test_preap_steering_engagement.py @@ -63,14 +63,14 @@ def test_preap_aol_stalk_cancel_and_reengagement(stock_main): assert safety.safety_tx_hook(libsafety_py.make_CANPacket(addr, bus, dat)), (frame, hex(addr), dat.hex()) if addr == 0x488 and dat[2] >> 6 == 1: active_commands += 1 - assert active_commands == (290 if stock_main else 140) + assert active_commands == (240 if stock_main else 140) @pytest.mark.parametrize('failure', ['stale', 'wrong_mode', 'wrong_param', 'rx_invalid', 'park', 'door', 'override', 'cancel']) def test_preap_authorization_fails_closed(failure): cp = CarInterface.get_non_essential_params(CAR.TESLA_MODEL_S_PREAP) out = SimpleNamespace(gearShifter=structs.CarState.GearShifter.drive, doorOpen=False, steeringDisengage=False) - cs = SimpleNamespace(out=out, engagement=SimpleNamespace(lateralEnabled=True), di_cruise_state='OFF') + cs = SimpleNamespace(out=out, engagement=SimpleNamespace(lateralEnabled=True, lateralRearmRequired=False), di_cruise_state='OFF') panda = SimpleNamespace(safetyModel=cp.safetyConfigs[0].safetyModel, safetyParam=0, safetyRxChecksInvalid=False, alternativeExperience=32, controlsAllowed=True) assert preap_lateral_authorized(cp, cs, [panda], True) @@ -106,5 +106,13 @@ def test_preap_physical_lateral_session_requires_new_pull_after_reset(reset): engagement.check_can_engage(reset == 'door', gear, False) engagement.check_can_engage(False, structs.CarState.GearShifter.drive, False) assert not engagement.lateralEnabled + cp = CarInterface.get_non_essential_params(CAR.TESLA_MODEL_S_PREAP) + cs = SimpleNamespace(engagement=engagement, di_cruise_state='STANDBY', + out=SimpleNamespace(gearShifter=structs.CarState.GearShifter.drive, doorOpen=False, steeringDisengage=False)) + panda = SimpleNamespace(safetyModel=cp.safetyConfigs[0].safetyModel, safetyParam=0, + safetyRxChecksInvalid=False, alternativeExperience=32, controlsAllowed=True) + for _ in range(50): + assert not preap_lateral_authorized(cp, cs, [panda], True) engagement.process_buttons(2, 0, 12000, 10., 'KPH', False, False, True, False) assert engagement.lateralEnabled + assert preap_lateral_authorized(cp, cs, [panda], True) diff --git a/starpilot/controls/lib/starpilot_events.py b/starpilot/controls/lib/starpilot_events.py index c748ba5f55..37a826250a 100644 --- a/starpilot/controls/lib/starpilot_events.py +++ b/starpilot/controls/lib/starpilot_events.py @@ -23,6 +23,7 @@ class StarPilotEvents: self.events = Events(starpilot=True) self.always_on_lateral_allowed_previously = False + self.aol_alert_active_previously = False self.previous_traffic_mode = False self.previous_switchback_mode = False self.random_event_playing = False @@ -58,11 +59,7 @@ class StarPilotEvents: else: self.max_acceleration = 0 - if sm["starpilotCarState"].alwaysOnLateralAllowed != self.always_on_lateral_allowed_previously: - if sm["starpilotCarState"].alwaysOnLateralAllowed: - self.events.add(StarPilotEventName.lkasEnable) - else: - self.events.add(StarPilotEventName.lkasDisable) + self.update_aol_alerts(sm["carParams"], sm["starpilotCarState"]) if self.starpilot_planner.starpilot_vcruise.forcing_stop: self.events.add(StarPilotEventName.forcingStop) @@ -220,3 +217,10 @@ class StarPilotEvents: self.always_on_lateral_allowed_previously = sm["starpilotCarState"].alwaysOnLateralAllowed self.played_events.update(STARPILOT_EVENT_NAME[event] for event in self.events.names) + + def update_aol_alerts(self, car_params, car_state): + active = (car_state.alwaysOnLateralEnabled if car_params.carFingerprint == "TESLA_MODEL_S_PREAP" + else car_state.alwaysOnLateralAllowed) + if active != self.aol_alert_active_previously: + self.events.add(StarPilotEventName.lkasEnable if active else StarPilotEventName.lkasDisable) + self.aol_alert_active_previously = active diff --git a/starpilot/controls/tests/test_preap_aol_alerts.py b/starpilot/controls/tests/test_preap_aol_alerts.py new file mode 100644 index 0000000000..c47f7138d9 --- /dev/null +++ b/starpilot/controls/tests/test_preap_aol_alerts.py @@ -0,0 +1,22 @@ +from types import SimpleNamespace + +from openpilot.starpilot.controls.lib.starpilot_events import StarPilotEvents, StarPilotEventName + + +def test_preap_chimes_when_authorized_not_when_merely_selected(): + events = StarPilotEvents(None, None, None) + cp = SimpleNamespace(carFingerprint='TESLA_MODEL_S_PREAP') + cs = SimpleNamespace(alwaysOnLateralAllowed=True, alwaysOnLateralEnabled=False) + for active, expected in ((False, []), (True, [StarPilotEventName.lkasEnable]), + (True, []), (False, [StarPilotEventName.lkasDisable]), (False, [])): + events.events.clear() + cs.alwaysOnLateralEnabled = active + events.update_aol_alerts(cp, cs) + assert events.events.names == expected + + +def test_ap1_preserves_selection_chime(): + events = StarPilotEvents(None, None, None) + events.update_aol_alerts(SimpleNamespace(carFingerprint='TESLA_MODEL_S_HW1'), + SimpleNamespace(alwaysOnLateralAllowed=True, alwaysOnLateralEnabled=False)) + assert events.events.names == [StarPilotEventName.lkasEnable]