hyundai: add speed-dependent steering torque limits for Kia EV6

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
 

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

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

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))

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(
     [

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) {

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

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()