Support EV6 2025 with LFA2

opendbc/car/hyundai/carcontroller.py

@@ -1,7 +1,7 @@
 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.common.numpy_fast import clip
+from opendbc.car.common.numpy_fast import clip, interp
 from opendbc.car.hyundai import hyundaicanfd, hyundaican
 from opendbc.car.hyundai.carstate import CarState
 from opendbc.car.hyundai.hyundaicanfd import CanBus
@@ -57,6 +57,9 @@ def __init__(self, dbc_names, CP):
     self.apply_steer_last = 0
     self.car_fingerprint = CP.carFingerprint
     self.last_button_frame = 0
+    self.apply_angle_last = 0
+    self.lkas_max_torque = 0
+    self.driver_applied_torque_reducer = 0
 
   def update(self, CC, CS, now_nanos):
     EsccCarController.update(self, CS)
@@ -72,8 +75,56 @@ def update(self, CC, CS, now_nanos):
                                                                        self.angle_limit_counter, MAX_ANGLE_FRAMES,
                                                                        MAX_ANGLE_CONSECUTIVE_FRAMES)
 
+    apply_angle = apply_std_steer_angle_limits(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgoRaw, self.params)
+
+    # Figure out torque value.  On Stock when LKAS is active, this is variable,
+    # but 0 when LKAS is not actively steering, so because we're "tricking" ADAS
+    # into thinking LKAS is always active, we need to make sure we're applying
+    # torque when the driver is not actively steering. The default value chosen
+    # here is based on observations of the stock LKAS system when it's engaged
+    # CS.out.steeringPressed and steeringTorque are based on the
+    # STEERING_COL_TORQUE value
+    MAX_TORQUE = 240
+    # Interpolate a percent to apply to max torque based on vEgo value, which is
+    # the "best estimate of speed".  This means that under 20 (units?) we will
+    # apply less torque, and over 20 we will apply the full calculated torque.
+    ego_weight = interp(CS.out.vEgo, [0, 5, 10, 20], [0.2, 0.3, 0.5, 1.0])
+
+    # Track if and how long the driver has been applying torque and create a
+    # value to reduce the max torque applied. This block will cause the
+    # `driver_applied_torque_reducer` to settle to value between 30 and 150.
+    # While the driver applies torque the value will decrease to 30, and while
+    # the driver is not applying torque the value will increase to 150.
+    if abs(CS.out.steeringTorque) > 200:
+      # If the driver is applying some torque manually, reduce the value down to 30 (the min)
+      self.driver_applied_torque_reducer -= 1
+      if self.driver_applied_torque_reducer < 30:
+        self.driver_applied_torque_reducer = 30
+    else:
+      # While the driver is not applying torque, increase the value up to 150 (the max)
+      self.driver_applied_torque_reducer += 1
+      if self.driver_applied_torque_reducer > 150:
+        self.driver_applied_torque_reducer = 150
+
+    if self.driver_applied_torque_reducer < 150:
+      # If the driver has just started applying torque, the reducer value will
+      # be around 150 so we won't reduce the max torque much. As the driver
+      # continues to apply torque, the reducer value will decrease to 30, so we
+      # will reduce the max torque more to fight them less (at this level we'll
+      # be doing 1/5 of the torque)
+      self.lkas_max_torque = int(round(MAX_TORQUE * ego_weight * (self.driver_applied_torque_reducer / 150)))
+    else:
+      # A torque reducer value of 150 means the driver has not been applying
+      # torque for a while, so we will apply the full max torque value, adjusted
+      # by the ego weight (based on driving speed)
+      self.lkas_max_torque = MAX_TORQUE * ego_weight
+
     if not CC.latActive:
+      apply_angle = CS.out.steeringAngleDeg
       apply_steer = 0
+      self.lkas_max_torque = 0
+
+    self.apply_angle_last = apply_angle
 
     # Hold torque with induced temporary fault when cutting the actuation bit
     torque_fault = CC.latActive and not apply_steer_req
@@ -112,7 +163,9 @@ def update(self, CC, CS, now_nanos):
       hda2_long = hda2 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_steer))
+      can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, self.CAN, CC.enabled,
+                                                             apply_steer_req, CS.out.steeringPressed,
+                                                             apply_steer, apply_angle, self.lkas_max_torque))
 
       # prevent LFA from activating on HDA2 by sending "no lane lines detected" to ADAS ECU
       if self.frame % 5 == 0 and hda2:
