From 2d7e22eca3e8083f50fc018ae8089105a75e7dc4 Mon Sep 17 00:00:00 2001 From: blue-genie <8130363+blue-genie@users.noreply.github.com> Date: Mon, 24 Feb 2025 09:40:20 -0600 Subject: [PATCH 1/4] Parse Ford CANFD Radar --- opendbc/car/ford/interface.py | 5 ++ opendbc/car/ford/radar_interface.py | 78 +++++++++++++++++++++++++++++ opendbc/car/ford/values.py | 2 + 3 files changed, 85 insertions(+) diff --git a/opendbc/car/ford/interface.py b/opendbc/car/ford/interface.py index 8b8a513d72..b947998fc4 100644 --- a/opendbc/car/ford/interface.py +++ b/opendbc/car/ford/interface.py @@ -34,6 +34,11 @@ def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, experime # MRR_Header_Timestamps->CAN_DET_TIME_SINCE_MEAS reports 61.3 ms ret.radarDelay = 0.06 + if not ret.radarUnavailable and DBC[candidate][Bus.radar] == RADAR.DELPHI_MRR_64: + # average of 20 Hz radar timestep / 4 scan modes = 60 ms + # MRR_Header_Timestamps->CAN_DET_TIME_SINCE_MEAS reports 61.3 ms + ret.radarDelay = 0.1 + CAN = CanBus(fingerprint=fingerprint) cfgs = [get_safety_config(structs.CarParams.SafetyModel.ford)] if CAN.main >= 4: diff --git a/opendbc/car/ford/radar_interface.py b/opendbc/car/ford/radar_interface.py index 5cc1574962..3f30fa1bb9 100644 --- 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): @@ -109,6 +118,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}") @@ -134,6 +146,11 @@ def update(self, can_strings): errors.extend(_errors) if not _update: return None + elif self.radar == RADAR.DELPHI_MRR_64: + _update, _errors = self._update_delphi_mrr_64() + errors.extend(_errors) + if not _update: + return None ret = structs.RadarData() ret.points = list(self.pts.values()) @@ -217,6 +234,67 @@ def _update_delphi_mrr(self): if headerScanIndex != 3: return False, [] + return self.do_clustering(errors) + + def _update_delphi_mrr_64(self): + # 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']) + + # Use points with Doppler coverage of +-60 m/s, reduces similar points + if headerScanIndex in (0, 1): + return False, [] + + errors = [] + # 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: + print(f"CAN_DET_HOST_VEH_CLUTTER: {msg[f"CAN_DET_HOST_VEH_CLUTTER_{ii:02d}_{iii:02d}"] }") if msg[f"CAN_DET_HOST_VEH_CLUTTER_{ii:02d}_{iii:02d}"] > 0 else None + 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(errors) + + # Do the common work for CAN and CANFD clustering and prepare the points to be sued for liveTracks + def do_clustering(self, errors): # 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 index aa1a1d42c5..26e79add2c 100644 --- a/opendbc/car/ford/values.py +++ b/opendbc/car/ford/values.py @@ -54,6 +54,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): @@ -99,6 +100,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): From 7e9c55b3bdd0bcc0779088bc629f4b85d2e7f1ec Mon Sep 17 00:00:00 2001 From: blue-genie <8130363+blue-genie@users.noreply.github.com> Date: Mon, 24 Feb 2025 09:43:51 -0600 Subject: [PATCH 2/4] Updating the comment --- opendbc/car/ford/interface.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/opendbc/car/ford/interface.py b/opendbc/car/ford/interface.py index b947998fc4..08d382e7b2 100644 --- a/opendbc/car/ford/interface.py +++ b/opendbc/car/ford/interface.py @@ -35,8 +35,7 @@ def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, experime ret.radarDelay = 0.06 if not ret.radarUnavailable and DBC[candidate][Bus.radar] == RADAR.DELPHI_MRR_64: - # average of 20 Hz radar timestep / 4 scan modes = 60 ms - # MRR_Header_Timestamps->CAN_DET_TIME_SINCE_MEAS reports 61.3 ms + # average of 20 Hz radar timestep / 4 scan modes = 100 ms ret.radarDelay = 0.1 CAN = CanBus(fingerprint=fingerprint) From efc53fc73b442149e8d550bcd110c11b568bd41c Mon Sep 17 00:00:00 2001 From: blue-genie <8130363+blue-genie@users.noreply.github.com> Date: Mon, 24 Feb 2025 16:17:29 -0600 Subject: [PATCH 3/4] Remove debug message. --- opendbc/car/ford/radar_interface.py | 1 - 1 file changed, 1 deletion(-) diff --git a/opendbc/car/ford/radar_interface.py b/opendbc/car/ford/radar_interface.py index 3f30fa1bb9..2d26822fa1 100644 --- a/opendbc/car/ford/radar_interface.py +++ b/opendbc/car/ford/radar_interface.py @@ -279,7 +279,6 @@ def _update_delphi_mrr_64(self): valid = False if valid: - print(f"CAN_DET_HOST_VEH_CLUTTER: {msg[f"CAN_DET_HOST_VEH_CLUTTER_{ii:02d}_{iii:02d}"] }") if msg[f"CAN_DET_HOST_VEH_CLUTTER_{ii:02d}_{iii:02d}"] > 0 else None 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 From 46806fc66d8f15e43aa322f5fa36560f6193daab Mon Sep 17 00:00:00 2001 From: blue-genie <8130363+blue-genie@users.noreply.github.com> Date: Mon, 24 Feb 2025 16:20:02 -0600 Subject: [PATCH 4/4] Fix misspell --- opendbc/car/ford/radar_interface.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc/car/ford/radar_interface.py b/opendbc/car/ford/radar_interface.py index 2d26822fa1..c513682342 100644 --- a/opendbc/car/ford/radar_interface.py +++ b/opendbc/car/ford/radar_interface.py @@ -292,7 +292,7 @@ def _update_delphi_mrr_64(self): return self.do_clustering(errors) - # Do the common work for CAN and CANFD clustering and prepare the points to be sued for liveTracks + # Do the common work for CAN and CANFD clustering and prepare the points to be used for liveTracks def do_clustering(self, errors): # 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]