diff --git a/opendbc/car/ford/interface.py b/opendbc/car/ford/interface.py index 8b8a513d72..08d382e7b2 100644 --- a/opendbc/car/ford/interface.py +++ b/opendbc/car/ford/interface.py @@ -34,6 +34,10 @@ 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 = 100 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..c513682342 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,66 @@ 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: + 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 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] 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):