commaai · adeebshihadeh · Jun 1, 2025 · Feb 24, 2025 · Feb 24, 2025 · Feb 24, 2025
diff --git a/opendbc/car/ford/interface.py b/opendbc/car/ford/interface.py
@@ -29,7 +29,6 @@ def get_pid_accel_limits(CP, current_speed, cruise_speed):
   def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, alpha_long, is_release, docs) -> structs.CarParams:
     ret.brand = "ford"
 
-    ret.radarUnavailable = Bus.radar not in DBC[candidate]
     ret.steerControlType = structs.CarParams.SteerControlType.angle
     ret.steerActuatorDelay = 0.2
     ret.steerLimitTimer = 1.0
@@ -38,10 +37,16 @@ def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, alpha_lo
     ret.longitudinalTuning.kiBP = [0.]
     ret.longitudinalTuning.kiV = [0.5]
 
-    if not ret.radarUnavailable and DBC[candidate][Bus.radar] == RADAR.DELPHI_MRR:
+    # TODO: verify MRR_64 before it's used for longitudinal control
+    ret.radarUnavailable = Bus.radar not in DBC[candidate]
+    ret.radarDelay = {
       # average of 33.3 Hz radar timestep / 4 scan modes = 60 ms
       # MRR_Header_Timestamps->CAN_DET_TIME_SINCE_MEAS reports 61.3 ms
-      ret.radarDelay = 0.06
+      RADAR.DELPHI_MRR: 0.06,
+
+      # average of 20 Hz radar timestep / 4 scan modes = 100 ms
+      RADAR.DELPHI_MRR_64:  0.1
+    }.get(Bus.radar, 0.1)
 
     CAN = CanBus(fingerprint=fingerprint)
     cfgs = [get_safety_config(structs.CarParams.SafetyModel.ford)]

diff --git a/opendbc/car/ford/radar_interface.py b/opendbc/car/ford/radar_interface.py
@@ -15,6 +15,7 @@
 DELPHI_MRR_RADAR_START_ADDR = 0x120
 DELPHI_MRR_RADAR_HEADER_ADDR = 0x174  # MRR_Header_SensorCoverage
 DELPHI_MRR_RADAR_MSG_COUNT = 64
+DELPHI_MRR_RADAR_MSG_COUNT_64 = 22 # 22 messages in CANFD
 
 DELPHI_MRR_RADAR_RANGE_COVERAGE = {0: 42, 1: 164, 2: 45, 3: 175}  # scan index to detection range (m)
 DELPHI_MRR_MIN_LONG_RANGE_DIST = 30  # meters
@@ -88,6 +89,14 @@ def _create_delphi_mrr_radar_can_parser(CP) -> CANParser:
 
   return CANParser(RADAR.DELPHI_MRR, messages, CanBus(CP).radar)
 
+def _create_delphi_mrr_radar_can_parser_64(CP) -> CANParser:
+  messages = []
+
+  for i in range(1, DELPHI_MRR_RADAR_MSG_COUNT_64 + 1):
+    msg = f"MRR_Detection_{i:03d}"
+    messages += [(msg, 20)]
+
+  return CANParser(RADAR.DELPHI_MRR_64, messages, CanBus(CP).radar)
 
 class RadarInterface(RadarInterfaceBase):
   def __init__(self, CP):
@@ -111,6 +120,9 @@ def __init__(self, CP):
     elif self.radar == RADAR.DELPHI_MRR:
       self.rcp = _create_delphi_mrr_radar_can_parser(CP)
       self.trigger_msg = DELPHI_MRR_RADAR_HEADER_ADDR
+    elif self.radar == RADAR.DELPHI_MRR_64:
+      self.rcp = _create_delphi_mrr_radar_can_parser_64(CP)
+      self.trigger_msg = DELPHI_MRR_RADAR_START_ADDR + DELPHI_MRR_RADAR_MSG_COUNT_64 - 1
     else:
       raise ValueError(f"Unsupported radar: {self.radar}")
 
@@ -135,6 +147,10 @@ def update(self, can_strings):
       _update = self._update_delphi_mrr(ret)
       if not _update:
         return None
+    elif self.radar == RADAR.DELPHI_MRR_64:
+      _update = self._update_delphi_mrr_64(ret)
+      if not _update:
+        return None
 
     ret.points = list(self.pts.values())
     return ret
@@ -229,6 +245,79 @@ def _update_delphi_mrr(self, ret: structs.RadarData):
     if headerScanIndex != 3:
       return False
 
