From 42f2e392130e99352f4d0c9ac317e2dae13a9fcf Mon Sep 17 00:00:00 2001
From: Cameron Clough <cameronjclough@gmail.com>
Date: Wed, 26 Apr 2023 00:00:08 -0700
Subject: [PATCH] ford: correct messages frequencies (#28027)

* ford: correct messages frequencies

* fix STEER_STEP

* fix comment

* cleanup
---
 selfdrive/car/ford/carcontroller.py | 15 ++++++++-------
 selfdrive/car/ford/values.py        | 16 ++++++----------
 2 files changed, 14 insertions(+), 17 deletions(-)

diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py
index bff7e93be9..e5224857ca 100644
--- a/selfdrive/car/ford/carcontroller.py
+++ b/selfdrive/car/ford/carcontroller.py
@@ -43,7 +43,7 @@ class CarController:
       can_sends.append(create_button_msg(self.packer, CS.buttons_stock_values, tja_toggle=True))
 
     ### lateral control ###
-    # send steering commands at 20Hz
+    # send steer msg at 20Hz
     if (self.frame % CarControllerParams.STEER_STEP) == 0:
       if CC.latActive:
         # apply limits to curvature and clip to signal range
@@ -53,7 +53,6 @@ class CarController:
         apply_curvature = 0.
 
       self.apply_curvature_last = apply_curvature
-      can_sends.append(create_lka_msg(self.packer))
 
       if self.CP.carFingerprint in CANFD_CARS:
         # TODO: extended mode
@@ -63,8 +62,12 @@ class CarController:
       else:
         can_sends.append(create_lat_ctl_msg(self.packer, CC.latActive, 0., 0., -apply_curvature, 0.))
 
+    # send lka msg at 33Hz
+    if (self.frame % CarControllerParams.LKA_STEP) == 0:
+      can_sends.append(create_lka_msg(self.packer))
+
     ### longitudinal control ###
-    # send acc command at 50Hz
+    # send acc msg at 50Hz
     if self.CP.openpilotLongitudinalControl and (self.frame % CarControllerParams.ACC_CONTROL_STEP) == 0:
       accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
 
@@ -80,12 +83,10 @@ class CarController:
 
     ### ui ###
     send_ui = (self.main_on_last != main_on) or (self.lkas_enabled_last != CC.latActive) or (self.steer_alert_last != steer_alert)
-
-    # send lkas ui command at 1Hz or if ui state changes
+    # send lkas ui msg at 1Hz or if ui state changes
     if (self.frame % CarControllerParams.LKAS_UI_STEP) == 0 or send_ui:
       can_sends.append(create_lkas_ui_msg(self.packer, main_on, CC.latActive, steer_alert, hud_control, CS.lkas_status_stock_values))
-
-    # send acc ui command at 20Hz or if ui state changes
+    # send acc ui msg at 5Hz or if ui state changes
     if (self.frame % CarControllerParams.ACC_UI_STEP) == 0 or send_ui:
       can_sends.append(create_acc_ui_msg(self.packer, main_on, CC.latActive, hud_control, CS.acc_tja_status_stock_values))
 
diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py
index c3652443c2..60c6ff5430 100644
--- a/selfdrive/car/ford/values.py
+++ b/selfdrive/car/ford/values.py
@@ -12,16 +12,12 @@ 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
-  ACC_UI_STEP = 5
-  # Message: Steering_Data_FD1, but send twice as fast
-  BUTTONS_STEP = 10 / 2
+  STEER_STEP = 5        # LateralMotionControl, 20Hz
+  LKA_STEP = 3          # Lane_Assist_Data1, 33Hz
+  ACC_CONTROL_STEP = 2  # ACCDATA, 50Hz
+  LKAS_UI_STEP = 100    # IPMA_Data, 1Hz
+  ACC_UI_STEP = 20      # ACCDATA_3, 5Hz
+  BUTTONS_STEP = 5      # Steering_Data_FD1, 10Hz, but send twice as fast
 
   CURVATURE_MAX = 0.02  # Max curvature for steering command, m^-1
   STEER_DRIVER_ALLOWANCE = 1.0  # Driver intervention threshold, Nm