mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-09-08 22:33:46 +08:00
Ford: add C2-free shared path experiment
This commit is contained in:
Isaac Barham
@@ -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.
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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__':
|
||||
|
||||
@@ -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__':
|
||||
|
||||
@@ -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"
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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"]
|
||||
|
||||
|
||||
Reference in New Issue
Block a user