diff --git a/docs/ford_model_action_drive_test.md b/docs/ford_model_action_drive_test.md index 9e5d453261..1e6cb59653 100644 --- a/docs/ford_model_action_drive_test.md +++ b/docs/ford_model_action_drive_test.md @@ -1,21 +1,22 @@ # Ford model-point drive-test branch The default-off **Selected-Action Path Tracking (Experimental)** Sunnylink toggle -now selects the v7 model-point candidate on the **Ford CAN FD F-150 Lightning**. +now selects the C1 early-release candidate on the **Ford CAN FD F-150 Lightning**. The stored key remains `FordModelActionController`; an already enabled setting selects this revision after updating and completing an offroad-to-onroad cycle. The controller reads **model lateral position and model heading at the same point**. Start with the model's predicted distance at one second, enforce the existing seven-metre minimum, and hold the available endpoint when necessary. -C0 is that point's lateral position in metres; C1 is its unwrapped heading in -radians. C2 and C3 stay zero. The 150 ms yaw-based forecast and the reconstruction -of heading from selected curvature are removed. +C0 is that point's lateral position in metres. C1 starts from its unwrapped +heading and is bounded toward zero using the model's terminal spatial curvature +at the same point. This asks for earlier release when the path straightens +ahead. C2 and C3 stay zero. See [the exact rule and its authority tradeoff](ford_model_release.md). This point choice is an engineering guess, not an identified Ford reference or PSCM calibration. `calibration_approved=false`: offline tests do not establish physical tracking, turn-exit behavior, or stability across different PSCMs. -See [the model-point decision and validation](ford_model_points.md). +The [v7 model-point decision and validation](ford_model_points.md) is historical. ## Select and restore @@ -29,9 +30,10 @@ See [the model-point decision and validation](ford_model_points.md). The startup event `Ford path controller selected` reports `FordModelActionController`. Periodic `Ford C2-free path tracking` events report -`hypothesis=model-pose-one-second-v7`, `pose_source=model`, `preview_time_s=1.0` +`hypothesis=model-pose-terminal-c1-v1`, `pose_source=model`, `preview_time_s=1.0` and `minimum_station_m=7.0`, plus input ages, slew state and the command tuple. -Selected desired curvature is still logged, but no longer constructs C0/C1. +Active diagnostics also report `c1_release=terminal_spatial_curvature`. +Selected desired curvature is still logged, but does not construct C0/C1. Turning the toggle off and completing another offroad-to-onroad cycle restores **PSCM Coefficient Observer** if selected, otherwise the original Ford path @@ -53,7 +55,7 @@ candidate at 20 Hz. Float32 publication, host-to-wire negation, packing and Panda safety are unchanged. The opendbc pin remains `87ca78e6e641eefb2d654f260a6ab08df3058bd5`. -Normal operation uses the model's point directly. The upstream scalar curvature +Normal operation uses the model point with the one-sided C1 release bound. The upstream scalar curvature and its clipping still exist for logging/other controllers, but no longer bound this candidate's heading target. Its C0/C1 field caps and slew still apply; passing Panda TX checks does not establish an actual vehicle acceleration bound. @@ -73,7 +75,7 @@ python -m pytest -q openpilot/selfdrive/controls/tests/test_ford*.py tools/ford_ python -m tools.ford_pscm_lab.stress_model_action --cycles 200000 --seed 20260908 --opendbc-revision 87ca78e6e641eefb2d654f260a6ab08df3058bd5 --output .cache/ford_model_points/stress.json ``` -Historical v1–v6 validation files retain their original source hashes and apply +Historical v1–v7 validation files retain their original source hashes and apply to those revisions. In particular, `ford_model_action_measured_pose_validation.json` describes v6, not the current model-point mapping. The hardware build and device boot are not performed by these offline checks. Pushing a branch does not update diff --git a/docs/ford_model_points.md b/docs/ford_model_points.md index 18c7fcab02..be7c94f806 100644 --- a/docs/ford_model_points.md +++ b/docs/ford_model_points.md @@ -1,5 +1,9 @@ # Ford model-point candidate v7 +This is the historical baseline at `4bd841ecc`. The driving branch now uses +the [C1 early-release experiment](ford_model_release.md). Results below remain +bound to their original sources and do not validate the newer request mapping. + ## Decision Use the model's own position and orientation at one shared path point. This diff --git a/docs/ford_model_release.md b/docs/ford_model_release.md new file mode 100644 index 0000000000..58107babe5 --- /dev/null +++ b/docs/ford_model_release.md @@ -0,0 +1,101 @@ +# Ford C1 early-release experiment + +The a9 recording contains holds followed by rapid steering release while C0/C1 +requests were already decreasing. Several occur without a PSCM limit flag or +detected driver input. The experiment changes C1 alone to ask for less turning +when the model's path straightens ahead. It does not establish which internal +PSCM mechanism caused those holds. + +## Request mapping + +Keep the v7 sample station `s`: model arc distance at one second, with the +existing seven-metre minimum and endpoint hold. Let `psi` be unwrapped model +heading at that station and `psi0` its initial heading. Using the same model +segment enclosing that point: + +``` +terminal_curvature = change in heading / change in arc distance +release_heading = psi0 + s * terminal_curvature +C0_target = model lateral position at s +C1_target = release_heading bounded between zero and psi +C2 = C3 = 0 +``` + +The C1 interval respects the sign of `psi`. The correction cannot amplify the +raw heading target or manufacture an opposite-direction target. A real model +sign reversal continues through the normal slew. At an exact model knot the +incoming segment supplies the slope. A duplicate selected station, zero span, +or nonfinite derived slope retains the original heading. + +Heading linear in the sampled cumulative arc distance, and constant heading, +retain the original mapping. Arc distance uses model-point chords, so even a +true circle can differ slightly under uneven sampling; exact preservation is +a statement about the discrete heading slope, not every sampled physical arc. +When the terminal curvature is gentler than the average over the preview, C1 +can decrease sooner; it can reach zero even while the future pose still has a +nonzero heading. C0 continues requesting that future lateral position. + +There is no temporal turn-mode latch, new strength multiplier, measured-yaw +correction, fitted PSCM model, or foreign firmware command cap. Only the two +existing C0/C1 slew positions persist. Limits remain ±5.11 m / ±0.5 rad and +4 m/s / 0.5 rad/s; transmission remains 20 Hz. + +## Physical question and tradeoff + +The question is whether unloading C1 sooner makes the measured wheel release +sooner, without unacceptable loss of turning authority. Constant discrete +heading slope is preserved; not every real turn-in is guaranteed unchanged. A model +point can already lie on the straight after a corner while the truck still +needs heading change to reach it. This rule can then remove useful C1. C0 and +the PSCM's physical response determine the result, which passive replay cannot +establish. + +The prior ML3V firmware's internal contribution limits are not treated as +Lightning limits. `LimitReached` is not used as a release trigger. No claim is +made that this fixes the a9 hang, tracking error, or stability across PSCMs. +`calibration_approved=false` remains. + +## Recorded command comparison + +The a9 comparison uses 7,199 original-clock models and 35,775 control cycles +from six recorded segments. Candidate C0 is exactly equal to baseline C0 at +raw target, publication, and sampled send times. Raw C1 never exceeds the +original heading magnitude or invents a reversal. Independent slew histories +can let the candidate reach a genuine opposite-direction target sooner, so +published C1 magnitude is not universally smaller on reversal. + +Entry and plateau changes are included, rather than scoring only exits: + +| Recorded window | Baseline mean absolute C1 | Candidate mean absolute C1 | +| --- | ---: | ---: | +| Right entry, 38:47–38:50 | 0.13643 rad | 0.12561 rad | +| Large left entry, 40:01–40:05 | unchanged | unchanged | +| Right plateau, 45:26–45:28.4 | 0.11442 rad | 0.10506 rad | +| Left entry, 45:34–45:36 | 0.16580 rad | 0.15962 rad | + +C1 crosses a common 0.1 rad level about 0.80–0.85 seconds earlier in three +selected exits. The level is a comparison marker, not a PSCM limit. On the +right exits near 38:51 and 45:29, it stops requesting the original turn +direction 0.454 and 0.356 seconds earlier. These are proposed command times +on frozen inputs, not measured improvements in wheel release. + +The baseline replay agrees with recorded publication within one wire quantum +after cache-boundary warmup; logged publication times do not provide every +internal controller-entry time. Original clocks, eligibility, and that +reconstruction tolerance are retained in the validation record. + +## Selection and verification + +Use the existing default-off Sunnylink **Selected-Action Path Tracking +(Experimental)** toggle (`FordModelActionController`). An already enabled +setting selects this revision after updating and an offroad-to-onroad cycle. +The diagnostic identity is `model-pose-terminal-c1-v1`; active diagnostics also +report `c1_release=terminal_spatial_curvature`. Turning the toggle off restores +the previous nonexperimental selection after another offroad-to-onroad cycle. + +See the [drive-test guide](ford_model_action_drive_test.md) and the +[validation record](ford_model_release_validation.json). Unit/integration checks +cover command construction, bounds, reset, sender cadence, and encoding. Route +comparisons report changes in commands, including entry attenuation; they do +not predict wheel motion under the changed commands. The next recording must +compare desired versus actual steering and release timing on the actual PSCM. diff --git a/docs/ford_model_release_validation.json b/docs/ford_model_release_validation.json new file mode 100644 index 0000000000..97c9e40ed5 --- /dev/null +++ b/docs/ford_model_release_validation.json @@ -0,0 +1,1120 @@ +{ + "experiment": "model-pose-terminal-c1-v1", + "baseline": "4bd841eccd116f535a2ee99b920a631d1718f0cc", + "opendbc": "87ca78e6e641eefb2d654f260a6ab08df3058bd5", + "scope": "Experimental command mapping and software validation only. No physical release, tracking or universal PSCM stability claim.", + "calibration_approved": false, + "tests": { + "ford_controls_sender_safety_lab": { + "passed": 487, + "skipped_inapplicable": 178, + "subtests_passed": 9146 + }, + "sunnylink_and_selection": { + "passed": 92, + "overlap_note": "Selection tests also included in Ford group; do not add group counts as unique cases." + }, + "new_replay_tool": { + "passed": 11 + }, + "new_geometry_against_archived_baseline": { + "expected_contract_failures": 6, + "preservation_passes": 19 + }, + "ruff": "passed", + "ty_controller": "passed", + "generated_sunnylink_schema": "matches source", + "independent_review": "No remaining blocker; physical response remains unverified." + }, + "numerical_stress": { + "seed": 20260908, + "random_cycles": 10000, + "mirrored_core_updates": 10000, + "invalid_or_inactive_resets": 176, + "field_boundary_cases": 18138, + "float32_can_round_trips": 28138, + "analytic_targets_scalar_slew_and_mirror_checks_pass": true, + "valid_yaw_does_not_affect_targets_checked": true, + "shared_model_point_checked": true, + "direct_raw_float32_packing_matches_host_output": true, + "max_continuous_step_c0_c1": [ + 0.400000000000001, + 0.05000000000000002 + ], + "calibration_approved": false, + "scope": "Numerical construction only; no PSCM response or closed-loop performance claims.", + "opendbc_import_head": "87ca78e6e641eefb2d654f260a6ab08df3058bd5" + }, + "command_replay": { + "scope": "Command-only comparison on exact consumed recorded models with frozen recorded eligibility and motion; no physical steering prediction.", + "baseline_revision": "4bd841eccd116f535a2ee99b920a631d1718f0cc", + "baseline_source_sha256": "e7502ab68d04aef52edcffcef4b1d42c04e9008ab6892789aac3a865c100cb6d", + "clock_source": "Cache row3 contains original shared model position/orientation times; omitted clocks are rejected.", + "models": 7199, + "control_cycles": 35775, + "eligible_cycles": 28298, + "three_second_cache_boundary_warmup_cycles": 896, + "baseline_matches_recorded_within_one_quantum_after_warmup": true, + "baseline_timing_limit": "Replay uses logged control-message times; internal core-entry times are not logged on every cycle. Baseline agreement is therefore checked within one DBC quantum, not asserted bit-for-bit.", + "baseline_nonidentical_published_values_after_warmup": 201, + "baseline_max_recorded_error_after_warmup": [ + 0.010000228881835938, + 0.000500023365020752 + ], + "exact_c0_equality_raw_published_and_scheduled": true, + "raw_c1_never_amplifies_or_reverses": true, + "float32_CAN_roundtrips": 71550, + "finite_caps_slew_zero_c2_c3_checks": true, + "recorded_schedule_samples": 7156, + "recorded_median_CAN_period_s": 0.050188601500053664, + "scheduled_command_scope": "Latest proposed publication at each original send timestamp, restricted to <=20ms old; existing sender behavior is unchanged.", + "crossing_method": "First command into the original turn at/below a diagnostic threshold plus one DBC quantum. Direction is the sign of the largest absolute valid baseline value per field/window and is shared by both versions; opposite-sign commands count as release. Downcross requires an immediately preceding above-threshold valid command. The 0.035 rad level is a comparison marker, not an established hardware limit. No physical-response score.", + "windows": { + "right38_entry": { + "window_s": [ + 2327.0, + 2330.0 + ], + "cycles": 298, + "eligible_cycles": 298, + "baseline_mean_abs_c1": 0.13643120805369124, + "candidate_mean_abs_c1": 0.12561241610738255, + "c1_reduced_more_than_one_quantum_percent": 27.516778523489933, + "c1_increased_more_than_one_quantum_percent": 0.0, + "largest_c1_magnitude_reduction_rad": 0.07199999999999995, + "exact_c0_equality": true, + "published_crossings": { + "c0": [ + { + "baseline": { + "threshold": 0.5, + "quantum_tolerance": 0.01, + "original_turn_direction": 1.0, + "starts_at_or_below": true, + "first_at_or_below_s": 2327.0009226849998, + "first_downcross_s": null + }, + "candidate": { + "threshold": 0.5, + "quantum_tolerance": 0.01, + "original_turn_direction": 1.0, + "starts_at_or_below": true, + "first_at_or_below_s": 2327.0009226849998, + "first_downcross_s": null + } + }, + { + "baseline": { + "threshold": 0.05, + "quantum_tolerance": 0.01, + "original_turn_direction": 1.0, + "starts_at_or_below": true, + "first_at_or_below_s": 2327.0009226849998, + "first_downcross_s": null + }, + "candidate": { + "threshold": 0.05, + "quantum_tolerance": 0.01, + "original_turn_direction": 1.0, + "starts_at_or_below": true, + "first_at_or_below_s": 2327.0009226849998, + "first_downcross_s": null + } + }, + { + "baseline": { + "threshold": 0.0, + "quantum_tolerance": 0.01, + "original_turn_direction": 1.0, + "starts_at_or_below": false, + "first_at_or_below_s": null, + "first_downcross_s": null + }, + "candidate": { + "threshold": 0.0, + "quantum_tolerance": 0.01, + "original_turn_direction": 1.0, + "starts_at_or_below": false, + "first_at_or_below_s": null, + "first_downcross_s": null + } + } + ], + "c1": [ + { + "baseline": { + "threshold": 0.1, + "quantum_tolerance": 0.0005, + "original_turn_direction": 1.0, + "starts_at_or_below": true, + "first_at_or_below_s": 2327.0009226849998, + "first_downcross_s": null + }, + "candidate": { + "threshold": 0.1, + "quantum_tolerance": 0.0005, + "original_turn_direction": 1.0, + "starts_at_or_below": true, + "first_at_or_below_s": 2327.0009226849998, + "first_downcross_s": null + } + }, + { + "baseline": { + "threshold": 0.035, + "quantum_tolerance": 0.0005, + "original_turn_direction": 1.0, + "starts_at_or_below": true, + "first_at_or_below_s": 2327.0009226849998, + "first_downcross_s": null + }, + "candidate": { + "threshold": 0.035, + "quantum_tolerance": 0.0005, + "original_turn_direction": 1.0, + "starts_at_or_below": true, + "first_at_or_below_s": 2327.0009226849998, + "first_downcross_s": null + } + }, + { + "baseline": { + "threshold": 0.0, + "quantum_tolerance": 0.0005, + "original_turn_direction": 1.0, + "starts_at_or_below": false, + "first_at_or_below_s": null, + "first_downcross_s": null + }, + "candidate": { + "threshold": 0.0, + "quantum_tolerance": 0.0005, + "original_turn_direction": 1.0, + "starts_at_or_below": false, + "first_at_or_below_s": null, + "first_downcross_s": null + } + } + ] + } + }, + "right38_release": { + "window_s": [ + 2330.0, + 2332.0 + ], + "cycles": 199, + "eligible_cycles": 199, + "baseline_mean_abs_c1": 0.11825628140703515, + "candidate_mean_abs_c1": 0.051060301507537695, + "c1_reduced_more_than_one_quantum_percent": 95.97989949748744, + "c1_increased_more_than_one_quantum_percent": 0.0, + "largest_c1_magnitude_reduction_rad": 0.08799999999999997, + "exact_c0_equality": true, + "published_crossings": { + "c0": [ + { + "baseline": { + "threshold": 0.5, + "quantum_tolerance": 0.01, + "original_turn_direction": 1.0, + "starts_at_or_below": false, + "first_at_or_below_s": 2331.636798378, + "first_downcross_s": 2331.636798378 + }, + "candidate": { + "threshold": 0.5, + "quantum_tolerance": 0.01, + "original_turn_direction": 1.0, + "starts_at_or_below": false, + "first_at_or_below_s": 2331.636798378, + "first_downcross_s": 2331.636798378 + } + }, + { + "baseline": { + "threshold": 0.05, + "quantum_tolerance": 0.01, + "original_turn_direction": 1.0, + "starts_at_or_below": false, + "first_at_or_below_s": 2331.899450648, + "first_downcross_s": 2331.899450648 + }, + "candidate": { + "threshold": 0.05, + "quantum_tolerance": 0.01, + "original_turn_direction": 1.0, + "starts_at_or_below": false, + "first_at_or_below_s": 2331.899450648, + "first_downcross_s": 2331.899450648 + } + }, + { + "baseline": { + "threshold": 0.0, + "quantum_tolerance": 0.01, + "original_turn_direction": 1.0, + "starts_at_or_below": false, + "first_at_or_below_s": 2331.927862663, + "first_downcross_s": 2331.927862663 + }, + "candidate": { + "threshold": 0.0, + "quantum_tolerance": 0.01, + "original_turn_direction": 1.0, + "starts_at_or_below": false, + "first_at_or_below_s": 2331.927862663, + "first_downcross_s": 2331.927862663 + } + } + ], + "c1": [ + { + "baseline": { + "threshold": 0.1, + "quantum_tolerance": 0.0005, + "original_turn_direction": 1.0, + "starts_at_or_below": false, + "first_at_or_below_s": 2331.275079763, + "first_downcross_s": 2331.275079763 + }, + "candidate": { + "threshold": 0.1, + "quantum_tolerance": 0.0005, + "original_turn_direction": 1.0, + "starts_at_or_below": false, + "first_at_or_below_s": 2330.420476894, + "first_downcross_s": 2330.420476894 + } + }, + { + "baseline": { + "threshold": 0.035, + "quantum_tolerance": 0.0005, + "original_turn_direction": 1.0, + "starts_at_or_below": false, + "first_at_or_below_s": 2331.72769878, + "first_downcross_s": 2331.72769878 + }, + "candidate": { + "threshold": 0.035, + "quantum_tolerance": 0.0005, + "original_turn_direction": 1.0, + "starts_at_or_below": false, + "first_at_or_below_s": 2331.12481298, + "first_downcross_s": 2331.12481298 + } + }, + { + "baseline": { + "threshold": 0.0, + "quantum_tolerance": 0.0005, + "original_turn_direction": 1.0, + "starts_at_or_below": false, + "first_at_or_below_s": 2331.927862663, + "first_downcross_s": 2331.927862663 + }, + "candidate": { + "threshold": 0.0, + "quantum_tolerance": 0.0005, + "original_turn_direction": 1.0, + "starts_at_or_below": false, + "first_at_or_below_s": 2331.473902151, + "first_downcross_s": 2331.473902151 + } + } + ] + } + }, + "left40_entry": { + "window_s": [ + 2401.0, + 2405.0 + ], + "cycles": 397, + "eligible_cycles": 397, + "baseline_mean_abs_c1": 0.23517632241813605, + "candidate_mean_abs_c1": 0.23517632241813605, + "c1_reduced_more_than_one_quantum_percent": 0.0, + "c1_increased_more_than_one_quantum_percent": 0.0, + "largest_c1_magnitude_reduction_rad": 0.0, + "exact_c0_equality": true, + "published_crossings": { + "c0": [ + { + "baseline": { + "threshold": 0.5, + "quantum_tolerance": 0.01, + "original_turn_direction": -1.0, + "starts_at_or_below": true, + "first_at_or_below_s": 2401.00047902, + "first_downcross_s": null + }, + "candidate": { + "threshold": 0.5, + "quantum_tolerance": 0.01, + "original_turn_direction": -1.0, + "starts_at_or_below": true, + "first_at_or_below_s": 2401.00047902, + "first_downcross_s": null + } + }, + { + "baseline": { + "threshold": 0.05, + "quantum_tolerance": 0.01, + "original_turn_direction": -1.0, + "starts_at_or_below": false, + "first_at_or_below_s": null, + "first_downcross_s": null + }, + "candidate": { + "threshold": 0.05, + "quantum_tolerance": 0.01, + "original_turn_direction": -1.0, + "starts_at_or_below": false, + "first_at_or_below_s": null, + "first_downcross_s": null + } + }, + { + "baseline": { + "threshold": 0.0, + "quantum_tolerance": 0.01, + "original_turn_direction": -1.0, + "starts_at_or_below": false, + "first_at_or_below_s": null, + "first_downcross_s": null + }, + "candidate": { + "threshold": 0.0, + "quantum_tolerance": 0.01, + "original_turn_direction": -1.0, + "starts_at_or_below": false, + "first_at_or_below_s": null, + "first_downcross_s": null + } + } + ], + "c1": [ + { + "baseline": { + "threshold": 0.1, + "quantum_tolerance": 0.0005, + "original_turn_direction": -1.0, + "starts_at_or_below": true, + "first_at_or_below_s": 2401.00047902, + "first_downcross_s": null + }, + "candidate": { + "threshold": 0.1, + "quantum_tolerance": 0.0005, + "original_turn_direction": -1.0, + "starts_at_or_below": true, + "first_at_or_below_s": 2401.00047902, + "first_downcross_s": null + } + }, + { + "baseline": { + "threshold": 0.035, + "quantum_tolerance": 0.0005, + "original_turn_direction": -1.0, + "starts_at_or_below": false, + "first_at_or_below_s": null, + "first_downcross_s": null + }, + "candidate": { + "threshold": 0.035, + "quantum_tolerance": 0.0005, + "original_turn_direction": -1.0, + "starts_at_or_below": false, + "first_at_or_below_s": null, + "first_downcross_s": null + } + }, + { + "baseline": { + "threshold": 0.0, + "quantum_tolerance": 0.0005, + "original_turn_direction": -1.0, + "starts_at_or_below": false, + "first_at_or_below_s": null, + "first_downcross_s": null + }, + "candidate": { + "threshold": 0.0, + "quantum_tolerance": 0.0005, + "original_turn_direction": -1.0, + "starts_at_or_below": false, + "first_at_or_below_s": null, + "first_downcross_s": null + } + } + ] + } + }, + "left40_release": { + "window_s": [ + 2405.0, + 2408.0 + ], + "cycles": 298, + "eligible_cycles": 298, + "baseline_mean_abs_c1": 0.27877852348993293, + "candidate_mean_abs_c1": 0.19871644295302013, + "c1_reduced_more_than_one_quantum_percent": 83.89261744966443, + "c1_increased_more_than_one_quantum_percent": 0.0, + "largest_c1_magnitude_reduction_rad": 0.16300000000000003, + "exact_c0_equality": true, + "published_crossings": { + "c0": [ + { + "baseline": { + "threshold": 0.5, + "quantum_tolerance": 0.01, + "original_turn_direction": -1.0, + "starts_at_or_below": false, + "first_at_or_below_s": 2407.739911588, + "first_downcross_s": 2407.739911588 + }, + "candidate": { + "threshold": 0.5, + "quantum_tolerance": 0.01, + "original_turn_direction": -1.0, + "starts_at_or_below": false, + "first_at_or_below_s": 2407.739911588, + "first_downcross_s": 2407.739911588 + } + }, + { + "baseline": { + "threshold": 0.05, + "quantum_tolerance": 0.01, + "original_turn_direction": -1.0, + "starts_at_or_below": false, + "first_at_or_below_s": null, + "first_downcross_s": null + }, + "candidate": { + "threshold": 0.05, + "quantum_tolerance": 0.01, + "original_turn_direction": -1.0, + "starts_at_or_below": false, + "first_at_or_below_s": null, + "first_downcross_s": null + } + }, + { + "baseline": { + "threshold": 0.0, + "quantum_tolerance": 0.01, + "original_turn_direction": -1.0, + "starts_at_or_below": false, + "first_at_or_below_s": null, + "first_downcross_s": null + }, + "candidate": { + "threshold": 0.0, + "quantum_tolerance": 0.01, + "original_turn_direction": -1.0, + "starts_at_or_below": false, + "first_at_or_below_s": null, + "first_downcross_s": null + } + } + ], + "c1": [ + { + "baseline": { + "threshold": 0.1, + "quantum_tolerance": 0.0005, + "original_turn_direction": -1.0, + "starts_at_or_below": false, + "first_at_or_below_s": 2407.62040377, + "first_downcross_s": 2407.62040377 + }, + "candidate": { + "threshold": 0.1, + "quantum_tolerance": 0.0005, + "original_turn_direction": -1.0, + "starts_at_or_below": false, + "first_at_or_below_s": 2406.804382573, + "first_downcross_s": 2406.804382573 + } + }, + { + "baseline": { + "threshold": 0.035, + "quantum_tolerance": 0.0005, + "original_turn_direction": -1.0, + "starts_at_or_below": false, + "first_at_or_below_s": 2407.889943961, + "first_downcross_s": 2407.889943961 + }, + "candidate": { + "threshold": 0.035, + "quantum_tolerance": 0.0005, + "original_turn_direction": -1.0, + "starts_at_or_below": false, + "first_at_or_below_s": 2407.078165522, + "first_downcross_s": 2407.078165522 + } + }, + { + "baseline": { + "threshold": 0.0, + "quantum_tolerance": 0.0005, + "original_turn_direction": -1.0, + "starts_at_or_below": false, + "first_at_or_below_s": null, + "first_downcross_s": null + }, + "candidate": { + "threshold": 0.0, + "quantum_tolerance": 0.0005, + "original_turn_direction": -1.0, + "starts_at_or_below": false, + "first_at_or_below_s": 2407.328151231, + "first_downcross_s": 2407.328151231 + } + } + ] + } + }, + "right45_plateau": { + "window_s": [ + 2726.0, + 2728.4 + ], + "cycles": 238, + "eligible_cycles": 238, + "baseline_mean_abs_c1": 0.11442436974789912, + "candidate_mean_abs_c1": 0.10505882352941175, + "c1_reduced_more_than_one_quantum_percent": 76.89075630252101, + "c1_increased_more_than_one_quantum_percent": 0.0, + "largest_c1_magnitude_reduction_rad": 0.02949999999999997, + "exact_c0_equality": true, + "published_crossings": { + "c0": [ + { + "baseline": { + "threshold": 0.5, + "quantum_tolerance": 0.01, + "original_turn_direction": 1.0, + "starts_at_or_below": false, + "first_at_or_below_s": null, + "first_downcross_s": null + }, + "candidate": { + "threshold": 0.5, + "quantum_tolerance": 0.01, + "original_turn_direction": 1.0, + "starts_at_or_below": false, + "first_at_or_below_s": null, + "first_downcross_s": null + } + }, + { + "baseline": { + "threshold": 0.05, + "quantum_tolerance": 0.01, + "original_turn_direction": 1.0, + "starts_at_or_below": false, + "first_at_or_below_s": null, + "first_downcross_s": null + }, + "candidate": { + "threshold": 0.05, + "quantum_tolerance": 0.01, + "original_turn_direction": 1.0, + "starts_at_or_below": false, + "first_at_or_below_s": null, + "first_downcross_s": null + } + }, + { + "baseline": { + "threshold": 0.0, + "quantum_tolerance": 0.01, + "original_turn_direction": 1.0, + "starts_at_or_below": false, + "first_at_or_below_s": null, + "first_downcross_s": null + }, + "candidate": { + "threshold": 0.0, + "quantum_tolerance": 0.01, + "original_turn_direction": 1.0, + "starts_at_or_below": false, + "first_at_or_below_s": null, + "first_downcross_s": null + } + } + ], + "c1": [ + { + "baseline": { + "threshold": 0.1, + "quantum_tolerance": 0.0005, + "original_turn_direction": 1.0, + "starts_at_or_below": false, + "first_at_or_below_s": null, + "first_downcross_s": null + }, + "candidate": { + "threshold": 0.1, + "quantum_tolerance": 0.0005, + "original_turn_direction": 1.0, + "starts_at_or_below": false, + "first_at_or_below_s": 2727.753763263, + "first_downcross_s": 2727.753763263 + } + }, + { + "baseline": { + "threshold": 0.035, + "quantum_tolerance": 0.0005, + "original_turn_direction": 1.0, + "starts_at_or_below": false, + "first_at_or_below_s": null, + "first_downcross_s": null + }, + "candidate": { + "threshold": 0.035, + "quantum_tolerance": 0.0005, + "original_turn_direction": 1.0, + "starts_at_or_below": false, + "first_at_or_below_s": null, + "first_downcross_s": null + } + }, + { + "baseline": { + "threshold": 0.0, + "quantum_tolerance": 0.0005, + "original_turn_direction": 1.0, + "starts_at_or_below": false, + "first_at_or_below_s": null, + "first_downcross_s": null + }, + "candidate": { + "threshold": 0.0, + "quantum_tolerance": 0.0005, + "original_turn_direction": 1.0, + "starts_at_or_below": false, + "first_at_or_below_s": null, + "first_downcross_s": null + } + } + ] + } + }, + "right45_release": { + "window_s": [ + 2728.4, + 2730.0 + ], + "cycles": 159, + "eligible_cycles": 159, + "baseline_mean_abs_c1": 0.05170440251572327, + "candidate_mean_abs_c1": 0.023647798742138362, + "c1_reduced_more_than_one_quantum_percent": 77.9874213836478, + "c1_increased_more_than_one_quantum_percent": 0.628930817610063, + "largest_c1_magnitude_reduction_rad": 0.05099999999999999, + "exact_c0_equality": true, + "published_crossings": { + "c0": [ + { + "baseline": { + "threshold": 0.5, + "quantum_tolerance": 0.01, + "original_turn_direction": 1.0, + "starts_at_or_below": false, + "first_at_or_below_s": 2729.053416002, + "first_downcross_s": 2729.053416002 + }, + "candidate": { + "threshold": 0.5, + "quantum_tolerance": 0.01, + "original_turn_direction": 1.0, + "starts_at_or_below": false, + "first_at_or_below_s": 2729.053416002, + "first_downcross_s": 2729.053416002 + } + }, + { + "baseline": { + "threshold": 0.05, + "quantum_tolerance": 0.01, + "original_turn_direction": 1.0, + "starts_at_or_below": false, + "first_at_or_below_s": 2729.918448983, + "first_downcross_s": 2729.918448983 + }, + "candidate": { + "threshold": 0.05, + "quantum_tolerance": 0.01, + "original_turn_direction": 1.0, + "starts_at_or_below": false, + "first_at_or_below_s": 2729.918448983, + "first_downcross_s": 2729.918448983 + } + }, + { + "baseline": { + "threshold": 0.0, + "quantum_tolerance": 0.01, + "original_turn_direction": 1.0, + "starts_at_or_below": false, + "first_at_or_below_s": 2729.927027474, + "first_downcross_s": 2729.927027474 + }, + "candidate": { + "threshold": 0.0, + "quantum_tolerance": 0.01, + "original_turn_direction": 1.0, + "starts_at_or_below": false, + "first_at_or_below_s": 2729.927027474, + "first_downcross_s": 2729.927027474 + } + } + ], + "c1": [ + { + "baseline": { + "threshold": 0.1, + "quantum_tolerance": 0.0005, + "original_turn_direction": 1.0, + "starts_at_or_below": false, + "first_at_or_below_s": 2728.460351498, + "first_downcross_s": 2728.460351498 + }, + "candidate": { + "threshold": 0.1, + "quantum_tolerance": 0.0005, + "original_turn_direction": 1.0, + "starts_at_or_below": true, + "first_at_or_below_s": 2728.40779688, + "first_downcross_s": null + } + }, + { + "baseline": { + "threshold": 0.035, + "quantum_tolerance": 0.0005, + "original_turn_direction": 1.0, + "starts_at_or_below": false, + "first_at_or_below_s": 2729.456606776, + "first_downcross_s": 2729.456606776 + }, + "candidate": { + "threshold": 0.035, + "quantum_tolerance": 0.0005, + "original_turn_direction": 1.0, + "starts_at_or_below": false, + "first_at_or_below_s": 2728.861576864, + "first_downcross_s": 2728.861576864 + } + }, + { + "baseline": { + "threshold": 0.0, + "quantum_tolerance": 0.0005, + "original_turn_direction": 1.0, + "starts_at_or_below": false, + "first_at_or_below_s": 2729.659822435, + "first_downcross_s": 2729.659822435 + }, + "candidate": { + "threshold": 0.0, + "quantum_tolerance": 0.0005, + "original_turn_direction": 1.0, + "starts_at_or_below": false, + "first_at_or_below_s": 2729.303377643, + "first_downcross_s": 2729.303377643 + } + } + ] + } + }, + "left45_entry": { + "window_s": [ + 2734.0, + 2736.0 + ], + "cycles": 198, + "eligible_cycles": 198, + "baseline_mean_abs_c1": 0.1658005050505051, + "candidate_mean_abs_c1": 0.1596237373737374, + "c1_reduced_more_than_one_quantum_percent": 37.878787878787875, + "c1_increased_more_than_one_quantum_percent": 0.0, + "largest_c1_magnitude_reduction_rad": 0.026499999999999968, + "exact_c0_equality": true, + "published_crossings": { + "c0": [ + { + "baseline": { + "threshold": 0.5, + "quantum_tolerance": 0.01, + "original_turn_direction": -1.0, + "starts_at_or_below": false, + "first_at_or_below_s": null, + "first_downcross_s": null + }, + "candidate": { + "threshold": 0.5, + "quantum_tolerance": 0.01, + "original_turn_direction": -1.0, + "starts_at_or_below": false, + "first_at_or_below_s": null, + "first_downcross_s": null + } + }, + { + "baseline": { + "threshold": 0.05, + "quantum_tolerance": 0.01, + "original_turn_direction": -1.0, + "starts_at_or_below": false, + "first_at_or_below_s": null, + "first_downcross_s": null + }, + "candidate": { + "threshold": 0.05, + "quantum_tolerance": 0.01, + "original_turn_direction": -1.0, + "starts_at_or_below": false, + "first_at_or_below_s": null, + "first_downcross_s": null + } + }, + { + "baseline": { + "threshold": 0.0, + "quantum_tolerance": 0.01, + "original_turn_direction": -1.0, + "starts_at_or_below": false, + "first_at_or_below_s": null, + "first_downcross_s": null + }, + "candidate": { + "threshold": 0.0, + "quantum_tolerance": 0.01, + "original_turn_direction": -1.0, + "starts_at_or_below": false, + "first_at_or_below_s": null, + "first_downcross_s": null + } + } + ], + "c1": [ + { + "baseline": { + "threshold": 0.1, + "quantum_tolerance": 0.0005, + "original_turn_direction": -1.0, + "starts_at_or_below": false, + "first_at_or_below_s": null, + "first_downcross_s": null + }, + "candidate": { + "threshold": 0.1, + "quantum_tolerance": 0.0005, + "original_turn_direction": -1.0, + "starts_at_or_below": false, + "first_at_or_below_s": null, + "first_downcross_s": null + } + }, + { + "baseline": { + "threshold": 0.035, + "quantum_tolerance": 0.0005, + "original_turn_direction": -1.0, + "starts_at_or_below": false, + "first_at_or_below_s": null, + "first_downcross_s": null + }, + "candidate": { + "threshold": 0.035, + "quantum_tolerance": 0.0005, + "original_turn_direction": -1.0, + "starts_at_or_below": false, + "first_at_or_below_s": null, + "first_downcross_s": null + } + }, + { + "baseline": { + "threshold": 0.0, + "quantum_tolerance": 0.0005, + "original_turn_direction": -1.0, + "starts_at_or_below": false, + "first_at_or_below_s": null, + "first_downcross_s": null + }, + "candidate": { + "threshold": 0.0, + "quantum_tolerance": 0.0005, + "original_turn_direction": -1.0, + "starts_at_or_below": false, + "first_at_or_below_s": null, + "first_downcross_s": null + } + } + ] + } + }, + "left45_release": { + "window_s": [ + 2736.0, + 2738.0 + ], + "cycles": 199, + "eligible_cycles": 199, + "baseline_mean_abs_c1": 0.11494472361809047, + "candidate_mean_abs_c1": 0.05394472361809047, + "c1_reduced_more_than_one_quantum_percent": 100.0, + "c1_increased_more_than_one_quantum_percent": 0.0, + "largest_c1_magnitude_reduction_rad": 0.08850000000000002, + "exact_c0_equality": true, + "published_crossings": { + "c0": [ + { + "baseline": { + "threshold": 0.5, + "quantum_tolerance": 0.01, + "original_turn_direction": -1.0, + "starts_at_or_below": false, + "first_at_or_below_s": 2737.704518096, + "first_downcross_s": 2737.704518096 + }, + "candidate": { + "threshold": 0.5, + "quantum_tolerance": 0.01, + "original_turn_direction": -1.0, + "starts_at_or_below": false, + "first_at_or_below_s": 2737.704518096, + "first_downcross_s": 2737.704518096 + } + }, + { + "baseline": { + "threshold": 0.05, + "quantum_tolerance": 0.01, + "original_turn_direction": -1.0, + "starts_at_or_below": false, + "first_at_or_below_s": null, + "first_downcross_s": null + }, + "candidate": { + "threshold": 0.05, + "quantum_tolerance": 0.01, + "original_turn_direction": -1.0, + "starts_at_or_below": false, + "first_at_or_below_s": null, + "first_downcross_s": null + } + }, + { + "baseline": { + "threshold": 0.0, + "quantum_tolerance": 0.01, + "original_turn_direction": -1.0, + "starts_at_or_below": false, + "first_at_or_below_s": null, + "first_downcross_s": null + }, + "candidate": { + "threshold": 0.0, + "quantum_tolerance": 0.01, + "original_turn_direction": -1.0, + "starts_at_or_below": false, + "first_at_or_below_s": null, + "first_downcross_s": null + } + } + ], + "c1": [ + { + "baseline": { + "threshold": 0.1, + "quantum_tolerance": 0.0005, + "original_turn_direction": -1.0, + "starts_at_or_below": false, + "first_at_or_below_s": 2737.25284043, + "first_downcross_s": 2737.25284043 + }, + "candidate": { + "threshold": 0.1, + "quantum_tolerance": 0.0005, + "original_turn_direction": -1.0, + "starts_at_or_below": false, + "first_at_or_below_s": 2736.458195634, + "first_downcross_s": 2736.458195634 + } + }, + { + "baseline": { + "threshold": 0.035, + "quantum_tolerance": 0.0005, + "original_turn_direction": -1.0, + "starts_at_or_below": false, + "first_at_or_below_s": 2737.874288432, + "first_downcross_s": 2737.874288432 + }, + "candidate": { + "threshold": 0.035, + "quantum_tolerance": 0.0005, + "original_turn_direction": -1.0, + "starts_at_or_below": false, + "first_at_or_below_s": 2737.107792079, + "first_downcross_s": 2737.107792079 + } + }, + { + "baseline": { + "threshold": 0.0, + "quantum_tolerance": 0.0005, + "original_turn_direction": -1.0, + "starts_at_or_below": false, + "first_at_or_below_s": null, + "first_downcross_s": null + }, + "candidate": { + "threshold": 0.0, + "quantum_tolerance": 0.0005, + "original_turn_direction": -1.0, + "starts_at_or_below": false, + "first_at_or_below_s": 2737.452322372, + "first_downcross_s": 2737.452322372 + } + } + ] + } + } + }, + "source_sha256": { + "tools/ford_pscm_lab/model_release_replay.py": "f2c099ac8dd31dbc6740d587a1e90d12449b3e5130ed19b964fb433eb43880bb", + ".cache/ford_routea9/route.npz": "3fa8cabfb729dd42d689de38618d1d21c8965c9d60f914546f4d7cc58db8c975", + ".cache/ford_routea9/geometry_models.npz": "a3b271a4280ca2cf6183e43ff7b0d32a8bfb8f729cb43dde9fabc227e57f945a", + ".cache/ford_routea9/metadata.json": "403d35504bf596144845ac2060ce067d571f49a2955d16223af28a79a7a66e93", + "openpilot/selfdrive/controls/lib/ford_model_action.py": "237f4fe301e836f6fcfb546c2db208296bd237965d5978e599d8deb3f5a8d540", + "tools/ford_pscm_lab/model_action_replay.py": "95d546cc87c065fa7d581b41382e1ab78bacc4030931b81388cb69c61adf32f7" + }, + "command_artifact": ".cache/ford_routea9/model_release_report.npz" + }, + "source_sha256": { + "openpilot/selfdrive/controls/lib/ford_model_action.py": "237f4fe301e836f6fcfb546c2db208296bd237965d5978e599d8deb3f5a8d540", + "openpilot/selfdrive/controls/tests/test_ford_model_release.py": "99bf92ffe636d302fb092589b234186645ad5c2dd1d3caf04e413c90f8f7955f", + "openpilot/selfdrive/controls/tests/test_ford_model_points.py": "0264c0163d4e949f969924f2f1e09274f8414649e00389c76a679be0589ca571", + "openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py": "3dc56ab6ac9d1460d661a29c1472110fac42c5d061a62cf9624ab132b7c4a8f1", + "openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml": "ac6d360132b780f4673b00b48b93cb2aac09428a5cad92204e1d5465de649125", + "openpilot/sunnypilot/sunnylink/settings_ui.json": "0490b1250450c430119eae176d1e7be0e797806aa2117fc90ea321e28402a693", + "tools/ford_pscm_lab/model_release_replay.py": "f2c099ac8dd31dbc6740d587a1e90d12449b3e5130ed19b964fb433eb43880bb", + "tools/ford_pscm_lab/test_model_release_replay.py": "9948ab154561adc6df1418153e89614ebe52382fd6889112d845c6948e80bbe8", + "docs/ford_model_release.md": "0a5aeb9f1bf077c89a444d5cd4395599193667db8010e438bf1bdc9df91a8032", + "docs/ford_model_action_drive_test.md": "07917b1de9d426553219eca24ae1bee64ade240d3ce5ca41d58bb81ef52d778f" + } +} diff --git a/openpilot/selfdrive/controls/lib/ford_model_action.py b/openpilot/selfdrive/controls/lib/ford_model_action.py index 56ca418620..0fa7854582 100644 --- a/openpilot/selfdrive/controls/lib/ford_model_action.py +++ b/openpilot/selfdrive/controls/lib/ford_model_action.py @@ -1,4 +1,4 @@ -"""Experimental Ford C2-free controller using one shared model-path point. +"""Experimental Ford model-point controller with earlier C1 release. The one-second preview and seven-metre minimum are engineering choices, not identified Ford reference points or PSCM calibration. @@ -31,11 +31,14 @@ def _finite(*values): def encode_model_action(model, desired_curvature, speed): - """Sample model lateral position and heading at the same arc station. + """Sample a model pose and unload C1 when its path is straightening ahead. Use the model's distance at one second, with a seven-metre minimum and an - endpoint hold. Selected curvature remains a health/diagnostic input; it does - not reconstruct heading or rotate the model geometry in this experiment. + endpoint hold. Terminal spatial curvature bounds the heading contribution + toward zero. A constant heading slope in sampled arc distance retains C1; + decreasing curvature can release C1 before that heading returns to zero. + This is an experimental request mapping, not an identified PSCM response. + Selected curvature remains a health/diagnostic input only. """ if not _finite(desired_curvature, speed) or not .3 <= speed <= 55 or abs(desired_curvature) > 1: return FordPath() @@ -54,6 +57,17 @@ def encode_model_action(model, desired_curvature, speed): sample_station = min(station[-1], max(MIN_STATION_M, float(np.interp(PREVIEW_TIME_S, times, station)))) c0 = float(np.interp(sample_station, station, lateral)) c1 = float(np.interp(sample_station, station, heading)) + # Use the same enclosing model segment as the pose interpolation. At an + # exact knot use its incoming segment; an ambiguous duplicate keeps C1. + upper = max(1, int(np.searchsorted(station, sample_station, side='left'))) + span = station[upper] - station[upper - 1] + duplicate = (upper + 1 < len(station) and station[upper] == sample_station == station[upper + 1]) + if span > 0. and not duplicate: + terminal_heading = heading[0] + sample_station * ((heading[upper] - heading[upper - 1]) / span) + if _finite(terminal_heading): + # One-sided: never amplify C1 or invent a reversal ahead of the model. + direction = math.copysign(1., c1) + c1 = direction * float(np.clip(direction * terminal_heading, 0., abs(c1))) return FordPath(True, c0, c1, 0., 0.) if _finite(c0, c1) else FordPath() @@ -97,7 +111,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-pose-one-second-v7', + self.diagnostics = {'status': status, 'hypothesis': 'model-pose-terminal-c1-v1', '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, @@ -130,10 +144,11 @@ class FordModelActionController: 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-pose-one-second-v7', + self.diagnostics = {'status': 'active', 'hypothesis': 'model-pose-terminal-c1-v1', 'calibration_approved': CALIBRATION_APPROVED, 'desired_curvature': desired_curvature, 'yaw_rate': yaw_rate, 'pose_source': 'model', 'preview_time_s': PREVIEW_TIME_S, 'minimum_station_m': MIN_STATION_M, + 'c1_release': 'terminal_spatial_curvature', '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.)} diff --git a/openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py b/openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py index 63518caeba..3fe2f0211c 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py +++ b/openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py @@ -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-pose-one-second-v7') + self.assertEqual(record['hypothesis'], 'model-pose-terminal-c1-v1') self.assertIs(record['calibration_approved'], False) self.assertEqual(record['command'][2:], [0., 0.]) self.assertEqual(record['status'], controller.diagnostics['status']) @@ -61,3 +61,4 @@ class TestFordControlsLogging(unittest.TestCase): self.assertEqual(record['pose_source'], 'model') self.assertEqual(record['preview_time_s'], 1.) self.assertEqual(record['minimum_station_m'], 7.) + self.assertEqual(record['c1_release'], 'terminal_spatial_curvature') diff --git a/openpilot/selfdrive/controls/tests/test_ford_model_points.py b/openpilot/selfdrive/controls/tests/test_ford_model_points.py index ea4a4e7b1e..2cd9c37375 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_model_points.py +++ b/openpilot/selfdrive/controls/tests/test_ford_model_points.py @@ -49,6 +49,8 @@ def test_orientation_and_position_must_describe_the_same_times(): assert not encode_model_action(m, .01, 20.).valid -def test_model_heading_unwraps_before_interpolation(): +def test_model_heading_unwraps_before_interpolation_and_terminal_slope(): m = model_points([0., .5, 1.5], [0., 10., 30.], [0., 0., 0.], [3., 3.1, -3.1]) - assert encode_model_action(m, 0., 20.).path_angle == pytest.approx(np.pi) + # Heading at 20 m is pi, but the terminal slope is gentler than the + # average slope. Its release bound is 3 + 20 * ((2*pi - 3.1) - 3.1)/20. + assert encode_model_action(m, 0., 20.).path_angle == pytest.approx(2*np.pi - 3.2) diff --git a/openpilot/selfdrive/controls/tests/test_ford_model_release.py b/openpilot/selfdrive/controls/tests/test_ford_model_release.py new file mode 100644 index 0000000000..aefe0b399f --- /dev/null +++ b/openpilot/selfdrive/controls/tests/test_ford_model_release.py @@ -0,0 +1,148 @@ +"""C1 release geometry checks; none predicts a PSCM or vehicle response.""" +import math +from types import SimpleNamespace + +import numpy as np +import pytest + +from openpilot.selfdrive.controls.lib.ford_model_action import ModelActionController, encode_model_action + + +def model_path(headings, *, origin=0., offset=.3, sign=1.): + # Known chord lengths make spatial heading profiles independent of speed. + station = (0., 6., 18., 30.) + heading = [sign*(origin+value) for value in headings] + x, y = [0.], [sign*offset] + for i in range(1, len(station)): + direction = (heading[i-1]+heading[i])/2 + distance = station[i]-station[i-1] + x.append(x[-1]+distance*math.cos(direction)) + y.append(y[-1]+distance*math.sin(direction)) + times = [0., .5, 1.5, 2.5] + return SimpleNamespace(position=SimpleNamespace(t=times, x=x, y=y), + orientation=SimpleNamespace(t=times, z=heading)) + + +@pytest.mark.parametrize('sign', [-1., 1.]) +@pytest.mark.parametrize('origin', [0., .15]) +def test_constant_spatial_curvature_preserves_original_heading(sign, origin): + model = model_path([0., .12, .36, .60], origin=origin, sign=sign) + out = encode_model_action(model, 0., 20.) + # The selected point is halfway from 6 m to 18 m: psi = psi0 + .02*12. + assert out.valid + assert out.path_angle == pytest.approx(sign*(origin+.24)) + assert out.curvature == out.curvature_rate == 0. + + +@pytest.mark.parametrize('heading', [-.3, .3]) +def test_constant_heading_line_keeps_its_nonzero_heading(heading): + out = encode_model_action(model_path([0.]*4, origin=heading), 0., 20.) + assert out.valid + assert out.path_angle == pytest.approx(heading) + + +@pytest.mark.parametrize('sign', [-1., 1.]) +def test_increasing_spatial_curvature_cannot_amplify_c1(sign): + # Selected heading .24; terminal curvature .03/m would yield .36 rad. + model = model_path([0., .06, .42, .90], sign=sign) + out = encode_model_action(model, 0., 20.) + assert out.path_angle == pytest.approx(sign*.24) + + +@pytest.mark.parametrize('sign', [-1., 1.]) +@pytest.mark.parametrize('origin', [0., .15]) +def test_decreasing_spatial_curvature_unloads_c1_without_erasing_origin(sign, origin): + # Selected heading is psi0+.36; terminal curvature .02/m gives psi0+.24. + model = model_path([0., .24, .48, .54], origin=origin, sign=sign) + out = encode_model_action(model, 0., 20.) + assert out.path_angle == pytest.approx(sign*(origin+.24)) + assert abs(out.path_angle) < origin+.36 + + +@pytest.mark.parametrize('sign', [-1., 1.]) +def test_opposite_terminal_curvature_releases_without_inventing_a_reversal(sign): + model = model_path([0., .30, .10, -.30], sign=sign) + out = encode_model_action(model, 0., 20.) + # The selected model heading is still sign*.20, though its slope has reversed. + assert out.valid + assert out.path_angle == 0. + + +def test_exact_model_knot_uses_incoming_segment(): + model = SimpleNamespace(position=SimpleNamespace(t=[0., .5, 1., 2.], x=[0., 6., 12., 24.], y=[.3]*4), + orientation=SimpleNamespace(t=[0., .5, 1., 2.], z=[0., .24, .48, .48])) + assert encode_model_action(model, 0., 20.).path_angle == pytest.approx(.48) + + +@pytest.mark.parametrize('sign', [-1., 1.]) +def test_duplicate_selected_station_retains_original_c1(sign): + times = [0., .5, 1., 2.] + model = SimpleNamespace(position=SimpleNamespace(t=times, x=[0., 7., 7., 14.], y=[sign*.4]*4), + orientation=SimpleNamespace(t=times, z=[0., sign*.3, sign*.2, sign*.2])) + out = encode_model_action(model, 0., 20.) + assert out.valid + assert out.path_offset == sign*.4 + assert out.path_angle == pytest.approx(sign*.2) + + +def test_actual_model_reversal_uses_existing_slew_and_reaches_opposite_c1(): + controller = ModelActionController() + positive = model_path([0., .12, .36, .60]) + negative = model_path([0., .12, .36, .60], sign=-1.) + for _ in range(50): + controller.update(positive, 0., speed=20., dt=.01) + before = controller.c1 + first = controller.update(negative, 0., speed=20., dt=.01) + assert controller.c1 == pytest.approx(before-.005) + assert first.path_angle > 0. + for _ in range(100): + final = controller.update(negative, 0., speed=20., dt=.01) + assert final.path_angle == pytest.approx(-.24) + + +def test_release_preserves_c0_exactly_through_target_slew_and_packing(): + actual, reference = ModelActionController(), ModelActionController() + for i in range(240): + model = model_path([0., .24, .48, .54], offset=8.*math.sin(i*.04), sign=1. if i < 120 else -1.) + # C0's reference has byte-for-byte identical position and clocks, but no C1. + zero_heading = SimpleNamespace(position=model.position, + orientation=SimpleNamespace(t=model.orientation.t, z=[0.]*4)) + target = encode_model_action(model, 0., 20.) + c0_target = encode_model_action(zero_heading, 0., 20.) + assert target.path_offset == c0_target.path_offset + output = actual.update(model, 0., speed=20., dt=.01) + c0_output = reference.update(zero_heading, 0., speed=20., dt=.01) + assert actual.c0 == reference.c0 + assert output.path_offset == c0_output.path_offset + + +@pytest.mark.parametrize('sign', [-1., 1.]) +def test_large_geometry_keeps_existing_field_caps_and_zero_c2_c3(sign): + controller = ModelActionController() + model = model_path([0.]*4, origin=.8, offset=20., sign=sign) + for _ in range(150): + out = controller.update(model, 0., speed=20., dt=.01) + assert out.valid + assert abs(out.path_offset) <= 5.11+1e-12 + assert abs(out.path_angle) <= .5+1e-12 + assert out.curvature == out.curvature_rate == 0. + assert out.path_offset == pytest.approx(sign*5.11) + assert out.path_angle == pytest.approx(sign*.5) + + +@pytest.mark.parametrize('field', ['x', 'y', 'z']) +def test_nonfinite_geometry_cannot_publish_a_release_request(field): + model = model_path([0., .24, .48, .54]) + values = getattr(model.orientation if field == 'z' else model.position, field) + values[2] = math.nan + assert not encode_model_action(model, 0., 20.).valid + + +def test_unrepresentable_terminal_slope_falls_back_to_finite_original_heading(): + times = [0., .5, 1.] + model = SimpleNamespace(position=SimpleNamespace(t=times, x=[0., 5e-320, 1e-319], y=[0.]*3), + orientation=SimpleNamespace(t=times, z=[0., .2, .4])) + out = encode_model_action(model, 0., 20.) + assert out.valid + assert np.isfinite(out.path_angle) + assert out.path_angle == pytest.approx(.4) diff --git a/openpilot/sunnypilot/sunnylink/settings_ui.json b/openpilot/sunnypilot/sunnylink/settings_ui.json index 0f4c7fbafd..a172bb6723 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui.json +++ b/openpilot/sunnypilot/sunnylink/settings_ui.json @@ -2183,8 +2183,8 @@ "widget": "toggle", "needs_onroad_cycle": true, "title": "Selected-Action Path Tracking (Experimental)", - "description": "Follow model position and heading from one shared path point on the Ford CAN FD F-150 Lightning.", - "details": "Uses the model's lateral position and heading one second ahead, with a seven-metre minimum distance and no extrapolation beyond the available path. Sends requests at 20 Hz with the existing command limits and rate limits. Default off; this model-point experiment is not road-validated. Enable only for controlled testing. Lateral maneuver test mode is unsupported and disengages this controller. 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.", + "description": "Follow the model path with experimental earlier steering release on the Ford CAN FD F-150 Lightning.", + "details": "Uses the model's lateral position and heading one second ahead, with a seven-metre minimum distance and no extrapolation beyond the available path. Reduces the heading request when the model path straightens ahead; this may also reduce turning authority. Sends requests at 20 Hz with the existing command limits and rate limits. Default off; earlier physical release is not road-validated. Enable only for controlled testing. Lateral maneuver test mode is unsupported and disengages this controller. 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" diff --git a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml index d361a75075..ae7d9fcf39 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml +++ b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml @@ -14,8 +14,8 @@ sections: widget: toggle needs_onroad_cycle: true title: Selected-Action Path Tracking (Experimental) - description: Follow model position and heading from one shared path point on the Ford CAN FD F-150 Lightning. - details: Uses the model's lateral position and heading one second ahead, with a seven-metre minimum distance and no extrapolation beyond the available path. Sends requests at 20 Hz with the existing command limits and rate limits. Default off; this model-point experiment is not road-validated. Enable only for controlled testing. Lateral maneuver test mode is unsupported and disengages this controller. 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. + description: Follow the model path with experimental earlier steering release on the Ford CAN FD F-150 Lightning. + details: Uses the model's lateral position and heading one second ahead, with a seven-metre minimum distance and no extrapolation beyond the available path. Reduces the heading request when the model path straightens ahead; this may also reduce turning authority. Sends requests at 20 Hz with the existing command limits and rate limits. Default off; earlier physical release is not road-validated. Enable only for controlled testing. Lateral maneuver test mode is unsupported and disengages this controller. 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 diff --git a/tools/ford_pscm_lab/model_release_replay.py b/tools/ford_pscm_lab/model_release_replay.py new file mode 100644 index 0000000000..06246d6b16 --- /dev/null +++ b/tools/ford_pscm_lab/model_release_replay.py @@ -0,0 +1,222 @@ +"""Compare C1 release commands on timestamp-preserving recorded model extracts. + +The model cache contains ns and points shaped (models,4,points): x,y,heading,t. +Its shared t must be copied from original model position/orientation clocks after +verifying that those clocks agree; this tool never synthesizes missing clocks. +Recorded motion and eligibility are frozen. Output compares commands, never +predicts steering or establishes whether physical unwind would improve. +""" +import argparse +import hashlib +import json +from pathlib import Path +from types import SimpleNamespace + +import numpy as np + +from openpilot.selfdrive.controls.lib import ford_model_action +from tools.ford_pscm_lab.model_action_replay import WireCheck, field_checks, load_controller, sample, table + +BASELINE_REVISION = '4bd841eccd116f535a2ee99b920a631d1718f0cc' +WARMUP_S = 3. + + +def read_models(filename): + with np.load(filename, allow_pickle=False) as raw: + if 'points' not in raw or 'ns' not in raw: + raise ValueError('Model cache requires ns and original x,y,heading,t arrays') + points, ns = raw['points'], raw['ns'] + if points.ndim != 3 or points.shape[1] != 4 or points.shape[2] < 2 or not len(points): + raise ValueError('Model cache requires four arrays per model, including original timestamps') + if ns.shape != (len(points),) or not np.issubdtype(ns.dtype, np.integer) or np.any(np.diff(ns) <= 0): + raise ValueError('Model ns must be strictly increasing integer timestamps') + if not np.isfinite(points).all() or np.any(points[:, 3, 0] != 0.) or np.any(np.diff(points[:, 3], axis=1) <= 0.): + raise ValueError('Model points and original timestamps must be finite, with times starting at zero and increasing') + models = [SimpleNamespace(position=SimpleNamespace(x=p[0], y=p[1], t=p[3]), + orientation=SimpleNamespace(z=p[2], t=p[3])) for p in points] + return ns, models + + +def crossings(t, values, valid, threshold, quantum, *, direction): + below = direction * values <= threshold + quantum + 1e-9 + selected = below & valid + first = np.flatnonzero(selected) + crossed = np.flatnonzero(selected[1:] & valid[:-1] & ~below[:-1]) + 1 + return {'threshold': threshold, 'quantum_tolerance': quantum, 'original_turn_direction': direction, + 'starts_at_or_below': bool(selected[0]), + 'first_at_or_below_s': float(t[first[0]]) if len(first) else None, + 'first_downcross_s': float(t[crossed[0]]) if len(crossed) else None} + + +def compare_window(t, old, new, valid, label, lo, hi): + keep = (t >= lo) & (t <= hi) + if not keep.any(): + raise ValueError(f'No exact consumed models for window {label}') + t, old, new, valid = t[keep], old[keep], new[keep], valid[keep] + if not valid.any(): + raise ValueError(f'No eligible commands in window {label}') + delta = abs(old[:, 1]) - abs(new[:, 1]) + fields = {} + for column, name, thresholds, quantum in [(0, 'c0', [.5, .05, 0.], .01), (1, 'c1', [.1, .035, 0.], .0005)]: + peak = old[valid, column][np.argmax(abs(old[valid, column]))] + direction = -1. if peak < 0. else 1. + fields[name] = [{'baseline': crossings(t, old[:, column], valid, threshold, quantum, direction=direction), + 'candidate': crossings(t, new[:, column], valid, threshold, quantum, direction=direction)} for threshold in thresholds] + return {'window_s': [lo, hi], 'cycles': len(t), 'eligible_cycles': int(valid.sum()), + 'baseline_mean_abs_c1': float(np.mean(abs(old[valid, 1]))), + 'candidate_mean_abs_c1': float(np.mean(abs(new[valid, 1]))), + 'c1_reduced_more_than_one_quantum_percent': float(100*np.mean(delta[valid] > .0005001)), + 'c1_increased_more_than_one_quantum_percent': float(100*np.mean(delta[valid] < -.0005001)), + 'largest_c1_magnitude_reduction_rad': float(np.max(delta[valid])), + 'exact_c0_equality': bool(np.array_equal(old[:, 0], new[:, 0])), 'published_crossings': fields} + + +def run(route_extract, models_file, metadata_file, output, windows): + inputs = [route_extract.resolve(), models_file.resolve(), metadata_file.resolve()] + output = output.resolve() + commands_output = output.with_suffix('.npz') + if output in inputs or commands_output in inputs: + raise ValueError('Output must not overwrite an input extract') + ns, models = read_models(models_file) + metadata = json.loads(metadata_file.read_text()) + if not metadata['identities'] or any(identity['commit'] != BASELINE_REVISION or identity['dirty'] for identity in metadata['identities']): + raise ValueError('Recorded route must use the pinned baseline revision') + with np.load(route_extract, allow_pickle=False) as raw: + streams = {name: table(raw, name) for name in ('controls', 'cs', 'path', 'wire')} + controls = streams['controls'] + mi = np.searchsorted(ns, controls['model_ns']) + exact = (mi < len(ns)) & (ns[np.minimum(mi, len(ns)-1)] == controls['model_ns']) + indices = np.flatnonzero(exact) + if not len(indices): + raise ValueError('No extracted model matches a consumed model timestamp') + mi = mi[indices] + controls = {key: values[indices] for key, values in controls.items()} + now = controls['t'] + if np.any(np.diff(now) <= 0): + raise ValueError('Control timestamps must increase strictly') + cs = sample(streams['cs'], now) + recorded = sample(streams['path'], now, nearest=True) + if np.any(abs(recorded['t'] - now) >= .005): + raise ValueError('Recorded command publication is not from the same control cycle') + active = recorded['active'].astype(bool) + archived = load_controller(BASELINE_REVISION) + old_core, new_core = archived.ModelActionController(), ford_model_action.ModelActionController() + old = np.zeros((len(now), 4)) + new = np.zeros_like(old) + valid = np.zeros(len(now), dtype=bool) + settled = np.zeros(len(now), dtype=bool) + old_targets, new_targets = [], [] + for model in models: + a = archived.encode_model_action(model, 0., 10.) + b = ford_model_action.encode_model_action(model, 0., 10.) + if not a.valid or not b.valid: + raise ValueError('Model cache contains a path rejected by one of the encoders') + old_targets.append([a.path_offset, a.path_angle]) + new_targets.append([b.path_offset, b.path_angle]) + old_targets, new_targets = np.array(old_targets), np.array(new_targets) + np.testing.assert_array_equal(old_targets[:, 0], new_targets[:, 0]) + assert (abs(new_targets[:, 1]) <= abs(old_targets[:, 1]) + 1e-12).all() + assert (old_targets[:, 1] * new_targets[:, 1] >= 0.).all() + wire = WireCheck() + span_start = now[0] + previous_valid = False + for i, timestamp in enumerate(now): + if i == 0 or indices[i] != indices[i-1]+1 or timestamp-now[i-1] > .1: + old_core.reset() + new_core.reset() + previous_valid = False + span_start = timestamp + dt = timestamp-now[i-1] if previous_valid else .01 + kwargs = {'speed': cs['speed'][i], 'yaw_rate': cs['yaw'][i], 'dt': dt, 'active': bool(active[i])} + a = old_core.update(models[mi[i]], controls['desired'][i], **kwargs) + b = new_core.update(models[mi[i]], controls['desired'][i], **kwargs) + assert a.valid == b.valid + old[i] = a.path_offset, a.path_angle, a.curvature, a.curvature_rate + new[i] = b.path_offset, b.path_angle, b.curvature, b.curvature_rate + valid[i] = a.valid + settled[i] = timestamp - span_start >= WARMUP_S + wire.check(a) + wire.check(b) + previous_valid = a.valid + np.testing.assert_array_equal(valid, active) + np.testing.assert_array_equal(old[:, 0], new[:, 0]) + for command in (old, new): + field_checks(command, valid, now) + baseline_recorded = np.column_stack([recorded['c0'], recorded['c1']]) + baseline_delta = abs(old[:, :2].astype(np.float32).astype(float) - baseline_recorded) + if not settled.any() or not valid[settled].any(): + raise ValueError('No eligible commands remain after the three-second cache-boundary warmup') + assert (baseline_delta[settled] <= [.010001, .0005001]).all(), 'Baseline differs from recorded output by more than one DBC quantum' + elapsed = now - metadata['t0'] + window_reports = {} + for label, lo, hi in windows: + keep = (elapsed >= lo) & (elapsed <= hi) + if not keep.any() or not settled[keep].all(): + raise ValueError(f'Window {label} lacks complete cache-boundary warmup') + window_reports[label] = compare_window(elapsed, old, new, valid, label, lo, hi) + # Keep the recorded 20 Hz transmission schedule. This samples the proposed + # publication; it does not simulate a different sender or vehicle response. + wire_t = streams['wire']['t'] + wi = np.searchsorted(now, wire_t, side='right')-1 + in_scope = (wi >= 0) & (wire_t-now[np.maximum(wi, 0)] <= .02) + wi = wi[in_scope] + scheduled_t = wire_t[in_scope] + scheduled_old, scheduled_new = old[wi], new[wi] + np.testing.assert_array_equal(scheduled_old[:, 0], scheduled_new[:, 0]) + output.parent.mkdir(parents=True, exist_ok=True) + np.savez_compressed(commands_output, t=elapsed, baseline=old, candidate=new, valid=valid, settled=settled, + recorded=baseline_recorded, model_ns=controls['model_ns'], model_cache_index=mi, + raw_baseline=old_targets[mi], raw_candidate=new_targets[mi], + scheduled_t=scheduled_t-metadata['t0'], scheduled_baseline=scheduled_old, scheduled_candidate=scheduled_new) + sources = [Path(__file__), *inputs, Path(ford_model_action.__file__), + Path(__file__).with_name('model_action_replay.py')] + report = {'scope': 'Command-only comparison on exact consumed recorded models with frozen recorded eligibility and motion; no physical steering prediction.', + 'baseline_revision': BASELINE_REVISION, 'baseline_source_sha256': archived.source_sha256, + 'clock_source': 'Cache row3 contains original shared model position/orientation times; omitted clocks are rejected.', + 'models': len(models), 'control_cycles': len(now), 'eligible_cycles': int(valid.sum()), + 'three_second_cache_boundary_warmup_cycles': int((~settled).sum()), + 'baseline_matches_recorded_within_one_quantum_after_warmup': True, + 'baseline_timing_limit': ('Replay uses logged control-message times; internal core-entry times are not logged on every cycle. ' + + 'Baseline agreement is therefore checked within one DBC quantum, not asserted bit-for-bit.'), + 'baseline_nonidentical_published_values_after_warmup': int(np.count_nonzero(baseline_delta[settled])), + 'baseline_max_recorded_error_after_warmup': np.max(baseline_delta[settled], axis=0).tolist(), + 'exact_c0_equality_raw_published_and_scheduled': True, + 'raw_c1_never_amplifies_or_reverses': True, + 'float32_CAN_roundtrips': wire.count, + 'finite_caps_slew_zero_c2_c3_checks': True, + 'recorded_schedule_samples': len(scheduled_t), + 'recorded_median_CAN_period_s': float(np.median(np.diff(streams['wire']['t']))), + 'scheduled_command_scope': ('Latest proposed publication at each original send timestamp, restricted to <=20ms old; ' + + 'existing sender behavior is unchanged.'), + 'crossing_method': ('First command into the original turn at/below a diagnostic threshold plus one DBC quantum. ' + + 'Direction is the sign of the largest absolute valid baseline value per field/window and is shared by both versions; ' + + 'opposite-sign commands count as release. Downcross requires an immediately preceding above-threshold valid command. ' + + 'The 0.035 rad level is a comparison marker, not an established hardware limit. No physical-response score.'), + 'windows': window_reports, + 'source_sha256': {str(path.resolve()): hashlib.sha256(path.read_bytes()).hexdigest() for path in sources}, + 'command_artifact': str(commands_output)} + output.write_text(json.dumps(report, indent=2, allow_nan=False)+'\n') + return report + + +def main(): + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument('--route-extract', type=Path, required=True) + parser.add_argument('--models', type=Path, required=True) + parser.add_argument('--metadata', type=Path, required=True) + parser.add_argument('--output', type=Path, required=True) + parser.add_argument('--window', action='append', default=[], metavar='LABEL:START:END') + args = parser.parse_args() + windows = [] + for value in args.window: + label, lo, hi = value.split(':') + lo, hi = float(lo), float(hi) + if not np.isfinite([lo, hi]).all() or hi <= lo: + parser.error('Window end must be finite and later than its start') + windows.append((label, lo, hi)) + report = run(args.route_extract, args.models, args.metadata, args.output, windows) + print(json.dumps({key: value for key, value in report.items() if key not in ('windows', 'source_sha256')}, indent=2)) + + +if __name__ == '__main__': + main() diff --git a/tools/ford_pscm_lab/test_model_release_replay.py b/tools/ford_pscm_lab/test_model_release_replay.py new file mode 100644 index 0000000000..881bd0de1b --- /dev/null +++ b/tools/ford_pscm_lab/test_model_release_replay.py @@ -0,0 +1,73 @@ +import numpy as np +import pytest + +from tools.ford_pscm_lab.model_release_replay import compare_window, crossings, read_models, run + + +def original_models(): + return np.array([[[0., 7., 14.], [0., .1, .2], [0., .02, .04], [0., 1., 2.]]]) + + +def test_model_clock_roundtrip(tmp_path): + filename = tmp_path/'models.npz' + points = original_models() + np.savez(filename, ns=np.array([123], dtype=np.int64), points=points) + ns, models = read_models(filename) + np.testing.assert_array_equal(ns, [123]) + np.testing.assert_array_equal(models[0].position.t, points[0, 3]) + np.testing.assert_array_equal(models[0].orientation.t, points[0, 3]) + np.testing.assert_array_equal(models[0].orientation.z, points[0, 2]) + + +@pytest.mark.parametrize('times', [[1., 2., 3.], [0., 1., 1.], [0., 2., 1.], [0., np.nan, 2.]]) +def test_reject_bad_original_model_clocks(tmp_path, times): + points = original_models() + points[0, 3] = times + filename = tmp_path/'models.npz' + np.savez(filename, ns=np.array([123]), points=points) + with pytest.raises(ValueError, match='timestamps'): + read_models(filename) + + +def test_run_rejects_missing_clocks_before_comparison(tmp_path): + filename = tmp_path/'models.npz' + np.savez(filename, ns=np.array([123]), points=original_models()[:, :3]) + with pytest.raises(ValueError, match='original timestamps'): + run(tmp_path/'route.npz', filename, tmp_path/'metadata.json', tmp_path/'report.json', []) + assert not (tmp_path/'report.json').exists() + + +def test_reject_noninteger_model_stamp(tmp_path): + filename = tmp_path/'models.npz' + np.savez(filename, ns=np.array([123.5]), points=original_models()) + with pytest.raises(ValueError, match='integer timestamps'): + read_models(filename) + + +def test_downcross_respects_quantum_and_invalid_cycles(): + stamps = np.arange(6.) + # Invalid zero cannot count as release; threshold plus one quantum can. + values = np.array([.2, 0., .2, .1005, .1, 0.]) + actual = crossings(stamps, values, np.array([True, False, True, True, True, True]), .1, .0005, direction=1.) + assert not actual['starts_at_or_below'] + assert actual['first_at_or_below_s'] == actual['first_downcross_s'] == 3. + no_cross = crossings(stamps, np.full(6, .1006), np.ones(6, dtype=bool), .1, .0005, direction=1.) + assert no_cross['first_at_or_below_s'] is no_cross['first_downcross_s'] is None + + +@pytest.mark.parametrize('direction', [-1., 1.]) +def test_sign_jump_counts_as_release(direction): + values = direction * np.array([.002, -.003, -.01]) + actual = crossings(np.arange(3.), values, np.ones(3, dtype=bool), 0., .0005, direction=direction) + assert actual['first_at_or_below_s'] == actual['first_downcross_s'] == 1. + + +def test_comparison_uses_baseline_direction_for_both_versions(): + old, new = np.zeros((3, 4)), np.zeros((3, 4)) + old[:, 1] = [.1, .05, -.01] + new[:, 1] = [.02, -.04, -.04] + result = compare_window(np.arange(3.), old, new, np.ones(3, dtype=bool), 'turn', 0., 2.) + zero = result['published_crossings']['c1'][-1] + assert zero['baseline']['original_turn_direction'] == zero['candidate']['original_turn_direction'] == 1. + assert zero['baseline']['first_downcross_s'] == 2. + assert zero['candidate']['first_downcross_s'] == 1.