diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py index dfa5c8f5bd..2f15d6797e 100644 --- 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) diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index c4284f54c5..72bc06a8ac 100644 --- 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 diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py index 1c6f925672..c4fe80ab3e 100644 --- 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', diff --git a/selfdrive/car/tests/routes.py b/selfdrive/car/tests/routes.py index 3e365991bd..66f4ad2de5 100644 --- 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, diff --git a/selfdrive/car/tests/test_fw_fingerprint.py b/selfdrive/car/tests/test_fw_fingerprint.py index 0fbdb1a8b4..fd8c23263a 100755 --- 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, } } diff --git a/selfdrive/car/torque_data/override.yaml b/selfdrive/car/torque_data/override.yaml index 6d514ee483..bb92c2a75d 100644 --- 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] ###