From 3ebc6b990366e7f2cfb15a6c6d8f21a0fab25522 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Tue, 1 Sep 2026 13:31:37 -0500 Subject: [PATCH] Break --- opendbc_repo/opendbc/car/hyundai/carstate.py | 36 ++- .../opendbc/car/hyundai/tests/test_hyundai.py | 16 +- opendbc_repo/opendbc/car/interfaces.py | 12 +- .../opendbc/car/tests/test_car_interfaces.py | 29 ++- .../opendbc/safety/modes/hyundai_common.h | 7 +- .../opendbc/safety/tests/test_hyundai.py | 94 +++++++ selfdrive/controls/controlsd.py | 11 +- selfdrive/controls/lib/drive_helpers.py | 8 + .../tests/test_hyundai_button_sequences.py | 240 ++++++++++++++++++ starpilot/car/ford/lateral.py | 5 + starpilot/car/ford/tests/test_lateral.py | 12 + starpilot/system/bluetooth/bluez.py | 8 +- starpilot/system/bluetooth/daemon.py | 34 +-- starpilot/system/bluetooth/protocol.py | 2 +- .../system/bluetooth/tests/test_bluetooth.py | 8 +- 15 files changed, 461 insertions(+), 61 deletions(-) create mode 100644 selfdrive/controls/tests/test_hyundai_button_sequences.py diff --git a/opendbc_repo/opendbc/car/hyundai/carstate.py b/opendbc_repo/opendbc/car/hyundai/carstate.py index 92906d55b..d0ae59300 100644 --- a/opendbc_repo/opendbc/car/hyundai/carstate.py +++ b/opendbc_repo/opendbc/car/hyundai/carstate.py @@ -173,8 +173,10 @@ class CarState(CarStateBase): # Main button also can trigger an engagement on these cars return any(btn in ENABLE_BUTTONS for btn in self.cruise_buttons) or any(self.main_buttons) - def update_main_cruise(self, ret: structs.CarState) -> bool: - if any(be.type == ButtonType.mainCruise and be.pressed for be in ret.buttonEvents): + def update_main_cruise(self, ret: structs.CarState, + button_events: list[structs.CarState.ButtonEvent] | None = None) -> bool: + button_events = ret.buttonEvents if button_events is None else button_events + if any(be.type == ButtonType.mainCruise and be.pressed for be in button_events): self.main_cruise_on = not self.main_cruise_on return bool(ret.cruiseState.available and self.main_cruise_on) @@ -244,6 +246,13 @@ class CarState(CarStateBase): self.lda_button = int(cp.vl["BCM_PO_11"]["LDA_BTN"]) if cp.ts_nanos["BCM_PO_11"]["LDA_BTN"] > 0 else 0 elif self.CP.carFingerprint == CAR.HYUNDAI_SONATA_HYBRID: self.lda_button = self.get_sonata_hybrid_lkas_button_state(cp) + elif self.CP.carFingerprint == CAR.HYUNDAI_ELANTRA_HEV_2024: + lda_samples = [ + *cp.vl_all["CLU13"]["CF_Clu_LdwsLkasSW"], + *cp.vl_all["BCM_PO_11"]["LDA_BTN"], + ] + if lda_samples: + self.lda_button = int(any(lda_samples)) else: source_states = ( int(cp.vl["CLU13"]["CF_Clu_LdwsLkasSW"]) if cp.ts_nanos["CLU13"]["CF_Clu_LdwsLkasSW"] > 0 else 0, @@ -327,6 +336,15 @@ class CarState(CarStateBase): ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > self.params.STEER_THRESHOLD, 5) ret.steerFaultTemporary = cp.vl["MDPS12"]["CF_Mdps_ToiUnavail"] != 0 or cp.vl["MDPS12"]["CF_Mdps_ToiFlt"] != 0 + prev_cruise_buttons = self.cruise_buttons[-1] + prev_main_buttons = self.main_buttons[-1] + prev_lda_button = self.lda_button + main_button_events = [] + if self.main_cruise_tracking: + self.cruise_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwState"]) + self.main_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwMain"]) + main_button_events = create_button_events(self.main_buttons[-1], prev_main_buttons, {1: ButtonType.mainCruise}) + # cruise state no_scc = bool(self.CP.flags & HyundaiFlags.NON_SCC) if self.CP.carFingerprint == CAR.KIA_RAY_EV: @@ -352,6 +370,9 @@ class CarState(CarStateBase): ret.cruiseState.nonAdaptive = cp_cruise.vl[scc_msg]["SCCInfoDisplay"] == 2. # Shows 'Cruise Control' on dash ret.cruiseState.speed = cp_cruise.vl[scc_msg]["VSetDis"] * speed_conv + if self.CP.openpilotLongitudinalControl and self.main_cruise_tracking: + ret.cruiseState.available = self.update_main_cruise(ret, main_button_events) + if self.CP.flags & HyundaiFlags.CAN_CANFD_BLENDED: if self.CP.flags & HyundaiFlags.CANFD_LKA_STEERING: self.lfa_block_msg = copy.copy(cp_cam.vl["CAM_0x2a4"]) @@ -421,19 +442,18 @@ class CarState(CarStateBase): self.lkas11 = copy.copy(cp_cam.vl["LKAS11"]) self.clu11 = copy.copy(cp.vl["CLU11"]) self.steer_state = cp.vl["MDPS12"]["CF_Mdps_ToiActive"] # 0 NOT ACTIVE, 1 ACTIVE - prev_cruise_buttons = self.cruise_buttons[-1] - prev_main_buttons = self.main_buttons[-1] - prev_lda_button = self.lda_button + if not self.main_cruise_tracking: + self.cruise_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwState"]) + self.main_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwMain"]) + main_button_events = create_button_events(self.main_buttons[-1], prev_main_buttons, {1: ButtonType.mainCruise}) lkas_button_events = [] - self.cruise_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwState"]) - self.main_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwMain"]) if self.CP.carFingerprint in ALT_BUS_LDA_BUTTON_CARS and cp_alt is not None and self.get_alt_bus_lda_button_raw_state(cp_alt)[1] > 0: lkas_button_events = self.create_alt_bus_lda_button_events(cp_alt) else: lkas_button_events = self.create_lkas_button_events(cp, prev_lda_button) ret.buttonEvents = [*self.create_cruise_button_events(self.cruise_buttons[-1], prev_cruise_buttons), - *create_button_events(self.main_buttons[-1], prev_main_buttons, {1: ButtonType.mainCruise}), + *main_button_events, *lkas_button_events] ret.blockPcmEnable = not self.recent_button_interaction() diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index ae02a302f..2f0b14f1d 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -813,7 +813,7 @@ class TestHyundaiFingerprint: @pytest.mark.parametrize("candidate, tracks_main_cruise", ( (CAR.HYUNDAI_ELANTRA_2021, False), - (CAR.HYUNDAI_ELANTRA_HEV_2024, False), + (CAR.HYUNDAI_ELANTRA_HEV_2024, True), (CAR.HYUNDAI_SONATA_HYBRID, False), )) def test_legacy_hyundai_long_main_cruise_tracking_is_vehicle_specific(self, candidate, tracks_main_cruise): @@ -1553,6 +1553,20 @@ class TestHyundaiFingerprint: ret = update(0, 3) assert any(be.type == ButtonType.lkas and not be.pressed for be in ret.buttonEvents) + def test_elantra_hev_lkas_button_keeps_a_short_parser_cycle_edge(self): + car_state = CarState.__new__(CarState) + car_state.CP = SimpleNamespace(carFingerprint=CAR.HYUNDAI_ELANTRA_HEV_2024) + car_state.lda_button = 0 + parser_cycle = SimpleNamespace(vl_all={ + "CLU13": {"CF_Clu_LdwsLkasSW": [0]}, + "BCM_PO_11": {"LDA_BTN": [1, 0]}, + }) + + events = car_state.create_lkas_button_events(parser_cycle, 0) + + assert any(be.type == ButtonType.lkas and be.pressed for be in events) + assert car_state.lda_button == 1 + def test_sonata_hybrid_uses_main_bus_lkas_parser(self): toggles = get_test_toggles() fingerprint = gen_empty_fingerprint() diff --git a/opendbc_repo/opendbc/car/interfaces.py b/opendbc_repo/opendbc/car/interfaces.py index c96b0bc95..646914924 100644 --- a/opendbc_repo/opendbc/car/interfaces.py +++ b/opendbc_repo/opendbc/car/interfaces.py @@ -245,6 +245,9 @@ class CarInterfaceBase(ABC): fp_ret.pcmCruiseSpeed = False CP.openpilotLongitudinalControl = True + if candidate == HYUNDAI.HYUNDAI_ELANTRA_HEV_2024 and CP.openpilotLongitudinalControl: + fp_ret.flags |= HyundaiStarPilotFlags.MAIN_CRUISE_STATE_TRACKING.value + hyundai_has_lda_button = not (CP.flags & HyundaiFlags.CANFD) and ( 0x391 in fingerprint[0] or 0x50C in fingerprint[0] or @@ -259,11 +262,14 @@ class CarInterfaceBase(ABC): if candidate in (HYUNDAI.HYUNDAI_ELANTRA_HEV_2024, HYUNDAI.HYUNDAI_SONATA_HYBRID) and \ getattr(starpilot_toggles, "always_on_lateral_main", False): - fp_ret.safetyConfigs[-1].safetyParam |= HyundaiStarPilotSafetyFlags.AOL_LKAS_ON_ENGAGE.value fp_ret.safetyConfigs[-1].safetyParam |= HyundaiStarPilotSafetyFlags.AOL_MAIN_LKAS_ON_ENGAGE.value + if candidate == HYUNDAI.HYUNDAI_SONATA_HYBRID: + fp_ret.safetyConfigs[-1].safetyParam |= HyundaiStarPilotSafetyFlags.AOL_LKAS_ON_ENGAGE.value - # LKASButtonControl == 9 means BUTTON_FUNCTIONS["AOL_TOGGLE"] in starpilot_variables. - if params.get_bool("AlwaysOnLateral") and params.get_int("LKASButtonControl") == 9: + # The refresh Elantra's safety mapping comes from the resolved Galaxy + # toggle above, not from this legacy persisted-parameter fallback. + if candidate != HYUNDAI.HYUNDAI_ELANTRA_HEV_2024 and \ + params.get_bool("AlwaysOnLateral") and params.get_int("LKASButtonControl") == 9: fp_ret.safetyConfigs[-1].safetyParam |= HyundaiStarPilotSafetyFlags.AOL_LKAS_ON_ENGAGE.value if candidate == HYUNDAI.HYUNDAI_SONATA_HYBRID and getattr(starpilot_toggles, "always_on_lateral_lkas", False) and \ diff --git a/opendbc_repo/opendbc/car/tests/test_car_interfaces.py b/opendbc_repo/opendbc/car/tests/test_car_interfaces.py index 102f3981b..4bd1a1eda 100644 --- a/opendbc_repo/opendbc/car/tests/test_car_interfaces.py +++ b/opendbc_repo/opendbc/car/tests/test_car_interfaces.py @@ -300,7 +300,7 @@ class TestCarInterfaces: ) assert fp_car_params.safetyConfigs[-1].safetyParam & HyundaiStarPilotSafetyFlags.AOL_LKAS_ON_ENGAGE.value - def test_hyundai_elantra_hev_auto_aol_sets_lkas_on_engage_flag(self): + def test_hyundai_elantra_hev_auto_aol_uses_main_engage_flag(self): toggles = get_test_starpilot_toggles() toggles.always_on_lateral_main = True fingerprint = {bus: {} for bus in range(8)} @@ -322,8 +322,33 @@ class TestCarInterfaces: toggles, ) - assert fp_car_params.safetyConfigs[-1].safetyParam & HyundaiStarPilotSafetyFlags.AOL_LKAS_ON_ENGAGE.value assert fp_car_params.safetyConfigs[-1].safetyParam & HyundaiStarPilotSafetyFlags.AOL_MAIN_LKAS_ON_ENGAGE.value + assert not (fp_car_params.safetyConfigs[-1].safetyParam & HyundaiStarPilotSafetyFlags.AOL_LKAS_ON_ENGAGE.value) + + def test_hyundai_elantra_hev_lkas_aol_uses_lkas_engage_flag(self): + toggles = get_test_starpilot_toggles() + toggles.always_on_lateral_lkas = True + fingerprint = {bus: {} for bus in range(8)} + + car_params = HyundaiCarInterface.get_params( + HYUNDAI_CAR.HYUNDAI_ELANTRA_HEV_2024, + fingerprint, + [], + alpha_long=True, + is_release=False, + docs=False, + starpilot_toggles=toggles, + ) + fp_car_params = HyundaiCarInterface.get_starpilot_params( + HYUNDAI_CAR.HYUNDAI_ELANTRA_HEV_2024, + fingerprint, + [], + car_params, + toggles, + ) + + assert fp_car_params.safetyConfigs[-1].safetyParam & HyundaiStarPilotSafetyFlags.AOL_LKAS_ON_ENGAGE.value + assert not (fp_car_params.safetyConfigs[-1].safetyParam & HyundaiStarPilotSafetyFlags.AOL_MAIN_LKAS_ON_ENGAGE.value) @pytest.mark.parametrize( ("candidate", "sets_main_aol_flag"), diff --git a/opendbc_repo/opendbc/safety/modes/hyundai_common.h b/opendbc_repo/opendbc/safety/modes/hyundai_common.h index 4d0cc32df..460a92c53 100644 --- a/opendbc_repo/opendbc/safety/modes/hyundai_common.h +++ b/opendbc_repo/opendbc/safety/modes/hyundai_common.h @@ -63,6 +63,9 @@ bool hyundai_cancel_button_enable = false; extern bool hyundai_can_refresh_msgs; bool hyundai_can_refresh_msgs = false; +extern bool hyundai_elantra_hev_2024; +bool hyundai_elantra_hev_2024 = false; + extern bool hyundai_aol_main_lkas_sync; bool hyundai_aol_main_lkas_sync = false; @@ -103,6 +106,7 @@ void hyundai_common_init(uint16_t param) { hyundai_non_scc = GET_FLAG(param, HYUNDAI_PARAM_NON_SCC); hyundai_cancel_button_enable = GET_FLAG(param, HYUNDAI_PARAM_CANCEL_BTN_ENABLE); hyundai_can_refresh_msgs = GET_FLAG(param, HYUNDAI_PARAM_CAN_REFRESH_MSGS); + hyundai_elantra_hev_2024 = hyundai_can_refresh_msgs && hyundai_hybrid_gas_signal && hyundai_camera_scc; hyundai_aol_main_lkas_sync = false; hyundai_last_button_interaction = HYUNDAI_PREV_BUTTON_SAMPLES; @@ -231,7 +235,8 @@ uint32_t get_acc_main_on_mismatches(void) { } void hyundai_lkas_button_check(const bool lkas_button) { - if (lkas_button && !lkas_button_prev) { + const bool lkas_button_controls_aol = !hyundai_elantra_hev_2024 || hyundai_aol_lkas_on_engage; + if (lkas_button_controls_aol && lkas_button && !lkas_button_prev) { lkas_on = !lkas_on; } lkas_button_prev = lkas_button; diff --git a/opendbc_repo/opendbc/safety/tests/test_hyundai.py b/opendbc_repo/opendbc/safety/tests/test_hyundai.py index aa6e7570d..21bc06392 100755 --- a/opendbc_repo/opendbc/safety/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/safety/tests/test_hyundai.py @@ -648,6 +648,100 @@ class TestHyundaiLongitudinalAolMainLkasOnEngageSafety(TestHyundaiLongitudinalSa self.assertTrue(self._tx(self._torque_cmd_msg(self.MAX_RATE_UP))) +class TestHyundaiElantraHev2024AolSafety(unittest.TestCase): + """The refresh Elantra's raw LKAS edge must agree with the app AOL mapping.""" + + TX_MSGS = [] + MAX_RATE_UP = 3 + SCC_BUS = 2 + BUTTON_BUS = 0 + + def setUp(self): + self.packer = CANPackerSafety("hyundai_can_refresh_generated") + self.safety = libsafety_py.libsafety + self.cnt_brake = 0 + self.cnt_button = 0 + self.safety.set_safety_hooks( + CarParams.SafetyModel.hyundai, + HyundaiSafetyFlags.LONG | HyundaiSafetyFlags.HYBRID_GAS | + HyundaiSafetyFlags.CAMERA_SCC | HyundaiSafetyFlags.CAN_REFRESH_MSGS | + HyundaiStarPilotSafetyFlags.HAS_LDA_BUTTON | HyundaiStarPilotSafetyFlags.AOL_LKAS_ON_ENGAGE, + ) + self.safety.init_tests() + + def _rx(self, msg): + return self.safety.safety_rx_hook(msg) + + def _tx(self, msg): + return self.safety.safety_tx_hook(msg) + + def _button_msg(self, buttons, main_button=0): + values = { + "CF_Clu_CruiseSwState": buttons, + "CF_Clu_CruiseSwMain": main_button, + "CF_Clu_AliveCnt1": self.cnt_button, + } + self.cnt_button += 1 + return self.packer.make_can_msg_safety("CLU11", self.BUTTON_BUS, values) + + def _user_brake_msg(self, brake): + values = { + "DriverOverride": 2 if brake else random.choice((0, 1, 3)), + "AliveCounterTCS": self.cnt_brake % 8, + } + self.cnt_brake += 1 + return self.packer.make_can_msg_safety("TCS13", 0, values, fix_checksum=checksum) + + def _torque_cmd_msg(self, torque, steer_req=1): + values = {"CR_Lkas_StrToqReq": torque, "CF_Lkas_ActToi": steer_req} + return self.packer.make_can_msg_safety("LKAS11", 0, values) + + def _set_prev_torque(self, torque): + self.safety.set_desired_torque_last(torque) + self.safety.set_rt_torque_last(torque) + + @staticmethod + def _lkas_button_msg(pressed): + dat = bytearray(8) + dat[0] = int(pressed) << 4 + return libsafety_py.make_CANPacket(0x391, 0, bytes(dat)) + + def test_lkas_mapping_survives_brake(self): + self.safety.set_alternative_experience(ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL) + self.safety.set_controls_allowed(False) + + self._rx(self._lkas_button_msg(False)) + self._rx(self._lkas_button_msg(True)) + self._rx(self._lkas_button_msg(False)) + self.assertTrue(self.safety.get_lkas_on()) + + self._rx(self._user_brake_msg(True)) + self.assertFalse(self.safety.get_controls_allowed()) + self._set_prev_torque(0) + self.assertTrue(self._tx(self._torque_cmd_msg(self.MAX_RATE_UP))) + + def test_main_mapping_does_not_toggle_on_raw_lkas_edge(self): + self.safety.set_safety_hooks( + CarParams.SafetyModel.hyundai, + HyundaiSafetyFlags.LONG | HyundaiSafetyFlags.HYBRID_GAS | + HyundaiSafetyFlags.CAMERA_SCC | HyundaiSafetyFlags.CAN_REFRESH_MSGS | + HyundaiStarPilotSafetyFlags.HAS_LDA_BUTTON | HyundaiStarPilotSafetyFlags.AOL_MAIN_LKAS_ON_ENGAGE, + ) + self.safety.init_tests() + self.safety.set_alternative_experience(ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL) + self.safety.set_controls_allowed(False) + + self._rx(self._lkas_button_msg(False)) + self._rx(self._lkas_button_msg(True)) + self._rx(self._lkas_button_msg(False)) + self.assertFalse(self.safety.get_lkas_on()) + + self._rx(self._button_msg(Buttons.NONE, main_button=True)) + self._rx(self._button_msg(Buttons.NONE, main_button=False)) + self.assertTrue(self.safety.get_acc_main_on()) + self.assertTrue(self.safety.get_lkas_on()) + + class TestHyundaiAolLkasOnEngageStockSafety(HyundaiAolLkasOnEngageStockBase, TestHyundaiSafety): def setUp(self): self.packer = CANPackerSafety("hyundai_kia_generic") diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 40b15bf3a..0df6581c2 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -21,6 +21,7 @@ from openpilot.selfdrive.controls.lib.drive_helpers import ( clip_curvature, get_kona_non_scc_lateral_active, get_lateral_active, + update_lateral_fault_latch, ) from openpilot.selfdrive.controls.lib.lane_centering import LaneCenteringController from openpilot.selfdrive.controls.lib.latcontrol import LatControl @@ -522,10 +523,12 @@ class Controls: always_on_lateral_enabled = self.sm['starpilotCarState'].alwaysOnLateralEnabled lateral_requested = (CC.enabled and self.sm['selfdriveState'].active) or always_on_lateral_enabled cruise_reenabled = CS.cruiseState.enabled and not self.elantra_hev_2024_previous_cruise_enabled - if not lateral_requested or cruise_reenabled: - self.elantra_hev_2024_lateral_faulted = False - elif CS.steerFaultTemporary: - self.elantra_hev_2024_lateral_faulted = True + self.elantra_hev_2024_lateral_faulted = update_lateral_fault_latch( + self.elantra_hev_2024_lateral_faulted, + lateral_requested, + CS.steerFaultTemporary, + reset=cruise_reenabled, + ) CC.latActive = get_lateral_active( CC.enabled, self.sm['selfdriveState'].active, always_on_lateral_enabled, diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index c5b97d02c..7a6d5b855 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -78,6 +78,14 @@ def get_lateral_active(enabled: bool, active: bool, always_on_lateral_enabled: b not steer_fault_latched and (not standstill or steer_at_standstill) and lateral_check +def update_lateral_fault_latch(previous_latched: bool, lateral_requested: bool, + steer_fault_temporary: bool, reset: bool = False) -> bool: + """Keep a temporary-fault latch until lateral is no longer requested or reset explicitly.""" + if not lateral_requested or reset: + return False + return previous_latched or steer_fault_temporary + + def get_kona_non_scc_lateral_active(enabled: bool, active: bool, always_on_lateral_enabled: bool, steer_fault_temporary: bool, steer_fault_permanent: bool, standstill: bool, steer_at_standstill: bool, lateral_check: bool, diff --git a/selfdrive/controls/tests/test_hyundai_button_sequences.py b/selfdrive/controls/tests/test_hyundai_button_sequences.py new file mode 100644 index 000000000..ae552da25 --- /dev/null +++ b/selfdrive/controls/tests/test_hyundai_button_sequences.py @@ -0,0 +1,240 @@ +from itertools import product +from types import SimpleNamespace + +import pytest + +from opendbc.car.hyundai.values import CAR as HYUNDAI_CAR, HyundaiFlags +from openpilot.selfdrive.controls.lib.drive_helpers import ( + get_lateral_active, + update_lateral_fault_latch, +) +from openpilot.starpilot.controls import starpilot_card as spc + + +class FakeParams: + def __init__(self, *args, **kwargs): + self._store = {} + + def get(self, key): + return self._store.get(key) + + def get_bool(self, key): + return bool(self._store.get(key, False)) + + def put_bool(self, key, value): + self._store[key] = bool(value) + + def put(self, key, value): + self._store[key] = value + + def get_int(self, key, default=0): + return int(self._store.get(key, default)) + + def put_int(self, key, value): + self._store[key] = int(value) + + def put_bool_nonblocking(self, key, value): + self.put_bool(key, value) + + +class FakeSM(dict): + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + self.updated = {"starpilotPlan": False} + + +def make_sm(): + return FakeSM({ + "carControl": SimpleNamespace(longActive=False, latActive=False), + "selfdriveState": SimpleNamespace(active=False, alertType=[], experimentalMode=False), + "starpilotSelfdriveState": SimpleNamespace(alertType=[]), + "starpilotPlan": SimpleNamespace(lateralCheck=True), + "liveCalibration": SimpleNamespace(calPerc=100), + }) + + +def make_toggles(**overrides): + defaults = { + "always_on_lateral": True, + "always_on_lateral_lkas": False, + "always_on_lateral_main": False, + "always_on_lateral_pause_speed": 0.0, + "lkas_allowed_for_aol": True, + "main_cruise_aol_toggle": False, + "main_cruise_slc_adopt": False, + "speed_limit_controller": False, + "openpilot_longitudinal": False, + "pulse_and_glide_available": False, + "pulse_and_glide_via_cancel": False, + "pulse_and_glide_via_cancel_long": False, + "pulse_and_glide_via_cancel_very_long": False, + "pulse_and_glide_via_lkas": False, + "has_canfd_media_buttons": False, + "experimental_mode_available": False, + "conditional_experimental_mode": False, + "conditional_chill_mode": False, + "safe_mode": False, + } + for key in ("lkas", "main_cruise", "cancel", "cancel_long", "cancel_very_long", "distance", + "distance_long", "distance_very_long", "mode", "mode_long", "mode_very_long", + "star", "star_long", "star_very_long"): + for prefix in ("experimental_mode_via_", "bookmark_via_", "force_coast_via_", + "pause_lateral_via_", "pause_longitudinal_via_", "switchback_mode_via_", + "traffic_mode_via_"): + defaults[f"{prefix}{key}"] = False + for slot in range(1, 4): + for key in ("lkas", "main_cruise", "cancel", "distance"): + defaults[f"favorite_{slot}_via_{key}"] = False + defaults.update(overrides) + return SimpleNamespace(**defaults) + + +def make_car_state(*, available=False, enabled=False, gear=None, button_type=None, + brake_pressed=False, v_ego=15.0): + events = [] if button_type is None else [SimpleNamespace(type=button_type, pressed=True)] + return SimpleNamespace( + buttonEvents=events, + cruiseState=SimpleNamespace(available=available, enabled=enabled), + gearShifter=spc.GearShifter.drive if gear is None else gear, + brakePressed=brake_pressed, + gasPressed=False, + standstill=False, + vEgo=v_ego, + ) + + +HYUNDAI_PLATFORM_CASES = ( + pytest.param(HYUNDAI_CAR.HYUNDAI_ELANTRA_HEV_2024, + HyundaiFlags.CHECKSUM_CRC8 | HyundaiFlags.CAMERA_SCC | HyundaiFlags.HYBRID, + id="elantra-hybrid-2024-26"), + pytest.param(HYUNDAI_CAR.HYUNDAI_SONATA, + HyundaiFlags.MANDO_RADAR | HyundaiFlags.CHECKSUM_CRC8, + id="sonata"), + pytest.param(HYUNDAI_CAR.HYUNDAI_SONATA_HYBRID, + HyundaiFlags.MANDO_RADAR | HyundaiFlags.CHECKSUM_CRC8 | HyundaiFlags.HYBRID, + id="sonata-hybrid"), + pytest.param(HYUNDAI_CAR.HYUNDAI_KONA_NON_SCC, + HyundaiFlags.NON_SCC | HyundaiFlags.ALT_LIMITS, + id="kona-non-scc"), +) + + +@pytest.mark.parametrize(("fingerprint", "flags"), HYUNDAI_PLATFORM_CASES) +@pytest.mark.parametrize("mapping", ("lkas", "main")) +def test_hyundai_aol_mapping_survives_brake_and_cruise_reenable( + monkeypatch, tmp_path, fingerprint, flags, mapping, +): + monkeypatch.setattr(spc, "Params", FakeParams) + monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path) + + card = spc.StarPilotCard( + SimpleNamespace(brand="hyundai", carFingerprint=fingerprint, flags=flags), + SimpleNamespace(alternativeExperience=spc.ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL), + ) + toggles = make_toggles( + always_on_lateral_lkas=mapping == "lkas", + main_cruise_aol_toggle=mapping == "main", + ) + sm = make_sm() + starpilot_car_state = SimpleNamespace(distancePressed=False) + button_type = spc.ButtonType.lkas if mapping == "lkas" else spc.ButtonType.mainCruise + + pressed = make_car_state(button_type=button_type) + card.update(pressed, starpilot_car_state, sm, toggles) + assert pressed.buttonEvents + assert starpilot_car_state.alwaysOnLateralAllowed is True + assert starpilot_car_state.alwaysOnLateralEnabled is True + + active = make_car_state(available=True, enabled=True) + sm["selfdriveState"].active = True + card.update(active, starpilot_car_state, sm, toggles) + + braking = make_car_state(available=True, enabled=False, brake_pressed=True) + sm["selfdriveState"].active = False + card.update(braking, starpilot_car_state, sm, toggles) + assert starpilot_car_state.alwaysOnLateralEnabled is True + + reenabled = make_car_state(available=True, enabled=True) + sm["selfdriveState"].active = True + card.update(reenabled, starpilot_car_state, sm, toggles) + assert starpilot_car_state.alwaysOnLateralAllowed is True + assert starpilot_car_state.alwaysOnLateralEnabled is True + + +@pytest.mark.parametrize(("fingerprint", "flags"), HYUNDAI_PLATFORM_CASES) +@pytest.mark.parametrize("mapping", ("lkas", "main")) +def test_hyundai_button_sequences_are_total(monkeypatch, tmp_path, fingerprint, flags, mapping): + monkeypatch.setattr(spc, "Params", FakeParams) + monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path) + + card = spc.StarPilotCard( + SimpleNamespace(brand="hyundai", carFingerprint=fingerprint, flags=flags), + SimpleNamespace(alternativeExperience=spc.ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL), + ) + toggles = make_toggles( + always_on_lateral_lkas=mapping == "lkas", + main_cruise_aol_toggle=mapping == "main", + ) + sm = make_sm() + starpilot_car_state = SimpleNamespace(distancePressed=False) + button_types = (None, spc.ButtonType.lkas, spc.ButtonType.mainCruise, + spc.ButtonType.cancel, spc.ButtonType.accelCruise, + spc.ButtonType.decelCruise) + + for sequence in product(button_types, repeat=3): + for frame, button_type in enumerate(sequence): + gear = spc.GearShifter.neutral if frame == 1 else spc.GearShifter.drive + car_state = make_car_state( + available=frame != 1, + enabled=frame == 2, + gear=gear, + button_type=button_type, + brake_pressed=frame == 1, + ) + sm["selfdriveState"].active = frame == 2 + result = card.update(car_state, starpilot_car_state, sm, toggles) + assert not result.alwaysOnLateralEnabled or result.alwaysOnLateralAllowed + + +def test_elantra_hybrid_fault_latch_rearms_on_cruise_rising_edge(): + faulted = False + previous_cruise_enabled = False + + def step(cruise_enabled, steer_fault_temporary): + nonlocal faulted, previous_cruise_enabled + cruise_reenabled = cruise_enabled and not previous_cruise_enabled + faulted = update_lateral_fault_latch( + faulted, + lateral_requested=True, + steer_fault_temporary=steer_fault_temporary, + reset=cruise_reenabled, + ) + lateral_active = get_lateral_active( + False, False, True, steer_fault_temporary, False, + False, False, True, faulted, + ) + previous_cruise_enabled = cruise_enabled + return lateral_active + + assert step(True, False) is True + assert step(False, True) is False + assert step(False, False) is False + assert step(True, False) is True + + +@pytest.mark.parametrize("gear", (spc.GearShifter.neutral, spc.GearShifter.park, spc.GearShifter.reverse)) +def test_lkas_aol_does_not_stick_enabled_in_non_driving_gears(monkeypatch, tmp_path, gear): + monkeypatch.setattr(spc, "Params", FakeParams) + monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path) + + card = spc.StarPilotCard( + SimpleNamespace(brand="hyundai", carFingerprint=HYUNDAI_CAR.HYUNDAI_SONATA), + SimpleNamespace(alternativeExperience=spc.ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL), + ) + toggles = make_toggles(always_on_lateral_lkas=True) + sm = make_sm() + starpilot_car_state = SimpleNamespace(distancePressed=False) + + card.update(make_car_state(button_type=spc.ButtonType.lkas), starpilot_car_state, sm, toggles) + result = card.update(make_car_state(gear=gear), starpilot_car_state, sm, toggles) + assert result.alwaysOnLateralEnabled is False diff --git a/starpilot/car/ford/lateral.py b/starpilot/car/ford/lateral.py index 2a4c35239..027bfb7c0 100644 --- a/starpilot/car/ford/lateral.py +++ b/starpilot/car/ford/lateral.py @@ -29,6 +29,9 @@ PATH_ANGLE_MAX = 0.5235 STEER_DT = CarControllerParams.STEER_STEP * DT_CTRL CURVATURE_LOOKAHEAD_MIN = 0.20 CURVATURE_LOOKAHEAD_MAX = 0.40 +FORD_CURVATURE_LOOKAHEAD = { + CAR.FORD_EXPLORER_MK6: 0.20, +} ANGLE_HANDOFF_PRESS_SECONDS = 0.5 HANDOFF_PAUSE_MIN_FRAMES = 3 HANDOFF_PAUSE_FRAMES = 6 @@ -173,6 +176,8 @@ class FordLateralController: return float(np.interp(lookup_time, ModelConstants.T_IDXS, curvatures)) def _curvature_lookahead(self) -> float: + if self.CP.carFingerprint in FORD_CURVATURE_LOOKAHEAD: + return FORD_CURVATURE_LOOKAHEAD[self.CP.carFingerprint] if self.sm is None: return CURVATURE_LOOKAHEAD_MIN live_delay = float(self.sm["liveDelay"].lateralDelay) diff --git a/starpilot/car/ford/tests/test_lateral.py b/starpilot/car/ford/tests/test_lateral.py index 8748ca128..8593c896f 100644 --- a/starpilot/car/ford/tests/test_lateral.py +++ b/starpilot/car/ford/tests/test_lateral.py @@ -3,6 +3,7 @@ from types import SimpleNamespace import pytest +from opendbc.car.ford.values import CAR from ..lateral import HANDOFF_PAUSE_FRAMES, HANDOFF_PAUSE_MIN_FRAMES, FordLateralController, HumanTurnDetector @@ -65,6 +66,17 @@ def test_curvature_lookahead_tracks_bounded_live_delay(controller): assert controller._curvature_lookahead() == pytest.approx(0.4) +def test_explorer_curvature_lookahead_does_not_follow_actuator_delay(monkeypatch): + messaging = SimpleNamespace(SubMaster=FakeSubMaster) + monkeypatch.setitem(sys.modules, "cereal.messaging", messaging) + CP = SimpleNamespace(flags=0, carFingerprint=CAR.FORD_EXPLORER_MK6) + controller = FordLateralController(CP) + controller.sm = FakeSubMaster(["modelV2", "liveDelay"]) + controller.sm["liveDelay"].lateralDelay = 0.42 + + assert controller._curvature_lookahead() == pytest.approx(0.20) + + def test_curvature_strategy_uses_learned_lookahead(controller, monkeypatch): controller.sm["liveDelay"].lateralDelay = 0.38 lookaheads = [] diff --git a/starpilot/system/bluetooth/bluez.py b/starpilot/system/bluetooth/bluez.py index a5cd5ba8a..a3765c775 100644 --- a/starpilot/system/bluetooth/bluez.py +++ b/starpilot/system/bluetooth/bluez.py @@ -38,7 +38,6 @@ class PairingAgent: self._response: tuple[bool, str] | None = None self._generation = 0 self._auto_accept_paths: set[str] = set() - self._auto_accept_incoming = False @property def prompt(self) -> dict[str, Any] | None: @@ -95,14 +94,9 @@ class PairingAgent: else: self._auto_accept_paths.discard(device_path) - def set_auto_accept_incoming(self, enabled: bool) -> None: - with self._condition: - self._auto_accept_incoming = enabled - def auto_accept(self, kind: str, device_path: str) -> bool: with self._condition: - return kind in {"confirmation", "authorization"} and (self._auto_accept_incoming or device_path in self._auto_accept_paths) - + return kind in {"confirmation", "authorization"} and device_path in self._auto_accept_paths class BlueZClient: def __init__(self): diff --git a/starpilot/system/bluetooth/daemon.py b/starpilot/system/bluetooth/daemon.py index a7cacdec4..b755322d8 100644 --- a/starpilot/system/bluetooth/daemon.py +++ b/starpilot/system/bluetooth/daemon.py @@ -17,7 +17,6 @@ OFFROAD_COMMANDS = {"set_power", "start_scan", "stop_scan", "pair", "forget", "t SCAN_DURATION = 20.0 AUDIO_TEST_START_DELAY = 3.0 AUDIO_TEST_HOLD_TIME = 3.0 -SCAN_RESULT_TTL = 30.0 class BluetoothController: @@ -33,7 +32,6 @@ class BluetoothController: self._pairing_error = "" self._last_reconnect = 0.0 self._scan_deadline = 0.0 - self._recent_devices: dict[str, tuple[dict[str, Any], float]] = {} self._audio_test_deadline = 0.0 self._sleep = sleep self.params.remove("BluetoothAudioTestActive") @@ -60,7 +58,6 @@ class BluetoothController: self._radio.start() self._bluez = self._bluez_factory() self._bluez.set_powered(True) - self._bluez.agent.set_auto_accept_incoming(self._offroad()) try: self._bluez.set_discoverable(True) except Exception as error: @@ -79,24 +76,6 @@ class BluetoothController: def _offroad(self) -> bool: return self.params.get_bool("IsOffroad") - def _merge_recent_devices(self, result: dict[str, Any]) -> None: - now = time.monotonic() - current = {str(device.get("address", "")).upper() for device in result["devices"]} - if result["discovering"]: - for device in result["devices"]: - address = str(device.get("address", "")).upper() - if address: - self._recent_devices[address] = (dict(device), now + SCAN_RESULT_TTL) - return - - for address, (device, expires) in list(self._recent_devices.items()): - if expires <= now: - self._recent_devices.pop(address, None) - elif address not in current: - result["devices"].append(dict(device)) - result["devices"].sort(key=lambda device: (not device["connected"], not device["paired"], - -(device["rssi"] or -127), device["name"].lower())) - def status(self) -> dict[str, Any]: # Status lazily initializes the radio, so serialize it with power changes. with self._lock: @@ -117,8 +96,6 @@ class BluetoothController: try: result.update(self._client().status()) result["available"] = True - self._bluez.agent.set_auto_accept_incoming(result["offroad"]) - self._merge_recent_devices(result) prompt = result.get("prompt") if prompt is not None and self._pairing_address: prompt["address"] = self._pairing_address @@ -145,6 +122,10 @@ class BluetoothController: self._pairing_error = str(error) cloudlog.exception("Bluetooth pairing failed") finally: + try: + self._client().stop_discovery() + except Exception: + pass self._pairing_address = "" def _test_audio_worker(self, address: str, deadline: float) -> None: @@ -196,7 +177,6 @@ class BluetoothController: self._radio.stop() self.params.put_bool("BluetoothEnabled", False) self._scan_deadline = 0.0 - self._recent_devices.clear() elif command == "start_scan": if not self.params.get_bool("BluetoothEnabled"): raise RuntimeError("Enable Bluetooth before scanning") @@ -210,11 +190,6 @@ class BluetoothController: if self._pairing_address: raise RuntimeError("Another Bluetooth device is already pairing") device = self._client().device_for_address(address) - try: - self._client().stop_discovery() - except Exception as error: - cloudlog.warning(f"Bluetooth discovery stop before pairing failed: {error}") - self._scan_deadline = 0.0 self._pairing_address = address self._pairing_error = "" threading.Thread(target=self._pair_worker, args=(address, device["path"]), daemon=True).start() @@ -224,7 +199,6 @@ class BluetoothController: self._client().disconnect(address) elif command == "forget": self._client().remove(address) - self._recent_devices.pop(address.upper(), None) if (self.params.get("BluetoothAudioAddress", encoding="utf-8") or "").upper() == address.upper(): self.params.remove("BluetoothAudioAddress") elif command == "select_audio": diff --git a/starpilot/system/bluetooth/protocol.py b/starpilot/system/bluetooth/protocol.py index 1594bba06..8c648ccd4 100644 --- a/starpilot/system/bluetooth/protocol.py +++ b/starpilot/system/bluetooth/protocol.py @@ -100,7 +100,7 @@ def show_pairing_device(address: str, name: str, paired: bool, trusted: bool, co normalized_address = "".join(character for character in address.upper() if character.isalnum()) normalized_name = "".join(character for character in name.upper() if character.isalnum()) named = bool(name) and name != "Unknown device" and normalized_name != normalized_address - return known or (named and not blocked and (audio or controller or discovering)) + return known or (named and not blocked and (audio or controller)) class _DesktopFakeBluetooth: diff --git a/starpilot/system/bluetooth/tests/test_bluetooth.py b/starpilot/system/bluetooth/tests/test_bluetooth.py index f8d334baa..503855be4 100644 --- a/starpilot/system/bluetooth/tests/test_bluetooth.py +++ b/starpilot/system/bluetooth/tests/test_bluetooth.py @@ -39,9 +39,6 @@ class FakeAgent: def __init__(self): self.responses = [] - def set_auto_accept_incoming(self, _enabled): - pass - def respond(self, prompt_id, accepted, value): self.responses.append((prompt_id, accepted, value)) return prompt_id == "prompt" @@ -56,6 +53,7 @@ class FakeBlueZ: self.closed = False self.actions = [] self.device = { + "path": "/fake/device", "address": "00:11:22:33:44:55", "name": "Speaker", "paired": True, @@ -87,7 +85,7 @@ class FakeBlueZ: def device_for_address(self, _address): return dict(self.device) - def pair(self, address): + def pair(self, address, _device_path=None): self.actions.append(("pair", address)) def connect(self, address): @@ -177,6 +175,8 @@ def test_pairing_list_filters_anonymous_and_irrelevant_advertisements(): assert not show_pairing_device("00:11:22:33:44:55", "00:11:22:33:44:55", False, False, False, False, False, False) assert not show_pairing_device("00:11:22:33:44:55", "Nearby sensor", False, False, False, False, False, False) assert show_pairing_device("00:11:22:33:44:55", "Media Remote", False, False, False, False, False, True) + assert show_pairing_device("00:11:22:33:44:55", "Media Remote", False, False, False, False, False, True, True) + assert not show_pairing_device("00:11:22:33:44:55", "Nearby sensor", False, False, False, False, False, False, True) assert show_pairing_device("00:11:22:33:44:55", "Known device", True, True, False, False, False, False)