sunnypilot/docs/ford_virtual_angle_experiment.md

Ford C2-free model-pose tracking with measured feedback

Hypothesis model-pose-c0-c1-feedback-v7 retains v6's model-pose C0/C1 base and adds narrow recovery of an opposing C1 bias during release. C0, model geometry, blending, gains, rates and output bounds are unchanged. Recovery can remove retained opposing bias but cannot create bias beyond zero or relax the PSCM LimitReached growth restriction. C2/C3 remain zero, and selected desired curvature remains the measured-yaw feedback target.

This is an experimental outer controller around the multivariable PSCM. Its geometry does not define a calibrated C0/C1-to-wheel mapping or an angle servo. Command replay cannot establish the truck's response, closed-loop stability, or an overshoot improvement. The v7 recovery change has offline validation only; its physical response is unproven.

Evidence and scope

Route80 ran v3 and contains both sustained under-response and over-response. Representative eligible windows had median CAN response/request ratios of 0.78, 1.77 and 0.69 with a declared 0.2-second comparison interval. These are descriptive tracking ratios, not identified controller gains.

V4 replaced separate model-heading C1 with selected-curvature C1 and reduced heading demand in several large maneuvers. The user subsequently reported weak turning and steering repeatedly stopping near 85 degrees. Older logs contain larger wheel angles; the inspected host code has no fixed 85-degree wheel stop, although upstream curvature limits depend on speed.

Route83 had the Sunnylink toggle on, but omitted EPS firmware responses. The former firmware gate selected the default FordPathController; replay reproduced its recorded C0/C1/C2 requests. Its favorable turns are evidence for the existing model-pose construction, not validation of v5 or v6. V6 reuses that construction while replacing its remaining C2 request with C0/C1 geometry. Removing C2 changes the request received by the PSCM, so matching large C0/C1 commands does not guarantee matching vehicle motion.

Base request

controlsd selects valid lateralManeuverPlan.desiredCurvature, otherwise modelV2.action.desiredCurvature, after the existing curvature limiter. This action already includes upstream delay handling; it receives no extra response advance here.

The model contribution uses the existing allocator's raw forward pose and bounded short-pose correction. _model_pose advances 0.1 seconds, retains the model's remaining forward geometry, and separately corrects the short pose using measured curvature and its recent change. Its offset preview is up to 7 m and its heading preview is up to max(7 m, speed × 1 s), bounded by available path length. This raw pose is not passed through a second model filter. The filtered, ego-aligned reference remains available for comparison and the existing geometry-validity checks.

share(k) = clip((k - 0.006/m) / (0.012/m - 0.006/m), 0, 1)
aligned = desired_curvature × model_forward_heading > 0
model_share = min(share(abs(desired_curvature)), share(model_curvature_demand))
              if aligned, otherwise 0
model_pair = existing_pose_encoder(model_pose, model_share, C2=0)

remaining_curvature = desired_curvature × (1 - model_share)
L0 = max(8 m, speed × 1 s)
L1 = max(7 m, speed × 1 s)
curvature_C0 = 0.5 × remaining_curvature × L0²
curvature_C1 = remaining_curvature × L1
C0_target = clip(model_pair.C0 + curvature_C0, ±5.11 m)
C1_base = clip(model_pair.C1 + curvature_C1, ±0.5 rad)

model_curvature_demand is the larger absolute curvature implied by the forward offset and heading previews. The share uses the existing allocator's 0.006–0.012/m thresholds. Both model and action must request a substantial turn in the same direction before model pose supplies the full base. Small, flat, opposed or zero requests use the curvature contribution; zero action produces a zero base. Partial shares combine both contributions. The existing pose encoder retains its quantization and field-allocation rules. The residual-curvature lift is geometric, not a claim of EPS equivalence to C2.

