Ford: cleanup and fix button press (#26033)

* cleanup

* use Veh_V_ActlBrk for vEgoRaw

* remove unused CarState.yaw_data

* less resume spam

* set steering ramp rate

* match DBC range

* add LCA/TJA notes

selfdrive/car/ford/carcontroller.py

@@ -3,7 +3,7 @@ from cereal import car
 from common.numpy_fast import clip, interp
 from opendbc.can.packer import CANPacker
 from selfdrive.car.ford import fordcan
-from selfdrive.car.ford.values import CarControllerParams
+from selfdrive.car.ford.values import CANBUS, CarControllerParams
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 
@@ -16,9 +16,9 @@ def apply_ford_steer_angle_limits(apply_angle, apply_angle_last, vEgo):
   apply_angle = clip(apply_angle, (apply_angle_last - max_angle_diff), (apply_angle_last + max_angle_diff))
 
   # absolute limit (LatCtlPath_An_Actl)
-  apply_path_angle = math.radians(apply_angle) / CarControllerParams.STEER_RATIO
+  apply_path_angle = math.radians(apply_angle) / CarControllerParams.LKAS_STEER_RATIO
-  apply_path_angle = clip(apply_path_angle, -0.4995, 0.5240)
+  apply_path_angle = clip(apply_path_angle, -0.5, 0.5235)
-  apply_angle = math.degrees(apply_path_angle) * CarControllerParams.STEER_RATIO
+  apply_angle = math.degrees(apply_path_angle) * CarControllerParams.LKAS_STEER_RATIO
 
   return apply_angle
 
@@ -47,40 +47,46 @@ class CarController:
     ### acc buttons ###
     if CC.cruiseControl.cancel:
       can_sends.append(fordcan.create_button_command(self.packer, CS.buttons_stock_values, cancel=True))
-    elif CC.cruiseControl.resume:
+      can_sends.append(fordcan.create_button_command(self.packer, CS.buttons_stock_values, cancel=True, bus=CANBUS.main))
+    elif CC.cruiseControl.resume and (self.frame % CarControllerParams.BUTTONS_STEP) == 0:
       can_sends.append(fordcan.create_button_command(self.packer, CS.buttons_stock_values, resume=True))
-
+      can_sends.append(fordcan.create_button_command(self.packer, CS.buttons_stock_values, resume=True, bus=CANBUS.main))
-    # if stock lane centering is active or in standby, toggle it off
+    # if stock lane centering isn't off, send a button press to toggle it off
     # the stock system checks for steering pressed, and eventually disengages cruise control
-    if (self.frame % 200) == 0 and CS.acc_tja_status_stock_values["Tja_D_Stat"] != 0:
+    elif CS.acc_tja_status_stock_values["Tja_D_Stat"] != 0 and (self.frame % CarControllerParams.ACC_UI_STEP) == 0:
       can_sends.append(fordcan.create_button_command(self.packer, CS.buttons_stock_values, tja_toggle=True))
 
 
     ### lateral control ###
     if CC.latActive:
+      lca_rq = 1
       apply_angle = apply_ford_steer_angle_limits(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgo)
     else:
-      apply_angle = CS.out.steeringAngleDeg
+      lca_rq = 0
+      apply_angle = 0.
 
     # send steering commands at 20Hz
     if (self.frame % CarControllerParams.LKAS_STEER_STEP) == 0:
-      lca_rq = 1 if CC.latActive else 0
-
       # use LatCtlPath_An_Actl to actuate steering
-      # path angle is the car wheel angle, not the steering wheel angle
+      path_angle = math.radians(apply_angle) / CarControllerParams.LKAS_STEER_RATIO
-      path_angle = math.radians(apply_angle) / CarControllerParams.STEER_RATIO
+
-
+      # set slower ramp type when small steering angle change
-      # ramp rate: 0=Slow, 1=Medium, 2=Fast, 3=Immediately
+      # 0=Slow, 1=Medium, 2=Fast, 3=Immediately
-      # TODO: try slower ramp speed when driver torque detected
+      steer_change = abs(CS.out.steeringAngleDeg - actuators.steeringAngleDeg)
+      if steer_change < 2.0:
+        ramp_type = 0
+      elif steer_change < 4.0:
+        ramp_type = 1
+      elif steer_change < 6.0:
+        ramp_type = 2
+      else:
         ramp_type = 3
       precision = 1  # 0=Comfortable, 1=Precise (the stock system always uses comfortable)
 
-      offset_roll_compensation_curvature = clip(self.VM.calc_curvature(0, CS.out.vEgo, -CS.yaw_data["VehYaw_W_Actl"]), -0.02, 0.02094)
-
       self.apply_angle_last = apply_angle
-      can_sends.append(fordcan.create_lka_command(self.packer, apply_angle, 0))
+      can_sends.append(fordcan.create_lka_command(self.packer, 0, 0))
       can_sends.append(fordcan.create_tja_command(self.packer, lca_rq, ramp_type, precision,
-                                                  0, path_angle, 0, offset_roll_compensation_curvature))
+                                                  0, path_angle, 0, 0))
 
 
     ### ui ###
@@ -99,7 +105,7 @@ class CarController:
     self.steer_alert_last = steer_alert
 
     new_actuators = actuators.copy()
-    new_actuators.steeringAngleDeg = apply_angle
+    new_actuators.steeringAngleDeg = self.apply_angle_last
 
     self.frame += 1
     return new_actuators, can_sends

selfdrive/car/ford/carstate.py

@@ -20,7 +20,7 @@ class CarState(CarStateBase):
     ret = car.CarState.new_message()
 
     # car speed
-    ret.vEgoRaw = cp.vl["EngVehicleSpThrottle2"]["Veh_V_ActlEng"] * CV.KPH_TO_MS
+    ret.vEgoRaw = cp.vl["BrakeSysFeatures"]["Veh_V_ActlBrk"] * CV.KPH_TO_MS
     ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
     ret.yawRate = cp.vl["Yaw_Data_FD1"]["VehYaw_W_Actl"]
     ret.standstill = cp.vl["DesiredTorqBrk"]["VehStop_D_Stat"] == 1
@@ -85,8 +85,6 @@ class CarState(CarStateBase):
     # Stock values from IPMA so that we can retain some stock functionality
     self.acc_tja_status_stock_values = cp_cam.vl["ACCDATA_3"]
     self.lkas_status_stock_values = cp_cam.vl["IPMA_Data"]
-    # Use stock sensor values
-    self.yaw_data = cp.vl["Yaw_Data_FD1"]
 
     return ret
 
@@ -94,7 +92,7 @@ class CarState(CarStateBase):
   def get_can_parser(CP):
     signals = [
       # sig_name, sig_address
-      ("Veh_V_ActlEng", "EngVehicleSpThrottle2"),            # ABS vehicle speed (kph)
+      ("Veh_V_ActlBrk", "BrakeSysFeatures"),                 # ABS vehicle speed (kph)
       ("VehYaw_W_Actl", "Yaw_Data_FD1"),                     # ABS vehicle yaw rate (rad/s)
       ("VehStop_D_Stat", "DesiredTorqBrk"),                  # ABS vehicle stopped
       ("PrkBrkStatus", "DesiredTorqBrk"),                    # ABS park brake status
@@ -156,7 +154,7 @@ class CarState(CarStateBase):
 
     checks = [
       # sig_address, frequency
-      ("EngVehicleSpThrottle2", 50),
+      ("BrakeSysFeatures", 50),
       ("Yaw_Data_FD1", 100),
       ("DesiredTorqBrk", 50),
       ("EngVehicleSpThrottle", 100),

selfdrive/car/ford/fordcan.py

@@ -8,8 +8,7 @@ def create_lka_command(packer, angle_deg: float, curvature: float):
   """
   Creates a CAN message for the Ford LKAS Command.
 
-  This command can apply "Lane Keeping Aid" manoeuvres, which are subject to the
+  This command can apply "Lane Keeping Aid" manoeuvres, which are subject to the PSCM lockout.
-  PSCM lockout.
 
   Frequency is 20Hz.
   """
@@ -30,12 +29,20 @@ def create_tja_command(packer, lca_rq: int, ramp_type: int, precision: int, path
   """
   Creates a CAN message for the Ford TJA/LCA Command.
 
-  This command can apply "Lane Centering" manoeuvres: continuous lane centering
+  This command can apply "Lane Centering" manoeuvres: continuous lane centering for traffic jam
-  for traffic jam assist and highway driving. It is not subject to the PSCM
+  assist and highway driving. It is not subject to the PSCM lockout.
-  lockout.
 
-  The PSCM should be configured to accept TJA/LCA commands before these
+  Ford lane centering command uses a third order polynomial to describe the road centerline. The
-  commands will be processed. This can be done using tools such as Forscan.
+  polynomial is defined by the following coefficients:
+    c0: lateral offset between the vehicle and the centerline
+    c1: heading angle between the vehicle and the centerline
+    c2: curvature of the centerline
+    c3: rate of change of curvature of the centerline
+  As the PSCM combines this information with other sensor data, such as the vehicle's yaw rate and
+  speed, the steering angle cannot be easily controlled.
+
+  The PSCM should be configured to accept TJA/LCA commands before these commands will be processed.
+  This can be done using tools such as Forscan.
 
   Frequency is 20Hz.
   """
@@ -47,7 +54,7 @@ def create_tja_command(packer, lca_rq: int, ramp_type: int, precision: int, path
     "LatCtlRampType_D_Rq": ramp_type,                       # Ramp speed: 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3]
     "LatCtlPrecision_D_Rq": precision,                      # Precision: 0=Comfortable, 1=Precise, 2/3=NotUsed [0|3]
     "LatCtlPathOffst_L_Actl": path_offset,                  # Path offset [-5.12|5.11] meter
-    "LatCtlPath_An_Actl": path_angle,                       # Path angle [-0.4995|0.5240] radians
+    "LatCtlPath_An_Actl": path_angle,                       # Path angle [-0.5|0.5235] radians
     "LatCtlCurv_NoRate_Actl": curvature_rate,               # Curvature rate [-0.001024|0.00102375] 1/meter^2
     "LatCtlCurv_No_Actl": curvature,                        # Curvature [-0.02|0.02094] 1/meter
   }
@@ -108,8 +115,8 @@ def create_lkas_ui_command(packer, main_on: bool, enabled: bool, steer_alert: bo
 
 def create_acc_ui_command(packer, main_on: bool, enabled: bool, hud_control, stock_values: dict):
   """
-  Creates a CAN message for the Ford IPC adaptive cruise, forward collision
+  Creates a CAN message for the Ford IPC adaptive cruise, forward collision warning and traffic jam
-  warning and traffic jam assist status.
+  assist status.
 
   Stock functionality is maintained by passing through unmodified signals.
 
@@ -141,7 +148,7 @@ def create_acc_ui_command(packer, main_on: bool, enabled: bool, hud_control, sto
   return packer.make_can_msg("ACCDATA_3", CANBUS.main, values)
 
 
-def create_button_command(packer, stock_values: dict, cancel = False, resume = False, tja_toggle = False, bus = CANBUS.camera):
+def create_button_command(packer, stock_values: dict, cancel = False, resume = False, tja_toggle = False, bus: int = CANBUS.camera):
   """
   Creates a CAN message for the Ford SCCM buttons/switches.
 

selfdrive/car/ford/interface.py

@@ -5,8 +5,7 @@ from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness,
 from selfdrive.car.ford.values import CAR, Ecu, TransmissionType, GearShifter
 from selfdrive.car.interfaces import CarInterfaceBase
 
-
+CarParams = car.CarParams
-EventName = car.CarEvent.EventName
 
 
 class CarInterface(CarInterfaceBase):
@@ -19,10 +18,10 @@ class CarInterface(CarInterfaceBase):
 
     ret.carName = "ford"
     ret.dashcamOnly = True
-    ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.ford)]
+    ret.safetyConfigs = [get_safety_config(CarParams.SafetyModel.ford)]
 
     # Angle-based steering
-    ret.steerControlType = car.CarParams.SteerControlType.angle
+    ret.steerControlType = CarParams.SteerControlType.angle
     ret.steerActuatorDelay = 0.4
     ret.steerLimitTimer = 1.0
     tire_stiffness_factor = 1.0
@@ -43,7 +42,7 @@ class CarInterface(CarInterfaceBase):
       ret.mass = 1350 + STD_CARGO_KG
 
     else:
-      raise ValueError(f"Unsupported car: ${candidate}")
+      raise ValueError(f"Unsupported car: {candidate}")
 
     # Auto Transmission: 0x732 ECU or Gear_Shift_by_Wire_FD1
     found_ecus = [fw.ecu for fw in car_fw]

selfdrive/car/ford/values.py

@@ -1,4 +1,4 @@
-from collections import namedtuple
+from collections import defaultdict, namedtuple
 from dataclasses import dataclass
 from enum import Enum
 from typing import Dict, List, Union
@@ -22,19 +22,17 @@ class CarControllerParams:
   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_RATIO = 2.75
+  LKAS_STEER_RATIO = 2.75       # Approximate ratio between LatCtlPath_An_Actl and steering angle in radians
-  STEER_DRIVER_ALLOWANCE = 0.8
+                                # TODO: remove this once we understand how the EPS calculates the steering angle better
+  STEER_DRIVER_ALLOWANCE = 0.8  # Driver intervention threshold in Nm
 
   RATE_LIMIT_UP = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., .8, .15])
   RATE_LIMIT_DOWN = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., 3.5, 0.4])
 
 
-class RADAR:
-  DELPHI_ESR = 'ford_fusion_2018_adas'
-  DELPHI_MRR = 'FORD_CADS'
-
-
 class CANBUS:
   main = 0
   radar = 1
@@ -47,6 +45,14 @@ class CAR:
   FOCUS_MK4 = "FORD FOCUS 4TH GEN"
 
 
+class RADAR:
+  DELPHI_ESR = 'ford_fusion_2018_adas'
+  DELPHI_MRR = 'FORD_CADS'
+
+
+DBC: Dict[str, Dict[str, str]] = defaultdict(lambda: dbc_dict("ford_lincoln_base_pt", RADAR.DELPHI_MRR))
+
+
 @dataclass
 class FordCarInfo(CarInfo):
   package: str = "Co-Pilot360 Assist+"
@@ -143,10 +149,3 @@ FW_VERSIONS = {
     ],
   },
 }
-
-
-DBC = {
-  CAR.ESCAPE_MK4: dbc_dict('ford_lincoln_base_pt', RADAR.DELPHI_MRR),
-  CAR.EXPLORER_MK6: dbc_dict('ford_lincoln_base_pt', RADAR.DELPHI_MRR),
-  CAR.FOCUS_MK4: dbc_dict('ford_lincoln_base_pt', RADAR.DELPHI_MRR),
-}