diff --git a/opendbc_repo/opendbc/car/hyundai/interface.py b/opendbc_repo/opendbc/car/hyundai/interface.py index a1d056266..f6e9af275 100644 --- a/opendbc_repo/opendbc/car/hyundai/interface.py +++ b/opendbc_repo/opendbc/car/hyundai/interface.py @@ -62,6 +62,25 @@ def apply_ecu_disable_failure_fallback(CP: structs.CarParams, params) -> None: CP.pcmCruise = True +def egmp_in_ready_state(can_recv, bus: int, timeout_s: float = 0.5) -> bool: + accelerator_addr = 0x35 + ready_bit_mask = 0x40 + deadline = time.monotonic() + timeout_s + + while time.monotonic() < deadline: + try: + can_packets = can_recv(wait_for_one=True) + except Exception: + break + + for packet in can_packets: + for msg in packet: + if msg.address == accelerator_addr and msg.src == bus and len(msg.dat) > 3 and msg.dat[3] & ready_bit_mask: + return True + + return False + + def detect_kona_non_scc_radar_fca(candidate, fingerprint, car_fw) -> bool: if candidate != CAR.HYUNDAI_KONA_NON_SCC: return False @@ -339,30 +358,37 @@ class CarInterface(CarInterfaceBase): if CP.flags & HyundaiFlags.CANFD_LKA_STEERING.value: addr, bus = 0x730, CanBus(CP).ECAN - # Try ECU disable. If it succeeds (IGN-ON mode), enable longitudinal. - # If it fails (READY mode returns NRC 0x22, or timeout), strip LONG safety flag - # so panda forwards stock SCC messages normally (lateral-only mode). - ecu_log(f"=== ECU DISABLE attempt: addr=0x{addr:x}, bus={bus} ===") - ecu_disabled = disable_ecu(can_recv, can_send, bus=bus, addr=addr, com_cont_req=communication_control, - reset=bool(CP.flags & HyundaiFlags.CAN_CANFD_BLENDED)) + skip_disable_ecu = False + if CP.carFingerprint in CANFD_ANGLE_LONGITUDINAL_CAR and egmp_in_ready_state(can_recv, bus): + apply_ecu_disable_failure_fallback(CP, params) + ecu_log(f"=== ECU DISABLE SKIPPED - READY detected, safetyParam stripped to {CP.safetyConfigs[-1].safetyParam}, lateral-only mode ===") + skip_disable_ecu = True - if CP.carFingerprint in (CAR.HYUNDAI_IONIQ_6, CAR.HYUNDAI_IONIQ_5_PE): - # Track success/failure to auto-switch between openpilot long and stock ACC - if ecu_disabled: - ECU_DISABLE_TIMESTAMP = time.monotonic() - params.put_bool("EcuDisableFailed", False) - params.put_bool("ExperimentalMode", True) - ecu_log("=== ECU DISABLE SUCCESS - Longitudinal + Experimental ENABLED ===") + if not skip_disable_ecu: + # Try ECU disable. If it succeeds (IGN-ON mode), enable longitudinal. + # If it fails (READY mode returns NRC 0x22, or timeout), strip LONG safety flag + # so panda forwards stock SCC messages normally (lateral-only mode). + ecu_log(f"=== ECU DISABLE attempt: addr=0x{addr:x}, bus={bus} ===") + ecu_disabled = disable_ecu(can_recv, can_send, bus=bus, addr=addr, com_cont_req=communication_control, + reset=bool(CP.flags & HyundaiFlags.CAN_CANFD_BLENDED)) + + if CP.carFingerprint in (CAR.HYUNDAI_IONIQ_6, CAR.HYUNDAI_IONIQ_5_PE): + # Track success/failure to auto-switch between openpilot long and stock ACC + if ecu_disabled: + ECU_DISABLE_TIMESTAMP = time.monotonic() + params.put_bool("EcuDisableFailed", False) + params.put_bool("ExperimentalMode", True) + ecu_log("=== ECU DISABLE SUCCESS - Longitudinal + Experimental ENABLED ===") + else: + apply_ecu_disable_failure_fallback(CP, params) + ecu_log(f"=== ECU DISABLE FAILED - safetyParam stripped to {CP.safetyConfigs[-1].safetyParam}, lateral-only mode ===") else: - apply_ecu_disable_failure_fallback(CP, params) - ecu_log(f"=== ECU DISABLE FAILED - safetyParam stripped to {CP.safetyConfigs[-1].safetyParam}, lateral-only mode ===") - else: - if ecu_disabled: - params.put_bool("EcuDisableFailed", False) - ecu_log("=== ECU DISABLE SUCCESS ===") - else: - apply_ecu_disable_failure_fallback(CP, params) - ecu_log(f"=== ECU DISABLE FAILED - safetyParam stripped to {CP.safetyConfigs[-1].safetyParam}, lateral-only mode ===") + if ecu_disabled: + params.put_bool("EcuDisableFailed", False) + ecu_log("=== ECU DISABLE SUCCESS ===") + else: + apply_ecu_disable_failure_fallback(CP, params) + ecu_log(f"=== ECU DISABLE FAILED - safetyParam stripped to {CP.safetyConfigs[-1].safetyParam}, lateral-only mode ===") # for blinkers if CP.flags & HyundaiFlags.ENABLE_BLINKERS: diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index 65463a3d3..750b38ee2 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -759,6 +759,34 @@ class TestHyundaiFingerprint: assert CP.pcmCruise assert not (CP.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.LONG) + @pytest.mark.parametrize("candidate", (CAR.KIA_EV9, CAR.HYUNDAI_IONIQ_5_PE)) + def test_angle_longitudinal_ready_state_skips_ecu_disable(self, candidate, monkeypatch): + toggles = get_test_toggles() + radar_config = get_radar_track_config(candidate) + fingerprint = gen_empty_fingerprint() + fingerprint[CanBus(None, fingerprint).CAM][0x110] = 32 + fingerprint[radar_config.bus][radar_config.start_addr] = radar_config.expected_length + car_fw = [CarParams.CarFw(ecu=Ecu.adas, fwVersion=b"", address=0x730, brand="hyundai")] + CP = CarInterface.get_params(candidate, fingerprint, car_fw, True, False, False, toggles) + bus = CanBus(CP).ECAN + disable_calls = [] + + def fake_disable_ecu(*args, **kwargs): + disable_calls.append(kwargs) + return True + + def can_recv(*, wait_for_one=True): + ready_msg = SimpleNamespace(address=0x35, src=bus, dat=bytes([0, 0, 0, 0x40])) + return [[ready_msg]] + + monkeypatch.setattr("opendbc.car.hyundai.interface.disable_ecu", fake_disable_ecu) + CarInterface.init(CP, can_recv, None) + + assert not any(call.get("addr") in (0x730, 0x7D0) for call in disable_calls) + assert not CP.openpilotLongitudinalControl + assert CP.pcmCruise + assert not (CP.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.LONG) + def test_xceed_phev_alpha_long_is_isolated_legacy_experiment(self): toggles = get_test_toggles()