From 42e1414bc43b37649fd33819a43b660cf5db6883 Mon Sep 17 00:00:00 2001
From: Isaac Barham <development@isaacbarham.com>
Date: Thu, 27 Aug 2026 19:53:59 -0400
Subject: [PATCH] ford: source C2 only from desired curvature

Prevent model-fit curvature jitter from directly modulating the PSCM's slow C2 channel.

Assisted-by: Codex
---
 openpilot/selfdrive/controls/lib/ford_path.py |  4 ++--
 .../controls/tests/test_ford_path.py          | 21 +++++++++++++------
 2 files changed, 17 insertions(+), 8 deletions(-)

diff --git a/openpilot/selfdrive/controls/lib/ford_path.py b/openpilot/selfdrive/controls/lib/ford_path.py
index 6ea90c2490..e7d3a8c853 100644
--- a/openpilot/selfdrive/controls/lib/ford_path.py
+++ b/openpilot/selfdrive/controls/lib/ford_path.py
@@ -102,7 +102,7 @@ def _encode_path(model, desired_curvature: float | None, v_ego: float, current_c
   tracking_demand = 0.0
   wheel_beyond_target = False
   if current_curvature is not None:
-    target_curvature = requested_curvature
+    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)
@@ -129,7 +129,7 @@ def _encode_path(model, desired_curvature: float | None, v_ego: float, current_c
     float(np.interp(maneuver_demand, _CENTERING_CURVATURE_BASEBAND, (0.0, 1.0)))
   path_offset *= maneuver_share
   path_angle *= maneuver_share
-  centering_curvature = requested_curvature
+  centering_curvature = action_curvature
 
   return FordPath(
     valid=True,
diff --git a/openpilot/selfdrive/controls/tests/test_ford_path.py b/openpilot/selfdrive/controls/tests/test_ford_path.py
index 1bb35deaff..68973fca21 100644
--- a/openpilot/selfdrive/controls/tests/test_ford_path.py
+++ b/openpilot/selfdrive/controls/tests/test_ford_path.py
@@ -126,7 +126,7 @@ def test_s_turn_reverses_fast_fields_while_c2_is_bounded():
 
 def test_reversal_does_not_add_software_persistence_to_centering_c2():
   controller = FordPathController()
-  assert controller.update(_path(0.002), 0.0, v_ego=8.0).curvature > 0.0
+  assert controller.update(_path(0.002), 0.002, v_ego=8.0).curvature > 0.0
 
   reversing = controller.update(_path(-0.02), -0.02, v_ego=8.0)
 
@@ -182,6 +182,15 @@ def test_minor_changing_curve_does_not_emit_ungated_c3():
   assert command.curvature_rate == 0.0
 
 
+def test_c2_uses_stable_action_curvature_not_independent_model_fit():
+  controller = FordPathController(dt=1.0)
+  first = controller.update(_path(0.004), 0.002, v_ego=8.0, current_curvature=0.002)
+  second = controller.update(_path(0.006), 0.002, v_ego=8.0, current_curvature=0.002)
+
+  assert np.isclose(first.curvature, 0.002)
+  assert np.isclose(second.curvature, 0.002)
+
+
 def test_action_curvature_corrects_stale_opposing_model_at_low_speed():
   command = FordPathController().update(_path(-0.001, speed=1.0), 0.005, v_ego=1.0, current_curvature=0.001)
 
@@ -266,18 +275,18 @@ def test_curvature_error_increases_forward_pose_command_while_behind():
   assert np.isclose(behind.curvature_rate, tracking.curvature_rate)
 
 
-def test_rolling_arc_stays_active_while_vehicle_unwinds():
+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, yaw_rate=0.3)
   outputs = [controller.update(_path(0.02), 0.003, v_ego=15.0, current_curvature=0.01, yaw_rate=0.15) for _ in range(4)]
   unwinding = outputs[-1]
 
-  assert 0.0 <= unwinding.curvature <= 0.003
-  assert unwinding.path_angle > 0.03
+  assert unwinding.curvature == 0.0
+  assert unwinding.path_angle < 0.0
 
 
-def test_geometric_c2_remains_active_for_centering():
-  centering = FordPathController(dt=1.0).update(_path(0.002), 0.0, v_ego=15.0, current_curvature=0.0)
+def test_action_c2_remains_active_for_centering():
+  centering = FordPathController(dt=1.0).update(_path(0.002), 0.002, v_ego=15.0, current_curvature=0.002)
 
   assert centering.curvature > 0.001
   assert abs(centering.path_offset) < 1e-9