Ship Ford Q4 to release

[adeebshihadeh]

We've got a Mach-E rented for COMMA_HACK (there's no F-150s available around us unfortunately).

To make the most of that time, post any quirks, bugs, issues, and feedback here. Ideally, include at least your Dongle ID and preferably a route too.

---

TODO

• validate new steering control
• ship the safety + pull out of dashcam
• identify and block cars with SecOc

Nice to haves:

• fuzzy fp
• radar parsing
• longitudinal control alpha
• longitudinal control release

[blue-genie]

I will try to comment on what we do for Q4 Ford to refine it - I will split it in multiple comments for easier consumtion

RADAR Parsing:
Q4 uses 64byte messages for Radar.
MRR_Detection_0XX goes from 0 to 22, instead of 0 to 64
MRR_Detection_001 to MRR_Detection_021 has 6 radar points, while MRR_Detection_022 has only 2.
Using: https://github.com/commaai/opendbc/blob/master/opendbc/dbc/FORD_CADS_64.dbc as the DBC file.

I had an attempt to use the same approach that is in master for Q3, even though Q4 is missing some control messages, like 'MRR_Header_InformationDetections', 'MRR_Header_SensorCoverage' - I just relied on the MRR device sending the right scanIndex, https://github.com/commaai/opendbc/blob/master/opendbc/car/ford/radar_interface.py#L198.

The outcome was the drivability was not bad, but the chevron arrow was jumping around a lot. Even pointing to location where nothing was present, just an empty lane.

This is my equivalent method:

def _update_delphi_mrr_64(self):
    # 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 = []
    # if DELPHI_MRR_RADAR_RANGE_COVERAGE[headerScanIndex] != int(self.rcp.vl["MRR_Header_SensorCoverage"]["CAN_RANGE_COVERAGE"]):
    #   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
      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 msg[f"CAN_DET_CONFID_AZIMUTH_{ii:02d}_{iii:02d}"] < 2:
        #  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, left is positive

          self.points.append([dRel, yRel * 2, distRate * 2])

    # Update once we've cycled through all 4 scan modes
    if headerScanIndex != 3:
      return False, []

    # 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)

    points_by_track_id = defaultdict(list)
    for idx, label in enumerate(labels):
      if label != -1:
        points_by_track_id[self.clusters[label].trackId].append(self.points[idx])
      else:
        points_by_track_id[self.track_id].append(self.points[idx])
        self.track_id += 1

    self.clusters = []
    for idx, (track_id, pts) in enumerate(points_by_track_id.items()):
      dRel = [p[0] for p in pts]
      min_dRel = min(dRel)
      dRel = sum(dRel) / len(dRel)

      yRel = [p[1] for p in pts]
      yRel = sum(yRel) / len(yRel) / 2

      vRel = [p[2] for p in pts]
      vRel = sum(vRel) / len(vRel) / 2

      # FIXME: creating capnp RadarPoint and accessing attributes are both expensive, so we store a dataclass and reuse the RadarPoint
      self.clusters.append(Cluster(dRel=dRel, yRel=yRel, vRel=vRel, trackId=track_id))

      if idx not in self.pts:
        self.pts[idx] = structs.RadarData.RadarPoint(measured=True, aRel=float('nan'), yvRel=float('nan'))

      self.pts[idx].dRel = min_dRel
      self.pts[idx].yRel = yRel
      self.pts[idx].vRel = vRel
      self.pts[idx].trackId = track_id

    for idx in range(len(points_by_track_id), len(self.pts)):
      del self.pts[idx]

    self.points = []

    return True, errors

[blue-genie]

Usable Radar:

Q4 fords are using this approach in sunnypilot https://github.com/sunnypilot/sunnypilot/blob/dev-c3/selfdrive/car/ford/radar_interface.py

Q4 with the MLSIM models - no extra tuning - was always stopping to close to the car in front or even trying to ram the car in front.

