IQ.Pilot Prebuilt Release @ 7a91404

artifacts/package_runtime/iqdbc/car/volkswagen/radar_interface.py

@@ -0,0 +1,120 @@
+import math
+
+from iqdbc.can import CANParser
+from iqdbc.can.dbc import DBC as DBCLoader
+from iqdbc.car import Bus, structs
+from iqdbc.car.interfaces import RadarInterfaceBase
+from iqdbc.car.common.conversions import Conversions as CV
+from iqdbc.car.volkswagen.values import DBC, VolkswagenFlags
+
+RADAR_ADDR = 0x24F
+NO_OBJECT = 0
+LANE_TYPES = ("Same_Lane", "Left_Lane", "Right_Lane")
+SIGNAL_SETS = tuple(
+  (
+    f"{prefix}_ObjectID",
+    f"{prefix}_Long_Distance",
+    f"{prefix}_Lat_Distance",
+    f"{prefix}_Rel_Velo",
+  )
+  for lane in LANE_TYPES
+  for idx in (1, 2)
+  for prefix in (f"{lane}_0{idx}",)
+)
+
+
+def get_radar_can_parser(CP):
+  if CP.flags & (VolkswagenFlags.MEB | VolkswagenFlags.MQB_EVO):
+    dbc_name = DBC[CP.carFingerprint][Bus.radar]
+    dbc = DBCLoader(dbc_name)
+    if "Strukturen_01" in dbc.name_to_msg:
+      messages = [("Strukturen_01", 25)]
+    elif "MEB_Distance_01" in dbc.name_to_msg:
+      messages = [("MEB_Distance_01", 25)]
+    else:
+      return None
+  else:
+    return None
+
+  return CANParser(DBC[CP.carFingerprint][Bus.radar], messages, 2)
+
+
+class RadarInterface(RadarInterfaceBase):
+  def __init__(self, CP, CP_IQ):
+    super().__init__(CP, CP_IQ)
+
+    self.updated_messages: set[int] = set()
+    self.trigger_msg: int = RADAR_ADDR
+    self._track_id_counter: int = 0
+
+    self.radar_off_can: bool = CP.radarUnavailable
+    self.rcp: CANParser | None = get_radar_can_parser(CP)
+
+    self._pts = self.pts
+
+  def update(self, can_strings):
+    """Entry-point called by the vehicle loop every CAN tick."""
+    if self.radar_off_can or self.rcp is None:
+      return super().update(None)
+
+    vls = self.rcp.update(can_strings)
+    self.updated_messages.update(vls)
+
+    if self.trigger_msg not in self.updated_messages:
+      return None
+
+    radar_data = self._process_radar_frame()
+    self.updated_messages.clear()
+    return radar_data
+
+  def _process_radar_frame(self):
+    ret = structs.RadarData()
+
+    if self.rcp is None:
+      return ret
+
+    if not self.rcp.can_valid:
+      ret.errors.canError = True
+      return ret
+
+    msg = self.rcp.vl["Strukturen_01"] if "Strukturen_01" in self.rcp.vl else self.rcp.vl["MEB_Distance_01"]
+    get = msg.__getitem__
+
+    active_objects: dict[int, tuple[float, float, float]] = {}
+    for obj_id_sig, long_sig, lat_sig, vel_sig in SIGNAL_SETS:
+      obj_id = get(obj_id_sig)
+      if obj_id == NO_OBJECT:
+        continue
+
+      if obj_id in active_objects:
+        ret.errors.canError = True
+        return ret
+
+      active_objects[obj_id] = (
+        get(long_sig),  # dRel
+        get(lat_sig),   # yRel
+        get(vel_sig),   # vRel
+      )
+
+    for obj_id, (d_rel, y_rel, v_rel) in active_objects.items():
+      if obj_id not in self._pts:
+        pt = structs.RadarData.RadarPoint()
+        pt.trackId = self._track_id_counter
+        self._track_id_counter += 1
+        self._pts[obj_id] = pt
+      else:
+        pt = self._pts[obj_id]
+
+      pt.measured = True
+      pt.dRel = d_rel
+      pt.yRel = y_rel
+      pt.vRel = v_rel
+      pt.aRel = math.nan
+      pt.yvRel = math.nan
+
+    inactive_ids = self._pts.keys() - active_objects.keys()
+    for obj_id in inactive_ids:
+      self._pts.pop(obj_id, None)
+
+    ret.points = list(self._pts.values())
+    return ret