From 49b651af7012f8a8d735f5ab83bbaa0801c4956f Mon Sep 17 00:00:00 2001 From: alan-polk <56047663+alan-polk@users.noreply.github.com> Date: Thu, 29 Oct 2026 17:08:32 -0500 Subject: [PATCH] openpilot: BluePilot lateral startup bits, per-frame inputs and settings registry - params_keys.h: every setting in the library's manifest (python -m opendbc.bluepilot_lateral.params.manifest), without BACKUP, which StarPilot's params don't have. disable_BP_lat_UI defaults to "1": BluePilot lateral is off until the user turns it on. - car init (VehicleStartupPreferences.prepare): init_ford_safety_param_starpilot ORs the library's startup bits into the Ford param. They stay off without the openpilot toggle, in Safe Mode, or when a startup setting can't be read. - card (configure_controller): the Ford CarController gets a BluePilotInputs, which fills the library's LateralInputs from modelV2, vehicleParameters and lateralDelay each frame and re-reads the per-frame settings every 3 s. - test_bluepilot_lateral.py: the init path's bits, the real CarController and libsafety with the same param, for a CAN and a CAN FD Ford, off/on/on+pinion/pinion-only, curvature and angle mode: every steering and LKA frame passes, and only angle mode with BluePilot on steers through the path angle. Co-Authored-By: Claude Opus 5.5 --- .../car/ford/tests/test_bluepilot_lateral.py | 141 ++++++++++++++++++ openpilot/common/params_keys.h | 19 +++ openpilot/starpilot/car/ford/bluepilot.py | 84 +++++++++++ openpilot/starpilot/controller_extensions.py | 4 + openpilot/starpilot/vehicle_preferences.py | 6 +- 5 files changed, 253 insertions(+), 1 deletion(-) create mode 100644 opendbc_repo/opendbc/car/ford/tests/test_bluepilot_lateral.py create mode 100644 openpilot/starpilot/car/ford/bluepilot.py diff --git a/opendbc_repo/opendbc/car/ford/tests/test_bluepilot_lateral.py b/opendbc_repo/opendbc/car/ford/tests/test_bluepilot_lateral.py new file mode 100644 index 0000000000..fc24044021 --- /dev/null +++ b/opendbc_repo/opendbc/car/ford/tests/test_bluepilot_lateral.py @@ -0,0 +1,141 @@ +"""BluePilot lateral end to end, without a device: StarPilot's car-init hook sets the startup bits, the real Ford +CarController runs with BluePilot inputs, and every steering and LKA frame it sends goes through libsafety configured +with the same Ford param.""" +import math + +import pytest + +from opendbc.can import CANPacker +from opendbc.car import gen_empty_fingerprint, structs +from opendbc.car.ford.carcontroller import CarController +from opendbc.car.ford.carstate import CarState +from opendbc.car.ford.interface import CarInterface +from opendbc.car.ford.values import CAR, DBC, FordFlags +from opendbc.bluepilot_lateral.core.carcontroller_ext import pinion_vehicle_model +from opendbc.bluepilot_lateral.core.values_ext import BP_LATERAL_PARAMS, FordSafetyFlagsSP +from opendbc.bluepilot_lateral.hosts.starpilot.lateral import FrameInputs +from opendbc.bluepilot_lateral.hosts.starpilot.safety_param import bp_safety_param, init_ford_safety_param_starpilot +from opendbc.bluepilot_lateral.params.manifest import value as manifest_value +from opendbc.bluepilot_lateral.params.param_store import ParamStore +from opendbc.safety.tests.libsafety import libsafety_py + +SPEED = 20.0 # m/s: above the 10 m/s gate of the angle-mode deviation check +T_IDXS = [10.0 * (i / 32) ** 2 for i in range(33)] +INACTIVE_PATH_ANGLE = 1000 +STARTUP = { + "off": {"disable_BP_lat_UI": "1"}, + "on": {"disable_BP_lat_UI": "0"}, + "on+pinion": {"disable_BP_lat_UI": "0", "FordPrefSteerAngleCurvature": "1"}, + "pinion-only": {"disable_BP_lat_UI": "1", "FordPrefSteerAngleCurvature": "1"}, +} + + +class StraightRoadInputs: + """A road of constant curvature, refreshed every frame like StarPilot's BluePilotInputs.""" + def __init__(self, settings): + self.settings = settings + self.curvature = 0.0 + + def update(self): + frame = FrameInputs(params=[FrameInputs.Param(k, v) for k, v in self.settings.items()]) + li = frame.lateralInputs + li.valid = True + li.pathYawRate = [self.curvature * SPEED] * 33 + li.pathX = [SPEED * t for t in T_IDXS] + li.pathY = [0.5 * self.curvature * (SPEED * t) ** 2 for t in T_IDXS] + li.laneLineLeftX = li.laneLineRightX = list(li.pathX) + li.laneLineLeftY = [y - 1.8 for y in li.pathY] + li.laneLineRightY = [y + 1.8 for y in li.pathY] + li.laneLineProbs = [0.1, 0.9, 0.9, 0.1] + li.laneLineStds = [1.0, 0.1, 0.1, 1.0] + li.steerRatio, li.stiffnessFactor, li.lateralDelay = 15.0, 1.0, 0.15 + return frame + + +def settings(startup, angle): + defaults = {k: manifest_value(ParamStore([]), k) for k in BP_LATERAL_PARAMS} + encoded = {k: (b"1" if v else b"0") if isinstance(v, bool) else str(v).encode() for k, v in defaults.items()} + encoded["disable_BP_lat_UI"] = startup["disable_BP_lat_UI"].encode() + encoded["FordPrefLateralControl"] = b"1" if angle else b"0" + return encoded + + +def path_angle(address, data): + return ((data[3] << 3) | (data[4] >> 5)) if address == 0x3D3 else (((data[3] & 0x1F) << 6) | (data[4] >> 2)) + + +@pytest.mark.parametrize("car", (CAR.FORD_BRONCO_SPORT_MK1, CAR.FORD_F_150_MK14)) +@pytest.mark.parametrize("startup", STARTUP) +@pytest.mark.parametrize("angle", (False, True), ids=("curvature", "angle")) +def test_every_steering_frame_passes_panda_safety(car, startup, angle): + fp = gen_empty_fingerprint() + fp[0][0x5A] = 8 + fp[2].update({0x3D6: 8, 0x186: 8}) + cp = CarInterface.get_params(car, fp, [], False, False, False) + init_ford_safety_param_starpilot(cp, STARTUP[startup]) + bits = bp_safety_param(cp) + lateral = startup in ("on", "on+pinion") + assert bool(bits & FordSafetyFlagsSP.BP_LATERAL) == lateral + assert bool(bits & FordSafetyFlagsSP.STEER_ANGLE_CURVATURE) == (startup == "on+pinion") + + controller = CarController(DBC[cp.carFingerprint], cp) + inputs = StraightRoadInputs(settings(STARTUP[startup], angle)) + controller.bluepilot_inputs = inputs + state = CarState(cp) + state.update(state.get_can_parsers(cp)) + vm = pinion_vehicle_model(cp) + + safety, packer = libsafety_py.libsafety, CANPacker("ford_lincoln_base_pt") + safety.init_tests() + safety.set_alternative_experience(0) + assert safety.set_safety_hooks(structs.CarParams.SafetyModel.ford, int(cp.safetyConfigs[-1].safetyParam)) == 0 + safety.set_controls_allowed(True) + + def rx(name, values): + address, data, bus = packer.make_can_msg(name, 0, values) + assert safety.safety_rx_hook(libsafety_py.make_CANPacket(address, bus, data)), name + + sent, blocked, path_angles = 0, [], set() + timer = 1_000_000 + try: + for frame in range(200): + curvature = 0.0015 * math.sin(frame / 60) + inputs.curvature = curvature + timer += 10_000 + safety.set_timer(timer) + # the car tracks the command: yaw (or the pinion angle) measures the commanded curvature + angle_deg = math.degrees(vm.get_steer_from_curvature(-curvature, SPEED, 0.0)) + if frame % 2 == 0: + rx("BrakeSysFeatures", {"Veh_V_ActlBrk": SPEED * 3.6, "VehVActlBrk_D_Qf": 3, "VehVActlBrk_No_Cnt": (frame // 2) % 16}) + rx("EngVehicleSpThrottle2", {"Veh_V_ActlEng": SPEED * 3.6, "VehVActlEng_D_Qf": 3}) + rx("DesiredTorqBrk", {"VehStop_D_Stat": 0}) + rx("Yaw_Data_FD1", {"VehYaw_W_Actl": -curvature * SPEED, "VehYawWActl_D_Qf": 3, "VehRollYaw_No_Cnt": frame % 256}) + rx("EngVehicleSpThrottle", {"ApedPos_Pc_ActlArb": 0}) + if frame % 10 == 0: + rx("EngBrakeData", {"BpedDrvAppl_D_Actl": 1, "CcStat_D_Actl": 5}) + rx("Steering_Data_FD1", {}) + if bits & FordSafetyFlagsSP.STEER_ANGLE_CURVATURE: + rx("SteeringPinion_Data", {"StePinComp_An_Est": angle_deg, "StePinCompAnEst_D_Qf": 3, "StePinAn_No_Cnt": frame % 16}) + + cs = structs.CarState(vEgo=SPEED, vEgoRaw=SPEED, yawRate=-curvature * SPEED, steeringAngleDeg=angle_deg, + canValid=True, gearShifter=structs.CarState.GearShifter.drive) + cs.cruiseState.available = cs.cruiseState.enabled = True + state.out = cs.as_reader() + cc = structs.CarControl(enabled=True, latActive=True) + cc.actuators.curvature = curvature + _, sends = controller.update(cc.as_reader(), state, timer * 1000) + for address, data, bus in sends: + if address in (0x3CA, 0x3D3, 0x3D6): + sent += 1 + if address != 0x3CA: + path_angles.add(path_angle(address, data)) + if not safety.safety_tx_hook(libsafety_py.make_CANPacket(address, bus, data)): + blocked.append((frame, hex(address), data.hex())) + finally: + safety.set_safety_hooks(structs.CarParams.SafetyModel.noOutput, 0) + + assert sent > 100 and not blocked, blocked[:5] + assert controller.bp.disable_BP_lat_UI == (not lateral) + # angle mode steers through the path angle; the stock path and curvature mode at this curvature don't + assert (path_angles != {INACTIVE_PATH_ANGLE}) == (lateral and angle), sorted(path_angles)[:5] + assert bool(cp.flags & FordFlags.CANFD) == (car == CAR.FORD_F_150_MK14) diff --git a/openpilot/common/params_keys.h b/openpilot/common/params_keys.h index dedfecff99..27044ae21f 100644 --- a/openpilot/common/params_keys.h +++ b/openpilot/common/params_keys.h @@ -44,6 +44,25 @@ inline static std::unordered_map keys = { {"HondaGasFactorParams", {PERSISTENT, FLOAT, "1.0"}}, {"HondaWindFactorParams", {PERSISTENT, FLOAT, "1.0"}}, {"FordHandsFreeCluster", {PERSISTENT, BOOL, "0"}}, + {"disable_BP_lat_UI", {PERSISTENT, BOOL, "1"}}, + {"enable_human_turn_detection_curv", {PERSISTENT, BOOL, "1"}}, + {"enable_lane_positioning_curv", {PERSISTENT, BOOL, "0"}}, + {"enable_lane_full_mode_curv", {PERSISTENT, BOOL, "0"}}, + {"enable_lane_positioning_ang", {PERSISTENT, BOOL, "0"}}, + {"FordPrefLateralControl", {PERSISTENT, INT, "0"}}, + {"custom_profile_curv", {PERSISTENT, INT, "0"}}, + {"lane_change_factor_high_curv", {PERSISTENT, FLOAT, "0.85"}}, + {"pc_blend_ratio_high_C_UI_curv", {PERSISTENT, FLOAT, "0.4"}}, + {"pc_blend_ratio_low_C_UI_curv", {PERSISTENT, FLOAT, "0.4"}}, + {"custom_path_offset_curv", {PERSISTENT, FLOAT, "0.0"}}, + {"LC_PID_gain_UI_curv", {PERSISTENT, FLOAT, "1.0"}}, + {"FordLowSpeedFactor_ang", {PERSISTENT, FLOAT, "1.0"}}, + {"FordHighSpeedFactor_ang", {PERSISTENT, FLOAT, "1.0"}}, + {"FordHighSpeedDampening_ang", {PERSISTENT, FLOAT, "1.0"}}, + {"lane_change_factor_high_ang", {PERSISTENT, FLOAT, "1.0"}}, + {"custom_path_offset_ang", {PERSISTENT, FLOAT, "0.0"}}, + {"lane_centering_strength_ang", {PERSISTENT, FLOAT, "0.25"}}, + {"FordPrefSteerAngleCurvature", {PERSISTENT, BOOL, "0"}}, {"OnroadCustomizations", {PERSISTENT | DONT_LOG, JSON}}, {"StarPilotFavoriteSlots", {PERSISTENT, JSON, R"([{"enabled":false,"show_onroad":false,"key":null,"label":""},{"enabled":false,"show_onroad":false,"key":null,"label":""},{"enabled":false,"show_onroad":false,"key":null,"label":""}])"}}, {"StarPilotControllerBindings", {PERSISTENT | DONT_LOG, JSON}}, diff --git a/openpilot/starpilot/car/ford/bluepilot.py b/openpilot/starpilot/car/ford/bluepilot.py new file mode 100644 index 0000000000..eac8d82c0e --- /dev/null +++ b/openpilot/starpilot/car/ford/bluepilot.py @@ -0,0 +1,84 @@ +"""BluePilot lateral: StarPilot's startup settings and the per-frame inputs its Ford CarController reads +(opendbc bluepilot_lateral/hosts/starpilot).""" +import openpilot.cereal.messaging as messaging +from openpilot.common.swaglog import cloudlog +from openpilot.starpilot.saved_source import read_saved + +from opendbc.bluepilot_lateral.core.values_ext import BP_LATERAL_PARAMS +from opendbc.bluepilot_lateral.hosts.starpilot.lateral import FrameInputs + +STARTUP_KEYS = ("disable_BP_lat_UI", "FordPrefSteerAngleCurvature") +SETTINGS_REFRESH_FRAMES = 300 # 3 s of card's 100 Hz loop + + +def startup_params(params, *, enabled: bool) -> dict[str, str]: + """The startup settings init_ford_safety_param_starpilot reads. BluePilot lateral stays off without the openpilot + toggle, in Safe Mode, or when a setting can't be read; an unset key gets its params_keys.h default.""" + safe, safe_readable = read_saved(params, "SafeMode", 8) + if not enabled or not safe_readable or safe not in (None, b"0"): + return {"disable_BP_lat_UI": "1"} + settings = {} + for key in STARTUP_KEYS: + raw, readable = read_saved(params, key, 8) + if not readable: + return {"disable_BP_lat_UI": "1"} + settings[key] = raw.decode() if raw is not None else ("1" if params.get_default_value(key) else "0") + return settings + + +def encode(value) -> bytes: + return (b"1" if value else b"0") if isinstance(value, bool) else str(value).encode() + + +class BluePilotInputs: + """Card-owned: this frame's model and vehicle-parameter inputs, and the per-frame settings (BP_LATERAL_PARAMS).""" + + def __init__(self, params): + self.params = params + self.sm = messaging.SubMaster(["modelV2", "vehicleParameters", "lateralDelay"]) + self.frames = 0 + self.settings = [] + self.unknown_keys = set() + + def read_settings(self): + entries = [] + for key in BP_LATERAL_PARAMS: + try: + value = self.params.get(key, return_default=True) + except Exception: + if key not in self.unknown_keys: + cloudlog.warning(f"BluePilot lateral param {key} not registered in params_keys.h; skipping") + self.unknown_keys.add(key) + continue + if value is not None: + entries.append(FrameInputs.Param(key, encode(value))) + return entries + + def update(self) -> FrameInputs: + self.sm.update(0) + if self.frames % SETTINGS_REFRESH_FRAMES == 0: + self.settings = self.read_settings() + self.frames += 1 + + frame = FrameInputs(params=self.settings) + li, md, vp = frame.lateralInputs, self.sm["modelV2"], self.sm["vehicleParameters"] + li.valid = bool(self.sm.seen["modelV2"] and self.sm.seen["vehicleParameters"] and + len(md.laneLines) >= 3 and len(md.laneLineProbs) >= 3 and len(md.laneLineStds) >= 3) + if li.valid: + li.pathYawRate = list(md.orientationRate.z) + li.pathX = list(md.position.x) + li.pathY = list(md.position.y) + li.laneLineLeftX = list(md.laneLines[1].x) + li.laneLineLeftY = list(md.laneLines[1].y) + li.laneLineRightX = list(md.laneLines[2].x) + li.laneLineRightY = list(md.laneLines[2].y) + li.laneLineProbs = list(md.laneLineProbs) + li.laneLineStds = list(md.laneLineStds) + li.laneChangeState = int(md.meta.laneChangeState.raw) + li.laneChangeDirection = int(md.meta.laneChangeDirection.raw) + li.steerRatio = vp.steerRatio + li.stiffnessFactor = vp.stiffnessFactor + li.roll = vp.roll + li.angleOffsetDeg = vp.angleOffsetDeg + li.lateralDelay = self.sm["lateralDelay"].lateralDelay + return frame diff --git a/openpilot/starpilot/controller_extensions.py b/openpilot/starpilot/controller_extensions.py index 8fda2250c7..f6091cf257 100644 --- a/openpilot/starpilot/controller_extensions.py +++ b/openpilot/starpilot/controller_extensions.py @@ -27,6 +27,10 @@ def configure_controller(CI, params): from openpilot.starpilot.car.ford.cluster_preferences import FordClusterPreference, qualified as cluster_qualified if controller is not None and cluster_qualified(cp): controller.hands_free_cluster_input = FordClusterPreference(params) + from opendbc.bluepilot_lateral.hosts.starpilot.safety_param import lateral_inputs_required + if controller is not None and lateral_inputs_required(cp) and hasattr(controller, "bluepilot_inputs"): + from openpilot.starpilot.car.ford.bluepilot import BluePilotInputs + controller.bluepilot_inputs = BluePilotInputs(params) if controller is not None and cp.brand == 'gm': from opendbc.car.gm.values import (is_bolt_euv_longitudinal, is_volt_longitudinal, camera_acc_pedal_profile, volt_cc_pedal_profile) diff --git a/openpilot/starpilot/vehicle_preferences.py b/openpilot/starpilot/vehicle_preferences.py index 5eb1881a68..d09f01245b 100644 --- a/openpilot/starpilot/vehicle_preferences.py +++ b/openpilot/starpilot/vehicle_preferences.py @@ -42,6 +42,7 @@ class VehicleStartupPreferences: gm_camera_pedal: bool = True toyota_filter: bool = True gm_longitudinal_tune: int = 0 + ford_bluepilot: tuple[tuple[str, str], ...] = field(default=(("disable_BP_lat_UI", "1"),), kw_only=True) toyota_lock_doors: bool = False toyota_unlock_doors: bool = False toyota_radar_mode: int = 0 @@ -55,6 +56,7 @@ class VehicleStartupPreferences: from openpilot.starpilot.car.tesla.preap_preferences import stock_configuration from openpilot.starpilot.car.tesla.radar_settings import read_radar_settings from openpilot.starpilot.car.tesla.pedal_preferences import read_pedal_settings + from openpilot.starpilot.car.ford.bluepilot import startup_params as ford_bluepilot smoothing = read_snapshot(params, enabled=enabled) radar = read_radar_settings(params) pedal = read_pedal_settings(params) @@ -111,7 +113,7 @@ class VehicleStartupPreferences: subaru_sng=bool(enabled and readable and safe in (None, b"0") and read_saved(params, "SubaruSNG", 8) == (b"1", True)), subaru_software_cruise=bool(enabled and readable and safe in (None, b"0") and read_saved(params, "SubaruRedneckCruise", 8) == (b"1", True)), toyota_radar_mode=toyota_radar_mode(params, enabled=enabled), - tesla_screen=screen, + ford_bluepilot=tuple(ford_bluepilot(params, enabled=enabled).items()), tesla_screen=screen, tesla_screen_brake=screen_brake, toyota_auto_hold=toyota, toyota_sng=bool(enabled and readable and safe in (None, b"0") and read_saved(params, "SNGHack", 8) == (b"1", True)), jeep_brake_hold=jeep_hold, @@ -220,6 +222,8 @@ class VehicleStartupPreferences: apply_toyota_auto_hold(cp, self.toyota_auto_hold) from opendbc.car.toyota.ars510_preferences import apply_radar_mode apply_radar_mode(cp, self.toyota_radar_mode) + from opendbc.bluepilot_lateral.hosts.starpilot.safety_param import init_ford_safety_param_starpilot + init_ford_safety_param_starpilot(cp, dict(self.ford_bluepilot)) return cp def finalize(self, cp) -> None: