From 0b8262175f51fdadb75eaea5aab5cfccb328dd20 Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Fri, 17 May 2024 04:48:46 +0000
Subject: [PATCH] Hyundai CAN-FD Longitudinal: Parse lead info for camera-based
 SCC platforms

---
 CHANGELOGS.md                            |  3 +++
 selfdrive/car/hyundai/interface.py       |  3 +++
 selfdrive/car/hyundai/radar_interface.py | 28 +++++++++++++++++-------
 3 files changed, 26 insertions(+), 8 deletions(-)

diff --git a/CHANGELOGS.md b/CHANGELOGS.md
index ca7bada670..8631ce8d75 100644
--- a/CHANGELOGS.md
+++ b/CHANGELOGS.md
@@ -24,6 +24,9 @@ sunnypilot - 0.9.7.0 (2024-05-xx)
   * comma Prime support
   * Personal Mapbox/Amap/Google Maps token support
   * Instructions on how to set up your iOS Siri Shortcuts: https://routinehub.co/shortcut/17677/
+* UPDATED: Hyundai CAN-FD Camera-based SCC
+  * NEW❗: Parse lead info for camera-based SCC platforms with longitudinal support
+    * Improve lead tracking when using openpilot longitudinal
 * RE-ENABLED: Map-based Turn Speed Control (M-TSC) for supported platforms
   * openpilot Longitudianl Control available cars
   * Custom Stock Longitudinal Control available cars
diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py
index c5a83fcbed..d2a719ccd9 100644
--- a/selfdrive/car/hyundai/interface.py
+++ b/selfdrive/car/hyundai/interface.py
@@ -97,6 +97,9 @@ class CarInterface(CarInterfaceBase):
       ret.longitudinalTuning.kpV = [0.1]
       ret.longitudinalTuning.kiV = [0.0]
       ret.experimentalLongitudinalAvailable = candidate not in (CANFD_UNSUPPORTED_LONGITUDINAL_CAR | CANFD_RADAR_SCC_CAR | NON_SCC_CAR)
+      if ret.flags & HyundaiFlags.CANFD_CAMERA_SCC and not hda2:
+        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 76f2c370d6..f00b993563 100644
--- a/selfdrive/car/hyundai/radar_interface.py
+++ b/selfdrive/car/hyundai/radar_interface.py
@@ -3,7 +3,8 @@ 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.hyundaicanfd import CanBus
+from openpilot.selfdrive.car.hyundai.values import DBC, HyundaiFlagsSP, CANFD_CAR
 
 RADAR_START_ADDR = 0x500
 RADAR_MSG_COUNT = 32
@@ -12,12 +13,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_CAMERA_SCC_LEAD:
+        lead_src, bus = "SCC_CONTROL", CanBus(CP).CAM
+      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 +35,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
@@ -68,9 +77,12 @@ 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['ACC_ObjDist'] < 204.6 if self.CP.carFingerprint in CANFD_CAR else \
+              msg['ACC_ObjStatus']
       for ii in range(1):
         if valid:
           if ii not in self.pts: