Add angle steering support for Hyundai Ioniq 5 PE 2025+

This commit introduces angle-based steering control for the Hyundai Ioniq 5 PE (2025+ models) with Highway Driving Assist II (HDA2). It includes fingerprint updates, platform configuration, and control modifications to enable precise steering control and fault handling for CAN FD angle steering systems.

Author: devtekve

opendbc/car/hyundai/carcontroller.py

@@ -1,6 +1,7 @@
 import numpy as np
 from opendbc.can.packer import CANPacker
-from opendbc.car import Bus, DT_CTRL, apply_driver_steer_torque_limits, common_fault_avoidance, make_tester_present_msg, structs
+from opendbc.car import Bus, DT_CTRL, apply_driver_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance, \
+                        make_tester_present_msg, structs
 from opendbc.car.common.conversions import Conversions as CV
 from opendbc.car.hyundai import hyundaicanfd, hyundaican
 from opendbc.car.hyundai.carstate import CarState
@@ -16,9 +17,9 @@
 
 # EPS faults if you apply torque while the steering angle is above 90 degrees for more than 1 second
 # All slightly below EPS thresholds to avoid fault
-MAX_ANGLE = 85
-MAX_ANGLE_FRAMES = 89
-MAX_ANGLE_CONSECUTIVE_FRAMES = 2
+MAX_FAULT_ANGLE = 85
+MAX_FAULT_ANGLE_FRAMES = 89
+MAX_FAULT_ANGLE_CONSECUTIVE_FRAMES = 2
 
 
 def process_hud_alert(enabled, fingerprint, hud_control):
@@ -53,33 +54,54 @@ def __init__(self, dbc_names, CP, CP_SP):
     self.CAN = CanBus(CP)
     self.params = CarControllerParams(CP)
     self.packer = CANPacker(dbc_names[Bus.pt])
-    self.angle_limit_counter = 0
+    self.car_fingerprint = CP.carFingerprint
 
     self.accel_last = 0
     self.apply_torque_last = 0
-    self.car_fingerprint = CP.carFingerprint
+    self.apply_angle_last = 0
+    self.lkas_max_torque = 0
     self.last_button_frame = 0
+    self.angle_limit_counter = 0
 
   def update(self, CC, CC_SP, CS, now_nanos):
     EsccCarController.update(self, CS)
     MadsCarController.update(self, self.CP, CC, CC_SP, self.frame)
     actuators = CC.actuators
     hud_control = CC.hudControl
 
+    # TODO: needed for angle control cars?
+    # >90 degree steering fault prevention
+    self.angle_limit_counter, apply_steer_req = common_fault_avoidance(abs(CS.out.steeringAngleDeg) >= MAX_FAULT_ANGLE, CC.latActive,
+                                                                       self.angle_limit_counter, MAX_FAULT_ANGLE_FRAMES,
+                                                                       MAX_FAULT_ANGLE_CONSECUTIVE_FRAMES)
+    # Hold torque with induced temporary fault when cutting the actuation bit
+    torque_fault = CC.latActive and not apply_steer_req
+
+    apply_torque = 0
+
     # 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)
+    if not self.CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING:
+      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)
 
-    # >90 degree steering fault prevention
-    self.angle_limit_counter, apply_steer_req = common_fault_avoidance(abs(CS.out.steeringAngleDeg) >= MAX_ANGLE, CC.latActive,
-                                                                       self.angle_limit_counter, MAX_ANGLE_FRAMES,
-                                                                       MAX_ANGLE_CONSECUTIVE_FRAMES)
+    # angle control
+    else:
+      if CS.out.steeringPressed:
+        self.apply_angle_last = actuators.steeringAngleDeg
+      self.apply_angle_last = apply_std_steer_angle_limits(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgoRaw,
+                                                           CS.out.steeringAngleDeg, CC.latActive, self.params.ANGLE_LIMITS)
+
+      # Similar to torque control driver torque override, we ramp up and down the max allowed torque,
+      # but this is a single threshold for simplicity. It also matches the stock system behavior.
+      if abs(CS.out.steeringTorque) > self.params.STEER_THRESHOLD:
+        self.lkas_max_torque = max(self.lkas_max_torque - self.params.ANGLE_TORQUE_DOWN_RATE, self.params.ANGLE_MIN_TORQUE)
+      else:
+        # ramp back up on engage as well
+        self.lkas_max_torque = min(self.lkas_max_torque + self.params.ANGLE_TORQUE_UP_RATE, self.params.ANGLE_MAX_TORQUE)
 
     if not CC.latActive:
       apply_torque = 0
