brianhansonxyz/clearpilot
1
0
Fork 0
Code Issues Pull Requests Actions 4 Packages Projects Releases Wiki Activity

clearpilot: initial commit of full source

Browse Source
This commit is contained in:
Brian Hanson
2026-04-11 06:25:25 +00:00
commit e2a0c1894a
3383 changed files with 834683 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

0
selfdrive/car/ford/__init__.py Executable file
View File

123
selfdrive/car/ford/carcontroller.py Executable file
View File

@@ -0,0 +1,123 @@
from cereal import car
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 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
VisualAlert = car.CarControl.HUDControl.VisualAlert
def apply_ford_curvature_limits(apply_curvature, apply_curvature_last, current_curvature, v_ego_raw):
# No blending at low speed due to lack of torque wind-up and inaccurate current curvature
if v_ego_raw > 9:
apply_curvature = clip(apply_curvature, current_curvature - CarControllerParams.CURVATURE_ERROR,
current_curvature + CarControllerParams.CURVATURE_ERROR)
# Curvature rate limit after driver torque limit
apply_curvature = apply_std_steer_angle_limits(apply_curvature, apply_curvature_last, v_ego_raw, CarControllerParams)
return clip(apply_curvature, -CarControllerParams.CURVATURE_MAX, CarControllerParams.CURVATURE_MAX)
class CarController(CarControllerBase):
def __init__(self, dbc_name, CP, VM):
self.CP = CP
self.VM = VM
self.packer = CANPacker(dbc_name)
self.CAN = fordcan.CanBus(CP)
self.frame = 0
self.apply_curvature_last = 0
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 = []
actuators = CC.actuators
hud_control = CC.hudControl
main_on = CS.out.cruiseState.available
steer_alert = hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw)
fcw_alert = hud_control.visualAlert == VisualAlert.fcw
### acc buttons ###
if CC.cruiseControl.cancel:
can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.camera, CS.buttons_stock_values, cancel=True))
can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.main, CS.buttons_stock_values, cancel=True))
elif CC.cruiseControl.resume and (self.frame % CarControllerParams.BUTTONS_STEP) == 0:
can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.camera, CS.buttons_stock_values, resume=True))
can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.main, CS.buttons_stock_values, resume=True))
# 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
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_msg(self.packer, self.CAN.camera, CS.buttons_stock_values, tja_toggle=True))
### lateral control ###
# send steer msg at 20Hz
if (self.frame % CarControllerParams.STEER_STEP) == 0:
if CC.latActive:
# apply rate limits, curvature error limit, and clip to signal range
current_curvature = -CS.out.yawRate / max(CS.out.vEgoRaw, 0.1)
apply_curvature = apply_ford_curvature_limits(actuators.curvature, self.apply_curvature_last, current_curvature, CS.out.vEgoRaw)
else:
apply_curvature = 0.
self.apply_curvature_last = apply_curvature
if self.CP.flags & FordFlags.CANFD:
# TODO: extended mode
mode = 1 if CC.latActive else 0
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.))
# send lka msg at 33Hz
if (self.frame % CarControllerParams.LKA_STEP) == 0:
can_sends.append(fordcan.create_lka_msg(self.packer, self.CAN))
### longitudinal control ###
# send acc msg at 50Hz
if self.CP.openpilotLongitudinalControl and (self.frame % CarControllerParams.ACC_CONTROL_STEP) == 0:
# Both gas and accel are in m/s^2, accel is used solely for braking
if frogpilot_variables.sport_plus:
accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX_PLUS)
else:
accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
gas = accel
if not CC.longActive or gas < CarControllerParams.MIN_GAS:
gas = CarControllerParams.INACTIVE_GAS
stopping = CC.actuators.longControlState == LongCtrlState.stopping
can_sends.append(fordcan.create_acc_msg(self.packer, self.CAN, CC.longActive, gas, accel, stopping, v_ego_kph=V_CRUISE_MAX))
### ui ###
send_ui = (self.main_on_last != main_on) or (self.lkas_enabled_last != CC.latActive) or (self.steer_alert_last != steer_alert)
# 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))
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
self.frame += 1
return new_actuators, can_sends

172
selfdrive/car/ford/carstate.py Executable file
View File

