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 <noreply@anthropic.com>

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)

openpilot/common/params_keys.h

@@ -44,6 +44,25 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> 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}},

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

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)

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: