Fixup ford radar

opendbc/car/car.capnp

@@ -324,6 +324,9 @@ struct RadarData @0x888ad6581cf0aacb {
 
     # some radars flag measurements VS estimates
     measured @6 :Bool;
+    flags @7 :Int32;  # radar specific flags
+    flags2 @8 :Int32;  # radar specific flags
+    flags3 @9 :Int32;  # radar specific flags
   }
 
   # deprecated

opendbc/car/ford/interface.py

@@ -14,7 +14,7 @@ def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, experime
     ret.carName = "ford"
     ret.dashcamOnly = bool(ret.flags & FordFlags.CANFD)
 
-    ret.radarUnavailable = True
+    ret.radarUnavailable = False
     ret.steerControlType = structs.CarParams.SteerControlType.angle
     ret.steerActuatorDelay = 0.2
     ret.steerLimitTimer = 1.0

opendbc/car/ford/radar_interface.py

@@ -40,6 +40,10 @@ class RadarInterface(RadarInterfaceBase):
   def __init__(self, CP):
     super().__init__(CP)
 
+    self.frame = 0
+
+    self.can_range_coverage = {0: 42, 1: 164, 2: 45, 3: 175}
+
     self.updated_messages = set()
     self.track_id = 0
     self.radar = DBC[CP.carFingerprint]['radar']
@@ -52,16 +56,19 @@ 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
+      print('trigger_msg:', self.trigger_msg)
     else:
       raise ValueError(f"Unsupported radar: {self.radar}")
 
   def update(self, can_strings):
+    self.frame += 1
     if self.rcp is None:
       return super().update(None)
 
     vls = self.rcp.update_strings(can_strings)
     self.updated_messages.update(vls)
 
+    # print('updated_messages:', self.updated_messages)
     if self.trigger_msg not in self.updated_messages:
       return None
 
@@ -74,7 +81,12 @@ def update(self, can_strings):
       self._update_delphi_esr()
     elif self.radar == RADAR.DELPHI_MRR:
       errors.extend(self._update_delphi_mrr())
+      # print('pts', len(self.pts), self.rcp.vl['MRR_Header_InformationDetections']['CAN_NUMBER_OF_DET'])
+      # if len(self.pts) != self.rcp.vl['MRR_Header_InformationDetections']['CAN_NUMBER_OF_DET'] and self.frame > 10:
+      #   print('mismatch!')
+      #   raise Exception
 
+    ret.errors = errors
     ret.points = list(self.pts.values())
     ret.errors = errors
     self.updated_messages.clear()
@@ -110,21 +122,54 @@ def _update_delphi_esr(self):
           del self.pts[ii]
 
   def _update_delphi_mrr(self):
+    errors = []
+
     headerScanIndex = int(self.rcp.vl["MRR_Header_InformationDetections"]['CAN_SCAN_INDEX']) & 0b11
 
+    print()
+    header = self.rcp.vl['MRR_Header_InformationDetections']
+    scan_index = int(header['CAN_SCAN_INDEX'])
+    look_index = int(header['CAN_LOOK_INDEX']) & 0b11  # or CAN_LOOK_ID?
+    look_id = int(header['CAN_LOOK_ID'])
+    # print('scan_index', int(scan_index) & 0b11, scan_index)
+
     errors = []
     if DELPHI_MRR_RADAR_RANGE_COVERAGE[headerScanIndex] != int(self.rcp.vl["MRR_Header_SensorCoverage"]["CAN_RANGE_COVERAGE"]):
       errors.append("wrongConfig")
 
+    print(self.can_range_coverage)
+
+    if self.frame > 10:
+      assert self.rcp.vl['MRR_Header_InformationDetections']['CAN_SCAN_INDEX'] == self.rcp.vl['MRR_Header_InformationDetections']['CAN_LOOK_INDEX']
+      if look_id == 0:
+        assert look_index == 1
+      elif look_id == 1:
+        assert look_index == 3
+      elif look_id == 2:
+        assert look_index == 0
+      elif look_id == 3:
+        assert look_index == 2
+
+    print('updated msgs', len(self.updated_messages))
+    if self.frame > 10:
+      assert len(self.updated_messages) == DELPHI_MRR_RADAR_MSG_COUNT + 2, len(self.updated_messages)
+
     for ii in range(1, DELPHI_MRR_RADAR_MSG_COUNT + 1):
+      # if look_index != 0:
+      #   continue
       msg = self.rcp.vl[f"MRR_Detection_{ii:03d}"]
 
       # 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)
       # TODO: filter out close range index 1 and 3 points, contain false positives
       # TODO: can we group into 2 groups?
       scanIndex = msg[f"CAN_SCAN_INDEX_2LSB_{ii:02d}"]
-      i = (ii - 1) * 4 + scanIndex
+      # i = (ii - 1) * 4 + scanIndex
+      i = (ii - 1) * 2 + (1 if scanIndex in (0, 2) else 0)
+      # print('pt scanIndex', scanIndex)
+      if scanIndex != int(scan_index) & 0b11 and self.frame > 10:
+        print('doesn\'t match!', scanIndex, int(scan_index) & 0b11, scan_index)
+        raise Exception
 
       # Throw out old measurements. Very unlikely to happen, but is proper behavior
       if scanIndex != headerScanIndex:
@@ -149,6 +194,7 @@ def _update_delphi_mrr(self):
         distRate = msg[f"CAN_DET_RANGE_RATE_{ii: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, left is positive
+        super_res = msg[f"CAN_DET_SUPER_RES_TARGET_{ii:02d}"]   # bool
 
         # delphi doesn't notify of track switches, so do it manually
         # TODO: refactor this to radard if more radars behave this way
@@ -159,6 +205,9 @@ def _update_delphi_mrr(self):
         self.pts[i].dRel = dRel
         self.pts[i].yRel = yRel
         self.pts[i].vRel = distRate
+        self.pts[i].flags = look_index
+        self.pts[i].flags2 = scanIndex
+        self.pts[i].flags3 = super_res
 
         self.pts[i].measured = True