Ford: restore model-path C0 with direct requests

docs/ford_model_action_drive_test.md

@@ -1,17 +1,17 @@
 # Ford selected-action drive-test branch
 
-This v9 controller removes the extra C0/C1 slew limits and uses [curvature-derived C0](ford_curvature_c0_v8.md)
+This v10 controller restores model-path C0 at 7 m and keeps direct C0/C1 requests
 and [continuous C1 PI feedback](ford_c1_minimal_pi.md)
 with **P=0.50 and I=0.25**.
-Only integrated tracking error accumulates correction; C0/C1 reflect the current bounded request. C0 is now a 7 m circular arc from selected desired curvature.
+Only integrated tracking error accumulates correction; C0/C1 reflect the current bounded request. C0 samples lateral position 7 m along the model path, holding the endpoint for shorter paths.
 [Base C1 overflow allocation to C0](ford_c1_overflow.md) remains.
 It is selectable on **any Ford CAN FD vehicle**
 through the existing persistent, default-off Sunnylink
 toggle. Offline checks establish software behavior; physical tracking,
 turn-exit behavior and closed-loop stability remain unvalidated.
 
-The v9 change follows the v8 maneuver route recorded on `57ae29f25`.
-See [direct C0/C1 commands and validation](ford_direct_path_v9.md). The gains remain
+V10 restores the C0 method used before `20485134e` and retains the
+[direct C0/C1 commands](ford_direct_path_v9.md) introduced in v9. The gains remain
 P=0.50 and I=0.25, and PSCM `LimitReached` handling is unchanged. The separate
 offline experiment that ignores the reached-limit integration block is not included.
 
@@ -27,7 +27,7 @@ offline experiment that ignores the reached-limit integration block is not inclu
 
 The startup event `Ford path controller selected` should report
 `FordModelActionController`. Periodic `Ford C2-free path tracking` events
-identify **`hypothesis=model-action-direct-c0-c1-pi-v9`**. They report desired and measured
+identify **`hypothesis=model-action-model-path-c0-direct-pi-v10`**. They report desired and measured
 curvature, base heading, proportional and accumulated correction, applied heading,
 feedback timing and driver/PSCM gating. `proportional_gain=0.5` and
 `integral_gain=0.25` identify the trial. `offset_overflow` reports the extra C0
@@ -57,9 +57,9 @@ combined feedforward/P/I amplitude envelope. There is no C0 confirmation
 threshold or remembered turn direction. Zero error removes P and holds I; it
 does not trigger a release. Final command limits still apply.
 
-C0 starts with the 7 m circular arc of selected desired curvature. It does not
-add independent live model-path position or heading. Valid model geometry is
-still required as a health gate. When the raw base heading
+C0 starts with model lateral position interpolated at 7 m of path arc length.
+Short paths hold their final lateral position without extrapolation. Selected
+desired curvature supplies C1 base heading. When the raw base heading
 exceeds ±0.5 rad, C0 additionally receives 7 m times the clipped-away heading.
 Accumulated C1 feedback does not spill into C0. The extra target returns to zero
 as the base heading falls below the cap. Applied C0 changes in that same update.
@@ -69,7 +69,7 @@ place. An explicit selection flag distinguishes upstream mode from an invalid
 experimental command; invalid experimental input cannot switch to upstream.
 The opendbc sender restores upstream behavior when that flag is false.
 
-See [direct-command validation](ford_direct_path_v9.md) for this candidate.
+[Direct-command validation](ford_direct_path_v9.md) records v9 with curvature-derived C0.
 [Curvature-C0](ford_curvature_c0_v8.md) and its validation JSON record v8.
 [Continuous PI](ford_c1_minimal_pi.md) and its validation JSON record v7.
 [Proportional feedback](ford_c1_pi.md) and its validation JSON record v6.

openpilot/selfdrive/controls/lib/ford_model_action.py