@@ -0,0 +1,172 @@
from cereal import car
from opendbc.can.can_define import CANDefine
from opendbc.can.parser import CANParser
from openpilot.common.conversions import Conversions as CV
from openpilot.selfdrive.car.ford.fordcan import CanBus
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
class CarState(CarStateBase):
def __init__(self, CP):
super().__init__(CP)
can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
if CP.transmissionType == TransmissionType.automatic:
self.shifter_values = can_define.dv["Gear_Shift_by_Wire_FD1"]["TrnRng_D_RqGsm"]
self.vehicle_sensors_valid = False
self.prev_distance_button = 0
self.distance_button = 0
def update(self, cp, cp_cam, frogpilot_variables):
ret = car.CarState.new_message()
# 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
# car speed
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
# gas pedal
ret.gas = cp.vl["EngVehicleSpThrottle"]["ApedPos_Pc_ActlArb"] / 100.
ret.gasPressed = ret.gas > 1e-6
# brake pedal
ret.brake = cp.vl["BrakeSnData_4"]["BrkTot_Tq_Actl"] / 32756. # torque in Nm
ret.brakePressed = cp.vl["EngBrakeData"]["BpedDrvAppl_D_Actl"] == 2
ret.parkingBrake = cp.vl["DesiredTorqBrk"]["PrkBrkStatus"] in (1, 2)
# steering wheel
ret.steeringAngleDeg = cp.vl["SteeringPinion_Data"]["StePinComp_An_Est"]
ret.steeringTorque = cp.vl["EPAS_INFO"]["SteeringColumnTorque"]
ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > CarControllerParams.STEER_DRIVER_ALLOWANCE, 5)
ret.steerFaultTemporary = cp.vl["EPAS_INFO"]["EPAS_Failure"] == 1
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.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)
# 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)
ret.cruiseState.available = cp.vl["EngBrakeData"]["CcStat_D_Actl"] in (3, 4, 5)
ret.cruiseState.nonAdaptive = cp.vl["Cluster_Info1_FD1"]["AccEnbl_B_RqDrv"] == 0
ret.cruiseState.standstill = cp.vl["EngBrakeData"]["AccStopMde_D_Rq"] == 3
ret.accFaulted = cp.vl["EngBrakeData"]["CcStat_D_Actl"] in (1, 2)
if not self.CP.openpilotLongitudinalControl:
ret.accFaulted = ret.accFaulted or cp_cam.vl["ACCDATA"]["CmbbDeny_B_Actl"] == 1
# gear
if self.CP.transmissionType == TransmissionType.automatic:
gear = self.shifter_values.get(cp.vl["Gear_Shift_by_Wire_FD1"]["TrnRng_D_RqGsm"])
ret.gearShifter = self.parse_gear_shifter(gear)
elif self.CP.transmissionType == TransmissionType.manual:
ret.clutchPressed = cp.vl["Engine_Clutch_Data"]["CluPdlPos_Pc_Meas"] > 0
if bool(cp.vl["BCM_Lamp_Stat_FD1"]["RvrseLghtOn_B_Stat"]):
ret.gearShifter = GearShifter.reverse
else:
ret.gearShifter = GearShifter.drive
# safety
ret.stockFcw = bool(cp_cam.vl["ACCDATA_3"]["FcwVisblWarn_B_Rq"])
ret.stockAeb = bool(cp_cam.vl["ACCDATA_2"]["CmbbBrkDecel_B_Rq"])
# button presses
ret.leftBlinker = cp.vl["Steering_Data_FD1"]["TurnLghtSwtch_D_Stat"] == 1
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"],
cp.vl["BodyInfo_3_FD1"]["DrStatRl_B_Actl"], cp.vl["BodyInfo_3_FD1"]["DrStatRr_B_Actl"]])
ret.seatbeltUnlatched = cp.vl["RCMStatusMessage2_FD1"]["FirstRowBuckleDriver"] == 2
# blindspot sensors
if self.CP.enableBsm:
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
# Stock steering buttons so that we can passthru blinkers etc.
self.buttons_stock_values = cp.vl["Steering_Data_FD1"]
# 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"]
self.lkas_previously_enabled = self.lkas_enabled
self.lkas_enabled = bool(cp.vl["Steering_Data_FD1"]["TjaButtnOnOffPress"])
return ret
@staticmethod
def get_can_parser(CP):
messages = [
# sig_address, frequency
("VehicleOperatingModes", 100),
("BrakeSysFeatures", 50),
("Yaw_Data_FD1", 100),
("DesiredTorqBrk", 50),
("EngVehicleSpThrottle", 100),
("BrakeSnData_4", 50),
("EngBrakeData", 10),
("Cluster_Info1_FD1", 10),
("SteeringPinion_Data", 100),
("EPAS_INFO", 50),
("Steering_Data_FD1", 10),
("BodyInfo_3_FD1", 2),
("RCMStatusMessage2_FD1", 10),
]
if CP.flags & FordFlags.CANFD:
messages += [
("Lane_Assist_Data3_FD1", 33),
]
if CP.transmissionType == TransmissionType.automatic:
messages += [
("Gear_Shift_by_Wire_FD1", 10),
]
elif CP.transmissionType == TransmissionType.manual:
messages += [
("Engine_Clutch_Data", 33),
("BCM_Lamp_Stat_FD1", 1),
]
if CP.enableBsm and not (CP.flags & FordFlags.CANFD):
messages += [
("Side_Detect_L_Stat", 5),
("Side_Detect_R_Stat", 5),
]
return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus(CP).main)
@staticmethod
def get_cam_can_parser(CP):
messages = [
# sig_address, frequency
("ACCDATA", 50),
("ACCDATA_2", 50),
("ACCDATA_3", 5),
("IPMA_Data", 1),
]
if CP.enableBsm and CP.flags & FordFlags.CANFD:
messages += [
("Side_Detect_L_Stat", 5),
("Side_Detect_R_Stat", 5),
]
return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus(CP).camera)

