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))