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