@@ -1,6 +1,6 @@
 """Opt-in Ford C2-free model mapping with measured-curvature PI feedback.
 
-C0 samples a desired-curvature arc at 7 m, including base-heading overflow. C1
+C0 samples the model path at 7 m, including base-heading overflow. C1
 combines the selected curvature's heading with proportional and integrated
 tracking error. Reference distance and gains are explicit trial choices.
 Commands use the current bounded request without an additional C0/C1 slew.
@@ -35,10 +35,10 @@ def _finite(*values):
 
 
 def encode_model_action(model, desired_curvature, speed):
-  """Encode a 7 m circular-arc offset and max(7, v*1s)*selected curvature.
+  """Encode model y(7 m) and max(7, v*1s)*selected curvature.
 
-  The arc starts at zero lateral position and heading. Original model geometry
-  remains a health gate; selected curvature supplies both path commands.
+  Sample by arc length, holding the available endpoint for paths shorter than
+  7 m without extrapolating unseen geometry.
   """
   if not _finite(desired_curvature, speed) or not .3 <= speed <= 55 or abs(desired_curvature) > 1:
     return FordPath()
@@ -48,10 +48,8 @@ def encode_model_action(model, desired_curvature, speed):
     return FordPath()
   if path is None or not all(_finite(*values) for values in path):
     return FordPath()
-  # (1-cos(S*k))/k, using sinc to avoid cancellation near zero curvature.
-  half_heading = .5*OFFSET_STATION_M*desired_curvature
-  sinc = math.sin(half_heading)/half_heading if half_heading else 1.
-  c0 = .5*desired_curvature*OFFSET_STATION_M**2*sinc**2
+  station, _, lateral, _ = path
+  c0 = float(np.interp(min(OFFSET_STATION_M, station[-1]), station, lateral))
   c1 = max(OFFSET_STATION_M, speed*HEADING_TIME_S)*desired_curvature
   return FordPath(True, c0, c1, 0., 0.) if _finite(c0, c1) else FordPath()
 
