From 24c858e618cbb31ab201aa8f35738dc26494a9f0 Mon Sep 17 00:00:00 2001 From: Isaac Barham Date: Sun, 30 Aug 2026 11:28:19 -0400 Subject: [PATCH] Ford: strengthen bounded path tracking feedback Keep action curvature authoritative while increasing bounded C0/C1 feedback when measured curvature is behind. Keep C2 allocation tied to maneuver demand instead of tracking error. Assisted-by: Codex --- openpilot/selfdrive/controls/lib/ford_path.py | 5 +++-- openpilot/selfdrive/controls/tests/test_ford_path.py | 7 +++++++ 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/openpilot/selfdrive/controls/lib/ford_path.py b/openpilot/selfdrive/controls/lib/ford_path.py index 5f70508735..47328eba5a 100644 --- a/openpilot/selfdrive/controls/lib/ford_path.py +++ b/openpilot/selfdrive/controls/lib/ford_path.py @@ -15,6 +15,7 @@ _CURVATURE_RATE_HORIZONS = (3.5, 5.0, 7.0) _FAST_POSE_CURVATURE_BAND = (0.009, 0.012) _CENTERING_CURVATURE_SHARE = 0.65 _TRACKING_ERROR_DEADZONE = 0.0005 +_TRACKING_ERROR_GAIN = 1.5 _TRACKING_ERROR_LIMIT = 0.012 _PATH_RATES = (4.0, 1.0, math.inf, math.inf) @@ -91,7 +92,7 @@ def _encode_path(model, desired_curvature: float, v_ego: float, current_curvatur target_curvature = action_curvature measured_curvature = _finite(current_curvature) tracking_error = target_curvature - measured_curvature - correction = math.copysign(max(abs(tracking_error) - _TRACKING_ERROR_DEADZONE, 0.0), tracking_error) + correction = _TRACKING_ERROR_GAIN * math.copysign(max(abs(tracking_error) - _TRACKING_ERROR_DEADZONE, 0.0), tracking_error) wheel_beyond_target = target_curvature * measured_curvature > 0.0 and \ abs(target_curvature) + _TRACKING_ERROR_DEADZONE < abs(measured_curvature) correction_limit = _TRACKING_ERROR_LIMIT @@ -103,7 +104,7 @@ def _encode_path(model, desired_curvature: float, v_ego: float, current_curvatur sustained_curvature = 0.0 if action_curvature * future_curvature > 0.0 and abs(future_curvature) > _TRACKING_ERROR_DEADZONE: sustained_curvature = math.copysign(min(abs(action_curvature), abs(future_curvature)), action_curvature) - maneuver_demand = max(abs(action_curvature), abs(correction)) + maneuver_demand = abs(action_curvature) maneuver_share = float(np.interp(maneuver_demand, _FAST_POSE_CURVATURE_BAND, (0.0, 1.0))) centering_curvature = 0.0 if wheel_beyond_target else \ sustained_curvature * _CENTERING_CURVATURE_SHARE * (1.0 - maneuver_share) diff --git a/openpilot/selfdrive/controls/tests/test_ford_path.py b/openpilot/selfdrive/controls/tests/test_ford_path.py index d278f8e809..58b5915b1d 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_path.py +++ b/openpilot/selfdrive/controls/tests/test_ford_path.py @@ -306,6 +306,13 @@ def test_curvature_error_increases_forward_pose_command_while_behind(): assert np.isclose(behind.curvature_rate, tracking.curvature_rate) +def test_major_turn_undertracking_uses_bounded_fast_feedback_authority(): + command = FordPathController(dt=1.0).update(_path(0.02), 0.02, v_ego=8.0, current_curvature=0.01) + + assert command.curvature == 0.0 + assert command.path_angle >= 0.25 + + def test_measured_wheel_beyond_action_countersteers_model_arc(): controller = FordPathController() controller.update(_path(0.02), 0.02, v_ego=15.0, current_curvature=0.02)