From 727c26ce8ce6908d961ec69b611bc290def0aa03 Mon Sep 17 00:00:00 2001 From: Isaac Barham Date: Fri, 4 Sep 2026 11:54:41 -0400 Subject: [PATCH] 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 --- .../controls/lib/ford_shared_path.py | 9 ++++--- .../controls/tests/test_ford_shared_path.py | 26 +++++++++++++++++++ 2 files changed, 32 insertions(+), 3 deletions(-) diff --git a/openpilot/selfdrive/controls/lib/ford_shared_path.py b/openpilot/selfdrive/controls/lib/ford_shared_path.py index 01c9992c0b..313fc3cdef 100644 --- a/openpilot/selfdrive/controls/lib/ford_shared_path.py +++ b/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)) diff --git a/openpilot/selfdrive/controls/tests/test_ford_shared_path.py b/openpilot/selfdrive/controls/tests/test_ford_shared_path.py index 62a4a9f9f1..9d9b4ae6ae 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_shared_path.py +++ b/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))