Ford sends: Steer_Assist_Data which can be parsed and a lead can be retrieved from there. This helps a lot and the lead chevron is stable and points in the right location:

  def _update_steer_assist_data(self):
    msg = self.rcp.vl["Steer_Assist_Data"]
    updated_msg = self.updated_messages

    dRel = msg['CmbbObjDistLong_L_Actl']
    confidence = msg['CmbbObjConfdnc_D_Stat']
    new_track = False

    # if dRel < 1022:
    if confidence > 0:
      if 0 not in self.pts:
        self.pts[0] = structs.RadarData.RadarPoint.new_message()
        self.pts[0].trackId = self.track_id
        self.vRelCol[0] = collections.deque(maxlen=20)
        self.track_id += 1
        new_track = True

      yRel = msg['CmbbObjDistLat_L_Actl']
      vRel = msg['CmbbObjRelLong_V_Actl']
      yvRel = msg['CmbbObjRelLat_V_Actl']
      calc = 0
      if not new_track:
        # if this is a newly created track - we don't have historical data so skip it
        # if we are on the same track
        # Let's see if we are moving:
        #   positive gap - lead is moving faster than us
        #   negative gap - lead is moving slower than us
        dDiff = dRel - self.pts[0].dRel
        if (abs(vRel) < 1.0e-2):
          self.vRelCol[0].append(dDiff)
          vRel = sum(self.vRelCol[0])
          calc = 1
        else:
          if len(self.vRelCol[0]) > 0:
            self.vRelCol[0].clear()

        if abs(self.pts[0].vRel - vRel) > 2 or abs(self.pts[0].dRel - dRel) > 5:
          self.pts[0].trackId = self.track_id
          if len(self.vRelCol[0]) > 0:
            self.vRelCol[0].clear()
          self.track_id += 1

      self.pts[0].dRel = dRel  # from front of car
      self.pts[0].yRel = yRel  # in car frame's y axis, left is positive
      self.pts[0].vRel = vRel
      self.pts[0].aRel = float('nan')
      self.pts[0].yvRel = yvRel
      self.pts[0].measured = True
    else:
      if 0 in self.pts:
        del self.pts[0]
        del self.vRelCol[0]

[blue-genie]

openpilot long usability in Q4:

Even in the latest master the brake actuator, since for ford we have to control it, it's very active. It activates for very small slowdowns - there is no "cruising" - no acceleration and no braking

here https://github.com/commaai/opendbc/blob/master/opendbc/car/ford/carcontroller.py#L128 we are using

      if self.accel_pitch_compensated > -0.08 or not CC.longActive:
        self.brake_request = False
      elif self.accel_pitch_compensated < -0.14:
        self.brake_request = True

This is what it was found to give a good balance to avoid false actuator activation, this might be considered just fine tuning. I think radar is more important than this fine tuning.

[hiimisaac]

openpilot long usability in Q4:

Even in the latest master the brake actuator, since for ford we have to control it, it's very active. It activates for very small slowdowns - there is no "cruising" - no acceleration and no braking

here https://github.com/commaai/opendbc/blob/master/opendbc/car/ford/carcontroller.py#L128 we are using

      if self.accel_pitch_compensated > -0.08 or not CC.longActive:
        self.brake_request = False
      elif self.accel_pitch_compensated < -0.14:
        self.brake_request = True

This is what it was found to give a good balance to avoid false actuator activation, this might be considered just fine tuning. I think radar is more important than this fine tuning.

This is probably why the EVs seem to be okay.

There's no engine braking, but the friction brake still activates the regen braking slightly before actuating the physical friction brakes. With the above hysteresis, this is a great feel for both EVs and ICE.

In addition to the above long items, the following lat item is noticed:

Biggest issue we have right now is that when you take control of the steering wheel to take a 90 degree turn, the steering wheel holds an angle that's not straight and you have to hold the wheel until it stops pulling.

right now, to address this on forks, we set all 4 control variables to zero and ramp_type to 3. this allows us to take control from and not experience the angle holding that master does

Further more, the wheel can only be actuated to 80 degrees when using the single curvature variable and actuation is slow. To get further beyond the 80 degree limit and wildly better micro-control, the other three signals need to be introduced.

[blue-genie]

e36b272d5679115f/00000308--b61d26edc1/0

A good route that can be analyzed:

• radar parsing - similar logic to Q3 - ignoring missing messages - note that service.py needs to lower the frequency of liveTrack from 20 to 5 - otherwise there are teepee reported.
• removed dashcam
• Update fingerprint to work for my truck

Found issue #1754 which causes failure to fingerprint.

[hiimisaac]

Standing by to test branches this weekend.

[OneFast440]

Commenting to help test during this weekends hackathon (2023 F-150 Raptor)

[windydrew]

I have a 23 Lightning running 3x on bluepilot. I would like to help with this process but not sure what is needed.