@@ -170,6 +223,7 @@ def update(self, CC, CS, now_nanos):
     new_actuators = actuators.as_builder()
     new_actuators.steer = apply_steer / self.params.STEER_MAX
     new_actuators.steerOutputCan = apply_steer
+    new_actuators.steeringAngleDeg = apply_angle
     new_actuators.accel = accel
 
     self.frame += 1

opendbc/car/hyundai/fingerprints.py

@@ -976,6 +976,14 @@
       b'\xf1\x00CV1 MFC  AT USA LHD 1.00 1.06 99210-CV000 220328',
     ],
   },
+  CAR.KIA_EV6_2025: {
+    (Ecu.fwdRadar, 0x7d0, None): [
+      b'\xf1\x00CV__ RDR -----      1.00 1.01 99110-CV500         ',
+    ],
+    (Ecu.fwdCamera, 0x7c4, None): [
+      b'\xf1\x00CV  MFC  AT KOR LHD 1.00 1.01 99210-CV500 240405',
+    ],
+  },
   CAR.HYUNDAI_IONIQ_5: {
     (Ecu.fwdRadar, 0x7d0, None): [
       b'\xf1\x00NE1_ RDR -----      1.00 1.00 99110-GI000         ',

opendbc/car/hyundai/hyundaicanfd.py

@@ -34,20 +34,32 @@ def CAM(self):
     return self._cam
 
 
-def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_steer):
+def create_steering_messages(packer, CP, CAN, enabled, lat_active, steering_pressed, apply_steer, apply_angle, max_torque):
 
   ret = []
 
   values = {
-    "LKA_MODE": 2,
+    "LKA_MODE": 0,
     "LKA_ICON": 2 if enabled else 1,
-    "TORQUE_REQUEST": apply_steer,
+    "TORQUE_REQUEST": 0,
     "LKA_ASSIST": 0,
-    "STEER_REQ": 1 if lat_active else 0,
+    "STEER_REQ": 0,
     "STEER_MODE": 0,
     "HAS_LANE_SAFETY": 0,  # hide LKAS settings
-    "NEW_SIGNAL_1": 0,
+    "LKA_ACTIVE": 3 if lat_active else 0,  # this changes sometimes, 3 seems to indicate engaged
     "NEW_SIGNAL_2": 0,
+    "LKAS_ANGLE_CMD": -apply_angle,
+    "LKAS_ANGLE_ACTIVE": 2 if lat_active else 1,
+    # a torque scale value? ramps up when steering, highest seen is 234
+    # "UNKNOWN": 50 if lat_active and not steering_pressed else 0,
+    "UNKNOWN": max_torque if lat_active else 0,
+    # TODO: Commenting these out to see if the fix a regression
+    #"NEW_SIGNAL_1": 10,
+    #"NEW_SIGNAL_3": 9,
+    #"NEW_SIGNAL_4": 1,
+    #"NEW_SIGNAL_5": 1,
+    #"NEW_SIGNAL_6": 1,
+    #"NEW_SIGNAL_7": 1,
   }
 
   if CP.flags & HyundaiFlags.CANFD_HDA2:

opendbc/car/hyundai/interface.py

@@ -66,6 +66,8 @@ def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, experime
           ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_CANFD_HDA2_ALT_STEERING
       if ret.flags & HyundaiFlags.CANFD_ALT_BUTTONS:
         ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_CANFD_ALT_BUTTONS
+      if ret.flags & HyundaiFlags.CANFD_LFA2:
+        ret.steerControlType = structs.CarParams.SteerControlType.angle
       if ret.flags & HyundaiFlags.CANFD_CAMERA_SCC:
         ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_CAMERA_SCC
 

opendbc/car/hyundai/values.py

@@ -3,7 +3,7 @@
 from enum import Enum, IntFlag
 
 from panda import uds
-from opendbc.car import Bus, CarSpecs, DbcDict, PlatformConfig, Platforms
+from opendbc.car import Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, AngleRateLimit
 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,10 @@
 class CarControllerParams:
   ACCEL_MIN = -3.5 # m/s
   ACCEL_MAX = 2.0 # m/s
+
+  # LFA2 / seen changing at 0.2 deg/frame down, 0.1 deg/frame up at 100Hz
+  ANGLE_RATE_LIMIT_UP = AngleRateLimit(speed_bp=[0., 5., 15.], angle_v=[5., .8, .15])
+  ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_bp=[0., 5., 15.], angle_v=[5., 3.5, 0.4])
 
   def __init__(self, CP):
     self.STEER_DELTA_UP = 3