The inherited pose encoder allocates heading overflow using its asymmetric limits (+0.5235/−0.5 rad), before the symmetric final ±0.5 rad heading bound. On clipped tails, this can leave mirrored C0 requests differing by up to 0.0235 rad × 7 m = 0.1645 m. The favorable comparison anchors lie below that heading cap; full model-base odd symmetry is not claimed.

Measured feedback and limits

past_request = selected curvature held at or before (measurement_time - delay)
yaw_error = measured_speed × past_request - measured_yaw_rate
bias_trial = released_bias + feedback_gain × yaw_error × measurement_dt
C1_unconstrained = clip(C1_base + accepted_bias, ±0.5 rad)
C1_target = temporary_backoff_ceiling(C1_unconstrained) if backoff_active
            otherwise C1_unconstrained

Measured yaw is negated Ford CAN yaw, matching the control sign convention. The historical request uses zero-order hold; it never interpolates toward a future publication. Nominal comparison delay is CP.steerActuatorDelay (0.2 seconds on the source vehicle). Feedback compares against selected curvature, not curvature inferred from the model-pose coefficients.

Quantity	Value
C0 / C1 final bounds	±5.11 m / ±0.5 rad
Independent C0 / C1 slew	4 m/s / 0.5 rad/s
Feedback integration scale	1.0
Feedback minimum speed	2 m/s
Maximum PSCM/core input age	150 ms
Allowed timestamp lead	5 ms
Release comparison tolerance	one C1 wire quantum, 0.0005 rad

The integration scale, preview distances and blend thresholds are effective gains; none establishes stability. No wheel-response gain is fitted. Zero yaw error retains acquired bias while an eligible turn continues. Host anti-windup admits reachable correction within the combined C1 field and slew limits. Feedback overflow is not transferred into C0.

The release logic scales bias as the bounded base decreases and resets on zero/reversal. When delayed curvature still represents a stronger or opposing request, or PSCM reports LimitReached, new integration is normally frozen. One exception permits measured-error backoff: measured turning must exceed both the delayed and current selected yaw requests in the base's direction, and total heading must still have the base's sign. Exceeding only an older, smaller request during turn-in does not qualify. The accepted increment may only reduce that existing total toward zero; it cannot grow the request or carry it through zero. Existing host field and slew limits still apply.

The new release-recovery exception requires fresh valid PSCM status with limit below 2, retained bias opposing the base, and both current and delayed requests aligned with that base. Measured turning must be below both requests in their direction. It then uses the current yaw deficit × the existing feedback gain × measurement interval to unwind only the opposing bias toward zero. The increment is clipped so recovery cannot cross zero bias or create demand beyond the existing base. Common host anti-windup still limits what can be accepted. All other constrained cases remain frozen; PSCM limit 2 never permits this request-increasing recovery. The no-new-bias restriction applies only to release_recovery. Once release ends, ordinary eligible integration can add correction beyond the base as before; its existing limits and guards are unchanged.

release_recovery and feedback_recovery_active=true indicate that the recovery branch actually changed bias on that update. If host anti-windup blocks the entire increment, the status remains host_limit and the flag is false. Recovery is evaluated only on fresh measurements; the flag is false on repeated-measurement updates and after reset.

Diagnostics distinguish release_backoff and pscm_backoff; a release takes precedence when both conditions apply. While feedback_backoff_active is true, total C1 is also capped at the preceding continuous heading request in the current request direction and at zero in the opposite direction. This ceiling affects the output only: it is not stored or projected into bias. The measured-error increment can still update bias under the normal limits, but a changing model base does not create persistent integral suppression. The ceiling persists between repeated measurements; C1 cannot grow or reverse while it applies. The next fresh measurement clears it unless backoff is again warranted. It does not cap C0, and normal feedback has its own rules outside backoff. Independent slew remains 0.5 rad/s for C1 and 4 m/s for C0. Backoff still compares against the delayed reference, so response lag remains. Reducing a request does not demonstrate that physical overshoot is resolved.