@@ -128,7 +126,7 @@ class FordModelActionController:
   """
   def __init__(self, proportional_gain=C1_PROPORTIONAL_GAIN, integral_gain=C1_INTEGRAL_GAIN):
     self.core = ModelActionController(proportional_gain=proportional_gain, integral_gain=integral_gain)
-    self.hypothesis = 'model-action-direct-c0-c1-pi-v9'
+    self.hypothesis = 'model-action-model-path-c0-direct-pi-v10'
     self.reset()
 
   def reset(self, status='inactive'):

openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py

@@ -53,7 +53,7 @@ class TestFordControlsLogging(unittest.TestCase):
       controls = SimpleNamespace(ford_path_controller=controller, desired_curvature=.03, curvature=.015,
                                  sm=SimpleNamespace(logMonoTime={'modelV2': 123456789, 'carState': 123450000}))
       record = self.emit_controls_event('Ford C2-free path tracking', controls)
-      self.assertEqual(record['hypothesis'], 'model-action-curvature-c0-pi-v8')
+      self.assertEqual(record['hypothesis'], 'model-action-model-path-c0-direct-pi-v10')
       self.assertIs(record['calibration_approved'], False)
       self.assertEqual(record['command'][2:], [0., 0.])
       self.assertEqual(record['status'], controller.diagnostics['status'])

openpilot/selfdrive/controls/tests/test_ford_model_action.py

@@ -29,31 +29,34 @@ def straight(offset=0.):
 @pytest.mark.parametrize('dt', [.002, .01, .1])
 def test_reversal_sends_current_bounded_request_in_same_cycle(sign, dt):
   controller = ModelActionController()
-  model = straight()
+  model = straight(sign*.4)
   for _ in range(100):
     controller.update(model, sign*.005, current_curvature=sign*.005, speed=20., dt=.01)
   # A new opposite request must not retain the previous command's sign while
   # an extra actuator ramp catches up. Matched feedback isolates that ramp.
+  model = straight(-sign*.4)
   out = controller.update(model, -sign*.005, current_curvature=-sign*.005, speed=20., dt=dt)
   target = encode_model_action(model, -sign*.005, 20.)
   assert out.path_angle == pytest.approx(-sign*.1)
   assert out.path_offset == pytest.approx(target.path_offset, abs=.005)
-  out = controller.update(model, 0., current_curvature=0., speed=20., dt=dt)
+  out = controller.update(straight(), 0., current_curvature=0., speed=20., dt=dt)
   assert out == FordPath(True, 0., 0., 0., 0.)
 
 
-def test_selected_action_controls_both_fields_even_when_model_previews_another_turn():
+def test_selected_action_controls_heading_even_when_model_previews_another_turn():
   model = circle(.02)
   assert encode_model_action(model, 0., 20.).path_angle == 0.
   assert encode_model_action(model, -.004, 20.).path_angle == pytest.approx(-.08)
-  assert encode_model_action(model, 0., 20.).path_offset == 0.
-  assert encode_model_action(model, -.004, 20.).path_offset < 0.
+  offset = encode_model_action(model, 0., 20.).path_offset
+  assert offset > 0.
+  assert encode_model_action(model, -.004, 20.).path_offset == offset
 
 
-def test_arc_offset_uses_selected_curvature_and_is_not_scaled_with_speed():
+def test_model_centering_is_independent_of_action_and_not_scaled_with_speed():
   for speed in (2., 7., 20., 35.):
-    target = encode_model_action(straight(.4), 0., speed)
-    assert target == FordPath(True, 0., 0., 0., 0.)
+    for curvature in (-.01, 0., .01):
+      target = encode_model_action(straight(.4), curvature, speed)
+      assert target == FordPath(True, .4, max(7., speed)*curvature, 0., 0.)
   for sign in (-1, 1):
     target = encode_model_action(circle(sign*.01), sign*.01, 20.)
     assert target.path_offset == pytest.approx(sign*(1-math.cos(.07))/.01, abs=1e-6)
@@ -92,7 +95,8 @@ def test_current_request_releases_both_outputs_in_one_cycle():
   controller = ModelActionController()
   for _ in range(150):
     controller.update(straight(1.), .04, current_curvature=.04, speed=20., dt=.01)
-  # C0 starts near .974 + 7*(.8-.5) = 3.074 m, including heading overflow.
+  # C0 starts at 1 + 7*(.8-.5) = 3.1 m, including heading overflow.
+  assert controller.c0 == pytest.approx(3.1)
   out = controller.update(straight(), 0., current_curvature=0., speed=20., dt=.01)
   assert out == FordPath(True, 0., 0., 0., 0.)
 
@@ -137,11 +141,9 @@ def test_selected_core_reversal_through_float32_and_wire_keeps_sign_and_zero_c2(
     assert decoded['LatCtlCurv_No_Actl'] == decoded['LatCtlCrv_NoRate2_Actl'] == 0.
 
 
-def test_short_valid_path_does_not_shorten_the_selected_curvature_arc():
+def test_short_path_holds_available_endpoint_without_extrapolation():
   model = make_model([0., 1.], [0., .1], [0., 0.])
-  target = encode_model_action(model, .01, 20.)
-  assert target == encode_model_action(straight(), .01, 20.)
-  assert target.path_offset == pytest.approx((1-math.cos(.07))/.01)
+  assert encode_model_action(model, .01, 20.) == FordPath(True, .1, .2, 0., 0.)
 
 
 def test_overflowing_arc_resets_instead_of_publishing_invalid_geometry():
@@ -190,31 +192,32 @@ def test_domain_and_elapsed_time_boundaries(field, value, valid):
   assert ModelActionController().update(straight(.4), current_curvature=kwargs['desired_curvature'], **kwargs).valid == valid
 
 
-def test_unrelated_live_model_position_and_heading_do_not_change_selected_arc():
+def test_arc_station_not_forward_x_or_model_heading_determines_offset():
   x = np.array([0., 6., 12.])
   y = .4+x*.75
   target = encode_model_action(make_model(x, y, [2., -2., 1.]), -.01, 20.)
-  assert target.path_offset == pytest.approx(-(1-math.cos(.07))/.01)
+  # Arc length is 1.25*x on this line, so y(arc=7)=.4+.75*(7/1.25).
+  assert target.path_offset == pytest.approx(4.6)
   assert target.path_angle == pytest.approx(-.2)
 
 
 def test_duplicate_stations_keep_valid_geometry_and_current_request():
   model = make_model([0., 0., 10.], [.4, .4, .4], [0., 0., 0.])
-  assert encode_model_action(model, .01, 20.) == encode_model_action(straight(), .01, 20.)
+  assert encode_model_action(model, .01, 20.) == FordPath(True, .4, .2, 0., 0.)
   out = ModelActionController().update(model, .01, current_curvature=.01, speed=20., dt=.002)
-  assert out.path_offset == pytest.approx(.24)
+  assert out.path_offset == pytest.approx(.4)
   assert out.path_angle == pytest.approx(.2)
 
 
 @pytest.mark.parametrize('curvature', [-1., -.2, -.1, -.01, -.001, .001, .01, .1, .2, 1.])
-def test_desired_curvature_arc_matches_circle_geometry(curvature):
+def test_model_offset_is_independent_of_selected_curvature(curvature):
   target = encode_model_action(straight(.4), curvature, 20.)
   assert target.valid
-  assert target.path_offset == pytest.approx((1-math.cos(7.*curvature))/curvature, abs=1e-12)
+  assert target.path_offset == pytest.approx(.4)
 
 
 @pytest.mark.parametrize('curvature', [-1e-12, -1e-100, 0., 1e-100, 1e-12])
-def test_near_zero_arc_is_finite_continuous_and_keeps_direction(curvature):
+def test_near_zero_selected_curvature_preserves_model_centering(curvature):
   target = encode_model_action(straight(.4), curvature, 20.)
   assert target.valid and math.isfinite(target.path_offset)
-  assert target.path_offset == pytest.approx(24.5*curvature, rel=1e-12, abs=0.)
+  assert target.path_offset == pytest.approx(.4)

openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py

@@ -26,8 +26,8 @@ from openpilot.selfdrive.controls.tests.test_ford_model_action import circle, st
 from openpilot.selfdrive.controls.tests.test_ford_model_action_selection import CANFD_CARS, car_params, startup
 
 
-def assert_current_request(core, desired, speed):
-  target = encode_model_action(straight(), desired, speed)
+def assert_current_request(core, model, desired, speed):
+  target = encode_model_action(model, desired, speed)
   base = min(.5, max(-.5, target.path_angle))
   assert core.c0 == pytest.approx(min(5.11, max(-5.11, target.path_offset+7.*(target.path_angle-base))))
   assert core.c1 == pytest.approx(min(.5, max(-.5, base+core.proportional+core.correction)))
@@ -48,7 +48,7 @@ def test_stale_or_future_service_clears_commands_and_reengages_with_current_requ
   update(controller)
   assert update(controller, 1.01, **{field: 1.01-age}) == FordPath()
   assert controller.diagnostics['status'] == 'stale_input'
-  assert update(controller, 1.02).path_offset == pytest.approx(.24)
+  assert update(controller, 1.02).path_offset == pytest.approx(.4)
 
 
 @pytest.mark.parametrize('change,reason', [
@@ -90,7 +90,7 @@ def test_repeated_measurements_use_current_request_and_revalidate_model_geometry
   controller = FordModelActionController()
   for i in range(10):
     result = update(controller, 1.+i*.01, measurement_time=1., model_time=1., reference_time=1.)
-  assert result.path_offset == pytest.approx(.24)
+  assert result.path_offset == pytest.approx(.4)
   assert result.path_angle == pytest.approx(.2)
   broken = straight(.4)
   broken.position.y[5] = math.nan
@@ -113,18 +113,18 @@ def test_reference_source_can_change_to_an_older_but_fresh_publication():
   assert update(controller, 1.01, reference_time=.98).valid
 
 
-def test_relaxing_selected_request_releases_both_fields_despite_growing_model_path():
+def test_relaxing_selected_request_releases_heading_while_offset_follows_model_path():
   for sign in (-1., 1.):
     controller = FordModelActionController()
     for i in range(100):
       before = update(controller, 1.+i*.01, model=circle(sign*.01), desired_curvature=sign*.005)
     for i in range(100):
       after = update(controller, 2.+i*.01, model=circle(sign*.02), desired_curvature=sign*.004)
-    assert abs(after.path_offset) < abs(before.path_offset)
+    assert abs(after.path_offset) > abs(before.path_offset)
     assert abs(after.path_angle) < abs(before.path_angle)
     for i in range(100):
       released = update(controller, 3.+i*.01, model=circle(sign*.02), desired_curvature=0.)
-    assert released.path_offset == pytest.approx(0.)
+    assert released.path_offset == after.path_offset
     assert released.path_angle == pytest.approx(0.)
 
 
@@ -190,7 +190,7 @@ def test_actual_controlsd_selection_limiting_publication_and_downstream_can(pipe
   assert controller.core.proportional == pytest.approx(.5*20.*expected_curvature)
   assert controller.core.correction == 0.  # First measurement has no elapsed feedback time.
   assert controls.ford_path.path_angle == pytest.approx((-1 if maneuver else 1)*.0035)
-  assert controls.ford_path.path_offset == pytest.approx(0.)  # Limited curvature arc is below one C0 step.
+  assert controls.ford_path.path_offset == pytest.approx(.4)  # Model centering survives either reference selection.
   assert cc.latActive and cc.actuators.curvature == 0.
   assert controller.diagnostics['reference_age'] == pytest.approx(.01 if maneuver else .02)
 
@@ -280,7 +280,7 @@ def test_feedback_through_actual_controlsd_publication_and_100hz_sender(pipeline
     assert core.correction == pytest.approx(expected)
     assert core.c1 == pytest.approx(sign*.08+expected_p+expected)
     assert controls.ford_path.path_angle == pytest.approx(core.c1, abs=.00025)
-    assert controls.ford_path.path_offset == pytest.approx(sign*.1)
+    assert controls.ford_path.path_offset == pytest.approx(.4)
 
 
 @pytest.mark.parametrize('service_valid', [False, True])
@@ -337,7 +337,7 @@ def test_continuous_pi_reversal_through_selected_limited_request_and_actual_can(
     exec(call, {'self': controls, 'CS': cs, 'CC': cc, 'actuators': cc.actuators, 'model_v2': model,
                 'lp': SimpleNamespace(roll=0.), 'clip_curvature': clip_curvature,
                 'time': SimpleNamespace(monotonic=lambda now=now: now)})
-    assert_current_request(core, controls.desired_curvature, cs.vEgo)
+    assert_current_request(core, model, controls.desired_curvature, cs.vEgo)
     increment = .25*speed*(controls.desired_curvature-controls.curvature)*.01
     assert abs(core.correction-before[2]) <= abs(increment)+1e-10
     msg = custom.CarControlSP.new_message()
@@ -355,13 +355,13 @@ def test_continuous_pi_reversal_through_selected_limited_request_and_actual_can(
     assert wire['LatCtlPath_No_Cs'] == calculate_lat_ctl2_checksum(2, frame % 16, packet[1])
     if frame == 199:
       assert sign*core.correction < 0. if same_turn else sign*core.correction > 0.
-  assert controls.ford_path_controller.diagnostics['hypothesis'] == 'model-action-direct-c0-c1-pi-v9'
+  assert controls.ford_path_controller.diagnostics['hypothesis'] == 'model-action-model-path-c0-direct-pi-v10'
   if same_turn:
     assert controls.desired_curvature == pytest.approx(sign*.01)
     assert sign*controls.ford_path.path_angle >= speed*.01  # No old unwind correction left below the new base.
   else:
     assert sign*controls.ford_path.path_angle < 0.
-  assert controls.ford_path.path_offset == pytest.approx(sign*(.24 if same_turn else -.02))
+  assert controls.ford_path.path_offset == pytest.approx(sign*(.2 if same_turn else -.2))
   controls.ford_path_controller.reset()
   assert core.c0 == core.c1 == core.correction == 0.
 
@@ -392,7 +392,7 @@ def test_unwind_and_catchup_through_selected_request_and_actual_can(pipeline, si
     exec(call, {'self': controls, 'CS': cs, 'CC': cc, 'actuators': cc.actuators, 'model_v2': model,
                 'lp': SimpleNamespace(roll=0.), 'clip_curvature': clip_curvature,
                 'time': SimpleNamespace(monotonic=lambda now=now: now)})
-    assert_current_request(core, controls.desired_curvature, cs.vEgo)
+    assert_current_request(core, model, controls.desired_curvature, cs.vEgo)
     if frame == 129:
       assert 0. < sign*core.correction < .02
     if frame == 130:
@@ -414,7 +414,7 @@ def test_unwind_and_catchup_through_selected_request_and_actual_can(pipeline, si
     assert wire['LatCtlPath_No_Cs'] == calculate_lat_ctl2_checksum(2, frame % 16, packet[1])
   assert controls.ford_path.path_angle == pytest.approx(core.correction, abs=.00025)
   assert 0. < sign*core.correction < .02
-  assert controls.ford_path.path_offset == pytest.approx(0.)
+  assert controls.ford_path.path_offset == pytest.approx(sign*.05)
 
 
 @pytest.mark.parametrize('sign', [-1., 1.])
@@ -442,7 +442,7 @@ def test_heading_overflow_and_release_through_actual_can(pipeline, sign, fingerp
     exec(call, {'self': controls, 'CS': cs, 'CC': cc, 'actuators': cc.actuators, 'model_v2': model,
                 'lp': SimpleNamespace(roll=0.), 'clip_curvature': clip_curvature,
                 'time': SimpleNamespace(monotonic=lambda now=now: now)})
-    assert_current_request(core, controls.desired_curvature, cs.vEgo)
+    assert_current_request(core, model, controls.desired_curvature, cs.vEgo)
     assert core.correction == 0.
     msg = custom.CarControlSP.new_message()
     exec(publication, {'self': controls, 'CC_SP': msg})
@@ -459,10 +459,10 @@ def test_heading_overflow_and_release_through_actual_can(pipeline, sign, fingerp
     assert wire['LatCtlPath_No_Cs'] == calculate_lat_ctl2_checksum(2, frame % 16, packet[1])
     if frame == 149:
       assert controls.desired_curvature == pytest.approx(sign*.1)
-      assert controls.ford_path.path_offset == pytest.approx(sign*((1-math.cos(.7))/.1+1.4), abs=.005)
+      assert controls.ford_path.path_offset == pytest.approx(sign*1.6)
       assert controls.ford_path.path_angle == pytest.approx(sign*.5)
       assert controls.ford_path_controller.diagnostics['offset_overflow'] == pytest.approx(sign*1.4)
-  assert controls.ford_path.path_offset == pytest.approx(sign*(1-math.cos(.28))/.04, abs=.005)
+  assert controls.ford_path.path_offset == pytest.approx(sign*.2)
   assert controls.ford_path.path_angle == pytest.approx(sign*.28)
   assert controls.ford_path_controller.diagnostics['offset_overflow'] == 0.
 

openpilot/selfdrive/controls/tests/test_ford_model_action_feedback.py

@@ -25,7 +25,7 @@ def test_feedback_builds_holds_and_unwinds_without_changing_c0(sign):
     baseline = tick(matched, sign*.004, sign*.004)
   assert controller.correction == pytest.approx(sign*.02)
   assert out.path_angle == pytest.approx(sign*.1)
-  assert out.path_offset == baseline.path_offset == pytest.approx(sign*.1)
+  assert out.path_offset == baseline.path_offset == pytest.approx(.4)
   for _ in range(100):
     out = tick(controller, sign*.004, sign*.004)
   assert controller.correction == pytest.approx(sign*.02)
@@ -72,7 +72,7 @@ def test_pscm_limit_only_blocks_feedback_further_into_measured_turn(sign):
   for _ in range(100):
     out = tick(controller, 0., 0., pscm_limited=True)
   assert abs(out.path_angle) < .001
-  assert out.path_offset == pytest.approx(0.)
+  assert out.path_offset == pytest.approx(.4)
 
 
 @pytest.mark.parametrize('sign', [-1., 1.])

openpilot/selfdrive/controls/tests/test_ford_model_action_overflow.py

@@ -1,6 +1,4 @@
 """C1 overflow allocation and release; no assumptions about PSCM response."""
-import math
-
 import pytest
 
 from openpilot.selfdrive.controls.lib.ford_model_action import ModelActionController
@@ -16,11 +14,10 @@ def test_clipped_base_heading_preserves_the_seven_metre_reference(sign, speed, h
   for _ in range(150):
     out = controller.update(straight(sign*.2), desired, current_curvature=desired, speed=speed, dt=.01)
   assert controller.correction == 0.
-  arc = (1-math.cos(7.*desired))/desired
-  assert controller.c0 == pytest.approx(arc+sign*7.*max(heading-.5, 0.))
+  assert controller.c0 == pytest.approx(sign*(.2+7.*max(heading-.5, 0.)))
   assert out.path_offset == pytest.approx(controller.c0, abs=.005)
   assert out.path_angle == pytest.approx(sign*min(heading, .5))
-  assert out.path_offset+7.*out.path_angle == pytest.approx(arc+sign*7.*heading, abs=.005)
+  assert out.path_offset+7.*out.path_angle == pytest.approx(sign*(.2+7.*heading), abs=.005)
   assert out.curvature == out.curvature_rate == 0.
 
 
@@ -39,7 +36,7 @@ def test_overflow_uses_existing_reference_with_short_model_path(sign):
   model = make_model([0., 1.], [0., sign*.2], [0., 0.])
   for _ in range(150):
     out = controller.update(model, sign*.03, current_curvature=sign*.03, speed=20., dt=.01)
-  assert out.path_offset == pytest.approx(sign*((1-math.cos(.21))/.03+.7), abs=.005)
+  assert out.path_offset == pytest.approx(sign*.9)
 
 
 @pytest.mark.parametrize('sign', [-1., 1.])
@@ -47,8 +44,8 @@ def test_extra_offset_releases_immediately_without_stored_overflow(sign):
   controller = ModelActionController()
   for _ in range(200):
     controller.update(straight(sign*.2), sign*.04, current_curvature=sign*.04, speed=20., dt=.01)
-  start = sign*((1-math.cos(.28))/.04+2.1)
-  target = sign*(1-math.cos(.14))/.02
+  start = sign*2.3
+  target = sign*.2
   assert controller.c0 == pytest.approx(start)
   for _ in range(70):
     before = controller.c0
@@ -56,7 +53,7 @@ def test_extra_offset_releases_immediately_without_stored_overflow(sign):
     assert sign*controller.c0 >= sign*target-1e-10
     assert sign*controller.c0 <= sign*before+1e-10
     assert controller.c0 == pytest.approx(target)
-  assert out.path_offset == pytest.approx(sign*(1-math.cos(.14))/.02, abs=.005)
+  assert out.path_offset == pytest.approx(sign*.2)
   assert out.path_angle == pytest.approx(sign*.4)
   assert controller.correction == 0.
 
@@ -68,7 +65,7 @@ def test_c1_feedback_saturation_does_not_spill_correction_into_c0(sign):
     out = controller.update(straight(sign*.2), sign*.02, current_curvature=0., speed=20., dt=.01)
   assert out.path_angle == pytest.approx(sign*.5)
   assert controller.correction == pytest.approx(sign*.1)
-  assert out.path_offset == pytest.approx(sign*(1-math.cos(.14))/.02, abs=.005)
+  assert out.path_offset == pytest.approx(sign*.2)
 
 
 @pytest.mark.parametrize('sign', [-1., 1.])
@@ -78,6 +75,6 @@ def test_overflow_is_base_geometry_with_existing_feedback_gates(sign, enabled, l
   for _ in range(150):
     out = controller.update(straight(sign*.2), sign*.03, current_curvature=sign*.02, speed=20., dt=.01,
                             feedback_enabled=enabled, pscm_limited=limited)
-  assert out.path_offset == pytest.approx(sign*((1-math.cos(.21))/.03+.7), abs=.005)
+  assert out.path_offset == pytest.approx(sign*.9)
   assert out.path_angle == pytest.approx(sign*.5)
   assert controller.correction == 0.

openpilot/selfdrive/controls/tests/test_ford_model_action_pi.py

@@ -30,7 +30,7 @@ def test_aligned_feedback_does_not_replace_current_feedforward_or_path(sign):
   out = controller.update(straight(sign*.4), sign*.01, current_curvature=sign*.008,
                           feedback_curvature=sign*.008, speed=20., dt=.1)
   assert controller.proportional == controller.correction == 0.
-  assert out.path_offset == pytest.approx(sign*(1-math.cos(.07))/.01, abs=.005)
+  assert out.path_offset == pytest.approx(sign*.4)
   assert out.path_angle == pytest.approx(sign*.2)
   # Latest request is ahead of measured steering, but the delay-aligned target
   # has already been exceeded. P and I must use the explicit feedback target.

openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py

@@ -48,7 +48,7 @@ def test_actual_startup_priority(candidate, observer, fingerprint):
     assert type(selected.ford_path_controller) is FordModelActionController
     assert selected.ford_path_controller.core.proportional_gain == C1_PROPORTIONAL_GAIN == .50
     assert selected.ford_path_controller.core.integral_gain == C1_INTEGRAL_GAIN == .25
-    assert selected.ford_path_controller.diagnostics['hypothesis'] == 'model-action-direct-c0-c1-pi-v9'
+    assert selected.ford_path_controller.diagnostics['hypothesis'] == 'model-action-model-path-c0-direct-pi-v10'
   else:
     assert selected.ford_path_controller is None
   assert selected.ford_model_action == candidate

openpilot/sunnypilot/sunnylink/settings_ui.json

@@ -2183,8 +2183,8 @@
           "widget": "toggle",
           "needs_onroad_cycle": true,
           "title": "Selected-Action Path Tracking (Experimental)",
-          "description": "Follow the selected desired curvature using path-offset and heading commands with measured steering feedback on any Ford CAN FD vehicle.",
-          "details": "Derives path offset and heading from the same selected desired curvature and adjusts the heading request using the difference between requested and measured steering. Uses remaining path-offset range when the base heading request reaches its limit. The correction holds when steering matches and clears on driver override. Default off; physical tracking and turn-exit behavior are not road-validated. Enable only for controlled testing. Turning it off restores upstream Ford curvature control, regardless of any previously stored experimental settings. Only Ford CAN FD vehicles can use this experiment. Changes apply after a real offroad-to-onroad cycle, not immediately or on disengagement alone.",
+          "description": "Follow the selected steering plan using model-path centering and measured steering feedback on any Ford CAN FD vehicle.",
+          "details": "Uses the nearby model path for centering and adjusts the heading request using the difference between requested and measured steering. Uses remaining path-offset range when the base heading request reaches its limit. The correction holds when steering matches and clears on driver override. Default off; physical tracking and turn-exit behavior are not road-validated. Enable only for controlled testing. Turning it off restores upstream Ford curvature control, regardless of any previously stored experimental settings. Only Ford CAN FD vehicles can use this experiment. Changes apply after a real offroad-to-onroad cycle, not immediately or on disengagement alone.",
           "enablement": [
             {
               "type": "offroad_only"

openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml

@@ -14,8 +14,8 @@ sections:
     widget: toggle
     needs_onroad_cycle: true
     title: Selected-Action Path Tracking (Experimental)
-    description: Follow the selected desired curvature using path-offset and heading commands with measured steering feedback on any Ford CAN FD vehicle.
-    details: Derives path offset and heading from the same selected desired curvature and adjusts the heading request using the difference between requested and measured steering. Uses remaining path-offset range when the base heading request reaches its limit. The correction holds when steering matches and clears on driver override. Default off; physical tracking and turn-exit behavior are not road-validated. Enable only for controlled testing. Turning it off restores upstream Ford curvature control, regardless of any previously stored experimental settings. Only Ford CAN FD vehicles can use this experiment. Changes apply after a real offroad-to-onroad cycle, not immediately or on disengagement alone.
+    description: Follow the selected steering plan using model-path centering and measured steering feedback on any Ford CAN FD vehicle.
+    details: Uses the nearby model path for centering and adjusts the heading request using the difference between requested and measured steering. Uses remaining path-offset range when the base heading request reaches its limit. The correction holds when steering matches and clears on driver override. Default off; physical tracking and turn-exit behavior are not road-validated. Enable only for controlled testing. Turning it off restores upstream Ford curvature control, regardless of any previously stored experimental settings. Only Ford CAN FD vehicles can use this experiment. Changes apply after a real offroad-to-onroad cycle, not immediately or on disengagement alone.
     enablement:
     - $ref: '#/macros/offroad'
 - id: hyundai