From daeb966d0548abe408d2e55f07b57e45021eb2f3 Mon Sep 17 00:00:00 2001 From: Isaac Barham Date: Fri, 4 Sep 2026 15:22:32 -0400 Subject: [PATCH] Ford: add C2-free shared path experiment --- docs/ford_shared_path_experiment.md | 125 ++--- .../controls/lib/ford_shared_path.py | 397 +++------------ .../tests/test_ford_controlsd_logging.py | 26 +- .../controls/tests/test_ford_shared_path.py | 474 +++++------------- .../sunnypilot/sunnylink/settings_ui.json | 4 +- .../settings_ui_src/pages/vehicle.yaml | 4 +- .../sunnylink/tests/test_settings_schema.py | 2 + 7 files changed, 287 insertions(+), 745 deletions(-) diff --git a/docs/ford_shared_path_experiment.md b/docs/ford_shared_path_experiment.md index ca23704f26..e5cca1bf4a 100644 --- a/docs/ford_shared_path_experiment.md +++ b/docs/ford_shared_path_experiment.md @@ -13,88 +13,73 @@ Turning it off restores the prior selection, including the older observer if that option was already enabled. No mid-drive controller switching is added. Neither the CAN frequency (100 Hz for LMC2), driver/fault enablement, nor the -existing downstream curvature and Panda checks are changed by this experiment. -No new per-vehicle tuning table or online learner is added. +existing Panda checks are changed by this experiment. No per-vehicle tuning +table, online learner, host-side slew, or coefficient handoff is added. -## One request, then allocation +## C2-free request -The controller keeps three decisions separate: +The controller samples one model pose at a firmware-derived temporal horizon: -1. **Hold request.** Use a single 7 m remaining-model preview for offset and - heading. Advance the reference by the existing 0.1 s nominal prediction - interval. Keep a bounded gentle C2 contribution, and add only the model pose - beyond the existing 0.006 /m gentle envelope. That excess grows linearly for - a circular-path fixture; it is not the old blend share multiplied by pose. -2. **Correction.** Compare model pose at the prediction interval with a - constant-curvature projection from measured steering-derived curvature. - Express offset/heading error in the predicted vehicle frame. Apply its - normalized contribution **after** hold-request saturation so a large raw - preview cannot swallow an unwind correction. At the modeled arc, this - correction is zero while the holding request remains. No noisy measured - curvature derivative or integral accumulation is used. If the model path - straightens while measured curvature is still large, recovery keeps the - opposing pose correction active until actual motion returns to the gentle - envelope; the reference alone cannot switch that correction off. -3. **Allocation.** Independently supply that total with reachable C0/C1/C2 - states. Channel preference cannot change the requested total. C2 is preferred - for settled gentle driving, reduced across the existing 0.006–0.012 /m band, - and requested zero for large maneuvers or a still-large measured turn. - Unreachable fast demand is reported, - not used as permission to refill C2. C3 remains zero. +```text +H = sqrt(0.30078125 / 0.25) = 1.096870548 seconds +C0 = model lateral displacement at H +C1 = wrap(model heading change at H - measured curvature * model arc to H) +C2 = 0 +C3 = 0 +``` -The allocator scores candidate packets against every nominal 4 ms tick in the -next 100 Hz period, not just its endpoint. It considers neighboring wire -quantizations. First minimize total-contribution error beyond half-LSB encoding -uncertainty, then favor the C2 endpoint and coordinated C0/C1 preference. Avoid -unnecessary latent coefficient accumulation beyond nominal contribution caps. +The model endpoint is translated and rotated from the model's first pose before +encoding. `position.y[0]` is not used because the rolling model begins at ego. +C0 carries the ordinary arc and centering request. C1 carries only the heading +that the current measured curvature is not predicted to cover: it adds when the +vehicle is behind, approaches zero on an aligned arc, and reverses when measured +curvature is ahead of the model. -## Explicit assumptions and limitations +The horizon comes from the decoded ML3V-14D003-BD normalized contributions: -The contribution/slew model comes from decoded **ML3V-14D003-BD**, not verified -Lightning RL38-14D003-AA or logged Raptor BC firmware. Factoring out its common -speed gain leaves: +```text +q0 = clip(0.5*C0_state, ±0.5) +q2 = clip(0.30078125*v²*C2_state, ±0.5) +``` -- `q0 = clip(0.5*C0_state, ±0.5)` -- `q1 = clip(10*C1_state, ±0.349609375)` -- `q2 = clip(0.30078125*vRaw²*C2_state, ±0.5)` +For a constant-curvature path, `y(H) ≈ 0.5*curvature*(vH)²`. Equating its C0 +contribution with C2 gives the horizon above. This is a field conversion from +one decoded firmware, not a fitted Lightning response gain. -These are nominal internal contributions, **not steering angle, torque, yaw, or -curvature**. They are fixed response assumptions, not a newly identified plant. -The hold request is bounded to nominal fast-channel authority before adding -bounded feedback. This sacrifices excess raw coefficient windup under the BD -hypothesis; if that hypothesis is wrong, actual maneuver authority may be weaker. +C0/C1 retain the full symmetric DBC-safe ranges (±5.11 m and ±0.5 rad). The +smaller contribution plateaus decoded from Raptor BD are not treated as proven +Lightning command limits; earlier physical testing found that doing so weakened +turn authority. The downstream CAN packer still quantizes the fields, and the +PSCM still owns any internal coefficient slew. -Primary states use the decoded 4 ms slew steps; inactive states drain at their -separate finite rates. Startup/gaps start with uncertainty intervals rather than -assumed zero. Before nominal history initializes, the prior default encoder is -used with output continuity. Missing/invalid model or motion input ramps the -requested path toward zero through existing limits rather than inventing error -correction. Packet prediction includes Float32 serialization, the existing -downstream C2 rate limiter, and sign-reversed DBC rounding. It does not have -PSCM execution acknowledgments or a verified delivery -delay model. Unmodeled firmware shaping remains unmodeled. +## Explicit limitations -The 0.1 s prediction is inherited as a short nominal horizon; it is **not a -verified learned lateral delay**. Wheel-derived curvature is not a complete -vehicle-motion measurement. This is not a claim of universal Ford stability or -servo-like tracking. Unchanged safety checks do not by themselves certify the -new control law. Offline replay holds actual motion/model replanning fixed and -cannot predict changed intervention rates or prove a physical steering cure. +The coefficient relationship has not been confirmed in Lightning RL38 firmware +or across Ford models. Model output updates at approximately 20 Hz even though +the latest command is repeated at 100 Hz. Wheel-derived curvature is not a +complete vehicle-motion measurement, and its multiplication by the model arc +can reintroduce a heading correction when model and measured motion are not +latency-aligned. Offline route analysis found no systematic high-speed early +entry and retained the large fast request at selected failed turns, but one +recorded hunting window became better and another became worse. + +Missing or invalid model input sends a valid zero-coefficient path while lateral +control remains active; it never falls back to a C2-producing controller. On +inactive lateral control the path is invalid and the existing car controller +sends zero coefficients. This experiment cannot prove physical tracking from +offline replay because the PSCM's inner controller and vehicle response remain +black boxes. ## Diagnostics and validation `Ford path controller selected` records the class at startup. When selected, -`Ford shared path experiment` records the nominal hypothesis, status, consumed -model timestamp, holding request, feedback, total, pose errors, state intervals, -predicted contribution error and shortfall at 5 Hz. Existing rlogs retain the -actual outgoing path/CAN commands at their original rate. `active` means the -experimental allocator is selected with initialized nominal history, not that -the model has been validated against the PSCM. +`Ford shared path experiment` records the hypothesis, status, consumed model +timestamp, temporal horizon, model offset/heading, predicted heading, heading +residual, model arc, and output fields at 5 Hz. Existing rlogs retain the actual +outgoing path/CAN commands at their original rate. -Unit tests cover hold-versus-correction behavior, both transfer directions, -unknown history, inactive drain, intermediate ticks, quantization, saturation, -S-shaped preferences, invalid input, timing gaps, unchanged downstream limits, -and default-off selection. Replay includes interventions; it is a command audit, -not a new simulated vehicle trajectory. Hardware validation must separately -assess authority, oscillation, tracking, overrides, and availability in a -controlled test environment before treating this as a driving improvement. +Unit tests cover temporal interpolation, coordinate transforms, constant-curve +equivalence, under/aligned/overtracking C1 behavior, direct sign reversal, +C2/C3 exclusion, invalid input, DBC bounds, logging, and default-off selection. +Hardware validation must separately assess turn authority, oscillation, +tracking, overrides, and availability in a controlled test environment. diff --git a/openpilot/selfdrive/controls/lib/ford_shared_path.py b/openpilot/selfdrive/controls/lib/ford_shared_path.py index 313fc3cdef..d50ff3c038 100644 --- a/openpilot/selfdrive/controls/lib/ford_shared_path.py +++ b/openpilot/selfdrive/controls/lib/ford_shared_path.py @@ -1,340 +1,105 @@ -"""Experimental shared path loop. The BD contribution map is a hypothesis, not a vehicle plant. - -All contributions below are divided by the firmware's common speed gain. They -are neither curvature nor steering angle. Keep this experiment separate from -the default controller until its closed-loop response has been validated. -""" +"""Default-off Ford C2-free path experiment.""" 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 -from openpilot.selfdrive.controls.lib.ford_path import ( - DBC_ANGLE, DBC_CURVATURE, DBC_OFFSET, FordPath, FordPathController, - _model_path, _path_pose, _predicted_pose, _relative_pose, -) +import numpy as np -_GENTLE_CURVATURE = 0.006 -_FULL_POSE_CURVATURE = 0.012 -_PREVIEW_DISTANCE = 7.0 -_PREDICTION_TIME = 0.1 -_CONTRIBUTION_LIMITS = (0.5, 0.349609375, 0.5) -_FAST_AUTHORITY = sum(_CONTRIBUTION_LIMITS[:2]) -_FIRMWARE_DT = 0.004 -_STATE_RATES = (1.5, 0.100006103515625, 0.0030059814453125) -_INACTIVE_RATES = (300.0, 30.0, 2.0) -_RESOLUTIONS = (0.01, 0.0005, 0.00002) -_RANGES = tuple((-min(abs(lo), abs(hi)), min(abs(lo), abs(hi))) for lo, hi in (DBC_OFFSET, DBC_ANGLE, DBC_CURVATURE)) -_UNKNOWN_RANGES = (*_RANGES[:2], (-0.03024, 0.03024)) # includes an earlier C2 + 10*C3 target +from opendbc.car.ford.values import FordFlags +from openpilot.selfdrive.controls.lib.ford_path import DBC_ANGLE, DBC_OFFSET, FordPath -def _clip(value, lower, upper): - return min(max(value, lower), upper) - - -def contributions(values, speed): - weights = (0.5, 10.0, 0.30078125 * speed ** 2) - return tuple(_clip(w * v, -limit, limit) for w, v, limit in zip(weights, values, _CONTRIBUTION_LIMITS, strict=True)) - - -def _values(path): - return path.path_offset, path.path_angle, path.curvature - - -def _advance(state, command, ticks, rates=_STATE_RATES): - return tuple(_clip(target, value - rate * ticks * _FIRMWARE_DT, value + rate * ticks * _FIRMWARE_DT) - for value, target, rate in zip(state, command, rates, strict=True)) +# For a constant-curvature path, y(H) ~= 0.5 * curvature * (speed * H)^2. +# This horizon makes the recovered C0 contribution equal the recovered C2 +# contribution: 0.5 * y(H) == 0.30078125 * speed^2 * curvature. +_C2_FREE_HORIZON_S = math.sqrt(0.30078125 / 0.25) +_C0_RANGE = (-min(abs(DBC_OFFSET[0]), abs(DBC_OFFSET[1])), min(abs(DBC_OFFSET[0]), abs(DBC_OFFSET[1]))) +_C1_RANGE = (-min(abs(DBC_ANGLE[0]), abs(DBC_ANGLE[1])), min(abs(DBC_ANGLE[0]), abs(DBC_ANGLE[1]))) @dataclass(frozen=True) -class PathRequest: - total: float - feedforward: float - feedback: float - preferred: FordPath - offset_error: float - heading_error: float - geometric_request: tuple[float, float] +class _C2FreeRequest: + command: FordPath + model_offset: float + model_heading: float + predicted_heading: float + arc: float -def request_for_model(model, desired_curvature: float, *, current_curvature: float, v_ego: float, - response_speed: float | None = None) -> PathRequest | None: - path = _model_path(model) if model is not None else None - if path is None or not all(math.isfinite(v) for v in (desired_curvature, current_curvature, v_ego)): +def _clip(value: float, limits: tuple[float, float]) -> float: + return min(max(value, limits[0]), limits[1]) + + +def _wrap(angle: float) -> float: + return math.atan2(math.sin(angle), math.cos(angle)) + + +def _sample(time: list[float], values: list[float]) -> float: + return float(np.interp(_C2_FREE_HORIZON_S, time, values)) + + +def _c2_free_request(model, current_curvature: float) -> _C2FreeRequest | None: + """Encode one temporal model pose through C0/C1 without charging C2.""" + try: + time = [float(value) for value in model.position.t] + x = [float(value) for value in model.position.x] + y = [float(value) for value in model.position.y] + heading = [float(value) for value in model.orientation.z] + current_curvature = float(current_curvature) + except (AttributeError, TypeError, ValueError): return None - speed = max(v_ego, 0.0) - response_speed = speed if response_speed is None else max(response_speed, 0.0) - if not math.isfinite(response_speed): + if (len(time) < 2 or len(time) != len(x) or len(time) != len(y) or len(time) != len(heading) or + not math.isfinite(current_curvature) or + not all(math.isfinite(value) for values in (time, x, y, heading) for value in values) or + any(after <= before for before, after in zip(time[:-1], time[1:], strict=True)) or + not time[0] <= _C2_FREE_HORIZON_S <= time[-1]): return None - advance = min(speed * _PREDICTION_TIME, path[0][-1]) - horizon = min(_PREVIEW_DISTANCE, path[0][-1] - advance) - if horizon <= 1e-3: - return None - offset, angle = _relative_pose(advance + horizon, path, _path_pose(advance, path)) - error_y, error_heading = _relative_pose(advance, path, _predicted_pose(advance, current_curvature, 0.0)) - demand = max(abs(2.0 * offset / horizon ** 2), abs(angle / horizon), abs(desired_curvature)) - # A soft residual beyond the existing gentle-curvature envelope, not p*pose. - # On a circular path its amplitude grows linearly with excess curvature. - excess = max(0.0, 1.0 - _GENTLE_CURVATURE / max(demand, _GENTLE_CURVATURE)) - recovery = max(0.0, 1.0 - _GENTLE_CURVATURE / max(abs(current_curvature), _GENTLE_CURVATURE)) - correction_share = max(excess, recovery) - offset_ff, angle_ff = excess * offset, excess * angle - base_curvature = _clip(desired_curvature, -_GENTLE_CURVATURE, _GENTLE_CURVATURE) - feedforward = _clip(sum(contributions((offset_ff, angle_ff, base_curvature), response_speed)), -_FAST_AUTHORITY, _FAST_AUTHORITY) - # Correct AFTER nominal feedforward saturation. Otherwise a large raw preview - # can swallow an unwind correction without changing predicted contribution. - feedback = _clip(correction_share * (0.5 * error_y + 10.0 * error_heading), -_FAST_AUTHORITY, _FAST_AUTHORITY) - total = _clip(feedforward + feedback, -_FAST_AUTHORITY, _FAST_AUTHORITY) + unwrapped_heading = np.unwrap(heading).tolist() + target_x = _sample(time, x) + target_y = _sample(time, y) + target_heading = _sample(time, unwrapped_heading) + distance = [0.0] + for i in range(1, len(x)): + distance.append(distance[-1] + math.hypot(x[i] - x[i - 1], y[i] - y[i - 1])) + arc = _sample(time, distance) - allocation_demand = max(demand, abs(current_curvature)) - share = _clip((allocation_demand - _GENTLE_CURVATURE) / (_FULL_POSE_CURVATURE - _GENTLE_CURVATURE), 0.0, 1.0) - # 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, geometric_request) - - -class ContributionAllocator: - """Allocate a separately supplied total under the nominal BD coefficient model. - - Unknown history starts as an interval, not zero. An interval becoming narrow - means the NOMINAL recurrence has initialized, not that firmware equivalence - or successful PSCM execution has been established. - """ - - def __init__(self, dt=0.01, *, initial_state=None): - self.dt = dt - self.lower = tuple(bounds[0] for bounds in _UNKNOWN_RANGES) if initial_state is None else tuple(initial_state) - self.upper = tuple(bounds[1] for bounds in _UNKNOWN_RANGES) if initial_state is None else tuple(initial_state) - self.command = FordPath() - self.last_path = FordPath() - self.sent_curvature = 0.0 - signals = CANPacker('ford_lincoln_base_pt').dbc.name_to_msg['LateralMotionControl2'].sigs - self._wire_signals = tuple(signals[name] for name in ('LatCtlPathOffst_L_Actl', 'LatCtlPath_An_Actl', 'LatCtlCurv_No_Actl')) - self._has_command = False - self.phase = 0.0 - self.predicted_total = 0.0 - self.shortfall = 0.0 - self.predicted_peak_error = 0.0 - - @property - def state(self): - return tuple((low + high) * 0.5 for low, high in zip(self.lower, self.upper, strict=True)) - - @property - def initialized(self): - return all(high - low <= resolution * 0.5 for low, high, resolution in zip(self.lower, self.upper, _RESOLUTIONS, strict=True)) - - def advance(self, elapsed): - if not math.isfinite(elapsed) or elapsed < 0.0 or elapsed > 0.1: - self.lower = tuple(bounds[0] for bounds in _UNKNOWN_RANGES) - self.upper = tuple(bounds[1] for bounds in _UNKNOWN_RANGES) - self.phase = 0.0 - return - if not self._has_command: - return - self.phase += elapsed - ticks = int((self.phase + 1e-12) / _FIRMWARE_DT) - self.phase -= ticks * _FIRMWARE_DT - rates = _STATE_RATES if self.command.valid else _INACTIVE_RATES - self.lower = _advance(self.lower, _values(self.command), ticks, rates) - self.upper = _advance(self.upper, _values(self.command), ticks, rates) - - 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. - 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, - ) - 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: - raise ValueError("C3 is not part of this experiment") - self.last_path = command - self._has_command = True - if command.valid: - packed, self.sent_curvature = self._packet(_values(command), speed) - self.command = FordPath(True, *packed, 0.0) - else: - self.command = FordPath() - self.sent_curvature = 0.0 - - @staticmethod - def tolerance(speed): - # Half a wire LSB in each normalized contribution, not a tuning deadband. - return 0.5 * (0.5 * _RESOLUTIONS[0] + 10.0 * _RESOLUTIONS[1] + 0.30078125 * speed ** 2 * _RESOLUTIONS[2]) - - def _bounds(self, preferred, speed): - previous = _values(self.last_path) - low = [_clip(previous[0] - 4.0 * self.dt, *_RANGES[0]), _clip(previous[1] - self.dt, *_RANGES[1])] - high = [_clip(previous[0] + 4.0 * self.dt, *_RANGES[0]), _clip(previous[1] + self.dt, *_RANGES[1])] - # C2 may move only toward the preferred range. In large maneuvers that - # range is exactly zero: unreachable fast demand must NOT refill C2. - limits = CarControllerParams.CURVATURE_LIMITS - for bounds, target in ((low, min(0.0, preferred.curvature)), (high, max(0.0, preferred.curvature))): - bounds.append(limits.apply_limits(target, self.sent_curvature, speed, 0.0, True, CarControllerParams.LMC2_STEP)) - return tuple(low), tuple(high) - - def _continuous_candidate(self, requested, preferred, speed, low, high, ticks): - state = self.state - qlow = contributions(_advance(state, low, ticks), speed) - qhigh = contributions(_advance(state, high, ticks), speed) - total = _clip(requested, sum(qlow), sum(qhigh)) - qpref = contributions(_advance(state, _values(preferred), ticks), speed) - slow = _clip(qpref[2], max(qlow[2], total - qhigh[0] - qhigh[1]), min(qhigh[2], total - qlow[0] - qlow[1])) - fast = total - slow - q0 = _clip(0.5 * (fast + qpref[0] - qpref[1]), max(qlow[0], fast - qhigh[1]), min(qhigh[0], fast - qlow[1])) - weights = (0.5, 10.0, 0.30078125 * speed ** 2) - # A minimum-state candidate under the nominal map, not a wire-authority - # limit. The full geometric preference is also considered by allocate(). - return tuple(_clip(q / weight if weight else pref, lo, hi) - for q, weight, pref, lo, hi in zip((q0, fast - q0, slow), weights, _values(preferred), low, high, strict=True)) - - def allocate(self, requested, preferred, speed): - if not self.initialized: - raise ValueError("coefficient history is not initialized") - if not all(math.isfinite(v) for v in (requested, speed, *_values(preferred))) or speed < 0.0 or preferred.curvature_rate != 0.0: - raise ValueError("invalid allocation request") - low, high = self._bounds(preferred, speed) - ticks = max(1, int((self.phase + self.dt + 1e-12) / _FIRMWARE_DT)) - preferred_values = _values(preferred) - qpref = contributions(preferred_values, speed) - weights = (0.5, 10.0, 0.30078125 * speed ** 2) - tolerance = self.tolerance(speed) - state = self.state - # Back-off relative to either the preferred path or retained state must - # drain latent coefficients, even if every next-tick candidate still sits - # on a nominal plateau. Extra outward demand must not erase raw geometry. - backing_off = any(total * (requested - total) < -tolerance * abs(total) - for total in (sum(qpref), sum(contributions(state, speed)))) - # Joint outward buildup need not wait for the nominal plateaus. If either - # fast field must reverse or drain toward its preference, keep the plateau - # guard: extending the other field can disrupt a settling correction. - building = requested != 0.0 and all(requested * current >= 0.0 and requested * (value - current) >= 0.0 - for current, value in zip(state[:2], preferred_values[:2], strict=True)) - preserve_geometry = tuple(not backing_off and abs(weight * value) > limit - and current * value > 0.0 and (building or abs(weight * current) >= limit) - for weight, value, current, limit in - zip(weights[:2], preferred_values[:2], state[:2], _CONTRIBUTION_LIMITS[:2], strict=True)) - candidates = {tuple(_clip(v, lo, hi) for v, lo, hi in zip(_values(path), low, high, strict=True)) - for path in (preferred, self.last_path)} - for horizon in {1, ticks}: - candidate = self._continuous_candidate(requested, preferred, speed, low, high, horizon) - neighbors = [{_clip(round(value / resolution) * resolution + shift * resolution, lo, hi) for shift in (-1, 0, 1)} - for value, resolution, lo, hi in zip(candidate, _RESOLUTIONS, low, high, strict=True)] - # Allow a larger geometric field alongside a corrected other field, - # without pulling that correction back toward the raw preference. - for i, preserve in enumerate(preserve_geometry): - if preserve: - neighbors[i].add(_clip(preferred_values[i], low[i], high[i])) - candidates.update(product(*neighbors)) - - effects = [] - # Candidate packets share most coefficient values. Packing, limiting and - # projecting each field once avoids repeating them for every combination. - # Keep every candidate and all intermediate ticks in the evaluation. - 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), - abs(packed - preferred_values[i]) / _RESOLUTIONS[i]) - effects.append(cache) - best = None - for candidate in sorted(candidates): - 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(field[2] for field in fields) - # Among nominally equivalent allocations, retain the requested fast - # geometry instead of treating the unverified BD plateaus as wire caps. - geometry_error = round(sum(field[3] for field, preserve in zip(fields[:2], preserve_geometry, strict=True) if preserve), 9) - score = (round(max(0.0, worst_error - tolerance), 12), abs(endpoint[2] - qpref[2]), - (endpoint[0] - qpref[0]) ** 2 + (endpoint[1] - qpref[1]) ** 2, geometry_error, latent, - sum((new - old) ** 2 for new, old in zip(candidate, _values(self.last_path), strict=True))) - if best is None or score < best[0]: - best = score, candidate, sum(endpoint), worst_error - - assert best is not None - _, command, self.predicted_total, self.predicted_peak_error = best - self.shortfall = requested - self.predicted_total - result = FordPath(True, *command, 0.0) - self.set_command(result, speed) - return result + dx = target_x - x[0] + dy = target_y - y[0] + cosine = math.cos(unwrapped_heading[0]) + sine = math.sin(unwrapped_heading[0]) + model_offset = -sine * dx + cosine * dy + model_heading = _wrap(target_heading - unwrapped_heading[0]) + predicted_heading = current_curvature * arc + heading_residual = _wrap(model_heading - predicted_heading) + command = FordPath( + valid=True, + path_offset=_clip(model_offset, _C0_RANGE), + path_angle=_clip(heading_residual, _C1_RANGE), + curvature=0.0, + curvature_rate=0.0, + ) + return _C2FreeRequest(command, model_offset, model_heading, predicted_heading, arc) class FordSharedPathController: - """Default-off live experiment: independent demand, feedback, and allocation.""" + """Use model C0 and residual C1 directly, with C2/C3 always zero.""" - def __init__(self, dt=0.01): - self.dt = dt - self.allocator = ContributionAllocator(dt) - self.fallback = FordPathController(dt) - self.last_time = None - self.diagnostics: dict[str, Any] = {"status": "initializing", "hypothesis": "ML3V-BD-normalized-v1"} + def __init__(self): + self.diagnostics: dict[str, object] = {"status": "initializing", "hypothesis": "ML3V-BD-C2-free-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): - elapsed = self.dt if now is None or self.last_time is None else now - self.last_time - self.last_time = now - self.allocator.advance(elapsed) - # Keep fallback history current, seeded from the command actually requested - # by this controller. A missing model/history must not cause an output jump. - self.fallback._last_path = self.allocator.last_path - valid_inputs = all(math.isfinite(v) for v in (desired_curvature, current_curvature, v_ego, v_ego_raw)) - baseline = self.fallback.update(model if valid_inputs else None, desired_curvature, current_curvature=current_curvature, - v_ego=v_ego, active=active) - speed = max(v_ego_raw, 0.0) if math.isfinite(v_ego_raw) else 0.0 - request = request_for_model(model, desired_curvature, current_curvature=current_curvature, v_ego=v_ego, - response_speed=speed) if active and valid_inputs else None - status = "inactive" if not active else "invalid_input" if request is None else "warming_history" if not self.allocator.initialized else "active" - # The reference uses filtered travel speed, while contribution scheduling - # and the unchanged downstream curvature limiter use raw wheel speed. - if status == "active": - result = self.allocator.allocate(request.total, request.preferred, speed) - else: - curvature = CarControllerParams.CURVATURE_LIMITS.apply_limits( - baseline.curvature, self.allocator.sent_curvature, speed, 0.0, True, CarControllerParams.LMC2_STEP, - ) if baseline.valid else 0.0 - result = FordPath(baseline.valid, _clip(baseline.path_offset, *_RANGES[0]), _clip(baseline.path_angle, *_RANGES[1]), curvature, 0.0) - self.allocator.set_command(result, speed) + def update(self, model, desired_curvature: float, *, current_curvature: float = 0.0, v_ego: float = 0.0, + v_ego_raw: float = 0.0, active: bool = True, now: float | None = None) -> FordPath: + request = _c2_free_request(model, current_curvature) if active and model is not None else None + status = "inactive" if not active else "invalid_input" if request is None else "active" + result = request.command if request is not None else FordPath(valid=active) self.diagnostics = { - "status": status, "hypothesis": "ML3V-BD-normalized-v1", - "requested": request.total if request else 0.0, - "feedforward": request.feedforward if request else 0.0, - "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, + "status": status, "hypothesis": "ML3V-BD-C2-free-v1", "horizon_s": _C2_FREE_HORIZON_S, + "model_offset": request.model_offset if request else None, + "model_heading": request.model_heading if request else None, + "predicted_heading": request.predicted_heading if request else None, + "heading_residual": _wrap(request.model_heading - request.predicted_heading) if request else None, + "arc": request.arc if request else None, + "command": (result.path_offset, result.path_angle, result.curvature, result.curvature_rate), } 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 d7c11dd398..ba3ada8557 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py +++ b/openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py @@ -52,28 +52,28 @@ class TestFordControlsLogging(unittest.TestCase): record = self.emit_controls_event('Ford shared path experiment', controls) self.assertEqual(record['model_mono_time'], 123456789) self.assertEqual(record['status'], controller.diagnostics['status']) - self.assertEqual(record['state'], list(controller.diagnostics['state'])) - self.assertEqual(record['requested'], controller.diagnostics['requested']) + self.assertEqual(record['command'], list(controller.diagnostics['command'])) + self.assertEqual(record['horizon_s'], controller.diagnostics['horizon_s']) - def test_geometric_request_logs_and_clears_without_changing_allocation_meaning(self): + def test_temporal_model_pose_diagnostics_log_and_clear(self): controller = FordSharedPathController() - model = SimpleNamespace(position=SimpleNamespace(x=[0.0, 20.0], y=[0.0, -10.0]), + model = SimpleNamespace(position=SimpleNamespace(t=[0.0, 2.0], 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) + 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) + self.assertEqual(record['model_offset'], controller.diagnostics['model_offset']) + self.assertEqual(record['model_heading'], controller.diagnostics['model_heading']) + self.assertEqual(record['predicted_heading'], controller.diagnostics['predicted_heading']) + self.assertEqual(record['heading_residual'], controller.diagnostics['heading_residual']) + self.assertEqual(record['arc'], controller.diagnostics['arc']) + self.assertEqual(record['command'], list(controller.diagnostics['command'])) 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']) + self.assertIsNone(record['model_offset']) + self.assertIsNone(record['heading_residual']) if __name__ == '__main__': diff --git a/openpilot/selfdrive/controls/tests/test_ford_shared_path.py b/openpilot/selfdrive/controls/tests/test_ford_shared_path.py index 9d9b4ae6ae..27a3876680 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_shared_path.py +++ b/openpilot/selfdrive/controls/tests/test_ford_shared_path.py @@ -1,291 +1,30 @@ -import cProfile import math -import random import tempfile import unittest from types import SimpleNamespace from opendbc.can import CANPacker, CANParser from opendbc.car.ford.fordcan import CanBus, create_lat_ctl2_msg -from opendbc.car.ford.values import CarControllerParams, FordFlags +from opendbc.car.ford.values import FordFlags from openpilot.selfdrive.controls.lib.ford_path import FordPath, FordPathController, FordPscmObserverPathController from openpilot.selfdrive.controls.lib.ford_shared_path import ( - ContributionAllocator, FordSharedPathController, contributions, request_for_model, select_shared_path_controller, + _C2_FREE_HORIZON_S, FordSharedPathController, _c2_free_request, select_shared_path_controller, ) -def circle(curvature, speed=8.0): +def circle(curvature: float, speed: float = 8.0): distance = [i * 0.1 for i in range(401)] heading = [curvature * s for s in distance] return SimpleNamespace( - position=SimpleNamespace(x=[math.sin(h) / curvature if curvature else s for s, h in zip(distance, heading, strict=True)], - y=[(1 - math.cos(h)) / curvature if curvature else 0.0 for h in heading]), + position=SimpleNamespace( + t=[s / speed for s in distance], + x=[math.sin(h) / curvature if curvature else s for s, h in zip(distance, heading, strict=True)], + y=[(1.0 - math.cos(h)) / curvature if curvature else 0.0 for h in heading], + ), orientation=SimpleNamespace(z=heading), ) -class TestSharedRequest(unittest.TestCase): - def test_gentle_request_is_c2_only_and_preserves_upstream_request(self): - for desired in (-0.004, 0.0, 0.004): - request = request_for_model(circle(0.003), desired, current_curvature=0.005, v_ego=20.0) - self.assertEqual(request.preferred.path_offset, 0.0) - self.assertEqual(request.preferred.path_angle, 0.0) - self.assertEqual(request.preferred.curvature, desired) - self.assertEqual(request.feedback, 0.0) - self.assertAlmostEqual(request.total, 0.30078125 * 20.0 ** 2 * desired) - - def test_saturated_turn_can_back_off_without_erasing_the_arc_at_target(self): - for sign in (-1, 1): - model = circle(sign * 0.04) - aligned = request_for_model(model, sign * 0.04, current_curvature=sign * 0.04, v_ego=8.0) - over = request_for_model(model, sign * 0.04, current_curvature=sign * 0.06, v_ego=8.0) - self.assertGreater(sign * aligned.total, 0.7) - self.assertLess(sign * over.feedback, -0.1) - self.assertLess(sign * over.total, sign * aligned.total - 0.1) - self.assertEqual(over.preferred.curvature, 0.0) - - def test_remaining_model_arc_survives_a_collapsed_action(self): - request = request_for_model(circle(0.04), 0.0, current_curvature=0.04, v_ego=8.0) - self.assertGreater(request.total, 0.5) - self.assertAlmostEqual(request.feedback, 0.0, places=5) - self.assertEqual(request.preferred.curvature, 0.0) - - def test_measured_turn_retains_unwind_after_model_straightens(self): - for sign in (-1, 1): - request = request_for_model(circle(0.0), sign * 0.002, current_curvature=sign * 0.03, v_ego=8.0) - self.assertLess(sign * request.feedback, -0.1) - self.assertLess(sign * request.total, 0.0) - self.assertEqual(request.preferred.curvature, 0.0) - - -class TestContributionAllocator(unittest.TestCase): - def test_fresh_turn_does_not_wait_for_nominal_plateaus_to_send_geometry(self): - for sign in (-1, 1): - allocator = ContributionAllocator(initial_state=(0.0, 0.0, 0.0)) - preferred = FordPath(True, sign * 2.0, sign * 0.3, 0.0) - requested = sum(contributions((preferred.path_offset, preferred.path_angle, 0.0), 8.0)) - for _ in range(10): - command = allocator.allocate(requested, preferred, 8.0) - self.assertEqual(command.curvature, 0.0) - allocator.advance(allocator.dt) - # Existing host slew permits 4 m/s and 1 rad/s. Allow one control tick - # for initialization, not the time needed to fill a nominal plateau. - self.assertGreaterEqual(sign * command.path_offset, 4.0 * 9 * allocator.dt - 1e-9) - self.assertGreaterEqual(sign * command.path_angle, 9 * allocator.dt - 1e-9) - - def test_zero_total_does_not_treat_opposing_fields_as_joint_buildup(self): - for sign in (-1, 1): - initial = (sign * 0.6, -sign * 0.03, 0.0) - allocator = ContributionAllocator(initial_state=initial) - allocator.set_command(FordPath(True, *initial), 8.0) - command = allocator.allocate(0.0, FordPath(True, sign * 0.7, -sign * 0.3, 0.0), 8.0) - self.assertLessEqual(abs(command.path_angle), 0.035) - self.assertEqual(command.curvature, 0.0) - - # Known limitation of experimental early geometry: queued commands prolong - # release in the nominal BD model. This is NOT verified physical behavior. - @unittest.expectedFailure - def test_short_turn_release_preserves_command_history_not_just_coefficient_state(self): - for sign in (-1, 1): - allocator = ContributionAllocator(initial_state=(0.0, 0.0, 0.0)) - preferred = FordPath(True, sign * 3.5, sign * 0.5, 0.0) - requested = sum(contributions((preferred.path_offset, preferred.path_angle, 0.0), 8.0)) - for _ in range(30): - allocator.allocate(requested, preferred, 8.0) - allocator.advance(allocator.dt) - initial_total = sign * sum(contributions(allocator.state, 8.0)) - # Continue the same allocator. Recreating it with command=state hides - # queued commands that can keep building after the request disappears. - for _ in range(30): - command = allocator.allocate(0.0, FordPath(True), 8.0) - self.assertEqual(command.curvature, 0.0) - allocator.advance(allocator.dt) - self.assertLessEqual(sign * sum(contributions(allocator.state, 8.0)), initial_total + allocator.tolerance(8.0)) - self.assertLessEqual(abs(sum(contributions(allocator.state, 8.0))), allocator.tolerance(8.0)) - - def test_nominal_plateau_does_not_erase_requested_fast_geometry(self): - allocator = ContributionAllocator(initial_state=(0.0, 0.0, 0.0)) - preferred = FordPath(True, 2.0, 0.12, 0.0, 0.0) - requested = sum(contributions((preferred.path_offset, preferred.path_angle, 0.0), 5.0)) - for _ in range(150): - command = allocator.allocate(requested, preferred, 5.0) - self.assertEqual(command.curvature, 0.0) - allocator.advance(0.01) - self.assertAlmostEqual(command.path_offset, preferred.path_offset) - self.assertAlmostEqual(command.path_angle, preferred.path_angle) - self.assertAlmostEqual(allocator.shortfall, 0.0) - - def test_overturn_correction_releases_large_geometry_still_in_model(self): - for sign in (-1, 1): - model = circle(sign * 0.06) - hold = request_for_model(model, sign * 0.06, current_curvature=sign * 0.06, v_ego=2.0) - correction = request_for_model(model, sign * 0.06, current_curvature=sign * 0.10, v_ego=2.0) - initial = (hold.preferred.path_offset, hold.preferred.path_angle, 0.0) - allocator = ContributionAllocator(initial_state=initial) - allocator.set_command(hold.preferred, 2.0) - self.assertLess(sign * correction.total, sign * hold.total) - self.assertGreater(abs(correction.preferred.path_angle), 0.3) - for _ in range(100): - command = allocator.allocate(correction.total, correction.preferred, 2.0) - self.assertEqual(command.curvature, 0.0) - allocator.advance(0.01) - self.assertLessEqual(abs(allocator.shortfall), allocator.tolerance(2.0)) - - def test_extra_outward_demand_does_not_erase_preferred_fast_geometry(self): - for sign in (-1, 1): - allocator = ContributionAllocator(initial_state=(0.0, 0.0, 0.0)) - preferred = FordPath(True, sign * 0.24, sign * 0.0515, 0.0) - requested = sign * 0.6644 - self.assertGreater(abs(requested), abs(sum(contributions((preferred.path_offset, preferred.path_angle, 0.0), 10.0)))) - for _ in range(150): - command = allocator.allocate(requested, preferred, 10.0) - self.assertEqual(command.curvature, 0.0) - allocator.advance(0.01) - self.assertAlmostEqual(command.path_angle, preferred.path_angle) - self.assertLessEqual(abs(allocator.shortfall), allocator.tolerance(10.0)) - - def test_straight_release_does_not_reintroduce_geometry_during_latent_unwind(self): - for sign in (-1, 1): - initial = (sign * 3.5, sign * 0.5, 0.0) - allocator = ContributionAllocator(initial_state=initial) - allocator.set_command(FordPath(True, *initial), 8.0) - # A deliberately charged nominal state takes seconds to drain. Once - # its total settles, preferring zero geometry must not create a second - # opposite contribution as the remaining C1 state leaves saturation. - for i in range(600): - command = allocator.allocate(0.0, FordPath(True), 8.0) - self.assertEqual(command.curvature, 0.0) - allocator.advance(0.01) - if i >= 310: - self.assertLessEqual(abs(sum(contributions(allocator.state, 8.0))), allocator.tolerance(8.0) + 1e-8) - - def test_larger_heading_does_not_displace_corrected_offset_during_model_unwind(self): - for sign in (-1, 1): - initial = (sign * 3.5, sign * 0.5, 0.0) - allocator = ContributionAllocator(initial_state=initial) - allocator.set_command(FordPath(True, *initial), 20.0) - request = request_for_model(circle(0.0), sign * 0.002, current_curvature=sign * 0.04, v_ego=20.0) - for i in range(650): - command = allocator.allocate(request.total, request.preferred, 20.0) - self.assertEqual(command.curvature, 0.0) - allocator.advance(0.01) - if i >= 500: - self.assertLessEqual(abs(sum(contributions(allocator.state, 20.0)) - request.total), allocator.tolerance(20.0) + 1e-8) - - 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) - requested = sum(contributions(initial, speed)) - allocator = ContributionAllocator(initial_state=initial) - allocator.set_command(FordPath(True, *initial)) - for _ in range(180): - command = allocator.allocate(requested, FordPath(True, 0.1, 0.01, 0.0), speed) - self.assertLessEqual(command.curvature, 0.004) - allocator.advance(0.01) - self.assertLessEqual(abs(sum(contributions(allocator.state, speed)) - requested), allocator.tolerance(speed) + 1e-8) - self.assertLess(abs(allocator.state[2]), 0.00004) - self.assertGreater(allocator.state[0] + allocator.state[1], 0.0) - - def test_c2_reloads_only_as_fast_contribution_can_be_removed(self): - speed = 20.0 - initial = (0.4, 0.01, 0.0) - requested = sum(contributions(initial, speed)) - desired_c2 = requested / (0.30078125 * speed ** 2) - allocator = ContributionAllocator(initial_state=initial) - allocator.set_command(FordPath(True, *initial)) - for _ in range(400): - allocator.allocate(requested, FordPath(True, 0.0, 0.0, desired_c2), speed) - self.assertLessEqual(allocator.predicted_peak_error, allocator.tolerance(speed) + 1e-8) - allocator.advance(0.01) - self.assertLess(abs(allocator.state[0]), 0.01) - self.assertLess(abs(allocator.state[1]), 0.0005) - self.assertAlmostEqual(allocator.state[2], desired_c2, delta=0.00002) - - def test_inactive_state_drains_instead_of_resetting_instantly(self): - allocator = ContributionAllocator(initial_state=(1.0, 0.1, 0.01)) - allocator.set_command(FordPath()) - self.assertEqual(allocator.state, (1.0, 0.1, 0.01)) - allocator.advance(0.004) - self.assertAlmostEqual(allocator.state[2], 0.002) - allocator.advance(0.004) - self.assertEqual(allocator.state, (0.0, 0.0, 0.0)) - - def test_unknown_history_and_gaps_cannot_be_used_as_exact_state(self): - allocator = ContributionAllocator() - self.assertFalse(allocator.initialized) - with self.assertRaises(ValueError): - allocator.allocate(0.1, FordPath(True), 10.0) - allocator.set_command(FordPath()) - allocator.advance(0.024) - self.assertTrue(allocator.initialized) - allocator.advance(0.25) - self.assertFalse(allocator.initialized) - - def test_startup_does_not_invent_an_inactive_command(self): - allocator = ContributionAllocator() - allocator.advance(0.1) - self.assertFalse(allocator.initialized) - - def test_unreachable_request_is_reported_and_never_refills_c2(self): - allocator = ContributionAllocator(initial_state=(0.0, 0.0, 0.0)) - command = allocator.allocate(0.84, FordPath(True, 1.0, 0.03, 0.0), 10.0) - self.assertEqual(command.curvature, 0.0) - self.assertGreater(allocator.shortfall, 0.7) - self.assertLessEqual(command.path_offset, 0.04) - self.assertLessEqual(command.path_angle, 0.01) - - def test_active_state_uses_250hz_slew_and_wire_values(self): - allocator = ContributionAllocator(initial_state=(0.0, 0.0, 0.0)) - allocator.set_command(FordPath(True, 0.1001, 0.0101, 0.002001)) - allocator.advance(0.01) - self.assertAlmostEqual(allocator.state[0], 1.5 * 0.008) - self.assertAlmostEqual(allocator.state[1], 0.100006103515625 * 0.008) - allocator.advance(0.01) - self.assertAlmostEqual(allocator.state[0], 1.5 * 0.020) - self.assertAlmostEqual(allocator.command.path_angle, 0.0100) - - def test_estimated_packet_matches_float32_carcontrol_and_can_packing(self): - from openpilot.cereal import custom - packer = CANPacker('ford_lincoln_base_pt') - parser = CANParser('ford_lincoln_base_pt', [('LateralMotionControl2', 0)], 0) - allocator = ContributionAllocator(initial_state=(0.0, 0.0, 0.0)) - for c0, c1, c2 in ((0.005, 0.00025, 0.00001), (-0.005, -0.00025, -0.00001), - (0.115, 0.03475, 0.00301), (-0.115, -0.03475, -0.00301)): - command = FordPath(True, c0, c1, c2) - allocator.set_command(command) - message = custom.CarControlSP.new_message() - path = message.fordLateralPath - path.pathOffset, path.pathAngle, path.curvature = c0, c1, c2 - packet = create_lat_ctl2_msg(packer, CanBus(fingerprint={0: {}}), 2, -path.pathOffset, -path.pathAngle, -path.curvature, 0.0, 0) - parser.update([0, [packet]]) - decoded = parser.vl['LateralMotionControl2'] - for value, field in ((allocator.command.path_offset, 'LatCtlPathOffst_L_Actl'), - (allocator.command.path_angle, 'LatCtlPath_An_Actl'), (allocator.command.curvature, 'LatCtlCurv_No_Actl')): - self.assertAlmostEqual(value, -decoded[field], places=10) - - def test_s_shape_preference_keeps_opposing_fast_fields(self): - allocator = ContributionAllocator(initial_state=(0.0, 0.0, 0.0)) - preferred = FordPath(True, 0.2, -0.01, 0.0) - for _ in range(100): - command = allocator.allocate(0.0, preferred, 8.0) - allocator.advance(0.01) - self.assertGreater(command.path_offset, 0.1) - self.assertLess(command.path_angle, -0.005) - self.assertLessEqual(abs(sum(contributions(allocator.state, 8.0))), allocator.tolerance(8.0)) - - class TestSharedController(unittest.TestCase): def test_native_toggle_is_default_off_and_selection_is_startup_only(self): from openpilot.common.params import Params @@ -298,89 +37,140 @@ class TestSharedController(unittest.TestCase): chosen = select_shared_path_controller('ford', FordFlags.CANFD, params.get_bool('FordSharedPathController'), prior) self.assertIsInstance(chosen, FordSharedPathController) params.put_bool('FordSharedPathController', False, block=True) - self.assertIsInstance(chosen, FordSharedPathController) # running selection is unchanged + self.assertIsInstance(chosen, FordSharedPathController) self.assertIs(select_shared_path_controller('ford', FordFlags.CANFD, params.get_bool('FordSharedPathController'), prior), prior) - def test_inactive_initializes_history_and_large_turn_uses_live_fast_fields(self): - controller = FordSharedPathController() - model = circle(0.04) - for _ in range(4): - self.assertFalse(controller.update(model, 0.04, current_curvature=0.0, v_ego=8.0, v_ego_raw=8.0, active=False).valid) - for _ in range(100): - command = controller.update(model, 0.04, current_curvature=0.0, v_ego=8.0, v_ego_raw=8.0) - self.assertTrue(command.valid) - self.assertEqual(command.curvature, 0.0) - self.assertEqual(controller.diagnostics['status'], 'active') - self.assertGreater(command.path_offset, 0.0) - self.assertGreater(command.path_angle, 0.0) - - def test_large_model_geometry_reaches_wire_limits_without_losing_raw_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.assertAlmostEqual(offset, command.path_offset) - self.assertLess(heading, -0.5) - self.assertAlmostEqual(diagnostic['packed_command'][0], offset, delta=0.005) - self.assertAlmostEqual(diagnostic['packed_command'][1], -0.5) - 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)): self.assertIs(select_shared_path_controller(brand, flags, enabled, previous), previous) self.assertIsInstance(select_shared_path_controller("ford", FordFlags.CANFD, True, previous), FordSharedPathController) - def test_invalid_model_and_timing_gap_do_not_jump_fast_fields(self): + def test_c2_and_c3_are_always_zero(self): controller = FordSharedPathController() - model = circle(0.04) - for i in range(4): - controller.update(model, 0.04, v_ego=8.0, v_ego_raw=8.0, active=False, now=i * 0.01) - for i in range(4, 104): - previous = controller.update(model, 0.04, v_ego=8.0, v_ego_raw=8.0, now=i * 0.01) - command = controller.update(None, 0.04, v_ego=8.0, v_ego_raw=8.0, now=1.04) - self.assertEqual(controller.diagnostics['status'], 'invalid_input') - self.assertLessEqual(abs(command.path_offset - previous.path_offset), 0.04 + 1e-9) - self.assertLessEqual(abs(command.path_angle - previous.path_angle), 0.01 + 1e-9) - previous = command - command = controller.update(model, 0.04, v_ego=8.0, v_ego_raw=8.0, now=2.0) - self.assertEqual(controller.diagnostics['status'], 'warming_history') - self.assertLessEqual(abs(command.path_offset - previous.path_offset), 0.04 + 1e-9) - self.assertLessEqual(abs(command.path_angle - previous.path_angle), 0.01 + 1e-9) - - def test_random_sequences_keep_existing_command_and_downstream_limits(self): - rng = random.Random(68) - controller = FordSharedPathController() - previous = FordPath() - for i in range(600): - speed = rng.uniform(1.0, 35.0) - curvature = rng.uniform(-0.12, 0.12) - active = i >= 4 and i % 37 != 0 - command = controller.update(circle(curvature), rng.uniform(-0.02, 0.02), - current_curvature=rng.uniform(-0.12, 0.12), v_ego=speed, v_ego_raw=speed, active=active) - self.assertTrue(all(math.isfinite(v) for v in (command.path_offset, command.path_angle, command.curvature))) - self.assertLessEqual(abs(command.path_offset), 5.11) - self.assertLessEqual(abs(command.path_angle), 0.5) - self.assertLessEqual(abs(command.curvature), 0.02) + requests = [ + controller.update(circle(curvature, speed), desired, current_curvature=actual, + v_ego=speed, v_ego_raw=speed) + for curvature, desired, actual, speed in ( + (0.0, 0.0, 0.0, 20.0), (0.003, -0.01, 0.005, 20.0), + (0.12, 0.12, 0.0, 5.0), (-0.12, 0.12, -0.2, 5.0), + ) + ] + requests += [controller.update(None, 0.1), controller.update(circle(0.1), 0.1, active=False)] + for command in requests: + self.assertEqual(command.curvature, 0.0) self.assertEqual(command.curvature_rate, 0.0) - if active: - self.assertLessEqual(abs(command.path_offset - previous.path_offset), 0.04 + 1e-9) - self.assertLessEqual(abs(command.path_angle - previous.path_angle), 0.01 + 1e-9) - limited = CarControllerParams.CURVATURE_LIMITS.apply_limits(command.curvature, previous.curvature, speed, 0.0, True, 1) - self.assertAlmostEqual(command.curvature, limited, places=10) - else: - self.assertEqual(command, FordPath()) - previous = command + + def test_constant_curvature_uses_firmware_derived_temporal_offset(self): + for sign in (-1, 1): + for speed in (5.0, 10.0, 15.0, 20.0): + curvature = sign * 0.001 + request = _c2_free_request(circle(curvature, speed), curvature) + self.assertIsNotNone(request) + c2_equivalent = 0.30078125 * speed ** 2 * curvature + self.assertAlmostEqual(0.5 * request.command.path_offset, c2_equivalent, delta=abs(c2_equivalent) * 1e-3) + self.assertAlmostEqual(request.command.path_angle, 0.0, delta=1e-5) + self.assertEqual(request.command.curvature, 0.0) + + def test_exact_time_sample_and_origin_transform(self): + horizon = _C2_FREE_HORIZON_S + local_x = [0.0, 2.0, 6.0] + local_y = [0.0, 0.4, 1.2] + local_heading = [0.0, 0.12, 0.3] + + def transformed(rotation, tx, ty): + cosine, sine = math.cos(rotation), math.sin(rotation) + return SimpleNamespace( + position=SimpleNamespace( + t=[0.0, horizon, 2.0 * horizon], + x=[tx + cosine * x - sine * y for x, y in zip(local_x, local_y, strict=True)], + y=[ty + sine * x + cosine * y for x, y in zip(local_x, local_y, strict=True)], + ), + orientation=SimpleNamespace(z=[rotation + heading for heading in local_heading]), + ) + + reference = _c2_free_request(transformed(0.0, 0.0, 0.0), 0.01) + moved = _c2_free_request(transformed(0.7, 40.0, -3.0), 0.01) + self.assertIsNotNone(reference) + self.assertIsNotNone(moved) + self.assertAlmostEqual(reference.model_offset, 0.4) + self.assertAlmostEqual(reference.model_heading, 0.12) + self.assertAlmostEqual(reference.arc, math.hypot(2.0, 0.4)) + self.assertAlmostEqual(moved.model_offset, reference.model_offset) + self.assertAlmostEqual(moved.model_heading, reference.model_heading) + self.assertAlmostEqual(moved.arc, reference.arc) + self.assertAlmostEqual(moved.command.path_offset, reference.command.path_offset) + self.assertAlmostEqual(moved.command.path_angle, reference.command.path_angle) + + def test_c1_is_only_remaining_heading_error(self): + for sign in (-1, 1): + curvature = sign * 0.02 + model = circle(curvature, 8.0) + behind = _c2_free_request(model, 0.0) + aligned = _c2_free_request(model, curvature) + ahead = _c2_free_request(model, 2.0 * curvature) + self.assertGreater(sign * behind.command.path_angle, 0.0) + self.assertAlmostEqual(aligned.command.path_angle, 0.0, delta=1e-5) + self.assertLess(sign * ahead.command.path_angle, 0.0) + for request in (behind, aligned, ahead): + expected = math.atan2(math.sin(request.model_heading - request.predicted_heading), + math.cos(request.model_heading - request.predicted_heading)) + self.assertAlmostEqual(request.command.path_angle, expected) + + def test_action_curvature_does_not_change_model_pose_command(self): + controller = FordSharedPathController() + model = circle(0.03, 10.0) + commands = [controller.update(model, desired, current_curvature=0.01, v_ego=10.0, v_ego_raw=10.0) + for desired in (-0.1, -0.005, 0.0, 0.005, 0.1)] + self.assertTrue(all(command == commands[0] for command in commands[1:])) + + def test_direct_targets_do_not_have_host_side_slew_or_history(self): + controller = FordSharedPathController() + positive = controller.update(circle(0.08, 5.0), 0.08, current_curvature=0.0) + negative = controller.update(circle(-0.08, 5.0), -0.08, current_curvature=0.0) + positive_again = controller.update(circle(0.08, 5.0), 0.08, current_curvature=0.0) + self.assertGreater(positive.path_offset, 0.0) + self.assertGreater(positive.path_angle, 0.0) + self.assertLess(negative.path_offset, 0.0) + self.assertLess(negative.path_angle, 0.0) + self.assertEqual(positive_again, positive) + + def test_invalid_model_never_falls_back_to_c2(self): + controller = FordSharedPathController() + invalid_models = [ + None, + SimpleNamespace(position=SimpleNamespace(t=[0.0], x=[0.0], y=[0.0]), orientation=SimpleNamespace(z=[0.0])), + SimpleNamespace(position=SimpleNamespace(t=[0.0, 2.0], x=[0.0, math.nan], y=[0.0, 0.0]), + orientation=SimpleNamespace(z=[0.0, 0.0])), + ] + for model in invalid_models: + command = controller.update(model, 0.02, current_curvature=0.01) + self.assertEqual(command, FordPath(valid=True)) + self.assertEqual(controller.diagnostics['status'], 'invalid_input') + self.assertEqual(controller.update(circle(0.02), 0.02, active=False), FordPath()) + self.assertEqual(controller.diagnostics['status'], 'inactive') + + def test_commands_use_full_symmetric_dbc_bounds(self): + horizon = _C2_FREE_HORIZON_S + packer = CANPacker('ford_lincoln_base_pt') + parser = CANParser('ford_lincoln_base_pt', [('LateralMotionControl2', 0)], 0) + for sign in (-1, 1): + model = SimpleNamespace( + position=SimpleNamespace(t=[0.0, horizon], x=[0.0, 0.0], y=[0.0, sign * 20.0]), + orientation=SimpleNamespace(z=[0.0, sign * 2.0]), + ) + command = FordSharedPathController().update(model, 0.0, current_curvature=0.0) + self.assertEqual(command.path_offset, sign * 5.11) + self.assertEqual(command.path_angle, sign * 0.5) + self.assertEqual(command.curvature, 0.0) + self.assertEqual(command.curvature_rate, 0.0) + packet = create_lat_ctl2_msg(packer, CanBus(fingerprint={0: {}}), 2, -command.path_offset, + -command.path_angle, -command.curvature, -command.curvature_rate, 0) + parser.update([0, [packet]]) + decoded = parser.vl['LateralMotionControl2'] + self.assertAlmostEqual(decoded['LatCtlPathOffst_L_Actl'], -command.path_offset) + self.assertAlmostEqual(decoded['LatCtlPath_An_Actl'], -command.path_angle) + self.assertEqual(decoded['LatCtlCurv_No_Actl'], 0.0) if __name__ == '__main__': diff --git a/openpilot/sunnypilot/sunnylink/settings_ui.json b/openpilot/sunnypilot/sunnylink/settings_ui.json index 4a94ba72f7..630a575102 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui.json +++ b/openpilot/sunnypilot/sunnylink/settings_ui.json @@ -2183,8 +2183,8 @@ "widget": "toggle", "needs_onroad_cycle": true, "title": "Shared Path Controller (Experimental)", - "description": "Experimental live steering for Ford CAN FD vehicles, combining path feedback with a state-aware C0/C1/C2 handoff.", - "details": "Default off. Uses nominal firmware response assumptions that are unverified across Ford models; this controller is not road-validated. Enable only for controlled testing. Takes priority over PSCM Coefficient Observer while enabled. Turning it off restores the previous controller selection. Changes apply after a real offroad-to-onroad cycle, not immediately or on disengagement alone.", + "description": "Experimental live steering for Ford CAN FD vehicles, using a temporal model-path C0 offset and C1 heading residual while keeping C2/C3 zero.", + "details": "Default off. The 1.097-second model horizon is derived from decoded Raptor firmware coefficients and is unverified across Ford models; this controller is not road-validated. Enable only for controlled testing. Takes priority over PSCM Coefficient Observer while enabled. Turning it off restores the previous controller selection. Changes apply after a real offroad-to-onroad cycle, not immediately or on disengagement alone.", "enablement": [ { "type": "offroad_only" diff --git a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml index 2a2c028ac6..08aea25774 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml +++ b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml @@ -14,8 +14,8 @@ sections: widget: toggle needs_onroad_cycle: true title: Shared Path Controller (Experimental) - description: Experimental live steering for Ford CAN FD vehicles, combining path feedback with a state-aware C0/C1/C2 handoff. - details: Default off. Uses nominal firmware response assumptions that are unverified across Ford models; this controller is not road-validated. Enable only for controlled testing. Takes priority over PSCM Coefficient Observer while enabled. Turning it off restores the previous controller selection. Changes apply after a real offroad-to-onroad cycle, not immediately or on disengagement alone. + description: Experimental live steering for Ford CAN FD vehicles, using a temporal model-path C0 offset and C1 heading residual while keeping C2/C3 zero. + details: Default off. The 1.097-second model horizon is derived from decoded Raptor firmware coefficients and is unverified across Ford models; this controller is not road-validated. Enable only for controlled testing. Takes priority over PSCM Coefficient Observer while enabled. Turning it off restores the previous controller selection. Changes apply after a real offroad-to-onroad cycle, not immediately or on disengagement alone. enablement: - $ref: '#/macros/offroad' - key: FordPscmObserver diff --git a/openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py b/openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py index 0307ca6e7b..d50026e194 100644 --- a/openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py +++ b/openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py @@ -288,6 +288,8 @@ class TestKnownVehicleSettings(OpenpilotTestCase): # No other toggle can prevent disabling this experiment while offroad. assert shared["enablement"] == [{"type": "offroad_only"}] assert "Ford CAN FD" in shared["description"] + assert "C0" in shared["description"] and "C1" in shared["description"] + assert "C2/C3 zero" in shared["description"] assert "not road-validated" in shared["details"] assert "offroad" in shared["details"] and "onroad" in shared["details"]