mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-09-09 00:43:43 +08:00
Ford: align C2-free heading with desired curvature
Derive full absolute C1 heading from the same selected curvature as C0, retaining existing bounds and independent slew. Keep the former filtered model heading as a diagnostic comparison and preserve input validity gates. Add real route80 command regressions and release/reversal checks. All 97 focused tests pass; 43-segment replay preserves C0 and gates exactly and matches the independent C1 candidate. Physical tracking and stability remain unvalidated for this revision.
This commit is contained in:
Isaac Barham
@@ -1,184 +1,160 @@
|
||||
# Ford C2-free path tracking
|
||||
|
||||
Hypothesis `curvature-c0-v3` uses the planner's selected desired curvature for
|
||||
C0 centering and turn demand, while retaining v2's full model-path heading for
|
||||
C1. C2 and C3 remain zero. The historical `ford_virtual_angle.py` filename,
|
||||
controller class and setting key remain for compatibility; this is not an
|
||||
angle servo or an identified C0/C1-to-wheel conversion.
|
||||
Hypothesis `curvature-c0-c1-v4` derives both C0 and C1 from the same selected
|
||||
desired-curvature request. C0 is unchanged from v3; C1 now describes the
|
||||
heading of that requested arc instead of following a separate far-model-path
|
||||
heading. C2 and C3 remain zero. The historical filename, class and
|
||||
`FordVirtualAngleController` setting key remain for compatibility.
|
||||
|
||||
This is an experimental geometric request to the PSCM's own controller, not
|
||||
an angle servo, fitted C0/C1-to-wheel conversion or physical tracking guarantee.
|
||||
|
||||
## Evidence and reason for the change
|
||||
|
||||
Route7a ran the weak v1 controller in `7d558c065` with the experiment selected.
|
||||
Before the steeringPressed flag in its first two turns, median measured to
|
||||
requested curvature ratios were 0.120 and 0.263. Median absolute C1 in the
|
||||
first window was only 0.0025 rad, versus roughly 0.08–0.25 rad in earlier
|
||||
large-maneuver examples. These were continuous mode-2 requests with full
|
||||
reported EPS capability, no reported limit or denial, matching wire commands
|
||||
and zero stall events. Full reported capability does not establish unlimited
|
||||
physical steering authority, and an unset steeringPressed flag does not rule
|
||||
out subthreshold driver torque.
|
||||
The weak v1 experiment produced very small C1 requests and poor turn response
|
||||
in route7a. v2 restored substantial spatial turn demand, but the user reported
|
||||
drifting without centering. Route7c had the experiment off and is a default
|
||||
controller baseline; it does not validate enabled v2 behavior.
|
||||
|
||||
The v2 controller in `10e354d66` restored spatial turn demand, but its C0 came
|
||||
from lateral displacement in a short model-path preview. A real model path
|
||||
can start at the truck and gradually move toward center. That representation
|
||||
may contain little short-preview displacement even while the planner requests
|
||||
a correction. A synthetic path translated sideways did not reproduce this
|
||||
case, so that original centering test was insufficient.
|
||||
Route80 confirms enabled v3 on all nine supplied segments: commit `98662df40`,
|
||||
setting enabled and hypothesis `curvature-c0-v3`. The user reported promising
|
||||
turn response, but measured motion did not consistently reproduce the selected desired curvature. v3's C0
|
||||
followed selected action curvature while its C1 followed filtered far-model
|
||||
heading. Those two references could request different turn magnitudes.
|
||||
|
||||
The supplied route7c was driven with the experiment **off** and is a default
|
||||
controller baseline. The user's subsequent enabled-v2 report describes better
|
||||
turns but drifting without centering; enabled logs are still pending. This
|
||||
supports investigating the reference choice, but does not identify a measured
|
||||
PSCM failure mechanism or validate this replacement on the vehicle.
|
||||
In the route80 overshoot window at 417–420 s, the common-curvature candidate
|
||||
reduces the replayed C1 request from approximately 0.5 to 0.286 rad. In the
|
||||
undertracking windows at 333–339 s and 430–435 s, the candidate requests are
|
||||
nearly unchanged. This supports testing reference consistency; it does not
|
||||
predict that overshoot will disappear or undertracking will improve.
|
||||
|
||||
## C0: planner curvature expressed as lateral demand
|
||||
The PSCM also reports generic lateral-control limits in parts of route80,
|
||||
including the overshoot window. The signal does not identify a torque, rate
|
||||
or other specific physical mechanism. An unasserted limit does not establish
|
||||
accurate tracking. Reference disagreement and PSCM
|
||||
limit reports must be assessed separately.
|
||||
|
||||
## Common reference and command construction
|
||||
|
||||
controlsd supplies its existing bounded `desired_curvature`: the
|
||||
`lateralManeuverPlan` request when that service is valid, otherwise the
|
||||
`modelV2.action` request, after the existing curvature limiter. This action
|
||||
already includes the planner's centering intent. v3 uses it directly rather
|
||||
than inferring centering from the near model path or building another
|
||||
lane-centering loop. The selected action already receives the upstream delay
|
||||
treatment; C0 does not add another response advance. C1 retains its existing
|
||||
model-geometry response alignment.
|
||||
`lateralManeuverPlan` request when that service is valid, otherwise
|
||||
`modelV2.action`, after the existing curvature limiter. The selected action
|
||||
includes the planner's centering intent and receives upstream delay treatment;
|
||||
neither channel adds another response advance.
|
||||
|
||||
The controller constructs C0 in its existing command coordinates as:
|
||||
The controller computes targets in its existing command coordinates:
|
||||
|
||||
```text
|
||||
L = max(8 m, speed × 1.0 s)
|
||||
C0_target = clip(0.5 × desired_curvature × L², -5.11 m, +5.11 m)
|
||||
L0 = max(8 m, speed × 1.0 s)
|
||||
L1 = max(7 m, speed × 1.0 s)
|
||||
C0_target = clip(0.5 × desired_curvature × L0², -5.11 m, +5.11 m)
|
||||
C1_target = clip(desired_curvature × L1, -0.5 rad, +0.5 rad)
|
||||
```
|
||||
|
||||
This is a small-angle arc-displacement construction, not a prediction of
|
||||
wheel response or a measurement of the truck's actual lane displacement.
|
||||
For example, desired curvature 0.02/m produces 0.64 m at the 8 m floor and
|
||||
1.00 m at 10 m. Curvature 0.04/m produces 1.28 m and 2.00 m respectively.
|
||||
The spatial floor prevents the preview from collapsing during slow maneuvers;
|
||||
it cannot restore turn intent absent from the desired-curvature reference.
|
||||
The formula is bounded rather than extrapolated into an unlimited request.
|
||||
C0 uses a small-angle arc-displacement construction; C1 uses the arc's
|
||||
heading change. Neither is a wheel-response prediction. At the preview
|
||||
floors, curvature 0.04/m requests C0=1.28 m and C1=0.28 rad; curvature 0.10/m
|
||||
requests C0=3.20 m and bounded C1=0.50 rad. Spatial floors keep preview
|
||||
distance from collapsing during slow turns. They cannot restore turn intent
|
||||
absent from the selected action.
|
||||
|
||||
The input is **absolute desired curvature**, not desired minus measured
|
||||
curvature. Matching the requested curvature therefore does not erase the
|
||||
turn command. This replaces v2's C0; it is not added to the previous lateral
|
||||
displacement calculation, and C1 overflow is not transferred into C0.
|
||||
Both channels use **absolute desired curvature**, not desired minus measured
|
||||
curvature. Reaching the requested curvature therefore does not erase steady
|
||||
turn demand. There is no additional centering integrator, wheel-error PID,
|
||||
learned EPS gain or stall latch. C1 overflow is not transferred into C0.
|
||||
|
||||
## C1: retain full model-path heading
|
||||
|
||||
C1 continues to use the full model heading at `max(7 m, speed × 1.0 s)`
|
||||
beyond a short predicted response interval, limited by available path
|
||||
coverage. The model's heading is expressed relative to the predicted ego
|
||||
heading, then bounded to ±0.5 rad. It is not reduced to a small curvature
|
||||
error term when the truck catches up with the turn.
|
||||
|
||||
The retained model geometry is moved into the current vehicle frame every
|
||||
control cycle using traveled distance and measured CAN yaw rate. New model
|
||||
geometry is aligned to the same frame and arc station before its difference
|
||||
from the retained path is filtered. Measured ego motion is accounted for
|
||||
immediately; only model innovation is filtered. Publication age is compensated
|
||||
with the current speed and yaw rate. This is a planar motion approximation,
|
||||
not a PSCM model or reconstruction of camera latency.
|
||||
|
||||
| Parameter | Value |
|
||||
| Command parameter | Value |
|
||||
|---|---:|
|
||||
| C0 preview | max(8 m, speed × 1.0 s) |
|
||||
| C1 preview | max(7 m, speed × 1.0 s), limited by model coverage |
|
||||
| Model-innovation filter time constant for C1 | 0.30 s |
|
||||
| C1 response interval | CarParams.steerActuatorDelay; 0.20 s in these routes |
|
||||
| Independent host C0 / C1 slew limits | 4 m/s / 0.5 rad/s |
|
||||
| C0 / C1 request bounds | ±5.11 m / ±0.5 rad |
|
||||
| C1 preview | max(7 m, speed × 1.0 s) |
|
||||
| Independent C0 / C1 slew limits | 4 m/s / 0.5 rad/s |
|
||||
| C0 / C1 bounds | ±5.11 m / ±0.5 rad |
|
||||
|
||||
Each channel has its own slew limit, so a heading transition does not consume
|
||||
C0's centering rate allowance. Fractional wire-resolution increments accumulate
|
||||
internally; published values mirror the Float32 and sign-reversed Ford CAN
|
||||
packing. C0 does not pass through the model-innovation filter.
|
||||
The preview choices and limits are retained; v4 adds no new gain tuning.
|
||||
Preview distances are still **effective gains**, because they change request
|
||||
magnitude. Each channel retains its independent slew limit. Fractional wire
|
||||
increments accumulate internally, and published commands mirror Float32 and
|
||||
sign-reversed Ford CAN packing.
|
||||
|
||||
Preview floors and time horizons are **effective gains**: they change command
|
||||
size and aggressiveness. Filtering and slew limits also change the response.
|
||||
This design minimizes separate tuning mechanisms, but it is not gain-free.
|
||||
There is no fitted EPS gain, external wheel-error PID, learned channel gain,
|
||||
integrator or stall latch. Geometry alone does not establish adequate PSCM
|
||||
authority or stability, particularly when C0 and C1 come from different
|
||||
planner representations.
|
||||
## Model comparison and tradeoffs
|
||||
|
||||
## Input validity, selection and rollback
|
||||
The existing `PathReference` remains for diagnostics and validity checks.
|
||||
Measured CAN yaw and traveled distance align retained model geometry to the
|
||||
current ego frame; the 0.30 s model-innovation filter and response interval
|
||||
produce `model_heading_target` for comparison with the selected-action C1.
|
||||
That filtered model heading and yaw-frame correction no longer contribute
|
||||
to the transmitted C1 magnitude or direction. Neither C0 nor C1 commands
|
||||
use an external measured-yaw feedback correction.
|
||||
|
||||
C0 requires a fresh timestamp from the selected action service; C1 separately
|
||||
requires a fresh valid model. controlsd checks both services plus carState,
|
||||
vehicleParameters and CAN validity. Model, action and measurement age must
|
||||
each be within the controller's 150 ms freshness window. The selected action
|
||||
source can switch between model and maneuver plan using the same validity
|
||||
choice as the existing desired-curvature calculation.
|
||||
C1 consequently follows changes in the selected action more directly than
|
||||
v3. The upstream curvature limiter and existing slew limit remain, but the
|
||||
model-innovation filter no longer smooths its command. This can reduce excess
|
||||
far-path heading demand, but can also expose action noise or remove helpful
|
||||
preview. The new logs must distinguish those outcomes.
|
||||
|
||||
Invalid services or geometry, nonfinite inputs, stale inputs, backward model
|
||||
or measurement timestamps, control intervals outside 2–100 ms, speed outside
|
||||
An earlier route7c near-stop turn already exposed a reference limitation:
|
||||
at 482–485 s, selected curvature was only about +0.00174/m while the default
|
||||
controller requested C1 near −0.5 rad from far-path geometry. v4 follows the
|
||||
selected action for both channels; it cannot recover that missing or opposing
|
||||
turn intent from the model path. Passing the supplied large-maneuver fixtures
|
||||
does not establish preservation of every possible maneuver.
|
||||
|
||||
## Validity, selection and rollback
|
||||
|
||||
Freshness and service gates remain unchanged. Both command channels require
|
||||
the selected action timestamp; model geometry remains required for the
|
||||
comparison and existing validity gate. controlsd checks modelV2, the selected
|
||||
action service, carState, vehicleParameters and CAN validity. Model, action
|
||||
and measurement age must each be within the 150 ms freshness window. Action
|
||||
source selection can switch between model and maneuver plan using the same
|
||||
validity choice as the existing desired-curvature calculation.
|
||||
|
||||
Invalid services or geometry, nonfinite or stale inputs, backward model or
|
||||
measurement timestamps, control intervals outside 2–100 ms, speed outside
|
||||
0.3–55 m/s, or yaw-rate magnitude above 3 rad/s invalidate the request and
|
||||
clear state. controlsd then clears latActive, producing inactive Ford lateral
|
||||
clear state. controlsd clears latActive, producing inactive Ford lateral
|
||||
mode. Reengagement starts from zero slew state.
|
||||
|
||||
While lateral control remains authorized, driver input leaves the path
|
||||
request present, as in the default allocator; the PSCM retains its existing
|
||||
driver arbitration. The diagnostic driver_override label records the flag,
|
||||
not a promise that lateral mode has been disabled.
|
||||
While lateral control remains authorized, driver input leaves the request
|
||||
present, as in the default allocator. The PSCM retains its driver arbitration.
|
||||
The diagnostic driver_override label records the steeringPressed flag; it
|
||||
does not promise that lateral mode has been disabled.
|
||||
|
||||
Vehicle → Ford → **C2-Free Path Tracking (Experimental)** uses the existing
|
||||
`FordVirtualAngleController` setting. An already-enabled setting selects the
|
||||
replacement after updating and restarting controlsd. New settings remain
|
||||
default off. Toggle changes require an offroad-to-onroad cycle.
|
||||
Vehicle → Ford → **C2-Free Path Tracking (Experimental)** retains the existing
|
||||
key and default-off setting. An already-enabled setting selects this version
|
||||
after updating and restarting controlsd. Toggle changes require an
|
||||
offroad-to-onroad cycle. Selection remains limited to CAN FD,
|
||||
`FORD_F_150_LIGHTNING_MK1`, and EPS firmware `RL38-14D003-AA`.
|
||||
|
||||
Selection remains limited to CAN FD, `FORD_F_150_LIGHTNING_MK1`, and EPS
|
||||
firmware `RL38-14D003-AA`. It takes priority over PSCM Coefficient Observer on
|
||||
that combination. Turning it off and cycling offroad/onroad restores the
|
||||
previous controller selection. The obsolete `FordSharedPathController`
|
||||
switch remains removed. No live device setting is changed by this commit.
|
||||
The experiment takes priority over PSCM Coefficient Observer on that
|
||||
combination. Turning it off and cycling offroad/onroad restores the previous
|
||||
controller selection. The obsolete `FordSharedPathController` switch remains
|
||||
removed. No live device setting is changed by this commit.
|
||||
|
||||
## Verification and remaining uncertainty
|
||||
|
||||
Regression checks cover action-driven C0 even when the near model path has
|
||||
little centering displacement, command growth for slow turns, sustained
|
||||
steady-turn demand, retained C1 heading, motion-frame alignment, filtering,
|
||||
independent slew limits, bounds, fault resets and actual CAN packing. The
|
||||
controlsd tests exercise both action-source choices and independently stale
|
||||
or invalid model/action services. Selection, logging and settings-schema
|
||||
checks remain included.
|
||||
Regression checks cover the common-curvature C0/C1 construction, retained
|
||||
large-turn command envelopes, steady-turn demand, independent slew limits,
|
||||
fault resets, actual CAN packing, source selection, diagnostic logging and
|
||||
settings schema. Recorded fixtures retain earlier large turns and route80's
|
||||
overshoot and undertracking cases.
|
||||
|
||||
Recorded-route fixtures include earlier large left/right maneuvers, the
|
||||
oscillating experiment and v1's weak turns. Replaying these frozen inputs
|
||||
checks the commands the new code would request. It does **not** replay the
|
||||
vehicle's response to those changed commands. Historical v2 command-envelope
|
||||
tables and replay outputs are evidence for that version only, not new v3
|
||||
vehicle validation. Driver intervention also limits physical interpretation
|
||||
of several large-maneuver windows.
|
||||
|
||||
Route7c exposes a material disagreement between action curvature and the
|
||||
far model geometry during a large left turn. These active, unpressed command
|
||||
windows use signed median values:
|
||||
|
||||
| Route7c window | Speed range | Desired curvature | Recorded C0 / C1 | v3 replay C0 / C1 |
|
||||
|---|---:|---:|---:|---:|
|
||||
| 482–485 s, initial near-stop turn | 0.485–3.439 m/s | +0.00174/m | −4.123 m / −0.5 rad | +0.06 m / −0.5 rad |
|
||||
| 489–492 s, later in the turn | 2.213–4.777 m/s | −0.05821/m | −3.222 m / −0.5 rad | −1.86 m / −0.5 rad |
|
||||
|
||||
The raw model action matches desired curvature in this case. The reference
|
||||
itself therefore disagrees with the far-path turn geometry; the curvature
|
||||
limiter and preview-distance collapse do not explain it. C1 retains the
|
||||
large turn heading, but the adequacy of the combined request is unknown.
|
||||
Earlier route77 large-maneuver command checks pass; that does not establish
|
||||
that v3 preserves every large maneuver, particularly this initial turn.
|
||||
|
||||
**v3 has not been validated on the vehicle.** Neither geometric examples nor
|
||||
command replay prove centering, damping, physical steering authority or
|
||||
closed-loop stability. The next enabled logs need to show the selected action,
|
||||
C0/C1 requests, actual motion and interventions. They must establish whether
|
||||
centering improves without losing the recovered turn response.
|
||||
Command replay holds recorded motion and planner outputs fixed. It can show
|
||||
what v4 would request, but cannot show how the truck or subsequent planner
|
||||
output would change in response. **v4 has not been validated on the vehicle.**
|
||||
The next enabled logs must establish whether reference consistency reduces
|
||||
overshoot without adding oscillation or weakening turns. The nearly unchanged
|
||||
undertracking requests remain a specific unresolved limitation.
|
||||
|
||||
Startup logs retain the controller class name. The 5 Hz event
|
||||
`Ford C2-free path tracking` identifies hypothesis `curvature-c0-v3` and
|
||||
records the selected action service/time, input ages, desired/measured
|
||||
curvature, CAN yaw, C0/C1 targets, preview horizons and actual commands.
|
||||
`Ford C2-free path tracking` identifies `curvature-c0-c1-v4` and records the
|
||||
selected action service/time, input ages, desired/measured curvature, CAN
|
||||
yaw, C0/C1 targets, the diagnostic model heading, preview horizons and actual
|
||||
commands.
|
||||
|
||||
Reproduce focused checks from the repository environment:
|
||||
|
||||
```sh
|
||||
python -m pytest -q openpilot/selfdrive/controls/tests/test_ford_curvature_c0.py openpilot/selfdrive/controls/tests/test_ford_path_reference.py openpilot/selfdrive/controls/tests/test_ford_virtual_angle.py openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py openpilot/selfdrive/controls/tests/test_ford_path.py openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py
|
||||
python -m pytest -q openpilot/selfdrive/controls/tests/test_ford_curvature_heading.py openpilot/selfdrive/controls/tests/test_ford_curvature_heading_routes.py openpilot/selfdrive/controls/tests/test_ford_curvature_c0.py openpilot/selfdrive/controls/tests/test_ford_path_reference.py openpilot/selfdrive/controls/tests/test_ford_virtual_angle.py openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py openpilot/selfdrive/controls/tests/test_ford_path.py openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py
|
||||
python openpilot/sunnypilot/sunnylink/tools/compile_settings_ui.py --check
|
||||
```
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
"""C2-free action offset and spatial heading for the Lightning RL38 PSCM.
|
||||
"""C2-free offset and heading from one curvature action for the Lightning RL38 PSCM.
|
||||
|
||||
The historical Virtual Angle name/key is retained for settings compatibility.
|
||||
C0/C1 remain path geometry, never a fitted wheel-angle or torque command.
|
||||
@@ -81,10 +81,10 @@ class PathReference:
|
||||
|
||||
|
||||
class FordVirtualAngleController:
|
||||
"""Encode the planned curvature as C0 and retain model heading as C1.
|
||||
"""Encode the same absolute planned curvature as C0 and C1.
|
||||
|
||||
Measured CAN yaw rate is used only to move the reference between ego frames
|
||||
and align its preview with the response interval. No EPS gain is assumed.
|
||||
The former spatial-heading reference is retained for diagnostic comparison
|
||||
and the existing input-validity gates. No EPS gain is assumed.
|
||||
"""
|
||||
def __init__(self, response_delay=.2, tuning: PathTuning | None = None):
|
||||
self.tuning = tuning if tuning is not None else PathTuning()
|
||||
@@ -104,7 +104,7 @@ class FordVirtualAngleController:
|
||||
self.last_time = None
|
||||
self.last_measurement_time = None
|
||||
self.offset_request = self.heading_request = 0.0
|
||||
self.diagnostics = {'status': 'inactive', 'hypothesis': 'curvature-c0-v3', 'command': (0., 0., 0., 0.)}
|
||||
self.diagnostics = {'status': 'inactive', 'hypothesis': 'curvature-c0-c1-v4', 'command': (0., 0., 0., 0.)}
|
||||
|
||||
def update(self, model, desired_curvature, *, yaw_rate, speed, now, measurement_time, model_time, reference_time,
|
||||
active, valid=True, steering_pressed=False):
|
||||
@@ -134,16 +134,20 @@ class FordVirtualAngleController:
|
||||
return self.command
|
||||
advance = min(speed * self.delay, path[0][-1])
|
||||
offset_horizon = max(self.tuning.offset_horizon, speed * self.tuning.heading_time)
|
||||
heading_horizon = min(max(speed * self.tuning.heading_time, self.tuning.heading_horizon), max(path[0][-1] - advance, 0.0))
|
||||
heading_horizon = max(speed * self.tuning.heading_time, self.tuning.heading_horizon)
|
||||
model_heading_horizon = min(heading_horizon, max(path[0][-1] - advance, 0.0))
|
||||
ego = _predicted_pose(advance, current_curvature, 0.)
|
||||
_, heading = _relative_pose(advance + heading_horizon, path, ego)
|
||||
_, model_heading = _relative_pose(advance + model_heading_horizon, path, ego)
|
||||
# The selected action already contains the planner's steering correction and
|
||||
# upstream delay handling. Encode absolute curvature as a virtual parabolic
|
||||
# displacement; measured curvature must not erase a sustained turn request.
|
||||
# This preview sets command scale, not a model of the PSCM's wheel response.
|
||||
offset = .5 * desired_curvature * offset_horizon ** 2
|
||||
target_offset = float(np.clip(offset, -5.11, 5.11))
|
||||
target_heading = float(np.clip(heading, -.5, .5))
|
||||
# Keep full absolute heading demand when actual curvature catches up, and
|
||||
# release it when the selected action changes. The spatial reference above
|
||||
# is diagnostic only: neither its filter nor yaw correction steers C1.
|
||||
target_heading = float(np.clip(desired_curvature * heading_horizon, -.5, .5))
|
||||
delta_offset = target_offset - self.offset_request
|
||||
delta_heading = target_heading - self.heading_request
|
||||
# A slow C1 transition must not hold a C0 correction after action releases it.
|
||||
@@ -154,8 +158,9 @@ class FordVirtualAngleController:
|
||||
offset = _packed(self.offset_request, .01, -5.12)
|
||||
heading = _packed(self.heading_request, .0005, -.5)
|
||||
self.command = FordPath(True, offset, heading, 0., 0.)
|
||||
self.diagnostics = {'status': 'driver_override' if steering_pressed else 'active', 'hypothesis': 'curvature-c0-v3',
|
||||
self.diagnostics = {'status': 'driver_override' if steering_pressed else 'active', 'hypothesis': 'curvature-c0-c1-v4',
|
||||
'desired_curvature': desired_curvature, 'offset_target': target_offset, 'heading_target': target_heading,
|
||||
'model_heading_target': float(np.clip(model_heading, -.5, .5)), 'model_heading_horizon': model_heading_horizon,
|
||||
'offset_slew_scale': offset_scale, 'heading_slew_scale': heading_scale,
|
||||
'measurement_age': now - measurement_time, 'model_age': now - model_time, 'reference_age': now - reference_time,
|
||||
'response_delay': self.delay, 'reference_filter_time': self.tuning.filter_time, 'yaw_rate': yaw_rate,
|
||||
|
||||
+67
@@ -0,0 +1,67 @@
|
||||
{
|
||||
"description": "Real route80 turn-command regressions. Signal-only fixture; no GPS. Counterfactual commands do not predict physical vehicle response.",
|
||||
"route": "84865544361f55cb_00000080--1643deea7e",
|
||||
"source_commit": "98662df401217a00ec9fc8e73b16857b6c220150",
|
||||
"frozen_v3_controller_sha256": "576f4ec6f2dbc93f7e6c93a69839f69447eb5a0c2f834bd48b24f84a163dc2eb",
|
||||
"fixture_sha256": "c1460e2cf1d3fd52b1a036d923fec7835a7d361126ee0c2decbc3f101ee6653c",
|
||||
"episodes": [
|
||||
{
|
||||
"name": "under_333_339",
|
||||
"range_seconds": [
|
||||
331.5,
|
||||
339.0
|
||||
],
|
||||
"evidence_seconds": [
|
||||
333.0,
|
||||
339.0
|
||||
],
|
||||
"samples": 745
|
||||
},
|
||||
{
|
||||
"name": "over_417_420",
|
||||
"range_seconds": [
|
||||
415.5,
|
||||
420.0
|
||||
],
|
||||
"evidence_seconds": [
|
||||
417.0,
|
||||
420.0
|
||||
],
|
||||
"samples": 447
|
||||
},
|
||||
{
|
||||
"name": "under_430_435",
|
||||
"range_seconds": [
|
||||
428.5,
|
||||
435.0
|
||||
],
|
||||
"evidence_seconds": [
|
||||
430.0,
|
||||
435.0
|
||||
],
|
||||
"samples": 646
|
||||
}
|
||||
],
|
||||
"sources": [
|
||||
{
|
||||
"name": "84865544361f55cb_00000080--1643deea7e--5--rlog.zst",
|
||||
"bytes": 12531711,
|
||||
"sha256": "059482830794cb0eabe6069b75a9610b900bf2a93d7a6624f53c575cef997157"
|
||||
},
|
||||
{
|
||||
"name": "84865544361f55cb_00000080--1643deea7e--6--rlog.zst",
|
||||
"bytes": 12560505,
|
||||
"sha256": "147276789f5b14913adc4cd16db18f3d4bd27ce8497c9ff96fdf0315c219339f"
|
||||
},
|
||||
{
|
||||
"name": "84865544361f55cb_00000080--1643deea7e--7--rlog.zst",
|
||||
"bytes": 12660797,
|
||||
"sha256": "b311b6ace75819db52b9618154d68c7d12e2751d5046b6d174adb89ef87a223c"
|
||||
}
|
||||
],
|
||||
"pairing": "Exact controlsState desiredCurvature and consumed model publication timestamp; causal carState speed, negative CAN yaw, and steeringPressed; nearest same-cycle carControl/carControlSP within 5 ms.",
|
||||
"reference_time": "Consumed modelV2 publication time. Controller audit confirms route80 used modelV2 as reference throughout.",
|
||||
"preroll": "Each episode starts from reset 1.5 s before evidence; v3_replay stores those exact cold-start commands and gates, while recorded stores original live path fields.",
|
||||
"benchmark_clean": "Existing route80 benchmark mask: whole interval request minus 0.5 s through response (0.2 s) plus 0.25 s active, unpressed, valid, fresh, and speed >= 2 m/s.",
|
||||
"expected_common_c1": "Independent shadow: clip(desiredCurvature * max(7 m, vEgo * 1 s), +/-0.5 rad), independently slewed at 0.5 rad/s and packed to Float32/sign-reversed CAN semantics. No subtraction of measured curvature."
|
||||
}
|
||||
BIN
Binary file not shown.
@@ -61,12 +61,15 @@ class TestFordControlsLogging(unittest.TestCase):
|
||||
self.assertEqual(record['reference_service'], 'modelV2')
|
||||
self.assertEqual(record['reference_mono_time'], 123456789)
|
||||
self.assertEqual(record['status'], controller.diagnostics['status'])
|
||||
self.assertEqual(record['hypothesis'], 'curvature-c0-c1-v4')
|
||||
self.assertEqual(record['command'], list(controller.diagnostics['command']))
|
||||
if active and valid:
|
||||
self.assertEqual(record['response_delay'], 0.2)
|
||||
self.assertEqual(record['desired_curvature'], 0.01)
|
||||
self.assertEqual(record['measured_curvature'], 0.005)
|
||||
self.assertTrue(all(key in record for key in ('offset_target', 'heading_target', 'model_age', 'reference_age', 'reference_filter_time')))
|
||||
self.assertAlmostEqual(record['heading_target'], .1)
|
||||
self.assertTrue(all(key in record for key in ('offset_target', 'heading_target', 'model_heading_target', 'model_heading_horizon',
|
||||
'model_age', 'reference_age', 'reference_filter_time')))
|
||||
|
||||
def test_actual_ford_branch_uses_selected_reference_and_disables_invalid_output(self):
|
||||
source_path = Path(__file__).resolve().parents[1] / 'controlsd.py'
|
||||
|
||||
@@ -1,10 +1,9 @@
|
||||
"""Action-to-C0 regressions; these do not simulate PSCM/vehicle response."""
|
||||
from dataclasses import replace
|
||||
import math
|
||||
import unittest
|
||||
|
||||
from openpilot.selfdrive.controls.lib.ford_path import FordPath
|
||||
from openpilot.selfdrive.controls.lib.ford_virtual_angle import FordVirtualAngleController, PathTuning
|
||||
from openpilot.selfdrive.controls.lib.ford_virtual_angle import FordVirtualAngleController
|
||||
from openpilot.selfdrive.controls.tests.test_ford_path_reference import circle
|
||||
|
||||
|
||||
@@ -23,7 +22,7 @@ class TestFordCurvatureC0(unittest.TestCase):
|
||||
# Action can request recovery even when the short model preview is flat.
|
||||
path = step(controller, i * .01, sign * .002, speed=20.)
|
||||
self.assertAlmostEqual(path.path_offset, sign * .4, delta=.0051)
|
||||
self.assertAlmostEqual(path.path_angle, 0., delta=.00025)
|
||||
self.assertAlmostEqual(path.path_angle, sign * .04, delta=.000251)
|
||||
self.assertEqual((path.curvature, path.curvature_rate), (0., 0.))
|
||||
|
||||
def test_slow_turns_retain_large_absolute_demand_after_curvature_matches(self):
|
||||
@@ -35,26 +34,29 @@ class TestFordCurvatureC0(unittest.TestCase):
|
||||
self.assertAlmostEqual(path.path_offset, sign * 1.28, delta=.0051)
|
||||
self.assertGreater(sign * path.path_angle, .2)
|
||||
|
||||
def test_model_heading_cannot_inject_c0_when_action_requests_zero(self):
|
||||
def test_model_heading_cannot_inject_commands_when_action_requests_zero(self):
|
||||
for sign in (-1, 1):
|
||||
controller = FordVirtualAngleController()
|
||||
for i in range(250):
|
||||
path = step(controller, i * .01, 0., circle(sign * .12), speed=5.)
|
||||
self.assertAlmostEqual(path.path_offset, 0., delta=.0051)
|
||||
self.assertGreater(sign * path.path_angle, .4)
|
||||
self.assertAlmostEqual(path.path_angle, 0., delta=.000251)
|
||||
self.assertGreater(sign * controller.diagnostics['model_heading_target'], .4)
|
||||
|
||||
def test_c1_reversal_cannot_delay_action_c0_release(self):
|
||||
controller = FordVirtualAngleController(tuning=replace(PathTuning(), filter_time=0.))
|
||||
controller = FordVirtualAngleController()
|
||||
for i in range(200):
|
||||
path = step(controller, i * .01, .1, circle(.12), speed=5.)
|
||||
for i in range(200, 280):
|
||||
path = step(controller, i * .01, .003125, circle(.12), speed=5.)
|
||||
self.assertAlmostEqual(path.path_offset, .1)
|
||||
self.assertAlmostEqual(path.path_angle, .5)
|
||||
for i in range(200, 203):
|
||||
self.assertAlmostEqual(path.path_angle, .1)
|
||||
for i in range(280, 283):
|
||||
path = step(controller, i * .01, 0., circle(-.12), speed=5.)
|
||||
self.assertAlmostEqual(path.path_offset, 0., delta=.0051)
|
||||
self.assertGreater(path.path_angle, .45) # C1 is still in its own limited transition.
|
||||
self.assertGreater(path.path_angle, .08) # C1 is still in its own limited transition.
|
||||
|
||||
def test_c0_can_reverse_while_model_heading_still_requests_the_old_turn(self):
|
||||
def test_both_commands_reverse_while_model_heading_requests_the_old_turn(self):
|
||||
controller = FordVirtualAngleController()
|
||||
model = circle(.04)
|
||||
for i in range(200):
|
||||
@@ -62,7 +64,7 @@ class TestFordCurvatureC0(unittest.TestCase):
|
||||
for i in range(200, 240):
|
||||
path = step(controller, i * .01, -.01, model)
|
||||
self.assertLess(path.path_offset, -.3)
|
||||
self.assertGreater(path.path_angle, .2)
|
||||
self.assertLess(path.path_angle, -.07)
|
||||
|
||||
def test_invalid_or_stale_action_clears_both_requests(self):
|
||||
for desired, overrides in ((float('nan'), {}), (float('inf'), {}), (2., {}), (.01, {'reference_time': 0.}),
|
||||
|
||||
@@ -0,0 +1,50 @@
|
||||
"""Command-reference regressions, not predictions of vehicle response."""
|
||||
import unittest
|
||||
|
||||
from openpilot.selfdrive.controls.lib.ford_virtual_angle import FordVirtualAngleController
|
||||
from openpilot.selfdrive.controls.tests.test_ford_curvature_c0 import step
|
||||
from openpilot.selfdrive.controls.tests.test_ford_path_reference import circle
|
||||
|
||||
|
||||
class TestFordCurvatureHeading(unittest.TestCase):
|
||||
def test_model_turn_cannot_hold_c1_after_action_releases(self):
|
||||
controller = FordVirtualAngleController()
|
||||
model = circle(.12)
|
||||
for i in range(200):
|
||||
path = step(controller, i * .01, .04, model, speed=5.)
|
||||
self.assertAlmostEqual(path.path_angle, .28, delta=.000251)
|
||||
for i in range(200, 270):
|
||||
path = step(controller, i * .01, 0., model, speed=5.)
|
||||
self.assertAlmostEqual(path.path_angle, 0., delta=.000251)
|
||||
self.assertAlmostEqual(path.path_offset, 0., delta=.0051)
|
||||
|
||||
def test_full_heading_survives_flat_geometry_and_matching_actual_curvature(self):
|
||||
for speed in (3., 8., 20.):
|
||||
for sign in (-1, 1):
|
||||
controller = FordVirtualAngleController()
|
||||
for i in range(200):
|
||||
path = step(controller, i * .01, sign * .02, circle(), speed=speed, yaw_rate=sign * .02 * speed)
|
||||
self.assertAlmostEqual(path.path_angle, sign * .02 * max(7., speed), delta=.000251)
|
||||
self.assertEqual((path.curvature, path.curvature_rate), (0., 0.))
|
||||
|
||||
def test_heading_reverses_with_action_while_model_keeps_old_turn(self):
|
||||
controller = FordVirtualAngleController()
|
||||
model = circle(.12)
|
||||
for i in range(200):
|
||||
path = step(controller, i * .01, .04, model, speed=5.)
|
||||
for i in range(200, 320):
|
||||
path = step(controller, i * .01, -.04, model, speed=5.)
|
||||
self.assertAlmostEqual(path.path_angle, -.28, delta=.000251)
|
||||
self.assertLess(path.path_offset, 0.)
|
||||
|
||||
def test_model_shape_does_not_change_valid_action_commands(self):
|
||||
straight, bent = FordVirtualAngleController(), FordVirtualAngleController()
|
||||
for i in range(300):
|
||||
desired = .04 if i < 150 else -.04
|
||||
left = step(straight, i * .01, desired, circle(), speed=8.)
|
||||
right = step(bent, i * .01, desired, circle(.12), speed=8.)
|
||||
self.assertEqual(left, right)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
unittest.main()
|
||||
@@ -0,0 +1,61 @@
|
||||
import hashlib
|
||||
import json
|
||||
from pathlib import Path
|
||||
from types import SimpleNamespace
|
||||
import unittest
|
||||
|
||||
import numpy as np
|
||||
|
||||
from openpilot.selfdrive.controls.lib.ford_virtual_angle import FordVirtualAngleController
|
||||
|
||||
|
||||
class TestFordCurvatureHeadingRoutes(unittest.TestCase):
|
||||
@classmethod
|
||||
def setUpClass(cls):
|
||||
fixture = Path(__file__).parent / 'fixtures/ford_curvature_heading_route80.npz'
|
||||
metadata = json.loads(fixture.with_suffix('.json').read_text())
|
||||
if hashlib.sha256(fixture.read_bytes()).hexdigest() != metadata['fixture_sha256']:
|
||||
raise ValueError('Route80 command fixture hash mismatch')
|
||||
cls.data = data = dict(np.load(fixture))
|
||||
models = [SimpleNamespace(position=SimpleNamespace(x=p[0], y=p[1]), orientation=SimpleNamespace(z=p[2])) for p in data['models']]
|
||||
previous_episode = None
|
||||
commands, gates, statuses = [], [], []
|
||||
for i, now in enumerate(data['t']):
|
||||
if data['episode'][i] != previous_episode:
|
||||
controller = FordVirtualAngleController()
|
||||
previous_episode = data['episode'][i]
|
||||
command = controller.update(models[data['model_index'][i]], data['desired_curvature'][i],
|
||||
yaw_rate=data['yaw_rate'][i], speed=data['speed'][i], now=now,
|
||||
measurement_time=data['measurement_time'][i], model_time=data['model_time'][i],
|
||||
reference_time=data['reference_time'][i], active=bool(data['active'][i]),
|
||||
valid=bool(data['valid'][i]), steering_pressed=bool(data['pressed'][i]))
|
||||
commands.append((command.path_offset, command.path_angle, command.curvature, command.curvature_rate))
|
||||
gates.append(command.valid)
|
||||
statuses.append(controller.diagnostics['status'])
|
||||
cls.commands = np.array(commands)
|
||||
cls.gates = np.array(gates)
|
||||
cls.statuses = np.array(statuses)
|
||||
|
||||
def test_c0_and_output_gates_match_frozen_v3(self):
|
||||
np.testing.assert_array_equal(self.commands[:, 0], self.data['v3_replay'][:, 0])
|
||||
np.testing.assert_array_equal(self.gates, self.data['v3_valid'])
|
||||
np.testing.assert_array_equal(self.statuses, self.data['v3_status'])
|
||||
np.testing.assert_array_equal(self.commands[:, 2:], 0.)
|
||||
|
||||
def test_recorded_turns_follow_the_common_curvature_heading(self):
|
||||
# Expected values come from the independent shadow candidate evaluated on
|
||||
# these frozen route inputs. This checks commands, not new vehicle motion.
|
||||
np.testing.assert_array_equal(self.commands[:, 1], self.data['expected_common_c1'])
|
||||
for episode, expected_heading in enumerate((.14375, .286, .1895)):
|
||||
mask = (self.data['episode'] == episode) & self.data['evidence'] & self.data['benchmark_clean']
|
||||
self.assertGreater(int(mask.sum()), 100)
|
||||
self.assertAlmostEqual(float(np.median(abs(self.commands[mask, 1]))), expected_heading, delta=.001)
|
||||
# The over-response witness previously held C1 at its bound even though the
|
||||
# selected action requested substantially less heading over the same preview.
|
||||
mask = (self.data['episode'] == 1) & self.data['evidence'] & self.data['benchmark_clean']
|
||||
self.assertAlmostEqual(float(np.median(abs(self.data['recorded'][mask, 1]))), .5, delta=.0005)
|
||||
self.assertLess(float(np.median(abs(self.commands[mask, 1]))), .30)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
unittest.main()
|
||||
@@ -68,7 +68,7 @@ class TestFordPathReference(unittest.TestCase):
|
||||
np.testing.assert_allclose(y, expected_y, atol=1e-10)
|
||||
np.testing.assert_allclose(heading, initial[3] - yaw, atol=1e-10)
|
||||
|
||||
def test_model_noise_is_filtered_without_losing_heading_demand(self):
|
||||
def test_model_noise_is_filtered_for_diagnostics_without_steering_the_command(self):
|
||||
controller = FordVirtualAngleController()
|
||||
values = []
|
||||
for i in range(1600):
|
||||
@@ -80,7 +80,8 @@ class TestFordPathReference(unittest.TestCase):
|
||||
model.position.x = model.position.x * math.cos(angle)
|
||||
model.orientation.z[:] = angle
|
||||
path = run_step(controller, model, t)
|
||||
values.append(path.path_angle)
|
||||
self.assertAlmostEqual(path.path_angle, 0.)
|
||||
values.append(controller.diagnostics['model_heading_target'])
|
||||
values = np.array(values[600:])
|
||||
self.assertAlmostEqual(float(np.mean(values)), .02, delta=.001)
|
||||
self.assertLess(float(np.ptp(values)), .009) # raw heading varies by 0.02 rad
|
||||
|
||||
@@ -2183,8 +2183,8 @@
|
||||
"widget": "toggle",
|
||||
"needs_onroad_cycle": true,
|
||||
"title": "C2-Free Path Tracking (Experimental)",
|
||||
"description": "Use the planner's centering correction and retain full turn heading on the F-150 Lightning with C2 off.",
|
||||
"details": "Updates the centering request while retaining turn heading. Default off and this version is not road-validated. Available only on the F-150 Lightning with RL38-14D003-AA steering firmware; other vehicles retain their existing controller. 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": "Use one planned turn request for centering and heading on the F-150 Lightning with C2 off.",
|
||||
"details": "Aligns centering and heading with the same planned curvature. Default off and this version is not road-validated. Available only on the F-150 Lightning with RL38-14D003-AA steering firmware; other vehicles retain their existing controller. 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: C2-Free Path Tracking (Experimental)
|
||||
description: Use the planner's centering correction and retain full turn heading on the F-150 Lightning with C2 off.
|
||||
details: Updates the centering request while retaining turn heading. Default off and this version is not road-validated. Available only on the F-150 Lightning with RL38-14D003-AA steering firmware; other vehicles retain their existing controller. 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: Use one planned turn request for centering and heading on the F-150 Lightning with C2 off.
|
||||
details: Aligns centering and heading with the same planned curvature. Default off and this version is not road-validated. Available only on the F-150 Lightning with RL38-14D003-AA steering firmware; other vehicles retain their existing controller. 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
|
||||
|
||||
Reference in New Issue
Block a user