149
selfdrive/car/ford/fingerprints.py Executable file
View File

@@ -0,0 +1,149 @@
from cereal import car
from openpilot.selfdrive.car.ford.values import CAR
Ecu = car.CarParams.Ecu
FW_VERSIONS = {
CAR.BRONCO_SPORT_MK1: {
(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: {
(Ecu.eps, 0x730, None): [
b'LX6C-14D003-AF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
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-NS\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LX6C-2D053-NT\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LX6C-2D053-NY\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LX6C-2D053-SA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LX6C-2D053-SD\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'LJ6T-14F397-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LJ6T-14F397-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LV4T-14F397-GG\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
CAR.EXPLORER_MK6: {
(Ecu.eps, 0x730, None): [
b'L1MC-14D003-AJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'L1MC-14D003-AK\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'L1MC-14D003-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'M1MC-14D003-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'M1MC-14D003-AC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'P1MC-14D003-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.abs, 0x760, None): [
b'L1MC-2D053-AJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'L1MC-2D053-BA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'L1MC-2D053-BB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'L1MC-2D053-BD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'L1MC-2D053-BF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'L1MC-2D053-KB\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'LB5T-14F397-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LB5T-14F397-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LB5T-14F397-AF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LC5T-14F397-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LC5T-14F397-AH\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'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',
],
},
CAR.F_150_LIGHTNING_MK1: {
(Ecu.abs, 0x760, None): [
b'PL38-2D053-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x706, None): [
b'ML3T-14H102-ABT\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',
],
},
CAR.MUSTANG_MACH_E_MK1: {
(Ecu.eps, 0x730, None): [
b'LJ9C-14D003-AM\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LJ9C-14D003-CC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.abs, 0x760, None): [
b'LK9C-2D053-CK\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'ML3T-14H102-ABS\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',
],
(Ecu.abs, 0x760, None): [
b'JX61-2D053-CJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x764, None): [
b'JX7T-14D049-AC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x706, None): [
b'JX7T-14F397-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
CAR.MAVERICK_MK1: {
(Ecu.eps, 0x730, None): [
b'NZ6C-14D003-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.abs, 0x760, None): [
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',
],
(Ecu.fwdCamera, 0x706, None): [
b'NZ6T-14F397-AC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
}

340
selfdrive/car/ford/fordcan.py Executable file
View File

@@ -0,0 +1,340 @@
from cereal import car
from openpilot.selfdrive.car import CanBusBase
HUDControl = car.CarControl.HUDControl
class CanBus(CanBusBase):
def __init__(self, CP=None, fingerprint=None) -> None:
super().__init__(CP, fingerprint)
@property
def main(self) -> int:
return self.offset
@property
def radar(self) -> int:
return self.offset + 1
@property
def camera(self) -> int:
return self.offset + 2
def calculate_lat_ctl2_checksum(mode: int, counter: int, dat: bytearray) -> int:
curvature = (dat[2] << 3) | ((dat[3]) >> 5)
curvature_rate = (dat[6] << 3) | ((dat[7]) >> 5)
path_angle = ((dat[3] & 0x1F) << 6) | ((dat[4]) >> 2)
path_offset = ((dat[4] & 0x3) << 8) | dat[5]
checksum = mode + counter
for sig_val in (curvature, curvature_rate, path_angle, path_offset):
checksum += sig_val + (sig_val >> 8)
return 0xFF - (checksum & 0xFF)
def create_lka_msg(packer, CAN: CanBus):
"""
Creates an empty CAN message for the Ford LKA Command.
This command can apply "Lane Keeping Aid" manoeuvres, which are subject to the PSCM lockout.
Frequency is 33Hz.
"""
return packer.make_can_msg("Lane_Assist_Data1", CAN.main, {})
def create_lat_ctl_msg(packer, CAN: CanBus, lat_active: bool, path_offset: float, path_angle: float, curvature: float,
curvature_rate: float):
"""
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.
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 (positive is right)
c1: heading angle between the vehicle and the centerline (positive is right)
c2: curvature of the centerline (positive is left)
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.
"""
values = {
"LatCtlRng_L_Max": 0, # Unknown [0|126] meter
"HandsOffCnfm_B_Rq": 0, # Unknown: 0=Inactive, 1=Active [0|1]
"LatCtl_D_Rq": 1 if lat_active else 0, # Mode: 0=None, 1=ContinuousPathFollowing, 2=InterventionLeft,
# 3=InterventionRight, 4-7=NotUsed [0|7]
"LatCtlRampType_D_Rq": 0, # Ramp speed: 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3]
# Makes no difference with curvature control
"LatCtlPrecision_D_Rq": 1, # Precision: 0=Comfortable, 1=Precise, 2/3=NotUsed [0|3]
# The stock system always uses comfortable
"LatCtlPathOffst_L_Actl": path_offset, # Path offset [-5.12|5.11] meter
"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
}
return packer.make_can_msg("LateralMotionControl", CAN.main, values)
def create_lat_ctl2_msg(packer, CAN: CanBus, mode: int, path_offset: float, path_angle: float, curvature: float,
curvature_rate: float, counter: int):
"""
Create a CAN message for the new Ford Lane Centering command.
This message is used on the CAN FD platform and replaces the old LateralMotionControl message. It is similar but has
additional signals for a counter and checksum.
Frequency is 20Hz.
"""
values = {
"LatCtl_D2_Rq": mode, # Mode: 0=None, 1=PathFollowingLimitedMode, 2=PathFollowingExtendedMode,
# 3=SafeRampOut, 4-7=NotUsed [0|7]
"LatCtlRampType_D_Rq": 0, # 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3]
"LatCtlPrecision_D_Rq": 1, # 0=Comfortable, 1=Precise, 2/3=NotUsed [0|3]
"LatCtlPathOffst_L_Actl": path_offset, # [-5.12|5.11] meter
"LatCtlPath_An_Actl": path_angle, # [-0.5|0.5235] radians
"LatCtlCurv_No_Actl": curvature, # [-0.02|0.02094] 1/meter
"LatCtlCrv_NoRate2_Actl": curvature_rate, # [-0.001024|0.001023] 1/meter^2
"HandsOffCnfm_B_Rq": 0, # 0=Inactive, 1=Active [0|1]
"LatCtlPath_No_Cnt": counter, # [0|15]
"LatCtlPath_No_Cs": 0, # [0|255]
}
# calculate checksum
dat = packer.make_can_msg("LateralMotionControl2", 0, values)[2]
values["LatCtlPath_No_Cs"] = calculate_lat_ctl2_checksum(mode, counter, dat)
return packer.make_can_msg("LateralMotionControl2", CAN.main, values)
def create_acc_msg(packer, CAN: CanBus, long_active: bool, gas: float, accel: float, stopping: bool, v_ego_kph: float):
"""
Creates a CAN message for the Ford ACC Command.
This command can be used to enable ACC, to set the ACC gas/brake/decel values
and to disable ACC.
Frequency is 50Hz.
"""
decel = accel < 0 and long_active
values = {
"AccBrkTot_A_Rq": accel, # Brake total accel request: [-20|11.9449] m/s^2
"Cmbb_B_Enbl": 1 if long_active else 0, # Enabled: 0=No, 1=Yes
"AccPrpl_A_Rq": gas, # Acceleration request: [-5|5.23] m/s^2
"AccPrpl_A_Pred": -5.0, # Acceleration request: [-5|5.23] m/s^2
"AccResumEnbl_B_Rq": 1 if long_active else 0,
"AccVeh_V_Trg": v_ego_kph, # Target speed: [0|255] km/h
# TODO: we may be able to improve braking response by utilizing pre-charging better
"AccBrkPrchg_B_Rq": 1 if decel else 0, # Pre-charge brake request: 0=No, 1=Yes
"AccBrkDecel_B_Rq": 1 if decel else 0, # Deceleration request: 0=Inactive, 1=Active
"AccStopStat_B_Rq": 1 if stopping else 0,
}
return packer.make_can_msg("ACCDATA", CAN.main, values)
def create_acc_ui_msg(packer, CAN: CanBus, CP, main_on: bool, enabled: bool, fcw_alert: bool, standstill: bool,
hud_control, stock_values: dict):
"""
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.
Frequency is 5Hz.
"""
# Tja_D_Stat
if enabled:
if hud_control.leftLaneDepart:
status = 3 # ActiveInterventionLeft
elif hud_control.rightLaneDepart:
status = 4 # ActiveInterventionRight
else:
status = 2 # Active
elif main_on:
if hud_control.leftLaneDepart:
status = 5 # ActiveWarningLeft
elif hud_control.rightLaneDepart:
status = 6 # ActiveWarningRight
else:
status = 1 # Standby
else:
status = 0 # Off
values = {s: stock_values[s] for s in [
"HaDsply_No_Cs",
"HaDsply_No_Cnt",
"AccStopStat_D_Dsply", # ACC stopped status message
"AccTrgDist2_D_Dsply", # ACC target distance
"AccStopRes_B_Dsply",
"TjaWarn_D_Rq", # TJA warning
"TjaMsgTxt_D_Dsply", # TJA text
"IaccLamp_D_Rq", # iACC status icon
"AccMsgTxt_D2_Rq", # ACC text
"FcwDeny_B_Dsply", # FCW disabled
"FcwMemStat_B_Actl", # FCW enabled setting
"AccTGap_B_Dsply", # ACC time gap display setting
"CadsAlignIncplt_B_Actl",
"AccFllwMde_B_Dsply", # ACC follow mode display setting
"CadsRadrBlck_B_Actl",
"CmbbPostEvnt_B_Dsply", # AEB event status
"AccStopMde_B_Dsply", # ACC stop mode display setting
"FcwMemSens_D_Actl", # FCW sensitivity setting
"FcwMsgTxt_D_Rq", # FCW text
"AccWarn_D_Dsply", # ACC warning
"FcwVisblWarn_B_Rq", # FCW visible alert
"FcwAudioWarn_B_Rq", # FCW audio alert
"AccTGap_D_Dsply", # ACC time gap
"AccMemEnbl_B_RqDrv", # ACC adaptive/normal setting
"FdaMem_B_Stat", # FDA enabled setting
]}
values.update({
"Tja_D_Stat": status, # TJA status
})
if CP.openpilotLongitudinalControl:
values.update({
"AccStopStat_D_Dsply": 2 if standstill else 0, # Stopping status text
"AccMsgTxt_D2_Rq": 0, # ACC text
"AccTGap_B_Dsply": 0, # Show time gap control UI
"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": hud_control.leadDistanceBars, # Time gap
})
# Forwards FCW alert from IPMA
if fcw_alert:
values["FcwVisblWarn_B_Rq"] = 1 # FCW visible alert
return packer.make_can_msg("ACCDATA_3", CAN.main, values)
def create_lkas_ui_msg(packer, CAN: CanBus, main_on: bool, enabled: bool, steer_alert: bool, hud_control,
stock_values: dict):
"""
Creates a CAN message for the Ford IPC IPMA/LKAS status.
Show the LKAS status with the "driver assist" lines in the IPC.
Stock functionality is maintained by passing through unmodified signals.
Frequency is 1Hz.
"""
# LaActvStats_D_Dsply
# R Intvn Warn Supprs Avail No
# L
# Intvn 24 19 14 9 4
# Warn 23 18 13 8 3
# Supprs 22 17 12 7 2
# Avail 21 16 11 6 1
# No 20 15 10 5 0
#
# TODO: test suppress state
if enabled:
lines = 0 # NoLeft_NoRight
if hud_control.leftLaneDepart:
lines += 4
elif hud_control.leftLaneVisible:
lines += 1
if hud_control.rightLaneDepart:
lines += 20
elif hud_control.rightLaneVisible:
lines += 5
elif main_on:
lines = 0
else:
if hud_control.leftLaneDepart:
lines = 3 # WarnLeft_NoRight
elif hud_control.rightLaneDepart:
lines = 15 # NoLeft_WarnRight
else:
lines = 30 # LA_Off
hands_on_wheel_dsply = 1 if steer_alert else 0
values = {s: stock_values[s] for s in [
"FeatConfigIpmaActl",
"FeatNoIpmaActl",
"PersIndexIpma_D_Actl",
"AhbcRampingV_D_Rq", # AHB ramping
"LaDenyStats_B_Dsply", # LKAS error
"CamraDefog_B_Req", # Windshield heater?
"CamraStats_D_Dsply", # Camera status
"DasAlrtLvl_D_Dsply", # DAS alert level
"DasStats_D_Dsply", # DAS status
"DasWarn_D_Dsply", # DAS warning
"AhbHiBeam_D_Rq", # AHB status
"Passthru_63",
"Passthru_48",
]}
values.update({
"LaActvStats_D_Dsply": lines, # LKAS status (lines) [0|31]
"LaHandsOff_D_Dsply": hands_on_wheel_dsply, # 0=HandsOn, 1=Level1 (w/o chime), 2=Level2 (w/ chime), 3=Suppressed
})
return packer.make_can_msg("IPMA_Data", CAN.main, values)
def create_button_msg(packer, bus: int, stock_values: dict, cancel=False, resume=False, tja_toggle=False):
"""
Creates a CAN message for the Ford SCCM buttons/switches.
Includes cruise control buttons, turn lights and more.
Frequency is 10Hz.
"""
values = {s: stock_values[s] for s in [
"HeadLghtHiFlash_D_Stat", # SCCM Passthrough the remaining buttons
"TurnLghtSwtch_D_Stat", # SCCM Turn signal switch
"WiprFront_D_Stat",
"LghtAmb_D_Sns",
"AccButtnGapDecPress",
"AccButtnGapIncPress",
"AslButtnOnOffCnclPress",
"AslButtnOnOffPress",
"LaSwtchPos_D_Stat",
"CcAslButtnCnclResPress",
"CcAslButtnDeny_B_Actl",
"CcAslButtnIndxDecPress",
"CcAslButtnIndxIncPress",
"CcAslButtnOffCnclPress",
"CcAslButtnOnOffCncl",
"CcAslButtnOnPress",
"CcAslButtnResDecPress",
"CcAslButtnResIncPress",
"CcAslButtnSetDecPress",
"CcAslButtnSetIncPress",
"CcAslButtnSetPress",
"CcButtnOffPress",
"CcButtnOnOffCnclPress",
"CcButtnOnOffPress",
"CcButtnOnPress",
"HeadLghtHiFlash_D_Actl",
"HeadLghtHiOn_B_StatAhb",
"AhbStat_B_Dsply",
"AccButtnGapTogglePress",
"WiprFrontSwtch_D_Stat",
"HeadLghtHiCtrl_D_RqAhb",
]}
values.update({
"CcAslButtnCnclPress": 1 if cancel else 0, # CC cancel button
"CcAsllButtnResPress": 1 if resume else 0, # CC resume button
"TjaButtnOnOffPress": 1 if tja_toggle else 0, # LCA/TJA toggle button
})
return packer.make_can_msg("Steering_Data_FD1", bus, values)

80
selfdrive/car/ford/interface.py Executable file
View File

@@ -0,0 +1,80 @@
from cereal import car, custom
from panda import Panda
from openpilot.common.conversions import Conversions as CV
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 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, disable_openpilot_long, experimental_long, docs):
ret.carName = "ford"
ret.dashcamOnly = False
ret.radarUnavailable = True
ret.steerControlType = car.CarParams.SteerControlType.angle
ret.steerActuatorDelay = 0.2
ret.steerLimitTimer = 1.0
ret.longitudinalTuning.kpBP = [0.]
ret.longitudinalTuning.kpV = [0.5]
ret.longitudinalTuning.kiV = [0.]
CAN = CanBus(fingerprint=fingerprint)
cfgs = [get_safety_config(car.CarParams.SafetyModel.ford)]
if CAN.main >= 4:
cfgs.insert(0, get_safety_config(car.CarParams.SafetyModel.noOutput))
ret.safetyConfigs = cfgs
ret.experimentalLongitudinalAvailable = True
if experimental_long:
ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_FORD_LONG_CONTROL
ret.openpilotLongitudinalControl = True
if ret.flags & FordFlags.CANFD:
ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_FORD_CANFD
# 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:
ret.transmissionType = TransmissionType.automatic
else:
ret.transmissionType = TransmissionType.manual
ret.minEnableSpeed = 20.0 * CV.MPH_TO_MS
# BSM: Side_Detect_L_Stat, Side_Detect_R_Stat
# TODO: detect bsm in car_fw?
ret.enableBsm = 0x3A6 in fingerprint[CAN.main] and 0x3A7 in fingerprint[CAN.main]
# LCA can steer down to zero
ret.minSteerSpeed = 0.
ret.autoResumeSng = ret.minEnableSpeed == -1.
ret.centerToFront = ret.wheelbase * 0.44
return ret
def _update(self, c, frogpilot_variables):
ret = self.CS.update(self.cp, self.cp_cam, frogpilot_variables)
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)
ret.events = events.to_msg()
return ret
def apply(self, c, now_nanos, frogpilot_variables):
return self.CC.update(c, self.CS, now_nanos, frogpilot_variables)

143
selfdrive/car/ford/radar_interface.py Executable file
View File

@@ -0,0 +1,143 @@
from math import cos, sin
from cereal import car
from opendbc.can.parser import CANParser
from openpilot.common.conversions import Conversions as CV
from openpilot.selfdrive.car.ford.fordcan import CanBus
from openpilot.selfdrive.car.ford.values import DBC, RADAR
from openpilot.selfdrive.car.interfaces import RadarInterfaceBase
DELPHI_ESR_RADAR_MSGS = list(range(0x500, 0x540))
DELPHI_MRR_RADAR_START_ADDR = 0x120
DELPHI_MRR_RADAR_MSG_COUNT = 64
def _create_delphi_esr_radar_can_parser(CP) -> CANParser:
msg_n = len(DELPHI_ESR_RADAR_MSGS)
messages = list(zip(DELPHI_ESR_RADAR_MSGS, [20] * msg_n, strict=True))
return CANParser(RADAR.DELPHI_ESR, messages, CanBus(CP).radar)
def _create_delphi_mrr_radar_can_parser(CP) -> CANParser:
messages = []
for i in range(1, DELPHI_MRR_RADAR_MSG_COUNT + 1):
msg = f"MRR_Detection_{i:03d}"
messages += [(msg, 20)]
return CANParser(RADAR.DELPHI_MRR, messages, CanBus(CP).radar)
class RadarInterface(RadarInterfaceBase):
def __init__(self, CP):
super().__init__(CP)
self.updated_messages = set()
self.track_id = 0
self.radar = DBC[CP.carFingerprint]['radar']
if self.radar is None or CP.radarUnavailable:
self.rcp = None
elif self.radar == RADAR.DELPHI_ESR:
self.rcp = _create_delphi_esr_radar_can_parser(CP)
self.trigger_msg = DELPHI_ESR_RADAR_MSGS[-1]
self.valid_cnt = {key: 0 for key in DELPHI_ESR_RADAR_MSGS}
elif self.radar == RADAR.DELPHI_MRR:
self.rcp = _create_delphi_mrr_radar_can_parser(CP)
self.trigger_msg = DELPHI_MRR_RADAR_START_ADDR + DELPHI_MRR_RADAR_MSG_COUNT - 1
else:
raise ValueError(f"Unsupported radar: {self.radar}")
def update(self, can_strings):
if self.rcp is None:
return super().update(None)
vls = self.rcp.update_strings(can_strings)
self.updated_messages.update(vls)
if self.trigger_msg not in self.updated_messages:
return None
ret = car.RadarData.new_message()
errors = []
if not self.rcp.can_valid:
errors.append("canError")
ret.errors = errors
if self.radar == RADAR.DELPHI_ESR:
self._update_delphi_esr()
elif self.radar == RADAR.DELPHI_MRR:
self._update_delphi_mrr()
ret.points = list(self.pts.values())
self.updated_messages.clear()
return ret
def _update_delphi_esr(self):
for ii in sorted(self.updated_messages):
cpt = self.rcp.vl[ii]
if cpt['X_Rel'] > 0.00001:
self.valid_cnt[ii] = 0 # reset counter
if cpt['X_Rel'] > 0.00001:
self.valid_cnt[ii] += 1
else:
self.valid_cnt[ii] = max(self.valid_cnt[ii] - 1, 0)
#print ii, self.valid_cnt[ii], cpt['VALID'], cpt['X_Rel'], cpt['Angle']
# radar point only valid if there have been enough valid measurements
if self.valid_cnt[ii] > 0:
if ii not in self.pts:
self.pts[ii] = car.RadarData.RadarPoint.new_message()
self.pts[ii].trackId = self.track_id
self.track_id += 1
self.pts[ii].dRel = cpt['X_Rel'] # from front of car
self.pts[ii].yRel = cpt['X_Rel'] * cpt['Angle'] * CV.DEG_TO_RAD # in car frame's y axis, left is positive
self.pts[ii].vRel = cpt['V_Rel']
self.pts[ii].aRel = float('nan')
self.pts[ii].yvRel = float('nan')
self.pts[ii].measured = True
else:
if ii in self.pts:
del self.pts[ii]
def _update_delphi_mrr(self):
for ii in range(1, DELPHI_MRR_RADAR_MSG_COUNT + 1):
msg = self.rcp.vl[f"MRR_Detection_{ii:03d}"]
# SCAN_INDEX rotates through 0..3 on each message
# treat these as separate points
scanIndex = msg[f"CAN_SCAN_INDEX_2LSB_{ii:02d}"]
i = (ii - 1) * 4 + scanIndex
if i not in self.pts:
self.pts[i] = car.RadarData.RadarPoint.new_message()
self.pts[i].trackId = self.track_id
self.pts[i].aRel = float('nan')
self.pts[i].yvRel = float('nan')
self.track_id += 1
valid = bool(msg[f"CAN_DET_VALID_LEVEL_{ii:02d}"])
if valid:
azimuth = msg[f"CAN_DET_AZIMUTH_{ii:02d}"] # rad [-3.1416|3.13964]
dist = msg[f"CAN_DET_RANGE_{ii:02d}"] # m [0|255.984]
distRate = msg[f"CAN_DET_RANGE_RATE_{ii:02d}"] # m/s [-128|127.984]
dRel = cos(azimuth) * dist # m from front of car
yRel = -sin(azimuth) * dist # in car frame's y axis, left is positive
# delphi doesn't notify of track switches, so do it manually
# TODO: refactor this to radard if more radars behave this way
if abs(self.pts[i].vRel - distRate) > 2 or abs(self.pts[i].dRel - dRel) > 5:
self.track_id += 1
self.pts[i].trackId = self.track_id
self.pts[i].dRel = dRel
self.pts[i].yRel = yRel
self.pts[i].vRel = distRate
self.pts[i].measured = True
else:
del self.pts[i]

0
selfdrive/car/ford/tests/__init__.py Executable file
View File

82
selfdrive/car/ford/tests/test_ford.py Executable file
View File

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

211
selfdrive/car/ford/values.py Executable file
View File

@@ -0,0 +1,211 @@
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, 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, p16
Ecu = car.CarParams.Ecu
class CarControllerParams:
STEER_STEP = 5 # LateralMotionControl, 20Hz
LKA_STEP = 3 # Lane_Assist_Data1, 33Hz
ACC_CONTROL_STEP = 2 # ACCDATA, 50Hz
LKAS_UI_STEP = 100 # IPMA_Data, 1Hz
ACC_UI_STEP = 20 # ACCDATA_3, 5Hz
BUTTONS_STEP = 5 # Steering_Data_FD1, 10Hz, but send twice as fast
CURVATURE_MAX = 0.02 # Max curvature for steering command, m^-1
STEER_DRIVER_ALLOWANCE = 1.0 # Driver intervention threshold, Nm
# Curvature rate limits
# The curvature signal is limited to 0.003 to 0.009 m^-1/sec by the EPS depending on speed and direction
# Limit to ~2 m/s^3 up, ~3 m/s^3 down at 75 mph
# Worst case, the low speed limits will allow 4.3 m/s^3 up, 4.9 m/s^3 down at 75 mph
ANGLE_RATE_LIMIT_UP = AngleRateLimit(speed_bp=[5, 25], angle_v=[0.0002, 0.0001])
ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_bp=[5, 25], angle_v=[0.000225, 0.00015])
CURVATURE_ERROR = 0.002 # ~6 degrees at 10 m/s, ~10 degrees at 35 m/s
ACCEL_MAX = 2.0 # m/s^2 max acceleration
ACCEL_MAX_PLUS = 4.0 # m/s^2 max acceleration
ACCEL_MIN = -3.5 # m/s^2 max deceleration
MIN_GAS = -0.5
INACTIVE_GAS = -5.0
def __init__(self, CP):
pass
class FordFlags(IntFlag):
# Static flags
CANFD = 1
class RADAR:
DELPHI_ESR = 'ford_fusion_2018_adas'
DELPHI_MRR = 'FORD_CADS'
class Footnote(Enum):
FOCUS = CarFootnote(
"Refers only to the Focus Mk4 (C519) available in Europe/China/Taiwan/Australasia, not the Focus Mk3 (C346) in " +
"North and South America/Southeast Asia.",
Column.MODEL,
)
@dataclass
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.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])
@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=[
# CAN and CAN FD queries are combined.
# FIXME: For CAN FD, ECUs respond with frames larger than 8 bytes on the powertrain bus
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=[
(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()