From 3711d5e91a440bbad99139df0075f0d27d2e3d64 Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Tue, 21 Nov 2023 13:36:13 -0500
Subject: [PATCH] Hyundai CAN-FD Longitudinal: Parse lead info for camera-based
 SCC platforms

---
 selfdrive/car/hyundai/interface.py       |  4 ++++
 selfdrive/car/hyundai/radar_interface.py | 26 ++++++++++++++++--------
 2 files changed, 22 insertions(+), 8 deletions(-)

diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py
index deb8685ae8..6c65c8ab10 100644
--- a/selfdrive/car/hyundai/interface.py
+++ b/selfdrive/car/hyundai/interface.py
@@ -272,6 +272,10 @@ class CarInterface(CarInterfaceBase):
       ret.longitudinalTuning.kiV = [0.0]
       ret.experimentalLongitudinalAvailable = (candidate in (HYBRID_CAR | EV_CAR) and candidate not in
                                                (CANFD_UNSUPPORTED_LONGITUDINAL_CAR | CANFD_RADAR_SCC_CAR | NON_SCC_CAR))
+      if (candidate in (HYBRID_CAR | EV_CAR) and candidate not in
+        (CANFD_UNSUPPORTED_LONGITUDINAL_CAR | CANFD_RADAR_SCC_CAR | NON_SCC_CAR)):  # TODO: Gate radar disable cars until we can verify the signals match
+        ret.spFlags |= HyundaiFlagsSP.SP_CAMERA_SCC_LEAD.value
+        ret.radarUnavailable = False
     else:
       ret.longitudinalTuning.kpV = [0.5]
       ret.longitudinalTuning.kiV = [0.0]
diff --git a/selfdrive/car/hyundai/radar_interface.py b/selfdrive/car/hyundai/radar_interface.py
index 5d7782c524..dca8b197ce 100644
--- a/selfdrive/car/hyundai/radar_interface.py
+++ b/selfdrive/car/hyundai/radar_interface.py
@@ -3,7 +3,7 @@ import math
 from cereal import car
 from opendbc.can.parser import CANParser
 from openpilot.selfdrive.car.interfaces import RadarInterfaceBase
-from openpilot.selfdrive.car.hyundai.values import DBC, HyundaiFlagsSP
+from openpilot.selfdrive.car.hyundai.values import DBC, HyundaiFlagsSP, CANFD_CAR
 
 RADAR_START_ADDR = 0x500
 RADAR_MSG_COUNT = 32
@@ -12,12 +12,18 @@ RADAR_MSG_COUNT = 32
 
 def get_radar_can_parser(CP):
   if DBC[CP.carFingerprint]['radar'] is None:
-    if CP.spFlags & HyundaiFlagsSP.SP_ENHANCED_SCC:
-      lead_src, bus = "ESCC", 0
-    elif CP.spFlags & HyundaiFlagsSP.SP_CAMERA_SCC_LEAD:
-      lead_src, bus = "SCC11", 2
+    if CP.carFingerprint in CANFD_CAR:
+      if CP.spFlags & HyundaiFlagsSP.SP_ENHANCED_SCC:
+        lead_src, bus = "SCC_CONTROL", 2
+      else:
+        return None
     else:
-      return None
+      if CP.spFlags & HyundaiFlagsSP.SP_ENHANCED_SCC:
+        lead_src, bus = "ESCC", 0
+      elif CP.spFlags & HyundaiFlagsSP.SP_CAMERA_SCC_LEAD:
+        lead_src, bus = "SCC11", 2
+      else:
+        return None
     messages = [(lead_src, 50)]
     return CANParser(DBC[CP.carFingerprint]['pt'], messages, bus)
 
@@ -28,10 +34,12 @@ def get_radar_can_parser(CP):
 class RadarInterface(RadarInterfaceBase):
   def __init__(self, CP):
     super().__init__(CP)
+    self.CP = CP
     self.enhanced_scc = (CP.spFlags & HyundaiFlagsSP.SP_ENHANCED_SCC) and DBC[CP.carFingerprint]['radar'] is None
     self.camera_scc = CP.spFlags & HyundaiFlagsSP.SP_CAMERA_SCC_LEAD
     self.updated_messages = set()
     self.trigger_msg = 0x2AB if self.enhanced_scc else \
+                       0x1A0 if self.camera_scc and CP.carFingerprint in CANFD_CAR else \
                        0x420 if self.camera_scc else \
                        (RADAR_START_ADDR + RADAR_MSG_COUNT - 1)
     self.track_id = 0
@@ -66,9 +74,11 @@ class RadarInterface(RadarInterfaceBase):
     ret.errors = errors
 
     if self.enhanced_scc or self.camera_scc:
-      msg_src = "ESCC" if self.enhanced_scc else "SCC11"
+      msg_src = "ESCC" if self.enhanced_scc else \
+                "SCC_CONTROL" if self.CP.carFingerprint in CANFD_CAR else \
+                "SCC11"
       msg = self.rcp.vl[msg_src]
-      valid = msg['ACC_ObjStatus']
+      valid = msg['ObjValid']
       for ii in range(1):
         if valid:
           if ii not in self.pts: