wip

2024-04-27 13:48:05 -05:00
parent 2fbe9dbea1
commit 931db76fc6
432 changed files with 12973 additions and 3300 deletions
--- a/selfdrive/car/ford/carcontroller.py
+++ b/selfdrive/car/ford/carcontroller.py
@@ -1,9 +1,10 @@
 from cereal import car
-from openpilot.common.numpy_fast import clip
 from opendbc.can.packer import CANPacker
+from openpilot.common.numpy_fast import clip
 from openpilot.selfdrive.car import apply_std_steer_angle_limits
 from openpilot.selfdrive.car.ford import fordcan
-from openpilot.selfdrive.car.ford.values import CANFD_CAR, CarControllerParams
+from openpilot.selfdrive.car.ford.values import CarControllerParams, FordFlags
+from openpilot.selfdrive.car.interfaces import CarControllerBase
 from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX

 LongCtrlState = car.CarControl.Actuators.LongControlState
@@ -22,7 +23,7 @@ def apply_ford_curvature_limits(apply_curvature, apply_curvature_last, current_c
  return clip(apply_curvature, -CarControllerParams.CURVATURE_MAX, CarControllerParams.CURVATURE_MAX)


-class CarController:
+class CarController(CarControllerBase):
  def __init__(self, dbc_name, CP, VM):
    self.CP = CP
    self.VM = VM
@@ -34,6 +35,7 @@ class CarController:
    self.main_on_last = False
    self.lkas_enabled_last = False
    self.steer_alert_last = False
+    self.lead_distance_bars_last = None

  def update(self, CC, CS, now_nanos, frogpilot_variables):
    can_sends = []
@@ -69,10 +71,10 @@ class CarController:

      self.apply_curvature_last = apply_curvature

-      if self.CP.carFingerprint in CANFD_CAR:
+      if self.CP.flags & FordFlags.CANFD:
        # TODO: extended mode
        mode = 1 if CC.latActive else 0
-        counter = (self.frame // CarControllerParams.STEER_STEP) % 0xF
+        counter = (self.frame // CarControllerParams.STEER_STEP) % 0x10
        can_sends.append(fordcan.create_lat_ctl2_msg(self.packer, self.CAN, mode, 0., 0., -apply_curvature, 0., counter))
      else:
        can_sends.append(fordcan.create_lat_ctl_msg(self.packer, self.CAN, CC.latActive, 0., 0., -apply_curvature, 0.))
@@ -100,15 +102,19 @@ class CarController:
    # 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(fordcan.create_lkas_ui_msg(self.packer, self.CAN, main_on, CC.latActive, steer_alert, hud_control, CS.lkas_status_stock_values))
+
    # send acc ui msg at 5Hz or if ui state changes
+    if hud_control.leadDistanceBars != self.lead_distance_bars_last:
+      send_ui = True
    if (self.frame % CarControllerParams.ACC_UI_STEP) == 0 or send_ui:
      can_sends.append(fordcan.create_acc_ui_msg(self.packer, self.CAN, self.CP, main_on, CC.latActive,
-                                         fcw_alert, CS.out.cruiseState.standstill, hud_control,
-                                         CS.acc_tja_status_stock_values))
+                                                 fcw_alert, CS.out.cruiseState.standstill, hud_control,
+                                                 CS.acc_tja_status_stock_values))

    self.main_on_last = main_on
    self.lkas_enabled_last = CC.latActive
    self.steer_alert_last = steer_alert
+    self.lead_distance_bars_last = hud_control.leadDistanceBars

    new_actuators = actuators.copy()
    new_actuators.curvature = self.apply_curvature_last
--- a/selfdrive/car/ford/carstate.py
+++ b/selfdrive/car/ford/carstate.py
@@ -1,10 +1,10 @@
 from cereal import car
-from openpilot.common.conversions import Conversions as CV
 from opendbc.can.can_define import CANDefine
 from opendbc.can.parser import CANParser
-from openpilot.selfdrive.car.interfaces import CarStateBase
+from openpilot.common.conversions import Conversions as CV
 from openpilot.selfdrive.car.ford.fordcan import CanBus
-from openpilot.selfdrive.car.ford.values import CANFD_CAR, CarControllerParams, DBC
+from openpilot.selfdrive.car.ford.values import DBC, CarControllerParams, FordFlags
+from openpilot.selfdrive.car.interfaces import CarStateBase

 GearShifter = car.CarState.GearShifter
 TransmissionType = car.CarParams.TransmissionType
@@ -18,17 +18,13 @@ class CarState(CarStateBase):
      self.shifter_values = can_define.dv["Gear_Shift_by_Wire_FD1"]["TrnRng_D_RqGsm"]

    self.vehicle_sensors_valid = False
-    self.unsupported_platform = False
+
+    self.prev_distance_button = 0
+    self.distance_button = 0

  def update(self, cp, cp_cam, frogpilot_variables):
    ret = car.CarState.new_message()

-    # Ford Q3 hybrid variants experience a bug where a message from the PCM sends invalid checksums,
-    # this must be root-caused before enabling support. Ford Q4 hybrids do not have this problem.
-    # TrnAin_Tq_Actl and its quality flag are only set on ICE platform variants
-    self.unsupported_platform = (cp.vl["VehicleOperatingModes"]["TrnAinTq_D_Qf"] == 0 and
-                                 self.CP.carFingerprint not in CANFD_CAR)
-
    # Occasionally on startup, the ABS module recalibrates the steering pinion offset, so we need to block engagement
    # The vehicle usually recovers out of this state within a minute of normal driving
    self.vehicle_sensors_valid = cp.vl["SteeringPinion_Data"]["StePinCompAnEst_D_Qf"] == 3
@@ -56,7 +52,7 @@ class CarState(CarStateBase):
    ret.steerFaultPermanent = cp.vl["EPAS_INFO"]["EPAS_Failure"] in (2, 3)
    ret.espDisabled = cp.vl["Cluster_Info1_FD1"]["DrvSlipCtlMde_D_Rq"] != 0  # 0 is default mode

-    if self.CP.carFingerprint in CANFD_CAR:
+    if self.CP.flags & FordFlags.CANFD:
      # 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)

@@ -90,6 +86,8 @@ class CarState(CarStateBase):
    ret.rightBlinker = cp.vl["Steering_Data_FD1"]["TurnLghtSwtch_D_Stat"] == 2
    # TODO: block this going to the camera otherwise it will enable stock TJA
    ret.genericToggle = bool(cp.vl["Steering_Data_FD1"]["TjaButtnOnOffPress"])
+    self.prev_distance_button = self.distance_button
+    self.distance_button = cp.vl["Steering_Data_FD1"]["AccButtnGapTogglePress"]

    # lock info
    ret.doorOpen = any([cp.vl["BodyInfo_3_FD1"]["DrStatDrv_B_Actl"], cp.vl["BodyInfo_3_FD1"]["DrStatPsngr_B_Actl"],
@@ -98,7 +96,7 @@ class CarState(CarStateBase):

    # blindspot sensors
    if self.CP.enableBsm:
-      cp_bsm = cp_cam if self.CP.carFingerprint in CANFD_CAR else cp
+      cp_bsm = cp_cam if self.CP.flags & FordFlags.CANFD 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

@@ -108,6 +106,9 @@ class CarState(CarStateBase):
    self.acc_tja_status_stock_values = cp_cam.vl["ACCDATA_3"]
    self.lkas_status_stock_values = cp_cam.vl["IPMA_Data"]

+    self.lkas_previously_enabled = self.lkas_enabled
+    self.lkas_enabled = bool(cp.vl["Steering_Data_FD1"]["TjaButtnOnOffPress"])
+
    return ret

  @staticmethod
@@ -129,7 +130,7 @@ class CarState(CarStateBase):
      ("RCMStatusMessage2_FD1", 10),
    ]

-    if CP.carFingerprint in CANFD_CAR:
+    if CP.flags & FordFlags.CANFD:
      messages += [
        ("Lane_Assist_Data3_FD1", 33),
      ]
@@ -144,7 +145,7 @@ class CarState(CarStateBase):
        ("BCM_Lamp_Stat_FD1", 1),
      ]

-    if CP.enableBsm and CP.carFingerprint not in CANFD_CAR:
+    if CP.enableBsm and not (CP.flags & FordFlags.CANFD):
      messages += [
        ("Side_Detect_L_Stat", 5),
        ("Side_Detect_R_Stat", 5),
@@ -162,7 +163,7 @@ class CarState(CarStateBase):
      ("IPMA_Data", 1),
    ]

-    if CP.enableBsm and CP.carFingerprint in CANFD_CAR:
+    if CP.enableBsm and CP.flags & FordFlags.CANFD:
      messages += [
        ("Side_Detect_L_Stat", 5),
        ("Side_Detect_R_Stat", 5),
--- a/selfdrive/car/ford/fingerprints.py
+++ b/selfdrive/car/ford/fingerprints.py
@@ -8,16 +8,19 @@ FW_VERSIONS = {
    (Ecu.eps, 0x730, None): [
      b'LX6C-14D003-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
      b'LX6C-14D003-AK\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+      b'LX6C-14D003-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
    ],
    (Ecu.abs, 0x760, None): [
      b'LX6C-2D053-RD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
      b'LX6C-2D053-RE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+      b'LX6C-2D053-RF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
    ],
    (Ecu.fwdRadar, 0x764, None): [
      b'LB5T-14D049-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
    ],
    (Ecu.fwdCamera, 0x706, None): [
      b'M1PT-14F397-AC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+      b'M1PT-14F397-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
    ],
  },
  CAR.ESCAPE_MK4: {
@@ -82,6 +85,7 @@ FW_VERSIONS = {
      b'ML3T-14D049-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
    ],
    (Ecu.fwdCamera, 0x706, None): [
+      b'ML3T-14H102-ABR\x00\x00\x00\x00\x00\x00\x00\x00\x00',
      b'PJ6T-14H102-ABJ\x00\x00\x00\x00\x00\x00\x00\x00\x00',
    ],
  },
@@ -133,6 +137,7 @@ FW_VERSIONS = {
      b'NZ6C-2D053-AG\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
      b'PZ6C-2D053-ED\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
      b'PZ6C-2D053-EE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+      b'PZ6C-2D053-EF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
    ],
    (Ecu.fwdRadar, 0x764, None): [
      b'NZ6T-14D049-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
--- a/selfdrive/car/ford/fordcan.py
+++ b/selfdrive/car/ford/fordcan.py
@@ -212,7 +212,7 @@ def create_acc_ui_msg(packer, CAN: CanBus, CP, main_on: bool, enabled: bool, fcw
      "AccFllwMde_B_Dsply": 1 if hud_control.leadVisible else 0,  # Lead indicator
      "AccStopMde_B_Dsply": 1 if standstill else 0,
      "AccWarn_D_Dsply": 0,                                       # ACC warning
-      "AccTGap_D_Dsply": 4,                                       # Fixed time gap in UI
+      "AccTGap_D_Dsply": hud_control.leadDistanceBars,        # Time gap
    })

  # Forwards FCW alert from IPMA
--- a/selfdrive/car/ford/interface.py
+++ b/selfdrive/car/ford/interface.py
@@ -1,18 +1,20 @@
-from cereal import car
+from cereal import car, custom
 from panda import Panda
 from openpilot.common.conversions import Conversions as CV
-from openpilot.selfdrive.car import get_safety_config
+from openpilot.selfdrive.car import create_button_events, get_safety_config
 from openpilot.selfdrive.car.ford.fordcan import CanBus
-from openpilot.selfdrive.car.ford.values import CANFD_CAR, CAR, Ecu
+from openpilot.selfdrive.car.ford.values import Ecu, FordFlags
 from openpilot.selfdrive.car.interfaces import CarInterfaceBase

+ButtonType = car.CarState.ButtonEvent.Type
 TransmissionType = car.CarParams.TransmissionType
 GearShifter = car.CarState.GearShifter
+FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type


 class CarInterface(CarInterfaceBase):
  @staticmethod
-  def _get_params(ret, params, candidate, fingerprint, car_fw, experimental_long, docs):
+  def _get_params(ret, params, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs):
    ret.carName = "ford"
    ret.dashcamOnly = False

@@ -34,56 +36,11 @@ class CarInterface(CarInterfaceBase):
    ret.experimentalLongitudinalAvailable = True
    if experimental_long:
      ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_FORD_LONG_CONTROL
-      ret.openpilotLongitudinalControl = True and not params.get_bool("DisableOpenpilotLongitudinal")
+      ret.openpilotLongitudinalControl = True

-    if candidate in CANFD_CAR:
+    if ret.flags & FordFlags.CANFD:
      ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_FORD_CANFD

-    if candidate == CAR.BRONCO_SPORT_MK1:
-      ret.wheelbase = 2.67
-      ret.steerRatio = 17.7
-      ret.mass = 1625
-
-    elif candidate == CAR.ESCAPE_MK4:
-      ret.wheelbase = 2.71
-      ret.steerRatio = 16.7
-      ret.mass = 1750
-
-    elif candidate == CAR.EXPLORER_MK6:
-      ret.wheelbase = 3.025
-      ret.steerRatio = 16.8
-      ret.mass = 2050
-
-    elif candidate == CAR.F_150_MK14:
-      # required trim only on SuperCrew
-      ret.wheelbase = 3.69
-      ret.steerRatio = 17.0
-      ret.mass = 2000
-
-    elif candidate == CAR.F_150_LIGHTNING_MK1:
-      # required trim only on SuperCrew
-      ret.wheelbase = 3.70
-      ret.steerRatio = 16.9
-      ret.mass = 2948
-
-    elif candidate == CAR.MUSTANG_MACH_E_MK1:
-      ret.wheelbase = 2.984
-      ret.steerRatio = 17.0  # guess
-      ret.mass = 2200
-
-    elif candidate == CAR.FOCUS_MK4:
-      ret.wheelbase = 2.7
-      ret.steerRatio = 15.0
-      ret.mass = 1350
-
-    elif candidate == CAR.MAVERICK_MK1:
-      ret.wheelbase = 3.076
-      ret.steerRatio = 17.0
-      ret.mass = 1650
-
-    else:
-      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]
    if Ecu.shiftByWire in found_ecus or 0x5A in fingerprint[CAN.main] or docs:
@@ -106,11 +63,14 @@ class CarInterface(CarInterfaceBase):
  def _update(self, c, frogpilot_variables):
    ret = self.CS.update(self.cp, self.cp_cam, frogpilot_variables)

-    events = self.create_common_events(ret, frogpilot_variables, extra_gears=[GearShifter.manumatic])
+    ret.buttonEvents = [
+      *create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}),
+      *create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}),
+    ]
+
+    events = self.create_common_events(ret, extra_gears=[GearShifter.manumatic])
    if not self.CS.vehicle_sensors_valid:
      events.add(car.CarEvent.EventName.vehicleSensorsInvalid)
