diff --git a/cereal/car.capnp b/cereal/car.capnp index e77e8c294..38be2d944 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -137,6 +137,7 @@ struct CarEvent @0x9b1657f34caf3ad3 { speedLimitPreActive @139; speedLimitConfirmed @140; torqueNNLoad @141; + hyundaiRadarTracksAvailable @142; radarCanErrorDEPRECATED @15; communityFeatureDisallowedDEPRECATED @62; diff --git a/common/params.cc b/common/params.cc index 5cb98babf..7cd366481 100644 --- a/common/params.cc +++ b/common/params.cc @@ -265,6 +265,9 @@ std::unordered_map keys = { {"HkgSmoothStop", PERSISTENT | BACKUP}, {"HotspotOnBoot", PERSISTENT}, {"HotspotOnBootConfirmed", PERSISTENT}, + {"HyundaiRadarTracksAvailable", PERSISTENT}, + {"HyundaiRadarTracksAvailableCache", PERSISTENT}, + {"HyundaiRadarTracksAvailablePersistent", PERSISTENT}, {"LastCarModel", PERSISTENT | BACKUP}, {"LastSpeedLimitSignTap", PERSISTENT}, {"LastSunnylinkPingTime", CLEAR_ON_MANAGER_START}, diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index a12bd0ed8..5be5f5e2d 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/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) diff --git a/selfdrive/car/param_manager.py b/selfdrive/car/param_manager.py index 6f393bd2d..00e7bb382 100644 --- a/selfdrive/car/param_manager.py +++ b/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"), diff --git a/selfdrive/car/sunnypilot/fingerprinting.py b/selfdrive/car/sunnypilot/fingerprinting.py new file mode 100644 index 000000000..355106720 --- /dev/null +++ b/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 diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index 13cef3796..f6d6f5bd0 100755 --- a/selfdrive/controls/lib/events.py +++ b/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") + } } diff --git a/system/manager/manager.py b/system/manager/manager.py index e4537cac7..34d78923a 100755 --- a/system/manager/manager.py +++ b/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"),