PSCM status and driver handling

card publishes Lane_Assist_Data3_FD1 in carStateSP.fordPscmStatus, retaining the original CAN receipt timestamp. Republishing carStateSP or receiving unrelated frames cannot refresh it. The opendbc submodule is unchanged.

Feedback requires valid fresh status, InProgress lateral state (2), capability LimitedModeAvailable or ExtendedModeAvailable (1 or 2), and no denial. Missing, malformed, stale, backward-timestamped, denied or unavailable status clears bias/history, leaving the new base subject to its core validity gates. LimitReached (2) permits only the bounded request-reducing backoff described above and otherwise freezes integration. LimitWithDriverActive (3) clears feedback. Backoff still requires fresh, valid, InProgress status with an available capability and no denial. These generic PSCM reports do not identify a specific torque or rate limit.

steeringPressed, raw torque above the existing Ford driver allowance, or nonfinite torque clear feedback. Below 2 m/s feedback also clears. A fresh reference interval is required after override. Base requests retain normal PSCM driver arbitration while lateral control remains authorized; an unset override flag cannot rule out subthreshold driver influence.

Gates and Sunnylink selection

Core model/action/car-state freshness, finite-value, clock and speed checks remain in place. Invalid core inputs reset both commands and clear latActive. Raw model geometry is validated on every update, including repeated model timestamps; an invalid raw path cannot reuse the cached valid reference. Missing PSCM status disables feedback, not an otherwise valid base request.

Vehicle → Ford → C2-Free Path Tracking (Experimental) retains the FordVirtualAngleController key, default-off setting and offroad/onroad cycle requirement. Enabled selects v7 on Ford CAN FD FORD_F_150_LIGHTNING_MK1 regardless of missing or different EPS firmware-query results. Other platforms retain their existing controller. V7 takes priority over PSCM Coefficient Observer while selected; disabling and cycling offroad/onroad restores the previous selection. Controller selection does not force lateral engagement.

The analyzed firmware is RL38-14D003-AA; removing the eligibility check is not validation of other firmware. No live device setting is changed.

Diagnostics and verification

The 5 Hz Ford C2-free path tracking event keeps its name and identifies v7. model_offset_base / model_heading_base report the already weighted and encoded model contribution; curvature_offset_base / curvature_heading_base report the residual-curvature contribution. model_share and base_guard identify model-pose, blended, curvature-only, opposed-model and zero-request cases. offset_target is the final bounded C0 target, heading_base the bounded pre-feedback C1, and heading_target the corrected C1 target.

The event retains source timestamps, measured curvature/yaw, final commands, slew scales, feedback bias/status/history, raw torque and PSCM status/age. feedback_backoff_active records the persistent heading ceiling, including cycles whose feedback status is no_new_measurement. feedback_recovery_active records an accepted recovery increment on this update only; it does not persist between measurements. feedback_yaw_error retains its delayed-reference meaning. Recovery instead uses current error, reconstructed from logged desired_curvature, synchronized car-state speed and yaw_rate; those two errors can differ. During backoff, heading_target can be lower in the request direction than the bounded sum of heading_base and heading_bias, because the temporary ceiling is not part of the stored bias. model_heading_target remains a filtered comparison reference; it is not the weighted model contribution. angleState.saturated is not an EPS-limit signal.

Validation must cover large recorded maneuvers, flat-model centering, both turn directions, model/action disagreement, share transitions, release and reversal, release/limit backoff without growth or zero crossing, status/driver resets, reference causality, bounds, slew and CAN packing with C2/C3 zero. Recovery checks cover both directions, stopping at zero bias, repeated measurements, current-and-delayed agreement, and rejection at PSCM limit 2. Old v3/v4 command-equality expectations do not define v7 success. Historical v5/v6 replay results remain historical observations. Replay fixes recorded motion and planner outputs, so enabled vehicle logs are still required to assess tracking error, oscillation and interventions.