Ford: preserve pose authority when C1 clips

Move heading authority lost at the DBC angle limit into available C0 endpoint authority while retaining the coordinated output limiter.

Assisted-by: Codex

openpilot/selfdrive/controls/lib/ford_path.py

@@ -88,11 +88,13 @@ def _encode_path(path: tuple[list[float], list[float], list[float]], desired_cur
   # expressed in those same pose units and cannot initiate the transfer.
   path_offset = pose_share * (model_offset + 0.5 * tracking_error * offset_horizon ** 2)
   path_angle = pose_share * (model_angle + tracking_error * angle_horizon)
+  limited_path_angle = float(np.clip(path_angle, *DBC_ANGLE))
+  path_offset += (path_angle - limited_path_angle) * offset_horizon
   curvature = desired_curvature * (1.0 - pose_share)
   return FordPath(
     valid=True,
     path_offset=float(np.clip(path_offset, *DBC_OFFSET)),
-    path_angle=float(np.clip(path_angle, *DBC_ANGLE)),
+    path_angle=limited_path_angle,
     curvature=float(np.clip(curvature, *DBC_CURVATURE)),
     curvature_rate=0.0,
   )

openpilot/selfdrive/controls/tests/test_ford_path.py

@@ -123,7 +123,7 @@ def test_low_speed_still_uses_available_model_pose():
 
 
 def test_higher_speed_extends_heading_horizon_without_moving_offset_horizon():
-  model = _changing_path(0.0, 0.04, speed=20.0)
+  model = _changing_path(0.0, 0.015, speed=20.0)
   slow = _command(model, 0.012, v_ego=7.0)
   fast = _command(model, 0.012, v_ego=20.0)
   assert np.isclose(fast.path_offset, slow.path_offset)
@@ -207,6 +207,21 @@ def test_output_limits_and_rates_are_bounded():
   assert np.max(np.abs(np.diff([command.path_angle for command in outputs]))) <= 0.01 + 1e-9
 
 
+def test_clipped_path_angle_uses_available_offset_to_preserve_endpoint():
+  horizon = 7.0
+  for curvature, angle_limit in ((-0.1, DBC_ANGLE[0]), (0.1, DBC_ANGLE[1])):
+    model = _path(curvature)
+    command = _command(model, curvature, current_curvature=curvature, v_ego=horizon)
+
+    distance = np.concatenate(([0.0], np.cumsum(np.hypot(np.diff(model.position.x), np.diff(model.position.y)))))
+    model_offset = np.interp(horizon, distance, model.position.y)
+    model_angle = np.interp(horizon, distance, model.orientation.z)
+
+    assert command.path_angle == angle_limit
+    assert np.isclose(command.path_offset + horizon * command.path_angle,
+                      model_offset + horizon * model_angle)
+
+
 def test_invalid_model_ramps_pose_to_zero_and_inactive_resets():
   controller = FordPathController(dt=0.01)
   for _ in range(20):