diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index 0fd8dd1564..dc7e6f48a2 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -938,7 +938,8 @@ class CarController(CarControllerBase): CS.stock_lfa_msg if preserve_stock_lfa_status else None, CS.stock_lkas_msg if preserve_stock_lkas else None, lka_icon=lka_icon, - longitudinal_active=lfa_longitudinal_active)) + longitudinal_active=lfa_longitudinal_active, + v_ego=getattr(getattr(CS, "out", None), "vEgo", None))) direct_steering_active = ccnc_angle_long and drive_gear and CC.latActive and self.direct_angle_request_allowed and not CS.angle_steering_fault inactive_steering_angle = float(np.clip(CS.angle_steering_angle, -self.params.ANGLE_LIMITS.STEER_ANGLE_MAX, diff --git a/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py b/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py index c4a7ef23de..76b4a90fc7 100644 --- a/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py +++ b/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py @@ -7,6 +7,28 @@ from opendbc.car.common.conversions import Conversions as CV from opendbc.car.crc import CRC16_XMODEM from opendbc.car.hyundai.values import HyundaiFlags, CAR, CANFD_ALT_BUTTONS_RESUME_CAR +# Ioniq 6 EPS damping, matched to the car's own LFA. With the stock ADAS driving, the Ioniq 6 steers +# through LFA (0x12A) and sends ONLY a damping value in LKAS_ALT (0x110 byte 8, Hyundai name +# Damping_Gain); both are scheduled purely on speed (drive 00000b41: no dependence on angle, torque or +# hands). openpilot sent a fixed 100 in LFA and never set LKAS_ALT's, so the EPS got 0 there. The +# breakpoints are the stock medians per 5 mph band (centers) while the stock system is steering, +# rising steadily to 163 (LFA) / 154 (LKAS_ALT) at 85-90 mph. openpilot's highway weave matched the +# stock system at 45-60 mph and grew past it above 60 mph, exactly where the stock damping pulls away. +# Below ~35 mph the previous values are kept (LFA floor 100, LKAS_ALT 0) so low-speed turn tuning +# is untouched. The panda only checks torque/steer_req on these messages, not the damping byte. +_DAMP_MPH = [37.5, 42.5, 47.5, 52.5, 57.5, 62.5, 67.5, 72.5, 77.5, 82.5, 87.5] +IONIQ_6_LFA_DAMP_BP = [mph * CV.MPH_TO_MS for mph in _DAMP_MPH] +IONIQ_6_LFA_DAMP_V = [100., 105., 107., 112., 116., 119., 133., 145., 151., 157., 163.] +IONIQ_6_LKAS_ALT_DAMP_BP = [mph * CV.MPH_TO_MS for mph in [35.0] + _DAMP_MPH[1:]] +IONIQ_6_LKAS_ALT_DAMP_V = [0., 125., 127., 132., 136., 139., 142., 144., 146., 150., 154.] + + +def get_ioniq_6_damp_factors(v_ego: float) -> tuple[int, int]: + """(LFA DAMP_FACTOR, LKAS_ALT DAMP_FACTOR) for the Ioniq 6 at this speed.""" + lfa = float(np.interp(v_ego, IONIQ_6_LFA_DAMP_BP, IONIQ_6_LFA_DAMP_V)) + lkas_alt = float(np.interp(v_ego, IONIQ_6_LKAS_ALT_DAMP_BP, IONIQ_6_LKAS_ALT_DAMP_V)) + return int(round(lfa)), int(round(lkas_alt)) + def _set_value(msg: bytearray, sig, ival: int) -> None: i = sig.lsb // 8 @@ -100,7 +122,7 @@ def create_angle_adas_cmd(packer, CAN, apply_angle: float, lat_active: bool, tor def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_torque, apply_angle, lfa_base_values=None, lkas_base_values=None, lka_icon=None, - longitudinal_active=None): + longitudinal_active=None, v_ego=None): if lka_icon is None: lka_icon = 2 if enabled else 1 if longitudinal_active is None: @@ -142,6 +164,10 @@ def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_torque, lfa_values["NEW_SIGNAL_2"] = 0 lfa_values["DAMP_FACTOR"] = 100 # can potentially tuned for better perf [3, 200] + if CP.carFingerprint == CAR.HYUNDAI_IONIQ_6 and v_ego is not None and \ + not CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING: + lfa_values["DAMP_FACTOR"], lkas_values["DAMP_FACTOR"] = get_ioniq_6_damp_factors(v_ego) + if CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING and CP.flags & HyundaiFlags.CANFD_LKA_STEERING_ALT: lkas_values["ADAS_StrAnglReqVal"] = apply_angle lkas_values["LKAS_ANGLE_ACTIVE"] = 2 if lat_active else 1 diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_ioniq6_damping.py b/opendbc_repo/opendbc/car/hyundai/tests/test_ioniq6_damping.py new file mode 100644 index 0000000000..974552eded --- /dev/null +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_ioniq6_damping.py @@ -0,0 +1,62 @@ +import pytest + +from opendbc.can import CANPacker, CANParser +from opendbc.car import Bus +from opendbc.car.common.conversions import Conversions as CV +from opendbc.car.structs import CarParams +from opendbc.car.hyundai import hyundaicanfd +from opendbc.car.hyundai.hyundaicanfd import CanBus, get_ioniq_6_damp_factors +from opendbc.car.hyundai.values import CAR, DBC, HyundaiFlags + + +def _cp(fingerprint): + CP = CarParams.new_message() + CP.carFingerprint = fingerprint + CP.flags = int(HyundaiFlags.CANFD | HyundaiFlags.EV | HyundaiFlags.CANFD_LKA_STEERING | + HyundaiFlags.CANFD_LKA_STEERING_ALT) + CP.openpilotLongitudinalControl = True + return CP + + +def _damping(fingerprint, v_ego, torque=100): + CP = _cp(fingerprint) + packer = CANPacker(DBC[CP.carFingerprint][Bus.pt]) + can_bus = CanBus(CP) + msgs = hyundaicanfd.create_steering_messages(packer, CP, can_bus, True, True, torque, 0.0, v_ego=v_ego) + names = [(packer.dbc.addr_to_msg[addr].name, bus) for addr, _, bus in msgs] + assert names == [("LFA", can_bus.ECAN), ("LKAS_ALT", can_bus.ACAN)] + + lfa = CANParser(DBC[CP.carFingerprint][Bus.pt], [("LFA", 0)], can_bus.ECAN) + lfa.update([(1, [msgs[0]])]) + lkas = CANParser(DBC[CP.carFingerprint][Bus.pt], [("LKAS_ALT", 0)], can_bus.ACAN) + lkas.update([(1, [msgs[1]])]) + assert lfa.can_valid and lkas.can_valid + assert lfa.vl["LFA"]["TORQUE_REQUEST"] == torque + assert lkas.vl["LKAS_ALT"]["TORQUE_REQUEST"] == torque + return lfa.vl["LFA"]["DAMP_FACTOR"], lkas.vl["LKAS_ALT"]["DAMP_FACTOR"], lkas.vl["LKAS_ALT"]["Damping_Gain"] + + +@pytest.mark.parametrize(("mph", "expected"), [ + (10, (100, 0)), # low speed unchanged: previous LFA 100 floor, LKAS_ALT 0 + (30, (100, 0)), + (42.5, (105, 125)), # stock medians, 40-45 mph band + (50, (110, 130)), # halfway between the 45-50 (107/127) and 50-55 (112/132) bands + (72.5, (145, 144)), # stock medians, 70-75 mph band + (95, (163, 154)), # held beyond the last breakpoint (85-90 mph band) +]) +def test_ioniq_6_damping_schedule(mph, expected): + assert get_ioniq_6_damp_factors(mph * CV.MPH_TO_MS) == expected + + +def test_ioniq_6_damping_is_encoded_on_both_steering_messages(): + lfa, lkas_alt, damping_gain = _damping(CAR.HYUNDAI_IONIQ_6, 72.5 * CV.MPH_TO_MS) + assert (lfa, lkas_alt) == (145, 144) + assert damping_gain == lkas_alt # same bits: Hyundai's name for the LKAS_ALT damping byte + + +def test_ioniq_6_without_speed_keeps_previous_values(): + assert _damping(CAR.HYUNDAI_IONIQ_6, None)[:2] == (100, 0) + + +def test_other_cars_unchanged(): + assert _damping(CAR.HYUNDAI_IONIQ_5, 70 * CV.MPH_TO_MS)[:2] == (100, 0)