Ford: add F-150 2023 support (dashcam only) (#28974)

2 years ago · 17e71cdf44
parent 1afdca2ef1
commit 17e71cdf44
6 changed files with 69 additions and 11 deletions
--- a/selfdrive/car/ford/carstate.py
+++ b/selfdrive/car/ford/carstate.py
@ -4,7 +4,7 @@ from opendbc.can.can_define import CANDefine
 from opendbc.can.parser import CANParser
 from selfdrive.car.interfaces import CarStateBase
 from selfdrive.car.ford.fordcan import CanBus
-from selfdrive.car.ford.values import DBC, CarControllerParams
+from selfdrive.car.ford.values import CANFD_CARS, CarControllerParams, DBC

 GearShifter = car.CarState.GearShifter
 TransmissionType = car.CarParams.TransmissionType
@ -55,6 +55,10 @@ class CarState(CarStateBase):
    ret.steerFaultPermanent = cp.vl["EPAS_INFO"]["EPAS_Failure"] in (2, 3)
    # ret.espDisabled = False  # TODO: find traction control signal

+    if self.CP.carFingerprint in CANFD_CARS:
+      # this signal is always 0 on non-CAN FD cars
+      ret.steerFaultTemporary |= cp.vl["Lane_Assist_Data3_FD1"]["LatCtlSte_D_Stat"] not in (1, 2, 3)
+
    # cruise state
    ret.cruiseState.speed = cp.vl["EngBrakeData"]["Veh_V_DsplyCcSet"] * CV.MPH_TO_MS
    ret.cruiseState.enabled = cp.vl["EngBrakeData"]["CcStat_D_Actl"] in (4, 5)
@ -93,8 +97,9 @@ class CarState(CarStateBase):

    # blindspot sensors
    if self.CP.enableBsm:
-      ret.leftBlindspot = cp.vl["Side_Detect_L_Stat"]["SodDetctLeft_D_Stat"] != 0
-      ret.rightBlindspot = cp.vl["Side_Detect_R_Stat"]["SodDetctRight_D_Stat"] != 0
+      cp_bsm = cp_cam if self.CP.carFingerprint in CANFD_CARS else cp
+      ret.leftBlindspot = cp_bsm.vl["Side_Detect_L_Stat"]["SodDetctLeft_D_Stat"] != 0
+      ret.rightBlindspot = cp_bsm.vl["Side_Detect_R_Stat"]["SodDetctRight_D_Stat"] != 0

    # Stock steering buttons so that we can passthru blinkers etc.
    self.buttons_stock_values = cp.vl["Steering_Data_FD1"]
@ -181,12 +186,19 @@ class CarState(CarStateBase):
      ("Cluster_Info1_FD1", 10),
      ("SteeringPinion_Data", 100),
      ("EPAS_INFO", 50),
-      ("Lane_Assist_Data3_FD1", 33),
      ("Steering_Data_FD1", 10),
      ("BodyInfo_3_FD1", 2),
      ("RCMStatusMessage2_FD1", 10),
    ]

+    if CP.carFingerprint in CANFD_CARS:
+      signals += [
+        ("LatCtlSte_D_Stat", "Lane_Assist_Data3_FD1"),       # PSCM lateral control status
+      ]
+      checks += [
+        ("Lane_Assist_Data3_FD1", 33),
+      ]
+
    if CP.transmissionType == TransmissionType.automatic:
      signals += [
        ("TrnRng_D_RqGsm", "Gear_Shift_by_Wire_FD1"),        # GWM transmission gear position
@ -204,7 +216,7 @@ class CarState(CarStateBase):
        ("BCM_Lamp_Stat_FD1", 1),
      ]

-    if CP.enableBsm:
+    if CP.enableBsm and CP.carFingerprint not in CANFD_CARS:
      signals += [
        ("SodDetctLeft_D_Stat", "Side_Detect_L_Stat"),       # Blindspot sensor, left
        ("SodDetctRight_D_Stat", "Side_Detect_R_Stat"),      # Blindspot sensor, right
@ -274,4 +286,14 @@ class CarState(CarStateBase):
      ("IPMA_Data", 1),
    ]

+    if CP.enableBsm and CP.carFingerprint in CANFD_CARS:
+      signals += [
+        ("SodDetctLeft_D_Stat", "Side_Detect_L_Stat"),       # Blindspot sensor, left
+        ("SodDetctRight_D_Stat", "Side_Detect_R_Stat"),      # Blindspot sensor, right
+      ]
+      checks += [
+        ("Side_Detect_L_Stat", 5),
+        ("Side_Detect_R_Stat", 5),
+      ]
+
    return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CanBus(CP).camera)
--- a/selfdrive/car/ford/interface.py
+++ b/selfdrive/car/ford/interface.py
@ -15,6 +15,7 @@ class CarInterface(CarInterfaceBase):
  @staticmethod
  def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
    ret.carName = "ford"
+    ret.dashcamOnly = candidate in {CAR.F_150_MK14}

    ret.radarUnavailable = True
    ret.steerControlType = car.CarParams.SteerControlType.angle
@ -50,6 +51,12 @@ class CarInterface(CarInterfaceBase):
      ret.steerRatio = 16.8
      ret.mass = 2050 + STD_CARGO_KG

+    elif candidate == CAR.F_150_MK14:
+      # required trim only on SuperCrew
+      ret.wheelbase = 3.69
+      ret.steerRatio = 17.0
+      ret.mass = 2000 + STD_CARGO_KG
+
    elif candidate == CAR.FOCUS_MK4:
      ret.wheelbase = 2.7
      ret.steerRatio = 15.0
--- a/selfdrive/car/ford/values.py
+++ b/selfdrive/car/ford/values.py
@ -1,7 +1,7 @@
 from collections import defaultdict
 from dataclasses import dataclass, field
 from enum import Enum
-from typing import Dict, List, Set, Union
+from typing import Dict, List, Union

 from cereal import car
 from selfdrive.car import AngleRateLimit, dbc_dict
@ -44,11 +44,12 @@ class CAR:
  BRONCO_SPORT_MK1 = "FORD BRONCO SPORT 1ST GEN"
  ESCAPE_MK4 = "FORD ESCAPE 4TH GEN"
  EXPLORER_MK6 = "FORD EXPLORER 6TH GEN"
+  F_150_MK14 = "FORD F-150 14TH GEN"
  FOCUS_MK4 = "FORD FOCUS 4TH GEN"
  MAVERICK_MK1 = "FORD MAVERICK 1ST GEN"


-CANFD_CARS: Set[str] = set()
+CANFD_CARS = {CAR.F_150_MK14}


 class RADAR:
@ -58,6 +59,9 @@ class RADAR:

 DBC: Dict[str, Dict[str, str]] = defaultdict(lambda: dbc_dict("ford_lincoln_base_pt", RADAR.DELPHI_MRR))

+# F-150 radar is not yet supported
+DBC[CAR.F_150_MK14] = dbc_dict("ford_lincoln_base_pt", None)
+

 class Footnote(Enum):
  FOCUS = CarFootnote(
@ -87,6 +91,7 @@ CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = {
    FordCarInfo("Ford Explorer 2020-22"),
    FordCarInfo("Lincoln Aviator 2020-21", "Co-Pilot360 Plus"),
  ],
+  CAR.F_150_MK14: FordCarInfo("Ford F-150 2023", "Co-Pilot360 Active 2.0"),
  CAR.FOCUS_MK4: FordCarInfo("Ford Focus 2018", "Adaptive Cruise Control with Lane Centering", footnotes=[Footnote.FOCUS]),
  CAR.MAVERICK_MK1: FordCarInfo("Ford Maverick 2022-23", "Co-Pilot360 Assist"),
 }
@ -96,13 +101,17 @@ FW_QUERY_CONFIG = FwQueryConfig(
    Request(
      [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST],
      [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE],
-      whitelist_ecus=[Ecu.engine],
    ),
    Request(
      [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST],
      [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE],
      bus=0,
-      whitelist_ecus=[Ecu.eps, Ecu.abs, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.shiftByWire],
+    ),
+    Request(
+      [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST],
+      [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE],
+      bus=0,
+      auxiliary=True,
    ),
  ],
  extra_ecus=[
@ -197,6 +206,23 @@ FW_VERSIONS = {
      b'NB5A-14C204-HB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
    ],
  },
+  CAR.F_150_MK14: {
+    (Ecu.eps, 0x730, None): [
+      b'ML3V-14D003-BC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+    ],
+    (Ecu.abs, 0x760, None): [
+      b'PL34-2D053-CA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+    ],
+    (Ecu.fwdRadar, 0x764, None): [
+      b'ML3T-14D049-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+    ],
+    (Ecu.fwdCamera, 0x706, None): [
+      b'PJ6T-14H102-ABJ\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+    ],
+    (Ecu.engine, 0x7E0, None): [
+      b'PL3A-14C204-BRB\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+    ],
+  },
  CAR.FOCUS_MK4: {
    (Ecu.eps, 0x730, None): [
      b'JX6C-14D003-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
--- a/selfdrive/car/tests/routes.py
+++ b/selfdrive/car/tests/routes.py
@ -16,6 +16,7 @@ from selfdrive.car.body.values import CAR as COMMA

 # TODO: add routes for these cars
 non_tested_cars = [
+  FORD.F_150_MK14,
  GM.CADILLAC_ATS,
  GM.HOLDEN_ASTRA,
  GM.MALIBU,
--- a/selfdrive/car/tests/test_fw_fingerprint.py
+++ b/selfdrive/car/tests/test_fw_fingerprint.py
@ -220,12 +220,12 @@ class TestFwFingerprintTiming(unittest.TestCase):
    print(f'get_vin, query time={vin_time / self.N} seconds')

  def test_fw_query_timing(self):
-    total_ref_time = 6.2
+    total_ref_time = 6.7
    brand_ref_times = {
      1: {
        'body': 0.1,
        'chrysler': 0.3,
-        'ford': 0.2,
+        'ford': 0.3,
        'honda': 0.5,
        'hyundai': 0.7,
        'mazda': 0.2,
@ -236,6 +236,7 @@ class TestFwFingerprintTiming(unittest.TestCase):
        'volkswagen': 0.2,
      },
      2: {
+        'ford': 0.4,
        'hyundai': 1.1,
      }
    }
--- a/selfdrive/car/torque_data/override.yaml
+++ b/selfdrive/car/torque_data/override.yaml
@ -19,6 +19,7 @@ TESLA AP2 MODEL S: [.nan, 2.5, .nan]
 FORD BRONCO SPORT 1ST GEN: [.nan, 1.5, .nan]
 FORD ESCAPE 4TH GEN: [.nan, 1.5, .nan]
 FORD EXPLORER 6TH GEN: [.nan, 1.5, .nan]
+FORD F-150 14TH GEN: [.nan, 1.5, .nan]
 FORD FOCUS 4TH GEN: [.nan, 1.5, .nan]
 FORD MAVERICK 1ST GEN: [.nan, 1.5, .nan]
 ###