-    if self.CS.unsupported_platform:
-      events.add(car.CarEvent.EventName.startupNoControl)

    ret.events = events.to_msg()

--- a/selfdrive/car/ford/tests/init.py
+++ b/selfdrive/car/ford/tests/init.py
--- a/selfdrive/car/ford/tests/test_ford.py
+++ b/selfdrive/car/ford/tests/test_ford.py
@@ -0,0 +1,82 @@
+#!/usr/bin/env python3
+import unittest
+from parameterized import parameterized
+from collections.abc import Iterable
+
+import capnp
+
+from cereal import car
+from openpilot.selfdrive.car.ford.values import FW_QUERY_CONFIG
+from openpilot.selfdrive.car.ford.fingerprints import FW_VERSIONS
+
+Ecu = car.CarParams.Ecu
+
+
+ECU_ADDRESSES = {
+  Ecu.eps: 0x730,          # Power Steering Control Module (PSCM)
+  Ecu.abs: 0x760,          # Anti-Lock Brake System (ABS)
+  Ecu.fwdRadar: 0x764,     # Cruise Control Module (CCM)
+  Ecu.fwdCamera: 0x706,    # Image Processing Module A (IPMA)
+  Ecu.engine: 0x7E0,       # Powertrain Control Module (PCM)
+  Ecu.shiftByWire: 0x732,  # Gear Shift Module (GSM)
+  Ecu.debug: 0x7D0,        # Accessory Protocol Interface Module (APIM)
+}
+
+
+ECU_FW_CORE = {
+  Ecu.eps: [
+    b"14D003",
+  ],
+  Ecu.abs: [
+    b"2D053",
+  ],
+  Ecu.fwdRadar: [
+    b"14D049",
+  ],
+  Ecu.fwdCamera: [
+    b"14F397",  # Ford Q3
+    b"14H102",  # Ford Q4
+  ],
+  Ecu.engine: [
+    b"14C204",
+  ],
+}
+
+
+class TestFordFW(unittest.TestCase):
+  def test_fw_query_config(self):
+    for (ecu, addr, subaddr) in FW_QUERY_CONFIG.extra_ecus:
+      self.assertIn(ecu, ECU_ADDRESSES, "Unknown ECU")
+      self.assertEqual(addr, ECU_ADDRESSES[ecu], "ECU address mismatch")
+      self.assertIsNone(subaddr, "Unexpected ECU subaddress")
+
+  @parameterized.expand(FW_VERSIONS.items())
+  def test_fw_versions(self, car_model: str, fw_versions: dict[tuple[capnp.lib.capnp._EnumModule, int, int | None], Iterable[bytes]]):
+    for (ecu, addr, subaddr), fws in fw_versions.items():
+      self.assertIn(ecu, ECU_FW_CORE, "Unexpected ECU")
+      self.assertEqual(addr, ECU_ADDRESSES[ecu], "ECU address mismatch")
+      self.assertIsNone(subaddr, "Unexpected ECU subaddress")
+
+      # Software part number takes the form: PREFIX-CORE-SUFFIX
+      # Prefix changes based on the family of part. It includes the model year
+      #   and likely the platform.
+      # Core identifies the type of the item (e.g. 14D003 = PSCM, 14C204 = PCM).
+      # Suffix specifies the version of the part. -AA would be followed by -AB.
+      #   Small increments in the suffix are usually compatible.
+      # Details: https://forscan.org/forum/viewtopic.php?p=70008#p70008
+      for fw in fws:
+        self.assertEqual(len(fw), 24, "Expected ECU response to be 24 bytes")
+
+        # TODO: parse with regex, don't need detailed error message
+        fw_parts = fw.rstrip(b'\x00').split(b'-')
+        self.assertEqual(len(fw_parts), 3, "Expected FW to be in format: prefix-core-suffix")
+
+        prefix, core, suffix = fw_parts
+        self.assertEqual(len(prefix), 4, "Expected FW prefix to be 4 characters")
+        self.assertIn(len(core), (5, 6), "Expected FW core to be 5-6 characters")
+        self.assertIn(core, ECU_FW_CORE[ecu], f"Unexpected FW core for {ecu}")
+        self.assertIn(len(suffix), (2, 3), "Expected FW suffix to be 2-3 characters")
+
+
+if __name__ == "__main__":
+  unittest.main()
--- a/selfdrive/car/ford/values.py
+++ b/selfdrive/car/ford/values.py
@@ -1,13 +1,13 @@
-from collections import defaultdict
-from dataclasses import dataclass
-from enum import Enum, StrEnum
-from typing import Dict, List, Union
+import copy
+from dataclasses import dataclass, field, replace
+from enum import Enum, IntFlag

+import panda.python.uds as uds
 from cereal import car
-from openpilot.selfdrive.car import AngleRateLimit, dbc_dict
-from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarInfo, CarParts, Column, \
+from openpilot.selfdrive.car import AngleRateLimit, CarSpecs, dbc_dict, DbcDict, PlatformConfig, Platforms
+from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Column, \
                                                     Device
-from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
+from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries, p16

 Ecu = car.CarParams.Ecu

@@ -41,18 +41,9 @@ class CarControllerParams:
    pass


-class CAR(StrEnum):
-  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"
-  F_150_LIGHTNING_MK1 = "FORD F-150 LIGHTNING 1ST GEN"
-  MUSTANG_MACH_E_MK1 = "FORD MUSTANG MACH-E 1ST GEN"
-
-
-CANFD_CAR = {CAR.F_150_MK14, CAR.F_150_LIGHTNING_MK1, CAR.MUSTANG_MACH_E_MK1}
+class FordFlags(IntFlag):
+  # Static flags
+  CANFD = 1


 class RADAR:
@@ -60,14 +51,6 @@ class RADAR:
  DELPHI_MRR = 'FORD_CADS'


-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)
-DBC[CAR.F_150_LIGHTNING_MK1] = dbc_dict("ford_lincoln_base_pt", None)
-DBC[CAR.MUSTANG_MACH_E_MK1] = dbc_dict("ford_lincoln_base_pt", None)
-
-
 class Footnote(Enum):
  FOCUS = CarFootnote(
    "Refers only to the Focus Mk4 (C519) available in Europe/China/Taiwan/Australasia, not the Focus Mk3 (C346) in " +
@@ -77,36 +60,120 @@ class Footnote(Enum):


@dataclass
-class FordCarInfo(CarInfo):
+class FordCarDocs(CarDocs):
  package: str = "Co-Pilot360 Assist+"
+  hybrid: bool = False
+  plug_in_hybrid: bool = False

  def init_make(self, CP: car.CarParams):
-    harness = CarHarness.ford_q4 if CP.carFingerprint in CANFD_CAR else CarHarness.ford_q3
-    if CP.carFingerprint in (CAR.BRONCO_SPORT_MK1, CAR.MAVERICK_MK1, CAR.F_150_MK14):
+    harness = CarHarness.ford_q4 if CP.flags & FordFlags.CANFD else CarHarness.ford_q3
+    if CP.carFingerprint in (CAR.BRONCO_SPORT_MK1, CAR.MAVERICK_MK1, CAR.F_150_MK14, CAR.F_150_LIGHTNING_MK1):
      self.car_parts = CarParts([Device.threex_angled_mount, harness])
    else:
      self.car_parts = CarParts([Device.threex, harness])


-CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = {
-  CAR.BRONCO_SPORT_MK1: FordCarInfo("Ford Bronco Sport 2021-22"),
-  CAR.ESCAPE_MK4: [
-    FordCarInfo("Ford Escape 2020-22"),
-    FordCarInfo("Ford Kuga 2020-22", "Adaptive Cruise Control with Lane Centering"),
-  ],
-  CAR.EXPLORER_MK6: [
-    FordCarInfo("Ford Explorer 2020-23"),
-    FordCarInfo("Lincoln Aviator 2020-21", "Co-Pilot360 Plus"),
-  ],
-  CAR.F_150_MK14: FordCarInfo("Ford F-150 2023", "Co-Pilot360 Active 2.0"),
-  CAR.F_150_LIGHTNING_MK1: FordCarInfo("Ford F-150 Lightning 2021-23", "Co-Pilot360 Active 2.0"),
-  CAR.MUSTANG_MACH_E_MK1: FordCarInfo("Ford Mustang Mach-E 2021-23", "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", "LARIAT Luxury"),
-    FordCarInfo("Ford Maverick 2023", "Co-Pilot360 Assist"),
-  ],
-}
+@dataclass
+class FordPlatformConfig(PlatformConfig):
+  dbc_dict: DbcDict = field(default_factory=lambda: dbc_dict('ford_lincoln_base_pt', RADAR.DELPHI_MRR))
+
+  def init(self):
+    for car_docs in list(self.car_docs):
+      if car_docs.hybrid:
+        name = f"{car_docs.make} {car_docs.model} Hybrid {car_docs.years}"
+        self.car_docs.append(replace(copy.deepcopy(car_docs), name=name))
+      if car_docs.plug_in_hybrid:
+        name = f"{car_docs.make} {car_docs.model} Plug-in Hybrid {car_docs.years}"
+        self.car_docs.append(replace(copy.deepcopy(car_docs), name=name))
+
+
+@dataclass
+class FordCANFDPlatformConfig(FordPlatformConfig):
+  dbc_dict: DbcDict = field(default_factory=lambda: dbc_dict('ford_lincoln_base_pt', None))
+
+  def init(self):
+    super().init()
+    self.flags |= FordFlags.CANFD
+
+
+class CAR(Platforms):
+  BRONCO_SPORT_MK1 = FordPlatformConfig(
+    "FORD BRONCO SPORT 1ST GEN",
+    [FordCarDocs("Ford Bronco Sport 2021-23")],
+    CarSpecs(mass=1625, wheelbase=2.67, steerRatio=17.7),
+  )
+  ESCAPE_MK4 = FordPlatformConfig(
+    "FORD ESCAPE 4TH GEN",
+    [
+      FordCarDocs("Ford Escape 2020-22", hybrid=True, plug_in_hybrid=True),
+      FordCarDocs("Ford Kuga 2020-22", "Adaptive Cruise Control with Lane Centering", hybrid=True, plug_in_hybrid=True),
+    ],
+    CarSpecs(mass=1750, wheelbase=2.71, steerRatio=16.7),
+  )
+  EXPLORER_MK6 = FordPlatformConfig(
+    "FORD EXPLORER 6TH GEN",
+    [
+      FordCarDocs("Ford Explorer 2020-23", hybrid=True),  # Hybrid: Limited and Platinum only
+      FordCarDocs("Lincoln Aviator 2020-23", "Co-Pilot360 Plus", plug_in_hybrid=True),  # Hybrid: Grand Touring only
+    ],
+    CarSpecs(mass=2050, wheelbase=3.025, steerRatio=16.8),
+  )
+  F_150_MK14 = FordCANFDPlatformConfig(
+    "FORD F-150 14TH GEN",
+    [FordCarDocs("Ford F-150 2022-23", "Co-Pilot360 Active 2.0", hybrid=True)],
+    CarSpecs(mass=2000, wheelbase=3.69, steerRatio=17.0),
+  )
+  F_150_LIGHTNING_MK1 = FordCANFDPlatformConfig(
+    "FORD F-150 LIGHTNING 1ST GEN",
+    [FordCarDocs("Ford F-150 Lightning 2021-23", "Co-Pilot360 Active 2.0")],
+    CarSpecs(mass=2948, wheelbase=3.70, steerRatio=16.9),
+  )
+  FOCUS_MK4 = FordPlatformConfig(
+    "FORD FOCUS 4TH GEN",
+    [FordCarDocs("Ford Focus 2018", "Adaptive Cruise Control with Lane Centering", footnotes=[Footnote.FOCUS], hybrid=True)],  # mHEV only
+    CarSpecs(mass=1350, wheelbase=2.7, steerRatio=15.0),
+  )
+  MAVERICK_MK1 = FordPlatformConfig(
+    "FORD MAVERICK 1ST GEN",
+    [
+      FordCarDocs("Ford Maverick 2022", "LARIAT Luxury", hybrid=True),
+      FordCarDocs("Ford Maverick 2023-24", "Co-Pilot360 Assist", hybrid=True),
+    ],
+    CarSpecs(mass=1650, wheelbase=3.076, steerRatio=17.0),
+  )
+  MUSTANG_MACH_E_MK1 = FordCANFDPlatformConfig(
+    "FORD MUSTANG MACH-E 1ST GEN",
+    [FordCarDocs("Ford Mustang Mach-E 2021-23", "Co-Pilot360 Active 2.0")],
+    CarSpecs(mass=2200, wheelbase=2.984, steerRatio=17.0),  # TODO: check steer ratio
+  )
+
+
+DATA_IDENTIFIER_FORD_ASBUILT = 0xDE00
+
+ASBUILT_BLOCKS: list[tuple[int, list]] = [
+  (1, [Ecu.debug, Ecu.fwdCamera, Ecu.eps]),
+  (2, [Ecu.abs, Ecu.debug, Ecu.eps]),
+  (3, [Ecu.abs, Ecu.debug, Ecu.eps]),
+  (4, [Ecu.debug, Ecu.fwdCamera]),
+  (5, [Ecu.debug]),
+  (6, [Ecu.debug]),
+  (7, [Ecu.debug]),
+  (8, [Ecu.debug]),
+  (9, [Ecu.debug]),
+  (16, [Ecu.debug, Ecu.fwdCamera]),
+  (18, [Ecu.fwdCamera]),
+  (20, [Ecu.fwdCamera]),
+  (21, [Ecu.fwdCamera]),
+]
+
+
+def ford_asbuilt_block_request(block_id: int):
+  return bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + p16(DATA_IDENTIFIER_FORD_ASBUILT + block_id - 1)
+
+
+def ford_asbuilt_block_response(block_id: int):
+  return bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + p16(DATA_IDENTIFIER_FORD_ASBUILT + block_id - 1)
+

 FW_QUERY_CONFIG = FwQueryConfig(
  requests=[
@@ -115,13 +182,30 @@ 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.abs, Ecu.debug, Ecu.engine, Ecu.eps, Ecu.fwdCamera, Ecu.fwdRadar, Ecu.shiftByWire],
+      logging=True,
+    ),
+    Request(
+      [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST],
+      [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE],
+      whitelist_ecus=[Ecu.abs, Ecu.debug, Ecu.engine, Ecu.eps, Ecu.fwdCamera, Ecu.fwdRadar, Ecu.shiftByWire],
      bus=0,
      auxiliary=True,
    ),
+    *[Request(
+      [StdQueries.TESTER_PRESENT_REQUEST, ford_asbuilt_block_request(block_id)],
+      [StdQueries.TESTER_PRESENT_RESPONSE, ford_asbuilt_block_response(block_id)],
+      whitelist_ecus=ecus,
+      bus=0,
+      logging=True,
+    ) for block_id, ecus in ASBUILT_BLOCKS],
  ],
  extra_ecus=[
-    # We are unlikely to get a response from the PCM from behind the gateway
-    (Ecu.engine, 0x7e0, None),
-    (Ecu.shiftByWire, 0x732, None),
+    (Ecu.engine, 0x7e0, None),        # Powertrain Control Module
+                                      # Note: We are unlikely to get a response from behind the gateway
+    (Ecu.shiftByWire, 0x732, None),   # Gear Shift Module
+    (Ecu.debug, 0x7d0, None),         # Accessory Protocol Interface Module
  ],
 )
+
+DBC = CAR.create_dbc_map()