From a69e0615420181eee89bfc8c7b321bf4fa38d9e7 Mon Sep 17 00:00:00 2001 From: Tony <108704697+TonyBinheWu@users.noreply.github.com> Date: Sat, 5 Sep 2026 23:35:45 +0800 Subject: [PATCH] hyundai: add speed-dependent steering torque limits for Kia EV6 --- opendbc/car/hyundai/carcontroller.py | 9 ++- opendbc/car/hyundai/interface.py | 2 + opendbc/car/hyundai/tests/test_hyundai.py | 18 ++++- opendbc/car/hyundai/tests/test_torque.py | 83 ++++++++++++++++++++++ opendbc/car/hyundai/values.py | 9 ++- opendbc/safety/modes/hyundai_canfd.h | 14 +++- opendbc/safety/tests/common.py | 2 +- opendbc/safety/tests/test_hyundai_canfd.py | 82 +++++++++++++++++++++ 8 files changed, 212 insertions(+), 7 deletions(-) create mode 100644 opendbc/car/hyundai/tests/test_torque.py diff --git a/opendbc/car/hyundai/carcontroller.py b/opendbc/car/hyundai/carcontroller.py index 16abe2e25ad..f1053cb1e9b 100644 --- a/opendbc/car/hyundai/carcontroller.py +++ b/opendbc/car/hyundai/carcontroller.py @@ -67,8 +67,11 @@ def update(self, CC, CS, now_nanos): hud_control = CC.hudControl # steering torque - new_torque = int(round(actuators.torque * self.params.STEER_MAX)) - apply_torque = apply_driver_steer_torque_limits(new_torque, self.apply_torque_last, CS.out.steeringTorque, self.params) + steer_max = self.params.STEER_MAX + if self.CP.flags & HyundaiFlags.CANFD_DYNAMIC_TORQUE: + steer_max = round(float(np.interp(CS.out.vEgoRaw, self.params.STEER_MAX_LOOKUP[0], self.params.STEER_MAX_LOOKUP[1]))) + new_torque = int(round(actuators.torque * steer_max)) + apply_torque = apply_driver_steer_torque_limits(new_torque, self.apply_torque_last, CS.out.steeringTorque, self.params, steer_max) # >90 degree steering fault prevention self.angle_limit_counter, apply_steer_req = common_fault_avoidance(abs(CS.out.steeringAngleDeg) >= MAX_ANGLE, CC.latActive, @@ -118,7 +121,7 @@ def update(self, CC, CS, now_nanos): stopping, hud_control, actuators, CS, CC)) new_actuators = actuators.as_builder() - new_actuators.torque = apply_torque / self.params.STEER_MAX + new_actuators.torque = apply_torque / steer_max new_actuators.torqueOutputCan = apply_torque new_actuators.accel = accel diff --git a/opendbc/car/hyundai/interface.py b/opendbc/car/hyundai/interface.py index 08a426f07c1..ff58268b91f 100644 --- a/opendbc/car/hyundai/interface.py +++ b/opendbc/car/hyundai/interface.py @@ -81,6 +81,8 @@ def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, alpha_lo ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.CANFD_ALT_BUTTONS.value if ret.flags & HyundaiFlags.CANFD_CAMERA_SCC: ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.CAMERA_SCC.value + if ret.flags & HyundaiFlags.CANFD_DYNAMIC_TORQUE: + ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.CANFD_DYNAMIC_TORQUE.value else: # Shared configuration for non CAN-FD cars diff --git a/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc/car/hyundai/tests/test_hyundai.py index 7b1241fc767..a7344cccda0 100644 --- a/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc/car/hyundai/tests/test_hyundai.py @@ -8,7 +8,7 @@ from opendbc.car.hyundai.radar_interface import RADAR_START_ADDR from opendbc.car.hyundai.values import CAR, DATE_FW_ECUS, FW_QUERY_CONFIG, CANFD_FUZZY_WHITELIST, \ PLATFORM_CODE_ECUS, HYUNDAI_VERSION_REQUEST_LONG, \ - HyundaiFlags, get_platform_codes, HyundaiSafetyFlags + HyundaiFlags, get_platform_codes, HyundaiSafetyFlags, CarControllerParams from opendbc.car.hyundai.fingerprints import FW_VERSIONS from opendbc.testing import fuzzy_test @@ -76,6 +76,22 @@ def test_alternate_limits(self): CP = CarInterface.get_params(car_model, fingerprint, [], False, False, False) assert bool(CP.flags & HyundaiFlags.ALT_LIMITS) == bool(CP.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.ALT_LIMITS) + def test_dynamic_torque_scope(self): + assert cars_with(HyundaiFlags.CANFD_DYNAMIC_TORQUE) == {CAR.KIA_EV6} + for car_model in CAR: + for lka_steering in (False, True): + fingerprint = gen_empty_fingerprint() + if lka_steering: + fingerprint[CanBus(None, fingerprint).CAM][0x50] = 16 + CP = CarInterface.get_params(car_model, fingerprint, [], False, False, False) + enabled = car_model == CAR.KIA_EV6 + assert bool(CP.flags & HyundaiFlags.CANFD_DYNAMIC_TORQUE) == enabled + assert bool(CP.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.CANFD_DYNAMIC_TORQUE) == enabled + if CP.flags & HyundaiFlags.CANFD: + params = CarControllerParams(CP) + assert params.STEER_MAX == (310 if enabled else 270) + assert (params.STEER_DELTA_UP, params.STEER_DELTA_DOWN) == (2, 3) + def test_can_features(self): for car_model in CAR: flags = car_model.config.flags diff --git a/opendbc/car/hyundai/tests/test_torque.py b/opendbc/car/hyundai/tests/test_torque.py new file mode 100644 index 00000000000..a8f34a45b31 --- /dev/null +++ b/opendbc/car/hyundai/tests/test_torque.py @@ -0,0 +1,83 @@ +import unittest + +from opendbc.can import CANParser +from opendbc.car import gen_empty_fingerprint +from opendbc.car.structs import CarControl +from opendbc.car.hyundai.interface import CarInterface +from opendbc.car.hyundai.values import CAR + + +class TestHyundaiCanfdTorque(unittest.TestCase): + def setUp(self): + CP = CarInterface.get_params(CAR.KIA_EV6, gen_empty_fingerprint(), [], False, False, False) + self.CI = CarInterface(CP) + self.CC = CarControl(enabled=True, latActive=True) + self.CC.actuators.torque = 1. + self.parser = CANParser("hyundai_canfd_generated", [("LFA", 100)], 0) + + def update(self): + timestamp = self.CI.CC.frame * 10_000_000 + actuators, msgs = self.CI.apply(self.CC.as_reader(), timestamp) + self.parser.update([timestamp, msgs]) + assert self.parser.vl["LFA"]["StrTqReqVal"] == actuators.torqueOutputCan + return actuators + + def test_curve_and_feedback(self): + # vEgo deliberately differs: the curve must use unfiltered wheel speed. + self.CI.CS.out.vEgo = 50. + for speed, maximum in ((0., 310), (9., 310), (13., 310), (13.1, 309), (13.4, 306), + (14., 300), (15., 290), (16., 280), (16.9, 271), (17., 270), (30., 270)): + for request in (-1., -0.5, 0.5, 1.): + with self.subTest(speed=speed, request=request): + self.CI.CS.out.vEgoRaw = speed + self.CC.actuators.torque = request + expected = round(request * maximum) + self.CI.CC.apply_torque_last = expected + actuators = self.update() + assert actuators.torqueOutputCan == expected + self.assertAlmostEqual(actuators.torque, expected / maximum, places=6) + + def test_driver_torque_limit_uses_dynamic_maximum(self): + for speed, maximum in ((13., 310), (15., 290), (17., 270)): + for sign in (-1, 1): + self.CI.CS.out.vEgoRaw = speed + self.CI.CS.out.steeringTorque = -sign * 260 + self.CC.actuators.torque = sign + self.CI.CC.apply_torque_last = sign * maximum + for _ in range(10): + actuators = self.update() + assert actuators.torqueOutputCan == sign * (maximum - 20) + + def test_rates_and_disengagement(self): + for request in (1., -1.): + self.CC.actuators.torque = request + for _ in range(400): + previous = self.CI.CC.apply_torque_last + actuators = self.update() + current = actuators.torqueOutputCan + if current * previous >= 0 and abs(current) > abs(previous): + assert abs(current - previous) <= 2 + else: + assert abs(current - previous) <= 3 + assert current == request * 310 + + self.CC.latActive = False + assert self.update().torqueOutputCan == 0 + assert self.parser.vl["LFA"]["ActToiSta"] == 0 + + def test_speed_transition(self): + self.CI.CC.apply_torque_last = 310 + for step in range(401): + self.CI.CS.out.vEgoRaw = 13. + step / 100 + previous = self.CI.CC.apply_torque_last + actuators = self.update() + assert 0 <= previous - actuators.torqueOutputCan <= 3 + if step % 10 == 0: + assert actuators.torqueOutputCan == 310 - step // 10 + assert actuators.torqueOutputCan == 270 + + def test_high_angle_fault_avoidance(self): + self.CI.CS.out.steeringAngleDeg = 85. + for frame in range(92): + self.update() + assert self.parser.vl["LFA"]["ActToiSta"] == (frame not in (89, 90)) diff --git a/opendbc/car/hyundai/values.py b/opendbc/car/hyundai/values.py index 2d764fbf4eb..82421aba690 100644 --- a/opendbc/car/hyundai/values.py +++ b/opendbc/car/hyundai/values.py @@ -32,6 +32,10 @@ def __init__(self, CP): self.STEER_DELTA_UP = 2 self.STEER_DELTA_DOWN = 3 + if CP.flags & HyundaiFlags.CANFD_DYNAMIC_TORQUE: + self.STEER_MAX = 310 + self.STEER_MAX_LOOKUP = ([9., 13., 17.], [310, 310, 270]) + # To determine the limit for your car, find the maximum value that the stock LKAS will request. # If the max stock LKAS request is <384, add your car to this list. elif CP.carFingerprint in (CAR.GENESIS_G80, CAR.HYUNDAI_ELANTRA, CAR.HYUNDAI_ELANTRA_GT_I30, CAR.HYUNDAI_IONIQ, @@ -66,6 +70,7 @@ class HyundaiSafetyFlags(IntFlag): CANFD_LKA_STEER_MSG_ALT = 128 FCEV_GAS = 256 ALT_LIMITS_2 = 512 + CANFD_DYNAMIC_TORQUE = 1024 # Hyundai/Kia/Genesis SCC (Smart Cruise Control) and steering architecture: @@ -147,6 +152,8 @@ class HyundaiFlags(IntFlag): ALT_LIMITS_2 = 2 ** 26 + CANFD_DYNAMIC_TORQUE = 2 ** 27 + @dataclass class HyundaiCarDocs(CarDocs): @@ -543,7 +550,7 @@ class CAR(Platforms): HyundaiCarDocs("Kia EV6 (with HDA II) 2022-24", "Highway Driving Assist II", car_parts=CarParts.common([CarHarness.hyundai_p])) ], CarSpecs(mass=2055, wheelbase=2.9, steerRatio=16, tireStiffnessFactor=0.65), - flags=HyundaiFlags.EV, + flags=HyundaiFlags.EV | HyundaiFlags.CANFD_DYNAMIC_TORQUE, ) KIA_CARNIVAL_4TH_GEN = HyundaiCanFDPlatformConfig( [ diff --git a/opendbc/safety/modes/hyundai_canfd.h b/opendbc/safety/modes/hyundai_canfd.h index bf08fb173bb..1d6af2fc46a 100644 --- a/opendbc/safety/modes/hyundai_canfd.h +++ b/opendbc/safety/modes/hyundai_canfd.h @@ -47,6 +47,7 @@ static bool hyundai_canfd_alt_buttons = false; static bool hyundai_canfd_lka_steer_msg_alt = false; +static bool hyundai_canfd_dynamic_torque = false; static unsigned int hyundai_canfd_get_lka_addr(void) { return hyundai_canfd_lka_steer_msg_alt ? 0x110U : 0x50U; @@ -136,8 +137,17 @@ static void hyundai_canfd_rx_hook(const CANPacket_t *msg) { } static bool hyundai_canfd_tx_hook(const CANPacket_t *msg) { + const struct lookup_t HYUNDAI_CANFD_MAX_TORQUE_LOOKUP = { + {9., 13., 17.}, + {310., 310., 270.}, + }; + // Cap the generic dynamic-limit tolerance at the nominal curve, including 270 at high speed. + const int max_torque = hyundai_canfd_dynamic_torque ? + ROUND(safety_interpolate(HYUNDAI_CANFD_MAX_TORQUE_LOOKUP, vehicle_speed.min / VEHICLE_SPEED_FACTOR)) : 270; const TorqueSteeringLimits HYUNDAI_CANFD_STEERING_LIMITS = { - .max_torque = 270, + .max_torque = max_torque, + .dynamic_max_torque = hyundai_canfd_dynamic_torque, + .max_torque_lookup = HYUNDAI_CANFD_MAX_TORQUE_LOOKUP, .max_rt_delta = 112, .max_rate_up = 2, .max_rate_down = 3, @@ -218,6 +228,7 @@ static bool hyundai_canfd_tx_hook(const CANPacket_t *msg) { static safety_config hyundai_canfd_init(uint16_t param) { const uint16_t HYUNDAI_PARAM_CANFD_LKA_STEER_MSG_ALT = 128; const uint16_t HYUNDAI_PARAM_CANFD_ALT_BUTTONS = 32; + const uint16_t HYUNDAI_PARAM_CANFD_DYNAMIC_TORQUE = 1024; static const CanMsg HYUNDAI_CANFD_LKA_STEER_MSG_TX_MSGS[] = { HYUNDAI_CANFD_LKA_STEER_MSG_COMMON_TX_MSGS(0, 1) @@ -267,6 +278,7 @@ static safety_config hyundai_canfd_init(uint16_t param) { gen_crc_lookup_table_16(0x1021, hyundai_canfd_crc_lut); hyundai_canfd_alt_buttons = GET_FLAG(param, HYUNDAI_PARAM_CANFD_ALT_BUTTONS); hyundai_canfd_lka_steer_msg_alt = GET_FLAG(param, HYUNDAI_PARAM_CANFD_LKA_STEER_MSG_ALT); + hyundai_canfd_dynamic_torque = GET_FLAG(param, HYUNDAI_PARAM_CANFD_DYNAMIC_TORQUE); safety_config ret; if (hyundai_longitudinal) { diff --git a/opendbc/safety/tests/common.py b/opendbc/safety/tests/common.py index 090a7fb325c..072bf9a20a0 100644 --- a/opendbc/safety/tests/common.py +++ b/opendbc/safety/tests/common.py @@ -1023,7 +1023,7 @@ def test_tx_hook_on_wrong_safety_mode(self): continue # overlapping TX addrs, but they're not actuating messages for either car - if attr == 'TestHyundaiCanfdLKASteeringLongEV' and current_test.startswith('TestToyota'): + if attr in ('TestHyundaiCanfdLKASteeringLongEV', 'TestHyundaiCanfdDynamicTorqueLKASLong') and current_test.startswith('TestToyota'): tx = list(filter(lambda m: m[0] not in [0x160, ], tx)) # Volkswagen MQB longitudinal actuating message overlaps with the Subaru lateral actuating message diff --git a/opendbc/safety/tests/test_hyundai_canfd.py b/opendbc/safety/tests/test_hyundai_canfd.py index 6cec7376ece..db0e2e4ffab 100755 --- a/opendbc/safety/tests/test_hyundai_canfd.py +++ b/opendbc/safety/tests/test_hyundai_canfd.py @@ -1,6 +1,7 @@ #!/usr/bin/env python3 from opendbc.testing import parameterized_class import unittest +import numpy as np from opendbc.car.hyundai.values import HyundaiSafetyFlags from opendbc.car.structs import CarParams @@ -284,5 +285,86 @@ def test_acc_cancel(self): pass +class HyundaiCanfdDynamicTorqueBase: + MAX_TORQUE_LOOKUP = [9., 13., 17.], [310, 310, 270] + DYNAMIC_MAX_TORQUE = True + STANDSTILL_THRESHOLD = 12 * 0.03125 / 3.6 + GAS_MSG = ("ACCELERATOR", "ACCELERATOR_PEDAL") + SAFETY_PARAM = HyundaiSafetyFlags.EV_GAS | HyundaiSafetyFlags.CAMERA_SCC + + def setUp(self): + super().setUp() + param = self.safety.get_current_safety_param() | HyundaiSafetyFlags.CANFD_DYNAMIC_TORQUE + self.safety.set_safety_hooks(CarParams.SafetyModel.hyundaiCanfd, param) + self.safety.init_tests() + + def _speed_msg(self, speed): + # Existing Hyundai tests use raw wheel-speed counts; dynamic tests use m/s. + return super()._speed_msg(speed * 3.6 / 0.03125) + + def _get_max_torque(self, speed): + # EV6 caps the generic speed/rounding tolerance at the nominal curve. + return round(float(np.interp(speed, self.MAX_TORQUE_LOOKUP[0], self.MAX_TORQUE_LOOKUP[1]))) + + def test_dynamic_torque_boundaries(self): + for speed, maximum in ((0., 310), (9., 310), (13., 310), (13.1, 309), (13.4, 306), + (14., 300), (15., 290), (16., 280), (17., 270), (18., 270), (30., 270)): + self._reset_speed_measurement(speed) + for sign in (-1, 1): + for torque in (maximum, maximum + 1): + self.safety.set_controls_allowed(True) + self._set_prev_torque(sign * torque) + assert self._tx(self._torque_cmd_msg(sign * torque)) == (torque == maximum) + + def test_dynamic_torque_flag_reset(self): + param = self.safety.get_current_safety_param() & ~HyundaiSafetyFlags.CANFD_DYNAMIC_TORQUE + self.safety.set_safety_hooks(CarParams.SafetyModel.hyundaiCanfd, param) + self.safety.init_tests() + for speed in (0., 13., 17.): + self._reset_speed_measurement(speed) + for torque in (-310, -271, -270, 270, 271, 310): + self.safety.set_controls_allowed(True) + self._set_prev_torque(torque) + assert self._tx(self._torque_cmd_msg(torque)) == (abs(torque) <= 270) + + def test_dynamic_torque_wheel_speed_quantization(self): + # Four independently quantized wheels can average to quarter-count speeds. + for raw_sum in range(5900, 7900): + counts = [raw_sum // 4 + (i < raw_sum % 4) for i in range(4)] + values = {f"WHL_Spd{pos}Val": count * 0.03125 for pos, count in zip(("FL", "FR", "RL", "RR"), counts, strict=True)} + for _ in range(common.MAX_SAMPLE_VALS): + self._rx(self.packer.make_can_msg_safety("WHEEL_SPEEDS", self.PT_BUS, values)) + speed = sum(values.values()) / 4 / 3.6 + # CarState.vEgoRaw is serialized as Float32 before the controller reads it. + torque = self._get_max_torque(float(np.float32(speed))) + self.safety.set_controls_allowed(True) + self._set_prev_torque(torque) + assert self._tx(self._torque_cmd_msg(torque)), (speed, torque, self.safety.get_vehicle_speed_min()) + + +class TestHyundaiCanfdDynamicTorqueLFA(HyundaiCanfdDynamicTorqueBase, TestHyundaiCanfdLFASteeringBase): + pass + + +class TestHyundaiCanfdDynamicTorqueLFAAltButtons(HyundaiCanfdDynamicTorqueBase, TestHyundaiCanfdLFASteeringAltButtonsBase): + pass + + +class TestHyundaiCanfdDynamicTorqueLKAS(HyundaiCanfdDynamicTorqueBase, TestHyundaiCanfdLKASteeringEV): + pass + + +class TestHyundaiCanfdDynamicTorqueLKASAlt(HyundaiCanfdDynamicTorqueBase, TestHyundaiCanfdLKASteeringAltEV): + pass + + +class TestHyundaiCanfdDynamicTorqueLKASLong(HyundaiCanfdDynamicTorqueBase, TestHyundaiCanfdLKASteeringLongEV): + pass + + +class TestHyundaiCanfdDynamicTorqueLFALong(HyundaiCanfdDynamicTorqueBase, TestHyundaiCanfdLFASteeringLongBase): + pass + + if __name__ == "__main__": unittest.main()