Hyundai CAN Longitudinal: Enable radar tracks for certain Sante Fe platforms

CHANGELOGS.md

@@ -1,5 +1,10 @@
 sunnypilot - 0.9.5.2 (2023-xx-xx)
 ========================
+* NEW❗: Hyundai CAN longitudinal:
+  * NEW❗: Enable radar tracks for certain Santa Fe platforms
+    * Internal Combustion Engine (ICE) 2021-23
+    * Hybrid 2022-23
+    * Plug-in Hybrid 2022-23
 * NEW❗: Lane Change: When manually braking with steering engaged, turning on the turn signal will default to Nudge mode
 
 sunnypilot - 0.9.5.1 (2023-11-17)

selfdrive/car/hyundai/enable_radar_tracks.py

@@ -0,0 +1,47 @@
+from selfdrive.car.isotp_parallel_query import IsoTpParallelQuery
+from system.swaglog import cloudlog
+
+EXT_DIAG_REQUEST = b'\x10\x07'
+EXT_DIAG_RESPONSE = b'\x50\x07'
+
+WRITE_DATA_REQUEST = b'\x2e'
+WRITE_DATA_RESPONSE = b'\x68'
+
+RADAR_TRACKS_CONFIG = b"\x00\x00\x00\x01\x00\x01"
+
+
+def enable_radar_tracks(logcan, sendcan, bus=0, addr=0x7d0, config_data_id=b'\x01\x42', timeout=0.1, retry=10, debug=False):
+  cloudlog.warning("radar_tracks: enabling ...")
+
+  for i in range(retry):
+    try:
+      query = IsoTpParallelQuery(sendcan, logcan, bus, [addr], [EXT_DIAG_REQUEST], [EXT_DIAG_RESPONSE], debug=debug)
+
+      for _, _ in query.get_data(timeout).items():
+        cloudlog.warning("radar_tracks: reconfigure radar to output radar points ...")
+
+        query = IsoTpParallelQuery(sendcan, logcan, bus, [addr],
+                                   [WRITE_DATA_REQUEST + config_data_id + RADAR_TRACKS_CONFIG],
+                                   [WRITE_DATA_RESPONSE], debug=debug)
+        query.get_data(0)
+
+        cloudlog.warning("radar_tracks: successfully enabled")
+        return True
+
+    except Exception as e:
+      cloudlog.exception(f"radar_tracks exception: {e}")
+
+    cloudlog.error(f"radar_tracks retry ({i + 1}) ...")
+  cloudlog.error(f"radar_tracks: failed")
+  return False
+
+
+if __name__ == "__main__":
+  import time
+  import cereal.messaging as messaging
+  sendcan = messaging.pub_sock('sendcan')
+  logcan = messaging.sub_sock('can')
+  time.sleep(1)
+
+  enabled = enable_radar_tracks(logcan, sendcan, bus=0, addr=0x7d0, config_data_id=b'\x01\x42', timeout=0.1, debug=False)
+  print(f"enabled: {enabled}")

selfdrive/car/hyundai/interface.py

@@ -2,6 +2,7 @@ from cereal import car
 from panda import Panda
 from openpilot.common.conversions import Conversions as CV
 from openpilot.common.params import Params
+from openpilot.selfdrive.car.hyundai.enable_radar_tracks import enable_radar_tracks
 from openpilot.selfdrive.car.hyundai.hyundaicanfd import CanBus
 from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, HyundaiFlagsSP, CAR, DBC, CANFD_CAR, CAMERA_SCC_CAR, CANFD_RADAR_SCC_CAR, \
                                          CANFD_UNSUPPORTED_LONGITUDINAL_CAR, NON_SCC_CAR, EV_CAR, HYBRID_CAR, LEGACY_SAFETY_MODE_CAR, \
@@ -84,6 +85,10 @@ class CarInterface(CarInterfaceBase):
       # Values from optimizer
       ret.steerRatio = 16.55  # 13.8 is spec end-to-end
       ret.tireStiffnessFactor = 0.82
+      if candidate in (CAR.SANTA_FE_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022):
+        if any(fw.ecu == "fwdRadar" and fw.fwVersion is not None for fw in car_fw):
+          ret.radarUnavailable = False
+          ret.spFlags |= HyundaiFlagsSP.SP_RADAR_TRACKS.value
     elif candidate in (CAR.SONATA, CAR.SONATA_HYBRID):
       ret.mass = 1513.
       ret.wheelbase = 2.84
@@ -371,6 +376,12 @@ class CarInterface(CarInterfaceBase):
     if CP.flags & HyundaiFlags.ENABLE_BLINKERS:
       disable_ecu(logcan, sendcan, bus=CanBus(CP).ECAN, addr=0x7B1, com_cont_req=b'\x28\x83\x01')
 
+    # for enabling radar tracks on startup
+    # some CAN platforms are able to enable radar tracks config at the radar ECU,
+    # but the config is reset after ignition cycle
+    if CP.openpilotLongitudinalControl and (CP.spFlags & HyundaiFlagsSP.SP_RADAR_TRACKS):
+      enable_radar_tracks(logcan, sendcan, bus=0, addr=0x7d0, config_data_id=b'\x01\x42')
+
   def _update(self, c):
     ret = self.CS.update(self.cp, self.cp_cam)
     self.CS = self.sp_update_params(self.CS)

selfdrive/car/hyundai/radar_interface.py

@@ -39,6 +39,8 @@ class RadarInterface(RadarInterfaceBase):
     self.radar_off_can = CP.radarUnavailable
     self.rcp = get_radar_can_parser(CP)
 
+    self.sp_radar_tracks = CP.spFlags & HyundaiFlagsSP.SP_RADAR_TRACKS
+
   def update(self, can_strings):
     if self.radar_off_can or (self.rcp is None):
       return super().update(None)
@@ -46,7 +48,7 @@ class RadarInterface(RadarInterfaceBase):
     vls = self.rcp.update_strings(can_strings)
     self.updated_messages.update(vls)
 
-    if self.trigger_msg not in self.updated_messages:
+    if self.trigger_msg not in self.updated_messages and not self.sp_radar_tracks:
       return None
 
     rr = self._update(self.updated_messages)

selfdrive/car/hyundai/values.py

@@ -73,6 +73,7 @@ class HyundaiFlagsSP(IntFlag):
   SP_NAV_MSG = 4
   SP_CAMERA_SCC_LEAD = 8
   SP_LKAS12 = 16
+  SP_RADAR_TRACKS = 32
 
 
 class CAR(StrEnum):
@@ -2310,9 +2311,9 @@ DBC = {
   CAR.KONA_EV_2022: dbc_dict('hyundai_kia_generic', None),
   CAR.KONA_HEV: dbc_dict('hyundai_kia_generic', None),
   CAR.SANTA_FE: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'),
-  CAR.SANTA_FE_2022: dbc_dict('hyundai_kia_generic', None),
-  CAR.SANTA_FE_HEV_2022: dbc_dict('hyundai_kia_generic', None),
-  CAR.SANTA_FE_PHEV_2022: dbc_dict('hyundai_kia_generic', None),
+  CAR.SANTA_FE_2022: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'),
+  CAR.SANTA_FE_HEV_2022: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'),
+  CAR.SANTA_FE_PHEV_2022: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'),
   CAR.SONATA: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'),
   CAR.SONATA_LF: dbc_dict('hyundai_kia_generic', None), # Has 0x5XX messages, but different format
   CAR.TUCSON: dbc_dict('hyundai_kia_generic', None),