+    return self.do_clustering()
+
+  def _update_delphi_mrr_64(self, ret: structs.RadarData):
+    # There is not discovered MRR_Header_InformationDetections message in CANFD
+    # headerScanIndex = int(self.rcp.vl["MRR_Header_InformationDetections"]['CAN_SCAN_INDEX']) & 0b11
+    headerScanIndex = int(self.rcp.vl["MRR_Detection_001"]['CAN_SCAN_INDEX_2LSB_01_01'])
+
+    # In reverse, the radar continually sends the last messages. Mark this as invalid
+    if (self.prev_headerScanIndex + 1) % 4 != headerScanIndex:
+      self.radar_unavailable_cnt += 1
+    else:
+      self.radar_unavailable_cnt = 0
+    self.prev_headerScanIndex = headerScanIndex
+
+    if self.radar_unavailable_cnt >= 5:
+      self.pts.clear()
+      self.points.clear()
+      self.clusters.clear()
+      ret.errors.radarUnavailableTemporary = True
+      return True
+
+    # Use points with Doppler coverage of +-60 m/s, reduces similar points
+    if headerScanIndex in (0, 1):
+      return False, []
+
+    # There is not discovered MRR_Header_SensorCoverage message in CANFD
+    # if DELPHI_MRR_RADAR_RANGE_COVERAGE[headerScanIndex] != int(self.rcp.vl["MRR_Header_SensorCoverage"]["CAN_RANGE_COVERAGE"]):
+    #   self.invalid_cnt += 1
+    # else:
+    #   self.invalid_cnt = 0
+
+    # # Rarely MRR_Header_InformationDetections can fail to send a message. The scan index is skipped in this case
+    # if self.invalid_cnt >= 5:
+    #   errors.append("wrongConfig")
+
+    for ii in range(1, DELPHI_MRR_RADAR_MSG_COUNT_64 + 1):
+      msg = self.rcp.vl[f"MRR_Detection_{ii:03d}"]
+
+      maxRangeID = 7 if ii < 22 else 4 # all messages have 7 points except the last one, which has only 4 points in CANFD
+      for iii in range(1,maxRangeID):
+
+        # SCAN_INDEX rotates through 0..3 on each message for different measurement modes
+        # Indexes 0 and 2 have a max range of ~40m, 1 and 3 are ~170m (MRR_Header_SensorCoverage->CAN_RANGE_COVERAGE)
+        # Indexes 0 and 1 have a Doppler coverage of +-71 m/s, 2 and 3 have +-60 m/s
+        scanIndex = msg[f"CAN_SCAN_INDEX_2LSB_{ii:02d}_{iii:02d}"]
+
+        # Throw out old measurements. Very unlikely to happen, but is proper behavior
+        if scanIndex != headerScanIndex:
+          continue
+
+        valid = bool(msg[f"CAN_DET_VALID_LEVEL_{ii:02d}_{iii:02d}"])
+
+        # Long range measurement mode is more sensitive and can detect the road surface
+        dist = msg[f"CAN_DET_RANGE_{ii:02d}_{iii:02d}"]  # m [0|255.984]
+        if scanIndex in (1, 3) and dist < DELPHI_MRR_MIN_LONG_RANGE_DIST:
+          valid = False
+
+        if valid:
+          azimuth = msg[f"CAN_DET_AZIMUTH_{ii:02d}_{iii:02d}"]              # rad [-3.1416|3.13964]
+          distRate = msg[f"CAN_DET_RANGE_RATE_{ii:02d}_{iii:02d}"]          # m/s [-128|127.984]
+          dRel = cos(azimuth) * dist                                        # m from front of car
+          yRel = sin(azimuth) * dist                                        # in car frame's y axis, right is positive
+
+          self.points.append([dRel, yRel * 2, distRate * 2])
+
+    # Update once we've cycled through all 4 scan modes
+    if headerScanIndex != 3:
+      return True, [] # MRR_Detection_* messages in CANFD are at 20Hz, services.py expects liveTracks to be at 20Hz - we'll send messages to meet the 20Hz
+
+    return self.do_clustering()
+
+  # Do the common work for CAN and CANFD clustering and prepare the points to be used for liveTracks
+  def do_clustering(self):
     # Cluster points from this cycle against the centroids from the previous cycle
     prev_keys = [[p.dRel, p.yRel * 2, p.vRel * 2] for p in self.clusters]
     labels = cluster_points(prev_keys, self.points, DELPHI_MRR_CLUSTER_THRESHOLD)

diff --git a/opendbc/car/ford/values.py b/opendbc/car/ford/values.py
@@ -56,6 +56,7 @@ class FordFlags(IntFlag):
 class RADAR:
   DELPHI_ESR = 'ford_fusion_2018_adas'
   DELPHI_MRR = 'FORD_CADS'
+  DELPHI_MRR_64 = 'FORD_CADS_64'
 
 
 class Footnote(Enum):
@@ -106,6 +107,7 @@ def init(self):
 class FordCANFDPlatformConfig(FordPlatformConfig):
   dbc_dict: DbcDict = field(default_factory=lambda: {
     Bus.pt: 'ford_lincoln_base_pt',
+    Bus.radar: RADAR.DELPHI_MRR_64,
   })
 
   def init(self):