From a5259057083ff8995a4ceefebb5cb3aba3eb2886 Mon Sep 17 00:00:00 2001 From: Isaac Barham Date: Fri, 4 Sep 2026 09:44:04 -0400 Subject: [PATCH] Ford: reuse coefficient calculations in shared allocator Cache per-field packet conversion and state projections within each allocation instead of recomputing them for every candidate combination. Preserve candidate ordering, scores, limits, and selected commands. Add a deterministic limiter-work regression budget. Local recorded-input mean controller CPU time falls 56%; 1292 recorded updates and 4000 randomized allocations match the previous outputs exactly. Device timing remains unverified. Assisted-by: Codex --- .../controls/lib/ford_shared_path.py | 44 +++++++++++++------ .../controls/tests/test_ford_shared_path.py | 13 ++++++ 2 files changed, 44 insertions(+), 13 deletions(-) diff --git a/openpilot/selfdrive/controls/lib/ford_shared_path.py b/openpilot/selfdrive/controls/lib/ford_shared_path.py index 83aceb2b65..6c8d2d73d5 100644 --- a/openpilot/selfdrive/controls/lib/ford_shared_path.py +++ b/openpilot/selfdrive/controls/lib/ford_shared_path.py @@ -142,17 +142,20 @@ class ContributionAllocator: self.lower = _advance(self.lower, _values(self.command), ticks, rates) self.upper = _advance(self.upper, _values(self.command), ticks, rates) - def _packet(self, values, speed=None): + def _packet_field(self, index, value, speed): # carControlSP uses Float32; CANPacker rounds in the sign-reversed DBC # coordinate system with floor(x + .5), NOT Python's ties-to-even round. - offset, angle, curvature = struct.unpack('fff', struct.pack('fff', *values)) - if speed is not None: - curvature = CarControllerParams.CURVATURE_LIMITS.apply_limits( - curvature, self.sent_curvature, speed, 0.0, True, CarControllerParams.LMC2_STEP, + value = struct.unpack('f', struct.pack('f', value))[0] + if index == 2 and speed is not None: + value = CarControllerParams.CURVATURE_LIMITS.apply_limits( + value, self.sent_curvature, speed, 0.0, True, CarControllerParams.LMC2_STEP, ) - packed = tuple(-(math.floor((-value - signal.offset) / signal.factor + 0.5) * signal.factor + signal.offset) - for value, signal in zip((offset, angle, curvature), self._wire_signals, strict=True)) - return packed, curvature + signal = self._wire_signals[index] + return -(math.floor((-value - signal.offset) / signal.factor + 0.5) * signal.factor + signal.offset), value + + def _packet(self, values, speed=None): + fields = tuple(self._packet_field(i, value, speed) for i, value in enumerate(values)) + return tuple(field[0] for field in fields), fields[2][1] def set_command(self, command, speed=None): if command.curvature_rate != 0.0: @@ -215,14 +218,29 @@ class ContributionAllocator: qpref = contributions(_values(preferred), speed) weights = (0.5, 10.0, 0.30078125 * speed ** 2) tolerance = self.tolerance(speed) + state = self.state + effects = [] + # Candidate packets share most coefficient values. Packing, limiting and + # projecting each field once avoids repeating them for every combination. + # Keep the original candidates, score order and all intermediate ticks. + for i, (weight, limit, rate) in enumerate(zip(weights, _CONTRIBUTION_LIMITS, _STATE_RATES, strict=True)): + cache = {} + for value in {candidate[i] for candidate in candidates}: + packed, _ = self._packet_field(i, value, speed) + totals = tuple(_clip(weight * _clip(packed, bound[i] - rate * tick * _FIRMWARE_DT, + bound[i] + rate * tick * _FIRMWARE_DT), -limit, limit) + for bound in (self.lower, self.upper) for tick in range(1, ticks + 1)) + endpoint = _clip(weight * _clip(packed, state[i] - rate * ticks * _FIRMWARE_DT, + state[i] + rate * ticks * _FIRMWARE_DT), -limit, limit) + cache[value] = totals, endpoint, max(0.0, abs(weight * packed) - limit) + effects.append(cache) best = None for candidate in sorted(candidates): - packed, _ = self._packet(candidate, speed) - totals = [sum(contributions(_advance(state, packed, tick), speed)) - for state in (self.lower, self.upper) for tick in range(1, ticks + 1)] - endpoint = contributions(_advance(self.state, packed, ticks), speed) + fields = tuple(cache[value] for cache, value in zip(effects, candidate, strict=True)) + totals = [sum(parts) for parts in zip(*(field[0] for field in fields), strict=True)] + endpoint = tuple(field[1] for field in fields) worst_error = max(abs(total - requested) for total in totals) - latent = sum(max(0.0, abs(weight * value) - limit) for weight, value, limit in zip(weights, packed, _CONTRIBUTION_LIMITS, strict=True)) + latent = sum(field[2] for field in fields) score = (round(max(0.0, worst_error - tolerance), 12), abs(endpoint[2] - qpref[2]), (endpoint[0] - qpref[0]) ** 2 + (endpoint[1] - qpref[1]) ** 2, latent, sum((new - old) ** 2 for new, old in zip(candidate, _values(self.last_path), strict=True))) diff --git a/openpilot/selfdrive/controls/tests/test_ford_shared_path.py b/openpilot/selfdrive/controls/tests/test_ford_shared_path.py index e11d7f9c8b..1182658e2b 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_shared_path.py +++ b/openpilot/selfdrive/controls/tests/test_ford_shared_path.py @@ -1,3 +1,4 @@ +import cProfile import math import random import tempfile @@ -58,6 +59,18 @@ class TestSharedRequest(unittest.TestCase): class TestContributionAllocator(unittest.TestCase): + def test_candidate_search_has_bounded_curvature_limiter_work(self): + allocator = ContributionAllocator(initial_state=(0.2, 0.01, 0.003)) + allocator.set_command(FordPath(True, 0.2, 0.01, 0.003)) + profile = cProfile.Profile() + profile.runcall(allocator.allocate, 0.3, FordPath(True, 0.4, 0.02, 0.002), 12.0) + limit_code = CarControllerParams.CURVATURE_LIMITS.apply_limits.__func__.__code__ + calls = sum(entry.callcount for entry in profile.getstats() if entry.code is limit_code) + # Two bounds, at most eight distinct candidate C2 values, and final packet. + # This operation budget catches repeated work without flaky wall-clock limits. + self.assertGreater(calls, 0) + self.assertLessEqual(calls, 11) + def test_fixed_request_is_preserved_while_c2_unloads(self): speed = 10.0 initial = (0.0, 0.0, 0.004)