diff --git a/docs/ford_virtual_angle_experiment.md b/docs/ford_virtual_angle_experiment.md index daa75bc2af..c7d73ab22f 100644 --- a/docs/ford_virtual_angle_experiment.md +++ b/docs/ford_virtual_angle_experiment.md @@ -1,181 +1,208 @@ -# Ford C2-free path tracking with measured feedback +# Ford C2-free model-pose tracking with measured feedback -Hypothesis `curvature-c0-c1-feedback-v5` retains v4's absolute desired-curvature -C0/C1 requests and adds a bounded yaw-error integral to C1. Persistent measured -shortfall can increase the request without requiring the planner to keep -increasing curvature; excess turning can reduce it. C0 and its centering -input are unchanged. C2/C3 remain zero. +Hypothesis `model-pose-c0-c1-feedback-v6` restores the existing allocator's +model-path C0/C1 demand for large turns when model geometry and selected +curvature agree. The remaining selected curvature becomes C0/C1 centering +and turn demand; C2/C3 stay zero. Selected desired curvature remains the +measured-yaw feedback target, even when model geometry supplies the base. -This is an experimental outer feedback loop around the multivariable PSCM. -It is not an angle servo or a calibrated C0/C1-to-wheel mapping. Its feedback -scale and response interval are not road-validated. +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. ## Evidence and scope -Route80 ran v3 (`98662df40`) 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 ratios, not gains or percentages of a maneuver completed. -Wheel-model curvature agrees on the directions of these discrepancies. +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 (`0ace0b051`) replaced separate model-heading C1 with the selected curvature -reference and reduced C1 in several large maneuvers. The user then reported -steering repeatedly stopping near 85 degrees. No recording of that new symptom -was available during implementation. Older logs contain much larger wheel -angles. The inspected host path has no fixed 85-degree wheel stop, but an -upstream speed-dependent curvature limit remains. +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. -Restoring larger C1 everywhere also restores excess demand in known -overshoot cases and barely changes some sustained shortfalls. V5 tests whether -measured correction can distinguish them. Replaying old motion verifies -command construction, not the truck's counterfactual response or a fix for -the unrecorded plateau. +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. -Route83 had the sunnylink toggle on, but its current CarParams omitted the -EPS firmware responses. The former firmware eligibility check therefore -selected the default `FordPathController`; replay reproduced its recorded -C0/C1/C2 requests. Those favorable driving results do not validate v5. The -toggle now selects this experiment on the supported Lightning platform -without depending on firmware-query results. +## Base request -## Base request and feedback +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. -controlsd uses valid `lateralManeuverPlan.desiredCurvature`, otherwise -`modelV2.action.desiredCurvature`, after the existing curvature limiter: +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. ```text +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) -C0_target = clip(0.5 × desired_curvature × L0², ±5.11 m) -C1_base = clip(desired_curvature × L1, ±0.5 rad) - -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_target = clip(C1_base + accepted_bias, ±0.5 rad) +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) ``` -Measured yaw is negated Ford CAN yaw rate, matching the control sign convention. -The historical request uses zero-order hold, never interpolation toward a -command published later. Nominal delay is `CP.steerActuatorDelay` (0.2 s on -the source vehicle). This delays only the error comparison; it does not -advance the already delay-aware current action again. +`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 integration scale is **1.0**. Integrating compatible rad/s and rad units -does not make it gain-free or establish stability. Preview distances are also -effective gains. No adaptive wheel-response gain is identified. +The inherited pose encoder allocates heading overflow using its asymmetric +limits (+0.5235/−0.5 rad), before v6 applies 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 + +```text +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 bounds | ±5.11 m / ±0.5 rad | +| C0 / C1 final bounds | ±5.11 m / ±0.5 rad | | Independent C0 / C1 slew | 4 m/s / 0.5 rad/s | -| New feedback scale | 1.0 | +| Feedback integration scale | 1.0 | | Feedback minimum speed | 2 m/s | -| Maximum PSCM status age | 150 ms | +| Maximum PSCM/core input age | 150 ms | | Allowed timestamp lead | 5 ms | | Release comparison tolerance | one C1 wire quantum, 0.0005 rad | -Zero yaw error retains the acquired bias and absolute base. Reaching the target -does not remove the additional demand that may be sustaining the turn. +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. -## Release and limits +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. Other error directions remain frozen, and existing +host field and slew limits still apply. -When the clipped base magnitude decreases, bias decreases in the same ratio. -Using the clipped base avoids increasing total C1 by shrinking a negative bias -while the base stays saturated. Zero request or reversal clears bias and -requires fresh reference history. Existing output slew still applies. - -Integration is inhibited if the delayed request has the old sign or exceeds -the current request by more than one heading quantum after multiplying the -curvature difference by L1. This prevents old demand from rebuilding correction -during release while tolerating sub-quantum planner changes. - -Host anti-windup considers both field and slew limits on the combined base -and trial bias. It admits only reachable correction in the intended increment -direction when an outward update hits a host limit, and permits inward -unwinding. A large error must not stall correction merely because its entire -increment cannot fit within one tick. Retained bias is bounded by available -C1 field headroom. No overflow goes into C0; no safety limit is raised. - -Actual PSCM LimitReached freezes all new integration while base-driven -release continues. C1 sign is not interpreted as motor-effort direction. -Host bounds and generic status cannot identify internal PSCM dynamics or -guarantee prevention of downstream windup. +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 the existing Ford parser's `Lane_Assist_Data3_FD1` status in -`carStateSP.fordPscmStatus`, using its original CAN receipt timestamp. -Republishing carStateSP or receiving unrelated CAN frames cannot refresh it. -The opendbc submodule is unchanged. +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 feedback bias/history. Base control keeps its existing validity rules. -LimitReached (2) freezes integration; LimitWithDriverActive (3) clears feedback. +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 also clear feedback immediately. The baseline request retains -its existing PSCM driver-arbitration behavior while lateral control remains -authorized. An unset override flag cannot exclude subthreshold driver influence. -A fresh reference interval is required after override. +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. -Below 2 m/s feedback clears; base C0/C1 keep their original speed gates. -The correction does not learn a persistent zero-request bias. It is not a -complete zero-yaw or lane-centering servo; centering intent continues to enter -through selected desired curvature and C0. - -## Existing gates and selection +## Gates and Sunnylink selection Core model/action/car-state freshness, finite-value, clock and speed checks -are unchanged. Invalid core inputs reset both commands and clear latActive. -Model geometry remains for diagnostics and its existing validity gate; its -filtered heading does not command C1. +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 existing -`FordVirtualAngleController` key and default-off setting. An already-enabled -setting selects v5 after updating and restarting controlsd. The toggle -controls selection on Ford CAN FD `FORD_F_150_LIGHTNING_MK1`: missing or -different EPS firmware-query results no longer cause a fallback. Other -platforms retain their existing controller. V5 takes priority over PSCM -Coefficient Observer while selected. Turning it off and cycling offroad/onroad -restores the previous controller selection; changes are not applied live onroad. +Vehicle → Ford → **C2-Free Path Tracking (Experimental)** retains the +`FordVirtualAngleController` key, default-off setting and offroad/onroad cycle +requirement. Enabled selects v6 on Ford CAN FD `FORD_F_150_LIGHTNING_MK1` +regardless of missing or different EPS firmware-query results. Other platforms +retain their existing controller. V6 takes priority over PSCM Coefficient +Observer while selected; disabling and cycling offroad/onroad restores the +previous selection. Controller selection does not force lateral engagement. -Controller selection does not bypass lateral engagement, input-validity, -driver-override or fresh-PSCM-status requirements. The feedback eligibility -rules above still apply, and all C0/C1 bounds and C2/C3 behavior are unchanged. -The analyzed firmware remains `RL38-14D003-AA`; removing the selection check -does not establish validation on other firmware. No live device setting is -changed by this commit. +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 identifies v5 and records the -selected reference, measured curvature/yaw, base and corrected C1 targets, -bias, integration status, historical request/time, yaw error, raw torque and -PSCM freshness/status. `angleState.saturated` is not an EPS-limit substitute: -it describes tracking error on this path. +The 5 Hz `Ford C2-free path tracking` event keeps its name and identifies v6. +`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. -Checks cover deficit/excess response, retained turn demand, release/reversal, -driver/status resets, delayed history, repeated measurements, host/PSCM limits, -unchanged C0, C2/C3 zero, telemetry timestamps, CAN packing and recorded -maneuvers. Missing-status behavior preserves v4 commands. Replay holds recorded -motion and planner outputs fixed and cannot establish physical improvement -or closed-loop stability. +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`. +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. -The final v5 replay covered 52,273 route80 cycles with reconstructed causal -PSCM status and raw driver torque. C0 and output-validity gates were unchanged; -C2/C3 stayed zero and command bounds, slew and reference causality passed. -Median eligible C1 magnitude changed from 0.144 to 0.178 rad in the 333–339 s -shortfall and 0.190 to 0.267 rad in the 430–435 s shortfall. The 417–420 s -over-response window stayed at 0.286 rad: 102 of its 105 eligible samples -reported LimitReached, suppressing new integration. This is a known limitation -of the guarded candidate, not a demonstrated overshoot improvement. -The separate no-status replay preserved v4 C1 and old C0/gates exactly over -246,961 cycles across all 43 supplied segments. - -The next enabled logs must show whether tracking error diminishes without -oscillation, excess release overshoot or increased intervention. A shortfall -when the selected reference or available commands are already limited remains -a separate case. This feedback cannot create new physical authority. +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. +Old v3/v4 command-equality expectations do not define +v6 success. Historical v5 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. diff --git a/openpilot/selfdrive/controls/lib/ford_path.py b/openpilot/selfdrive/controls/lib/ford_path.py index 24d1deff78..3c923fd609 100644 --- a/openpilot/selfdrive/controls/lib/ford_path.py +++ b/openpilot/selfdrive/controls/lib/ford_path.py @@ -48,6 +48,15 @@ class FordPscmState: curvature: float = 0.0 +@dataclass(frozen=True) +class FordModelPose: + path_offset: float + path_angle: float + offset_horizon: float + curvature_demand: float + forward_angle: float + + def _finite(value: float) -> float: return float(value) if math.isfinite(value) else 0.0 @@ -122,8 +131,8 @@ def _bounded_feedback(feedforward: float, feedback: float, resolution: float, ze return float(np.clip(feedback, -limit, limit)) -def _encode_path(path: tuple[list[float], list[float], list[float], list[float]], desired_curvature: float, - current_curvature: float, curvature_delta: float, v_ego: float) -> FordPath: +def _model_pose(path: tuple[list[float], list[float], list[float], list[float]], + current_curvature: float, curvature_delta: float, v_ego: float) -> FordModelPose: distance, _, _, _ = path advance = min(v_ego * _POSE_PREDICTION_TIME, distance[-1]) offset_horizon = min(_PATH_MIN_LOOKAHEAD, distance[-1] - advance) @@ -145,26 +154,19 @@ def _encode_path(path: tuple[list[float], list[float], list[float], list[float]] offset_curvature = 2.0 * model_offset / max(offset_horizon, 1e-3) ** 2 angle_curvature = model_angle / max(angle_horizon, 1e-3) - pose_share = _blend_share(max(abs(offset_curvature), abs(angle_curvature), abs(desired_curvature))) + return FordModelPose(model_offset + feedback_offset, model_angle + feedback_angle, offset_horizon, + max(abs(offset_curvature), abs(angle_curvature)), model_angle) - # Match upstream's C2-only normal driving, then continuously transfer the - # command to the model pose for larger maneuvers. An opposing/finished model - # path must unload sticky C2 and retain the fast pose needed to unwind it. - c2_opposes_path = desired_curvature != 0.0 and desired_curvature * model_angle <= 0.0 - if c2_opposes_path: - pose_share = 1.0 - curvature = 0.0 - else: - curvature = desired_curvature * (1.0 - pose_share) - path_offset = pose_share * (model_offset + feedback_offset) - path_angle = pose_share * (model_angle + feedback_angle) +def _encode_pose(pose: FordModelPose, pose_share: float, curvature: float) -> FordPath: + path_offset = pose_share * pose.path_offset + path_angle = pose_share * pose.path_angle if abs(path_offset) < 0.5 * DBC_OFFSET_RESOLUTION: path_offset = 0.0 if abs(path_angle) < 0.5 * DBC_ANGLE_RESOLUTION: path_angle = 0.0 limited_path_angle = float(np.clip(path_angle, *DBC_ANGLE)) - path_offset += (path_angle - limited_path_angle) * offset_horizon + path_offset += (path_angle - limited_path_angle) * pose.offset_horizon return FordPath( valid=True, path_offset=float(np.clip(path_offset, *DBC_OFFSET)), @@ -174,6 +176,24 @@ def _encode_path(path: tuple[list[float], list[float], list[float], list[float]] ) +def _encode_path(path: tuple[list[float], list[float], list[float], list[float]], desired_curvature: float, + current_curvature: float, curvature_delta: float, v_ego: float) -> FordPath: + pose = _model_pose(path, current_curvature, curvature_delta, v_ego) + pose_share = _blend_share(max(pose.curvature_demand, abs(desired_curvature))) + + # Match upstream's C2-only normal driving, then continuously transfer the + # command to the model pose for larger maneuvers. An opposing/finished model + # path must unload sticky C2 and retain the fast pose needed to unwind it. + c2_opposes_path = desired_curvature != 0.0 and desired_curvature * pose.forward_angle <= 0.0 + if c2_opposes_path: + pose_share = 1.0 + curvature = 0.0 + else: + curvature = desired_curvature * (1.0 - pose_share) + + return _encode_pose(pose, pose_share, curvature) + + class FordPathController: """Blend normal C2 following into the model's forward C0/C1 pose.""" diff --git a/openpilot/selfdrive/controls/lib/ford_virtual_angle.py b/openpilot/selfdrive/controls/lib/ford_virtual_angle.py index b397ed2ed7..d2e65b807d 100644 --- a/openpilot/selfdrive/controls/lib/ford_virtual_angle.py +++ b/openpilot/selfdrive/controls/lib/ford_virtual_angle.py @@ -9,7 +9,7 @@ import math import struct import numpy as np -from openpilot.selfdrive.controls.lib.ford_path import FordPath, _model_path, _relative_pose, _predicted_pose +from openpilot.selfdrive.controls.lib.ford_path import FordPath, _blend_share, _encode_pose, _model_path, _model_pose, _relative_pose, _predicted_pose from opendbc.car.ford.values import CarControllerParams, FordFlags @@ -71,8 +71,9 @@ class HeadingFeedback: self.bias = 0. self.previous_base = None self.last_measurement_time = self.last_pscm_time = None + self.backoff_active = False self.diagnostics = {'heading_bias': 0., 'feedback_status': status, 'feedback_reference_time': None, - 'feedback_reference_curvature': None, 'feedback_yaw_error': None} + 'feedback_reference_curvature': None, 'feedback_yaw_error': None, 'feedback_backoff_active': False} def update(self, base, desired, *, yaw_rate, speed, now, measurement_time, dt, previous_command, heading_horizon, driver_override, pscm_status): reason = ('missing_pscm' if pscm_status is None else pscm_status.invalid_reason(now)) @@ -100,6 +101,7 @@ class HeadingFeedback: status = 'no_new_measurement' reference_time = reference_curvature = yaw_error = None if measurement_time != self.last_measurement_time: + self.backoff_active = False measurement_dt = 0. if self.last_measurement_time is None else measurement_time - self.last_measurement_time self.last_measurement_time = measurement_time target_time = measurement_time - self.delay @@ -114,14 +116,23 @@ class HeadingFeedback: reference_time, reference_curvature = reference yaw_error = speed * reference_curvature - yaw_rate releasing = reference_curvature * desired <= 0. or (abs(reference_curvature) - abs(desired)) * heading_horizon > HEADING_RESOLUTION - if releasing: - status = 'release' - elif pscm_status.limit >= 2: - # Freeze all integration: C1 direction is not measured motor effort. - # Base-driven release still removes stored demand as the action eases. - status = 'pscm_limit' + constrained = releasing or pscm_status.limit >= 2 + heading_before = base + self.bias + # Do not brake turn-in merely for exceeding an older, smaller request: + # measured turning must also exceed the current selected action. + current_yaw_error = speed * desired - yaw_rate + backoff = constrained and yaw_error * base < 0. and current_yaw_error * base < 0. and heading_before * base > 0. + if constrained and not backoff: + status = 'release' if releasing else 'pscm_limit' else: increment = self.tuning.feedback_gain * yaw_error * measurement_dt + if backoff: + # A release/limit may still reduce an excessive same-direction + # heading request. It cannot grow that request or cross through + # zero. This does not identify the PSCM's limiting mechanism or + # equate C1 with motor effort; all other status/driver gates apply. + reduced = float(np.clip(heading_before + increment, min(0., heading_before), max(0., heading_before))) + increment = reduced - heading_before proposed = base + self.bias + increment field_limited = float(np.clip(proposed, -.5, .5)) host_limited = previous_command + float(np.clip(field_limited - previous_command, @@ -135,10 +146,22 @@ class HeadingFeedback: else: self.bias += increment status = 'integrating' + if backoff: + self.backoff_active = True + status = 'release_backoff' if releasing else 'pscm_backoff' self.bias = float(np.clip(self.bias, -.5 - base, .5 - base)) + target = float(np.clip(base + self.bias, -.5, .5)) + if self.backoff_active: + # A rising geometry base or an unfinished slew must not outweigh + # backoff and increase the sent heading, even between measurements. + # Keep this temporary ceiling out of the integral: a new model base + # is not measured yaw error and must not create persistent suppression. + ceiling = max(0., math.copysign(1., base) * previous_command) + target = float(np.clip(target, -ceiling if base < 0. else 0., ceiling if base > 0. else 0.)) self.diagnostics = {'heading_bias': self.bias, 'feedback_status': status, 'feedback_reference_time': reference_time, - 'feedback_reference_curvature': reference_curvature, 'feedback_yaw_error': yaw_error} - return float(np.clip(base + self.bias, -.5, .5)) + 'feedback_reference_curvature': reference_curvature, 'feedback_yaw_error': yaw_error, + 'feedback_backoff_active': self.backoff_active} + return target class PathReference: @@ -194,11 +217,11 @@ class PathReference: class FordVirtualAngleController: - """Encode the same absolute planned curvature as C0 and C1. + """Retain the Ford model-pose turn request and encode centering without C2. - The former spatial-heading reference is retained for diagnostic comparison - and the existing input-validity gates. A bounded yaw-error integral corrects - C1 when fresh PSCM status permits; no fixed EPS gain is assumed. + The selected curvature gates model anticipation and remains the measured + tracking target. A bounded yaw-error integral corrects C1 when fresh PSCM + status permits; no fixed EPS gain is assumed. """ def __init__(self, response_delay=.2, tuning: PathTuning | None = None): self.tuning = tuning if tuning is not None else PathTuning() @@ -219,8 +242,9 @@ class FordVirtualAngleController: self.command = FordPath() self.last_time = None self.last_measurement_time = None + self.curvature_history = deque() self.offset_request = self.heading_request = 0.0 - self.diagnostics = {'status': 'inactive', 'hypothesis': 'curvature-c0-c1-feedback-v5', 'command': (0., 0., 0., 0.), + self.diagnostics = {'status': 'inactive', 'hypothesis': 'model-pose-c0-c1-feedback-v6', 'command': (0., 0., 0., 0.), **self.feedback.diagnostics} def update(self, model, desired_curvature, *, yaw_rate, speed, now, measurement_time, model_time, reference_time, @@ -245,7 +269,8 @@ class FordVirtualAngleController: self.last_time = now self.last_measurement_time = measurement_time path = self.reference.update(model, model_time=model_time, now=now, dt=dt, speed=speed, curvature=current_curvature) - if path is None or path[0][-1] <= 0: + raw_path = _model_path(model) + if path is None or raw_path is None or path[0][-1] <= 0: self.reset() self.diagnostics['status'] = 'invalid_path' return self.command @@ -255,15 +280,25 @@ class FordVirtualAngleController: model_heading_horizon = min(heading_horizon, max(path[0][-1] - advance, 0.0)) ego = _predicted_pose(advance, current_curvature, 0.) _, 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)) - # Absolute feedforward retains sustained turn demand. Measured tracking can - # add or subtract a bounded correction; model geometry remains diagnostic. - base_heading = float(np.clip(desired_curvature * heading_horizon, -.5, .5)) + self.curvature_history.append((now, current_curvature)) + while len(self.curvature_history) > 2 and self.curvature_history[1][0] <= now - .1: + self.curvature_history.popleft() + curvature_delta = current_curvature - self.curvature_history[0][1] if now - self.curvature_history[0][0] >= .1 else 0. + # Reuse the working allocator's raw forward geometry and bounded short-pose + # correction. Filtering that geometry again would delay the turn request. + pose = _model_pose(raw_path, current_curvature, curvature_delta, speed) + aligned = desired_curvature * pose.forward_angle > 0. + model_share = min(_blend_share(abs(desired_curvature)), _blend_share(pose.curvature_demand)) if aligned else 0. + model_base = _encode_pose(pose, model_share, 0.) + residual_curvature = desired_curvature * (1. - model_share) + curvature_offset = .5 * residual_curvature * offset_horizon ** 2 + curvature_heading = residual_curvature * heading_horizon + # This geometric lift replaces the remaining C2 request. It is not an EPS + # transfer-function equivalence or a fitted coefficient-to-wheel mapping. + target_offset = float(np.clip(model_base.path_offset + curvature_offset, -5.11, 5.11)) + base_heading = float(np.clip(model_base.path_angle + curvature_heading, -.5, .5)) + base_guard = ('zero_request' if desired_curvature == 0. else 'opposed_model' if not aligned else + 'curvature_only' if model_share == 0. else 'model_pose' if model_share == 1. else 'blended') driver_override = steering_pressed or not math.isfinite(steering_torque) or abs(steering_torque) > CarControllerParams.STEER_DRIVER_ALLOWANCE target_heading = self.feedback.update(base_heading, desired_curvature, yaw_rate=yaw_rate, speed=speed, now=now, measurement_time=measurement_time, dt=dt, previous_command=self.heading_request, @@ -278,8 +313,11 @@ 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 driver_override else 'active', 'hypothesis': 'curvature-c0-c1-feedback-v5', + self.diagnostics = {'status': 'driver_override' if driver_override else 'active', 'hypothesis': 'model-pose-c0-c1-feedback-v6', 'desired_curvature': desired_curvature, 'offset_target': target_offset, 'heading_target': target_heading, + 'model_offset_base': model_base.path_offset, 'model_heading_base': model_base.path_angle, + 'curvature_offset_base': curvature_offset, 'curvature_heading_base': curvature_heading, + 'model_share': model_share, 'base_guard': base_guard, 'heading_base': base_heading, 'feedback_gain': self.tuning.feedback_gain, 'feedback_min_speed': FEEDBACK_MIN_SPEED, 'steering_torque': steering_torque if math.isfinite(steering_torque) else None, 'pscm_valid': pscm_status.valid if pscm_status is not None else False, diff --git a/openpilot/selfdrive/controls/tests/fixtures/ford_large_turn_requests_route83.json b/openpilot/selfdrive/controls/tests/fixtures/ford_large_turn_requests_route83.json new file mode 100644 index 0000000000..42a2e512e0 --- /dev/null +++ b/openpilot/selfdrive/controls/tests/fixtures/ford_large_turn_requests_route83.json @@ -0,0 +1,247 @@ +{ + "description": "Signal-only historical fallback evidence and frozen-v5 comparison; no GPS or inferred counterfactual vehicle response.", + "route": "route83", + "recorded_commit": "79a4caa1f6b71488949108aee9ae6ae6566347b1", + "fixture_sha256": "d00312c430ace47000c05b8284ee8d56df56ec24bb17a9ea8f4dce83133527c3", + "samples": 11744, + "model_count": 2367, + "source_cache_sha256": "53d786aff2e0b6338e1991320145305fda3101f7b76e50bd2929adbcaea95b28", + "response_delay": 0.20000000298023224, + "episodes": [ + [ + 1861.2933736250002, + 1874.756970279 + ], + [ + 1878.07372132, + 1892.07372132 + ], + [ + 1950.874232409, + 1964.874232409 + ], + [ + 2440.9020600930003, + 2456.964158177 + ], + [ + 2580.722658577, + 2611.364366768 + ], + [ + 2734.478264791, + 2764.574374172 + ] + ], + "windows": [ + { + "name": "successful_large_early", + "role": "authority_target", + "range_s": [ + 1866.722720383, + 1874.756970279 + ], + "samples": 426, + "substantial_demand_required": true, + "recorded_can_ratio_02s_median": 1.0233371460413845, + "published_median_abs_c0_c1": [ + 1.6002928018569946, + 0.2796146124601364 + ], + "send_clamped_median_abs_c0_c1": [ + 1.6002928018569946, + 0.2796146124601364 + ], + "phase_samples": { + "phase_turn_in": 15, + "phase_held": 122, + "phase_release": 402, + "phase_reversal": 0 + } + }, + { + "name": "centering_reversal_positive_to_negative", + "role": "reversal", + "range_s": [ + 1888.07372132, + 1892.07372132 + ], + "samples": 396, + "substantial_demand_required": false, + "recorded_can_ratio_02s_median": null, + "published_median_abs_c0_c1": [ + 0.0, + 0.0 + ], + "send_clamped_median_abs_c0_c1": [ + 0.0, + 0.0 + ], + "phase_samples": { + "phase_turn_in": 283, + "phase_held": 48, + "phase_release": 104, + "phase_reversal": 21 + } + }, + { + "name": "centering_reversal_negative_to_positive", + "role": "reversal", + "range_s": [ + 1960.874232409, + 1964.874232409 + ], + "samples": 397, + "substantial_demand_required": false, + "recorded_can_ratio_02s_median": null, + "published_median_abs_c0_c1": [ + 0.0, + 0.0 + ], + "send_clamped_median_abs_c0_c1": [ + 0.0, + 0.0 + ], + "phase_samples": { + "phase_turn_in": 154, + "phase_held": 0, + "phase_release": 183, + "phase_reversal": 21 + } + }, + { + "name": "clean_release", + "role": "release", + "range_s": [ + 2453.714158177, + 2456.964158177 + ], + "samples": 323, + "substantial_demand_required": false, + "recorded_can_ratio_02s_median": null, + "published_median_abs_c0_c1": [ + 0.0, + 0.0 + ], + "send_clamped_median_abs_c0_c1": [ + 0.0, + 0.0 + ], + "phase_samples": { + "phase_turn_in": 4, + "phase_held": 0, + "phase_release": 305, + "phase_reversal": 17 + } + }, + { + "name": "successful_smaller_positive", + "role": "sign_coverage_only", + "range_s": [ + 2590.722658577, + 2600.918740146 + ], + "samples": 175, + "substantial_demand_required": true, + "recorded_can_ratio_02s_median": 1.0960646334373787, + "published_median_abs_c0_c1": [ + 0.42173025012016296, + 0.1222948431968689 + ], + "send_clamped_median_abs_c0_c1": [ + 0.42173025012016296, + 0.1222948431968689 + ], + "phase_samples": { + "phase_turn_in": 170, + "phase_held": 61, + "phase_release": 0, + "phase_reversal": 0 + } + }, + { + "name": "large_under_response", + "role": "under_response_challenge", + "range_s": [ + 2604.2254721, + 2611.364366768 + ], + "samples": 128, + "substantial_demand_required": true, + "recorded_can_ratio_02s_median": 0.7322859508492778, + "published_median_abs_c0_c1": [ + 2.4204851388931274, + 0.42145511507987976 + ], + "send_clamped_median_abs_c0_c1": [ + 2.4204851388931274, + 0.42145511507987976 + ], + "phase_samples": { + "phase_turn_in": 68, + "phase_held": 96, + "phase_release": 56, + "phase_reversal": 0 + } + }, + { + "name": "successful_large_181deg", + "role": "authority_target", + "range_s": [ + 2744.478264791, + 2750.573209708 + ], + "samples": 207, + "substantial_demand_required": true, + "recorded_can_ratio_02s_median": 1.0087938914780248, + "published_median_abs_c0_c1": [ + 2.1044259071350098, + 0.3815947473049164 + ], + "send_clamped_median_abs_c0_c1": [ + 2.1044259071350098, + 0.3815947473049164 + ], + "phase_samples": { + "phase_turn_in": 137, + "phase_held": 94, + "phase_release": 64, + "phase_reversal": 0 + } + }, + { + "name": "large_over_response_290deg", + "role": "over_response_challenge_not_target", + "range_s": [ + 2760.493612962, + 2764.574374172 + ], + "samples": 181, + "substantial_demand_required": true, + "recorded_can_ratio_02s_median": 1.2515789463064766, + "published_median_abs_c0_c1": [ + 4.737145900726318, + 0.5235000252723694 + ], + "send_clamped_median_abs_c0_c1": [ + 4.737145900726318, + 0.5 + ], + "phase_samples": { + "phase_turn_in": 139, + "phase_held": 90, + "phase_release": 41, + "phase_reversal": 0 + } + } + ], + "selection": "Authority targets require automatic turn windows with >=1 second strict torque eligibility, eligible |wheel|>=150 degrees, and whole-window CAN response ratio median 0.90..1.10 at fixed 0.2 s. No positive-request large turn qualifies.", + "non_targets": "Positive smaller turn supplies sign coverage only. Under/over response and release/reversal windows are regression challenges, not authority targets.", + "context": "At least 10 s pre-roll or available route start, extended to include the preceding feedback reset/sign reversal. Overlapping intervals are merged. First episode begins at the partial route boundary with unobserved earlier history.", + "phase_policy": "Held means request curvature range over +/-0.25 s times speed squared <0.15 m/s2 at demand>=0.5. Turn-in/release compare current absolute curvature with the historical held request at measurement_time-delay, scaled by max(7,speed), using +/-0.0005 rad. These masks can overlap held; reversal means opposing delayed/current signs.", + "wire_policy": "Published coefficients preserve Float32 values. Send-clamped copy caps C0 to +/-5.11 and C1 to +/-0.5 before packing. Actual decoded wire is normalized to controller sign, nearest within 15 ms; wire_time/fresh/mode expose timing approximation.", + "model_schema": "models[model_index] contains position.x, position.y, orientation.z; Float32 conversion preserves the original model payload precision.", + "v5_reference": "Frozen full sequential replay from command_replay.npz, whose source hash and limitations are recorded in command_replay.json.", + "frozen_v5_revision": "09acf8ec2f327769f00ee53563ad2dd9225e37a7", + "preroll_validation": "Compact reset replay exactly matches full sequential frozen-v5 C0/C1, gates and bias on all 2233 evidence samples." +} diff --git a/openpilot/selfdrive/controls/tests/fixtures/ford_large_turn_requests_route83.npz b/openpilot/selfdrive/controls/tests/fixtures/ford_large_turn_requests_route83.npz new file mode 100644 index 0000000000..f2faab29c4 Binary files /dev/null and b/openpilot/selfdrive/controls/tests/fixtures/ford_large_turn_requests_route83.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 e44672c01b..43af68e744 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py +++ b/openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py @@ -62,15 +62,66 @@ 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-feedback-v5') + self.assertEqual(record['hypothesis'], 'model-pose-c0-c1-feedback-v6') self.assertEqual(record['command'], list(controller.diagnostics['command'])) + self.assertIs(record['feedback_backoff_active'], False) 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.assertAlmostEqual(record['heading_target'], .1) + self.assertEqual(record['base_guard'], 'blended') + self.assertGreater(record['model_share'], 0.) + self.assertLess(record['model_share'], 1.) + self.assertEqual(record['heading_target'], record['heading_base']) # missing PSCM status leaves the base intact 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'))) + 'model_age', 'reference_age', 'reference_filter_time', 'model_offset_base', 'model_heading_base', + 'curvature_offset_base', 'curvature_heading_base', 'model_share', 'base_guard'))) + + def test_periodic_diagnostics_distinguish_model_curvature_and_blended_bases(self): + for desired, geometry, guard, share in ((.02, .02, 'model_pose', 1.), (.002, .002, 'curvature_only', 0.), + (.01, .01, 'blended', 2 / 3), (-.02, .02, 'opposed_model', 0.), + (.002, 0., 'opposed_model', 0.), (0., .02, 'zero_request', 0.)): + with self.subTest(desired=desired, geometry=geometry): + controller = FordVirtualAngleController() + controller.update(circle(geometry), desired, yaw_rate=0., speed=10., now=1., measurement_time=1., + model_time=1., reference_time=1., active=True) + controls = SimpleNamespace(ford_path_controller=controller, desired_curvature=desired, curvature=0., + sm=SimpleNamespace(logMonoTime={'modelV2': 1_000_000_000, 'carState': 1_000_000_000})) + record = self.emit_controls_event('Ford C2-free path tracking', controls) + self.assertEqual(record['base_guard'], guard) + self.assertAlmostEqual(record['model_share'], share) + for key in ('model_offset_base', 'model_heading_base', 'curvature_offset_base', 'curvature_heading_base'): + self.assertEqual(record[key], controller.diagnostics[key]) + self.assertAlmostEqual(record['offset_target'], record['model_offset_base'] + record['curvature_offset_base']) + self.assertAlmostEqual(record['heading_base'], record['model_heading_base'] + record['curvature_heading_base']) + self.assertEqual(record['feedback_status'], 'missing_pscm') + self.assertEqual(record['heading_bias'], 0.) + self.assertEqual(record['command'][2:], [0., 0.]) + if share == 0.: + self.assertEqual((record['model_offset_base'], record['model_heading_base']), (0., 0.)) + if share == 1.: + self.assertEqual((record['curvature_offset_base'], record['curvature_heading_base']), (0., 0.)) + + def test_periodic_diagnostics_log_backoff_between_measurements(self): + controller = FordVirtualAngleController() + for i in range(50): + now = 1. + i * .01 + controller.update(circle(.02), .02, yaw_rate=.2, speed=10., now=now, measurement_time=now, + model_time=now, reference_time=now, active=True, pscm_status=PscmStatus(now, 2, 0, 2, False)) + cases = ((1.5, .02, 1.5, 'pscm_backoff', True), (1.51, .02, 1.5, 'no_new_measurement', True), + (1.52, .05, 1.52, 'pscm_limit', False)) + for now, desired, measurement, expected_status, backoff in cases: + controller.update(circle(.03), desired, yaw_rate=.5, speed=10., now=now, measurement_time=measurement, + model_time=now, reference_time=now, active=True, pscm_status=PscmStatus(now, 2, 2, 2, False)) + controls = SimpleNamespace(ford_path_controller=controller, desired_curvature=desired, curvature=.05, + sm=SimpleNamespace(logMonoTime={'modelV2': int(now * 1e9), 'carState': int(measurement * 1e9)})) + record = self.emit_controls_event('Ford C2-free path tracking', controls) + self.assertEqual(record['feedback_status'], expected_status) + self.assertIs(record['feedback_backoff_active'], backoff) + self.assertEqual(record['heading_bias'], controller.diagnostics['heading_bias']) + if backoff: + # The output ceiling is observable separately from the stored integral. + self.assertLess(record['heading_target'], record['heading_base'] + record['heading_bias']) 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 fb0ef14ac6..0b12c76948 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_curvature_c0.py +++ b/openpilot/selfdrive/controls/tests/test_ford_curvature_c0.py @@ -2,7 +2,7 @@ import math import unittest -from openpilot.selfdrive.controls.lib.ford_path import FordPath +from openpilot.selfdrive.controls.lib.ford_path import FordPath, FordPathController from openpilot.selfdrive.controls.lib.ford_virtual_angle import FordVirtualAngleController from openpilot.selfdrive.controls.tests.test_ford_path_reference import circle @@ -29,9 +29,13 @@ class TestFordCurvatureC0(unittest.TestCase): for speed in (2., 4., 6.): for sign in (-1, 1): controller = FordVirtualAngleController() + baseline = FordPathController() + model = circle(sign * .04) for i in range(250): - path = step(controller, i * .01, sign * .04, circle(sign * .04), speed, sign * .04 * speed) - self.assertAlmostEqual(path.path_offset, sign * 1.28, delta=.0051) + path = step(controller, i * .01, sign * .04, model, speed, sign * .04 * speed) + recorded_base = baseline.update(model, sign * .04, current_curvature=sign * .04, v_ego=speed) + self.assertAlmostEqual(path.path_offset, recorded_base.path_offset, delta=.0051) + self.assertGreater(sign * path.path_offset, .9) self.assertGreater(sign * path.path_angle, .2) def test_model_heading_cannot_inject_commands_when_action_requests_zero(self): @@ -45,13 +49,17 @@ class TestFordCurvatureC0(unittest.TestCase): def test_c1_reversal_cannot_delay_action_c0_release(self): controller = FordVirtualAngleController() + # A shallow lateral displacement with a stronger heading request exercises + # independent release: the small C0 move must finish before the C1 slew. + model = circle(.06) + model.position.y *= .1 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.) + path = step(controller, i * .01, .04, model, speed=5.) + for i in range(200, 240): + path = step(controller, i * .01, .003125, model, speed=5.) self.assertAlmostEqual(path.path_offset, .1) - self.assertAlmostEqual(path.path_angle, .1) - for i in range(280, 283): + self.assertGreater(path.path_angle, .2) + for i in range(240, 243): path = step(controller, i * .01, 0., circle(-.12), speed=5.) self.assertAlmostEqual(path.path_offset, 0., delta=.0051) self.assertGreater(path.path_angle, .08) # C1 is still in its own limited transition. @@ -61,7 +69,8 @@ class TestFordCurvatureC0(unittest.TestCase): model = circle(.04) for i in range(200): path = step(controller, i * .01, .01, model) - for i in range(200, 240): + # Allow the bounded larger initial C1 request to cross zero at 0.5 rad/s. + for i in range(200, 320): path = step(controller, i * .01, -.01, model) self.assertLess(path.path_offset, -.3) self.assertLess(path.path_angle, -.07) diff --git a/openpilot/selfdrive/controls/tests/test_ford_curvature_heading.py b/openpilot/selfdrive/controls/tests/test_ford_curvature_heading.py index 2601464e7f..31e504b318 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_curvature_heading.py +++ b/openpilot/selfdrive/controls/tests/test_ford_curvature_heading.py @@ -12,8 +12,8 @@ class TestFordCurvatureHeading(unittest.TestCase): 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): + self.assertAlmostEqual(path.path_angle, .5, delta=.000251) + for i in range(200, 330): 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) @@ -32,18 +32,22 @@ class TestFordCurvatureHeading(unittest.TestCase): model = circle(.12) for i in range(200): path = step(controller, i * .01, .04, model, speed=5.) - for i in range(200, 320): + for i in range(200, 360): 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): + def test_forward_geometry_supplies_large_turns_only_while_aligned(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) + for i in range(200): + plain = step(straight, i * .01, .04, circle(), speed=5.) + turn = step(bent, i * .01, .04, circle(.065), speed=5.) + self.assertGreater(turn.path_offset, plain.path_offset) + self.assertGreater(turn.path_angle, plain.path_angle) + for i in range(200, 400): + plain = step(straight, i * .01, -.04, circle(), speed=5.) + turn = step(bent, i * .01, -.04, circle(.065), speed=5.) + self.assertEqual(turn, plain) if __name__ == '__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 index 633289608e..16b230978b 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_curvature_heading_routes.py +++ b/openpilot/selfdrive/controls/tests/test_ford_curvature_heading_routes.py @@ -19,7 +19,7 @@ class TestFordCurvatureHeadingRoutes(unittest.TestCase): 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 = [], [], [] + commands, gates, statuses, biases = [], [], [], [] for i, now in enumerate(data['t']): if data['episode'][i] != previous_episode: controller = FordVirtualAngleController() @@ -32,29 +32,35 @@ class TestFordCurvatureHeadingRoutes(unittest.TestCase): commands.append((command.path_offset, command.path_angle, command.curvature, command.curvature_rate)) gates.append(command.valid) statuses.append(controller.diagnostics['status']) + biases.append(controller.diagnostics['heading_bias']) cls.commands = np.array(commands) cls.gates = np.array(gates) cls.statuses = np.array(statuses) + cls.biases = np.array(biases) - def test_c0_and_output_gates_match_frozen_v3(self): - np.testing.assert_array_equal(self.commands[:, 0], self.data['v3_replay'][:, 0]) + def test_output_gates_match_frozen_v3(self): 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)): + def test_missing_pscm_retains_bounded_base_without_integrating(self): + # These older inputs omit PSCM status. They must retain a usable base and + # normal output guards without inventing feedback eligibility. Large-turn + # authority and measured backoff have separate route83 evidence fixtures. + np.testing.assert_array_equal(self.biases, 0.) + self.assertTrue(np.isfinite(self.commands).all()) + self.assertLessEqual(float(np.max(abs(self.commands[:, 0]))), 5.11 + 1e-9) + self.assertLessEqual(float(np.max(abs(self.commands[:, 1]))), .5 + 1e-9) + np.testing.assert_array_equal(self.commands[~self.gates], 0.) + for episode in range(3): 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) + self.assertGreater(float(np.median(abs(self.commands[mask, 1]))), .03) + continuing = self.gates[1:] & self.gates[:-1] & (np.diff(self.data['episode']) == 0) + elapsed = np.diff(self.data['t'])[continuing] + steps = abs(np.diff(self.commands[:, :2], axis=0))[continuing] + self.assertTrue(np.all(steps[:, 0] <= 4. * elapsed + .010001)) + self.assertTrue(np.all(steps[:, 1] <= .5 * elapsed + .000501)) if __name__ == '__main__': diff --git a/openpilot/selfdrive/controls/tests/test_ford_heading_feedback.py b/openpilot/selfdrive/controls/tests/test_ford_heading_feedback.py index 4a73cd24ab..16c4dda2cd 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_heading_feedback.py +++ b/openpilot/selfdrive/controls/tests/test_ford_heading_feedback.py @@ -10,7 +10,7 @@ import numpy as np from opendbc.can import CANPacker, CANParser from opendbc.car.ford.fordcan import CanBus, create_lat_ctl2_msg from openpilot.cereal import custom -from openpilot.selfdrive.controls.lib.ford_virtual_angle import FordVirtualAngleController, PscmStatus +from openpilot.selfdrive.controls.lib.ford_virtual_angle import FordVirtualAngleController, HeadingFeedback, PathTuning, PscmStatus MODEL = SimpleNamespace(position=SimpleNamespace(x=np.linspace(0., 100., 33), y=np.zeros(33)), @@ -36,6 +36,31 @@ def warm(controller, desired=.02, yaw_rate=.08, speed=8., count=200): class TestFordHeadingFeedback(unittest.TestCase): + def test_limited_backoff_cannot_grow_the_command_when_model_base_rises(self): + for sign in (-1, 1): + feedback = HeadingFeedback(.2, PathTuning()) + previous = sign * .2 + for i in range(40): + now = i * .01 + previous = feedback.update(sign * .2, sign * .02, yaw_rate=sign * .1, speed=5., now=now, + measurement_time=now, dt=.01, previous_command=previous, heading_horizon=7., + driver_override=False, pscm_status=PscmStatus(now, 2, 0, 2, False)) + # A larger model base must not defeat the measured backoff by outweighing + # its subtractive integral increment while the PSCM is already limited. + target = feedback.update(sign * .4, sign * .02, yaw_rate=sign * .3, speed=5., now=.4, + measurement_time=.4, dt=.01, previous_command=previous, heading_horizon=7., + driver_override=False, pscm_status=PscmStatus(.4, 2, 2, 2, False)) + self.assertGreaterEqual(sign * target, 0.) + self.assertLessEqual(sign * target, sign * previous) + # A model-base change is not measured yaw error. Its temporary output + # ceiling must not become a persistent, artificially large integral. + self.assertAlmostEqual(sign * feedback.bias, -.002) + repeated = feedback.update(sign * .5, sign * .02, yaw_rate=sign * .3, speed=5., now=.41, + measurement_time=.4, dt=.01, previous_command=target, heading_horizon=7., + driver_override=False, pscm_status=PscmStatus(.41, 2, 2, 2, False)) + self.assertGreaterEqual(sign * repeated, 0.) + self.assertLessEqual(sign * repeated, sign * target) + def test_under_and_over_response_change_only_heading(self): for sign in (-1, 1): deficient = FordVirtualAngleController() @@ -58,16 +83,36 @@ class TestFordHeadingFeedback(unittest.TestCase): self.assertAlmostEqual(command.path_angle, .16, delta=.0005) self.assertAlmostEqual(command.path_offset, .64, delta=.01) - def test_generic_eps_limit_freezes_both_error_directions(self): - for yaw_rate in (0., .4): + def test_generic_eps_limit_blocks_growth_but_allows_same_direction_backoff(self): + for sign in (-1, 1): + for yaw_rate in (0., .4): + controller = FordVirtualAngleController() + before = warm(controller, desired=sign * .02, yaw_rate=sign * .08) + previous = sign * before.path_angle + for i in range(200, 500): + now = i * .01 + command = step(controller, now, desired=sign * .02, yaw_rate=sign * yaw_rate, + pscm_status=PscmStatus(now, 2, 2, 2, False)) + self.assertGreaterEqual(sign * command.path_angle, -.000501) + self.assertLessEqual(sign * command.path_angle, previous + .000501) + previous = sign * command.path_angle + if yaw_rate == 0.: + self.assertAlmostEqual(command.path_angle, before.path_angle, delta=.0005) + if yaw_rate > 0.: + self.assertAlmostEqual(command.path_angle, 0., delta=.0005) + + def test_release_allows_backoff_without_rebuilding_turn_demand(self): + for sign in (-1, 1): controller = FordVirtualAngleController() - before = warm(controller) - self.assertGreater(before.path_angle, .20) - for i in range(200, 260): - now = i * .01 - command = step(controller, now, yaw_rate=yaw_rate, - pscm_status=PscmStatus(now, 2, 2, 2, False)) - self.assertAlmostEqual(command.path_angle, before.path_angle, delta=.0005) + warm(controller, desired=sign * .04, yaw_rate=sign * .32) + previous = .32 + for i in range(200, 219): + command = step(controller, i * .01, desired=sign * .035, yaw_rate=sign * .5) + self.assertGreaterEqual(sign * command.path_angle, -.000501) + self.assertLessEqual(sign * command.path_angle, previous + .000501) + previous = sign * command.path_angle + self.assertLess(sign * controller.diagnostics['heading_bias'], 0.) + self.assertEqual(controller.diagnostics['feedback_status'], 'release_backoff') def test_ineligible_feedback_clears_bias_and_requires_fresh_history(self): # These guards affect feedback eligibility, while the existing base path @@ -168,8 +213,9 @@ class TestFordHeadingFeedback(unittest.TestCase): # Both requests give clipped base C1=.5. Scaling a negative bias by the raw # curvature reduction would increase total C1 during a release. after = step(controller, 2., desired=.09, yaw_rate=1., pscm_status=PscmStatus(2., 2, 2, 2, False)) - self.assertAlmostEqual(controller.diagnostics['heading_bias'], before_bias, places=10) - self.assertAlmostEqual(after.path_angle, before.path_angle, delta=.0005) + self.assertLessEqual(controller.diagnostics['heading_bias'], before_bias) + self.assertLessEqual(after.path_angle, before.path_angle + .0005) + self.assertGreaterEqual(after.path_angle, 0.) def test_host_field_limit_prevents_hidden_integral_growth(self): controller = FordVirtualAngleController() @@ -242,36 +288,39 @@ class TestFordHeadingFeedback(unittest.TestCase): np.testing.assert_array_equal(data['t'], eps['t']) models = [SimpleNamespace(position=SimpleNamespace(x=p[0], y=p[1]), orientation=SimpleNamespace(z=p[2])) for p in data['models']] previous_episode = None - commands, biases, gates = [], [], [] - limit_freezes = 0 + commands, bases, biases, gates = [], [], [], [] + limit_guards = 0 for i, now in enumerate(data['t']): if data['episode'][i] != previous_episode: controller = FordVirtualAngleController() + base_controller = FordVirtualAngleController(tuning=PathTuning(feedback_gain=0.)) previous_episode = data['episode'][i] pscm = PscmStatus(float(eps['pscm_timestamp'][i]), int(eps['lateral_state'][i]), int(eps['limit'][i]), int(eps['capability'][i]), bool(eps['denied'][i]), bool(eps['valid'][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]), steering_torque=float(eps['steering_torque'][i]), pscm_status=pscm) + inputs = {'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]), 'steering_torque': float(eps['steering_torque'][i]), 'pscm_status': pscm} + command = controller.update(models[data['model_index'][i]], data['desired_curvature'][i], **inputs) + base = base_controller.update(models[data['model_index'][i]], data['desired_curvature'][i], **inputs) commands.append((command.path_offset, command.path_angle, command.curvature, command.curvature_rate)) + bases.append((base.path_offset, base.path_angle)) gates.append(command.valid) biases.append(controller.diagnostics['heading_bias']) if pscm.limit == 2: self.assertNotEqual(controller.diagnostics['feedback_status'], 'integrating') - limit_freezes += controller.diagnostics['feedback_status'] == 'pscm_limit' - commands, biases = np.array(commands), np.array(biases) - np.testing.assert_array_equal(commands[:, 0], data['v3_replay'][:, 0]) + limit_guards += controller.diagnostics['feedback_status'] in ('pscm_limit', 'pscm_backoff') + commands, bases, biases = np.array(commands), np.array(bases), np.array(biases) + np.testing.assert_array_equal(commands[:, 0], bases[:, 0]) np.testing.assert_array_equal(gates, data['v3_valid']) np.testing.assert_array_equal(commands[:, 2:], 0.) - self.assertGreater(limit_freezes, 0) + self.assertGreater(limit_guards, 0) for episode in (0, 2): mask = (data['episode'] == episode) & data['evidence'] & data['benchmark_clean'] # Recorded motion is frozen: this establishes correction direction only, # not that a new vehicle drive will close the observed tracking deficit. self.assertGreater(float(np.max(biases[mask])), .001) - self.assertGreater(float(np.max(commands[mask, 1] - data['expected_common_c1'][mask])), .005) + self.assertGreater(float(np.max(commands[mask, 1] - bases[mask, 1])), .005) if __name__ == '__main__': diff --git a/openpilot/selfdrive/controls/tests/test_ford_large_maneuver_base.py b/openpilot/selfdrive/controls/tests/test_ford_large_maneuver_base.py new file mode 100644 index 0000000000..324f1e171b --- /dev/null +++ b/openpilot/selfdrive/controls/tests/test_ford_large_maneuver_base.py @@ -0,0 +1,50 @@ +"""Large-turn command regressions, not a model of the PSCM's wheel response.""" +import unittest + +from openpilot.selfdrive.controls.lib.ford_path import FordPathController +from openpilot.selfdrive.controls.lib.ford_virtual_angle import FordVirtualAngleController, PscmStatus +from openpilot.selfdrive.controls.tests.test_ford_curvature_c0 import step +from openpilot.selfdrive.controls.tests.test_ford_path_reference import circle + + +class TestFordLargeManeuverBase(unittest.TestCase): + def test_aligned_large_turn_keeps_baseline_pose_without_integral_authority(self): + # A stronger forward path than the instantaneous curvature is present in + # the recorded successful turns. A frozen integral cannot supply that base. + for sign in (-1, 1): + with self.subTest(sign=sign): + model = circle(sign * .065) + previous, controller = FordPathController(), FordVirtualAngleController() + for i in range(300): + now = i * .01 + baseline = previous.update(model, sign * .04, current_curvature=sign * .04, v_ego=5.) + actual = step(controller, now, sign * .04, model, speed=5., yaw_rate=sign * .2, + pscm_status=PscmStatus(now, 2, 2, 2, False)) + self.assertEqual(controller.diagnostics['heading_bias'], 0.) + self.assertAlmostEqual(actual.path_offset, baseline.path_offset, delta=.010001) + self.assertAlmostEqual(actual.path_angle, baseline.path_angle, delta=.000501) + self.assertEqual((actual.curvature, actual.curvature_rate), (0., 0.)) + + def test_small_action_remains_a_centering_request_despite_a_distant_turn(self): + for sign in (-1, 1): + controller = FordVirtualAngleController() + for i in range(300): + actual = step(controller, i * .01, sign * .002, circle(sign * .065), speed=5.) + self.assertAlmostEqual(actual.path_offset, sign * .064, delta=.005001) + self.assertAlmostEqual(actual.path_angle, sign * .014, delta=.000501) + + def test_zero_and_reversed_action_supersede_old_model_turn(self): + for next_action in (0., -.04): + controller = FordVirtualAngleController() + model = circle(.065) + for i in range(300): + step(controller, i * .01, .04, model, speed=5.) + for i in range(300, 510): + actual = step(controller, i * .01, next_action, model, speed=5.) + self.assertAlmostEqual(actual.path_offset, 32. * next_action, delta=.005001) + self.assertAlmostEqual(actual.path_angle, 7. * next_action, delta=.000501) + self.assertEqual(controller.diagnostics['heading_bias'], 0.) + + +if __name__ == '__main__': + unittest.main() diff --git a/openpilot/selfdrive/controls/tests/test_ford_large_turn_routes.py b/openpilot/selfdrive/controls/tests/test_ford_large_turn_routes.py new file mode 100644 index 0000000000..7eb950dff1 --- /dev/null +++ b/openpilot/selfdrive/controls/tests/test_ford_large_turn_routes.py @@ -0,0 +1,177 @@ +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, PscmStatus + + +class TestFordLargeTurnRoutes(unittest.TestCase): + @classmethod + def setUpClass(cls): + cls.fixture = Path(__file__).parent / 'fixtures/ford_large_turn_requests_route83.npz' + cls.metadata = json.loads(cls.fixture.with_suffix('.json').read_text()) + cls.data = dict(np.load(cls.fixture)) + cls.models = [SimpleNamespace(position=SimpleNamespace(x=p[0], y=p[1]), orientation=SimpleNamespace(z=p[2])) for p in cls.data['models']] + data = cls.data + commands, gates, statuses, bases, targets, errors, before_backoff = [], [], [], [], [], [], [] + backoff_active, previous_commands, repeated_measurements = [], [], [] + previous_episode = None + for i, now in enumerate(data['t']): + if data['episode'][i] != previous_episode: + controller = FordVirtualAngleController(response_delay=cls.metadata['response_delay']) + previous_episode = data['episode'][i] + pscm = PscmStatus(float(data['pscm_timestamp'][i]), int(data['pscm_lateral_state'][i]), int(data['pscm_limit'][i]), + int(data['pscm_capability'][i]), bool(data['pscm_denied'][i]), bool(data['pscm_valid'][i])) + previous_base, previous_bias = controller.feedback.previous_base, controller.feedback.bias + previous_commands.append(controller.heading_request) + repeated_measurements.append(data['measurement_time'][i] == controller.feedback.last_measurement_time) + path = controller.update(cls.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]), steering_torque=data['steering_torque'][i], pscm_status=pscm) + commands.append((path.path_offset, path.path_angle, path.curvature, path.curvature_rate)) + gates.append(path.valid) + statuses.append(controller.diagnostics['feedback_status']) + base = controller.diagnostics.get('heading_base', 0.) + # Account for release of the base before testing the direction of the + # separate constrained feedback step on these changing recorded requests. + retained_bias = previous_bias + if previous_base is None or previous_base * base < 0.: + retained_bias = 0. + elif previous_base: + retained_bias *= min(1., abs(base / previous_base)) + before_backoff.append(base + retained_bias) + bases.append(base) + targets.append(controller.diagnostics.get('heading_target', 0.)) + errors.append(controller.diagnostics.get('feedback_yaw_error') or 0.) + backoff_active.append(controller.diagnostics.get('feedback_backoff_active', False)) + cls.commands, cls.gates, cls.statuses = np.array(commands), np.array(gates), np.array(statuses) + cls.bases, cls.targets, cls.errors, cls.before_backoff = np.array(bases), np.array(targets), np.array(errors), np.array(before_backoff) + cls.backoff_active = np.array(backoff_active) + cls.previous_commands, cls.repeated_measurements = np.array(previous_commands), np.array(repeated_measurements) + + def test_fixture_authority_targets_are_recorded_successes(self): + self.assertEqual(hashlib.sha256(self.fixture.read_bytes()).hexdigest(), self.metadata['fixture_sha256']) + authority_targets = 0 + for i, window in enumerate(self.metadata['windows']): + if window['role'] != 'authority_target': + continue + authority_targets += 1 + mask = self.data['window_masks'][:, i] + ratio = np.median(self.data['recorded_response_curvature_02s'][mask] / self.data['desired_curvature'][mask]) + self.assertGreaterEqual(ratio, .90) + self.assertLessEqual(ratio, 1.10) + self.assertGreaterEqual(np.max(abs(self.data['wheel_deg'][mask])), 150.) + self.assertGreaterEqual(authority_targets, 2) + over = next(window for window in self.metadata['windows'] if window['name'] == 'large_over_response_290deg') + self.assertEqual(over['role'], 'over_response_challenge_not_target') + + def test_successful_large_turns_retain_recorded_command_scale(self): + # The requirement is command construction, not a predicted wheel response. + # Retain at least 85% of the successful send-clamped C0/C1 medians during + # the complete eligible turn, held request, and eligible increasing request. + for i, window in enumerate(self.metadata['windows']): + if window['role'] != 'authority_target': + continue + for phase in (None, 'phase_held', 'phase_turn_in'): + with self.subTest(window=window['name'], phase=phase): + mask = self.data['window_masks'][:, i].copy() + if phase is not None: + mask &= self.data[phase] + self.assertGreaterEqual(int(mask.sum()), 10) + recorded = np.median(abs(self.data['recorded_send_clamped'][mask, :2]), axis=0) + candidate = np.median(abs(self.commands[mask, :2]), axis=0) + self.assertTrue(np.all(candidate >= .85 * recorded), (candidate, recorded)) + self.assertGreater(np.median(self.commands[mask, 0] * self.data['desired_curvature'][mask]), 0.) + self.assertGreater(np.median(self.commands[mask, 1] * self.data['desired_curvature'][mask]), 0.) + + def test_small_release_and_reversal_keep_curvature_centering(self): + for i, window in enumerate(self.metadata['windows']): + if window['role'] not in ('release', 'reversal'): + continue + with self.subTest(window=window['name']): + mask = self.data['window_masks'][:, i] + np.testing.assert_array_equal(self.commands[mask, 0], self.data['v5_full_replay'][mask, 0]) + np.testing.assert_array_equal(self.bases[mask], self.data['v5_full_heading_base'][mask]) + + def test_all_windows_respect_gates_limits_and_pscm_guards(self): + data = self.data + np.testing.assert_array_equal(self.commands[:, 2:], 0.) + np.testing.assert_array_equal(self.gates[data['evidence']], data['v5_full_valid'][data['evidence']]) + self.assertTrue(np.isfinite(self.commands).all()) + self.assertTrue((abs(self.commands[:, :2]) <= np.array([5.110000001, .500000001])).all()) + continuous = (data['episode'][1:] == data['episode'][:-1]) & self.gates[1:] & self.gates[:-1] + limits = np.diff(data['t'])[:, None] * np.array([4., .5]) + np.array([.01, .0005]) + 1e-8 + self.assertTrue((abs(np.diff(self.commands[:, :2], axis=0))[continuous] <= limits[continuous]).all()) + limited = data['pscm_limit'] >= 2 + self.assertFalse(np.isin(self.statuses[limited], ('integrating', 'host_limit')).any()) + + def test_constrained_backoff_only_reduces_same_sign_heading(self): + backoff = np.isin(self.statuses, ('release_backoff', 'pscm_backoff')) + self.assertGreater(int(backoff.sum()), 100) + self.assertTrue((self.errors[backoff] * self.bases[backoff] < 0.).all()) + current_error = self.data['speed'] * self.data['desired_curvature'] - self.data['yaw_rate'] + self.assertTrue((current_error[backoff] * self.bases[backoff] < 0.).all()) + self.assertTrue((self.before_backoff[backoff] * self.bases[backoff] > 0.).all()) + self.assertTrue((abs(self.targets[backoff]) <= abs(self.before_backoff[backoff]) + 1e-10).all()) + self.assertTrue((self.targets[backoff] * self.bases[backoff] >= -1e-12).all()) + + def test_recorded_over_response_gets_heading_backoff(self): + index = next(i for i, window in enumerate(self.metadata['windows']) if window['name'] == 'large_over_response_290deg') + mask = self.data['window_masks'][:, index] + # With this same v6 feedforward and the former freeze-only feedback policy, + # the recorded challenge's median C1 is .5 rad. Require a measurable command + # reduction, not a simulated improvement in the old vehicle trajectory. + self.assertLess(float(np.median(abs(self.commands[mask, 1]))), .5 - .02) + self.assertTrue(np.isin(self.statuses[mask], ('release_backoff', 'pscm_backoff')).any()) + # The overshooting fallback C0 is a ceiling comparison, never an authority + # target that a test should force the candidate to reach or exceed. + self.assertLessEqual(float(np.median(abs(self.commands[mask, 0]))), + float(np.median(abs(self.data['recorded_send_clamped'][mask, 0]))) + .01) + + def test_backoff_ceiling_prevents_heading_growth_between_measurements(self): + # A rising model heading must not outweigh a measured backoff, including + # controller ticks that reuse the same CAN yaw observation. C0 is separate. + mask = self.backoff_active + self.assertGreater(int(mask.sum()), 100) + ceiling = np.maximum(0., np.sign(self.bases[mask]) * self.previous_commands[mask]) + self.assertTrue((abs(self.targets[mask]) <= ceiling + 1e-10).all()) + self.assertTrue((self.targets[mask] * self.bases[mask] >= -1e-12).all()) + self.assertTrue((abs(self.commands[mask, 1]) <= abs(self.previous_commands[mask]) + .0005 + 1e-10).all()) + repeated = mask & self.repeated_measurements + self.assertGreater(int(repeated.sum()), 0) + self.assertTrue((self.statuses[repeated] == 'no_new_measurement').all()) + + def test_feedback_error_sign_with_a_large_recorded_model(self): + window_index = next(i for i, window in enumerate(self.metadata['windows']) if window['name'] == 'successful_large_181deg') + indices = np.flatnonzero(self.data['window_masks'][:, window_index] & self.data['phase_held']) + index = int(indices[len(indices) // 2]) + recorded_model = self.data['models'][self.data['model_index'][index]] + magnitude = abs(self.data['desired_curvature'][index]) + speed = self.data['speed'][index] + original_sign = np.sign(self.data['desired_curvature'][index]) + # Hold this recorded geometry and request while varying the yaw observation. + # This tests feedback direction with model-pose feedforward, not plant motion. + for sign in (-1., 1.): + model = SimpleNamespace(position=SimpleNamespace(x=recorded_model[0], y=recorded_model[1] * sign / original_sign), + orientation=SimpleNamespace(z=recorded_model[2] * sign / original_sign)) + for response_fraction in (.5, 1.5): + with self.subTest(sign=sign, response_fraction=response_fraction): + controller = FordVirtualAngleController() + for i in range(400): + now = i * .01 + controller.update(model, sign * magnitude, yaw_rate=sign * magnitude * speed * response_fraction, + speed=speed, now=now, measurement_time=now, model_time=now, reference_time=now, active=True, + pscm_status=PscmStatus(now, 2, 0, 2, False)) + self.assertEqual(controller.diagnostics['base_guard'], 'model_pose') + correction_along_error = controller.diagnostics['heading_bias'] * sign * np.sign(1. - response_fraction) + self.assertGreater(correction_along_error, .01) + + +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 3302d84b4f..25f0e94963 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_path_reference.py +++ b/openpilot/selfdrive/controls/tests/test_ford_path_reference.py @@ -114,16 +114,17 @@ class TestFordPathReference(unittest.TestCase): malformed[2].position.x = [] malformed[3].position.x[:] = 0. for model in malformed: - controller = FordVirtualAngleController() - run_step(controller, circle(.03), 1.) - self.assertEqual(run_step(controller, model, 1.05), FordPath()) - self.assertIsNone(controller.reference.path) + for now, model_time in ((1.05, 1.05), (1.01, 1.)): + controller = FordVirtualAngleController() + run_step(controller, circle(.03), 1.) + self.assertEqual(run_step(controller, model, now, model_time=model_time), FordPath()) + self.assertIsNone(controller.reference.path) def test_independent_slew_and_dbc_bounds_during_large_reversal(self): controller = FordVirtualAngleController() previous = FordPath() for i in range(900): - curvature = .1 if i < 400 else -.1 + curvature = .2 if i < 400 else -.2 path = run_step(controller, circle(curvature), i * .01, speed=5., desired_curvature=2 * curvature) self.assertLessEqual(abs(path.path_offset), 5.11) self.assertLessEqual(abs(path.path_angle), .5) diff --git a/openpilot/sunnypilot/sunnylink/settings_ui.json b/openpilot/sunnypilot/sunnylink/settings_ui.json index 88b86adda7..c0a3428b6a 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": "Follow the planned turn with a measured steering correction on the F-150 Lightning with C2 off.", - "details": "Uses planned curvature for centering and heading, with a bounded correction when measured turning differs from the request. The correction requires fresh steering-controller status and clears during driver override. Default off and this version is not road-validated. When enabled, this controller is always selected on the Ford CAN FD F-150 Lightning regardless of steering-firmware identification; 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": "Follow large turns from the model path while retaining planned-curvature centering on the F-150 Lightning with C2 off.", + "details": "Uses the existing controller's model-path geometry for large turns when the model and planned curvature agree. Smaller or opposing requests use planned curvature for centering. A bounded measured-turning correction requires fresh, valid steering-controller status and clears during driver override. During turn release or a reported steering limit, that correction can only reduce the existing turn request toward zero. Default off and this version is not road-validated. When enabled, this controller is always selected on the Ford CAN FD F-150 Lightning regardless of steering-firmware identification; 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 8e4463331f..599547896a 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: Follow the planned turn with a measured steering correction on the F-150 Lightning with C2 off. - details: Uses planned curvature for centering and heading, with a bounded correction when measured turning differs from the request. The correction requires fresh steering-controller status and clears during driver override. Default off and this version is not road-validated. When enabled, this controller is always selected on the Ford CAN FD F-150 Lightning regardless of steering-firmware identification; 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: Follow large turns from the model path while retaining planned-curvature centering on the F-150 Lightning with C2 off. + details: Uses the existing controller's model-path geometry for large turns when the model and planned curvature agree. Smaller or opposing requests use planned curvature for centering. A bounded measured-turning correction requires fresh, valid steering-controller status and clears during driver override. During turn release or a reported steering limit, that correction can only reduce the existing turn request toward zero. Default off and this version is not road-validated. When enabled, this controller is always selected on the Ford CAN FD F-150 Lightning regardless of steering-firmware identification; 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 diff --git a/openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py b/openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py index 65d5e64174..ae27d533e6 100644 --- a/openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py +++ b/openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py @@ -289,6 +289,8 @@ class TestKnownVehicleSettings(OpenpilotTestCase): # No other toggle can prevent disabling this experiment while offroad. assert servo["enablement"] == [{"type": "offroad_only"}] assert "F-150 Lightning" in servo["description"] + assert "model path" in servo["description"] + assert "planned-curvature centering" in servo["description"] assert "always selected on the Ford CAN FD F-150 Lightning regardless of steering-firmware identification" in servo["details"] assert "Turning it off restores the previous controller selection" in servo["details"] assert "RL38-14D003-AA" not in servo["details"]