new

opendbc/car/hyundai/carcontroller.py

@@ -118,8 +118,8 @@ def update(self, CC, CS, now_nanos):
       if self.frame % 5 == 0 and (not hda2 or hda2_long) and self.car_fingerprint not in (CAR.HYUNDAI_SONATA_2024,):
         can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CAN, CC.enabled))
       if self.frame % 5 == 0 and (not hda2 or hda2_long) and self.car_fingerprint in (CAR.HYUNDAI_SONATA_2024,):
-        can_sends.append(hyundaicanfd.create_msg_161(self.packer, self.CAN, CC.enabled, CS.msg_161))
-        can_sends.append(hyundaicanfd.create_msg_162(self.packer, self.CAN, CC.enabled, CS.msg_162))
+        can_sends.append(hyundaicanfd.create_msg_161(self.packer, self.CAN, CC.enabled, CS.msg_161, self.CP, hud_control, CS, CC, self.frame))
+        can_sends.append(hyundaicanfd.create_msg_162(self.packer, self.CAN, CC.enabled, CS.msg_162, self.CP, hud_control))
 
       # blinkers
       if hda2 and self.CP.flags & HyundaiFlags.ENABLE_BLINKERS:

opendbc/car/hyundai/hyundaicanfd.py

@@ -1,4 +1,5 @@
 from opendbc.car import CanBusBase
+from opendbc.car.common.conversions import Conversions as CV
 from opendbc.car.common.numpy_fast import clip
 from opendbc.car.hyundai.values import HyundaiFlags
 
@@ -119,29 +120,97 @@ def create_lfahda_cluster(packer, CAN, enabled):
   }
   return packer.make_can_msg("LFAHDA_CLUSTER", CAN.ECAN, values)
 
-def create_msg_161(packer, CAN, enabled, msg_161):
+def create_msg_161(packer, CAN, enabled, msg_161, car_params, hud_control, car_state, car_control, frame):
   values = msg_161.copy()
+
+  # HIDE ALERTS
+  if values.get("ALERTS_5") == 5:  # USE SWITCH OR PEDAL TO ACCELERATE
+    values["ALERTS_5"] = 0
+  if values.get("ALERTS_2") == 5:  # CONSIDER TAKING A BREAK
+    values.update({"ALERTS_2": 0, "SOUNDS_2": 0, "DAW_ICON": 0})
+
+  # LANELINES
+  curvature = {
+    i: (31 if i == -1 else 13 - abs(i + 15)) if i < 0 else 15 + i
+    for i in range(-15, 16)
+  }
   values.update({
+    "LANELINE_CURVATURE": curvature.get(max(-15, min(int(car_state.out.steeringAngleDeg / 3), 15)), 14) if enabled else 15,
     "LFA_ICON": 2 if enabled else 0,
     "LKA_ICON": 4 if enabled else 0,
     "LANELINE_LEFT": 2 if enabled else 0,
     "LANELINE_RIGHT": 2 if enabled else 0,
     "CENTERLINE": 1 if enabled else 0,
-    "DAW_ICON": 0,
   })
-  if values.get("ALERTS_5") == 5:  # use_switch_or_pedal_to_accelerate
-    values["ALERTS_5"] = 0
-  if values.get("ALERTS_2") == 5:  # coffee
-    values.update({"ALERTS_2": 0, "SOUNDS_2": 0})
+
+  # LCA
+  if enabled:
+    speed_below_threshold = car_state.out.vEgo < 8.94
+    values.update({
+      "LCA_LEFT_ICON": 0 if car_state.out.leftBlindspot or speed_below_threshold else 2 if car_control.leftBlinker else 1,
+      "LCA_RIGHT_ICON": 0 if car_state.out.rightBlindspot or speed_below_threshold else 2 if car_control.rightBlinker else 1,
+      "LCA_LEFT_ARROW": 2 if car_control.leftBlinker else 0,
+      "LCA_RIGHT_ARROW": 2 if car_control.rightBlinker else 0,
+    })
+
+  # LANE DEPARTURE
+  if hud_control.leftLaneDepart:
+    values["LANELINE_LEFT"] = 4 if (frame // 50) % 2 == 0 else 1
+  if hud_control.rightLaneDepart:
+    values["LANELINE_RIGHT"] = 4 if (frame // 50) % 2 == 0 else 1
+
+  if car_params.openpilotLongitudinalControl:
+    # HIDE ALERTS
+    if values.get("ALERTS_5") == 4:  # SMART CRUISE CONTROL CONDITIONS NOT MET
+      values["ALERTS_5"] = 0
+
+    # SETSPEED
+    values["SETSPEED"] = 3 if enabled else 1
+    values["SETSPEED_HUD"] = 2 if enabled else 1
+    values["SETSPEED_SPEED"] = 25 if (s := round(car_state.out.vCruiseCluster * CV.KPH_TO_MPH)) > 100 else s
+
+    # DISTANCE
+    if 1 <= hud_control.leadDistanceBars <= 3:
+      values["DISTANCE"] = hud_control.leadDistanceBars
+      values["DISTANCE_SPACING"] = 1 if enabled else 0
+      values["DISTANCE_LEAD"] = 2 if enabled and hud_control.leadVisible else 1 if enabled else 0
+      values["DISTANCE_CAR"] = 2 if enabled else 1
+      values["ALERTS_3"] = hud_control.leadDistanceBars + 6
+    else:
+      values["DISTANCE"] = 0
+      values["DISTANCE_SPACING"] = 0
+      values["DISTANCE_LEAD"] = 0
+      values["DISTANCE_CAR"] = 0
+
+    # BACKGROUND
+    values["BACKGROUND"] = 1 if enabled else 7
+
   return packer.make_can_msg("MSG_161", CAN.ECAN, values)
 
-def create_msg_162(packer, CAN, enabled, msg_162):
+def create_msg_162(packer, CAN, enabled, msg_162, car_params, hud_control):
   values = msg_162.copy()
+
+  # HIDE FAULTS
   values.update({
     "FAULT_LSS": 0,
     "FAULT_HDA": 0,
     "FAULT_DAS": 0,
   })
+
+  # LANE DEPARTURE
+  if hud_control.leftLaneDepart or hud_control.rightLaneDepart:
+    values["VIBRATE"] = 1
+
+  if car_params.openpilotLongitudinalControl:
+    # *** TODO *** LEAD_DISTANCE/LEAD_LATERAL
+    # LEAD
+    if hud_control.leadVisible:
+      values["LEAD"] = 2 if enabled else 1
+      values["LEAD_DISTANCE"] = 100
+    else:
+      values["LEAD"] = 0
+      values["LEAD_DISTANCE"] = 0
+
   return packer.make_can_msg("MSG_162", CAN.ECAN, values)
 
 def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_override, set_speed, hud_control):