mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-09-09 22:23:46 +08:00
Ford: damp excess-yaw offset demand in selected-action controller
Attenuate same-direction C0 when measured yaw exceeds the nonnegative requested turn plus a deadband. Keep C1, two slew states, input gates and zero C2/C3. Opposed planned curvature cannot amplify small yaw bias. Segment 10 replay reduces residual exit demand while preserving peak entry C0. This remains an experimental, physically unvalidated candidate under the existing default-off Sunnylink toggle. Validation: 325 tests and 26 subtests; 100% controller statement/branch coverage; 204,946 route cycles; 628,030 Float32/CAN round trips; independent standards/spec reviews. Assisted-by: OpenAI Codex
This commit is contained in:
Isaac Barham
@@ -0,0 +1,85 @@
|
||||
# Experimental Ford offset damping, v2
|
||||
|
||||
Segment 10 of the supplied route9b recording shows measured turning persisting
|
||||
as requested right curvature falls. At about 643.0 s, before strong driver
|
||||
intervention, device-gyro curvature is approximately 0.01786/m against a
|
||||
0.01172/m request. Around 643.9 s, heading demand has reversed slightly but
|
||||
C0 still requests approximately +0.12 m into the turn. Strong column input
|
||||
starts around 643.852 s; later motion cannot establish autonomous recovery.
|
||||
Earlier light driver input also exists.
|
||||
|
||||
The outgoing CAN commands match preceding publications. All 70,937 decoded
|
||||
frames have zero C2/C3 and valid checksums. Focused exit diagnostics have fresh
|
||||
model/carState inputs and targets within normal quantization of the outputs.
|
||||
This supports trying less residual C0; it does not identify PSCM dynamics or
|
||||
prove C0 alone caused the physical oversteer.
|
||||
|
||||
## Change
|
||||
|
||||
C0 starts from the clipped current model offset at 7 m. When C0 and measured
|
||||
host yaw point in the same direction, compute:
|
||||
|
||||
```
|
||||
requested_yaw = max(0, sign(C0) * speed * desiredCurvature)
|
||||
excess = max(0, sign(C0) * yaw - requested_yaw - 0.02 rad/s)
|
||||
reduction = 7 m * 0.2 s * excess
|
||||
target = sign(C0) * max(0, abs(C0) - reduction)
|
||||
```
|
||||
|
||||
Opposing centering demand is unchanged. The correction cannot increase the
|
||||
target's magnitude or reverse its sign. Opposite-direction planned curvature
|
||||
cannot amplify a small yaw bias into a correction. Existing 4 m/s C0 slew still
|
||||
applies; this target bound is not a claim that every stateful output is smaller than a
|
||||
separate v1 controller after arbitrary direction reversals. C1 construction,
|
||||
clipping and slew are unchanged; C2=C3=0. Only C0/C1 slew states persist.
|
||||
There is no integral, model-history filter, turn state machine or fitted plant.
|
||||
|
||||
Host yaw is `-carState.yawRate`, as in the existing Ford call path. The
|
||||
0.02 rad/s deadband exceeds the approximately 0.008 rad/s offset measured
|
||||
against the device gyro on quiet straights. The 0.2 s scale is an initial
|
||||
engineering choice, not an identified delay or gain. Both remain physically
|
||||
unvalidated. Large biased or noisy yaw within the existing sanity gate can
|
||||
still attenuate useful centering; fixed-input replay cannot establish stability.
|
||||
|
||||
## Offline evidence
|
||||
|
||||
The complete 12-rlog route is replayed at original controls publication times,
|
||||
with exact consumed model geometry, causal carState, and carControl matched
|
||||
within 5 ms. These times proxy computation; full SubMaster health is unavailable.
|
||||
V1 reconstruction is within one field quantum of all 64,701 paired active
|
||||
publications. V2 has identical eligibility and exactly identical C1.
|
||||
|
||||
On 381.78 seconds of driver-clean low requests above 8 m/s, only 3 of 37,937 cycles
|
||||
change C0, each by one 0.01 m quantum. Across the 642.7–643.852 s exit window,
|
||||
C0 changes on all 115 cycles, averaging 0.080 m reduction. Entry/peak C0
|
||||
maximum stays 2.80 m; some entry-window samples decrease by up to 0.05 m.
|
||||
These are command comparisons on recorded inputs, not predicted tracking.
|
||||
Low request means requested lateral acceleration below 0.15 m/s²; it is a
|
||||
proxy for straight driving and does not establish a physically straight path.
|
||||
|
||||
The original routes90/95 also run through v2. Their zero-yaw baseline pass
|
||||
checks archived v1 compatibility; the measured-yaw adapter pass checks current
|
||||
construction, eligibility, field limits and packing. It is not an exact match
|
||||
to v1 or v8. Randomized testing checks the damping against an independent
|
||||
piecewise oracle, mirror symmetry, resets and slew, with real Float32/CAN
|
||||
round trips. See `ford_model_action_damping_validation.json` for counts and hashes.
|
||||
|
||||
Final validation passes 325 tests and 26 subtests, with 100% controller
|
||||
statement/branch coverage, 204,946 original route cycles and 628,030 CAN round
|
||||
trips. The module is 166 total lines, including 107 code lines excluding
|
||||
comments, blanks and docstrings. Standards and Spec reviews have no remaining findings.
|
||||
|
||||
## Reproduce
|
||||
|
||||
Use the dependency setup and suite command in the [drive-test guide](ford_model_action_drive_test.md).
|
||||
The new route replay requires the deployment opendbc pin recorded there:
|
||||
|
||||
```sh
|
||||
python -m tools.ford_pscm_lab.damping_replay /path/to/complete/rlogs --output /path/to/separate/results
|
||||
python -m tools.ford_pscm_lab.stress_model_action --cycles 200000 --seed 20260907 --opendbc-revision c21a9013700734dd20b09e05aa68329ad8cc20f9 --output /path/to/stress.json
|
||||
```
|
||||
|
||||
The same default-off Sunnylink toggle selects v2; no additional setting is
|
||||
introduced. Updating an installation with the toggle already enabled selects
|
||||
v2 at the next controlsd startup. `calibration_approved=false` remains explicit.
|
||||
No physical fix, hardware build or device boot is established by these checks.
|
||||
@@ -0,0 +1,164 @@
|
||||
{
|
||||
"date": "2026-09-07",
|
||||
"baseline_commit": "5fc16abc7662020706e29f57d31a6d5e2bc1293a",
|
||||
"deployment_target": {
|
||||
"repository": "sunnypilot/sunnypilot",
|
||||
"branch": "hiimisaac-dev"
|
||||
},
|
||||
"hypothesis": "model-action-c0-c1-yaw-damping-v2",
|
||||
"scope": "Experimental bounded offset damping; fixed-input offline evidence only.",
|
||||
"calibration_approved": false,
|
||||
"hardware_build_and_device_boot": "not performed",
|
||||
"controller_size": {
|
||||
"total_lines": 166,
|
||||
"code_lines_excluding_blanks_comments_docstrings": 107,
|
||||
"core_persistent_values": 2,
|
||||
"adapter_timestamps": 3
|
||||
},
|
||||
"checks": {
|
||||
"combined_ford_params_sunnylink_suite": "325 passed, 26 subtests passed; no skips",
|
||||
"suite_log_sha256": "69cb8ab40e93e00a9ff7b6ea1933e4c3554336746860efb53dad3b29f94fc9d3",
|
||||
"coverage": {
|
||||
"statements": 98,
|
||||
"branches": 28,
|
||||
"percent": 100.0
|
||||
},
|
||||
"ruff": "pass",
|
||||
"ty_controller_and_lab": "pass",
|
||||
"settings_compiler_check": "pass",
|
||||
"standards_review_remaining_findings": 0,
|
||||
"spec_review_remaining_findings": 0,
|
||||
"review_resolutions": [
|
||||
"Prevent opposite-direction planned curvature amplifying small yaw bias; eight new cases failed before the fix and passed after it.",
|
||||
"Relabel requested-acceleration cohort as low request; it does not establish physically straight driving."
|
||||
],
|
||||
"mutation_probe": "Disabling damping fails all four mirrored recorded-exit cases."
|
||||
},
|
||||
"stress": {
|
||||
"random_cycles": 200000,
|
||||
"mirrored_core_updates": 200000,
|
||||
"field_boundary_cases": 18138,
|
||||
"float32_can_round_trips": 218138,
|
||||
"bounded_excess_yaw_damping_checked": true,
|
||||
"opendbc_import_head": "c21a9013700734dd20b09e05aa68329ad8cc20f9"
|
||||
},
|
||||
"routes": {
|
||||
"route90": {
|
||||
"cycles": 78812,
|
||||
"core_exact_archived_match": true,
|
||||
"adapter_active_cycles": 73055,
|
||||
"adapter_matches_current_core_with_yaw_and_fresh_engagement_dt": true,
|
||||
"adapter_validity_differs_from_archive_cycles": 0,
|
||||
"adapter_command_differs_from_archive_cycles": 2280,
|
||||
"adapter_max_absolute_command_difference_c0_c1": [
|
||||
0.1900000000000004,
|
||||
0.0020000000000000018
|
||||
],
|
||||
"float32_can_round_trips": 157624,
|
||||
"opendbc_import_head": "72a775d35e54c21ff5c5798acef22016eedcc0a7"
|
||||
},
|
||||
"route95": {
|
||||
"cycles": 54738,
|
||||
"core_exact_archived_match": true,
|
||||
"adapter_active_cycles": 37614,
|
||||
"adapter_matches_current_core_with_yaw_and_fresh_engagement_dt": true,
|
||||
"adapter_validity_differs_from_archive_cycles": 0,
|
||||
"adapter_command_differs_from_archive_cycles": 2051,
|
||||
"adapter_max_absolute_command_difference_c0_c1": [
|
||||
0.22999999999999998,
|
||||
0.0010000000000000009
|
||||
],
|
||||
"float32_can_round_trips": 109476,
|
||||
"opendbc_import_head": "72a775d35e54c21ff5c5798acef22016eedcc0a7"
|
||||
},
|
||||
"route9b": {
|
||||
"cycles": 71396,
|
||||
"eligible_cycles": 64701,
|
||||
"same_validity": true,
|
||||
"c1_exactly_unchanged": true,
|
||||
"field_slew_zero_c2_c3_pass": true,
|
||||
"float32_can_round_trips": 142792,
|
||||
"v1_reconstruction_vs_recorded": {
|
||||
"paired_cycles": 64701,
|
||||
"within_one_quantum_cycles": 64701,
|
||||
"maximum_absolute_error_c0_c1": [
|
||||
0.010000114440917862,
|
||||
0.0005000143051147043
|
||||
]
|
||||
},
|
||||
"timing": "Publication-time proxy, causal carState, exact consumed model; full SubMaster health unavailable.",
|
||||
"baseline": "Current adapter with zero yaw retains v1 targets and actual-yaw sanity gate.",
|
||||
"opendbc_head": "c21a9013700734dd20b09e05aa68329ad8cc20f9",
|
||||
"cohorts": {
|
||||
"driver_clean_low_request_above_8mps": {
|
||||
"cycles": 37937,
|
||||
"seconds": 381.77672216900055,
|
||||
"changed_c0_cycles": 3,
|
||||
"mean_absolute_c0_change_m": 8.419774473786704e-07,
|
||||
"max_absolute_c0_change_m": 0.009999999999999787,
|
||||
"v1_peak_absolute_c0_m": 0.08999999999999986,
|
||||
"v2_peak_absolute_c0_m": 0.08999999999999986
|
||||
},
|
||||
"segment10_entry_peak": {
|
||||
"cycles": 298,
|
||||
"seconds": 2.9932484459999387,
|
||||
"changed_c0_cycles": 100,
|
||||
"mean_absolute_c0_change_m": 0.007564164795694466,
|
||||
"max_absolute_c0_change_m": 0.04999999999999982,
|
||||
"v1_peak_absolute_c0_m": 2.8000000000000003,
|
||||
"v2_peak_absolute_c0_m": 2.8000000000000003
|
||||
},
|
||||
"segment10_exit_before_strong_input": {
|
||||
"cycles": 115,
|
||||
"seconds": 1.156770048999988,
|
||||
"changed_c0_cycles": 115,
|
||||
"mean_absolute_c0_change_m": 0.0799313220721162,
|
||||
"max_absolute_c0_change_m": 0.1200000000000001,
|
||||
"v1_peak_absolute_c0_m": 0.75,
|
||||
"v2_peak_absolute_c0_m": 0.7000000000000002
|
||||
}
|
||||
},
|
||||
"exit_c0_strictly_lower_on_all_115_cycles": true,
|
||||
"rlog_sha256_by_segment": {
|
||||
"0": "22746f7119109b73ed7f2c26ce8c99f87136e9124fb7fc14c9554409a28a7c3f",
|
||||
"1": "4c2e1d7083c31a2b37d0f8dd3be4d330898511b7e02c26f7d40ca9bc2779397d",
|
||||
"2": "62f3e049e220cd3681fadf386f2969537bd571998ae2f6ba2d08479428b5a28f",
|
||||
"3": "83bf0131b2d36b2ba7e5ba050bbc13c0a3350feb5c9b89dc9c87d3a37abebfb3",
|
||||
"4": "430985a80dd6e10f7abeb89457a17022e6bb6978617f415c905f584b1647603e",
|
||||
"5": "8c0c5ae6323ec33b3e14f84ca834f70cb56f6b29f471a350f1e3efc06b6ba553",
|
||||
"6": "db53dfa8156b9d66792c3eff0b2ce5d31b71ad41cc580dec85f528845593c184",
|
||||
"7": "91b0b3be10cb7d7d7f7dd2024d8f9ee99d1e9fd2204203a3a9a2f2f1c6e3fa03",
|
||||
"8": "687dbbfc49837efbfe8fa6bc091e40f7fad2908832234d7884f4616d1bc9ccff",
|
||||
"9": "a88ec4d25b04cdbf5844686fc77f6b28dca920c9b164e37ebf69844a3ae398fc",
|
||||
"10": "fe6b29580a6c94e1c236d13e18db4cd9f31cc1b25d52e1e6e19a5021125c9932",
|
||||
"11": "150d31b1944d7a1b8c562f3aee20b66cefa6c4e8d02660ec889907d835142f45"
|
||||
}
|
||||
}
|
||||
},
|
||||
"total_original_route_cycles": 204946,
|
||||
"total_float32_can_round_trips": 628030,
|
||||
"source_sha256": {
|
||||
"docs/ford_model_action_damping.md": "1aca8ca8e78d953beeda5b0c9803161a1d7c58556b1966040c71f809c3960bf8",
|
||||
"docs/ford_model_action_drive_test.md": "3ce8bf6cb511a4461fa7abf194f47020af7ef5a6c90fcae7ed09571d1f750846",
|
||||
"openpilot/selfdrive/controls/lib/ford_model_action.py": "59d66297a017557f3d4f28b115be3f6220b800566c11935e2284b3814783fb7e",
|
||||
"openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py": "318742bd707ae526d0f5181bf7f081660c2c55de4bdf28518ccdf50d60e88080",
|
||||
"openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py": "de6f8524347f7c4a339941bc8565ccaa131cb93aa0418c75006ce08ab7edeb99",
|
||||
"openpilot/selfdrive/controls/tests/test_ford_model_action_damping.py": "76466997ab4fef435f44339a6cb2d06303d5f0ab8717d487f27655297bd429d1",
|
||||
"openpilot/sunnypilot/sunnylink/settings_ui.json": "dbb78c98f57eef532f0dff0cb0b38396442882876e6d115f0d3c36159f64baf5",
|
||||
"openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml": "b92b55e23467e74227988fb39fff13a4ebba3c960fb90e34a6b341411699fc95",
|
||||
"tools/ford_pscm_lab/damping_replay.py": "2f52fef12116ce87c0a1f465cf13d6af5e9fd76ce05d4318d72e34b70bc6a11f",
|
||||
"tools/ford_pscm_lab/model_action_replay.py": "05a658dfcaf81693bf0d92184c0edff0172802f351a61a9e866a7967e74ae46d",
|
||||
"tools/ford_pscm_lab/stress_model_action.py": "3e308733f4af0101ad0c414fbd724f6a99f56269ed8e791f0e15526d8bfd8f17"
|
||||
},
|
||||
"artifact_sha256": {
|
||||
"route9b/report.json": "1b9317850c1724f433269c6a58349d0a0ee4eb6c9a03d6ec5858fa17713796c5",
|
||||
"route9b/commands.npz": "d9f553a5384c84416a27c52b5dda0e751dda25edc5d6620fc311977f34a7a946",
|
||||
"route90/report.json": "e37f71dc032e375b1c9b0beb4d6e0c915257bf72e59b0785ac0ca49c2472d2c2",
|
||||
"route95/report.json": "d42d5a080fef8a1f0b3c7ae2cabcad20c88ce01d91be07ab33770ff5948c06b6",
|
||||
"stress.json": "06e69a23340e3f5ed174e8e0b2e5791b320686dce7df963233d50a9982dca17b",
|
||||
"coverage.json": "8f7915b9bd884abedfdbc2e0c18ef4474737e27225a714414384242535cc396f",
|
||||
"mutation.txt": "67a76549fd7bb71e7092a155d4d0c3459be7b04dc2ca36eef9b54fbb574ad83d",
|
||||
"bias_regression_red.txt": "8903c5a967e8c376050db85f7cf5f73abf71ee972f6025b25eb874479f94c62c",
|
||||
"segment10_damping.png": "7d60cf9aabdcca9000fcf49bd14bd6130418aa6ab1b57af2678b171498ee9505"
|
||||
}
|
||||
}
|
||||
@@ -1,8 +1,9 @@
|
||||
# Ford selected-action drive-test branch
|
||||
|
||||
The candidate is selectable on the **Ford CAN FD F-150 Lightning** behind
|
||||
its own persistent, default-off Sunnylink toggle. The command law and input
|
||||
gates from the [offline candidate](ford_model_action_candidate.md) are unchanged.
|
||||
its own persistent, default-off Sunnylink toggle. Version 2 adds
|
||||
[bounded excess-yaw offset damping](ford_model_action_damping.md) to the
|
||||
[original candidate](ford_model_action_candidate.md). Input gates are unchanged.
|
||||
`calibration_approved=false`: offline checks do not establish physical tracking,
|
||||
turn-exit behavior or closed-loop stability.
|
||||
|
||||
@@ -19,7 +20,7 @@ turn-exit behavior or closed-loop stability.
|
||||
|
||||
The startup log event `Ford path controller selected` should report
|
||||
`FordModelActionController`. Periodic `Ford C2-free path tracking` events
|
||||
identify `hypothesis=model-action-c0-c1-v1` and report the command tuple.
|
||||
identify `hypothesis=model-action-c0-c1-yaw-damping-v2` and report host yaw and the command tuple.
|
||||
|
||||
Turning the new toggle off and completing another offroad-to-onroad cycle
|
||||
restores **PSCM Coefficient Observer** if selected, otherwise the original
|
||||
@@ -53,16 +54,13 @@ returns separate strings owned by the parameter handle. Regression tests
|
||||
check distinct registered keys across flags, and toggle tests check its
|
||||
persistence and backup registration using the rebuilt native library.
|
||||
|
||||
The current validation record is `ford_model_action_drive_test_validation.json`.
|
||||
The final combined Ford, Params and Sunnylink suite passes **284 tests and
|
||||
26 subtests**, with no skips. The candidate has **100% statement and branch
|
||||
coverage** (87 statements, 26 branches). Ruff, Ty, settings compilation and
|
||||
both review axes pass. The fresh route/stress runs check **485,238 Float32/CAN
|
||||
round trips**, including 200,000 randomized and 200,000 mirrored core updates.
|
||||
The controller is 145 total lines, including 95 code lines excluding comments,
|
||||
blanks and docstrings; v8's 469-line module is removed.
|
||||
The current validation record is `ford_model_action_damping_validation.json`;
|
||||
the [damping notes](ford_model_action_damping.md) explain its scope and limitations.
|
||||
`ford_model_action_drive_test_validation.json` archives v1 wiring validation
|
||||
at the recorded source hashes, including 284 tests and 26 subtests. Its counts
|
||||
and 145-line controller size describe v1. The 469-line v8 module remains removed.
|
||||
|
||||
The previous 133,550-cycle route reconstruction, 485,238 packing round trips
|
||||
The original 133,550-cycle route reconstruction, 485,238 packing round trips
|
||||
and mutation probes remain recorded separately in
|
||||
`ford_model_action_validation.json` at the offline-stage source hashes.
|
||||
|
||||
|
||||
@@ -14,6 +14,8 @@ from openpilot.selfdrive.controls.lib.ford_path import FordPath, _model_path
|
||||
|
||||
OFFSET_STATION_M = 7.0
|
||||
HEADING_TIME_S = 1.0
|
||||
EXCESS_YAW_DEADBAND = .02 # rad/s; above the observed approximately .008 rad/s Ford yaw offset
|
||||
EXCESS_YAW_LOOKAHEAD_S = .2 # engineering choice, not an identified PSCM delay
|
||||
CALIBRATION_APPROVED = False
|
||||
|
||||
|
||||
@@ -50,11 +52,27 @@ def encode_model_action(model, desired_curvature, speed):
|
||||
return FordPath(True, c0, c1, 0., 0.) if _finite(c0, c1) else FordPath()
|
||||
|
||||
|
||||
def damp_offset(c0, desired_curvature, speed, yaw_rate):
|
||||
"""Attenuate same-direction C0 demand when yaw exceeds the requested turn.
|
||||
|
||||
Inputs are finite and range-checked by the caller. The deadband avoids
|
||||
chasing small yaw offsets. Opposing centering demand is left intact.
|
||||
"""
|
||||
if c0*yaw_rate <= 0.:
|
||||
return c0
|
||||
direction = math.copysign(1., c0)
|
||||
# An opposed plan must not amplify near-zero yaw bias into a large correction.
|
||||
requested_yaw = max(0., direction*speed*desired_curvature)
|
||||
excess = max(0., direction*yaw_rate-requested_yaw-EXCESS_YAW_DEADBAND)
|
||||
reduction = OFFSET_STATION_M*EXCESS_YAW_LOOKAHEAD_S*excess
|
||||
return direction*max(0., abs(c0)-reduction)
|
||||
|
||||
|
||||
class ModelActionController:
|
||||
"""Only two control states: unquantized, independently slewed C0 and C1.
|
||||
|
||||
Freshness and engagement belong to the caller. No measured yaw, model
|
||||
history, heading integral, blending or release modes enter the law.
|
||||
Freshness and engagement belong to the caller. Excess yaw attenuates the
|
||||
offset target without model history, an integral or release modes.
|
||||
"""
|
||||
__slots__ = ('c0', 'c1')
|
||||
|
||||
@@ -64,8 +82,9 @@ class ModelActionController:
|
||||
def reset(self):
|
||||
self.c0 = self.c1 = 0.
|
||||
|
||||
def update(self, model, desired_curvature, *, speed, dt, active=True, valid=True):
|
||||
if not active or not valid or not _finite(dt) or not .002 <= dt <= .1:
|
||||
def update(self, model, desired_curvature, *, speed, dt, yaw_rate=0., active=True, valid=True):
|
||||
# Zero yaw preserves the archived v1 command construction for lab comparisons.
|
||||
if not active or not valid or not _finite(dt, yaw_rate) or not .002 <= dt <= .1 or abs(yaw_rate) > 3:
|
||||
self.reset()
|
||||
return FordPath()
|
||||
target = encode_model_action(model, desired_curvature, speed)
|
||||
@@ -73,6 +92,7 @@ class ModelActionController:
|
||||
self.reset()
|
||||
return FordPath()
|
||||
c0 = float(np.clip(target.path_offset, -5.11, 5.11))
|
||||
c0 = damp_offset(c0, desired_curvature, speed, yaw_rate)
|
||||
c1 = float(np.clip(target.path_angle, -.5, .5))
|
||||
self.c0 += float(np.clip(c0-self.c0, -4.*dt, 4.*dt))
|
||||
self.c1 += float(np.clip(c1-self.c1, -.5*dt, .5*dt))
|
||||
@@ -87,9 +107,9 @@ class FordModelActionController:
|
||||
core. Its timestamps and diagnostics never affect the targets. Raw model
|
||||
geometry is checked on every cycle, even at a repeated model timestamp.
|
||||
|
||||
Yaw is checked only for the inherited finite/range input gate. Engagement
|
||||
and downstream driver arbitration still apply. This controller does not use
|
||||
PSCM status or driver torque as control-law inputs.
|
||||
Validated host-coordinate yaw supplies stateless offset damping. Engagement
|
||||
and downstream driver arbitration still apply. PSCM status and driver torque
|
||||
are not control-law inputs.
|
||||
"""
|
||||
def __init__(self):
|
||||
self.core = ModelActionController()
|
||||
@@ -98,7 +118,7 @@ class FordModelActionController:
|
||||
def reset(self, status='inactive'):
|
||||
self.core.reset()
|
||||
self.last_time = self.last_measurement_time = self.last_model_time = None
|
||||
self.diagnostics = {'status': status, 'hypothesis': 'model-action-c0-c1-v1',
|
||||
self.diagnostics = {'status': status, 'hypothesis': 'model-action-c0-c1-yaw-damping-v2',
|
||||
'calibration_approved': CALIBRATION_APPROVED, 'command': (0., 0., 0., 0.)}
|
||||
|
||||
def update(self, model, desired_curvature, *, yaw_rate, speed, now, measurement_time, model_time, reference_time,
|
||||
@@ -124,13 +144,14 @@ class FordModelActionController:
|
||||
):
|
||||
self.reset('timing_reset')
|
||||
return FordPath()
|
||||
command = self.core.update(model, desired_curvature, speed=speed, dt=dt)
|
||||
command = self.core.update(model, desired_curvature, speed=speed, dt=dt, yaw_rate=yaw_rate)
|
||||
if not command.valid:
|
||||
self.reset('invalid_path')
|
||||
return command
|
||||
self.last_time, self.last_measurement_time, self.last_model_time = now, measurement_time, model_time
|
||||
self.diagnostics = {'status': 'active', 'hypothesis': 'model-action-c0-c1-v1',
|
||||
self.diagnostics = {'status': 'active', 'hypothesis': 'model-action-c0-c1-yaw-damping-v2',
|
||||
'calibration_approved': CALIBRATION_APPROVED, 'desired_curvature': desired_curvature,
|
||||
'yaw_rate': yaw_rate,
|
||||
'model_age': now - model_time, 'measurement_age': now - measurement_time, 'reference_age': now - reference_time,
|
||||
'dt': dt, 'offset_request': self.core.c0, 'heading_request': self.core.c1,
|
||||
'command': (command.path_offset, command.path_angle, 0., 0.)}
|
||||
|
||||
@@ -53,7 +53,7 @@ class TestFordControlsLogging(unittest.TestCase):
|
||||
controls = SimpleNamespace(ford_path_controller=controller, desired_curvature=.005, curvature=.0025,
|
||||
sm=SimpleNamespace(logMonoTime={'modelV2': 123456789, 'carState': 123450000}))
|
||||
record = self.emit_controls_event('Ford C2-free path tracking', controls)
|
||||
self.assertEqual(record['hypothesis'], 'model-action-c0-c1-v1')
|
||||
self.assertEqual(record['hypothesis'], 'model-action-c0-c1-yaw-damping-v2')
|
||||
self.assertIs(record['calibration_approved'], False)
|
||||
self.assertEqual(record['command'][2:], [0., 0.])
|
||||
self.assertEqual(record['status'], controller.diagnostics['status'])
|
||||
|
||||
@@ -162,7 +162,8 @@ class Subscriptions:
|
||||
|
||||
|
||||
@pytest.mark.parametrize('maneuver', [False, True])
|
||||
def test_actual_controlsd_selection_limiting_publication_and_downstream_can(pipeline, maneuver):
|
||||
@pytest.mark.parametrize('host_yaw', [.0072, .3])
|
||||
def test_actual_controlsd_selection_limiting_publication_and_downstream_can(pipeline, maneuver, host_yaw):
|
||||
call, publication = pipeline
|
||||
sm = Subscriptions(maneuver)
|
||||
controls = startup()
|
||||
@@ -171,14 +172,16 @@ def test_actual_controlsd_selection_limiting_publication_and_downstream_can(pipe
|
||||
model = straight(.4)
|
||||
model.action = SimpleNamespace(desiredCurvature=.1)
|
||||
cc = structs.CarControl(latActive=True)
|
||||
cs = SimpleNamespace(vEgo=20., yawRate=-.0072, canValid=True, steeringPressed=False, steeringTorque=0.)
|
||||
cs = SimpleNamespace(vEgo=20., yawRate=-host_yaw, canValid=True, steeringPressed=False, steeringTorque=0.)
|
||||
environment = {'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.)}
|
||||
exec(call, environment)
|
||||
expected_curvature = (-1 if maneuver else 1)*.000125
|
||||
assert controls.desired_curvature == pytest.approx(expected_curvature)
|
||||
assert controls.ford_path.path_angle == pytest.approx(20.*expected_curvature)
|
||||
assert controls.ford_path.path_offset == pytest.approx(.04)
|
||||
expected_offset = .04 if host_yaw < .02 else .01
|
||||
assert controls.ford_path.path_offset == pytest.approx(expected_offset)
|
||||
assert controller.diagnostics['yaw_rate'] == host_yaw
|
||||
assert cc.latActive and cc.actuators.curvature == 0.
|
||||
assert controller.diagnostics['reference_age'] == pytest.approx(.01 if maneuver else .02)
|
||||
|
||||
|
||||
@@ -0,0 +1,75 @@
|
||||
"""Bounded excess-yaw damping: remove offset demand without integral or modes."""
|
||||
import math
|
||||
import pytest
|
||||
|
||||
from openpilot.selfdrive.controls.lib.ford_model_action import ModelActionController, damp_offset
|
||||
from openpilot.selfdrive.controls.lib.ford_path import FordPath
|
||||
from openpilot.selfdrive.controls.tests.test_ford_model_action import straight
|
||||
|
||||
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
def test_recorded_turn_exit_reduces_same_direction_offset_before_driver_intervention(sign):
|
||||
# Route9b, segment10, about643.0s; fresh Ford yaw in host coordinates.
|
||||
c0, desired, speed, yaw = sign*.5, sign*.0117238564, 9.71, sign*.180
|
||||
reduced = damp_offset(c0, desired, speed, yaw)
|
||||
assert reduced == pytest.approx(sign*(.5-1.4*(.180-9.71*.0117238564-.02)))
|
||||
assert 0. < sign*reduced < .45
|
||||
|
||||
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
def test_recorded_late_exit_removes_remaining_offset_without_creating_countersteer(sign):
|
||||
# About643.9s. C1 is already slightly opposite; C0 still points into the turn.
|
||||
assert damp_offset(sign*.12, sign*-.0004078, 10.77, sign*.1002) == pytest.approx(sign*.00772)
|
||||
assert damp_offset(sign*.12, 0., 10.77, sign*.2) == 0.
|
||||
|
||||
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
@pytest.mark.parametrize('bias', [-.013, -.008, 0., .008, .013])
|
||||
def test_matched_turn_and_straight_bias_cannot_reduce_offset(sign, bias):
|
||||
for desired in (0., sign*.01, sign*.05):
|
||||
assert damp_offset(sign*.4, desired, 10., 10.*desired+bias) == sign*.4
|
||||
|
||||
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
@pytest.mark.parametrize('bias', [-.013, -.008, 0., .008, .013, .02])
|
||||
def test_opposed_plan_cannot_amplify_small_yaw_bias(sign, bias):
|
||||
for desired in (-sign*.01, -sign*.1):
|
||||
assert damp_offset(sign*.4, desired, 20., sign*bias) == sign*.4
|
||||
|
||||
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
def test_damping_begins_continuously_above_the_yaw_deadband(sign):
|
||||
assert damp_offset(sign*.4, -sign*.1, 20., sign*.020001) == pytest.approx(sign*(.4-1.4e-6))
|
||||
|
||||
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
def test_entry_deficit_opposing_centering_and_zero_offset_are_preserved(sign):
|
||||
assert damp_offset(sign*2.75, sign*.053889, 5.283, sign*.272) == sign*2.75
|
||||
assert damp_offset(sign*-.25, sign*.001, 10., sign*.2) == sign*-.25
|
||||
assert damp_offset(0., sign*.001, 10., sign*.2) == 0.
|
||||
|
||||
|
||||
@pytest.mark.parametrize('sign', [-1., 1.])
|
||||
def test_core_keeps_heading_unchanged_and_slews_offset_independently(sign):
|
||||
baseline, damped = ModelActionController(), ModelActionController()
|
||||
for i in range(300):
|
||||
desired = sign*(.02 if i < 100 else .001)
|
||||
a = baseline.update(straight(sign*.4), desired, speed=10., dt=.01)
|
||||
b = damped.update(straight(sign*.4), desired, speed=10., dt=.01, yaw_rate=sign*.2)
|
||||
assert a.path_angle == b.path_angle
|
||||
assert a.curvature == b.curvature == a.curvature_rate == b.curvature_rate == 0.
|
||||
assert a.path_offset == pytest.approx(sign*.4)
|
||||
assert b.path_offset == pytest.approx(sign*.16)
|
||||
# No damping memory: a fresh copied pair of actuator states behaves identically.
|
||||
copied = ModelActionController()
|
||||
copied.c0, copied.c1 = damped.c0, damped.c1
|
||||
assert copied.update(straight(sign*.4), 0., speed=10., dt=.01, yaw_rate=0.) == damped.update(
|
||||
straight(sign*.4), 0., speed=10., dt=.01, yaw_rate=0.)
|
||||
|
||||
|
||||
@pytest.mark.parametrize('yaw', [math.nan, math.inf, -math.inf, None, 'bad', 3.001, -3.001])
|
||||
def test_invalid_yaw_resets_core(yaw):
|
||||
c = ModelActionController()
|
||||
c.update(straight(.4), .01, speed=10., dt=.01)
|
||||
assert c.update(straight(.4), .01, speed=10., dt=.01, yaw_rate=yaw) == FordPath()
|
||||
assert c.c0 == c.c1 == 0.
|
||||
@@ -2184,7 +2184,7 @@
|
||||
"needs_onroad_cycle": true,
|
||||
"title": "Selected-Action Path Tracking (Experimental)",
|
||||
"description": "Follow the selected steering plan with nearby model-path centering on the Ford CAN FD F-150 Lightning.",
|
||||
"details": "Uses nearby model-path offset and a heading request based directly on selected planned curvature. There is no accumulated measured-turning correction. Default off; physical tracking and turn-exit behavior are not road-validated. Enable only for controlled testing. On the Ford CAN FD F-150 Lightning this takes priority over PSCM Coefficient Observer; other vehicles retain their existing controller. Turning it off restores PSCM Coefficient Observer if selected, otherwise the original Ford path controller. Changes apply after a real offroad-to-onroad cycle, not immediately or on disengagement alone.",
|
||||
"details": "Uses nearby model-path offset and a heading request based directly on selected planned curvature. Reduces same-direction offset demand when measured turning exceeds the requested turn, without accumulating a correction. Default off; this revised turn-exit behavior is not road-validated. Enable only for controlled testing. On the Ford CAN FD F-150 Lightning this takes priority over PSCM Coefficient Observer; other vehicles retain their existing controller. Turning it off restores PSCM Coefficient Observer if selected, otherwise the original Ford path controller. Changes apply after a real offroad-to-onroad cycle, not immediately or on disengagement alone.",
|
||||
"enablement": [
|
||||
{
|
||||
"type": "offroad_only"
|
||||
|
||||
@@ -15,7 +15,7 @@ sections:
|
||||
needs_onroad_cycle: true
|
||||
title: Selected-Action Path Tracking (Experimental)
|
||||
description: Follow the selected steering plan with nearby model-path centering on the Ford CAN FD F-150 Lightning.
|
||||
details: Uses nearby model-path offset and a heading request based directly on selected planned curvature. There is no accumulated measured-turning correction. Default off; physical tracking and turn-exit behavior are not road-validated. Enable only for controlled testing. On the Ford CAN FD F-150 Lightning this takes priority over PSCM Coefficient Observer; other vehicles retain their existing controller. Turning it off restores PSCM Coefficient Observer if selected, otherwise the original Ford path controller. Changes apply after a real offroad-to-onroad cycle, not immediately or on disengagement alone.
|
||||
details: Uses nearby model-path offset and a heading request based directly on selected planned curvature. Reduces same-direction offset demand when measured turning exceeds the requested turn, without accumulating a correction. Default off; this revised turn-exit behavior is not road-validated. Enable only for controlled testing. On the Ford CAN FD F-150 Lightning this takes priority over PSCM Coefficient Observer; other vehicles retain their existing controller. Turning it off restores PSCM Coefficient Observer if selected, otherwise the original Ford path controller. Changes apply after a real offroad-to-onroad cycle, not immediately or on disengagement alone.
|
||||
enablement:
|
||||
- $ref: '#/macros/offroad'
|
||||
- key: FordPscmObserver
|
||||
|
||||
@@ -0,0 +1,156 @@
|
||||
"""Compare v1 construction and v2 damping on complete local rlogs, offline.
|
||||
|
||||
Original controls publication times proxy computation time. Consumed model
|
||||
timestamps are exact; carState is causal and carControl is matched within 5 ms.
|
||||
This compares commands on a fixed recording, never counterfactual vehicle motion.
|
||||
"""
|
||||
import argparse
|
||||
from collections import Counter
|
||||
import hashlib
|
||||
import json
|
||||
from pathlib import Path
|
||||
from types import SimpleNamespace
|
||||
|
||||
import numpy as np
|
||||
import zstandard
|
||||
|
||||
from openpilot.cereal import log
|
||||
from openpilot.selfdrive.controls.lib import ford_model_action
|
||||
from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController
|
||||
from tools.ford_pscm_lab.model_action_replay import WireCheck, field_checks, sample, verify_dependency
|
||||
|
||||
|
||||
DEPLOYMENT_OPENDBC = 'c21a9013700734dd20b09e05aa68329ad8cc20f9'
|
||||
|
||||
|
||||
def extract(directory):
|
||||
columns = {'cs': 't valid can_valid speed yaw torque pressed', 'controls': 't valid desired model_ns',
|
||||
'cc': 't valid active', 'params': 't valid', 'path': 't valid active c0 c1', 'model': 't valid ns'}
|
||||
rows = {name: [] for name in columns}
|
||||
models, sources = [], {}
|
||||
t0 = None
|
||||
files = sorted(directory.glob('*--rlog.zst'), key=lambda p: int(p.name.split('--')[-2]))
|
||||
if not files:
|
||||
raise ValueError('No complete rlogs found')
|
||||
for file in files:
|
||||
compressed = file.read_bytes()
|
||||
sources[file.name] = hashlib.sha256(compressed).hexdigest()
|
||||
data = zstandard.ZstdDecompressor().stream_reader(compressed).read()
|
||||
for event in log.Event.read_multiple_bytes(data):
|
||||
kind, t, valid = event.which(), event.logMonoTime*1e-9, event.valid
|
||||
if t0 is None:
|
||||
t0 = t
|
||||
if kind == 'carState':
|
||||
cs = event.carState
|
||||
rows['cs'].append((t, valid, cs.canValid, cs.vEgo, -cs.yawRate, cs.steeringTorque, cs.steeringPressed))
|
||||
elif kind == 'controlsState':
|
||||
cs = event.controlsState
|
||||
rows['controls'].append((t, valid, cs.desiredCurvature, cs.lateralPlanMonoTime))
|
||||
elif kind == 'carControl':
|
||||
rows['cc'].append((t, valid, event.carControl.latActive))
|
||||
elif kind == 'vehicleParameters':
|
||||
rows['params'].append((t, valid))
|
||||
elif kind == 'carControlSP':
|
||||
path = event.carControlSP.fordLateralPath
|
||||
rows['path'].append((t, valid, path.valid, path.pathOffset, path.pathAngle))
|
||||
elif kind == 'modelV2':
|
||||
model = event.modelV2
|
||||
models.append(SimpleNamespace(position=SimpleNamespace(x=list(model.position.x), y=list(model.position.y)),
|
||||
orientation=SimpleNamespace(z=list(model.orientation.z))))
|
||||
rows['model'].append((t, valid, event.logMonoTime))
|
||||
elif kind == 'lateralManeuverPlan':
|
||||
raise ValueError('This replay requires routes without a separate maneuver reference')
|
||||
streams = {}
|
||||
for name, fields in columns.items():
|
||||
values = np.array(rows[name])
|
||||
if not len(values) or np.any(np.diff(values[:, 0]) < 0.):
|
||||
raise ValueError(f'Missing or backward {name} stream')
|
||||
streams[name] = dict(zip(fields.split(), values.T, strict=True))
|
||||
return streams, models, sources, t0
|
||||
|
||||
|
||||
def run(directory, output):
|
||||
directory, output = directory.resolve(), output.resolve()
|
||||
if output == directory or directory in output.parents:
|
||||
raise ValueError('Output must be outside the source route directory')
|
||||
verify_dependency(DEPLOYMENT_OPENDBC)
|
||||
streams, models, sources, t0 = extract(directory)
|
||||
controls, model = streams['controls'], streams['model']
|
||||
t = controls['t']
|
||||
cs, params = (sample(streams[name], t) for name in ('cs', 'params'))
|
||||
cc, recorded = (sample(streams[name], t, nearest=True) for name in ('cc', 'path'))
|
||||
mi = np.clip(np.searchsorted(model['ns'], controls['model_ns']), 0, len(models)-1)
|
||||
exact = model['ns'][mi] == controls['model_ns']
|
||||
services = ((controls['valid'] == 1) & (cc['valid'] == 1) & (abs(cc['t']-t) < .005) &
|
||||
(cs['valid'] == 1) & (cs['can_valid'] == 1) & (params['valid'] == 1) &
|
||||
(t-params['t'] >= 0.) & (t-params['t'] <= .15) & exact & (model['valid'][mi] == 1))
|
||||
baseline, candidate, wire = FordModelActionController(), FordModelActionController(), WireCheck()
|
||||
before, after = np.zeros((len(t), 4)), np.zeros((len(t), 4))
|
||||
eligible = np.zeros(len(t), bool)
|
||||
reasons = Counter()
|
||||
for i, now in enumerate(t):
|
||||
kwargs = {'speed': cs['speed'][i], 'now': now, 'measurement_time': cs['t'][i], 'model_time': model['t'][mi[i]],
|
||||
'reference_time': model['t'][mi[i]], 'active': bool(cc['active'][i]), 'valid': bool(services[i])}
|
||||
geometry = models[mi[i]] if exact[i] else None
|
||||
# Zero yaw retains v1 targets. Both passes enforce the actual yaw range gate.
|
||||
kwargs['valid'] &= bool(np.isfinite(cs['yaw'][i]) and abs(cs['yaw'][i]) <= 3.)
|
||||
a = baseline.update(geometry, controls['desired'][i], yaw_rate=0., **kwargs)
|
||||
b = candidate.update(geometry, controls['desired'][i], yaw_rate=cs['yaw'][i], **kwargs)
|
||||
assert a.valid == b.valid and a.path_angle == b.path_angle
|
||||
before[i] = a.path_offset, a.path_angle, a.curvature, a.curvature_rate
|
||||
after[i] = b.path_offset, b.path_angle, b.curvature, b.curvature_rate
|
||||
eligible[i] = b.valid
|
||||
reasons[candidate.diagnostics['status']] += 1
|
||||
wire.check(a)
|
||||
wire.check(b)
|
||||
field_checks(before, eligible, t)
|
||||
field_checks(after, eligible, t)
|
||||
clean = eligible & (cs['pressed'] == 0) & (abs(cs['torque']) <= 1.)
|
||||
# Erode driver eligibility by one second in each direction on original time.
|
||||
bad = np.r_[0, np.cumsum(~clean)]
|
||||
left, right = np.searchsorted(t, t-1.), np.searchsorted(t, t+1., side='right')
|
||||
clean &= (bad[right] == bad[left]) & (t >= t[0]+1.) & (t <= t[-1]-1.)
|
||||
relative = t-t0
|
||||
masks = {'eligible': eligible, 'driver_clean': clean,
|
||||
'driver_clean_low_request_above_8mps': clean & (cs['speed'] >= 8.) & (abs(controls['desired'])*cs['speed']**2 < .15),
|
||||
'turn': clean & (abs(controls['desired'])*cs['speed']**2 >= .5),
|
||||
'segment10_entry_peak': eligible & (relative >= 637.) & (relative < 640.),
|
||||
'segment10_exit_before_strong_input': eligible & (relative >= 642.7) & (relative < 643.852)}
|
||||
weight = np.minimum(np.diff(t, append=t[-1]+.01), .03)
|
||||
difference = abs(before[:, 0]-after[:, 0])
|
||||
cohorts = {}
|
||||
for name, mask in masks.items():
|
||||
if mask.any():
|
||||
cohorts[name] = {'cycles': int(mask.sum()), 'seconds': float(weight[mask].sum()),
|
||||
'changed_c0_cycles': int((difference[mask] > 1e-9).sum()),
|
||||
'mean_absolute_c0_change_m': float(np.average(difference[mask], weights=weight[mask])),
|
||||
'max_absolute_c0_change_m': float(difference[mask].max()),
|
||||
'v1_peak_absolute_c0_m': float(abs(before[mask, 0]).max()),
|
||||
'v2_peak_absolute_c0_m': float(abs(after[mask, 0]).max())}
|
||||
paired = eligible & (recorded['valid'] == 1) & (recorded['active'] == 1) & (abs(recorded['t']-t) < .005)
|
||||
actual = np.column_stack((recorded['c0'], recorded['c1']))
|
||||
error = abs(before[:, :2]-actual)
|
||||
report = {'scope': 'Fixed-input command replay only; no physical improvement or stability claim.', 'calibration_approved': False,
|
||||
'cycles': len(t), 'eligible_cycles': int(eligible.sum()), 'status_counts': dict(reasons),
|
||||
'c1_exactly_unchanged': True, 'same_validity': True, 'field_slew_zero_c2_c3_pass': True,
|
||||
'float32_can_round_trips': wire.count, 'cohorts': cohorts,
|
||||
'v1_reconstruction_vs_recorded': {'paired_cycles': int(paired.sum()),
|
||||
'within_one_quantum_cycles': int(np.all(error[paired] <= [.010001, .0005001], axis=1).sum()),
|
||||
'maximum_absolute_error_c0_c1': np.max(error[paired], axis=0).tolist()},
|
||||
'timing': 'Publication-time proxy, causal carState, exact consumed model; full SubMaster health unavailable.',
|
||||
'baseline': 'Current adapter with zero yaw retains v1 targets and actual-yaw sanity gate.',
|
||||
'source_rlog_sha256': sources, 'opendbc_head': DEPLOYMENT_OPENDBC,
|
||||
'source_sha256': {str(p): hashlib.sha256(p.read_bytes()).hexdigest() for p in (Path(__file__), Path(ford_model_action.__file__))}}
|
||||
output.mkdir(parents=True, exist_ok=True)
|
||||
np.savez_compressed(output/'commands.npz', t=relative, before=before, after=after, eligible=eligible,
|
||||
speed=cs['speed'], yaw=cs['yaw'], desired=controls['desired'], torque=cs['torque'])
|
||||
(output/'report.json').write_text(json.dumps(report, indent=2, allow_nan=False)+'\n')
|
||||
print(json.dumps({k: v for k, v in report.items() if k not in ('source_rlog_sha256', 'source_sha256')}, indent=2))
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument('rlog_directory', type=Path)
|
||||
parser.add_argument('--output', type=Path, required=True)
|
||||
args = parser.parse_args()
|
||||
run(args.rlog_directory, args.output)
|
||||
@@ -1,7 +1,7 @@
|
||||
"""Replay the selected core and its adapter on route90/95 original-time extracts.
|
||||
|
||||
The historical pass deliberately uses the archived eligibility mask to check
|
||||
command compatibility. The separate adapter pass reconstructs input eligibility
|
||||
The historical pass uses zero yaw and the archived eligibility mask to check
|
||||
v1 command compatibility. The separate v2 adapter pass reconstructs input eligibility
|
||||
from service records, never from candidate/baseline output validity. Neither
|
||||
pass scores counterfactual motion. Source extracts and archived reports are
|
||||
read-only; --output selects a separate destination.
|
||||
@@ -143,11 +143,11 @@ def run(directory, output):
|
||||
adapter_valid[i] = new_gate.valid
|
||||
reasons[adapter.diagnostics['status']] += 1
|
||||
wire.check(new_gate)
|
||||
# Isolate the adapter's fresh 10 ms engagement tick from the archived
|
||||
# harness, which used the preceding publication interval even on engage.
|
||||
# Current core receives actual yaw and a fresh 10 ms engagement tick;
|
||||
# the archived v1 construction above deliberately receives zero yaw.
|
||||
entry_dt = dt[i] if i > 0 and baseline['valid'][i-1] else .01
|
||||
expected_adapter = entry_clock_core.update(selected_model, controls['desired'][i], speed=cs['speed'][i], dt=entry_dt,
|
||||
active=bool(baseline['valid'][i]))
|
||||
yaw_rate=cs['yaw'][i], active=bool(baseline['valid'][i]))
|
||||
assert new_gate == expected_adapter, f'Unexplained adapter difference at cycle {i}'
|
||||
np.testing.assert_array_equal(valid, baseline['valid'])
|
||||
np.testing.assert_array_equal(commands[:, :2], baseline['action_heading'])
|
||||
@@ -190,7 +190,7 @@ def run(directory, output):
|
||||
'calibration_approved': False, 'executes_live_selector': False, 'cycles': len(t),
|
||||
'core_active_cycles': int(valid.sum()), 'core_exact_archived_match': True, 'cohorts_reproduced': True,
|
||||
'adapter_active_cycles': int(adapter_valid.sum()), 'adapter_status_counts': dict(reasons),
|
||||
'adapter_exact_match_with_fresh_engagement_dt': True,
|
||||
'adapter_matches_current_core_with_yaw_and_fresh_engagement_dt': True,
|
||||
'core_active_path_shorter_than_7m_cycles': int(np.sum(valid & (coverage < 7.))),
|
||||
'adapter_validity_differs_from_archive_cycles': int(np.sum(adapter_valid != valid)),
|
||||
'adapter_command_differs_from_archive_cycles': int(np.any(abs(adapted-commands) > 1e-9, axis=1).sum()),
|
||||
@@ -199,7 +199,7 @@ def run(directory, output):
|
||||
'timing': 'Original controls publication timestamps proxy computation time; repeated frames and gaps retained. No identified delay.',
|
||||
'eligibility': 'Adapter checks recorded services independently; full SubMaster health is unavailable. Core uses archived validity.',
|
||||
'reference': 'Recorded controlsState.desiredCurvature, already selected/limited. These two routes have no maneuver publications.',
|
||||
'host_yaw': 'Extract cs.yaw already equals -carState.yawRate. Used only for inherited finite/range gate, never feedback.',
|
||||
'host_yaw': 'Extract cs.yaw equals -carState.yawRate; v2 adapter uses it for bounded damping. Archived core pass uses zero yaw.',
|
||||
'cohorts': cohorts, 'workspace_head': revision(root), 'opendbc_import_head': revision(dependency),
|
||||
'opendbc_import_path': str(dependency),
|
||||
'source_sha256': {str(p.resolve()): hashlib.sha256(p.read_bytes()).hexdigest() for p in sources}}
|
||||
|
||||
@@ -56,6 +56,7 @@ def run(cycles, seed, output, opendbc_revision=PINNED_OPENDBC):
|
||||
offset, heading = float(rng.uniform(-8., 8.)), float(rng.uniform(-1.2, 1.2))
|
||||
speed = float(rng.uniform(.3, 55.))
|
||||
desired = float(rng.uniform(-.15, .15))
|
||||
yaw = float(rng.uniform(-3., 3.))
|
||||
dt = dt_values[i % len(dt_values)]
|
||||
active = i % 137 != 0
|
||||
valid = i % 211 != 0
|
||||
@@ -64,14 +65,21 @@ def run(cycles, seed, output, opendbc_revision=PINNED_OPENDBC):
|
||||
model, mirror = line(offset, heading), line(-offset, -heading)
|
||||
if i % 401 == 0:
|
||||
model.position.y[4] = mirror.position.y[4] = math.nan
|
||||
out = controller.update(model, desired, speed=speed, dt=dt, active=active, valid=valid)
|
||||
other = mirrored.update(mirror, -desired, speed=speed, dt=dt, active=active, valid=valid)
|
||||
out = controller.update(model, desired, speed=speed, dt=dt, yaw_rate=yaw, active=active, valid=valid)
|
||||
other = mirrored.update(mirror, -desired, speed=speed, dt=dt, yaw_rate=-yaw, active=active, valid=valid)
|
||||
expected_valid = active and valid and dt <= .1 and i % 401 != 0
|
||||
assert out.valid == other.valid == expected_valid
|
||||
previous = np.array([c0, c1])
|
||||
if expected_valid:
|
||||
target = (max(-5.11, min(5.11, offset+7.*math.sin(heading))), max(-.5, min(.5, max(7., speed)*desired)))
|
||||
c0 += max(-4.*dt, min(4.*dt, target[0]-c0))
|
||||
# Independent piecewise scalar oracle; do not call the production helper.
|
||||
offset_target = target[0]
|
||||
if offset_target > 0. and yaw > 0.:
|
||||
offset_target = max(0., offset_target-1.4*max(0., yaw-max(0., speed*desired)-.02))
|
||||
elif offset_target < 0. and yaw < 0.:
|
||||
offset_target = min(0., offset_target+1.4*max(0., -yaw-max(0., -speed*desired)-.02))
|
||||
assert abs(offset_target) <= abs(target[0]) and offset_target*target[0] >= 0.
|
||||
c0 += max(-4.*dt, min(4.*dt, offset_target-c0))
|
||||
c1 += max(-.5*dt, min(.5*dt, target[1]-c1))
|
||||
step = abs(np.array([controller.c0, controller.c1])-previous)
|
||||
assert (step <= np.array(rates)*dt+1e-10).all()
|
||||
@@ -105,6 +113,7 @@ def run(cycles, seed, output, opendbc_revision=PINNED_OPENDBC):
|
||||
report = {'seed': seed, 'random_cycles': cycles, 'mirrored_core_updates': cycles,
|
||||
'invalid_or_inactive_resets': resets, 'field_boundary_cases': boundary_cases,
|
||||
'float32_can_round_trips': wire.count, 'analytic_targets_scalar_slew_and_mirror_checks_pass': True,
|
||||
'bounded_excess_yaw_damping_checked': True,
|
||||
'direct_raw_float32_packing_matches_host_output': True, 'max_continuous_step_c0_c1': max_continuous_step.tolist(),
|
||||
'calibration_approved': False, 'scope': 'Numerical construction only; no PSCM response or closed-loop performance claims.',
|
||||
'opendbc_import_head': revision(dependency),
|
||||
|
||||
Reference in New Issue
Block a user