From 1aeb66b58d693739530f4d40b0010885b8468b0a Mon Sep 17 00:00:00 2001
From: Cameron Clough <cameronjclough@gmail.com>
Date: Mon, 30 Jan 2023 22:37:54 -0800
Subject: [PATCH] Ford longitudinal control (#27161)

ford long
old-commit-hash: ef9deeb6eba3823982dadd485a3676441320226b
---
 selfdrive/car/ford/carcontroller.py | 19 ++++++++++++++++++-
 selfdrive/car/ford/fordcan.py       | 20 ++++++++++++++++++++
 selfdrive/car/ford/values.py        |  5 +++++
 3 files changed, 43 insertions(+), 1 deletion(-)

diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py
index 06da87bf34..cfff37f6d5 100644
--- a/selfdrive/car/ford/carcontroller.py
+++ b/selfdrive/car/ford/carcontroller.py
@@ -2,7 +2,8 @@ from cereal import car
 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_ui_msg, create_button_msg, create_lat_ctl_msg, create_lka_msg, create_lkas_ui_msg
+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
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
@@ -55,6 +56,22 @@ class CarController:
       can_sends.append(create_lka_msg(self.packer))
       can_sends.append(create_lat_ctl_msg(self.packer, CC.latActive, 0., 0., -apply_curvature, 0.))
 
+    ### longitudinal control ###
+    # send acc command at 50Hz
+    if self.CP.openpilotLongitudinalControl and (self.frame % CarControllerParams.ACC_CONTROL_STEP) == 0:
+      accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
+
+      precharge_brake = accel < -0.1
+      if accel > -0.5:
+        gas = accel
+        decel = False
+      else:
+        gas = -5.0
+        decel = True
+
+      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)
 
diff --git a/selfdrive/car/ford/fordcan.py b/selfdrive/car/ford/fordcan.py
index 9ddde80f87..d9c37426f8 100644
--- a/selfdrive/car/ford/fordcan.py
+++ b/selfdrive/car/ford/fordcan.py
@@ -55,6 +55,26 @@ 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_acc_command(packer, long_active: bool, gas: float, accel: float, precharge_brake: bool, decel: bool):
+  """
+  Creates a CAN message for the Ford ACC Command.
+
+  This command can be used to enable ACC, to set the ACC gas/brake/decel values
+  and to disable ACC.
+
+  Frequency is 50Hz.
+  """
+
+  values = {
+    "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
+    "AccBrkPrchg_B_Rq": 1 if precharge_brake else 0,  # Pre-charge brake request: 0=No, 1=Yes
+    "AccBrkDecel_B_Rq": 1 if decel else 0,            # Deceleration request: 0=Inactive, 1=Active
+  }
+  return packer.make_can_msg("ACCDATA", CANBUS.main, values)
+
+
 def create_lkas_ui_msg(packer, main_on: bool, enabled: bool, steer_alert: bool, hud_control, stock_values: dict):
   """
   Creates a CAN message for the Ford IPC IPMA/LKAS status.
diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py
index b93be61448..7b075f1547 100644
--- a/selfdrive/car/ford/values.py
+++ b/selfdrive/car/ford/values.py
@@ -14,6 +14,8 @@ Ecu = car.CarParams.Ecu
 class CarControllerParams:
   # Messages: Lane_Assist_Data1, LateralMotionControl
   STEER_STEP = 5
+  # Message: ACCDATA
+  ACC_CONTROL_STEP = 2
   # Message: IPMA_Data
   LKAS_UI_STEP = 100
   # Message: ACCDATA_3
@@ -29,6 +31,9 @@ class CarControllerParams:
   ANGLE_RATE_LIMIT_UP = AngleRateLimit(speed_bp=[5, 15, 25], angle_v=[0.005, 0.00056, 0.0002])
   ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_bp=[5, 15, 25], angle_v=[0.008, 0.00089, 0.00032])
 
+  ACCEL_MAX = 2.0               # m/s^s max acceleration
+  ACCEL_MIN = -3.5              # m/s^s max deceleration
+
   def __init__(self, CP):
     pass