-
-    # Hold torque with induced temporary fault when cutting the actuation bit
-    torque_fault = CC.latActive and not apply_steer_req
+      self.lkas_max_torque = 0
 
     self.apply_torque_last = apply_torque
 
@@ -115,7 +137,8 @@ def update(self, CC, CC_SP, CS, now_nanos):
       lka_steering_long = lka_steering and self.CP.openpilotLongitudinalControl
 
       # steering control
-      can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, self.CAN, CC.enabled, apply_steer_req, apply_torque, self.lkas_icon))
+      can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, self.CAN, CC.enabled, apply_steer_req, apply_torque,
+                                                             self.apply_angle_last, self.lkas_max_torque, self.lkas_icon))
 
       # prevent LFA from activating on LKA steering cars by sending "no lane lines detected" to ADAS ECU
       if self.frame % 5 == 0 and lka_steering:
@@ -177,6 +200,7 @@ def update(self, CC, CC_SP, CS, now_nanos):
     new_actuators = actuators.as_builder()
     new_actuators.torque = apply_torque / self.params.STEER_MAX
     new_actuators.torqueOutputCan = apply_torque
+    new_actuators.steeringAngleDeg = self.apply_angle_last
     new_actuators.accel = accel
 
     self.frame += 1

opendbc/car/hyundai/carstate.py

@@ -243,16 +243,20 @@ def update_canfd(self, can_parsers) -> structs.CarState:
     ret.steeringTorqueEps = cp.vl["MDPS"]["STEERING_OUT_TORQUE"]
     ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > self.params.STEER_THRESHOLD, 5)
     ret.steerFaultTemporary = cp.vl["MDPS"]["LKA_FAULT"] != 0
+    if self.CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING:
+      ret.steerFaultTemporary = ret.steerFaultTemporary or cp.vl["MDPS"]["LKA_ANGLE_FAULT"] != 0
 
     # TODO: alt signal usage may be described by cp.vl['BLINKERS']['USE_ALT_LAMP']
     left_blinker_sig, right_blinker_sig = "LEFT_LAMP", "RIGHT_LAMP"
-    if self.CP.carFingerprint == CAR.HYUNDAI_KONA_EV_2ND_GEN:
+    if self.CP.carFingerprint in (CAR.HYUNDAI_KONA_EV_2ND_GEN, CAR.HYUNDAI_IONIQ_5_PE):
       left_blinker_sig, right_blinker_sig = "LEFT_LAMP_ALT", "RIGHT_LAMP_ALT"
     ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(50, cp.vl["BLINKERS"][left_blinker_sig],
                                                                       cp.vl["BLINKERS"][right_blinker_sig])
     if self.CP.enableBsm:
-      ret.leftBlindspot = cp.vl["BLINDSPOTS_REAR_CORNERS"]["FL_INDICATOR"] != 0
-      ret.rightBlindspot = cp.vl["BLINDSPOTS_REAR_CORNERS"]["FR_INDICATOR"] != 0
+      left_bsm_sig = "FL_INDICATOR_ALT" if self.CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING else "FL_INDICATOR"
+      right_bsm_sig = "FR_INDICATOR_ALT" if self.CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING else "FR_INDICATOR"
+      ret.leftBlindspot = cp.vl["BLINDSPOTS_REAR_CORNERS"][left_bsm_sig] != 0
+      ret.rightBlindspot = cp.vl["BLINDSPOTS_REAR_CORNERS"][right_bsm_sig] != 0
 
     # cruise state
     # CAN FD cars enable on main button press, set available if no TCS faults preventing engagement

opendbc/car/hyundai/fingerprints.py

@@ -1026,6 +1026,16 @@
       b'\xf1\x00NE1 MFC  AT USA LHD 1.00 1.06 99211-GI010 230110',
     ],
   },
