mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-08-20 07:43:48 +08:00
Code Issues Packages Projects Releases Wiki Activity

Ford Stuff

Browse Source 
Huge thanks to BluePilot guys!
This commit is contained in:
firestar5683
2026-08-15 20:00:42 -05:00
parent 4ccfc1c00c
commit 3f6ccd104e
19 changed files with 1018 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files
common/params_keys.h
+10
View File
@@ -335,6 +335,16 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"ForceStopDistanceOffset", {PERSISTENT, INT, "0", "0", 2, SETTINGS_SIMPLE}},
{"ForceStandstill", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
{"ForceTorqueController", {PERSISTENT, BOOL, "0", "0", 3}},
{"FordAngleBlend", {PERSISTENT, FLOAT, "0.5", "0.5", 2}},
{"FordAngleHighSpeedDamping", {PERSISTENT, FLOAT, "1.0", "1.0", 2}},
{"FordAngleHighSpeedFactor", {PERSISTENT, FLOAT, "1.0", "1.0", 2}},
{"FordAngleLaneChangeFactor", {PERSISTENT, FLOAT, "1.0", "1.0", 2}},
{"FordAngleLowSpeedFactor", {PERSISTENT, FLOAT, "1.0", "1.0", 2}},
{"FordCurvatureBlendHigh", {PERSISTENT, FLOAT, "0.4", "0.4", 2}},
{"FordCurvatureBlendLow", {PERSISTENT, FLOAT, "0.4", "0.4", 2}},
{"FordCurvatureLaneChangeFactor", {PERSISTENT, FLOAT, "0.85", "0.85", 2}},
{"FordHumanTurnDetection", {PERSISTENT, BOOL, "1", "1", 2}},
{"FordLateralMode", {PERSISTENT, INT, "1", "1", 2}},
{"FLMActiveOverrides", {PERSISTENT, JSON, "{}", "{}", 2}},
{"FLMActiveProfileId", {PERSISTENT, STRING, "", "", 2}},
{"FLMSubmittedTune", {CLEAR_ON_MANAGER_START, JSON, "{}", "{}"}},
opendbc_repo/opendbc/car/ford/carcontroller.py
+65 -25
View File
@@ -6,6 +6,8 @@ from opendbc.car.lateral import ISO_LATERAL_ACCEL, apply_std_steer_angle_limits
from opendbc.car.ford import fordcan
from opendbc.car.ford.values import CarControllerParams, FordFlags, CAR
from opendbc.car.interfaces import CarControllerBase, V_CRUISE_MAX
from openpilot.starpilot.car.ford import fordcan as starpilot_fordcan
from openpilot.starpilot.car.ford.lateral import FordLateralController, FordLateralMode, FordLateralResult
LongCtrlState = structs.CarControl.Actuators.LongControlState
VisualAlert = structs.CarControl.HUDControl.VisualAlert
@@ -80,6 +82,9 @@ class CarController(CarControllerBase):
self.steer_alert_last = False
self.lead_distance_bars_last = None
self.distance_bar_frame = 0
self.ford_lateral = None if CP.flags & FordFlags.LKA_STEERING else FordLateralController(CP)
self.ford_shadow_curvature = 0.0
self.ford_lateral_announced_mode = FordLateralMode.native
def update(self, CC, CS, now_nanos, starpilot_toggles):
can_sends = []
@@ -91,6 +96,9 @@ class CarController(CarControllerBase):
steer_alert = hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw)
fcw_alert = hud_control.visualAlert == VisualAlert.fcw
if self.ford_lateral is not None:
self.ford_lateral.update_inputs()
### acc buttons ###
if CC.cruiseControl.cancel:
can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.camera, CS.buttons_stock_values, cancel=True))
@@ -128,38 +136,70 @@ class CarController(CarControllerBase):
can_sends.append(fordcan.create_lka_msg(self.packer, self.CAN, active=lka_active, apply_angle=self.apply_angle_last,
direction=direction, ramp_type=ramp_type, curvature=-self.apply_curvature_last))
else:
# send steer msg at 20Hz
if (self.frame % CarControllerParams.STEER_STEP) == 0:
# Bronco and some other cars consistently overshoot curv requests
# Apply some deadzone + smoothing convergence to avoid oscillations
if self.CP.carFingerprint in (CAR.FORD_BRONCO_SPORT_MK1, CAR.FORD_F_150_MK14):
self.anti_overshoot_curvature_last = anti_overshoot(actuators.curvature, self.anti_overshoot_curvature_last, CS.out.vEgoRaw)
apply_curvature = self.anti_overshoot_curvature_last
lateral_mode = self.ford_lateral.mode
lateral_mode_ready = lateral_mode == self.ford_lateral_announced_mode
# Keep the original Ford path available without changing its command behavior.
if lateral_mode == FordLateralMode.native:
if (self.frame % CarControllerParams.STEER_STEP) == 0:
if not lateral_mode_ready:
self.apply_curvature_last = 0.0
apply_curvature = 0.0
elif self.CP.carFingerprint in (CAR.FORD_BRONCO_SPORT_MK1, CAR.FORD_F_150_MK14):
self.anti_overshoot_curvature_last = anti_overshoot(
actuators.curvature, self.anti_overshoot_curvature_last, CS.out.vEgoRaw)
apply_curvature = self.anti_overshoot_curvature_last
else:
apply_curvature = actuators.curvature
current_curvature = -CS.out.yawRate / max(CS.out.vEgoRaw, 0.1)
self.apply_curvature_last = apply_ford_curvature_limits(
apply_curvature, self.apply_curvature_last, current_curvature,
CS.out.vEgoRaw, 0., CC.latActive and lateral_mode_ready, self.CP)
if self.CP.flags & FordFlags.CANFD:
mode = 1 if CC.latActive and lateral_mode_ready else 0
counter = (self.frame // CarControllerParams.STEER_STEP) % 0x10
can_sends.append(fordcan.create_lat_ctl2_msg(
self.packer, self.CAN, mode, 0., 0., -self.apply_curvature_last, 0., counter))
else:
can_sends.append(fordcan.create_lat_ctl_msg(
self.packer, self.CAN, CC.latActive and lateral_mode_ready, 0., 0., -self.apply_curvature_last, 0.))
elif (self.frame % CarControllerParams.STEER_STEP) == 0:
if not lateral_mode_ready:
lateral = FordLateralResult(shadow_curvature=self.ford_lateral._current_curvature(CS))
elif lateral_mode == FordLateralMode.angle:
lateral = self.ford_lateral.update_angle(CC, CS, actuators)
else:
apply_curvature = actuators.curvature
# apply rate limits, curvature error limit, and clip to signal range
current_curvature = -CS.out.yawRate / max(CS.out.vEgoRaw, 0.1)
self.apply_curvature_last = apply_ford_curvature_limits(apply_curvature, self.apply_curvature_last, current_curvature,
CS.out.vEgoRaw, 0., CC.latActive, self.CP)
lateral = self.ford_lateral.update_curvature(CC, CS, actuators)
self.apply_curvature_last = lateral.curvature
self.ford_shadow_curvature = lateral.shadow_curvature
if self.CP.flags & FordFlags.CANFD:
# TODO: extended mode
# Ford uses four individual signals to dictate how to drive to the car. Curvature alone (limited to 0.02m/s^2)
# can actuate the steering for a large portion of any lateral movements. However, in order to get further control on
# steer actuation, the other three signals are necessary. Ford controls vehicles differently than most other makes.
# A detailed explanation on ford control can be found here:
# https://www.f150gen14.com/forum/threads/introducing-bluepilot-a-ford-specific-fork-for-comma3x-openpilot.24241/#post-457706
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., -self.apply_curvature_last, 0., counter))
can_sends.append(starpilot_fordcan.create_lat_ctl2_msg(
self.packer, self.CAN, 1 if lateral.active else 0,
lateral.ramp_type, lateral.precision_type,
-lateral.path_offset, -lateral.path_angle,
-lateral.curvature, -lateral.curvature_rate, counter))
else:
can_sends.append(fordcan.create_lat_ctl_msg(self.packer, self.CAN, CC.latActive, 0., 0., -self.apply_curvature_last, 0.))
can_sends.append(starpilot_fordcan.create_lat_ctl_msg(
self.packer, self.CAN, lateral.active,
lateral.ramp_type, lateral.precision_type,
-lateral.path_offset, -lateral.path_angle,
-lateral.curvature, -lateral.curvature_rate))
# send lka msg at 33Hz
if (self.frame % CarControllerParams.LKA_STEP) == 0:
can_sends.append(fordcan.create_lka_msg(self.packer, self.CAN))
if lateral_mode == FordLateralMode.native:
can_sends.append(fordcan.create_lka_msg(self.packer, self.CAN))
else:
angle_mode = lateral_mode == FordLateralMode.angle
shadow_curvature = -self.ford_lateral._current_curvature(CS)
if angle_mode:
shadow_curvature = -self.ford_shadow_curvature
can_sends.append(starpilot_fordcan.create_lka_msg(
self.packer, self.CAN, angle_mode=angle_mode, shadow_curvature=shadow_curvature))
self.ford_lateral_announced_mode = lateral_mode
### longitudinal control ###
# send acc msg at 50Hz
opendbc_repo/opendbc/car/ford/carstate.py
+53 -7
View File
@@ -16,12 +16,18 @@ class CarState(CarStateBase):
super().__init__(CP, FPCP)
can_define = CANDefine(DBC[CP.carFingerprint][Bus.pt])
if CP.transmissionType == TransmissionType.automatic:
self.shifter_values = can_define.dv["PowertrainData_10"]["TrnRng_D_Rq"]
if CP.flags & FordFlags.CANFD:
self.shifter_values = can_define.dv["Gear_Shift_by_Wire_FD1"]["TrnRng_D_RqGsm"]
elif CP.flags & FordFlags.ALT_STEER_ANGLE:
self.shifter_values = can_define.dv["TransGearData"]["GearLvrPos_D_Actl"]
else:
self.shifter_values = can_define.dv["PowertrainData_10"]["TrnRng_D_Rq"]
self.distance_button = 0
self.lc_button = 0
self.lkas_available = False
self.lateral_motion_control = None
self.steering_angle_offset_deg = 0.0
def update(self, can_parsers, starpilot_toggles) -> structs.CarState:
cp = can_parsers[Bus.pt]
@@ -29,13 +35,24 @@ class CarState(CarStateBase):
ret = structs.CarState()
# 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
ret.vehicleSensorsInvalid = cp.vl["SteeringPinion_Data"]["StePinCompAnEst_D_Qf"] != 3
if self.CP.flags & FordFlags.ALT_STEER_ANGLE:
sensors_valid = (
int((cp.vl["ParkAid_Data"]["ExtSteeringAngleReq2"] + 1000) * 10) not in (32766, 32767)
and cp.vl["ParkAid_Data"]["EPASExtAngleStatReq"] == 0
and cp.vl["ParkAid_Data"]["ApaSys_D_Stat"] in (0, 1)
)
ret.vehicleSensorsInvalid = not sensors_valid
else:
# The ABS can recalibrate the steering pinion offset briefly after startup.
ret.vehicleSensorsInvalid = 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)
if self.CP.flags & FordFlags.CANFD:
ret.vEgoCluster = ((cp.vl["Cluster_Info_3_FD1"]["DISPLAY_SPEED_SCALING"] / 100) *
cp.vl["EngVehicleSpThrottle2"]["Veh_V_ActlEng"] +
cp.vl["Cluster_Info_3_FD1"]["DISPLAY_SPEED_OFFSET"]) * CV.KPH_TO_MS
ret.yawRate = cp.vl["Yaw_Data_FD1"]["VehYaw_W_Actl"]
ret.standstill = cp.vl["DesiredTorqBrk"]["VehStop_D_Stat"] == 1
@@ -48,7 +65,14 @@ class CarState(CarStateBase):
ret.parkingBrake = cp.vl["DesiredTorqBrk"]["PrkBrkStatus"] in (1, 2)
# steering wheel
ret.steeringAngleDeg = cp.vl["SteeringPinion_Data"]["StePinComp_An_Est"]
if self.CP.flags & FordFlags.ALT_STEER_ANGLE:
steering_angle_init = cp.vl["SteeringPinion_Data_Alt"]["StePinRelInit_An_Sns"]
if not ret.vehicleSensorsInvalid:
steering_angle_est = cp.vl["ParkAid_Data"]["ExtSteeringAngleReq2"]
self.steering_angle_offset_deg = steering_angle_est - steering_angle_init
ret.steeringAngleDeg = steering_angle_init + self.steering_angle_offset_deg
else:
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
@@ -60,7 +84,8 @@ class CarState(CarStateBase):
ret.steerFaultTemporary |= cp.vl["Lane_Assist_Data3_FD1"]["LatCtlSte_D_Stat"] not in (1, 2, 3)
# cruise state
is_metric = cp.vl["INSTRUMENT_PANEL"]["METRIC_UNITS"] == 1 if not self.CP.flags & FordFlags.CANFD else False
is_metric = cp.vl["INSTRUMENT_PANEL"]["METRIC_UNITS"] == 1 if not self.CP.flags & FordFlags.CANFD else \
cp_cam.vl["IPMA_Data2"]["IsaVLimUnit_D_Rq"] == 1
ret.cruiseState.speed = cp.vl["EngBrakeData"]["Veh_V_DsplyCcSet"] * (CV.KPH_TO_MS if is_metric else 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)
@@ -72,7 +97,12 @@ class CarState(CarStateBase):
# gear
if self.CP.transmissionType == TransmissionType.automatic:
gear = self.shifter_values.get(cp.vl["PowertrainData_10"]["TrnRng_D_Rq"])
if self.CP.flags & FordFlags.CANFD:
gear = self.shifter_values.get(cp.vl["Gear_Shift_by_Wire_FD1"]["TrnRng_D_RqGsm"])
elif self.CP.flags & FordFlags.ALT_STEER_ANGLE:
gear = self.shifter_values.get(cp.vl["TransGearData"]["GearLvrPos_D_Actl"])
else:
gear = self.shifter_values.get(cp.vl["PowertrainData_10"]["TrnRng_D_Rq"])
ret.gearShifter = self.parse_gear_shifter(gear)
elif self.CP.transmissionType == TransmissionType.manual:
if bool(cp.vl["BCM_Lamp_Stat_FD1"]["RvrseLghtOn_B_Stat"]):
@@ -126,6 +156,22 @@ class CarState(CarStateBase):
]
fp_ret = custom.StarPilotCarState.new_message()
fp_ret.brakeLights = ret.brakePressed
try:
fp_ret.brakeLights = bool(cp.vl["BCM_Lamp_Stat_FD1"]["StopLghtOn_B_Stat"])
except (KeyError, AttributeError):
try:
fp_ret.brakeLights = cp.vl["BrakeSysFeatures_2"]["BrkLamp_B_Rq"] == 1
except (KeyError, AttributeError):
pass
try:
speed_limit = cp_cam.vl["Traffic_RecognitnData"]["TsrVLim1MsgTxt_D_Rq"]
speed_limit_unit = cp_cam.vl["Traffic_RecognitnData"]["TsrVlUnitMsgTxt_D_Rq"]
speed_factor = CV.MPH_TO_MS if speed_limit_unit == 2 else CV.KPH_TO_MS if speed_limit_unit == 1 else 0.0
fp_ret.dashboardSpeedLimit = speed_limit * speed_factor if speed_limit not in (0, 255) else 0.0
except (KeyError, AttributeError):
fp_ret.dashboardSpeedLimit = 0.0
return ret, fp_ret
opendbc_repo/opendbc/car/ford/fingerprints.py
+74
View File
@@ -12,6 +12,8 @@ FW_VERSIONS = {
b'LX6C-14D003-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.abs, 0x760, None): [
b'LX6C-2D053-KF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LX6C-2D053-KG\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
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',
@@ -24,6 +26,21 @@ FW_VERSIONS = {
b'M1PT-14F397-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
CAR.FORD_EDGE_MK2: {
(Ecu.eps, 0x730, None): [
b'M2GC-14D003-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.abs, 0x760, None): [
b'M2GC-2D053-CB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'M2GC-2D053-EA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x764, None): [
b'JX7T-14D049-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x706, None): [
b'KT4T-14F397-AF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
CAR.FORD_ESCAPE_MK4: {
(Ecu.eps, 0x730, None): [
b'LX6C-14D003-AF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
@@ -49,7 +66,9 @@ FW_VERSIONS = {
},
CAR.FORD_ESCAPE_MK4_5: {
(Ecu.eps, 0x730, None): [
b'PZ11-14D003-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PZ11-14D003-EA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PZ11-14D003-FA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.abs, 0x760, None): [
b'PZ1C-2D053-EJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
@@ -58,11 +77,20 @@ FW_VERSIONS = {
b'ML3T-14D049-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x706, None): [
b'PJ6T-14H102-ABE\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PJ6T-14H102-ABL\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PJ6T-14H102-MDC\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PJ6T-14H102-MDF\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PJ6T-14H102-SCD\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PJ6T-14H102-SCG\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
CAR.FORD_EXPLORER_MK6: {
(Ecu.eps, 0x730, None): [
b'R1MC-14D003-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'R1MC-14D003-AF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'R1MC-14D003-AG\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'R1MC-14D003-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
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',
@@ -96,6 +124,7 @@ FW_VERSIONS = {
],
(Ecu.abs, 0x760, None): [
b'RL14-2D053-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PL14-2D053-AB\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',
@@ -106,6 +135,7 @@ FW_VERSIONS = {
},
CAR.FORD_F_150_MK14: {
(Ecu.eps, 0x730, None): [
b'MB3C-14D003-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'ML3V-14D003-BA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'ML3V-14D003-BC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'ML3V-14D003-BD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
@@ -136,8 +166,11 @@ FW_VERSIONS = {
},
CAR.FORD_F_150_LIGHTNING_MK1: {
(Ecu.abs, 0x760, None): [
b'NL38-2D053-AF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PL38-2D053-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'RL38-2D053-BC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'RL38-2D053-BD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'TL38-2D053-AD\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',
@@ -145,9 +178,13 @@ FW_VERSIONS = {
b'RJ6T-14H102-BBC\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x764, None): [
b'ML3T-14D049-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'ML3T-14D049-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'RB5T-14D049-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.eps, 0x730, None): [
b'NL38-14D003-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'NL38-14D003-AC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'RL38-14D003-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
@@ -164,6 +201,8 @@ FW_VERSIONS = {
b'LK9C-2D053-CN\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x764, None): [
b'ML3T-14D049-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'ML3T-14D049-AK\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'ML3T-14D049-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x706, None): [
@@ -174,15 +213,42 @@ FW_VERSIONS = {
CAR.FORD_FOCUS_MK4: {
(Ecu.eps, 0x730, None): [
b'JX6C-14D003-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'JX6C-14D003-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'JX6C-14D003-BB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'NX6C-14D003-AL\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',
b'LX61-2D053-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'NX61-2D053-MD\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',
b'JX7T-14D049-AD\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',
b'N1BT-14F397-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
CAR.FORD_MONDEO_MK5: {
(Ecu.fwdCamera, 0x706, None): [
b'KT4T-14F397-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.abs, 0x760, None): [
b'KG9C-2D053-DF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.eps, 0x730, None): [
b'K2GC-14D003-AJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x764, None): [
b'JX7T-14D049-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x7e0, None): [
b'HS7A-14C204-CJD\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.debug, 0x7d0, None): [
b'1U5T-14G374-DA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
CAR.FORD_MAVERICK_MK1: {
@@ -207,17 +273,25 @@ FW_VERSIONS = {
},
CAR.FORD_RANGER_MK2: {
(Ecu.eps, 0x730, None): [
b'JR3C-14D003-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'NB3C-14D003-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'NL14-14D003-AC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'NL14-14D003-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'RB3C-14D003-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.abs, 0x760, None): [
b'MB3C-2D053-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'MB3C-2D053-ZJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PB3C-2D053-ZB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PB3C-2D053-ZC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PB3C-2D053-ZD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PB3C-2D053-ZG\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PB3C-2D053-ZJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PB9C-2D053-ZG\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'RB3C-2D053-AK\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x764, None): [
b'JX7T-14D049-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'ML3T-14D049-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x706, None): [
opendbc_repo/opendbc/car/ford/interface.py
+6 -1
View File
@@ -31,12 +31,13 @@ class CarInterface(CarInterfaceBase):
ret.radarUnavailable = Bus.radar not in DBC[candidate]
ret.steerControlType = structs.CarParams.SteerControlType.angle
ret.steerActuatorDelay = 0.05 if ret.flags & FordFlags.LKA_STEERING else 0.2
ret.steerActuatorDelay = 0.05 if ret.flags & FordFlags.LKA_STEERING else 0.22
ret.steerLimitTimer = 1.0
ret.steerAtStandstill = True
ret.longitudinalTuning.kiBP = [0.]
ret.longitudinalTuning.kiV = [0.5]
ret.longitudinalTuning.kpV = [0.]
if not ret.radarUnavailable and DBC[candidate][Bus.radar] == RADAR.DELPHI_MRR:
# average of 33.3 Hz radar timestep / 4 scan modes = 60 ms
@@ -44,6 +45,10 @@ class CarInterface(CarInterfaceBase):
ret.radarDelay = 0.06
CAN = CanBus(fingerprint=fingerprint)
if 0x365 in fingerprint[CAN.main]:
ret.flags |= int(FordFlags.HEV_CLUSTER_DATA)
if all(address in fingerprint[CAN.main] for address in (0x07A, 0x24B, 0x24C)):
ret.flags |= int(FordFlags.HEV_BATTERY_DATA)
cfgs = [get_safety_config(structs.CarParams.SafetyModel.ford)]
if CAN.main >= 4:
cfgs.insert(0, get_safety_config(structs.CarParams.SafetyModel.noOutput))
opendbc_repo/opendbc/car/ford/radar_interface.py
+45
View File
@@ -1,4 +1,5 @@
import numpy as np
from collections import deque
from typing import cast
from collections import defaultdict
from math import cos, sin
@@ -19,6 +20,7 @@ DELPHI_MRR_RADAR_MSG_COUNT = 64
DELPHI_MRR_RADAR_RANGE_COVERAGE = {0: 42, 1: 164, 2: 45, 3: 175} # scan index to detection range (m)
DELPHI_MRR_MIN_LONG_RANGE_DIST = 30 # meters
DELPHI_MRR_CLUSTER_THRESHOLD = 5 # meters, lateral distance and relative velocity are weighted
STEER_ASSIST_DATA_ADDR = 0x3D7
@dataclass
@@ -89,12 +91,17 @@ def _create_delphi_mrr_radar_can_parser(CP) -> CANParser:
return CANParser(RADAR.DELPHI_MRR, messages, CanBus(CP).radar)
def _create_steer_assist_radar_can_parser(CP) -> CANParser:
return CANParser(RADAR.STEER_ASSIST_DATA, [("Steer_Assist_Data", 20)], CanBus(CP).camera)
class RadarInterface(RadarInterfaceBase):
def __init__(self, CP):
super().__init__(CP)
self.points: list[list[float]] = []
self.clusters: list[Cluster] = []
self.v_rel_history = deque(maxlen=20)
self.updated_messages = set()
self.track_id = 0
@@ -111,6 +118,9 @@ class RadarInterface(RadarInterfaceBase):
elif self.radar == RADAR.DELPHI_MRR:
self.rcp = _create_delphi_mrr_radar_can_parser(CP)
self.trigger_msg = DELPHI_MRR_RADAR_HEADER_ADDR
elif self.radar == RADAR.STEER_ASSIST_DATA:
self.rcp = _create_steer_assist_radar_can_parser(CP)
self.trigger_msg = STEER_ASSIST_DATA_ADDR
else:
raise ValueError(f"Unsupported radar: {self.radar}")
@@ -135,10 +145,45 @@ class RadarInterface(RadarInterfaceBase):
_update = self._update_delphi_mrr(ret)
if not _update:
return None
elif self.radar == RADAR.STEER_ASSIST_DATA:
self._update_steer_assist()
ret.points = list(self.pts.values())
return ret
def _update_steer_assist(self):
msg = self.rcp.vl["Steer_Assist_Data"]
confidence = msg["CmbbObjConfdnc_D_Stat"]
if confidence <= 0:
self.pts.pop(0, None)
self.v_rel_history.clear()
return
d_rel = msg["CmbbObjDistLong_L_Actl"]
v_rel = msg["CmbbObjRelLong_V_Actl"]
new_track = 0 not in self.pts
if new_track:
self.pts[0] = structs.RadarData.RadarPoint()
self.pts[0].trackId = self.track_id
self.track_id += 1
elif abs(v_rel) < 1e-2:
self.v_rel_history.append(d_rel - self.pts[0].dRel)
v_rel = sum(self.v_rel_history)
else:
self.v_rel_history.clear()
if not new_track and (abs(self.pts[0].vRel - v_rel) > 2.0 or abs(self.pts[0].dRel - d_rel) > 5.0):
self.pts[0].trackId = self.track_id
self.track_id += 1
self.v_rel_history.clear()
self.pts[0].dRel = d_rel
self.pts[0].yRel = msg["CmbbObjDistLat_L_Actl"]
self.pts[0].vRel = v_rel
self.pts[0].aRel = float('nan')
self.pts[0].yvRel = msg["CmbbObjRelLat_V_Actl"]
self.pts[0].measured = True
def _update_delphi_esr(self):
for ii in sorted(self.updated_messages):
cpt = self.rcp.vl[ii]
opendbc_repo/opendbc/car/ford/tests/test_ford.py
+16 -2
View File
@@ -6,7 +6,7 @@ from parameterized import parameterized
from opendbc.car.structs import CarParams
from opendbc.car.fw_versions import build_fw_dict
from opendbc.car.ford.values import CAR, FW_QUERY_CONFIG, FW_PATTERN, get_platform_codes
from opendbc.car.ford.values import CAR, FW_QUERY_CONFIG, FW_PATTERN, get_platform_codes, match_vin_to_car
from opendbc.car.ford.fingerprints import FW_VERSIONS
Ecu = CarParams.Ecu
@@ -20,6 +20,7 @@ ECU_ADDRESSES = {
Ecu.engine: 0x7E0, # Powertrain Control Module (PCM)
Ecu.shiftByWire: 0x732, # Gear Shift Module (GSM)
Ecu.debug: 0x7D0, # Accessory Protocol Interface Module (APIM)
Ecu.hud: 0x720, # Instrument Cluster Module (ICM)
}
@@ -41,6 +42,16 @@ ECU_PART_NUMBER = {
class TestFordFW:
def test_vin_fallback(self):
def vin(wmi, vds, powertrain, year):
return f"{wmi}{vds}{powertrain}0{year}1234567"
assert match_vin_to_car(vin("2FM", "PK4A", "A", "N")) == {str(CAR.FORD_EDGE_MK2)}
assert match_vin_to_car(vin("3FM", "K1RA", "A", "M")) == {str(CAR.FORD_MUSTANG_MACH_E_MK1)}
assert match_vin_to_car(vin("1FT", "F1CA", "A", "M")) == {str(CAR.FORD_F_150_MK14)}
assert match_vin_to_car(vin("1FT", "F1CA", "L", "N")) == {str(CAR.FORD_F_150_LIGHTNING_MK1)}
assert match_vin_to_car("0" * 17) == set()
def test_fw_query_config(self):
for (ecu, addr, subaddr) in FW_QUERY_CONFIG.extra_ecus:
assert ecu in ECU_ADDRESSES, "Unknown ECU"
@@ -50,10 +61,13 @@ class TestFordFW:
@parameterized.expand(FW_VERSIONS.items())
def test_fw_versions(self, car_model: str, fw_versions: dict[tuple[int, int, int | None], Iterable[bytes]]):
for (ecu, addr, subaddr), fws in fw_versions.items():
assert ecu in ECU_PART_NUMBER, "Unexpected ECU"
assert ecu in ECU_ADDRESSES, "Unknown ECU"
assert addr == ECU_ADDRESSES[ecu], "ECU address mismatch"
assert subaddr is None, "Unexpected ECU subaddress"
if ecu not in ECU_PART_NUMBER:
continue
for fw in fws:
assert len(fw) == 24, "Expected ECU response to be 24 bytes"
opendbc_repo/opendbc/car/ford/values.py
+67 -10
View File
@@ -8,6 +8,7 @@ from opendbc.car.lateral import AngleSteeringLimits
from opendbc.car.structs import CarParams
from opendbc.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Column
from opendbc.car.fw_query_definitions import FwQueryConfig, LiveFwVersions, OfflineFwVersions, Request, StdQueries, p16
from opendbc.car.vin import Vin, is_valid_vin
Ecu = CarParams.Ecu
@@ -53,11 +54,15 @@ class FordFlags(IntFlag):
# Static flags
CANFD = 1
LKA_STEERING = 2
ALT_STEER_ANGLE = 4
HEV_CLUSTER_DATA = 8
HEV_BATTERY_DATA = 16
class RADAR:
DELPHI_ESR = 'ford_fusion_2018_adas'
DELPHI_MRR = 'FORD_CADS'
STEER_ASSIST_DATA = 'ford_lincoln_base_pt'
class Footnote(Enum):
@@ -87,6 +92,10 @@ class FordCarDocs(CarDocs):
@dataclass
class FordPlatformConfig(PlatformConfig):
wmis: set[str] = field(default_factory=set)
vds_codes: set[str] = field(default_factory=set)
years: set[str] = field(default_factory=set)
dbc_dict: DbcDict = field(default_factory=lambda: {
Bus.pt: 'ford_lincoln_base_pt',
Bus.radar: RADAR.DELPHI_MRR,
@@ -106,6 +115,7 @@ class FordPlatformConfig(PlatformConfig):
class FordCANFDPlatformConfig(FordPlatformConfig):
dbc_dict: DbcDict = field(default_factory=lambda: {
Bus.pt: 'ford_lincoln_base_pt',
Bus.radar: RADAR.STEER_ASSIST_DATA,
})
def init(self):
@@ -129,10 +139,23 @@ class FordF150LightningPlatform(FordCANFDPlatformConfig):
self.car_docs = []
MY_2020, MY_2021, MY_2022, MY_2023, MY_2024, MY_2025 = 'L', 'M', 'N', 'P', 'R', 'S'
F150_VDS_CODES = {'F1C', 'F1E', 'W1C', 'W1E', 'X1C', 'X1E', 'W1R', 'W1P', 'W1S', 'W1T'}
F150_ELECTRIC_CODES = {'L', 'V'}
MACH_E_VDS_CODES = {'K1R', 'K1S', 'K2S', 'K3R', 'K3S', 'K4S'}
class CAR(Platforms):
FORD_BRONCO_SPORT_MK1 = FordPlatformConfig(
[FordCarDocs("Ford Bronco Sport 2021-24")],
CarSpecs(mass=1625, wheelbase=2.67, steerRatio=17.7),
wmis={'3FM'}, vds_codes={'CR9'}, years={MY_2021, MY_2022, MY_2023, MY_2024},
)
FORD_EDGE_MK2 = FordPlatformConfig(
[FordCarDocs("Ford Edge 2022")],
CarSpecs(mass=1933, wheelbase=2.824, steerRatio=15.3),
flags=FordFlags.ALT_STEER_ANGLE,
wmis={'2FM'}, vds_codes={'PK4'}, years={MY_2022},
)
FORD_ESCAPE_MK4 = FordPlatformConfig(
[
@@ -140,6 +163,7 @@ class CAR(Platforms):
FordCarDocs("Ford Kuga 2020-23", "Adaptive Cruise Control with Lane Centering", hybrid=True, plug_in_hybrid=True),
],
CarSpecs(mass=1750, wheelbase=2.71, steerRatio=16.7),
wmis={'1FM'}, vds_codes={'CU0', 'CU9'}, years={MY_2020, MY_2021, MY_2022},
)
FORD_ESCAPE_MK4_5 = FordCANFDPlatformConfig(
[
@@ -148,6 +172,7 @@ class CAR(Platforms):
FordCarDocs("Ford Kuga Plug-in Hybrid 2024", "All"),
],
CarSpecs(mass=1750, wheelbase=2.71, steerRatio=16.7),
wmis={'1FM'}, vds_codes={'CU0', 'CU9'}, years={MY_2023, MY_2024},
)
FORD_EXPLORER_MK6 = FordPlatformConfig(
[
@@ -155,37 +180,49 @@ class CAR(Platforms):
FordCarDocs("Lincoln Aviator 2020-24", "Co-Pilot360 Plus", plug_in_hybrid=True), # Hybrid: Grand Touring only
],
CarSpecs(mass=2050, wheelbase=3.025, steerRatio=16.8),
wmis={'1FM', '5LM'}, vds_codes={'5K7', '5K8', '5J7'},
years={MY_2020, MY_2021, MY_2022, MY_2023, MY_2024},
)
FORD_EXPEDITION_MK4 = FordCANFDPlatformConfig(
[FordCarDocs("Ford Expedition 2022-24", "Co-Pilot360 Assist 2.0", hybrid=False)],
CarSpecs(mass=2000, wheelbase=3.69, steerRatio=17.0),
wmis={'1FM'}, vds_codes={'JU1', 'JU2', 'JK1'}, years={MY_2022, MY_2023, MY_2024},
)
FORD_F_150_MK14 = FordCANFDPlatformConfig(
[FordCarDocs("Ford F-150 2021-23", "Co-Pilot360 Assist 2.0", hybrid=True)],
CarSpecs(mass=2000, wheelbase=3.69, steerRatio=17.0),
CarSpecs(mass=3334, wheelbase=3.99, steerRatio=17.0),
wmis={'1FT'}, vds_codes=F150_VDS_CODES, years={MY_2021, MY_2022, MY_2023},
)
FORD_F_150_LIGHTNING_MK1 = FordF150LightningPlatform(
[FordCarDocs("Ford F-150 Lightning 2022-23", "Co-Pilot360 Assist 2.0")],
[FordCarDocs("Ford F-150 Lightning 2022-25", "Co-Pilot360 Assist 2.0")],
CarSpecs(mass=2948, wheelbase=3.70, steerRatio=16.9),
wmis={'1FT'}, vds_codes=F150_VDS_CODES, years={MY_2022, MY_2023, MY_2024, MY_2025},
)
FORD_FOCUS_MK4 = FordPlatformConfig(
[FordCarDocs("Ford Focus 2018", "Adaptive Cruise Control with Lane Centering", footnotes=[Footnote.FOCUS], hybrid=True)], # mHEV only
[FordCarDocs("Ford Focus 2018-22", "Adaptive Cruise Control with Lane Centering", footnotes=[Footnote.FOCUS], hybrid=True)], # mHEV only
CarSpecs(mass=1350, wheelbase=2.7, steerRatio=15.0),
)
FORD_MONDEO_MK5 = FordCANFDPlatformConfig(
[FordCarDocs("Ford Mondeo 2014-22", "Adaptive Cruise Control with Lane Centering")],
CarSpecs(mass=1550, wheelbase=2.85, steerRatio=14.8),
)
FORD_MAVERICK_MK1 = FordPlatformConfig(
[
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),
wmis={'3FT'}, vds_codes={'TW8'}, years={MY_2022, MY_2023, MY_2024},
)
FORD_MUSTANG_MACH_E_MK1 = FordCANFDPlatformConfig(
[FordCarDocs("Ford Mustang Mach-E 2021-24", "All", setup_video="https://www.youtube.com/watch?v=AR4_eTF3b_A")],
CarSpecs(mass=2200, wheelbase=2.984, steerRatio=17.0), # TODO: check steer ratio
wmis={'3FM'}, vds_codes=MACH_E_VDS_CODES, years={MY_2021, MY_2022, MY_2023, MY_2024},
)
FORD_RANGER_MK2 = FordCANFDPlatformConfig(
[FordCarDocs("Ford Ranger 2024", "Adaptive Cruise Control with Lane Centering", setup_video="https://www.youtube.com/watch?v=2oJlXCKYOy0")],
CarSpecs(mass=2000, wheelbase=3.27, steerRatio=17.0),
wmis={'1FT'}, vds_codes={'ER4'}, years={MY_2024},
)
FORD_TRANSIT_MK5 = FordLKASteeringPlatformConfig(
[FordCarDocs("Ford Transit 2025", "Co-Pilot360 Assist+")],
@@ -257,7 +294,25 @@ def match_fw_to_car_fuzzy(live_fw_versions: LiveFwVersions, vin: str, offline_fw
if valid_expected_ecus.issubset(valid_found_ecus):
candidates.add(candidate)
return candidates
return candidates or match_vin_to_car(vin)
def match_vin_to_car(vin: str) -> set[str]:
if not is_valid_vin(vin):
return set()
vin_obj = Vin(vin)
vds = vin[3:7]
model_year = vin[9]
candidates = {platform for platform in CAR if vin_obj.wmi in platform.config.wmis and
model_year in platform.config.years and
any(code in vds for code in platform.config.vds_codes)}
if {CAR.FORD_F_150_MK14, CAR.FORD_F_150_LIGHTNING_MK1} & candidates:
electric = vin[7] in F150_ELECTRIC_CODES
candidates.discard(CAR.FORD_F_150_MK14 if electric else CAR.FORD_F_150_LIGHTNING_MK1)
return {str(candidate) for candidate in candidates}
# All of these ECUs must be present and are expected to have platform codes we can match
@@ -266,10 +321,10 @@ PLATFORM_CODE_ECUS = (Ecu.abs, Ecu.fwdCamera, Ecu.fwdRadar, Ecu.eps)
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]),
(1, [Ecu.debug, Ecu.fwdCamera, Ecu.eps, Ecu.hud]),
(2, [Ecu.abs, Ecu.debug, Ecu.eps, Ecu.hud]),
(3, [Ecu.abs, Ecu.debug, Ecu.eps, Ecu.hud]),
(4, [Ecu.debug, Ecu.fwdCamera, Ecu.hud]),
(5, [Ecu.debug]),
(6, [Ecu.debug]),
(7, [Ecu.debug]),
@@ -297,13 +352,13 @@ 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],
whitelist_ecus=[Ecu.abs, Ecu.debug, Ecu.engine, Ecu.eps, Ecu.fwdCamera, Ecu.fwdRadar, Ecu.shiftByWire, Ecu.hud],
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],
whitelist_ecus=[Ecu.abs, Ecu.debug, Ecu.engine, Ecu.eps, Ecu.fwdCamera, Ecu.fwdRadar, Ecu.shiftByWire, Ecu.hud],
bus=0,
auxiliary=True,
),
@@ -320,7 +375,9 @@ FW_QUERY_CONFIG = FwQueryConfig(
# 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
(Ecu.hud, 0x720, None), # Instrument Cluster Module
],
non_essential_ecus={Ecu.eps: [CAR.FORD_F_150_LIGHTNING_MK1, CAR.FORD_EXPEDITION_MK4]},
# Custom fuzzy fingerprinting function using platform and model year hints
match_fw_to_car_fuzzy=match_fw_to_car_fuzzy,
)
opendbc_repo/opendbc/car/torque_data/override.toml
+2
View File
@@ -31,6 +31,7 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"]
"FORD_BRONCO_SPORT_MK1" = [nan, 1.5, nan]
"FORD_ESCAPE_MK4" = [nan, 1.5, nan]
"FORD_ESCAPE_MK4_5" = [nan, 1.5, nan]
"FORD_EDGE_MK2" = [nan, 1.5, nan]
"FORD_EXPLORER_MK6" = [nan, 1.5, nan]
"FORD_EXPEDITION_MK4" = [nan, 1.5, nan]
"FORD_F_150_MK14" = [nan, 1.5, nan]
@@ -39,6 +40,7 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"]
"FORD_F_150_LIGHTNING_MK1" = [nan, 1.5, nan]
"FORD_MUSTANG_MACH_E_MK1" = [nan, 1.5, nan]
"FORD_RANGER_MK2" = [nan, 1.5, nan]
"FORD_MONDEO_MK5" = [nan, 1.5, nan]
"FORD_TRANSIT_MK5" = [nan, 1.5, nan]
###
opendbc_repo/opendbc/safety/modes/ford.h
+133 -10
View File
@@ -87,6 +87,9 @@ static bool ford_get_quality_flag_valid(const CANPacket_t *msg) {
#define FORD_CANFD_INACTIVE_CURVATURE_RATE 1024U
static bool ford_lka_steering = false;
static bool ford_extended_lateral = false;
static bool ford_angle_mode = false;
static int16_t ford_shadow_curvature = 0;
// Curvature rate limits
#define FORD_LIMITS(limit_lateral_acceleration) { \
@@ -112,6 +115,59 @@ static bool ford_lka_steering = false;
static const AngleSteeringLimits FORD_STEERING_LIMITS = FORD_LIMITS(false);
#define FORD_EXTENDED_LIMITS(limit_lateral_acceleration) { \
.max_angle = 1000, \
.angle_deg_to_can = 50000, \
.max_angle_error = 100, \
.angle_rate_up_lookup = { \
{5., 16., 25.}, \
{0.0025, 0.0014, 0.00018} \
}, \
.angle_rate_down_lookup = { \
{5., 16., 25.}, \
{0.0025, 0.0014, 0.00018} \
}, \
.angle_error_min_speed = 10.0, \
.frequency = 20U, \
.angle_is_curvature = (limit_lateral_acceleration), \
.enforce_angle_error = true, \
.inactive_angle_is_zero = true, \
}
static const AngleSteeringLimits FORD_EXTENDED_STEERING_LIMITS = FORD_EXTENDED_LIMITS(false);
static int ford_desired_path_angle_last = 0;
static bool ford_path_angle_checks(int desired_path_angle, bool steer_control_enabled) {
bool violation = false;
if (steer_control_enabled) {
float speed = ((float)vehicle_speed.min / VEHICLE_SPEED_FACTOR) - 1.0;
const struct lookup_t path_angle_rate = {
.x = {10., 15., 25.},
.y = {0.0561, 0.04335, 0.00918},
};
int max_delta = (safety_interpolate(path_angle_rate, speed) * 2000.0) + 1.0;
violation |= safety_max_limit_check(desired_path_angle,
ford_desired_path_angle_last + max_delta,
ford_desired_path_angle_last - max_delta);
} else {
violation |= desired_path_angle != 0;
}
ford_desired_path_angle_last = violation ? 0 : desired_path_angle;
return violation;
}
static bool ford_shadow_curvature_check(int desired_curvature, bool steer_control_enabled,
const AngleSteeringLimits limits) {
if (steer_control_enabled && limits.enforce_angle_error &&
((vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR) > limits.angle_error_min_speed)) {
int lowest_allowed = angle_meas.min - limits.max_angle_error - 1;
int highest_allowed = angle_meas.max + limits.max_angle_error + 1;
return safety_max_limit_check(desired_curvature, highest_allowed, lowest_allowed);
}
return false;
}
static void ford_rx_hook(const CANPacket_t *msg) {
if (msg->bus == FORD_MAIN_BUS) {
// Update in motion state from standstill signal
@@ -235,6 +291,15 @@ static bool ford_tx_hook(const CANPacket_t *msg) {
if (!valid_lka_action) {
tx = false;
}
if (!ford_lka_steering) {
ford_angle_mode = (msg->data[4] & 0x1U) != 0U;
ford_extended_lateral = (msg->data[4] & 0x2U) != 0U;
ford_shadow_curvature = (int16_t)((msg->data[5] << 8) | msg->data[6]);
if (ford_angle_mode && !ford_extended_lateral) {
tx = false;
}
}
}
// Safety check for LateralMotionControl action
@@ -246,12 +311,38 @@ static bool ford_tx_hook(const CANPacket_t *msg) {
unsigned int raw_path_angle = (msg->data[3] << 3) | (msg->data[4] >> 5);
unsigned int raw_path_offset = (msg->data[5] << 2) | (msg->data[6] >> 6);
// These signals are not yet tested with the current safety limits
bool violation = (raw_curvature_rate != FORD_INACTIVE_CURVATURE_RATE) || (raw_path_angle != FORD_INACTIVE_PATH_ANGLE) || (raw_path_offset != FORD_INACTIVE_PATH_OFFSET);
// Check angle error and steer_control_enabled
int desired_curvature = raw_curvature - FORD_INACTIVE_CURVATURE; // /FORD_STEERING_LIMITS.angle_deg_to_can to get real curvature
violation |= steer_angle_cmd_checks(desired_curvature, steer_control_enabled, FORD_STEERING_LIMITS);
int desired_curvature_rate = raw_curvature_rate - FORD_INACTIVE_CURVATURE_RATE;
int desired_path_angle = raw_path_angle - FORD_INACTIVE_PATH_ANGLE;
int desired_path_offset = raw_path_offset - FORD_INACTIVE_PATH_OFFSET;
bool violation = false;
if (ford_extended_lateral) {
violation |= desired_path_offset != 0;
violation |= (desired_curvature_rate < -4096) || (desired_curvature_rate > 4095);
violation |= ford_path_angle_checks(desired_path_angle, steer_control_enabled);
if (ford_angle_mode) {
violation |= (desired_path_angle < -1000) || (desired_path_angle > 1047);
violation |= desired_curvature != 0;
violation |= steer_control_enabled && !(aol_allowed || controls_allowed);
int shadow_curvature_can = ROUND((float)ford_shadow_curvature * 0.05);
violation |= ford_shadow_curvature_check(shadow_curvature_can, steer_control_enabled,
FORD_EXTENDED_STEERING_LIMITS);
desired_angle_last = 0;
} else {
violation |= desired_path_angle != 0;
violation |= steer_angle_cmd_checks(desired_curvature, steer_control_enabled,
FORD_EXTENDED_STEERING_LIMITS);
}
if (!steer_control_enabled) {
violation |= (desired_curvature != 0) || (desired_curvature_rate != 0);
}
} else {
violation |= (raw_curvature_rate != FORD_INACTIVE_CURVATURE_RATE) ||
(raw_path_angle != FORD_INACTIVE_PATH_ANGLE) ||
(raw_path_offset != FORD_INACTIVE_PATH_OFFSET);
violation |= steer_angle_cmd_checks(desired_curvature, steer_control_enabled, FORD_STEERING_LIMITS);
}
if (violation) {
tx = false;
@@ -261,6 +352,7 @@ static bool ford_tx_hook(const CANPacket_t *msg) {
// Safety check for LateralMotionControl2 action
if (msg->addr == FORD_LateralMotionControl2) {
static const AngleSteeringLimits FORD_CANFD_STEERING_LIMITS = FORD_LIMITS(true);
static const AngleSteeringLimits FORD_CANFD_EXTENDED_STEERING_LIMITS = FORD_EXTENDED_LIMITS(true);
// Signal: LatCtl_D2_Rq
bool steer_control_enabled = ((msg->data[0] >> 4) & 0x7U) != 0U;
@@ -269,12 +361,39 @@ static bool ford_tx_hook(const CANPacket_t *msg) {
unsigned int raw_path_angle = ((msg->data[3] & 0x1FU) << 6) | (msg->data[4] >> 2);
unsigned int raw_path_offset = ((msg->data[4] & 0x3U) << 8) | msg->data[5];
// These signals are not yet tested with the current safety limits
bool violation = (raw_curvature_rate != FORD_CANFD_INACTIVE_CURVATURE_RATE) || (raw_path_angle != FORD_INACTIVE_PATH_ANGLE) || (raw_path_offset != FORD_INACTIVE_PATH_OFFSET);
// Check angle error and steer_control_enabled
int desired_curvature = raw_curvature - FORD_INACTIVE_CURVATURE; // /FORD_STEERING_LIMITS.angle_deg_to_can to get real curvature
violation |= steer_angle_cmd_checks(desired_curvature, steer_control_enabled, FORD_CANFD_STEERING_LIMITS);
int desired_curvature_rate = raw_curvature_rate - FORD_CANFD_INACTIVE_CURVATURE_RATE;
int desired_path_angle = raw_path_angle - FORD_INACTIVE_PATH_ANGLE;
int desired_path_offset = raw_path_offset - FORD_INACTIVE_PATH_OFFSET;
bool violation = false;
if (ford_extended_lateral) {
violation |= desired_path_offset != 0;
violation |= (desired_curvature_rate < -1024) || (desired_curvature_rate > 1023);
violation |= ford_path_angle_checks(desired_path_angle, steer_control_enabled);
if (ford_angle_mode) {
violation |= (desired_path_angle < -1000) || (desired_path_angle > 1047);
violation |= desired_curvature != 0;
violation |= steer_control_enabled && !(aol_allowed || controls_allowed);
int shadow_curvature_can = ROUND((float)ford_shadow_curvature * 0.05);
violation |= ford_shadow_curvature_check(shadow_curvature_can, steer_control_enabled,
FORD_CANFD_EXTENDED_STEERING_LIMITS);
desired_angle_last = 0;
} else {
violation |= desired_path_angle != 0;
violation |= steer_angle_cmd_checks(desired_curvature, steer_control_enabled,
FORD_CANFD_EXTENDED_STEERING_LIMITS);
}
if (!steer_control_enabled) {
violation |= (desired_curvature != 0) || (desired_curvature_rate != 0);
}
} else {
violation |= (raw_curvature_rate != FORD_CANFD_INACTIVE_CURVATURE_RATE) ||
(raw_path_angle != FORD_INACTIVE_PATH_ANGLE) ||
(raw_path_offset != FORD_INACTIVE_PATH_OFFSET);
violation |= steer_angle_cmd_checks(desired_curvature, steer_control_enabled,
FORD_CANFD_STEERING_LIMITS);
}
if (violation) {
tx = false;
@@ -328,6 +447,10 @@ static safety_config ford_init(uint16_t param) {
const uint16_t FORD_PARAM_LKA_STEERING = 4;
const bool ford_canfd = GET_FLAG(param, FORD_PARAM_CANFD);
ford_lka_steering = GET_FLAG(param, FORD_PARAM_LKA_STEERING);
ford_extended_lateral = false;
ford_angle_mode = false;
ford_shadow_curvature = 0;
ford_desired_path_angle_last = 0;
bool ford_longitudinal = false;
opendbc_repo/opendbc/safety/tests/test_ford.py
+32
View File
@@ -410,6 +410,38 @@ class TestFordCANFDStockSafety(TestFordSafetyBase):
self.safety.set_safety_hooks(CarParams.SafetyModel.ford, FordSafetyFlags.CANFD)
self.safety.init_tests()
def _extended_lka_msg(self, angle_mode=False, shadow_curvature=0.0):
msg = self._lkas_command_msg(0)
raw_shadow = int(round(shadow_curvature / 1e-6)) & 0xFFFF
msg[0].data[4] |= 0x2 | int(angle_mode)
msg[0].data[5] = raw_shadow >> 8
msg[0].data[6] = raw_shadow & 0xFF
return msg
def test_extended_curvature_signals(self):
speed = 15.0
self.safety.set_controls_allowed(True)
self._reset_curvature_measurement(0.0, speed)
self.assertTrue(self._tx(self._extended_lka_msg()))
self.assertTrue(self._tx(self._lat_ctl_msg(True, 0.0, 0.0, 0.001, 0.0005)))
self.assertTrue(self._tx(self._extended_lka_msg()))
self.assertFalse(self._tx(self._lat_ctl_msg(True, 0.1, 0.0, 0.001, 0.0005)))
def test_extended_angle_signals(self):
speed = 15.0
curvature = 0.005
self.safety.set_controls_allowed(True)
self._reset_curvature_measurement(curvature, speed)
self.assertTrue(self._tx(self._extended_lka_msg(angle_mode=True, shadow_curvature=curvature)))
self.assertTrue(self._tx(self._lat_ctl_msg(True, 0.0, 0.02, 0.0, 0.0)))
self.assertTrue(self._tx(self._extended_lka_msg(angle_mode=True, shadow_curvature=curvature)))
self.assertFalse(self._tx(self._lat_ctl_msg(True, 0.0, 0.2, 0.0, 0.0)))
self.assertTrue(self._tx(self._extended_lka_msg(angle_mode=True, shadow_curvature=-curvature)))
self.assertFalse(self._tx(self._lat_ctl_msg(True, 0.0, 0.01, 0.0, 0.0)))
class TestFordLongitudinalSafetyBase(TestFordSafetyBase):
MAX_ACCEL = 2.0 # accel is used for brakes, but openpilot can set positive values
selfdrive/ui/layouts/settings/starpilot/lateral.py
+113 -1
View File
@@ -5,6 +5,7 @@ from openpilot.selfdrive.ui.lib.starpilot_state import starpilot_state
from openpilot.system.ui.lib.application import gui_app, FontWeight
from openpilot.system.ui.lib.multilang import tr, tr_noop
from openpilot.system.ui.widgets import DialogResult
from openpilot.system.ui.widgets.option_dialog import MultiOptionDialog
from openpilot.selfdrive.ui.layouts.settings.starpilot.panel import _SettingsPage
from openpilot.selfdrive.ui.layouts.settings.starpilot.aethergrid import (
@@ -52,7 +53,7 @@ class SteeringManagerView(CardHubManagerView):
super().__init__(controller, [], **kwargs)
def _build_cards(self):
return [
cards = [
{
"title": tr("Steering Behavior"),
"desc": tr("Configure Always On Lateral (AOL), pause speed thresholds, and turn signal behaviors."),
@@ -72,6 +73,14 @@ class SteeringManagerView(CardHubManagerView):
"on_click": lambda: self._controller._navigate_to("advanced"),
},
]
if starpilot_state.car_state.isFord:
cards.append({
"title": tr("Ford Lateral Tuning"),
"desc": tr("Select the Ford steering strategy and tune prediction, lane-change, and speed response."),
"icon": "steering",
"on_click": lambda: self._controller._navigate_to("ford"),
})
return cards
# ═══════════════════════════════════════════════════════════════
@@ -319,6 +328,88 @@ class StarPilotLateralLayout(_SettingsPage):
),
]
# ── 4. Ford Lateral Tuning ──
def ford_curvature_mode():
return p.get_int("FordLateralMode") == 1
def ford_angle_mode():
return p.get_int("FordLateralMode") == 2
def ford_enhanced_mode():
return p.get_int("FordLateralMode") != 0
self._ford_rows = [
SettingRow(
"FordLateralMode", "value", tr_noop("Steering Strategy"),
subtitle=tr_noop("Native keeps the existing Ford controls. Curvature and Angle enable the enhanced Ford strategies."),
get_value=self._get_ford_lateral_mode,
on_click=self._show_ford_lateral_mode,
),
SettingRow(
"FordHumanTurnDetection", "toggle", tr_noop("Manual Turn Release"),
subtitle=tr_noop("Release lateral control during a sustained hands-on turn, then ramp back in smoothly."),
get_state=lambda: p.get_bool("FordHumanTurnDetection"),
set_state=lambda s: p.put_bool("FordHumanTurnDetection", s),
visible=ford_enhanced_mode,
),
SettingRow(
"FordCurvatureBlendLow", "value", tr_noop("Small-Curve Prediction"),
subtitle=tr_noop("Blend model-predicted curvature into gentle turns."),
get_value=lambda: f"{p.get_float('FordCurvatureBlendLow') * 100:.0f}%",
on_click=lambda: self._show_slider("FordCurvatureBlendLow", 0.0, 1.0, step=0.05, unit="", value_type="float"),
visible=ford_curvature_mode,
),
SettingRow(
"FordCurvatureBlendHigh", "value", tr_noop("Large-Curve Prediction"),
subtitle=tr_noop("Blend model-predicted curvature into tighter turns."),
get_value=lambda: f"{p.get_float('FordCurvatureBlendHigh') * 100:.0f}%",
on_click=lambda: self._show_slider("FordCurvatureBlendHigh", 0.0, 1.0, step=0.05, unit="", value_type="float"),
visible=ford_curvature_mode,
),
SettingRow(
"FordCurvatureLaneChangeFactor", "value", tr_noop("Curvature Lane-Change Factor"),
subtitle=tr_noop("Scale steering during high-speed lane changes in Curvature mode."),
get_value=lambda: f"{p.get_float('FordCurvatureLaneChangeFactor'):.2f}x",
on_click=lambda: self._show_slider("FordCurvatureLaneChangeFactor", 0.5, 1.25, step=0.05, unit="x", value_type="float"),
visible=ford_curvature_mode,
),
SettingRow(
"FordAngleBlend", "value", tr_noop("Angle Prediction Blend"),
subtitle=tr_noop("Blend model prediction into the path-angle command."),
get_value=lambda: f"{p.get_float('FordAngleBlend') * 100:.0f}%",
on_click=lambda: self._show_slider("FordAngleBlend", 0.0, 1.0, step=0.05, unit="", value_type="float"),
visible=ford_angle_mode,
),
SettingRow(
"FordAngleLowSpeedFactor", "value", tr_noop("Low-Speed Angle Response"),
subtitle=tr_noop("Adjust path-angle strength at lower speeds and higher curvature."),
get_value=lambda: f"{p.get_float('FordAngleLowSpeedFactor'):.2f}x",
on_click=lambda: self._show_slider("FordAngleLowSpeedFactor", 0.5, 1.5, step=0.05, unit="x", value_type="float"),
visible=ford_angle_mode,
),
SettingRow(
"FordAngleHighSpeedFactor", "value", tr_noop("High-Speed Angle Response"),
subtitle=tr_noop("Adjust path-angle strength through larger highway curves."),
get_value=lambda: f"{p.get_float('FordAngleHighSpeedFactor'):.2f}x",
on_click=lambda: self._show_slider("FordAngleHighSpeedFactor", 0.5, 1.5, step=0.05, unit="x", value_type="float"),
visible=ford_angle_mode,
),
SettingRow(
"FordAngleHighSpeedDamping", "value", tr_noop("High-Speed Damping"),
subtitle=tr_noop("Dampen small steering corrections at highway speed."),
get_value=lambda: f"{p.get_float('FordAngleHighSpeedDamping'):.2f}x",
on_click=lambda: self._show_slider("FordAngleHighSpeedDamping", 0.25, 1.25, step=0.05, unit="x", value_type="float"),
visible=ford_angle_mode,
),
SettingRow(
"FordAngleLaneChangeFactor", "value", tr_noop("Angle Lane-Change Factor"),
subtitle=tr_noop("Scale steering during high-speed lane changes in Angle mode."),
get_value=lambda: f"{p.get_float('FordAngleLaneChangeFactor'):.2f}x",
on_click=lambda: self._show_slider("FordAngleLaneChangeFactor", 0.5, 1.5, step=0.05, unit="x", value_type="float"),
visible=ford_angle_mode,
),
]
self._manager_view = SteeringManagerView(
self,
header_title=tr_noop("Steering"),
@@ -362,6 +453,13 @@ class StarPilotLateralLayout(_SettingsPage):
parent_toggle=pt_advanced,
panel_style=PANEL_STYLE,
)
self._sub_panels["ford"] = AetherSettingsView(
self,
[SettingSection(title="", rows=self._ford_rows)],
header_title=tr_noop("Ford Lateral Tuning"),
header_subtitle=tr_noop("Tune Ford-specific polynomial steering while retaining the native strategy as a fallback."),
panel_style=PANEL_STYLE,
)
self._wire_sub_panels()
def _on_pause_lateral_speed_clicked(self):
@@ -402,3 +500,17 @@ class StarPilotLateralLayout(_SettingsPage):
current = self._params.get_int("LaneChangeSmoothing") if self._params.get_int("LaneChangeSmoothing") > 0 else 5
gui_app.push_widget(AetherSliderDialog(tr("Lane Change Smoothing"), 1, 10, 1, current, on_close,
color=self.SLIDER_COLOR))
def _get_ford_lateral_mode(self) -> str:
return tr(("Native", "Curvature", "Angle")[max(0, min(2, self._params.get_int("FordLateralMode")))])
def _show_ford_lateral_mode(self):
options = [tr("Native"), tr("Curvature"), tr("Angle")]
current = options[max(0, min(2, self._params.get_int("FordLateralMode")))]
def on_select(res):
if res == DialogResult.CONFIRM and dialog.selection in options:
self._params.put_int("FordLateralMode", options.index(dialog.selection))
dialog = MultiOptionDialog(tr("Ford Steering Strategy"), options, current, callback=on_select)
gui_app.push_widget(dialog)
selfdrive/ui/lib/starpilot_state.py
+3
View File
@@ -15,6 +15,7 @@ from openpilot.starpilot.common.lateral_delay import full_lateral_delay
class StarPilotCarState:
# ========== Car Type Detection ==========
isGM: bool = False
isFord: bool = False
isHKG: bool = False
isJeep: bool = False
isToyota: bool = False
@@ -91,6 +92,7 @@ class StarPilotState:
brand = FINGERPRINT_MAKE_TO_VALUES_DIR.get(fallback_make_lower, fallback_make_lower)
fallback_model_str = fallback_model or ""
self.car_state.isGM = brand == "gm"
self.car_state.isFord = brand == "ford"
self.car_state.isHKG = brand == "hyundai"
self.car_state.isJeep = brand == "chrysler" and fallback_model_str.startswith("JEEP_")
self.car_state.isSubaru = brand == "subaru"
@@ -164,6 +166,7 @@ class StarPilotState:
self.car_state.isAngleCar = self._safe_get(CP, "steerControlType", None) == car.CarParams.SteerControlType.angle
self.car_state.isBolt = car_fingerprint.startswith("CHEVROLET_BOLT")
self.car_state.isGM = car_make == "gm"
self.car_state.isFord = car_make == "ford"
self.car_state.isHKG = car_make == "hyundai"
self.car_state.isHKGCanFd = self.car_state.isHKG and safety_model == car.CarParams.SafetyModel.hyundaiCanfd
self.car_state.isJeep = car_make == "chrysler" and car_fingerprint.startswith("JEEP_")
starpilot/car/__init__.py
+1
View File
@@ -0,0 +1 @@
"""Vehicle-specific StarPilot extensions."""
starpilot/car/ford/__init__.py
+1
View File
@@ -0,0 +1 @@
"""Ford-specific control extensions."""
starpilot/car/ford/fordcan.py
+53
View File
@@ -0,0 +1,53 @@
from opendbc.car.ford.fordcan import CanBus, calculate_lat_ctl2_checksum
SHADOW_CURVATURE_SCALE = 1e-6
def create_lka_msg(packer, CAN: CanBus, angle_mode: bool = False, shadow_curvature: float = 0.0,
extended_mode: bool = True):
addr, dat, bus = packer.make_can_msg("Lane_Assist_Data1", CAN.main, {})
dat = bytearray(dat)
shadow_raw = int(round(shadow_curvature / SHADOW_CURVATURE_SCALE))
shadow_raw = max(-32768, min(32767, shadow_raw)) & 0xFFFF
dat[4] |= int(angle_mode) | (int(extended_mode) << 1)
dat[5] = shadow_raw >> 8
dat[6] = shadow_raw & 0xFF
return addr, bytes(dat), bus
def create_lat_ctl_msg(packer, CAN: CanBus, active: bool, ramp_type: int, precision_type: int,
path_offset: float, path_angle: float, curvature: float, curvature_rate: float):
values = {
"LatCtlRng_L_Max": 0,
"HandsOffCnfm_B_Rq": 0,
"LatCtl_D_Rq": 1 if active else 0,
"LatCtlRampType_D_Rq": ramp_type,
"LatCtlPrecision_D_Rq": precision_type,
"LatCtlPathOffst_L_Actl": path_offset,
"LatCtlPath_An_Actl": path_angle,
"LatCtlCurv_NoRate_Actl": curvature_rate,
"LatCtlCurv_No_Actl": curvature,
}
return packer.make_can_msg("LateralMotionControl", CAN.main, values)
def create_lat_ctl2_msg(packer, CAN: CanBus, mode: int, ramp_type: int, precision_type: int,
path_offset: float, path_angle: float, curvature: float,
curvature_rate: float, counter: int):
values = {
"LatCtl_D2_Rq": mode,
"LatCtlRampType_D_Rq": ramp_type,
"LatCtlPrecision_D_Rq": precision_type,
"LatCtlPathOffst_L_Actl": path_offset,
"LatCtlPath_An_Actl": path_angle,
"LatCtlCurv_No_Actl": curvature,
"LatCtlCrv_NoRate2_Actl": curvature_rate,
"HandsOffCnfm_B_Rq": 0,
"LatCtlPath_No_Cnt": counter,
"LatCtlPath_No_Cs": 0,
}
dat = packer.make_can_msg("LateralMotionControl2", 0, values)[1]
values["LatCtlPath_No_Cs"] = calculate_lat_ctl2_checksum(mode, counter, dat)
return packer.make_can_msg("LateralMotionControl2", CAN.main, values)
starpilot/car/ford/lateral.py
+275
View File
@@ -0,0 +1,275 @@
from collections import deque
from dataclasses import dataclass
from enum import IntEnum
import numpy as np
from opendbc.car import ACCELERATION_DUE_TO_GRAVITY, DT_CTRL
from opendbc.car.ford.values import CAR, CarControllerParams, FordFlags
from opendbc.car.lateral import AngleSteeringLimits, ISO_LATERAL_ACCEL, apply_std_steer_angle_limits
from openpilot.common.params import Params
from openpilot.selfdrive.modeld.constants import ModelConstants
class FordLateralMode(IntEnum):
native = 0
curvature = 1
angle = 2
FORD_ANGLE_LIMITS = AngleSteeringLimits(
0.02,
([5, 16, 25], [0.0025, 0.0012, 0.00008]),
([5, 16, 25], [0.0025, 0.0014, 0.00018]),
)
MAX_LATERAL_ACCEL = ISO_LATERAL_ACCEL - ACCELERATION_DUE_TO_GRAVITY * 0.06
PATH_ANGLE_MIN = -0.5
PATH_ANGLE_MAX = 0.5235
STEER_DT = CarControllerParams.STEER_STEP * DT_CTRL
CANFD_BODY_ON_FRAME = frozenset({
CAR.FORD_F_150_MK14,
CAR.FORD_F_150_LIGHTNING_MK1,
CAR.FORD_EXPEDITION_MK4,
CAR.FORD_RANGER_MK2,
})
CANFD_UNIBODY = frozenset({
CAR.FORD_MUSTANG_MACH_E_MK1,
CAR.FORD_ESCAPE_MK4_5,
})
@dataclass(frozen=True)
class FordLateralResult:
curvature: float = 0.0
curvature_rate: float = 0.0
path_offset: float = 0.0
path_angle: float = 0.0
ramp_type: int = 0
precision_type: int = 1
active: bool = False
shadow_curvature: float = 0.0
class HumanTurnDetector:
ANGLE_DEG = 45.0
HOLD_SECONDS = 1.5
PRETURNED_HOLD_SECONDS = 3.0
def __init__(self):
self.timer = 0.0
self.active = False
self._pressed_last = False
self._press_started_preturned = False
def update(self, enabled: bool, steering_pressed: bool, steering_angle_deg: float) -> bool:
if steering_pressed and not self._pressed_last:
self._press_started_preturned = abs(steering_angle_deg) > self.ANGLE_DEG
self._pressed_last = steering_pressed
if enabled and steering_pressed and abs(steering_angle_deg) > self.ANGLE_DEG:
self.timer += STEER_DT
else:
self.timer = 0.0
hold_time = self.PRETURNED_HOLD_SECONDS if self._press_started_preturned else self.HOLD_SECONDS
self.active = self.timer + 1e-9 >= hold_time
return self.active
def reset(self):
self.timer = 0.0
self.active = False
self._pressed_last = False
self._press_started_preturned = False
class FordLateralController:
"""Ford polynomial lateral strategies kept outside the native car implementation."""
def __init__(self, CP):
self.CP = CP
self.params = Params(return_defaults=True)
try:
import cereal.messaging as messaging
self.sm = messaging.SubMaster(["modelV2", "liveDelay"])
except ImportError:
# The host interface tests don't load the device messaging extension.
self.sm = None
self.model = None
self.mode = FordLateralMode.curvature
self.human_turn_enabled = True
self.curvature_blend_low = 0.4
self.curvature_blend_high = 0.4
self.angle_blend = 0.5
self.curvature_lane_change_factor = 0.85
self.angle_lane_change_factor = 1.0
self.angle_low_speed_factor = 1.0
self.angle_high_speed_factor = 1.0
self.angle_high_speed_damping = 1.0
self.human_turn = HumanTurnDetector()
self.curvature_samples = deque(maxlen=max(2, round(0.3 / STEER_DT)))
self.path_angle_last = 0.0
self.curvature_last = 0.0
self._frame = 0
self._update_params()
def _update_params(self):
try:
self.mode = FordLateralMode(int(np.clip(self.params.get_int("FordLateralMode", return_default=True), 0, 2)))
except ValueError:
self.mode = FordLateralMode.native
self.human_turn_enabled = self.params.get_bool("FordHumanTurnDetection")
self.curvature_blend_low = float(np.clip(self.params.get_float("FordCurvatureBlendLow", return_default=True), 0.0, 1.0))
self.curvature_blend_high = float(np.clip(self.params.get_float("FordCurvatureBlendHigh", return_default=True), 0.0, 1.0))
self.angle_blend = float(np.clip(self.params.get_float("FordAngleBlend", return_default=True), 0.0, 1.0))
self.curvature_lane_change_factor = float(np.clip(
self.params.get_float("FordCurvatureLaneChangeFactor", return_default=True), 0.5, 1.25))
self.angle_lane_change_factor = float(np.clip(
self.params.get_float("FordAngleLaneChangeFactor", return_default=True), 0.5, 1.5))
self.angle_low_speed_factor = float(np.clip(
self.params.get_float("FordAngleLowSpeedFactor", return_default=True), 0.5, 1.5))
self.angle_high_speed_factor = float(np.clip(
self.params.get_float("FordAngleHighSpeedFactor", return_default=True), 0.5, 1.5))
self.angle_high_speed_damping = float(np.clip(
self.params.get_float("FordAngleHighSpeedDamping", return_default=True), 0.25, 1.25))
def update_inputs(self):
if self.sm is not None:
self.sm.update(0)
if self.sm.updated["modelV2"]:
self.model = self.sm["modelV2"]
if self._frame % 100 == 0:
self._update_params()
self._frame += 1
def _predicted_curvature(self, v_ego: float, lookup_time: float) -> float:
if self.model is None or len(self.model.orientationRate.z) < 17:
return 0.0
curvatures = np.asarray(self.model.orientationRate.z) / max(v_ego, 0.01)
return float(np.interp(lookup_time, ModelConstants.T_IDXS, curvatures))
def _lane_change(self) -> tuple[bool, int]:
if self.model is None:
return False, 0
state = int(self.model.meta.laneChangeState)
return state in (1, 2, 3), int(self.model.meta.laneChangeDirection)
@staticmethod
def _current_curvature(CS) -> float:
return -CS.out.yawRate / max(CS.out.vEgoRaw, 0.1)
def _blend_and_scale(self, desired: float, predicted: float, v_ego: float, angle_mode: bool) -> tuple[float, int]:
if angle_mode:
blend = self.angle_blend
high_factor = self.angle_lane_change_factor
else:
blend = float(np.interp(abs(desired), [0.0, 0.001], [self.curvature_blend_low, self.curvature_blend_high]))
high_factor = self.curvature_lane_change_factor
requested = predicted * blend + desired * (1.0 - blend)
lane_change, direction = self._lane_change()
precision = 1
if lane_change:
factor = float(np.interp(v_ego, [4.4, 40.23], [0.95, high_factor]))
if (direction == 1 and requested < 0.0) or (direction == 2 and requested > 0.0):
requested *= factor
precision = 0
return requested, precision
def _manual_turn(self, CC, CS) -> bool:
if not CC.latActive:
self.human_turn.reset()
return False
return self.human_turn.update(
self.human_turn_enabled, CS.out.steeringPressed, CS.out.steeringAngleDeg)
def update_curvature(self, CC, CS, actuators) -> FordLateralResult:
if not CC.latActive or self._manual_turn(CC, CS) or CS.out.vEgoRaw < 0.1:
self.curvature_samples.clear()
self.curvature_last = 0.0
return FordLateralResult(shadow_curvature=self._current_curvature(CS))
v_ego = float(CS.out.vEgoRaw)
predicted = self._predicted_curvature(v_ego, 0.2)
requested, precision = self._blend_and_scale(float(actuators.curvature), predicted, v_ego, False)
current = self._current_curvature(CS)
if v_ego > 9.0:
requested = float(np.clip(requested, current - CarControllerParams.CURVATURE_ERROR,
current + CarControllerParams.CURVATURE_ERROR))
applied = float(apply_std_steer_angle_limits(
requested, self.curvature_last, v_ego, CS.out.steeringAngleDeg, True, FORD_ANGLE_LIMITS))
if self.CP.flags & FordFlags.CANFD:
max_curvature = MAX_LATERAL_ACCEL / max(v_ego, 1.0) ** 2
applied = float(np.clip(applied, -max_curvature, max_curvature))
self.curvature_samples.append(predicted)
curvature_rate = 0.0
if len(self.curvature_samples) > 1:
sample_time = (len(self.curvature_samples) - 1) * STEER_DT
curvature_rate = (self.curvature_samples[-1] - self.curvature_samples[0]) / max(sample_time * v_ego, 0.01)
curvature_rate *= float(np.interp(abs(predicted), [0.0, 0.008, 0.01], [0.0, 0.0, 1.0]))
curvature_rate *= float(np.interp(v_ego, [0.0, 14.5, 15.5], [1.0, 1.0, 0.0]))
if self._lane_change()[0]:
curvature_rate = 0.0
self.curvature_last = float(np.clip(applied, -0.02, 0.02))
curvature_rate = float(np.clip(curvature_rate, -0.001024, 0.001023))
return FordLateralResult(
curvature=self.curvature_last,
curvature_rate=curvature_rate,
ramp_type=2,
precision_type=precision,
active=True,
)
def _platform_angle_gains(self) -> tuple[float, float]:
if self.CP.carFingerprint in CANFD_BODY_ON_FRAME:
return 0.95, 0.95
if self.CP.carFingerprint in CANFD_UNIBODY:
return 1.0, 1.05
return 1.0, 1.15
def update_angle(self, CC, CS, actuators) -> FordLateralResult:
current = self._current_curvature(CS)
if not CC.latActive or self._manual_turn(CC, CS):
self.path_angle_last = 0.0
return FordLateralResult(shadow_curvature=current)
v_ego = float(CS.out.vEgoRaw)
live_delay = 0.12 if self.sm is None else float(np.clip(self.sm["liveDelay"].lateralDelay, 0.1, 0.15))
speed_factor = float(np.interp(v_ego, [11.176, 24.587], [1.0, 0.0]))
curvature_factor = float(np.interp(abs(actuators.curvature), [0.005, 0.02], [1.0, 0.0]))
lookup_time = live_delay + 0.05 + 0.10 * speed_factor * curvature_factor
predicted = self._predicted_curvature(v_ego, lookup_time)
requested, precision = self._blend_and_scale(float(actuators.curvature), predicted, v_ego, True)
if v_ego > 9.0:
requested = float(np.clip(requested, current - CarControllerParams.CURVATURE_ERROR,
current + CarControllerParams.CURVATURE_ERROR))
low_gain_high_speed, high_gain_high_speed = self._platform_angle_gains()
low_gain = float(np.interp(v_ego, [13.5, 26.82],
[1.0, low_gain_high_speed * self.angle_high_speed_damping]))
high_gain = float(np.interp(v_ego, [13.5, 26.82],
[1.30 * self.angle_low_speed_factor,
high_gain_high_speed * self.angle_high_speed_factor]))
gain = float(np.interp(abs(requested), [0.0007, 0.001], [low_gain, high_gain]))
path_angle = float(np.clip(requested * v_ego * gain, PATH_ANGLE_MIN, PATH_ANGLE_MAX))
max_delta = float(np.interp(v_ego, [9.0, 10.0, 15.0, 25.0], [0.055, 0.055, 0.0425, 0.009]))
path_angle = float(np.clip(path_angle, self.path_angle_last - max_delta, self.path_angle_last + max_delta))
self.path_angle_last = path_angle
shadow = current if CS.out.steeringPressed else requested
return FordLateralResult(
path_angle=path_angle,
ramp_type=2,
precision_type=precision,
active=True,
shadow_curvature=shadow,
)
starpilot/car/ford/tests/__init__.py
View File
starpilot/car/ford/tests/test_lateral.py
+69
View File
@@ -0,0 +1,69 @@
import sys
from types import SimpleNamespace
import pytest
from ..lateral import FordLateralController, HumanTurnDetector
class FakeSubMaster(dict):
def __init__(self, services):
super().__init__({"liveDelay": SimpleNamespace(lateralDelay=0.12)})
self.updated = dict.fromkeys(services, False)
def update(self, timeout):
pass
@pytest.fixture
def controller(monkeypatch):
messaging = SimpleNamespace(SubMaster=FakeSubMaster)
monkeypatch.setitem(sys.modules, "cereal.messaging", messaging)
CP = SimpleNamespace(flags=0, carFingerprint="FORD_EDGE_MK2")
return FordLateralController(CP)
def car_state(speed=15.0, curvature=0.0, steering_pressed=False, steering_angle=0.0):
return SimpleNamespace(out=SimpleNamespace(
vEgoRaw=speed,
yawRate=-curvature * speed,
steeringPressed=steering_pressed,
steeringAngleDeg=steering_angle,
))
def test_human_turn_requires_sustained_input():
detector = HumanTurnDetector()
assert not detector.update(True, True, 0.0)
for _ in range(29):
assert not detector.update(True, True, 50.0)
assert detector.update(True, True, 50.0)
assert not detector.update(True, False, 50.0)
def test_curvature_strategy_uses_polynomial_signals(controller):
result = controller.update_curvature(
SimpleNamespace(latActive=True), car_state(), SimpleNamespace(curvature=0.001))
assert result.active
assert 0.0 < result.curvature <= 0.001
assert result.ramp_type == 2
def test_angle_strategy_uses_path_angle_and_shadow(controller):
result = controller.update_angle(
SimpleNamespace(latActive=True), car_state(curvature=0.001), SimpleNamespace(curvature=0.001))
assert result.active
assert result.curvature == 0.0
assert result.path_angle > 0.0
assert result.shadow_curvature == pytest.approx(0.001)
def test_manual_turn_releases_lateral(controller):
controller.human_turn_enabled = True
CC = SimpleNamespace(latActive=True)
CS = car_state(steering_pressed=True, steering_angle=50.0)
actuators = SimpleNamespace(curvature=0.001)
for _ in range(61):
result = controller.update_angle(CC, CS, actuators)
assert not result.active
assert result.path_angle == 0.0