Hyundai Longitudinal: Enable radar tracks based on availability (#406)

* Hyundai Longitudinal: Enable radar tracks based on availability

* don't loop

* just 2

* missed

* missed

* fix

* do this

* every time

* quicker

* less

* nope

* more

* cleanup

cereal/car.capnp

@@ -137,6 +137,7 @@ struct CarEvent @0x9b1657f34caf3ad3 {
     speedLimitPreActive @139;
     speedLimitConfirmed @140;
     torqueNNLoad @141;
+    hyundaiRadarTracksAvailable @142;
 
     radarCanErrorDEPRECATED @15;
     communityFeatureDisallowedDEPRECATED @62;

common/params.cc

@@ -265,6 +265,9 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"HkgSmoothStop", PERSISTENT | BACKUP},
     {"HotspotOnBoot", PERSISTENT},
     {"HotspotOnBootConfirmed", PERSISTENT},
+    {"HyundaiRadarTracksAvailable", PERSISTENT},
+    {"HyundaiRadarTracksAvailableCache", PERSISTENT},
+    {"HyundaiRadarTracksAvailablePersistent", PERSISTENT},
     {"LastCarModel", PERSISTENT | BACKUP},
     {"LastSpeedLimitSignTap", PERSISTENT},
     {"LastSunnylinkPingTime", CLEAR_ON_MANAGER_START},

selfdrive/car/hyundai/interface.py

@@ -1,6 +1,8 @@
+import cereal.messaging as messaging
 from cereal import car
 from panda import Panda
 from openpilot.common.params import Params
+from openpilot.selfdrive.car.sunnypilot.fingerprinting import can_fingerprint, get_one_can
 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, \
@@ -126,7 +128,8 @@ class CarInterface(CarInterfaceBase):
 
       if ret.flags & HyundaiFlags.MANDO_RADAR and ret.radarUnavailable:
         ret.spFlags |= HyundaiFlagsSP.SP_RADAR_TRACKS.value
-        ret.radarUnavailable = False
+        if Params().get_bool("HyundaiRadarTracksAvailable"):
+          ret.radarUnavailable = False
 
     # *** panda safety config ***
     if candidate in CANFD_CAR:
@@ -202,6 +205,17 @@ class CarInterface(CarInterfaceBase):
     if CP.spFlags & HyundaiFlagsSP.SP_RADAR_TRACKS:
       enable_radar_tracks(logcan, sendcan, bus=0, addr=0x7d0, config_data_id=b'\x01\x42')
 
+      params = Params()
+      rt_avail = params.get_bool("HyundaiRadarTracksAvailable")
+      rt_avail_persist = params.get_bool("HyundaiRadarTracksAvailablePersistent")
+      params.put_bool_nonblocking("HyundaiRadarTracksAvailableCache", rt_avail)
+      if not rt_avail_persist:
+        messaging.drain_sock_raw(logcan)
+        fingerprint = can_fingerprint(lambda: get_one_can(logcan))
+        radar_unavailable = RADAR_START_ADDR not in fingerprint[1] or DBC[CP.carFingerprint]["radar"] is None
+        params.put_bool_nonblocking("HyundaiRadarTracksAvailable", not radar_unavailable)
+        params.put_bool_nonblocking("HyundaiRadarTracksAvailablePersistent", True)
+
   def _update(self, c):
     if not self.CS.control_initialized and not self.CP.pcmCruise:
       can_cruise_main_default = self.CP.spFlags & HyundaiFlagsSP.SP_CAN_LFA_BTN and not self.CP.flags & HyundaiFlags.CANFD and \
@@ -272,6 +286,9 @@ class CarInterface(CarInterfaceBase):
     if self.low_speed_alert and self.CS.madsEnabled:
       events.add(car.CarEvent.EventName.belowSteerSpeed)
 
+    if self.CS.params_list.hyundai_radar_tracks_available and not self.CS.params_list.hyundai_radar_tracks_available_cache:
+      events.add(car.CarEvent.EventName.hyundaiRadarTracksAvailable)
+
     ret.customStockLong = self.CS.update_custom_stock_long(self.CC.cruise_button, self.CC.final_speed_kph,
                                                            self.CC.target_speed, self.CC.v_set_dis,
                                                            self.CC.speed_diff, self.CC.button_type)

selfdrive/car/param_manager.py

@@ -9,6 +9,8 @@ class ParamManager:
       "acc_mads_combo": False,
       "below_speed_pause": False,
       "experimental_mode": False,
+      "hyundai_radar_tracks_available": False,
+      "hyundai_radar_tracks_available_cache": False,
       "is_metric": False,
       "last_speed_limit_sign_tap": False,
       "hyundai_cruise_main_default": False,
@@ -36,6 +38,8 @@ class ParamManager:
       "acc_mads_combo": params.get_bool("AccMadsCombo"),
       "below_speed_pause": params.get_bool("BelowSpeedPause"),
       "experimental_mode": params.get_bool("ExperimentalMode"),
+      "hyundai_radar_tracks_available": params.get_bool("HyundaiRadarTracksAvailable"),
+      "hyundai_radar_tracks_available_cache": params.get_bool("HyundaiRadarTracksAvailableCache"),
       "is_metric": params.get_bool("IsMetric"),
       "last_speed_limit_sign_tap": params.get_bool("LastSpeedLimitSignTap"),
       "hyundai_cruise_main_default": params.get_bool("HyundaiCruiseMainDefault"),

selfdrive/car/sunnypilot/fingerprinting.py

@@ -0,0 +1,37 @@
+from collections.abc import Callable
+
+import cereal.messaging as messaging
+from openpilot.selfdrive.car import gen_empty_fingerprint
+
+FRAME_FINGERPRINT = 25  # 0.25s
+
+
+def get_one_can(logcan):
+  while True:
+    can = messaging.recv_one_retry(logcan)
+    if len(can.can) > 0:
+      return can
+
+
+def can_fingerprint(next_can: Callable) -> tuple[str | None, dict[int, dict]]:
+  finger = gen_empty_fingerprint()
+  frame = 0
+  done = False
+
+  while not done:
+    a = next_can()
+
+    for can in a.can:
+      # The fingerprint dict is generated for all buses, this way the car interface
+      # can use it to detect a (valid) multipanda setup and initialize accordingly
+      if can.src < 128:
+        if can.src not in finger:
+          finger[can.src] = {}
+        finger[can.src][can.address] = len(can.dat)
+
+    # bail if we've been waiting for more than 2s
+    done = frame > 100
+
+    frame += 1
+
+  return finger

selfdrive/controls/lib/events.py

@@ -1141,6 +1141,9 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
     ET.PERMANENT: torque_nn_load_alert,
   },
 
+  EventName.hyundaiRadarTracksAvailable: {
+    ET.PERMANENT: NormalPermanentAlert("Radar tracks available. Restart the car to initialize")
+  }
 }
 
 

system/manager/manager.py

@@ -71,8 +71,8 @@ def manager_init() -> None:
     ("FeatureStatus", "1"),
     ("HandsOnWheelMonitoring", "0"),
     ("HasAcceptedTermsSP", "0"),
-    ("HyundaiCruiseMainDefault", "0"),
     ("HideVEgoUi", "0"),
+    ("HyundaiCruiseMainDefault", "0"),
     ("LastSpeedLimitSignTap", "0"),
     ("LkasToggle", "0"),
     ("MadsIconToggle", "1"),