+  CAR.HYUNDAI_IONIQ_5_PE: {
+    (Ecu.fwdRadar, 0x7d0, None): [
+      b'\xf1\x00NE__ RDR -----      1.00 1.00 99110-PI000         ',
+      b'\xf1\x00NE__ RDR -----      1.00 1.01 99110-GI500         '
+    ],
+    (Ecu.fwdCamera, 0x7C4, None): [
+      b'\xf1\x00NE  MFC  AT USA LHD 1.00 1.01 99211-PI000 240905',
+      b'\xf1\x00NE  MFC  AT EUR LHD 1.00 1.03 99211-GI500 240809',
+    ],
+  },
   CAR.HYUNDAI_IONIQ_6: {
     (Ecu.fwdRadar, 0x7d0, None): [
       b'\xf1\x00CE__ RDR -----      1.00 1.01 99110-KL000         ',

opendbc/car/hyundai/hyundaicanfd.py

@@ -35,7 +35,7 @@ def CAM(self):
     return self._cam
 
 
-def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_torque, lkas_icon):
+def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_torque, apply_angle, angle_max_torque, lkas_icon):
   common_values = {
     "LKA_MODE": 2,
     "LKA_ICON": lkas_icon,
@@ -50,9 +50,25 @@ def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_torque,
   lkas_values = copy.copy(common_values)
   lkas_values["LKA_AVAILABLE"] = 0
 
+  # Angle control doesn't support using LFA yet
+  if CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING:
+    # TODO: HAS_LANE_SAFETY isn't used by the stock system
+    lkas_values |= {
+      "LKA_MODE": 0,  # TODO: not used by the stock system
+      "TORQUE_REQUEST": 0,  # we don't use torque
+      "STEER_REQ": 0,  # we don't use torque
+      # this goes 0 when LFA lane changes, 3 when LKA_ICON is >=green
+      "LKA_AVAILABLE": 3 if lat_active else 0,
+      "LKAS_ANGLE_CMD": apply_angle,
+      "LKAS_ANGLE_ACTIVE": 2 if lat_active else 1,
+      "LKAS_ANGLE_MAX_TORQUE": angle_max_torque if lat_active else 0,
+    }
+
   lfa_values = copy.copy(common_values)
   lfa_values["NEW_SIGNAL_1"] = 0
 
+  # For cars with an ADAS ECU (commonly HDA2), by sending LKAS actuation messages we're
+  # telling the ADAS ECU to forward our steering and disable stock LFA lane centering.
   ret = []
   if CP.flags & HyundaiFlags.CANFD_LKA_STEERING:
     lkas_msg = "LKAS_ALT" if CP.flags & HyundaiFlags.CANFD_LKA_STEERING_ALT else "LKAS"

opendbc/car/hyundai/interface.py

@@ -42,7 +42,8 @@ def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, experime
       if lka_steering:
         # detect LKA steering
         ret.flags |= HyundaiFlags.CANFD_LKA_STEERING.value
-        if 0x110 in fingerprint[CAN.CAM]:
+        # we only have validated ALT messages for angle steering cars
+        if 0x110 in fingerprint[CAN.CAM] or ret.flags & HyundaiFlags.CANFD_ANGLE_STEERING:
           ret.flags |= HyundaiFlags.CANFD_LKA_STEERING_ALT.value
       else:
         # no LKA steering
@@ -72,6 +73,9 @@ def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, experime
         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_ANGLE_STEERING:
+        ret.steerControlType = structs.CarParams.SteerControlType.angle
+        ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.CANFD_ANGLE_STEERING.value
 
     else:
       # Shared configuration for non CAN-FD cars
@@ -104,7 +108,9 @@ def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, experime
     ret.centerToFront = ret.wheelbase * 0.4
     ret.steerActuatorDelay = 0.1
     ret.steerLimitTimer = 0.4
-    CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
+
+    if not ret.flags & HyundaiFlags.CANFD_ANGLE_STEERING:
+      CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
 
     if ret.flags & HyundaiFlags.ALT_LIMITS:
       ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.ALT_LIMITS.value

opendbc/car/hyundai/values.py

@@ -2,7 +2,7 @@
 from dataclasses import dataclass, field
 from enum import Enum, IntFlag
 
-from opendbc.car import Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, uds
+from opendbc.car import AngleSteeringLimits, Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, uds
 from opendbc.car.common.conversions import Conversions as CV
 from opendbc.car.structs import CarParams
 from opendbc.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Column
@@ -15,6 +15,21 @@ class CarControllerParams:
   ACCEL_MIN = -3.5 # m/s
   ACCEL_MAX = 2.0 # m/s
 
+  ANGLE_LIMITS: AngleSteeringLimits = AngleSteeringLimits(
+    # LKAS angle command is unlimited, but LFA is limited to 176.7 deg (but does not fault if requesting above)
+    180,  # deg
+    # seen changing at 0.2 deg/frame down, 0.1 deg/frame up at 100Hz
+    ([5, 25], [0.3, 0.15]),
+    ([5, 25], [0.36, 0.26]),
+  )
+
+  # Stock LFA system is seen sending 250 max, but for LKAS events it's 175 max.
+  # 250 can at least achieve 4 m/s^2, 80 corresponds to ~2.5 m/s^2
+  ANGLE_MAX_TORQUE = 150  # The maximum amount of torque that will be allowed
+  ANGLE_MIN_TORQUE = 25  # equivalent to ~0.8 m/s^2 of torque (based on ANGLE_MAX_TORQUE) when overriding
+  ANGLE_TORQUE_UP_RATE = 2  # Indicates how fast the torque ramps up after user intervention.
+  ANGLE_TORQUE_DOWN_RATE = 4  # Indicates how fast the torque ramps down during user intervention (handing off).
+
   def __init__(self, CP):
     self.STEER_DELTA_UP = 3
     self.STEER_DELTA_DOWN = 7
@@ -66,6 +81,7 @@ class HyundaiSafetyFlags(IntFlag):
   CANFD_LKA_STEERING_ALT = 128
   FCEV_GAS = 256
   ALT_LIMITS_2 = 512
+  CANFD_ANGLE_STEERING = 1024
   FLAG_HYUNDAI_ESCC = 8192
   FLAG_HYUNDAI_LONG_MAIN_CRUISE_TOGGLEABLE = 16384
 
@@ -128,6 +144,7 @@ class HyundaiFlags(IntFlag):
 
   ALT_LIMITS_2 = 2 ** 26
 
+  CANFD_ANGLE_STEERING = 2 ** 27
 
 class Footnote(Enum):
   CANFD = CarFootnote(
@@ -356,6 +373,11 @@ class CAR(Platforms):
     CarSpecs(mass=1948, wheelbase=2.97, steerRatio=14.26, tireStiffnessFactor=0.65),
     flags=HyundaiFlags.EV,
   )
+  HYUNDAI_IONIQ_5_PE = HyundaiCanFDPlatformConfig(
+    [HyundaiCarDocs("Hyundai Ioniq 5 PE (with HDA II) 2025+", "Highway Driving Assist II", car_parts=CarParts.common([CarHarness.hyundai_q]))],
+    HYUNDAI_IONIQ_5.specs,
+    flags=HyundaiFlags.EV | HyundaiFlags.CANFD_ANGLE_STEERING,
+  )
   HYUNDAI_IONIQ_6 = HyundaiCanFDPlatformConfig(
     [HyundaiCarDocs("Hyundai Ioniq 6 (with HDA II) 2023-24", "Highway Driving Assist II", car_parts=CarParts.common([CarHarness.hyundai_p]))],
     HYUNDAI_IONIQ_5.specs,

opendbc/car/tests/routes.py

@@ -144,6 +144,7 @@ class CarTestRoute(NamedTuple):
   CarTestRoute("628935d7d3e5f4f7|2022-11-30--01-12-46", HYUNDAI.KIA_SORENTO_HEV_4TH_GEN),  # plug-in hybrid
   CarTestRoute("9c917ba0d42ffe78|2020-04-17--12-43-19", HYUNDAI.HYUNDAI_PALISADE),
   CarTestRoute("05a8f0197fdac372|2022-10-19--14-14-09", HYUNDAI.HYUNDAI_IONIQ_5),  # LKA steering
+  CarTestRoute("e1107f9d04dfb1e2/00000455--9b2328ec73", HYUNDAI.HYUNDAI_IONIQ_5_PE),  # LKA steering HDA2 LFA2
   CarTestRoute("eb4eae1476647463|2023-08-26--18-07-04", HYUNDAI.HYUNDAI_IONIQ_6, segment=6),  # LKA steering
   CarTestRoute("3f29334d6134fcd4|2022-03-30--22-00-50", HYUNDAI.HYUNDAI_IONIQ_PHEV_2019),
   CarTestRoute("fa8db5869167f821|2021-06-10--22-50-10", HYUNDAI.HYUNDAI_IONIQ_PHEV),

opendbc/car/torque_data/override.toml

@@ -82,3 +82,6 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"]
 # Manually checked
 "HONDA_CIVIC_2022" = [2.5, 1.2, 0.15]
 "HONDA_HRV_3G" = [2.5, 1.2, 0.2]
+
+# Hyundai/Kia/Genesis angle control
+"HYUNDAI_IONIQ_5_PE" = [nan, 3.0, nan]

opendbc/sunnypilot/car/car_list.json

@@ -1173,6 +1173,14 @@
       "2024"
     ],
     "package": "Highway Driving Assist"
+  },
+   "Hyundai Ioniq 5 PE (with HDA II) 2025+": {
+    "platform": "HYUNDAI_IONIQ_5_PE",
+    "make": "Hyundai",
+    "brand": "hyundai",
+    "model": "Ioniq 5 PE (with HDA II) 2025+",
+    "year": [],
+    "package": "Highway Driving Assist II"
   },
   "Hyundai Ioniq 6 (with HDA II) 2023-24": {
     "platform": "HYUNDAI_IONIQ_6",