Keep Ford curvature handoffs within native steering limits

openpilot/selfdrive/car/card.py

@@ -90,7 +90,7 @@ from openpilot.starpilot.feature_runtime import (requested as feature_requested,
 from openpilot.starpilot.saved_source import read_saved
 from openpilot.starpilot.vehicle_selection import startup_candidate
 from openpilot.starpilot.vehicle_preferences import VehicleStartupPreferences
-from openpilot.starpilot.controller_extensions import configure_controller
+from openpilot.starpilot.controller_extensions import configure_controller, observe_controller_can
 from openpilot.starpilot.vehicle_startup import VehicleStartupOwner
 from openpilot.starpilot.controllers.wheel_actions import WheelPublisher, key_for
 from openpilot.starpilot.controllers.cruise_action import CruiseActionConsumer, eligible as cruise_action_eligible
@@ -513,6 +513,7 @@ class Car:
     can_list = can_capnp_to_list(can_strs)
 
     # Update carState from CAN
+    observe_controller_can(self.CI, can_list)
     CS = self.CI.update(can_list)
     from openpilot.starpilot.car.gm.aol import f1_conventional_lateral_owner
     cancel_boot_ns = (time.clock_gettime_ns(time.CLOCK_BOOTTIME)

openpilot/starpilot/car/ford/bluepilot.py

@@ -1,11 +1,20 @@
 """BluePilot lateral: StarPilot's startup settings and the per-frame inputs its Ford CarController reads
 (opendbc bluepilot_lateral/hosts/starpilot)."""
+from collections import deque
+import math
+import numpy as np
+
 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
+from opendbc.bluepilot_lateral.hosts.starpilot.lateral import BluePilotLateral, FrameInputs
+from opendbc.bluepilot_lateral.core.lateral_curv_ext import PrimaryLateralControl, MAX_LATERAL_ACCEL
+from opendbc.bluepilot_lateral.core.values_ext import BP_ANGLE_LIMITS
+from opendbc.bluepilot_lateral.can.fordcan_ext import lat_ctl2_checksum, create_lat_ctl2_msg
+from opendbc.car.ford.values import CarControllerParams, FordFlags
+from opendbc.car import structs
 
 STARTUP_KEYS = ("FordPrefSteerAngleCurvature",)
 # Global StarPilot lane settings are the only lane-positioning owner.
@@ -93,3 +102,79 @@ class BluePilotInputs:
       li.angleOffsetDeg = vp.angleOffsetDeg
       li.lateralDelay = self.sm["lateralDelay"].lateralDelay
     return frame
+
+
+class FordBluePilotController(BluePilotLateral):
+  """Keep the library's curvature handoff inside its unchanged native convergence envelope."""
+
+  def __init__(self, CP, packer, CAN):
+    super().__init__(CP, packer, CAN)
+    self.measured_history = deque([0] * 6, maxlen=6)
+    self.measured_speed = 0
+    self.measured_seen = False
+
+  def observe_can(self, can_list):
+    # Match Panda's receive order: yaw can precede the new speed in the same batch.
+    if not self.CP.flags & FordFlags.CANFD or self.inputs.bp_pinion_curvature_enabled:
+      return
+    for _, batch in can_list:
+      for address, data, bus in batch:
+        if bus != self.CAN.main or len(data) != 8:
+          continue
+        if address == 0x415 and data[2] >> 6 == 3:
+          speed = np.float32(((data[0] << 8) | data[1]) * .01 * (1. / 3.6))
+          self.measured_speed = math.floor(float(np.float32(speed * 1000.)) + .5)
+        elif address == 0x91 and (data[6] >> 4) & 3 == 3:
+          yaw = np.float32(((data[2] << 8) | data[3]) * .0002 - 6.5)
+          speed = np.float32(self.measured_speed / 1000.)
+          curvature = np.float32(yaw / max(speed, np.float32(.1)))
+          raw = float(np.float32(curvature * 50000.))
+          self.measured_history.append(math.floor(raw + .5) if raw >= 0 else math.ceil(raw - .5))
+          self.measured_seen = True
+
+  def step_lateral(self, CC, CS, actuators, apply_curvature_last):
+    packets, applied = super().step_lateral(CC, CS, actuators, apply_curvature_last)
+    state = CS.out
+    if (not CC.latActive or not self.CP.flags & FordFlags.CANFD or
+        self.curv.primary_lateral_control == PrimaryLateralControl.angle or applied == 0. or
+        not state.canValid or state.canTimeout or state.steerFaultTemporary or state.steerFaultPermanent or
+        not math.isfinite(state.vEgoRaw) or state.vEgoRaw <= 10.):
+      return packets, applied
+    measured = self.inputs.get_current_curvature(CS)
+    if not math.isfinite(measured):
+      return packets, applied
+    error = CarControllerParams.CURVATURE_ERROR
+    low, high = measured - error, measured + error
+    if self.measured_seen and not self.inputs.bp_pinion_curvature_enabled:
+      low = max(low, -max(self.measured_history) * .00002 - error)
+      high = min(high, -min(self.measured_history) * .00002 + error)
+    if low > high:
+      return packets, applied
+    limit = min(self.curv.curvature_max, MAX_LATERAL_ACCEL / state.vEgoRaw ** 2)
+    if state.steeringPressed and (low > limit or high < -limit):
+      # Use the curvature controller's neutral reset while the driver exceeds its acceleration cap.
+      inactive = structs.CarControl.new_message(latActive=False)
+      super().step_lateral(inactive, CS, actuators, apply_curvature_last)
+      for index, (address, data, bus) in enumerate(packets):
+        if address == 0x3D6 and bus == self.CAN.main:
+          packets[index] = create_lat_ctl2_msg(self.packer, self.CAN, 1, 3, 1, 0., 0., 0., 0., (data[7] >> 1) & 0xF)
+          return packets, 0.
+      return packets, applied
+    delta = float(np.interp(state.vEgoRaw, *BP_ANGLE_LIMITS.ANGLE_RATE_LIMIT_DOWN))
+    # Rejoin the measured band with the library's existing unwind rate.
+    target = float(np.clip(applied, low, high))
+    corrected = float(np.clip(target, apply_curvature_last - delta, apply_curvature_last + delta))
+    corrected = float(np.clip(corrected, -limit, limit))
+    if corrected == applied:
+      return packets, applied
+    for index, (address, data, bus) in enumerate(packets):
+      if address != 0x3D6 or bus != self.CAN.main or (data[0] >> 4) & 7 != 1:
+        continue
+      raw = round(-corrected / 0.00002) + 1000
+      payload = bytearray(data)
+      payload[2] = raw >> 3
+      payload[3] = (payload[3] & 0x1F) | ((raw & 7) << 5)
+      payload[1] = lat_ctl2_checksum(1, (payload[7] >> 1) & 0xF, bytes(payload))
+      packets[index] = (address, bytes(payload), bus)
+      return packets, float((1000 - raw) * 0.00002)
+    return packets, applied

openpilot/starpilot/controller_extensions.py

@@ -29,8 +29,10 @@ def configure_controller(CI, params):
     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
+    from openpilot.starpilot.car.ford.bluepilot import BluePilotInputs, FordBluePilotController
     controller.bluepilot_inputs = BluePilotInputs(params)
+    if hasattr(controller, "bp") and controller.frame == 0:
+      controller.bp = FordBluePilotController(cp, controller.packer, controller.CAN)
   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)
@@ -170,3 +172,11 @@ def configure_controller(CI, params):
     except OSError:
       controller.torque_ev_lead_inputs = None
       cloudlog.exception('Optional torque EV lead input transport unavailable')
+
+
+def observe_controller_can(CI, can_list):
+  controller = CI.CC
+  if controller is not None and CI.CP.brand == 'ford':
+    from openpilot.starpilot.car.ford.bluepilot import FordBluePilotController
+    if isinstance(getattr(controller, 'bp', None), FordBluePilotController):
+      controller.bp.observe_can(can_list)

openpilot/starpilot/tests/ford_lifecycle_helpers.py

@@ -333,7 +333,7 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag
             fields['CcAslButtnCnclResPress'] = int(cancel)
           if scenario in ('route49', 'cancel-native-cadence', 'neutral-main-cadence', 'bluepilot-gas', 'bluepilot-cancel-gap') and name == 'Steering_Data_FD1':
             fields['CcAslButtnCnclResPress'] = int(cancel)
-          if scenario in ('route45', 'route49', 'route22', 'bluepilot-manual-alert') and name == 'EPAS_INFO':
+          if scenario in ('route45', 'route49', 'route22', 'bluepilot-manual-alert', 'curvature-handoff') and name == 'EPAS_INFO':
             fields['SteeringColumnTorque'] = 2 if steering else 0
           if scenario == 'route45' and name == 'Lane_Assist_Data3_FD1':
             fields['LatCtlSte_D_Stat'] = 0 if fault in ('temporary-lkas', 'temporary-lkas-gas') else 1
@@ -372,6 +372,7 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag
             heartbeat=heartbeat and fault != 'heartbeat',
             speed=(3.1 + .12 * ((steps // 2) % 12) / 11 if getattr(monkeypatch, "ford_speed_window", False) else getattr(joined, 'speed', 15.0)),
             eps=1 if fault in ('temporary-eps', 'temporary-eps-gas') else 0,
+            curvature=getattr(joined, 'measured_curvature', 0.),
           )
         assert native.tick == steps
         safety.safety_tick()
@@ -487,7 +488,7 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag
                 getattr(monkeypatch, 'ford_main_off_warmup', False) and steps <= start_tick + 130 or
                 availability_transition and not state.cruiseState.enabled
                 or scenario == 'route45' and getattr(monkeypatch, 'ford_route45_refusal', None) == 'main-off')
-        assert scenario in ('route45', 'route49', 'route22', 'bluepilot-manual-alert') or not state.steeringPressed
+        assert scenario in ('route45', 'route49', 'route22', 'bluepilot-manual-alert', 'curvature-handoff') or not state.steeringPressed
         assert bool(state.gasPressed) == (fault in ('gas', 'temporary-eps-gas', 'temporary-lkas-gas')
                                           and not any(frame[0] == 0x204 for frame in delayed_pedal_frames))
         if not source_gap:
@@ -685,6 +686,31 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag
           assert not command.enabled and not command.latActive and not command.longActive
           assert sd_state.alertType != 'steerTempUnavailableSilent/warning', 'Unarmed parked startup shows steering warning'
         return
+      if scenario == 'curvature-handoff':
+        from openpilot.starpilot.car.ford.bluepilot import FordBluePilotController
+        assert isinstance(host.CI.CC.bp, FordBluePilotController)
+        for _ in range(41):
+          state, axis, sd_state, command = step()
+        joined.speed = monkeypatch.ford_curve_speed
+        joined.measured_curvature = monkeypatch.ford_curve_sign * min(.018, (3.8 if monkeypatch.ford_curve_manual else 2.0) / joined.speed ** 2)
+        for _ in range(40):
+          state, axis, sd_state, command = step()
+        assert abs(state.yawRate / state.vEgoRaw - joined.measured_curvature) < .0001
+        for tick in range(70):
+          state, axis, sd_state, command = step(engaged=True, steering=monkeypatch.ford_curve_manual, fault='gas' if monkeypatch.ford_curve_gas else None)
+          if tick >= 40:
+            assert command.latActive and host.aol_card_intent.allowed_latch
+          assert not state.steerFaultTemporary and not state.steerFaultPermanent
+          assert sd_state.alertType not in ('controlsMismatch/immediateDisable', 'commIssue/softDisable')
+        if monkeypatch.ford_curve_manual:
+          assert state.steeringPressed and host.CI.CC.apply_curvature_last == 0.
+          joined.measured_curvature = monkeypatch.ford_curve_sign * (1.6 / joined.speed ** 2)
+          for _ in range(70):
+            state, axis, sd_state, command = step(engaged=True)
+            assert not state.steerFaultTemporary and not state.steerFaultPermanent
+        assert command.latActive and host.aol_card_intent.allowed_latch
+        assert abs(host.CI.CC.apply_curvature_last) > 0.
+        return
       if scenario == 'bluepilot-gas':
         for _ in range(50):
           state, axis, sd_state, command = step(engaged=True, fault='gas')

openpilot/starpilot/tests/test_ford_bluepilot_library_lifecycle.py

@@ -100,3 +100,18 @@ def test_manual_steering_retires_held_turn_limit_alert(ford_joined, monkeypatch)
 def test_fresh_cruise_withdrawal_after_previous_output_expires(ford_joined, monkeypatch, brake):
   monkeypatch.ford_gap_brake = brake
   run_actual_producer(ford_joined, monkeypatch, scenario='bluepilot-cancel-gap')
+
+
+@pytest.mark.parametrize('identity,alpha,hands_free,topology,mode,pinion', [
+  ((CAR.FORD_MUSTANG_MACH_E_MK1, 2), True, True, 'automatic', 'curvature', False),
+])
+@pytest.mark.parametrize('speed', [12.4, 13.6, 16., 25.])
+@pytest.mark.parametrize('sign', [-1., 1.])
+@pytest.mark.parametrize('gas', [False, True])
+@pytest.mark.parametrize('manual', [False, True])
+def test_reengaging_in_a_manual_curve_respects_native_convergence(ford_joined, monkeypatch, speed, sign, gas, manual):
+  monkeypatch.ford_curve_speed = speed
+  monkeypatch.ford_curve_sign = sign
+  monkeypatch.ford_curve_gas = gas
+  monkeypatch.ford_curve_manual = manual
+  run_actual_producer(ford_joined, monkeypatch, scenario='curvature-handoff')

openpilot/starpilot/tests/test_ford_ci_registration.py

@@ -43,3 +43,16 @@ def test_actual_host_collector_discovers_release_ui_guards(tmp_path):
   target.parent.mkdir(parents=True)
   target.write_bytes((runner.ROOT / name).read_bytes())
   assert name in pytest_files(tmp_path)
+
+
+def test_imported_native_cases_retain_complete_release_coverage():
+  native = {str(path.relative_to(runner.ROOT)) for path in
+            (runner.ROOT / 'opendbc_repo/opendbc/bluepilot_lateral/safety/tests').glob('test_*.py')}
+  assert native
+  assert native <= set(runner.targets_for('safety-release'))
+  assert native <= set(runner.targets_for('safety-debug'))
+  assert native.isdisjoint(runner.targets_for('aol-lifecycle'))
+  # Actual host/native AOL lifecycle contracts still run in both required lanes.
+  for path in FAMILY_TARGETS:
+    assert path in runner.targets_for('aol-lifecycle')
+    assert path in runner.targets_for('safety-release')

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "ffc9729968978ec4246b4ae402960624a53120f1dc0ae93bb2133678f20d63bb", "schema_version": 1}
+{"protected_inventory_sha256": "723acc8c1cdc255733f98c7c5d8d5ad375f8ae21683d07f75f0b0b5f6e2b4c77", "schema_version": 1}

tools/ci/run_vehicle_tests.py

@@ -29,10 +29,13 @@ FORD_BLUEPILOT_TARGETS = (
   'openpilot/starpilot/tests/test_bluepilot_vendor_pin.py',
   'opendbc_repo/opendbc/safety/tests/test_ford_bluepilot_hooks.py',
   *(str(path.relative_to(ROOT))
-    for folder in ('opendbc_repo/opendbc/bluepilot_lateral/tests', 'opendbc_repo/opendbc/bluepilot_lateral/safety/tests')
-    for path in sorted((ROOT / folder).glob('test_*.py'))),
+    for path in sorted((ROOT / 'opendbc_repo/opendbc/bluepilot_lateral/tests').glob('test_*.py'))),
 )
 
