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
2026-06-24 15:32:07 +08:00 · 2022-10-12 14:23:12 -07:00
parent 3c0904a18f
commit 80259f377f
5 changed files with 66 additions and 57 deletions
@@ -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 = clip(apply_path_angle, -0.4995, 0.5240)
-  apply_angle = math.degrees(apply_path_angle) * CarControllerParams.STEER_RATIO
+  apply_path_angle = math.radians(apply_angle) / CarControllerParams.LKAS_STEER_RATIO
+  apply_path_angle = clip(apply_path_angle, -0.5, 0.5235)
+  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))
-
-    # if stock lane centering is active or in standby, toggle it off
+      can_sends.append(fordcan.create_button_command(self.packer, CS.buttons_stock_values, resume=True, bus=CANBUS.main))
+    # 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.STEER_RATIO
+      path_angle = math.radians(apply_angle) / CarControllerParams.LKAS_STEER_RATIO

-      # ramp rate: 0=Slow, 1=Medium, 2=Fast, 3=Immediately
-      # TODO: try slower ramp speed when driver torque detected
-      ramp_type = 3
+      # set slower ramp type when small steering angle change
+      # 0=Slow, 1=Medium, 2=Fast, 3=Immediately
+      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
@@ -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),
@@ -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
-  PSCM lockout.
+  This command can apply "Lane Keeping Aid" manoeuvres, which are subject to the 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
-  for traffic jam assist and highway driving. It is not subject to the PSCM
-  lockout.
+  This command can apply "Lane Centering" manoeuvres: continuous lane centering for traffic jam
+  assist and highway driving. It is not subject to the PSCM lockout.

-  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.
+  Ford lane centering command uses a third order polynomial to describe the road centerline. The
+  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
-  warning and traffic jam assist status.
+  Creates a CAN message for the Ford IPC adaptive cruise, forward collision warning and traffic jam
+  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.

@@ -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

-
-EventName = car.CarEvent.EventName
+CarParams = car.CarParams


 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]
@@ -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
-  STEER_DRIVER_ALLOWANCE = 0.8
+  LKAS_STEER_RATIO = 2.75       # Approximate ratio between LatCtlPath_An_Actl and steering angle in radians
+                                # 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),
-}