mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-09-18 12:33:43 +08:00
Ford: trial direct model-path offset and heading
Sample position and orientation at the existing distance stations, retain measured-steering feedback against the path heading, and apply request limits independently to C0 and C1. Reuse the geometry toggle with upstream and maneuver fallback unchanged. Defer the untested raw-torque filtering change so this trial compares against the driven baseline. Validate with 593 tests and paired full-route replays; no physical tracking improvement is claimed.
This commit is contained in:
Isaac Barham
@@ -0,0 +1,95 @@
|
||||
# Direct model-path C0/C1 trial
|
||||
|
||||
This trial takes priority over the untested filtered-driver change. Driver
|
||||
arbitration is restored to the last driven baseline, `18ded0380`: a raw torque
|
||||
crossing above 1 Nm, filtered driver input, or fresh PSCM driver override still
|
||||
clears feedback. The filtered-driver experiment remains available in history at
|
||||
`4f7d2b8d2` and is not included in this trial.
|
||||
|
||||
Keep **Selected-Action Path Tracking** and **Model Geometry Reference** enabled
|
||||
in Sunnylink. The existing geometry toggle now selects direct-path mapping at
|
||||
controlsd startup. Keep **C0 one-second distance** off for fixed 7 m C0. Apply an
|
||||
offroad-to-onroad cycle after updating. Geometry off restores the original
|
||||
model-action controller; the master controller toggle off restores upstream
|
||||
Ford control. Joystick and lateral-maneuver overrides retain their priority.
|
||||
|
||||
## Mapping
|
||||
|
||||
The path is parameterized by accumulated arc length in the model frame. C0
|
||||
samples model lateral position at 7 m, or max(7 m, speed × 1 s) when the existing
|
||||
C0 distance setting is enabled. C1 samples model orientation at max(7 m,
|
||||
speed × 1 s). A shorter path holds its endpoint; invalid paths disable the
|
||||
experimental command. Both distances are 7 m below about 15.7 mph. These are
|
||||
engineering choices, not identified Ford reference points.
|
||||
|
||||
C0 and C1 are independent base requests. C0 is no longer reconstructed as a
|
||||
circular arc from the curvature used for C1. C2/C3 remain zero, so this still
|
||||
does not transmit the model's entire curved path to the PSCM.
|
||||
|
||||
For the existing request limits and feedback, heading is divided by its sample
|
||||
distance to form a heading-equivalent curvature. controlsd applies its normal
|
||||
curvature limits to that value, logs it, and uses it for desired steering angle
|
||||
and feedback. Multiplication by the same distance recovers the model heading
|
||||
when unconstrained. C0 independently uses 2 × offset / distance² to apply the
|
||||
same request-limit function, then reverses that normalization. This does not
|
||||
infer C0 from C1. These envelopes constrain base requests; they are not proof of
|
||||
physical lateral acceleration or jerk under the unknown PSCM response.
|
||||
|
||||
P/I gains, measured-steering feedback, C1 anti-windup, C1-overflow allocation to
|
||||
C0, field bounds, driver arbitration, and CAN cadence retain the driven
|
||||
baseline behavior. There is no new C0/C1 actuator slew limiter. The independent
|
||||
C0 envelope adds one previous-reference state, reset with the controller.
|
||||
|
||||
This changes preview semantics: direct commands use the distance stations
|
||||
above, rather than the prior modeld curvature preview (about 0.744 s at low
|
||||
speed on these drives). The raw model points do not pass through the old
|
||||
curvature reference's 0.1 s smoother. Existing request limits still apply.
|
||||
|
||||
## Logging
|
||||
|
||||
Controller diagnostics identify `model-path-direct-feedback-v17`, with
|
||||
`direct_path=true` only when the model path is in control. A valid maneuver
|
||||
reference uses the original curvature-to-arc mapping and logs
|
||||
`direct_path=false`. `offset_reference` records the independently limited C0
|
||||
normalization. Desired curvature and desired steering angle describe the C1
|
||||
feedback target, not an independent measure of lateral path position.
|
||||
|
||||
modeld's geometry-reference telemetry is retained for comparison. Its
|
||||
`selectedCurvature` and published model action are not the direct controller's
|
||||
selected reference; use controlsState and the Ford controller diagnostics.
|
||||
|
||||
## Offline results
|
||||
|
||||
The replay compares the driven baseline against the direct-path candidate on
|
||||
identical recorded model frames and frozen vehicle measurements from full
|
||||
rlogs 15a and 15b. It covers 44,145 control cycles and 4,415 candidate CAN
|
||||
serialization checks. All commands remain finite and within field bounds;
|
||||
C2/C3 remain zero and driver/PSCM override behavior matches the baseline.
|
||||
The baseline reproduces recorded C1 to Float32 precision; C0 differs by at
|
||||
most one 0.01 m command quantum on 15b.
|
||||
|
||||
| Metric | 15a baseline → direct | 15b baseline → direct |
|
||||
|---|---:|---:|
|
||||
| C1 field-bound time | 2.57 → 6.77 s | 6.70 → 7.54 s |
|
||||
| Low-speed C0 steps > 0.25 m | 52 → 52 | 85 → 129 |
|
||||
| Low-speed C1 steps > 0.05 rad | 36 → 46 | 32 → 57 |
|
||||
|
||||
Low-speed counts use valid consecutive commands below 15 mph, normally 10 ms
|
||||
apart. They are descriptive command changes, not physical wheel jerk. The
|
||||
direct mapping changes requests substantially and does **not** establish a
|
||||
smoothness improvement: abrupt command changes and C1 field-bound time can
|
||||
increase. Fixed recorded measurements cannot predict the resulting wheel
|
||||
response, centering, or unwind. No on-road improvement is claimed.
|
||||
|
||||
**593 tests pass** (547 controller/path/geometry tests and 46 settings tests).
|
||||
The tests cover independent position/heading commands, distances, endpoint
|
||||
handling, invalid input resets, request limits through reversals, feedback,
|
||||
baseline driver overrides, toggle selection, maneuver priority, publication,
|
||||
and CAN encoding. Reproduce replay with:
|
||||
|
||||
```sh
|
||||
PYTHONPATH=.:opendbc_repo python tools/ford_pscm_lab/direct_model_path_replay.py \
|
||||
--source .cache/ford_route15a/rlog_full --output .cache/ford_direct_path/15a
|
||||
```
|
||||
|
||||
Detailed measurements and source hashes: `ford_direct_path_validation.json`.
|
||||
@@ -19,18 +19,23 @@
|
||||
"latched_angle_match_fraction": 1.0,
|
||||
"command_changes": {
|
||||
"c0": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.541800000000003,
|
||||
1.6400000000000006
|
||||
0.2799999999999998,
|
||||
1.3800000000000003,
|
||||
1.8000000000000007,
|
||||
2.51
|
||||
],
|
||||
"c1": [
|
||||
0.0,
|
||||
0.1825,
|
||||
0.20749999999999996,
|
||||
0.21100000000000008
|
||||
0.01749999999999996,
|
||||
0.124,
|
||||
0.2940450000000001,
|
||||
0.45200000000000007
|
||||
]
|
||||
},
|
||||
"reference_change_p50_p95_max": [
|
||||
0.0015834828704371431,
|
||||
0.012526763883651669,
|
||||
0.03905121179470369
|
||||
],
|
||||
"variants": {
|
||||
"old": {
|
||||
"feedback_switches_active": 112,
|
||||
@@ -41,25 +46,27 @@
|
||||
"c0": 0.4996000000000004,
|
||||
"c1": 0.05668000000000003
|
||||
},
|
||||
"c0_bound_active_s": 0.0,
|
||||
"c1_bound_active_s": 2.5702606670000137
|
||||
},
|
||||
"new": {
|
||||
"feedback_switches_active": 50,
|
||||
"feedback_switches_low_speed": 25,
|
||||
"low_speed_c0_steps_over_025m": 21,
|
||||
"low_speed_c1_steps_over_005rad": 18,
|
||||
"feedback_switches_active": 112,
|
||||
"feedback_switches_low_speed": 61,
|
||||
"low_speed_c0_steps_over_025m": 52,
|
||||
"low_speed_c1_steps_over_005rad": 46,
|
||||
"low_speed_step_p99": {
|
||||
"c0": 0.15480000000000044,
|
||||
"c1": 0.02622000000000008
|
||||
"c0": 0.8499999999999996,
|
||||
"c1": 0.08540000000000011
|
||||
},
|
||||
"c1_bound_active_s": 3.8107828889998814
|
||||
"c0_bound_active_s": 0.0,
|
||||
"c1_bound_active_s": 6.774772933000349
|
||||
}
|
||||
},
|
||||
"method": "Compare driver-input arbitration on recorded references and frozen vehicle motion.\n\nInput: extract.py route.npz/model_paths.npz/metadata.json directories. This checks\ncommand continuity and overrides; it does not predict a changed wheel response.\n",
|
||||
"method": "Compare direct model-path commands against the driven geometry-curvature controller.\n\nInput: extract.py route.npz/model_paths.npz/metadata.json directories. This checks\nreference selection, command continuity and overrides; it does not predict a changed wheel response.\n",
|
||||
"provenance": {
|
||||
"baseline_commit": "18ded0380045c29520cf1f690688e48ebad043d8",
|
||||
"baseline_controller_sha256": "8bc8554ed6743c5faf433deaa6de3f07fae27d183f62b04f49f9c3b9f1e8dcca",
|
||||
"candidate_controller_sha256": "c1132b826a37e6b600ef75683ab8f0a8c784c668472f69f08e217a0a91cca60a",
|
||||
"candidate_controller_sha256": "02b64f40a56cadefb181afbf7c3720affa3c7fe4ac18a2902dc9c546fd6b395d",
|
||||
"fixed_c0_distance_m": 7.0
|
||||
},
|
||||
"sources_sha256": {
|
||||
@@ -88,18 +95,23 @@
|
||||
"latched_angle_match_fraction": 0.9999321895978843,
|
||||
"command_changes": {
|
||||
"c0": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.2600000000000003,
|
||||
1.740000000000001
|
||||
0.040000000000000036,
|
||||
1.08,
|
||||
1.7699999999999996,
|
||||
2.46
|
||||
],
|
||||
"c1": [
|
||||
0.0020000000000000018,
|
||||
0.012999999999999994,
|
||||
0.07999999999999996,
|
||||
0.15400000000000003
|
||||
0.003500000000000003,
|
||||
0.12449999999999994,
|
||||
0.22634999999999764,
|
||||
0.406
|
||||
]
|
||||
},
|
||||
"reference_change_p50_p95_max": [
|
||||
0.00021085212911765025,
|
||||
0.013382994089416386,
|
||||
0.03945627628640698
|
||||
],
|
||||
"variants": {
|
||||
"old": {
|
||||
"feedback_switches_active": 238,
|
||||
@@ -110,25 +122,27 @@
|
||||
"c0": 0.36999999999999944,
|
||||
"c1": 0.03771999999999984
|
||||
},
|
||||
"c0_bound_active_s": 0.0,
|
||||
"c1_bound_active_s": 6.701652654999748
|
||||
},
|
||||
"new": {
|
||||
"feedback_switches_active": 82,
|
||||
"feedback_switches_low_speed": 40,
|
||||
"low_speed_c0_steps_over_025m": 39,
|
||||
"low_speed_c1_steps_over_005rad": 17,
|
||||
"feedback_switches_active": 238,
|
||||
"feedback_switches_low_speed": 107,
|
||||
"low_speed_c0_steps_over_025m": 129,
|
||||
"low_speed_c1_steps_over_005rad": 57,
|
||||
"low_speed_step_p99": {
|
||||
"c0": 0.22999999999999904,
|
||||
"c1": 0.03050000000000002
|
||||
"c0": 0.6187999999999921,
|
||||
"c1": 0.05121999999999982
|
||||
},
|
||||
"c1_bound_active_s": 7.123324506999893
|
||||
"c0_bound_active_s": 0.0,
|
||||
"c1_bound_active_s": 7.542629173000023
|
||||
}
|
||||
},
|
||||
"method": "Compare driver-input arbitration on recorded references and frozen vehicle motion.\n\nInput: extract.py route.npz/model_paths.npz/metadata.json directories. This checks\ncommand continuity and overrides; it does not predict a changed wheel response.\n",
|
||||
"method": "Compare direct model-path commands against the driven geometry-curvature controller.\n\nInput: extract.py route.npz/model_paths.npz/metadata.json directories. This checks\nreference selection, command continuity and overrides; it does not predict a changed wheel response.\n",
|
||||
"provenance": {
|
||||
"baseline_commit": "18ded0380045c29520cf1f690688e48ebad043d8",
|
||||
"baseline_controller_sha256": "8bc8554ed6743c5faf433deaa6de3f07fae27d183f62b04f49f9c3b9f1e8dcca",
|
||||
"candidate_controller_sha256": "c1132b826a37e6b600ef75683ab8f0a8c784c668472f69f08e217a0a91cca60a",
|
||||
"candidate_controller_sha256": "02b64f40a56cadefb181afbf7c3720affa3c7fe4ac18a2902dc9c546fd6b395d",
|
||||
"fixed_c0_distance_m": 7.0
|
||||
},
|
||||
"sources_sha256": {
|
||||
@@ -1,96 +0,0 @@
|
||||
# Ford feedback continuity trial
|
||||
|
||||
The v16 experimental controller uses Ford CarState's existing filtered
|
||||
`steeringPressed` signal to detect driver input. It no longer independently
|
||||
checks the same 1 Nm threshold on each raw torque sample. Fresh PSCM driver
|
||||
override (`limit == 3`) and nonfinite torque still immediately clear feedback.
|
||||
Filtered driver input also still clears both P and I; nothing changes in Ford's
|
||||
CarState filter or the vehicle's disengagement logic.
|
||||
|
||||
The full rlogs from `84865544361f55cb/0000015a--ef86556819` and
|
||||
`84865544361f55cb/0000015b--22b70521d4` contain short torque crossings with
|
||||
`steeringPressed == false`. At 15a +196.19 s, two samples reach 1.0625 Nm.
|
||||
The old duplicate check drops proportional correction and clears the integral,
|
||||
then restores proportional correction when torque falls. During the large left
|
||||
in 15b, similar pulses at +168.06, +168.14, and +168.28 s repeatedly interrupt
|
||||
correction. The logs establish these interruptions, not whether each pulse was
|
||||
deliberate driver input. v16 follows the same driver-input classification already
|
||||
used elsewhere in controls, instead of maintaining a second interpretation.
|
||||
|
||||
## Scope
|
||||
|
||||
This affects the opted-in Ford C0/C1 controller, with either action or geometry
|
||||
as its reference. Disabling `FordModelActionController` still selects upstream
|
||||
Ford control. No new toggle is needed. Diagnostics identify
|
||||
`model-action-curvature-c0-feedback-v16-filtered-driver`.
|
||||
|
||||
Reference selection, preview, smoothing, gains, integration, command bounds,
|
||||
PSCM limit handling, and message timing are unchanged. This is a feedback
|
||||
continuity change, not a wheel-speed limiter or a general cure for abrupt
|
||||
low-speed motion.
|
||||
|
||||
## Validation
|
||||
|
||||
The regression uses the actual shared `CarStateBase.update_steering_pressed`
|
||||
filter with Ford's count of 5. Two-sample pulses in either direction preserve
|
||||
feedback; sustained input clears it on the first filtered pressed sample.
|
||||
PSCM driver override remains immediate. Before the change, 4 of the 5 new
|
||||
tests failed; afterwards all 5 passed. Integration tests exercise publication
|
||||
through controlsd and the real CAN sender. The related controller, path, and
|
||||
geometry suites pass: **534 tests**.
|
||||
|
||||
The paired replay uses native control cadence and identical recorded
|
||||
references, steering, speed, driver state, and PSCM status. Baseline is commit
|
||||
`18ded0380045c29520cf1f690688e48ebad043d8`, with fixed 7 m C0. It replays
|
||||
44,145 control cycles and checks CAN serialization of 4,415 candidate commands.
|
||||
All commands remain finite and within field bounds; C2/C3 remain zero. Every
|
||||
recorded filtered driver input or fresh PSCM override clears feedback.
|
||||
|
||||
Counts below use consecutive valid active commands below 15 mph, with sample
|
||||
spacing below 30 ms (normally 10 ms). A "step" is an adjacent command change,
|
||||
not a measured wheel movement or an inferred comfort threshold.
|
||||
|
||||
| Metric | 15a old → new | 15b old → new | Combined old → new |
|
||||
|---|---:|---:|---:|
|
||||
| Feedback enabled/disabled transitions | 61 → 25 | 107 → 40 | 168 → 65 |
|
||||
| C0 steps larger than 0.25 m | 52 → 21 | 85 → 39 | 137 → 60 |
|
||||
| C1 steps larger than 0.05 rad | 36 → 18 | 32 → 17 | 68 → 35 |
|
||||
|
||||
The baseline matches recorded C1 within Float32 precision on both routes.
|
||||
Maximum C0 mismatch is Float32 rounding on 15a and one 0.01 m command quantum
|
||||
on 15b. Logged publication time approximates the core call time, and the replay
|
||||
cannot reproduce every retained-message service-health check. These limitations
|
||||
are reported with the replay results, rather than assuming exact reproduction.
|
||||
|
||||
Preserving I can also sustain stronger commands. Time at the C1 field bound
|
||||
increases from **2.57 to 3.81 s** on 15a and **6.70 to 7.12 s** on 15b.
|
||||
The candidate's wheel response and unwind cannot be inferred from the recorded
|
||||
motion: the physical vehicle did not receive these candidate commands.
|
||||
|
||||
Reproduce for each extracted route directory:
|
||||
|
||||
```sh
|
||||
PYTHONPATH=.:opendbc_repo python tools/ford_pscm_lab/filtered_driver_replay.py \
|
||||
--source .cache/ford_route15a/rlog_full \
|
||||
--output .cache/ford_filtered_driver/15a
|
||||
```
|
||||
|
||||
Input and controller hashes and detailed results are in
|
||||
`ford_filtered_driver_validation.json`.
|
||||
|
||||
## Why not add preview at the same time?
|
||||
|
||||
The geometry and learned action are separate model outputs; code does not
|
||||
require them to agree. Geometry converts previewed heading and initial heading
|
||||
rate to curvature. For fixed heading perturbations, sensitivity grows as speed
|
||||
falls. This can magnify frame-to-frame plan revisions at low speed; it does not
|
||||
prove the resulting plan is incorrect. The model's training objectives cannot
|
||||
be established from inference code alone.
|
||||
|
||||
A separate frozen-plan check added 0.1 or 0.2 s of geometry preview. Selected
|
||||
turn exits relaxed earlier, but some low-speed reference changes got steeper.
|
||||
For example, on 15b the maximum target-angle change over 0.2 s rose from about
|
||||
352 degrees/s to 386 and 429 degrees/s. These are reference changes, not wheel
|
||||
rates. Extra preview is therefore not included in this feedback continuity
|
||||
trial. Remaining sharp requests and geometric exit rebounds need to be judged
|
||||
separately from command interruptions.
|
||||
@@ -1,5 +1,10 @@
|
||||
# Ford model geometry reference trial
|
||||
|
||||
This documents the initial curvature-reference trial at `18ded0380`. The
|
||||
subsequent [direct model-path trial](ford_direct_path.md) reuses the geometry
|
||||
toggle and changes controlsd's reference and C0/C1 mapping. modeld telemetry
|
||||
described here remains available for comparison.
|
||||
|
||||
`FordGeometryReference` changes the steering reference used by the existing
|
||||
Ford C0/C1 feedback controller. It is default off and requires
|
||||
`FordModelActionController` and a Ford CAN FD vehicle.
|
||||
@@ -50,10 +55,7 @@ the toggle is read at modeld startup.
|
||||
publication time, original action curvature, raw geometric curvature, selected
|
||||
curvature, preview, and smoothing time. `valid=false` while enabled identifies
|
||||
fallback. A startup log also identifies action or geometry mode. The existing
|
||||
Ford diagnostics identified the unchanged v15 feedback controller in the initial
|
||||
geometry trial. The subsequent [v16 feedback continuity trial](ford_filtered_driver.md)
|
||||
uses Ford's filtered driver-input signal and leaves this reference calculation
|
||||
unchanged.
|
||||
Ford diagnostics continue to identify the unchanged v15 feedback controller.
|
||||
|
||||
## Offline validation
|
||||
|
||||
|
||||
@@ -56,7 +56,8 @@ class Controls(ControlsExt):
|
||||
self.curvature = 0.0
|
||||
self.desired_curvature = 0.0
|
||||
self.ford_path_controller = select_model_action_controller(self.CP, self.params.get_bool("FordModelActionController"),
|
||||
c0_time_based=self.params.get_bool("FordC0TimeBased"))
|
||||
c0_time_based=self.params.get_bool("FordC0TimeBased"),
|
||||
direct_path=self.params.get_bool("FordGeometryReference"))
|
||||
self.ford_model_action = isinstance(self.ford_path_controller, FordModelActionController)
|
||||
if self.CP.brand == "ford":
|
||||
cloudlog.event("Ford path controller selected",
|
||||
@@ -151,6 +152,8 @@ class Controls(ControlsExt):
|
||||
# Reset desired curvature to current to avoid violating the limits on engage
|
||||
if self.sm.valid['lateralManeuverPlan']:
|
||||
new_desired_curvature = self.sm['lateralManeuverPlan'].desiredCurvature if CC.latActive else self.curvature
|
||||
elif self.ford_model_action and self.ford_path_controller.direct_path:
|
||||
new_desired_curvature = self.ford_path_controller.path_curvature(model_v2, CS.vEgo) if CC.latActive else self.curvature
|
||||
else:
|
||||
new_desired_curvature = model_v2.action.desiredCurvature if CC.latActive else self.curvature
|
||||
self.desired_curvature, curvature_limited = clip_curvature(CS.vEgo, self.desired_curvature, new_desired_curvature, lp.roll)
|
||||
@@ -178,6 +181,7 @@ class Controls(ControlsExt):
|
||||
reference_time=self.sm.logMonoTime[reference_service] * 1e-9,
|
||||
active=CC.latActive, valid=CS.canValid and self.sm.all_checks(['carState', 'vehicleParameters', 'modelV2', reference_service]),
|
||||
driver_pressed=CS.steeringPressed, driver_torque=CS.steeringTorque,
|
||||
reference_source=reference_service, roll=lp.roll,
|
||||
pscm_status=self.sm['carStateSP'].fordPscmStatus if self.sm.valid['carStateSP'] else None,
|
||||
)
|
||||
if not self.ford_path.valid:
|
||||
|
||||
@@ -5,13 +5,16 @@ including base-heading overflow, plus proportional tracking correction. C1
|
||||
combines the selected curvature's heading with proportional and integrated
|
||||
tracking error. Reference distance and gains are explicit trial choices.
|
||||
Commands use the current bounded request without an additional C0/C1 slew.
|
||||
The direct-path trial samples model position and heading separately, retains
|
||||
independent upstream request limits, and uses heading-equivalent feedback.
|
||||
"""
|
||||
import math
|
||||
import struct
|
||||
|
||||
import numpy as np
|
||||
|
||||
from opendbc.car.ford.values import FordFlags
|
||||
from opendbc.car.ford.values import CarControllerParams, FordFlags
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
|
||||
from openpilot.selfdrive.controls.lib.ford_path import FordPath, _model_path
|
||||
|
||||
|
||||
@@ -62,13 +65,34 @@ def encode_model_action(model, desired_curvature, speed, *, c0_time_based=False)
|
||||
return FordPath(True, c0, c1, 0., 0.) if _finite(c0, c1) else FordPath()
|
||||
|
||||
|
||||
def encode_model_path(model, speed, *, c0_time_based=False):
|
||||
"""Sample lateral position and heading independently in the model's frame.
|
||||
|
||||
Preserve the existing distance choices and hold a short path's endpoint.
|
||||
These preview points are trial choices, not identified Ford reference points.
|
||||
"""
|
||||
if not _finite(speed) or not .3 <= speed <= 55:
|
||||
return FordPath()
|
||||
try:
|
||||
path = _model_path(model)
|
||||
except OverflowError:
|
||||
return FordPath()
|
||||
if path is None or not all(_finite(*values) for values in path):
|
||||
return FordPath()
|
||||
distance, _, lateral, heading = path
|
||||
heading_distance = max(OFFSET_STATION_M, speed*HEADING_TIME_S)
|
||||
offset_distance = heading_distance if c0_time_based else OFFSET_STATION_M
|
||||
return FordPath(True, float(np.interp(offset_distance, distance, lateral)),
|
||||
float(np.interp(heading_distance, distance, heading)), 0., 0.)
|
||||
|
||||
|
||||
class ModelActionController:
|
||||
"""Integrated tracking error is the only accumulated correction.
|
||||
|
||||
Freshness, measurement cadence and driver/PSCM arbitration belong to the caller.
|
||||
"""
|
||||
__slots__ = ('c0', 'c1', 'correction', 'proportional_gain', 'integral_gain', 'proportional', 'feedback_curvature', 'c0_time_based',
|
||||
'c0_proportional_gain', 'offset_proportional')
|
||||
'c0_proportional_gain', 'offset_proportional', 'offset_reference')
|
||||
|
||||
def __init__(self, proportional_gain=C1_PROPORTIONAL_GAIN, integral_gain=C1_INTEGRAL_GAIN, *, c0_time_based=False,
|
||||
c0_proportional_gain=C0_PROPORTIONAL_GAIN):
|
||||
@@ -82,22 +106,37 @@ class ModelActionController:
|
||||
def reset(self):
|
||||
self.c0 = self.c1 = self.correction = self.proportional = self.feedback_curvature = 0.
|
||||
self.offset_proportional = 0.
|
||||
self.offset_reference = None
|
||||
|
||||
def update(self, model, desired_curvature, *, current_curvature, speed, dt, active=True, valid=True,
|
||||
feedback_dt=None, feedback_enabled=True, pscm_limited=False, feedback_curvature=None, curvature_scale=1.):
|
||||
feedback_dt=None, feedback_enabled=True, pscm_limited=False, feedback_curvature=None, curvature_scale=1.,
|
||||
direct_path=False, roll=0.):
|
||||
feedback_dt = dt if feedback_dt is None else feedback_dt
|
||||
reference = desired_curvature if feedback_curvature is None else feedback_curvature
|
||||
if (not active or not valid or not _finite(dt, feedback_dt, current_curvature, reference, curvature_scale) or not .002 <= dt <= .1
|
||||
or not 0. <= feedback_dt <= .15 or abs(current_curvature) > 1. or abs(reference) > 1.):
|
||||
if (not active or not valid or not _finite(dt, feedback_dt, desired_curvature, current_curvature, reference, curvature_scale, roll)
|
||||
or not .002 <= dt <= .1 or not 0. <= feedback_dt <= .15
|
||||
or abs(desired_curvature) > 1. or abs(current_curvature) > 1. or abs(reference) > 1.):
|
||||
self.reset()
|
||||
return FordPath()
|
||||
if curvature_scale <= 0.:
|
||||
self.reset()
|
||||
return FordPath()
|
||||
target = encode_model_action(model, desired_curvature, speed, c0_time_based=self.c0_time_based)
|
||||
target = (encode_model_path(model, speed, c0_time_based=self.c0_time_based) if direct_path else
|
||||
encode_model_action(model, desired_curvature, speed, c0_time_based=self.c0_time_based))
|
||||
if not target.valid:
|
||||
self.reset()
|
||||
return FordPath()
|
||||
if direct_path:
|
||||
# Limit the channels independently, without rebuilding C0 from C1.
|
||||
# Normalizing C0 by distance only applies the existing curvature envelope;
|
||||
# when unconstrained, the output is exactly the sampled model offset.
|
||||
distance = max(OFFSET_STATION_M, speed*HEADING_TIME_S) if self.c0_time_based else OFFSET_STATION_M
|
||||
previous = current_curvature if self.offset_reference is None else self.offset_reference
|
||||
self.offset_reference, _ = clip_curvature(speed, previous, 2.*target.path_offset/distance**2, roll)
|
||||
target = FordPath(True, .5*self.offset_reference*distance**2,
|
||||
max(OFFSET_STATION_M, speed*HEADING_TIME_S)*desired_curvature, 0., 0.)
|
||||
else:
|
||||
self.offset_reference = None
|
||||
self.feedback_curvature = reference
|
||||
error = reference-current_curvature
|
||||
self.proportional = self.proportional_gain*max(OFFSET_STATION_M, speed*HEADING_TIME_S)*error if feedback_enabled else 0.
|
||||
@@ -140,18 +179,23 @@ class FordModelActionController:
|
||||
Feedback advances once per fresh steering measurement; repeated samples
|
||||
still use the current request. Raw model geometry is checked on every cycle.
|
||||
|
||||
CAN yaw remains a health gate, not the feedback measurement. Ford's filtered
|
||||
steeringPressed and fresh PSCM driver overrides clear the correction.
|
||||
Fresh PSCM limits only inhibit outward integration;
|
||||
CAN yaw remains a health gate, not the feedback measurement. Driver override
|
||||
clears the correction. Fresh PSCM limits only inhibit outward integration;
|
||||
neither a limit nor a repeated measurement freezes the model request.
|
||||
"""
|
||||
def __init__(self, proportional_gain=C1_PROPORTIONAL_GAIN, integral_gain=C1_INTEGRAL_GAIN, *, c0_time_based=False,
|
||||
c0_proportional_gain=C0_PROPORTIONAL_GAIN):
|
||||
c0_proportional_gain=C0_PROPORTIONAL_GAIN, direct_path=False):
|
||||
self.core = ModelActionController(proportional_gain=proportional_gain, integral_gain=integral_gain, c0_time_based=c0_time_based,
|
||||
c0_proportional_gain=c0_proportional_gain)
|
||||
self.hypothesis = 'model-action-curvature-c0-feedback-v16-filtered-driver'
|
||||
self.direct_path = bool(direct_path)
|
||||
self.hypothesis = 'model-path-direct-feedback-v17' if self.direct_path else 'model-action-curvature-c0-feedback-v15'
|
||||
self.reset()
|
||||
|
||||
def path_curvature(self, model, speed):
|
||||
"""Heading-equivalent feedback target; controlsd limits and logs this value."""
|
||||
target = encode_model_path(model, speed, c0_time_based=self.core.c0_time_based)
|
||||
return target.path_angle/max(OFFSET_STATION_M, speed*HEADING_TIME_S) if target.valid else 0.
|
||||
|
||||
def set_c0_time_based(self, enabled, *, lateral_engaged):
|
||||
"""Apply a distance change only after lateral assistance is disengaged."""
|
||||
if lateral_engaged or self.core.c0_time_based == bool(enabled):
|
||||
@@ -169,7 +213,7 @@ class FordModelActionController:
|
||||
|
||||
def update(self, model, desired_curvature, *, current_curvature, yaw_rate, speed, now, measurement_time, model_time,
|
||||
reference_time, active, valid=True, driver_pressed=False, driver_torque=0., pscm_status=None,
|
||||
feedback_curvature=None, curvature_scale=1.):
|
||||
feedback_curvature=None, curvature_scale=1., reference_source='modelV2', roll=0.):
|
||||
reason = None
|
||||
if not active:
|
||||
reason = 'inactive'
|
||||
@@ -195,14 +239,14 @@ class FordModelActionController:
|
||||
status_fresh = (pscm_status is not None and pscm_status.valid and pscm_status.canMonoTime > 0
|
||||
and -.005 <= now-pscm_status.canMonoTime*1e-9 <= .15)
|
||||
pscm_limited = bool(status_fresh and pscm_status.limit == 2)
|
||||
# CarState already filters Ford's noisy torque signal into steeringPressed.
|
||||
# Rechecking its raw threshold here bypasses that filter and chatters P/I.
|
||||
driver_override = bool(driver_pressed or not _finite(driver_torque)
|
||||
or abs(driver_torque) > CarControllerParams.STEER_DRIVER_ALLOWANCE
|
||||
or (status_fresh and pscm_status.limit == 3))
|
||||
feedback_enabled = not (driver_override or (status_fresh and (pscm_status.denied or pscm_status.lateralState != 2)))
|
||||
direct_path = self.direct_path and reference_source == 'modelV2'
|
||||
command = self.core.update(model, desired_curvature, current_curvature=current_curvature, speed=speed, dt=dt,
|
||||
feedback_dt=feedback_dt, feedback_enabled=feedback_enabled, pscm_limited=pscm_limited,
|
||||
feedback_curvature=feedback_curvature, curvature_scale=curvature_scale)
|
||||
feedback_curvature=feedback_curvature, curvature_scale=curvature_scale, direct_path=direct_path, roll=roll)
|
||||
if not command.valid:
|
||||
self.reset('invalid_path')
|
||||
return command
|
||||
@@ -210,6 +254,7 @@ class FordModelActionController:
|
||||
raw_heading = max(OFFSET_STATION_M, speed*HEADING_TIME_S)*desired_curvature
|
||||
base_heading = float(np.clip(raw_heading, -.5, .5))
|
||||
self.diagnostics = {'status': 'active', 'hypothesis': self.hypothesis,
|
||||
'direct_path': direct_path, 'offset_reference': self.core.offset_reference,
|
||||
'c0_time_based': self.core.c0_time_based,
|
||||
'offset_distance': max(OFFSET_STATION_M, speed*HEADING_TIME_S) if self.core.c0_time_based else OFFSET_STATION_M,
|
||||
'calibration_approved': CALIBRATION_APPROVED, 'desired_curvature': desired_curvature,
|
||||
@@ -229,9 +274,10 @@ class FordModelActionController:
|
||||
return command
|
||||
|
||||
|
||||
def select_model_action_controller(CP, enabled, *, c0_time_based=False):
|
||||
def select_model_action_controller(CP, enabled, *, c0_time_based=False, direct_path=False):
|
||||
"""Only opt-in Ford CAN FD vehicles override upstream curvature control."""
|
||||
compatible = CP.brand == 'ford' and CP.flags & FordFlags.CANFD
|
||||
if enabled and compatible:
|
||||
return FordModelActionController(proportional_gain=C1_PROPORTIONAL_GAIN, integral_gain=C1_INTEGRAL_GAIN, c0_time_based=c0_time_based)
|
||||
return FordModelActionController(proportional_gain=C1_PROPORTIONAL_GAIN, integral_gain=C1_INTEGRAL_GAIN,
|
||||
c0_time_based=c0_time_based, direct_path=direct_path)
|
||||
return None
|
||||
|
||||
@@ -0,0 +1,131 @@
|
||||
"""Direct-path reference selection, independent channels, feedback and real CAN encoding."""
|
||||
from types import SimpleNamespace
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from opendbc.car import structs
|
||||
from openpilot.cereal import custom
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
|
||||
from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController, encode_model_path
|
||||
from openpilot.selfdrive.controls.lib.ford_path import FordPath
|
||||
from openpilot.selfdrive.controls.tests.test_ford_model_action import circle, make_model, straight
|
||||
from openpilot.selfdrive.controls.tests.test_ford_model_action_adapter import Subscriptions, pipeline # noqa: F401
|
||||
from openpilot.selfdrive.controls.tests.test_ford_model_action_selection import startup
|
||||
from tools.ford_pscm_lab.model_action_replay import WireCheck
|
||||
|
||||
|
||||
def step(controller, model, now, previous, *, measured=0., speed=5., **kwargs):
|
||||
desired, _ = clip_curvature(speed, previous, controller.path_curvature(model, speed), 0.)
|
||||
result = controller.update(model, desired, current_curvature=measured, speed=speed, yaw_rate=0., now=now,
|
||||
measurement_time=now, model_time=now, reference_time=now, active=True, **kwargs)
|
||||
return desired, result
|
||||
|
||||
|
||||
@pytest.mark.parametrize('speed', [2., 5., 15., 30.])
|
||||
@pytest.mark.parametrize('time_based', [False, True])
|
||||
def test_position_and_heading_sample_existing_distances_independently(speed, time_based):
|
||||
x = np.linspace(0, 80, 801)
|
||||
model = make_model(x, x*.03+.2, x*.002)
|
||||
target = encode_model_path(model, speed, c0_time_based=time_based)
|
||||
# Arc-length station differs from forward x on this sloping path.
|
||||
d0, d1 = (max(7., speed) if time_based else 7.), max(7., speed)
|
||||
assert target.path_offset == pytest.approx(.2+.03*d0/np.hypot(1., .03))
|
||||
assert target.path_angle == pytest.approx(.002*d1/np.hypot(1., .03))
|
||||
assert target.curvature == target.curvature_rate == 0.
|
||||
|
||||
|
||||
def test_short_path_holds_endpoint_and_bad_path_never_leaves_old_commands():
|
||||
model = make_model([0., 1.], [0., .2], [0., .03])
|
||||
target = encode_model_path(model, 20.)
|
||||
assert target.valid and target.path_offset == pytest.approx(.2) and target.path_angle == pytest.approx(.03)
|
||||
controller = FordModelActionController(direct_path=True)
|
||||
desired, command = step(controller, model, 1., 0.)
|
||||
assert command.valid
|
||||
model.position.y[1] = np.nan
|
||||
assert step(controller, model, 1.01, desired)[1] == FordPath()
|
||||
assert controller.core.offset_reference is None
|
||||
assert controller.core.correction == 0.
|
||||
|
||||
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
def test_lateral_position_changes_c0_without_inventing_heading(sign):
|
||||
controller = FordModelActionController(0., 0., c0_proportional_gain=0., direct_path=True)
|
||||
model = straight(sign*.4)
|
||||
desired = 0.
|
||||
for i in range(100):
|
||||
desired, command = step(controller, model, 1.+i*.01, desired)
|
||||
assert desired == 0.
|
||||
assert command.path_offset == pytest.approx(sign*.4)
|
||||
assert command.path_angle == 0.
|
||||
|
||||
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
def test_heading_changes_c1_without_rebuilding_c0_as_a_circle(sign):
|
||||
controller = FordModelActionController(0., 0., c0_proportional_gain=0., direct_path=True)
|
||||
model = straight()
|
||||
model.orientation.z = np.full(len(model.position.x), sign*.07)
|
||||
desired = 0.
|
||||
for i in range(100):
|
||||
desired, command = step(controller, model, 1.+i*.01, desired)
|
||||
assert desired == pytest.approx(sign*.01)
|
||||
assert command.path_angle == pytest.approx(sign*.07)
|
||||
assert command.path_offset == 0.
|
||||
|
||||
|
||||
def test_both_base_channels_retain_upstream_request_limits_on_entry_reversal_and_release():
|
||||
controller = FordModelActionController(0., 0., c0_proportional_gain=0., direct_path=True)
|
||||
desired = offset = 0.
|
||||
for i in range(300):
|
||||
sign = 1 if i < 100 else -1 if i < 200 else 0
|
||||
model = straight(sign*5.)
|
||||
model.orientation.z = np.full(len(model.position.x), sign*.4)
|
||||
requested_offset = 2.*sign*5./49.
|
||||
offset, _ = clip_curvature(20., offset, requested_offset, 0.)
|
||||
desired, command = step(controller, model, 1.+i*.01, desired, speed=20.)
|
||||
assert controller.core.offset_reference == pytest.approx(offset)
|
||||
assert abs(desired) <= 3./20.**2
|
||||
assert command.path_offset == pytest.approx(.5*offset*49., abs=.0051)
|
||||
assert command.path_angle == pytest.approx(desired*20., abs=.000251)
|
||||
assert abs(command.path_offset) < .01 and abs(command.path_angle) < .001
|
||||
|
||||
|
||||
def test_feedback_tracks_path_heading_and_raw_torque_override_is_still_the_driven_baseline():
|
||||
controller = FordModelActionController(direct_path=True)
|
||||
model = circle(.01)
|
||||
desired = 0.
|
||||
for i in range(100):
|
||||
desired, _ = step(controller, model, 1.+i*.01, desired, measured=.005)
|
||||
assert controller.core.proportional > 0. and controller.core.correction > 0.
|
||||
retained = controller.core.correction
|
||||
desired, _ = step(controller, model, 2., desired, measured=desired)
|
||||
assert controller.core.proportional == pytest.approx(0.)
|
||||
assert controller.core.correction == pytest.approx(retained)
|
||||
step(controller, model, 2.01, desired, driver_torque=1.0625)
|
||||
assert controller.core.correction == controller.core.proportional == controller.core.offset_proportional == 0.
|
||||
|
||||
|
||||
@pytest.mark.parametrize('geometry', [False, True])
|
||||
@pytest.mark.parametrize('maneuver', [False, True])
|
||||
def test_actual_controlsd_path_selection_feedback_logging_publication_and_can(pipeline, geometry, maneuver): # noqa: F811
|
||||
settings = {'FordModelActionController': True, 'FordGeometryReference': geometry}
|
||||
controls = startup(params=SimpleNamespace(get_bool=lambda key: settings.get(key, False)))
|
||||
controls.sm, controls.desired_curvature, controls.curvature = Subscriptions(maneuver), 0., 0.
|
||||
assert controls.ford_path_controller.direct_path == geometry
|
||||
model = circle(.01)
|
||||
model.action = SimpleNamespace(desiredCurvature=-.03) # Deliberately opposite to the model path.
|
||||
cc = structs.CarControl(latActive=True)
|
||||
cs = SimpleNamespace(vEgo=5., yawRate=0., canValid=True, steeringPressed=False, steeringTorque=0.)
|
||||
exec(pipeline[0], {'self': controls, 'CS': cs, 'CC': cc, 'actuators': cc.actuators, 'model_v2': model,
|
||||
'lp': SimpleNamespace(roll=0.), 'clip_curvature': clip_curvature, 'time': SimpleNamespace(monotonic=lambda: 1.)})
|
||||
direct = geometry and not maneuver
|
||||
expected = .002 if direct else -.002
|
||||
assert controls.desired_curvature == pytest.approx(expected)
|
||||
assert controls.ford_path_controller.core.feedback_curvature == controls.desired_curvature
|
||||
assert controls.ford_path_controller.diagnostics['direct_path'] == direct
|
||||
assert controls.ford_path.path_angle*expected > 0.
|
||||
msg = custom.CarControlSP.new_message()
|
||||
exec(pipeline[1], {'self': controls, 'CC_SP': msg})
|
||||
assert msg.fordLateralPath.pathOffset == pytest.approx(controls.ford_path.path_offset)
|
||||
assert msg.fordLateralPath.pathAngle == pytest.approx(controls.ford_path.path_angle)
|
||||
WireCheck().check(controls.ford_path)
|
||||
@@ -17,7 +17,6 @@ from opendbc.car import Bus, structs
|
||||
from opendbc.car.ford.carcontroller import CarController
|
||||
from opendbc.car.ford.fordcan import calculate_lat_ctl2_checksum
|
||||
from opendbc.car.ford.values import CAR, CarControllerParams, FordFlags
|
||||
from opendbc.car.interfaces import CarStateBase
|
||||
from openpilot.cereal import custom
|
||||
from openpilot.selfdrive.car.helpers import convert_carControlSP
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
|
||||
@@ -243,7 +242,6 @@ def test_feedback_through_actual_controlsd_publication_and_100hz_sender(pipeline
|
||||
model.action = SimpleNamespace(desiredCurvature=sign*.004)
|
||||
cc = structs.CarControl(latActive=True)
|
||||
cs = SimpleNamespace(vEgo=20., yawRate=.2, canValid=True, steeringPressed=False, steeringTorque=0.)
|
||||
driver_filter = SimpleNamespace(steering_pressed_cnt=0)
|
||||
cp = structs.CarParams(flags=int(FordFlags.CANFD), carFingerprint='FORD_F_150_LIGHTNING_MK1')
|
||||
downstream = CarController({Bus.pt: 'ford_lincoln_base_pt'}, cp, structs.CarParamsSP())
|
||||
vehicle = SimpleNamespace(out=structs.CarState(vEgo=20., vEgoRaw=20.), acc_tja_status_stock_values=defaultdict(int),
|
||||
@@ -253,13 +251,11 @@ def test_feedback_through_actual_controlsd_publication_and_100hz_sender(pipeline
|
||||
# Every fresh error sample integrates within amplitude headroom.
|
||||
# Matched steering removes P and preserves I.
|
||||
for measured, torque, count, expected in [(sign*.004, 0., 100, 0.), (sign*.003, 0., 100, sign*.02),
|
||||
(sign*.004, 0., 100, sign*.02), (sign*.004, 1.0625, 2, sign*.02),
|
||||
(sign*.005, 0., 100, 0.), (sign*.003, 0., 100, sign*.02), (0., 1.0625, 12, 0.)]:
|
||||
(sign*.004, 0., 100, sign*.02), (sign*.005, 0., 100, 0.),
|
||||
(sign*.003, 0., 100, sign*.02), (0., 1.0625, 5, 0.)]:
|
||||
for _ in range(count):
|
||||
now = 1.+frame*.01
|
||||
controls.curvature, cs.steeringTorque = measured, torque
|
||||
cs.steeringPressed = CarStateBase.update_steering_pressed(
|
||||
driver_filter, abs(torque) > CarControllerParams.STEER_DRIVER_ALLOWANCE, 5)
|
||||
sm.logMonoTime.update(carState=round(now*1e9), modelV2=round(now*1e9))
|
||||
environment = {'self': controls, 'CS': cs, 'CC': cc, 'actuators': cc.actuators, 'model_v2': model,
|
||||
'lp': SimpleNamespace(roll=0.), 'clip_curvature': clip_curvature,
|
||||
@@ -281,14 +277,14 @@ def test_feedback_through_actual_controlsd_publication_and_100hz_sender(pipeline
|
||||
assert wire['LatCtlPath_No_Cs'] == calculate_lat_ctl2_checksum(2, frame % 16, packet[1])
|
||||
frame += 1
|
||||
core = controls.ford_path_controller.core
|
||||
expected_p = .75*20.*(sign*.004-measured) if not cs.steeringPressed else 0.
|
||||
expected_p = .75*20.*(sign*.004-measured) if torque == 0. else 0.
|
||||
assert core.proportional == pytest.approx(expected_p)
|
||||
assert core.correction == pytest.approx(expected)
|
||||
assert core.c1 == pytest.approx(sign*.08+expected_p+expected)
|
||||
assert controls.ford_path.path_angle == pytest.approx(core.c1, abs=.00025)
|
||||
base = encode_model_action(straight(), sign*.004, cs.vEgo).path_offset
|
||||
assert controls.ford_path.path_offset == pytest.approx(base+core.offset_proportional, abs=.005)
|
||||
assert (core.offset_proportional == 0.) == (cs.steeringPressed or measured == sign*.004)
|
||||
assert (core.offset_proportional == 0.) == (torque != 0. or measured == sign*.004)
|
||||
|
||||
|
||||
@pytest.mark.parametrize('service_valid', [False, True])
|
||||
@@ -363,7 +359,7 @@ def test_continuous_pi_reversal_through_selected_limited_request_and_actual_can(
|
||||
assert wire['LatCtlPath_No_Cs'] == calculate_lat_ctl2_checksum(2, frame % 16, packet[1])
|
||||
if frame == 199:
|
||||
assert sign*core.correction < 0. if same_turn else sign*core.correction > 0.
|
||||
assert controls.ford_path_controller.diagnostics['hypothesis'] == 'model-action-curvature-c0-feedback-v16-filtered-driver'
|
||||
assert controls.ford_path_controller.diagnostics['hypothesis'] == 'model-action-curvature-c0-feedback-v15'
|
||||
if same_turn:
|
||||
assert controls.desired_curvature == pytest.approx(sign*.01)
|
||||
assert sign*controls.ford_path.path_angle >= speed*.01 # No old unwind correction left below the new base.
|
||||
|
||||
@@ -1,61 +0,0 @@
|
||||
"""Exercise Ford's actual driver-input filter together with path feedback."""
|
||||
from types import SimpleNamespace
|
||||
|
||||
import pytest
|
||||
|
||||
from opendbc.car.ford.values import CarControllerParams
|
||||
from opendbc.car.interfaces import CarStateBase
|
||||
from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController
|
||||
from openpilot.selfdrive.controls.tests.test_ford_model_action_feedback import adapter_tick, status
|
||||
|
||||
|
||||
def pressed(state, torque):
|
||||
# Ford CarState.update uses this shared filter with minimum count 5.
|
||||
return CarStateBase.update_steering_pressed(state, abs(torque) > CarControllerParams.STEER_DRIVER_ALLOWANCE, 5)
|
||||
|
||||
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
def test_twenty_ms_torque_spike_does_not_drop_feedback_before_ford_detects_driver_input(sign):
|
||||
state = SimpleNamespace(steering_pressed_cnt=0)
|
||||
controller, steady = FordModelActionController(), FordModelActionController()
|
||||
# Route 15a at 196.19 s: two 1.0625 Nm samples, steeringPressed remained false.
|
||||
torques = [0.]*50+[sign*1.0625]*2+[0.]*20
|
||||
for i, torque in enumerate(torques):
|
||||
now = 1.+i*.01
|
||||
driver = pressed(state, torque)
|
||||
assert not driver
|
||||
actual = adapter_tick(controller, now, speed=3.2, driver_pressed=driver, driver_torque=torque)
|
||||
expected = adapter_tick(steady, now, speed=3.2)
|
||||
assert actual == expected
|
||||
assert controller.core.correction == steady.core.correction
|
||||
assert controller.diagnostics['feedback_enabled']
|
||||
assert controller.core.correction > 0.
|
||||
|
||||
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
def test_sustained_driver_input_clears_feedback_on_the_first_filtered_pressed_sample(sign):
|
||||
state = SimpleNamespace(steering_pressed_cnt=0)
|
||||
controller = FordModelActionController()
|
||||
for i in range(50):
|
||||
adapter_tick(controller, 1.+i*.01, speed=3.2)
|
||||
assert controller.core.correction > 0.
|
||||
detected = False
|
||||
for i in range(12):
|
||||
torque = sign*1.0625
|
||||
driver = pressed(state, torque)
|
||||
adapter_tick(controller, 1.5+i*.01, speed=3.2, driver_pressed=driver, driver_torque=torque)
|
||||
assert controller.diagnostics['feedback_enabled'] == (not driver)
|
||||
if driver:
|
||||
detected = True
|
||||
assert controller.core.correction == controller.core.proportional == controller.core.offset_proportional == 0.
|
||||
assert detected
|
||||
|
||||
|
||||
def test_pscm_override_remains_immediate_before_driver_filter_triggers():
|
||||
controller = FordModelActionController()
|
||||
for i in range(50):
|
||||
adapter_tick(controller, 1.+i*.01, speed=3.2)
|
||||
adapter_tick(controller, 1.5, speed=3.2, driver_pressed=False, driver_torque=1.0625,
|
||||
pscm_status=status(1.5, limit=3))
|
||||
assert not controller.diagnostics['feedback_enabled']
|
||||
assert controller.core.correction == controller.core.proportional == controller.core.offset_proportional == 0.
|
||||
@@ -139,8 +139,8 @@ def test_repeated_steering_samples_do_not_reintegrate_error():
|
||||
assert controller.diagnostics['feedback_dt'] == pytest.approx(.06)
|
||||
|
||||
|
||||
@pytest.mark.parametrize('overrides', [{'driver_pressed': True}, {'driver_torque': math.nan},
|
||||
{'driver_torque': math.inf}, {'driver_torque': -math.inf}, {'driver_torque': None},
|
||||
@pytest.mark.parametrize('overrides', [{'driver_pressed': True}, {'driver_torque': 1.01},
|
||||
{'driver_torque': -1.01}, {'driver_torque': math.nan},
|
||||
{'pscm_status': status(2.01, limit=3)},
|
||||
{'pscm_status': status(2.01, denied=True)},
|
||||
{'pscm_status': status(2.01, lateralState=1)}])
|
||||
|
||||
@@ -53,7 +53,7 @@ def test_actual_startup_priority(candidate, observer, fingerprint):
|
||||
assert selected.ford_path_controller.core.proportional_gain == C1_PROPORTIONAL_GAIN == .75
|
||||
assert selected.ford_path_controller.core.integral_gain == C1_INTEGRAL_GAIN == 1.
|
||||
assert selected.ford_path_controller.core.c0_proportional_gain == .5
|
||||
assert selected.ford_path_controller.diagnostics['hypothesis'] == 'model-action-curvature-c0-feedback-v16-filtered-driver'
|
||||
assert selected.ford_path_controller.diagnostics['hypothesis'] == 'model-action-curvature-c0-feedback-v15'
|
||||
else:
|
||||
assert selected.ford_path_controller is None
|
||||
assert selected.ford_model_action == candidate
|
||||
|
||||
@@ -2183,8 +2183,8 @@
|
||||
"widget": "toggle",
|
||||
"needs_onroad_cycle": true,
|
||||
"title": "Selected-Action Path Tracking (Experimental)",
|
||||
"description": "Follow the selected desired curvature using path-offset and heading commands with measured steering feedback on any Ford CAN FD vehicle.",
|
||||
"details": "Derives path offset and heading from the same selected desired curvature and adjusts the heading request using the difference between requested and measured steering. Uses remaining path-offset range when the base heading request reaches its limit. The correction holds when steering matches and clears on driver override. Default off; physical tracking and turn-exit behavior are not road-validated. Enable only for controlled testing. Turning it off restores upstream Ford curvature control, regardless of any previously stored experimental settings. Only Ford CAN FD vehicles can use this experiment. Changes apply after a real offroad-to-onroad cycle, not immediately or on disengagement alone.",
|
||||
"description": "Follow the selected model reference using path-offset and heading commands with measured steering feedback on any Ford CAN FD vehicle.",
|
||||
"details": "Uses the selected desired curvature, or the model path when Model Geometry Reference is enabled, with correction based on requested versus measured steering. Uses remaining path-offset range when the base heading request reaches its limit. The correction holds when steering matches and clears on driver override. Default off; physical tracking and turn-exit behavior are not road-validated. Enable only for controlled testing. Turning it off restores upstream Ford curvature control, regardless of any previously stored experimental settings. Only Ford CAN FD vehicles can use this experiment. Changes apply after a real offroad-to-onroad cycle, not immediately or on disengagement alone.",
|
||||
"enablement": [
|
||||
{
|
||||
"type": "offroad_only"
|
||||
@@ -2196,8 +2196,8 @@
|
||||
"widget": "toggle",
|
||||
"needs_onroad_cycle": true,
|
||||
"title": "Model Geometry Reference (Experimental)",
|
||||
"description": "Use model heading and heading rate as the steering reference for the Ford C0/C1 controller.",
|
||||
"details": "On uses the published model path to calculate desired curvature at the model's existing preview time, with the same output smoothing. Off uses the original model action. Both use the existing C0/C1 mapping, steering feedback, and limits. Requires Selected-Action Path Tracking; otherwise upstream Ford control remains selected. The original action and selected reference are both logged. Default off. Changes apply after an offroad-to-onroad cycle.",
|
||||
"description": "Sample model lateral position for C0 and model heading for C1, with measured steering feedback.",
|
||||
"details": "On uses direct model-path commands. C0 samples at 7 metres unless the C0 distance option is enabled; C1 samples at the larger of 7 metres or one second of travel. Feedback follows the heading from that same path. Command bounds and request limits remain active. Off restores the original model-action mapping. Requires Selected-Action Path Tracking; otherwise upstream Ford control remains selected. This changes the steering reference and has not been road-validated. Default off. Changes apply after an offroad-to-onroad cycle.",
|
||||
"enablement": [
|
||||
{
|
||||
"type": "offroad_only"
|
||||
|
||||
@@ -14,16 +14,16 @@ sections:
|
||||
widget: toggle
|
||||
needs_onroad_cycle: true
|
||||
title: Selected-Action Path Tracking (Experimental)
|
||||
description: Follow the selected desired curvature using path-offset and heading commands with measured steering feedback on any Ford CAN FD vehicle.
|
||||
details: Derives path offset and heading from the same selected desired curvature and adjusts the heading request using the difference between requested and measured steering. Uses remaining path-offset range when the base heading request reaches its limit. The correction holds when steering matches and clears on driver override. Default off; physical tracking and turn-exit behavior are not road-validated. Enable only for controlled testing. Turning it off restores upstream Ford curvature control, regardless of any previously stored experimental settings. Only Ford CAN FD vehicles can use this experiment. Changes apply after a real offroad-to-onroad cycle, not immediately or on disengagement alone.
|
||||
description: Follow the selected model reference using path-offset and heading commands with measured steering feedback on any Ford CAN FD vehicle.
|
||||
details: Uses the selected desired curvature, or the model path when Model Geometry Reference is enabled, with correction based on requested versus measured steering. Uses remaining path-offset range when the base heading request reaches its limit. The correction holds when steering matches and clears on driver override. Default off; physical tracking and turn-exit behavior are not road-validated. Enable only for controlled testing. Turning it off restores upstream Ford curvature control, regardless of any previously stored experimental settings. Only Ford CAN FD vehicles can use this experiment. Changes apply after a real offroad-to-onroad cycle, not immediately or on disengagement alone.
|
||||
enablement:
|
||||
- $ref: '#/macros/offroad'
|
||||
- key: FordGeometryReference
|
||||
widget: toggle
|
||||
needs_onroad_cycle: true
|
||||
title: Model Geometry Reference (Experimental)
|
||||
description: Use model heading and heading rate as the steering reference for the Ford C0/C1 controller.
|
||||
details: On uses the published model path to calculate desired curvature at the model's existing preview time, with the same output smoothing. Off uses the original model action. Both use the existing C0/C1 mapping, steering feedback, and limits. Requires Selected-Action Path Tracking; otherwise upstream Ford control remains selected. The original action and selected reference are both logged. Default off. Changes apply after an offroad-to-onroad cycle.
|
||||
description: Sample model lateral position for C0 and model heading for C1, with measured steering feedback.
|
||||
details: On uses direct model-path commands. C0 samples at 7 metres unless the C0 distance option is enabled; C1 samples at the larger of 7 metres or one second of travel. Feedback follows the heading from that same path. Command bounds and request limits remain active. Off restores the original model-action mapping. Requires Selected-Action Path Tracking; otherwise upstream Ford control remains selected. This changes the steering reference and has not been road-validated. Default off. Changes apply after an offroad-to-onroad cycle.
|
||||
enablement:
|
||||
- $ref: '#/macros/offroad'
|
||||
- type: param
|
||||
|
||||
+17
-9
@@ -1,7 +1,7 @@
|
||||
"""Compare driver-input arbitration on recorded references and frozen vehicle motion.
|
||||
"""Compare direct model-path commands against the driven geometry-curvature controller.
|
||||
|
||||
Input: extract.py route.npz/model_paths.npz/metadata.json directories. This checks
|
||||
command continuity and overrides; it does not predict a changed wheel response.
|
||||
reference selection, command continuity and overrides; it does not predict a changed wheel response.
|
||||
"""
|
||||
import argparse
|
||||
import hashlib
|
||||
@@ -14,6 +14,7 @@ import numpy as np
|
||||
|
||||
from opendbc.car.vehicle_model import VehicleModel
|
||||
from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
|
||||
from tools.ford_pscm_lab.model_action_replay import WireCheck, sample, table
|
||||
|
||||
|
||||
@@ -35,11 +36,12 @@ def replay(source, destination, baseline_class, provenance):
|
||||
cp = SimpleNamespace(**{k: car[k] for k in ('mass', 'wheelbase', 'centerToFront', 'steerRatioRear', 'tireStiffnessFront', 'tireStiffnessRear')},
|
||||
steerRatio=car['steer_ratio'], rotationalInertia=0.)
|
||||
vm = VehicleModel(cp)
|
||||
cores = [baseline_class(), FordModelActionController()]
|
||||
cores = [baseline_class(), FordModelActionController(direct_path=True)]
|
||||
selected = 0.
|
||||
wire = WireCheck()
|
||||
names = ['t', 'active', 'valid', 'speed', 'pressed', 'raw_torque', 'pscm_override', 'limit',
|
||||
'old_c0', 'new_c0', 'old_c1', 'new_c1', 'old_i', 'new_i', 'old_feedback', 'new_feedback',
|
||||
'recorded_c0', 'recorded_c1', 'latched_angle_error']
|
||||
'recorded_c0', 'recorded_c1', 'latched_angle_error', 'old_reference', 'new_reference']
|
||||
rows = np.zeros((len(t), len(names)))
|
||||
for i, now in enumerate(t):
|
||||
vm.update_params(max(pa['stiffness'][i], .1), max(pa['steer_ratio'][i], .1))
|
||||
@@ -51,16 +53,20 @@ def replay(source, destination, baseline_class, provenance):
|
||||
limit=int(ps['limit'][i]), lateralState=int(ps['lateral_state'][i]), denied=bool(ps['denied'][i]))
|
||||
active = bool(cc['active'][i])
|
||||
valid = bool(c['valid'][i] and cs['valid'][i] and cs['can_valid'][i] and pa['valid'][i] and exact[i])
|
||||
selected, _ = clip_curvature(cs['speed'][i], selected,
|
||||
cores[1].path_curvature(models[mi[i]], cs['speed'][i]) if active else c['measured'][i], pa['roll'][i])
|
||||
commands = []
|
||||
for core in cores:
|
||||
command = core.update(models[mi[i]], c['desired'][i], current_curvature=c['measured'][i],
|
||||
for variant, core in enumerate(cores):
|
||||
target = c['desired'][i] if variant == 0 else selected
|
||||
options = {} if variant == 0 else {'roll': pa['roll'][i]}
|
||||
command = core.update(models[mi[i]], target, current_curvature=c['measured'][i],
|
||||
speed=cs['speed'][i], yaw_rate=cs['yaw'][i], now=now, measurement_time=cs['t'][i],
|
||||
model_time=c['model_ns'][i]*1e-9, reference_time=c['model_ns'][i]*1e-9,
|
||||
active=active, valid=valid, driver_pressed=bool(cs['pressed'][i]), driver_torque=cs['torque'][i],
|
||||
pscm_status=status, curvature_scale=scale)
|
||||
pscm_status=status, curvature_scale=scale, **options)
|
||||
assert abs(command.path_offset) <= 5.1100001 and abs(command.path_angle) <= .5000001
|
||||
assert command.curvature == command.curvature_rate == 0.
|
||||
if command.valid and (cs['pressed'][i] or (status.valid and -.005 <= now-ps['stamp'][i] <= .15 and status.limit == 3)):
|
||||
if command.valid and (cs['pressed'][i] or abs(cs['torque'][i]) > 1. or (status.valid and -.005 <= now-ps['stamp'][i] <= .15 and status.limit == 3)):
|
||||
assert not core.diagnostics['feedback_enabled']
|
||||
assert core.core.correction == core.core.proportional == core.core.offset_proportional == 0.
|
||||
commands.append(command)
|
||||
@@ -71,7 +77,7 @@ def replay(source, destination, baseline_class, provenance):
|
||||
rows[i] = [now-meta['t0'], active, new.valid, cs['speed'][i], cs['pressed'][i], cs['torque'][i], status.limit == 3, status.limit == 2,
|
||||
old.path_offset, new.path_offset, old.path_angle, new.path_angle, cores[0].core.correction, cores[1].core.correction,
|
||||
cores[0].diagnostics.get('feedback_enabled', False), cores[1].diagnostics.get('feedback_enabled', False),
|
||||
sent['c0'][i], sent['c1'][i], abs(cs['angle'][i]-c['actual_angle'][i])]
|
||||
sent['c0'][i], sent['c1'][i], abs(cs['angle'][i]-c['actual_angle'][i]), c['desired'][i], selected]
|
||||
a = dict(zip(names, rows.T, strict=True))
|
||||
live = a['valid'].astype(bool)
|
||||
consecutive = live[1:] & live[:-1] & (np.diff(t) < .03)
|
||||
@@ -81,6 +87,7 @@ def replay(source, destination, baseline_class, provenance):
|
||||
k: np.quantile(abs(a['old_'+k][live]-a['recorded_'+k][live]), [.5, .95, .99, 1]).tolist() for k in ('c0', 'c1')},
|
||||
'latched_angle_match_fraction': float(np.mean(a['latched_angle_error'][live] < 1e-4)),
|
||||
'command_changes': {k: np.quantile(abs(a['new_'+k][live]-a['old_'+k][live]), [.5, .95, .99, 1]).tolist() for k in ('c0', 'c1')},
|
||||
'reference_change_p50_p95_max': np.quantile(abs(a['new_reference'][live]-a['old_reference'][live]), [.5, .95, 1]).tolist(),
|
||||
'variants': {}}
|
||||
for name in ('old', 'new'):
|
||||
metrics['variants'][name] = {
|
||||
@@ -89,6 +96,7 @@ def replay(source, destination, baseline_class, provenance):
|
||||
'low_speed_c0_steps_over_025m': int(np.count_nonzero(abs(np.diff(a[name+'_c0'])[low]) > .250001)),
|
||||
'low_speed_c1_steps_over_005rad': int(np.count_nonzero(abs(np.diff(a[name+'_c1'])[low]) > .050001)),
|
||||
'low_speed_step_p99': {k: float(np.quantile(abs(np.diff(a[name+'_'+k])[low]), .99)) for k in ('c0', 'c1')},
|
||||
'c0_bound_active_s': float(np.sum(np.minimum(np.diff(t, append=t[-1]+.01), .03)[live & (abs(a[name+'_c0']) >= 5.105)])),
|
||||
'c1_bound_active_s': float(np.sum(np.minimum(np.diff(t, append=t[-1]+.01), .03)[live & (abs(a[name+'_c1']) >= .49975)])),
|
||||
}
|
||||
# Baseline agreement is reported rather than hidden: control publication time
|
||||
Reference in New Issue
Block a user