Ford: allow earlier joint fast-path buildup experimentally

Preserve geometric C0/C1 before nominal plateaus during same-direction buildup. Keep reversal guards, command limits, C2 policy, and the existing default-off Shared Path Controller selection. Require nonzero demand for joint buildup.

Known limitation: nominal short-turn cancellation settles later with queued commands. Retain that regression as an explicit expected failure; this experiment does not establish physical response or resolve unwind. Add entry and zero-demand coverage.

Assisted-by: OpenAI Codex

openpilot/selfdrive/controls/lib/ford_shared_path.py

@@ -222,10 +222,13 @@ class ContributionAllocator:
     # on a nominal plateau. Extra outward demand must not erase raw geometry.
     backing_off = any(total * (requested - total) < -tolerance * abs(total)
                       for total in (sum(qpref), sum(contributions(state, speed))))
-    # Extend only a field already on its same-direction nominal plateau, not
-    # one still reversing or helping the other coefficient settle a correction.
+    # Joint outward buildup need not wait for the nominal plateaus. If either
+    # fast field must reverse or drain toward its preference, keep the plateau
+    # guard: extending the other field can disrupt a settling correction.
+    building = requested != 0.0 and all(requested * current >= 0.0 and requested * (value - current) >= 0.0
+                                       for current, value in zip(state[:2], preferred_values[:2], strict=True))
     preserve_geometry = tuple(not backing_off and abs(weight * value) > limit
-                              and current * value > 0.0 and abs(weight * current) >= limit
+                              and current * value > 0.0 and (building or abs(weight * current) >= limit)
                               for weight, value, current, limit in
                               zip(weights[:2], preferred_values[:2], state[:2], _CONTRIBUTION_LIMITS[:2], strict=True))
     candidates = {tuple(_clip(v, lo, hi) for v, lo, hi in zip(_values(path), low, high, strict=True))

openpilot/selfdrive/controls/tests/test_ford_shared_path.py

@@ -59,6 +59,32 @@ class TestSharedRequest(unittest.TestCase):
 
 
 class TestContributionAllocator(unittest.TestCase):
+  def test_fresh_turn_does_not_wait_for_nominal_plateaus_to_send_geometry(self):
+    for sign in (-1, 1):
+      allocator = ContributionAllocator(initial_state=(0.0, 0.0, 0.0))
+      preferred = FordPath(True, sign * 2.0, sign * 0.3, 0.0)
+      requested = sum(contributions((preferred.path_offset, preferred.path_angle, 0.0), 8.0))
+      for _ in range(10):
+        command = allocator.allocate(requested, preferred, 8.0)
+        self.assertEqual(command.curvature, 0.0)
+        allocator.advance(allocator.dt)
+      # Existing host slew permits 4 m/s and 1 rad/s. Allow one control tick
+      # for initialization, not the time needed to fill a nominal plateau.
+      self.assertGreaterEqual(sign * command.path_offset, 4.0 * 9 * allocator.dt - 1e-9)
+      self.assertGreaterEqual(sign * command.path_angle, 9 * allocator.dt - 1e-9)
+
+  def test_zero_total_does_not_treat_opposing_fields_as_joint_buildup(self):
+    for sign in (-1, 1):
+      initial = (sign * 0.6, -sign * 0.03, 0.0)
+      allocator = ContributionAllocator(initial_state=initial)
+      allocator.set_command(FordPath(True, *initial), 8.0)
+      command = allocator.allocate(0.0, FordPath(True, sign * 0.7, -sign * 0.3, 0.0), 8.0)
+      self.assertLessEqual(abs(command.path_angle), 0.035)
+      self.assertEqual(command.curvature, 0.0)
+
+  # Known limitation of experimental early geometry: queued commands prolong
+  # release in the nominal BD model. This is NOT verified physical behavior.
+  @unittest.expectedFailure
   def test_short_turn_release_preserves_command_history_not_just_coefficient_state(self):
     for sign in (-1, 1):
       allocator = ContributionAllocator(initial_state=(0.0, 0.0, 0.0))