+# Ordinary native suites include inherited, intentionally unsupported cases; RELEASE preserves those skips.
+FORD_BLUEPILOT_NATIVE_TARGETS = tuple(str(path.relative_to(ROOT)) for path in sorted(
+  (ROOT / 'opendbc_repo/opendbc/bluepilot_lateral/safety/tests').glob('test_*.py')))
+
 # Representative production callers plus their source/native refusal cases.
 # The emitted selected_test_ids record exact parametrized configurations.
 AOL_LIFECYCLE_TARGETS = (
@@ -577,9 +580,9 @@ def targets_for(suite):
     paths = [str(path.relative_to(ROOT)) for path in sorted((ROOT / "opendbc_repo/opendbc/safety/tests").glob("test_*.py"))]
     return [selected for path in paths for selected in DEBUG_SAFETY_CLASS_TARGETS.get(path, (path,))] + [
       "openpilot/starpilot/aol/tests/test_classic_startup_neutral_replacement.py",
-    ]
+    ] + list(FORD_BLUEPILOT_NATIVE_TARGETS)
   if suite == "safety-release":
-    return list(RELEASE_TARGETS + FORD_BLUEPILOT_TARGETS)
+    return list(RELEASE_TARGETS + FORD_BLUEPILOT_TARGETS + FORD_BLUEPILOT_NATIVE_TARGETS)
   raise ValueError(f"Unknown vehicle test suite: {suite}")