Ford: radar + long control fixups (#28948)

* enable radar

* manual new track

* Don't filter points

* cleanup

* Update radar_interface.py

* This is non-zero in stock

* add speed

* more extreme

* A few more things

* fixes

* cleanup

* revert that

---------

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>
old-commit-hash: 8c25741e46

selfdrive/car/ford/carcontroller.py

@@ -1,5 +1,6 @@
 from cereal import car
 from openpilot.common.numpy_fast import clip
+from openpilot.common.conversions import Conversions as CV
 from opendbc.can.packer import CANPacker
 from openpilot.selfdrive.car import apply_std_steer_angle_limits
 from openpilot.selfdrive.car.ford import fordcan
@@ -88,9 +89,8 @@ class CarController:
       gas = accel
       if not CC.longActive or gas < CarControllerParams.MIN_GAS:
         gas = CarControllerParams.INACTIVE_GAS
-
       stopping = CC.actuators.longControlState == LongCtrlState.stopping
-      can_sends.append(fordcan.create_acc_msg(self.packer, self.CAN, CC.longActive, gas, accel, stopping))
+      can_sends.append(fordcan.create_acc_msg(self.packer, self.CAN, CC.longActive, gas, accel, stopping, v_ego_kph=40 * CV.MPH_TO_KPH))
 
     ### ui ###
     send_ui = (self.main_on_last != main_on) or (self.lkas_enabled_last != CC.latActive) or (self.steer_alert_last != steer_alert)

selfdrive/car/ford/fordcan.py

@@ -118,7 +118,7 @@ def create_lat_ctl2_msg(packer, CAN: CanBus, mode: int, path_offset: float, path
   return packer.make_can_msg("LateralMotionControl2", CAN.main, values)
 
 
-def create_acc_msg(packer, CAN: CanBus, long_active: bool, gas: float, accel: float, stopping: bool):
+def create_acc_msg(packer, CAN: CanBus, long_active: bool, gas: float, accel: float, stopping: bool, v_ego_kph: float):
   """
   Creates a CAN message for the Ford ACC Command.
 
@@ -133,7 +133,9 @@ def create_acc_msg(packer, CAN: CanBus, long_active: bool, gas: float, accel: fl
     "AccBrkTot_A_Rq": accel,                          # Brake total accel request: [-20|11.9449] m/s^2
     "Cmbb_B_Enbl": 1 if long_active else 0,           # Enabled: 0=No, 1=Yes
     "AccPrpl_A_Rq": gas,                              # Acceleration request: [-5|5.23] m/s^2
+    "AccPrpl_A_Pred": gas,                            # Acceleration request: [-5|5.23] m/s^2
     "AccResumEnbl_B_Rq": 1 if long_active else 0,
+    "AccVeh_V_Trg": v_ego_kph,                        # Target speed: [0|255] km/h
     # TODO: we may be able to improve braking response by utilizing pre-charging better
     "AccBrkPrchg_B_Rq": 1 if decel else 0,            # Pre-charge brake request: 0=No, 1=Yes
     "AccBrkDecel_B_Rq": 1 if decel else 0,            # Deceleration request: 0=Inactive, 1=Active

selfdrive/car/ford/radar_interface.py

@@ -119,17 +119,23 @@ class RadarInterface(RadarInterfaceBase):
         self.track_id += 1
 
       valid = bool(msg[f"CAN_DET_VALID_LEVEL_{ii:02d}"])
-      amplitude = msg[f"CAN_DET_AMPLITUDE_{ii:02d}"]            # dBsm [-64|63]
 
-      if valid and 0 < amplitude <= 15:
+      if valid:
         azimuth = msg[f"CAN_DET_AZIMUTH_{ii:02d}"]              # rad [-3.1416|3.13964]
         dist = msg[f"CAN_DET_RANGE_{ii:02d}"]                   # m [0|255.984]
         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
 
-        # *** openpilot radar point ***
-        self.pts[i].dRel = cos(azimuth) * dist                  # m from front of car
-        self.pts[i].yRel = -sin(azimuth) * dist                 # in car frame's y axis, left is positive
-        self.pts[i].vRel = distRate                             # m/s
+        # delphi doesn't notify of track switches, so do it manually
+        # TODO: refactor this to radard if more radars behave this way
+        if abs(self.pts[i].vRel - distRate) > 2 or abs(self.pts[i].dRel - dRel) > 5:
+          self.track_id += 1
+          self.pts[i].trackId = self.track_id
+
+        self.pts[i].dRel = dRel
+        self.pts[i].yRel = yRel
+        self.pts[i].vRel = distRate
 
         self.pts[i].measured = True