@@ -98,6 +102,8 @@ class HyundaiFlags(IntFlag):
 
   MIN_STEER_32_MPH = 2 ** 23
 
+  CANFD_LFA2 = 2 ** 24
+
 
 class Footnote(Enum):
   CANFD = CarFootnote(
@@ -488,6 +494,13 @@ class CAR(Platforms):
     CarSpecs(mass=2055, wheelbase=2.9, steerRatio=16, tireStiffnessFactor=0.65),
     flags=HyundaiFlags.EV,
   )
+  KIA_EV6_2025 = HyundaiCanFDPlatformConfig(
+    [
+      HyundaiCarDocs("Kia EV6 (with HDA II) 2025", "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 | HyundaiFlags.CANFD_LFA2,
+  )
   KIA_CARNIVAL_4TH_GEN = HyundaiCanFDPlatformConfig(
     [
       HyundaiCarDocs("Kia Carnival 2022-24", car_parts=CarParts.common([CarHarness.hyundai_a])),

opendbc/car/tests/routes.py

@@ -162,6 +162,7 @@ class CarTestRoute(NamedTuple):
   CarTestRoute("d545129f3ca90f28|2022-10-19--09-22-54", HYUNDAI.KIA_EV6),  # HDA2
   CarTestRoute("68d6a96e703c00c9|2022-09-10--16-09-39", HYUNDAI.KIA_EV6),  # HDA1
   CarTestRoute("9b25e8c1484a1b67|2023-04-13--10-41-45", HYUNDAI.KIA_EV6),
+  CarTestRoute("84711a8268bf3b9a|2024-11-27--00-04-53", HYUNDAI.KIA_EV6_2025),  # 2025 HDA2
   CarTestRoute("007d5e4ad9f86d13|2021-09-30--15-09-23", HYUNDAI.KIA_K5_2021),
   CarTestRoute("c58dfc9fc16590e0|2023-01-14--13-51-48", HYUNDAI.KIA_K5_HEV_2020),
   CarTestRoute("78ad5150de133637|2023-09-13--16-15-57", HYUNDAI.KIA_K8_HEV_1ST_GEN),

opendbc/car/torque_data/params.toml

@@ -40,6 +40,7 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"]
 "JEEP_GRAND_CHEROKEE_2019" = [2.30972, 1.289689569171081, 0.117048]
 "JEEP_GRAND_CHEROKEE" = [2.27116, 1.4057367824262523, 0.11725947414922003]
 "KIA_EV6" = [3.2, 2.093457, 0.005]
+"KIA_EV6_2025" = [3.2, 2.093457, 0.005]
 "KIA_K5_2021" = [2.405339728085138, 1.460032270828705, 0.11650989850813716]
 "KIA_NIRO_EV" = [2.9215954981365337, 2.1500583840260044, 0.09236802474810267]
 "KIA_SORENTO" = [2.464854685101844, 1.5335274218367956, 0.12056170567599558]

opendbc/dbc/hyundai_canfd.dbc

@@ -68,6 +68,9 @@ BO_ 80 LKAS: 16 XXX
  SG_ HAS_LANE_SAFETY : 80|1@0+ (1,0) [0|1] "" XXX
  SG_ NEW_SIGNAL_3 : 111|8@0+ (1,0) [0|255] "" XXX
  SG_ FCA_SYSWARN : 40|1@0+ (1,0) [0|1] "" XXX
+ SG_ LKAS_ANGLE_ACTIVE : 77|2@0+ (1,0) [0|3] "" XXX
+ SG_ LKAS_ANGLE_CMD : 82|14@1- (-0.1,0) [0|511] "" XXX
+ SG_ UNKNOWN : 96|8@1+ (1,0) [0|255] "" XXX
 
 BO_ 81 ADRV_0x51: 32 ADRV
  SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX
@@ -128,7 +131,7 @@ BO_ 272 LKAS_ALT: 32 XXX
  SG_ STEER_REQ : 52|1@1+ (1,0) [0|1] "" XXX
  SG_ TORQUE_REQUEST : 41|11@1+ (1,-1024) [0|4095] "" XXX
  SG_ LKA_ICON : 38|2@1+ (1,0) [0|255] "" XXX
- SG_ NEW_SIGNAL_1 : 27|2@1+ (1,0) [0|255] "" XXX
+ SG_ LKA_ACTIVE : 27|2@1+ (1,0) [0|255] "" XXX
  SG_ LFA_BUTTON : 56|1@1+ (1,0) [0|255] "" XXX
  SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX
  SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX
@@ -137,9 +140,17 @@ BO_ 272 LKAS_ALT: 32 XXX
  SG_ LKA_ASSIST : 62|1@1+ (1,0) [0|1] "" XXX
  SG_ LKA_MODE : 24|3@1+ (1,0) [0|7] "" XXX
  SG_ NEW_SIGNAL_2 : 70|2@0+ (1,0) [0|3] "" XXX
+ SG_ LKAS_ANGLE_ACTIVE : 77|2@0+ (1,0) [0|3] "" XXX
  SG_ HAS_LANE_SAFETY : 80|1@0+ (1,0) [0|1] "" XXX
- SG_ NEW_SIGNAL_3 : 111|8@0+ (1,0) [0|255] "" XXX
+ SG_ LKAS_ANGLE_CMD : 82|14@1- (-0.1,0) [0|511] "" XXX
+ SG_ UNKNOWN : 96|8@1+ (1,0) [0|255] "" XXX
+ SG_ NEW_SIGNAL_3 : 108|5@0+ (1,0) [0|255] "" XXX
  SG_ FCA_SYSWARN : 40|1@0+ (1,0) [0|1] "" XXX
+ SG_ NEW_SIGNAL_1 : 75|4@0+ (1,0) [0|15] "" XXX
+ SG_ NEW_SIGNAL_6 : 225|1@0+ (1,0) [0|1] "" XXX
+ SG_ NEW_SIGNAL_5 : 228|1@0+ (1,0) [0|1] "" XXX
+ SG_ NEW_SIGNAL_4 : 231|1@0+ (1,0) [0|1] "" XXX
+ SG_ NEW_SIGNAL_7 : 232|1@0+ (1,0) [0|1] "" XXX
 
 BO_ 293 STEERING_SENSORS: 16 XXX
  SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX
@@ -666,6 +677,7 @@ CM_ 1043 "Lamp signals do not seem universal on cars that use LKAS_ALT, but stal
 CM_ SG_ 80 HAS_LANE_SAFETY "If 0, hides LKAS 'Lane Safety' menu from vehicle settings";
 CM_ SG_ 96 BRAKE_PRESSURE "User applied brake pedal pressure. Ramps from computer applied pressure on falling edge of cruise. Cruise cancels if !=0";
 CM_ SG_ 101 BRAKE_POSITION "User applied brake pedal position, max is ~700. Signed on some vehicles";
+CM_ SG_ 272 LKAS_ANGLE_CMD "tracks MDPS->STEERING_ANGLE when not engaged, not STEERING_SENSORS->STEERING_ANGLE";
 CM_ SG_ 373 PROBABLY_EQUIP "aeb equip?";
 CM_ SG_ 373 ACCEnable "Likely a copy of CAN's TCS13->ACCEnable";
 CM_ SG_ 373 DriverBraking "Likely derived from BRAKE->BRAKE_POSITION";
@@ -689,6 +701,7 @@ VAL_ 80 LKA_ICON 0 "hidden" 1 "grey" 2 "green" 3 "flashing green" ;
 VAL_ 80 LKA_MODE 1 "warning only" 2 "assist" 6 "off" ;
 VAL_ 96 TRACTION_AND_STABILITY_CONTROL 0 "On" 5 "Limited" 1 "Off";
 VAL_ 234 LKA_FAULT 0 "ok" 1 "lka fault" ;
+VAL_ 272 LKAS_ANGLE_ACTIVE 1 "not active" 2 "active";
 VAL_ 272 LKA_ICON 0 "hidden" 1 "grey" 2 "green" 3 "flashing green" ;
 VAL_ 272 LKA_MODE 1 "warning only" 2 "assist" 6 "off" ;
 VAL_ 298 LKA_ICON 0 "hidden" 1 "grey" 2 "green" 3 "flashing green" ;