diff --git a/opendbc_repo/opendbc/car/hyundai/carstate.py b/opendbc_repo/opendbc/car/hyundai/carstate.py index 383f23f26..6b8e210fa 100644 --- a/opendbc_repo/opendbc/car/hyundai/carstate.py +++ b/opendbc_repo/opendbc/car/hyundai/carstate.py @@ -235,13 +235,12 @@ class CarState(CarStateBase): def create_lkas_button_events(self, cp: CANParser, prev_lda_button: int) -> list[structs.CarState.ButtonEvent]: if self.CP.carFingerprint == CAR.HYUNDAI_SONATA_HYBRID: self.lda_button = self.get_sonata_hybrid_lkas_button_state(cp) - # Some classic HKG platforms publish the LKAS button on the cluster bus instead of BCM_PO_11. - elif 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 + source_states = ( + int(cp.vl["CLU13"]["CF_Clu_LdwsLkasSW"]) if cp.ts_nanos["CLU13"]["CF_Clu_LdwsLkasSW"] > 0 else 0, + int(cp.vl["BCM_PO_11"]["LDA_BTN"]) if cp.ts_nanos["BCM_PO_11"]["LDA_BTN"] > 0 else 0, + ) + self.lda_button = int(any(source_states)) return create_button_events(self.lda_button, prev_lda_button, {1: ButtonType.lkas}) diff --git a/opendbc_repo/opendbc/car/hyundai/interface.py b/opendbc_repo/opendbc/car/hyundai/interface.py index f6e9af275..4e9673047 100644 --- a/opendbc_repo/opendbc/car/hyundai/interface.py +++ b/opendbc_repo/opendbc/car/hyundai/interface.py @@ -224,7 +224,7 @@ class CarInterface(CarInterfaceBase): ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.CAN_REFRESH_MSGS.value # These cars expose an LKAS/LFA steering-wheel button that StarPilot can customize. - if 0x391 in fingerprint[0] or ret.flags & HyundaiFlags.CAN_CANFD_BLENDED: + if 0x391 in fingerprint[0] or 0x50C in fingerprint[0] or ret.flags & HyundaiFlags.CAN_CANFD_BLENDED: ret.safetyConfigs[-1].safetyParam |= HyundaiStarPilotSafetyFlags.HAS_LDA_BUTTON.value if ret.flags & HyundaiFlags.CAN_CANFD_BLENDED: ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.CAN_CANFD_BLENDED.value diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index 1d78beee8..41659c16a 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -1256,6 +1256,37 @@ class TestHyundaiFingerprint: ret = update(0, 3) assert any(be.type == ButtonType.lkas and not be.pressed for be in ret.buttonEvents) + def test_elantra_lkas_button_event_is_not_masked_by_live_clu13(self): + toggles = get_test_toggles() + fingerprint = gen_empty_fingerprint() + fingerprint[0][0x391] = 8 + fingerprint[0][0x50C] = 8 + CP = CarInterface.get_params(CAR.HYUNDAI_ELANTRA_2021, fingerprint, [], False, False, False, toggles) + FPCP = CarInterface.get_starpilot_params(CAR.HYUNDAI_ELANTRA_2021, 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(bcm_lkas_button: int, frame: int): + msgs = [ + packer.make_can_msg("CLU13", 0, { + "CF_Clu_LdwsLkasSW": 0, + }), + packer.make_can_msg("BCM_PO_11", 0, { + "LDA_BTN": bcm_lkas_button, + }), + ] + can_parsers[Bus.pt].update([(frame, msgs)]) + 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_sonata_hybrid_uses_main_bus_lkas_parser(self): toggles = get_test_toggles() fingerprint = gen_empty_fingerprint() diff --git a/opendbc_repo/opendbc/car/subaru/carcontroller.py b/opendbc_repo/opendbc/car/subaru/carcontroller.py index d777f714b..47db23e36 100644 --- a/opendbc_repo/opendbc/car/subaru/carcontroller.py +++ b/opendbc_repo/opendbc/car/subaru/carcontroller.py @@ -16,6 +16,7 @@ _SNG_ACC_MIN_DIST = 3 _SNG_ACC_MAX_DIST = 4.5 _LEGACY_2025_MADS_MIN_SPEED = 0.44704 _LEGACY_2025_MADS_MAX_STEER_ANGLE = 120.0 +_LEGACY_2025_REENGAGE_MAX_STEER_RATE = 3.0 def get_safety_CP(): @@ -52,9 +53,14 @@ class CarController(CarControllerBase): mads_only = CC.latActive and not CC.enabled mads_only_ok = CS.out.vEgoRaw > _LEGACY_2025_MADS_MIN_SPEED and \ abs(CS.out.steeringAngleDeg) < _LEGACY_2025_MADS_MAX_STEER_ANGLE - lkas_active = CC.latActive and (not mads_only or mads_only_ok) and \ + lkas_available = CC.latActive and (not mads_only or mads_only_ok) and \ CS.out.gearShifter == structs.CarState.GearShifter.drive and not CS.out.standstill + manual_handoff = CS.out.steeringPressed or ( + not self.legacy_2025_lkas_active and abs(CS.out.steeringRateDeg) > _LEGACY_2025_REENGAGE_MAX_STEER_RATE + ) + lkas_active = lkas_available and not manual_handoff + if lkas_active and not self.legacy_2025_lkas_active: self.apply_steer_last = CS.out.steeringAngleDeg diff --git a/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py b/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py index 5f17bf4f6..593d3a876 100644 --- a/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py +++ b/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py @@ -207,6 +207,8 @@ def test_legacy_2025_uses_validated_angle_request_limits(): CS = SimpleNamespace(out=SimpleNamespace( vEgoRaw=2.4, steeringAngleDeg=-75.27, + steeringRateDeg=0.0, + steeringPressed=False, gearShifter=structs.CarState.GearShifter.drive, standstill=False, )) @@ -226,6 +228,43 @@ def test_legacy_2025_uses_validated_angle_request_limits(): assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(CS.out.steeringAngleDeg) +def test_legacy_2025_waits_for_manual_steering_to_settle_before_reengaging(): + CP = CarInterface.get_non_essential_params(CAR.SUBARU_LEGACY_2025) + controller = CarController({}, CP) + CC = SimpleNamespace( + enabled=True, + latActive=True, + actuators=SimpleNamespace(steeringAngleDeg=-100.0), + ) + CS = SimpleNamespace(out=SimpleNamespace( + vEgoRaw=6.2, + steeringAngleDeg=-121.55, + steeringRateDeg=350.0, + steeringPressed=True, + gearShifter=structs.CarState.GearShifter.drive, + standstill=False, + )) + parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("ES_LKAS_ANGLE", 0)], CanBus.main) + + msg = controller.lateral_angle(CC, CS) + parser.update([(1, [msg])]) + assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0 + assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(CS.out.steeringAngleDeg) + + CS.out.steeringPressed = False + CS.out.steeringAngleDeg = -113.78 + msg = controller.lateral_angle(CC, CS) + parser.update([(2, [msg])]) + assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0 + assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(CS.out.steeringAngleDeg) + + CS.out.steeringRateDeg = 2.0 + msg = controller.lateral_angle(CC, CS) + parser.update([(3, [msg])]) + assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 1 + assert abs(parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] - CS.out.steeringAngleDeg) < 1.0 + + def test_ascent_2023_uses_gen2_angle_bus_layout(): CP = CarInterface.get_non_essential_params(CAR.SUBARU_ASCENT_2023) parsers = CarState.get_can_parsers(CP)