Ford: retain geometric demand in shared path diagnostics

openpilot/selfdrive/controls/lib/ford_shared_path.py

@@ -8,6 +8,7 @@ from dataclasses import dataclass
 from itertools import product
 import math
 import struct
+from typing import Any
 
 from opendbc.can import CANPacker
 from opendbc.car.ford.values import CarControllerParams, FordFlags
@@ -56,6 +57,7 @@ class PathRequest:
   preferred: FordPath
   offset_error: float
   heading_error: float
+  geometric_request: tuple[float, float]
 
 
 def request_for_model(model, desired_curvature: float, *, current_curvature: float, v_ego: float,
@@ -90,10 +92,13 @@ def request_for_model(model, desired_curvature: float, *, current_curvature: flo
 
   allocation_demand = max(demand, abs(current_curvature))
   share = _clip((allocation_demand - _GENTLE_CURVATURE) / (_FULL_POSE_CURVATURE - _GENTLE_CURVATURE), 0.0, 1.0)
-  preferred = FordPath(True, _clip(offset_ff + correction_share * error_y, *_RANGES[0]),
-                       _clip(angle_ff + correction_share * error_heading, *_RANGES[1]),
+  # Retain metres/radians before wire or nominal-contribution clipping. These
+  # are requested geometry, not measured path error or additional authority.
+  geometric_request = (offset_ff + correction_share * error_y, angle_ff + correction_share * error_heading)
+  preferred = FordPath(True, _clip(geometric_request[0], *_RANGES[0]),
+                       _clip(geometric_request[1], *_RANGES[1]),
                        _clip(desired_curvature * (1.0 - share), *_RANGES[2]), 0.0)
-  return PathRequest(total, feedforward, feedback, preferred, error_y, error_heading)
+  return PathRequest(total, feedforward, feedback, preferred, error_y, error_heading, geometric_request)
 
 
 class ContributionAllocator:
@@ -263,7 +268,7 @@ class FordSharedPathController:
     self.allocator = ContributionAllocator(dt)
     self.fallback = FordPathController(dt)
     self.last_time = None
-    self.diagnostics = {"status": "initializing", "hypothesis": "ML3V-BD-normalized-v1"}
+    self.diagnostics: dict[str, Any] = {"status": "initializing", "hypothesis": "ML3V-BD-normalized-v1"}
 
   def update(self, model, desired_curvature: float, *, current_curvature=0.0, v_ego=0.0,
              v_ego_raw=0.0, active=True, now=None):
@@ -297,10 +302,14 @@ class FordSharedPathController:
       "feedback": request.feedback if request else 0.0,
       "offset_error": request.offset_error if request else 0.0,
       "heading_error": request.heading_error if request else 0.0,
+      "geometric_request": request.geometric_request if request else None,
+      # Locally predicted packet fields, not a PSCM execution acknowledgment.
+      "packed_command": _values(self.allocator.command),
       "state": self.allocator.state,
       "state_width": tuple(hi - lo for lo, hi in zip(self.allocator.lower, self.allocator.upper, strict=True)),
       "predicted_total": self.allocator.predicted_total if status == "active" else 0.0,
       "predicted_peak_error": self.allocator.predicted_peak_error if status == "active" else 0.0,
+      # Nominal allocation error only: zero is NOT successful path tracking.
       "shortfall": self.allocator.shortfall if status == "active" else 0.0,
     }
     return result

openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py

@@ -55,6 +55,26 @@ class TestFordControlsLogging(unittest.TestCase):
       self.assertEqual(record['state'], list(controller.diagnostics['state']))
       self.assertEqual(record['requested'], controller.diagnostics['requested'])
 
+  def test_geometric_request_logs_and_clears_without_changing_allocation_meaning(self):
+    controller = FordSharedPathController()
+    model = SimpleNamespace(position=SimpleNamespace(x=[0.0, 20.0], y=[0.0, -10.0]),
+                            orientation=SimpleNamespace(z=[0.0, -1.0]))
+    for _ in range(4):
+      controller.update(model, 0.0, active=False)
+    for _ in range(150):
+      controller.update(model, 0.0, current_curvature=-0.019, v_ego=2.1, v_ego_raw=2.1)
+    controls = SimpleNamespace(ford_path_controller=controller, sm=SimpleNamespace(logMonoTime={'modelV2': 123456789}))
+    record = self.emit_controls_event('Ford shared path experiment', controls)
+    self.assertEqual(record['geometric_request'], list(controller.diagnostics['geometric_request']))
+    self.assertEqual(record['packed_command'], list(controller.diagnostics['packed_command']))
+    self.assertLess(record['geometric_request'][1], record['packed_command'][1])
+    self.assertAlmostEqual(record['shortfall'], 0.0)
+
+    for active in (True, False):
+      controller.update(None, 0.0, active=active)
+      record = self.emit_controls_event('Ford shared path experiment', controls)
+      self.assertIsNone(record['geometric_request'])
+
 
 if __name__ == '__main__':
   unittest.main()

openpilot/selfdrive/controls/tests/test_ford_shared_path.py

@@ -201,6 +201,27 @@ class TestSharedController(unittest.TestCase):
     self.assertGreater(command.path_offset, 0.0)
     self.assertGreater(command.path_angle, 0.0)
 
+  def test_nominal_allocation_success_preserves_unsatisfied_geometric_request(self):
+    controller = FordSharedPathController()
+    model = circle(-0.12)
+    for _ in range(4):
+      controller.update(model, 0.0, active=False)
+    for _ in range(150):
+      command = controller.update(model, 0.0, current_curvature=-0.019, v_ego=2.1, v_ego_raw=2.1)
+
+    diagnostic = controller.diagnostics
+    self.assertEqual(diagnostic['status'], 'active')
+    self.assertAlmostEqual(diagnostic['shortfall'], 0.0)
+    # Allocation success is only agreement with the nominal coefficient map.
+    # Keep the geometric request even beyond the DBC heading range, rather
+    # than presenting the held coefficient command as the model's full path.
+    offset, heading = diagnostic['geometric_request']
+    self.assertLess(offset, command.path_offset - 1.0)
+    self.assertLess(heading, -0.5)
+    self.assertAlmostEqual(diagnostic['packed_command'][0], -1.0)
+    self.assertAlmostEqual(diagnostic['packed_command'][1], -0.035)
+    self.assertEqual(diagnostic['packed_command'][2], 0.0)
+
   def test_default_off_and_unsupported_cars_retain_the_exact_previous_object(self):
     for previous in (FordPathController(), FordPscmObserverPathController()):
       for brand, flags, enabled in (("ford", FordFlags.CANFD, False), ("ford", 0, True), ("tesla", FordFlags.CANFD, True)):