Ford CAN FD (#27166)

* add LateralMotionControl2 message

* send LatCtl2 message on CAN FD cars

selfdrive/car/ford/carcontroller.py

@@ -3,8 +3,8 @@ from common.numpy_fast import clip
 from opendbc.can.packer import CANPacker
 from selfdrive.car import apply_std_steer_angle_limits
 from selfdrive.car.ford.fordcan import create_acc_command, create_acc_ui_msg, create_button_msg, create_lat_ctl_msg, \
-  create_lka_msg, create_lkas_ui_msg
-from selfdrive.car.ford.values import CANBUS, CarControllerParams
+  create_lat_ctl2_msg, create_lka_msg, create_lkas_ui_msg
+from selfdrive.car.ford.values import CANBUS, CANFD_CARS, CarControllerParams
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 
@@ -54,7 +54,14 @@ class CarController:
 
       self.apply_curvature_last = apply_curvature
       can_sends.append(create_lka_msg(self.packer))
-      can_sends.append(create_lat_ctl_msg(self.packer, CC.latActive, 0., 0., -apply_curvature, 0.))
+
+      if self.CP.carFingerprint in CANFD_CARS:
+        # TODO: extended mode
+        mode = 1 if CC.latActive else 0
+        counter = self.frame // CarControllerParams.STEER_STEP
+        can_sends.append(create_lat_ctl2_msg(self.packer, mode, 0., 0., -apply_curvature, 0., counter))
+      else:
+        can_sends.append(create_lat_ctl_msg(self.packer, CC.latActive, 0., 0., -apply_curvature, 0.))
 
     ### longitudinal control ###
     # send acc command at 50Hz
@@ -71,7 +78,6 @@ class CarController:
 
       can_sends.append(create_acc_command(self.packer, CC.longActive, gas, accel, precharge_brake, decel))
 
-
     ### 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

@@ -4,6 +4,15 @@ from selfdrive.car.ford.values import CANBUS
 HUDControl = car.CarControl.HUDControl
 
 
+def calculate_lat_ctl2_checksum(mode: int, counter: int, dat: bytearray):
+  checksum = mode + counter
+  checksum += dat[2] + ((dat[3] & 0xE0) >> 5)           # curvature
+  checksum += dat[6] + ((dat[7] & 0xE0) >> 5)           # curvature rate
+  checksum += (dat[3] & 0x1F) + ((dat[4] & 0xFC) >> 2)  # path angle
+  checksum += (dat[4] & 0x3) + dat[5]                   # path offset
+  return 0xFF - (checksum & 0xFF)
+
+
 def create_lka_msg(packer):
   """
   Creates an empty CAN message for the Ford LKA Command.
@@ -16,7 +25,8 @@ def create_lka_msg(packer):
   return packer.make_can_msg("Lane_Assist_Data1", CANBUS.main, {})
 
 
-def create_lat_ctl_msg(packer, lat_active: bool, path_offset: float, path_angle: float, curvature: float, curvature_rate: float):
+def create_lat_ctl_msg(packer, lat_active: bool, path_offset: float, path_angle: float, curvature: float,
+                       curvature_rate: float):
   """
   Creates a CAN message for the Ford TJA/LCA Command.
 
@@ -55,6 +65,38 @@ def create_lat_ctl_msg(packer, lat_active: bool, path_offset: float, path_angle:
   return packer.make_can_msg("LateralMotionControl", CANBUS.main, values)
 
 
+def create_lat_ctl2_msg(packer, mode: int, path_offset: float, path_angle: float, curvature: float,
+                        curvature_rate: float, counter: int):
+  """
+  Create a CAN message for the new Ford Lane Centering command.
+
+  This message is used on the CAN FD platform and replaces the old LateralMotionControl message. It is similar but has
+  additional signals for a counter and checksum.
+
+  Frequency is 20Hz.
+  """
+
+  values = {
+    "LatCtl_D2_Rq": mode,                       # Mode: 0=None, 1=PathFollowingLimitedMode, 2=PathFollowingExtendedMode,
+                                                #       3=SafeRampOut, 4-7=NotUsed [0|7]
+    "LatCtlRampType_D_Rq": 0,                   # 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3]
+    "LatCtlPrecision_D_Rq": 1,                  # 0=Comfortable, 1=Precise, 2/3=NotUsed [0|3]
+    "LatCtlPathOffst_L_Actl": path_offset,      # [-5.12|5.11] meter
+    "LatCtlPath_An_Actl": path_angle,           # [-0.5|0.5235] radians
+    "LatCtlCurv_No_Actl": curvature,            # [-0.02|0.02094] 1/meter
+    "LatCtlCrv_NoRate2_Actl": curvature_rate,   # [-0.001024|0.001023] 1/meter^2
+    "HandsOffCnfm_B_Rq": 0,                     # 0=Inactive, 1=Active [0|1]
+    "LatCtlPath_No_Cnt": counter,               # [0|15]
+    "LatCtlPath_No_Cs": 0,                      # [0|255]
+  }
+
+  # calculate checksum
+  dat = packer.make_can_msg("LateralMotionControl2", CANBUS.main, values)[2]
+  values["LatCtlPath_No_Cs"] = calculate_lat_ctl2_checksum(mode, counter, dat)
+
+  return packer.make_can_msg("LateralMotionControl2", CANBUS.main, values)
+
+
 def create_acc_command(packer, long_active: bool, gas: float, accel: float, precharge_brake: bool, decel: bool):
   """
   Creates a CAN message for the Ford ACC Command.

selfdrive/car/ford/values.py

@@ -1,7 +1,7 @@
 from collections import defaultdict
 from dataclasses import dataclass
 from enum import Enum
-from typing import Dict, List, Union
+from typing import Dict, List, Set, Union
 
 from cereal import car
 from selfdrive.car import AngleRateLimit, dbc_dict
@@ -52,6 +52,9 @@ class CAR:
   MAVERICK_MK1 = "FORD MAVERICK 1ST GEN"
 
 
+CANFD_CARS: Set[str] = set()
+
+
 class RADAR:
   DELPHI_ESR = 'ford_fusion_2018_adas'
   DELPHI_MRR = 'FORD_CADS'