diff --git a/opendbc_repo/opendbc/car/hyundai/carstate.py b/opendbc_repo/opendbc/car/hyundai/carstate.py index 2a9ea8860..6a59143de 100644 --- a/opendbc_repo/opendbc/car/hyundai/carstate.py +++ b/opendbc_repo/opendbc/car/hyundai/carstate.py @@ -8,7 +8,7 @@ from opendbc.car import Bus, create_button_events, structs from opendbc.car.common.conversions import Conversions as CV from opendbc.car.hyundai.hyundaicanfd import CanBus from opendbc.car.hyundai.values import HyundaiFlags, HyundaiStarPilotFlags, HyundaiStarPilotSafetyFlags, CAR, DBC, Buttons, CarControllerParams, \ - hyundai_cancel_button_enables_cruise + hyundai_cancel_button_enables_cruise, ALT_BUS_LDA_BUTTON_CARS from opendbc.car.interfaces import CarStateBase ButtonType = structs.CarState.ButtonEvent.Type @@ -25,6 +25,7 @@ BUTTONS_DICT = {Buttons.RES_ACCEL: ButtonType.accelCruise, Buttons.SET_DECEL: Bu IONIQ_6_BLINDSPOT_RIGHT_MASK = 0x08 IONIQ_6_BLINDSPOT_LEFT_MASK = 0x10 CANFD_CAMERA_LEAD_MIN_DISTANCE = 0.1 +ALT_BUS_LDA_BUTTON_BURST_DEBOUNCE_NS = int(1.3e9) def get_non_scc_cruise_signals(CP) -> tuple[str, str, str, str, str, str]: @@ -74,6 +75,8 @@ class CarState(CarStateBase): self.cruise_buttons: deque = deque([Buttons.NONE] * PREV_BUTTON_SAMPLES, maxlen=PREV_BUTTON_SAMPLES) self.main_buttons: deque = deque([Buttons.NONE] * PREV_BUTTON_SAMPLES, maxlen=PREV_BUTTON_SAMPLES) self.lda_button = 0 + self.lda_button_raw = 0 + self.lda_button_last_raw_rise_ts_nanos = 0 self.left_paddle = 0 self.mode_button = 0 self.custom_button = 0 @@ -171,9 +174,40 @@ class CarState(CarStateBase): return False + def create_alt_bus_lda_button_events(self, cp_source: CANParser) -> list[structs.CarState.ButtonEvent]: + raw_lda_button = int(cp_source.vl["CLU13"]["CF_Clu_LdwsLkasSW"]) + raw_lda_button_ts_nanos = cp_source.ts_nanos["CLU13"]["CF_Clu_LdwsLkasSW"] + button_events: list[structs.CarState.ButtonEvent] = [] + + # Some alt-bus LKAS button layouts pulse several times per physical press burst. + # Collapse each burst into a single synthetic press/release pair. + if raw_lda_button and not self.lda_button_raw: + if self.lda_button_last_raw_rise_ts_nanos == 0 or \ + raw_lda_button_ts_nanos - self.lda_button_last_raw_rise_ts_nanos > ALT_BUS_LDA_BUTTON_BURST_DEBOUNCE_NS: + button_events = [ + structs.CarState.ButtonEvent(pressed=True, type=ButtonType.lkas), + structs.CarState.ButtonEvent(pressed=False, type=ButtonType.lkas), + ] + self.lda_button_last_raw_rise_ts_nanos = raw_lda_button_ts_nanos + + self.lda_button_raw = raw_lda_button + return button_events + + def create_lkas_button_events(self, cp: CANParser, prev_lda_button: int) -> list[structs.CarState.ButtonEvent]: + # Some classic HKG platforms publish the LKAS button on the cluster bus instead of BCM_PO_11. + if cp.ts_nanos["CLU13"]["CF_Clu_LdwsLkasSW"] > 0: + self.lda_button = int(cp.vl["CLU13"]["CF_Clu_LdwsLkasSW"]) + elif cp.ts_nanos["BCM_PO_11"]["LDA_BTN"] > 0: + self.lda_button = int(cp.vl["BCM_PO_11"]["LDA_BTN"]) + else: + self.lda_button = 0 + + return create_button_events(self.lda_button, prev_lda_button, {1: ButtonType.lkas}) + def update(self, can_parsers, starpilot_toggles) -> structs.CarState: cp = can_parsers[Bus.pt] cp_cam = can_parsers[Bus.cam] + cp_alt = can_parsers.get(Bus.alt) if self.CP.flags & HyundaiFlags.CANFD: return self.update_canfd(can_parsers) @@ -307,14 +341,17 @@ class CarState(CarStateBase): prev_cruise_buttons = self.cruise_buttons[-1] prev_main_buttons = self.main_buttons[-1] prev_lda_button = self.lda_button + 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.FPCP.safetyConfigs[-1].safetyParam & HyundaiStarPilotSafetyFlags.HAS_LDA_BUTTON: - self.lda_button = cp.vl["BCM_PO_11"]["LDA_BTN"] + if self.CP.carFingerprint in ALT_BUS_LDA_BUTTON_CARS and cp_alt is not None and cp_alt.ts_nanos["CLU13"]["CF_Clu_LdwsLkasSW"] > 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}), - *create_button_events(self.lda_button, prev_lda_button, {1: ButtonType.lkas})] + *lkas_button_events] ret.blockPcmEnable = not self.recent_button_interaction() @@ -525,11 +562,17 @@ class CarState(CarStateBase): if CP.flags & HyundaiFlags.CANFD: return self.get_can_parsers_canfd(CP) - msgs = [] + msgs = [ + ("BCM_PO_11", 0), + ("CLU13", 0), + ] if CP.flags & HyundaiFlags.NON_SCC and not (CP.flags & HyundaiFlags.NON_SCC_NO_FCA): msgs.append(("FCA11", 0)) # Non-SCC trims can stop publishing FCA11; don't let it poison canValid - return { + parsers = { Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], msgs, 0), Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], [], 2), } + if CP.carFingerprint in ALT_BUS_LDA_BUTTON_CARS: + parsers[Bus.alt] = CANParser(DBC[CP.carFingerprint][Bus.pt], [("CLU13", 0)], 1) + return parsers diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index 649c00c0f..566cf9fce 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -271,6 +271,14 @@ class TestHyundaiFingerprint: sonata = CarInterface.get_params(CAR.HYUNDAI_SONATA, fingerprint, [], False, False, False, None) assert sonata.safetyConfigs[-1].safetyParam & HyundaiStarPilotSafetyFlags.HAS_LDA_BUTTON + fingerprint = gen_empty_fingerprint() + fingerprint[0][0x50C] = 8 + forte_non_scc = CarInterface.get_params(CAR.KIA_FORTE_2021_NON_SCC, fingerprint, [], False, False, False, None) + assert forte_non_scc.safetyConfigs[-1].safetyParam & HyundaiStarPilotSafetyFlags.HAS_LDA_BUTTON + + g90 = CarInterface.get_params(CAR.GENESIS_G90, gen_empty_fingerprint(), [], False, False, False, None) + assert g90.safetyConfigs[-1].safetyParam & HyundaiStarPilotSafetyFlags.HAS_LDA_BUTTON + sonata_without_lda = CarInterface.get_params(CAR.HYUNDAI_SONATA, gen_empty_fingerprint(), [], False, False, False, None) assert not (sonata_without_lda.safetyConfigs[-1].safetyParam & HyundaiStarPilotSafetyFlags.HAS_LDA_BUTTON) @@ -596,6 +604,31 @@ class TestHyundaiFingerprint: ret = update(0, 3) assert any(be.type == ButtonType.altButton2 and not be.pressed for be in ret.buttonEvents) + def test_forte_non_scc_clu13_lkas_button_event(self): + toggles = get_test_toggles() + fingerprint = gen_empty_fingerprint() + fingerprint[0][0x50C] = 8 + CP = CarInterface.get_params(CAR.KIA_FORTE_2021_NON_SCC, fingerprint, [], False, False, False, toggles) + FPCP = CarInterface.get_starpilot_params(CAR.KIA_FORTE_2021_NON_SCC, fingerprint, [], CP, toggles) + + car_state = CarState(CP, FPCP) + can_parsers = car_state.get_can_parsers(CP) + packer = CANPacker(DBC[CP.carFingerprint][Bus.pt]) + + def update(lkas_button: int, frame: int): + msg = packer.make_can_msg("CLU13", 0, { + "CF_Clu_LdwsLkasSW": lkas_button, + }) + can_parsers[Bus.pt].update([(frame, [msg])]) + return car_state.update(can_parsers, toggles)[0] + + update(0, 1) + ret = update(1, 2) + assert any(be.type == ButtonType.lkas and be.pressed for be in ret.buttonEvents) + + ret = update(0, 3) + assert any(be.type == ButtonType.lkas and not be.pressed for be in ret.buttonEvents) + def test_ioniq_6_longitudinal_params_match_canfd_tune(self): toggles = get_test_toggles() CP = CarInterface.get_params(CAR.HYUNDAI_IONIQ_6, gen_empty_fingerprint(), [], True, False, False, toggles) diff --git a/opendbc_repo/opendbc/car/hyundai/values.py b/opendbc_repo/opendbc/car/hyundai/values.py index 0e2db5565..53f8b90ab 100644 --- a/opendbc_repo/opendbc/car/hyundai/values.py +++ b/opendbc_repo/opendbc/car/hyundai/values.py @@ -927,6 +927,11 @@ CANCEL_BUTTON_ENABLE_CARS = frozenset({ }) +ALT_BUS_LDA_BUTTON_CARS = frozenset({ + CAR.GENESIS_G90, +}) + + def hyundai_cancel_button_enables_cruise(car_fingerprint) -> bool: return car_fingerprint in CANCEL_BUTTON_ENABLE_CARS diff --git a/opendbc_repo/opendbc/car/interfaces.py b/opendbc_repo/opendbc/car/interfaces.py index 61b10d0f2..da1cf5b3b 100644 --- a/opendbc_repo/opendbc/car/interfaces.py +++ b/opendbc_repo/opendbc/car/interfaces.py @@ -20,7 +20,7 @@ from opendbc.car.common.simple_kalman import KF1D, get_kalman_gain from opendbc.car.gm.values import CAR as GM from opendbc.car.honda.values import CAR as HONDA, HONDA_BOSCH, HondaFlags, HondaSafetyFlags, HondaStarPilotFlags from opendbc.car.hyundai.hyundaicanfd import CanBus -from opendbc.car.hyundai.values import CAR as HYUNDAI, CANFD_CAR, HyundaiFlags, HyundaiStarPilotFlags, HyundaiStarPilotSafetyFlags +from opendbc.car.hyundai.values import CAR as HYUNDAI, CANFD_CAR, HyundaiFlags, HyundaiStarPilotFlags, HyundaiStarPilotSafetyFlags, ALT_BUS_LDA_BUTTON_CARS from opendbc.car.mock.values import CAR as MOCK from opendbc.car.toyota.values import CAR as TOYOTA, NO_DSU_CAR, TSS2_CAR, UNSUPPORTED_DSU_CAR, ToyotaStarPilotFlags, ToyotaSafetyFlags from opendbc.car.values import PLATFORMS @@ -199,6 +199,7 @@ class CarInterfaceBase(ABC): def get_starpilot_params(cls, candidate: str, fingerprint: dict[int, dict[int, int]], car_fw: list[structs.CarParams.CarFw], CP: structs.CarParams, starpilot_toggles: SimpleNamespace): fp_ret = custom.StarPilotCarParams.new_message() fp_ret.pcmCruiseSpeed = True + params = Params(return_defaults=True) platform = PLATFORMS[candidate] @@ -229,13 +230,20 @@ class CarInterfaceBase(ABC): fp_ret.flags |= HyundaiStarPilotFlags.SPEED_LIMIT_AVAILABLE.value fp_ret.redneckCruiseAvailable = bool(CP.flags & HyundaiFlags.NON_SCC) and not bool(CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS) - if fp_ret.redneckCruiseAvailable and Params(return_defaults=True).get_bool("RedneckCruise") and \ + if fp_ret.redneckCruiseAvailable and params.get_bool("RedneckCruise") and \ not CP.openpilotLongitudinalControl: fp_ret.pcmCruiseSpeed = False - if 0x391 in fingerprint[0] or CP.flags & HyundaiFlags.CAN_CANFD_BLENDED: + hyundai_has_lda_button = ( + 0x391 in fingerprint[0] or + 0x50C in fingerprint[0] or + candidate in ALT_BUS_LDA_BUTTON_CARS or + bool(CP.flags & HyundaiFlags.CAN_CANFD_BLENDED) + ) + if hyundai_has_lda_button: fp_ret.safetyConfigs[-1].safetyParam |= HyundaiStarPilotSafetyFlags.HAS_LDA_BUTTON.value - if starpilot_toggles.always_on_lateral_lkas: + + if params.get_bool("AlwaysOnLateral") and params.get_bool("AlwaysOnLateralLKAS"): fp_ret.safetyConfigs[-1].safetyParam |= HyundaiStarPilotSafetyFlags.AOL_LKAS_ON_ENGAGE.value elif platform in TOYOTA: fp_ret.canUsePedal = not CP.autoResumeSng diff --git a/opendbc_repo/opendbc/safety/modes/hyundai.h b/opendbc_repo/opendbc/safety/modes/hyundai.h index 94771dacf..f24ab60e3 100644 --- a/opendbc_repo/opendbc/safety/modes/hyundai.h +++ b/opendbc_repo/opendbc/safety/modes/hyundai.h @@ -56,7 +56,9 @@ const LongitudinalLimits HYUNDAI_LONG_LIMITS = { {.msg = {{0x91, 0, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \ #define HYUNDAI_LDA_BUTTON_ADDR_CHECK \ - {.msg = {{0x391, 0, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \ + {.msg = {{0x391, 0, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, \ + {0x50C, 0, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, \ + {0x50C, 1, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}}}, \ #define HYUNDAI_NON_SCC_HEV_ADDR_CHECK \ {.msg = {{0x595U, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \ @@ -226,6 +228,10 @@ static void hyundai_rx_hook(const CANPacket_t *msg) { if (msg->addr == 0x391U) { hyundai_lkas_button_check(GET_BIT(msg, 4U)); } + + if ((msg->addr == 0x50CU) && ((msg->bus == 0U) || (msg->bus == 1U))) { + hyundai_lkas_button_check(GET_BIT(msg, 56U)); + } } hyundai_common_reset_acc_main_on_mismatches(); diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 024d79445..f0365a6fe 100644 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -1,4 +1,5 @@ #!/usr/bin/env python3 +import copy import math import os import time @@ -216,13 +217,18 @@ class Car: if self.CP.brand == 'mock': CS, FPCS = self.mock_carstate.update(CS, FPCS) - self._filter_redneck_button_events(CS) - # Update radar tracks from CAN RD: structs.RadarDataT | None = self.RI.update(can_list) self.sm.update(0) + self._advance_redneck_button_feedback_filter() + original_button_events = CS.buttonEvents + filtered_button_events = self._get_filtered_redneck_button_events(CS) + using_filtered_button_events = filtered_button_events is not original_button_events + if using_filtered_button_events: + CS.buttonEvents = filtered_button_events + can_rcv_valid = len(can_strs) > 0 # Check for CAN timeout @@ -279,6 +285,9 @@ class Car: elif any(be.type in (ButtonType.decelCruise, ButtonType.setCruise) for be in CS.buttonEvents): self.resume_prev_button = False + if using_filtered_button_events: + CS.buttonEvents = original_button_events + FPCS = self.starpilot_card.update(CS, FPCS, self.sm, self.starpilot_toggles) return CS, RD, FPCS @@ -382,6 +391,13 @@ class Car: if self.redneck_cruise is None: return + original_button_events = CS.buttonEvents + filtered_button_events = self._get_filtered_redneck_button_events(CS) + using_filtered_button_events = filtered_button_events is not original_button_events + if using_filtered_button_events: + CS = copy.copy(CS) + CS.buttonEvents = filtered_button_events + send_button, v_target = self.redneck_cruise.run(CS, CC, self._get_redneck_target_speed(CS), self.is_metric) self.CI.CS.redneck_send_button = send_button self.CI.CS.redneck_v_target = v_target @@ -405,24 +421,30 @@ class Car: return fallback_target_speed - def _filter_redneck_button_events(self, CS: car.CarState) -> None: + def _advance_redneck_button_feedback_filter(self) -> None: if self.redneck_cruise is None: return for button_type in self.redneck_button_event_filter_frames: self.redneck_button_event_filter_frames[button_type] = max(0, self.redneck_button_event_filter_frames[button_type] - 1) - if len(CS.buttonEvents) == 0: - return + def _get_filtered_redneck_button_events(self, CS: car.CarState): + if self.redneck_cruise is None or len(CS.buttonEvents) == 0: + return CS.buttonEvents + + if not any(self.redneck_button_event_filter_frames.values()): + return CS.buttonEvents filtered_button_events = [] + filtered_any = False for event in CS.buttonEvents: button_type = event.type.raw if hasattr(event.type, "raw") else int(event.type) if self.redneck_button_event_filter_frames.get(button_type, 0) > 0: + filtered_any = True continue filtered_button_events.append(event) - CS.buttonEvents = filtered_button_events + return filtered_button_events if filtered_any else CS.buttonEvents def _record_redneck_button_feedback_filter(self) -> None: if self.redneck_cruise is None: diff --git a/starpilot/controls/tests/test_starpilot_card.py b/starpilot/controls/tests/test_starpilot_card.py index bae444bbf..0dcb9cbb8 100644 --- a/starpilot/controls/tests/test_starpilot_card.py +++ b/starpilot/controls/tests/test_starpilot_card.py @@ -94,6 +94,27 @@ def test_honda_lkas_button_can_toggle_always_on_lateral(monkeypatch, tmp_path): assert ret.pauseLateral is False +def test_hyundai_lkas_button_still_toggles_aol_with_cruise_button_events(monkeypatch, tmp_path): + monkeypatch.setattr(spc, "Params", FakeParams) + monkeypatch.setattr(spc, "is_FrogsGoMoo", lambda: False) + monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path) + + card = spc.StarPilotCard(SimpleNamespace(brand="hyundai"), SimpleNamespace(alternativeExperience=0)) + + car_state = make_car_state(button_events=[ + SimpleNamespace(type=spc.ButtonType.decelCruise, pressed=True), + SimpleNamespace(type=spc.ButtonType.lkas, pressed=True), + ]) + starpilot_car_state = SimpleNamespace(distancePressed=False) + sm = make_sm() + toggles = make_toggles(always_on_lateral_lkas=True) + + ret = card.update(car_state, starpilot_car_state, sm, toggles) + + assert ret.alwaysOnLateralAllowed is True + assert ret.pauseLateral is False + + def test_honda_lkas_button_pauses_lateral_when_cruise_is_active(monkeypatch, tmp_path): monkeypatch.setattr(spc, "Params", FakeParams) monkeypatch.setattr(spc, "is_FrogsGoMoo", lambda: False)