From 336ce75f3d1d77cec0fb301b0f926f10ba101f3d Mon Sep 17 00:00:00 2001 From: Isaac Barham Date: Wed, 2 Sep 2026 09:53:56 -0400 Subject: [PATCH] Ford: keep gentle driving on C2 only Remove model-pose residuals and tracking trim from the gentle regime. Blend the model pose into C0/C1 only as maneuver demand rises, while retaining opposing-path C2 unload and the coordinated 100 Hz handoff. Assisted-by: Codex --- openpilot/selfdrive/controls/lib/ford_path.py | 16 +++++++----- .../controls/tests/test_ford_path.py | 26 ++++++++++++------- 2 files changed, 26 insertions(+), 16 deletions(-) diff --git a/openpilot/selfdrive/controls/lib/ford_path.py b/openpilot/selfdrive/controls/lib/ford_path.py index 00d98c2724..5bd3be8497 100644 --- a/openpilot/selfdrive/controls/lib/ford_path.py +++ b/openpilot/selfdrive/controls/lib/ford_path.py @@ -127,14 +127,18 @@ def _encode_path(path: tuple[list[float], list[float], list[float], list[float]] angle_curvature = model_angle / max(angle_horizon, 1e-3) pose_share = _blend_share(max(abs(offset_curvature), abs(angle_curvature), abs(desired_curvature))) - # Preserve upstream-strength C2 for normal driving. C0/C1 carry the model - # geometry not represented by C2, so action collapse cannot erase the path. - curvature = desired_curvature * (1.0 - pose_share) - if curvature * model_angle <= 0.0: + # Match upstream's C2-only normal driving, then continuously transfer the + # command to the model pose for larger maneuvers. An opposing/finished model + # path must unload sticky C2 and retain the fast pose needed to unwind it. + c2_opposes_path = desired_curvature != 0.0 and desired_curvature * model_angle <= 0.0 + if c2_opposes_path: + pose_share = 1.0 curvature = 0.0 + else: + curvature = desired_curvature * (1.0 - pose_share) - path_offset = model_offset - 0.5 * curvature * offset_horizon ** 2 + feedback_offset - path_angle = model_angle - curvature * angle_horizon + feedback_angle + path_offset = pose_share * (model_offset + feedback_offset) + path_angle = pose_share * (model_angle + feedback_angle) if abs(path_offset) < 0.5 * DBC_OFFSET_RESOLUTION: path_offset = 0.0 if abs(path_angle) < 0.5 * DBC_ANGLE_RESOLUTION: diff --git a/openpilot/selfdrive/controls/tests/test_ford_path.py b/openpilot/selfdrive/controls/tests/test_ford_path.py index 3c0197df50..b39e92a1e0 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_path.py +++ b/openpilot/selfdrive/controls/tests/test_ford_path.py @@ -63,6 +63,13 @@ def test_gentle_path_uses_only_c2(): assert command.curvature_rate == 0.0 +def test_gentle_path_uses_only_c2_when_model_and_action_disagree(): + command = _command(_path(0.005), 0.002, current_curvature=0.005) + assert command.path_offset == 0.0 + assert command.path_angle == 0.0 + assert np.isclose(command.curvature, 0.002, atol=1e-6) + + def test_spatially_growing_path_adds_fast_pose_before_action_becomes_large(): controller = FordPathController(dt=1.0) command = controller.update(_changing_path(0.0, 0.04), 0.012, current_curvature=0.0, v_ego=8.0) @@ -106,17 +113,18 @@ def test_model_pose_can_trigger_maneuver_when_action_is_late(): assert command.curvature == 0.0 -def test_model_pose_preserves_a_gentle_arc_when_the_action_collapses(): - command = _command(_path(0.006), 0.002, current_curvature=0.006) - assert command.path_offset > 0.05 - assert command.path_angle > 0.02 - assert np.isclose(command.curvature, 0.002, atol=2e-6) +def test_gentle_model_pose_does_not_replace_a_collapsed_action(): + command = _command(_path(0.005), 0.0, current_curvature=0.005) + assert command.path_offset == 0.0 + assert command.path_angle == 0.0 + assert command.curvature == 0.0 def test_changing_gentle_curve_keeps_upstream_strength_c2(): command = _command(_changing_path(0.0, 0.008), 0.004, current_curvature=0.0) assert np.isclose(command.curvature, 0.004) - assert command.path_angle > 0.0 + assert command.path_offset == 0.0 + assert command.path_angle == 0.0 def test_action_only_maneuver_cannot_invent_large_model_pose(): @@ -210,15 +218,13 @@ def test_measured_tracking_error_closes_bidirectionally_without_abandoning_the_t assert 0.0 < over.path_angle < on_target.path_angle -def test_gentle_curve_keeps_c2_and_adds_only_a_small_fast_tracking_trim(): +def test_gentle_curve_does_not_add_fast_tracking_trim(): model = _path(0.004) under = _command(model, 0.004, current_curvature=0.002) on_target = _command(model, 0.004, current_curvature=0.004) over = _command(model, 0.004, current_curvature=0.006) assert under.path_offset == on_target.path_offset == over.path_offset == 0.0 - assert 0.0 < under.path_angle < 0.002 - assert on_target.path_angle == 0.0 - assert -0.002 < over.path_angle < 0.0 + assert under.path_angle == on_target.path_angle == over.path_angle == 0.0 assert np.allclose([under.curvature, on_target.curvature, over.curvature], 0.004, atol=2e-6)