From 3eb7938aad0570ed8fc2bb14efe3eff7f46378cb Mon Sep 17 00:00:00 2001 From: Isaac Barham Date: Fri, 4 Sep 2026 10:36:17 -0400 Subject: [PATCH] Ford: retain geometric demand in shared path diagnostics --- .../controls/lib/ford_shared_path.py | 17 +++++++++++---- .../tests/test_ford_controlsd_logging.py | 20 ++++++++++++++++++ .../controls/tests/test_ford_shared_path.py | 21 +++++++++++++++++++ 3 files changed, 54 insertions(+), 4 deletions(-) diff --git a/openpilot/selfdrive/controls/lib/ford_shared_path.py b/openpilot/selfdrive/controls/lib/ford_shared_path.py index 6c8d2d73d5..d5f1b24553 100644 --- a/openpilot/selfdrive/controls/lib/ford_shared_path.py +++ b/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 diff --git a/openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py b/openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py index e350f9b79b..d7c11dd398 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py +++ b/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() diff --git a/openpilot/selfdrive/controls/tests/test_ford_shared_path.py b/openpilot/selfdrive/controls/tests/test_ford_shared_path.py index 1182658e2b..85a9f49d1d 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_shared_path.py +++ b/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)):