From c7d3b28b93faa6c955fb24bc64031512ee985ee9 Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Thu, 18 May 2023 14:30:07 -0700
Subject: [PATCH] Ford: longitudinal clean up (#28231)

clean ups
---
 selfdrive/car/ford/carcontroller.py | 9 ++++-----
 selfdrive/car/ford/fordcan.py       | 4 +++-
 selfdrive/car/ford/values.py        | 2 ++
 3 files changed, 9 insertions(+), 6 deletions(-)

diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py
index 29b86f4ff66c45..d9a9ae6bc0902f 100644
--- a/selfdrive/car/ford/carcontroller.py
+++ b/selfdrive/car/ford/carcontroller.py
@@ -82,15 +82,14 @@ def update(self, CC, CS, now_nanos):
     ### longitudinal control ###
     # send acc msg at 50Hz
     if self.CP.openpilotLongitudinalControl and (self.frame % CarControllerParams.ACC_CONTROL_STEP) == 0:
+      # Both gas and accel are in m/s^2, accel is used solely for braking
       accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
-
       gas = accel
-      decel = accel < 0.0
-      if accel < -0.5:
-        gas = -5.0
+      if not CC.longActive or gas < CarControllerParams.MIN_GAS:
+        gas = CarControllerParams.INACTIVE_GAS
 
       stopping = CC.actuators.longControlState == LongCtrlState.stopping
-      can_sends.append(create_acc_msg(self.packer, CC.longActive, gas, accel, decel, stopping))
+      can_sends.append(create_acc_msg(self.packer, CC.longActive, gas, accel, stopping))
 
     ### 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 daf880aa8254e7..97a8c025d4c914 100644
--- a/selfdrive/car/ford/fordcan.py
+++ b/selfdrive/car/ford/fordcan.py
@@ -101,7 +101,7 @@ def create_lat_ctl2_msg(packer, mode: int, path_offset: float, path_angle: float
   return packer.make_can_msg("LateralMotionControl2", CANBUS.main, values)
 
 
-def create_acc_msg(packer, long_active: bool, gas: float, accel: float, decel: bool, stopping: bool):
+def create_acc_msg(packer, long_active: bool, gas: float, accel: float, stopping: bool):
   """
   Creates a CAN message for the Ford ACC Command.
 
@@ -111,11 +111,13 @@ def create_acc_msg(packer, long_active: bool, gas: float, accel: float, decel: b
   Frequency is 50Hz.
   """
 
+  decel = accel < 0 and long_active
   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
     "AccResumEnbl_B_Rq": 1 if long_active else 0,
+    # 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
     "AccStopStat_B_Rq": 1 if stopping else 0,
diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py
index 6a0639cee36819..6425179a27b108 100644
--- a/selfdrive/car/ford/values.py
+++ b/selfdrive/car/ford/values.py
@@ -31,6 +31,8 @@ class CarControllerParams:
 
   ACCEL_MAX = 2.0               # m/s^s max acceleration
   ACCEL_MIN = -3.5              # m/s^s max deceleration
+  MIN_GAS = -0.5
+  INACTIVE_GAS = -5.0
 
   def __init__(self, CP):
     pass