mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-09-11 17:43:43 +08:00
Ford: guard turn release and recover remaining tracking deficit
Prevent same-direction C0/C1 growth when measured turning exceeds current and delayed requests during release, retaining request history across driver feedback resets. Permit bounded C1 correction after opposing bias reaches zero when both requests remain undertracked and measured curvature is no longer catching up. Keep model allocation, gain, field and slew limits, platform selection, and zero C2/C3 unchanged. Add anonymous recorded-input regressions and diagnostics. Validation: 139 Ford tests plus 150 subtests, 46 Sunnylink tests, Ruff, generated settings check, and recorded-command replays. Physical response and stability remain unverified.
This commit is contained in:
Isaac Barham
@@ -1,18 +1,18 @@
|
||||
# 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.
|
||||
Hypothesis `model-pose-c0-c1-feedback-v8` retains the model-pose C0/C1 base
|
||||
and adds two guarded release policies. When measured turning exceeds both
|
||||
current and delayed requests, a separate output guard prevents same-direction
|
||||
C0/C1 growth, including while feedback history rebuilds after driver input.
|
||||
When turning instead falls below both requests and is no longer increasing,
|
||||
bounded C1 tracking can use remaining release-entry command headroom.
|
||||
Existing opposing-bias recovery still stops at zero bias. Geometry, blending,
|
||||
feedback gain, slew rates and field limits are unchanged; C2/C3 remain zero.
|
||||
|
||||
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.
|
||||
servo. V8 has offline validation only. Command replay cannot establish the
|
||||
truck's response, closed-loop stability, or an overshoot improvement.
|
||||
|
||||
## Evidence and scope
|
||||
|
||||
@@ -35,6 +35,16 @@ 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.
|
||||
|
||||
Route8a ran v6 and was reported as the best drive. Route8e ran v7 throughout
|
||||
with the experiment enabled; it includes entry lag and excessive turning
|
||||
while requests release. Fixed-input v6/v7 replay produced identical commands
|
||||
in the main reversal and over-response examples, so the v7 recovery change
|
||||
does not directly explain their command behavior. In the over-response
|
||||
example, model C0/C1 grew while selected curvature fell and driver resets
|
||||
repeatedly removed feedback history. Another exit remained deficient after
|
||||
opposing bias reached zero. These observations motivate the v8 guards; they
|
||||
do not isolate an EPS transfer function or demonstrate the proposed response.
|
||||
|
||||
## Base request
|
||||
|
||||
controlsd selects valid `lateralManeuverPlan.desiredCurvature`, otherwise
|
||||
@@ -63,7 +73,7 @@ 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)
|
||||
C0_base = clip(model_pair.C0 + curvature_C0, ±5.11 m)
|
||||
C1_base = clip(model_pair.C1 + curvature_C1, ±0.5 rad)
|
||||
```
|
||||
|
||||
@@ -125,18 +135,20 @@ 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
|
||||
The existing 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.
|
||||
can be accepted. A separate release-tracking exception is described below;
|
||||
other constrained cases remain frozen. PSCM limit 2 never permits either
|
||||
request-increasing exception.
|
||||
The no-new-bias restriction applies to `release_recovery`. It does not apply
|
||||
to the separate bounded `release_tracking` branch. 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
|
||||
@@ -158,6 +170,46 @@ 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.
|
||||
|
||||
## V8 release guard and tracking
|
||||
|
||||
`ReleaseGuard` retains selected-request history independently of feedback
|
||||
bias history. Driver-related feedback resets do not erase that reference,
|
||||
but the guard still requires current fresh valid PSCM status, no current
|
||||
driver override, and the existing input and speed eligibility. Invalid core
|
||||
input or disengagement resets its history with the controller.
|
||||
|
||||
During release, measured yaw must exceed both the current and delay-matched
|
||||
requests in the requested turn direction. Only then does the guard cap
|
||||
same-direction C0/C1 growth at each preceding continuous request. Terms
|
||||
already reducing the turn, including an opposing C0 centering offset, remain
|
||||
available. The guard follows base allocation and C1 feedback, so changing
|
||||
model geometry cannot bypass it. Its ceilings affect outputs, never stored
|
||||
bias. No scalar-curvature cap replaces strong model geometry during turn-in
|
||||
or undertracking. Existing independent slew and field limits still apply.
|
||||
|
||||
`release_tracking` addresses an eligible release deficit once bias is zero
|
||||
or already in the base's direction. Both current and delayed requests must
|
||||
align with that base, measured turning must be below both, and measured
|
||||
curvature must not be rising in the turn direction across the response
|
||||
interval by more than one C1 wire quantum after scaling by heading preview.
|
||||
Fresh valid PSCM status with limit below 2 is required. The current yaw deficit
|
||||
uses the existing integration gain and measurement interval;
|
||||
new C1 tracking increments are limited by command headroom captured at
|
||||
release entry, tapered with remaining desired curvature. The allowance is
|
||||
`max(0, entry_command_magnitude - abs(base)) × min(1, abs(desired) / entry_reference)`
|
||||
above the current base; any existing same-direction bias consumes it first.
|
||||
This limits new tracking integration, not the existing model base or bias.
|
||||
Only that additional allowance is tapered; strong model geometry remains
|
||||
available. A brief pause does not reacquire a higher entry
|
||||
ceiling; a full response interval without release ends the retained episode.
|
||||
Common host anti-windup, field and slew bounds still apply. Opposing bias
|
||||
continues through `release_recovery`, which stops at zero, before any separate
|
||||
tracking exception can be considered.
|
||||
|
||||
Neither exception relaxes the PSCM LimitReached growth restriction. The
|
||||
reference delay and finite response time remain; these output policies are
|
||||
command-construction changes, not evidence of improved physical tracking.
|
||||
|
||||
## PSCM status and driver handling
|
||||
|
||||
card publishes `Lane_Assist_Data3_FD1` in `carStateSP.fordPscmStatus`, retaining
|
||||
@@ -167,7 +219,8 @@ 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.
|
||||
clears feedback bias/history and disables the separate release guard,
|
||||
leaving the 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
|
||||
@@ -176,7 +229,9 @@ 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
|
||||
feedback reference interval is required after override; the independent
|
||||
release guard can use retained valid request history once its current gates
|
||||
are satisfied. Base requests retain normal
|
||||
PSCM driver arbitration while lateral control remains authorized; an unset
|
||||
override flag cannot rule out subthreshold driver influence.
|
||||
|
||||
@@ -190,9 +245,9 @@ 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`
|
||||
requirement. Enabled selects v8 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
|
||||
retain their existing controller. V8 takes priority over PSCM Coefficient
|
||||
Observer while selected; disabling and cycling offroad/onroad restores the
|
||||
previous selection. Controller selection does not force lateral engagement.
|
||||
|
||||
@@ -201,24 +256,35 @@ 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.
|
||||
The 5 Hz `Ford C2-free path tracking` event keeps its name and identifies v8.
|
||||
`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.
|
||||
cases. `heading_base` is the bounded pre-feedback C1. `offset_target` and
|
||||
`heading_target` are the final targets after the independent release guard;
|
||||
`offset_target_unguarded` and `heading_target_unguarded` retain the inputs to
|
||||
that guard. The latter C1 already includes its normal feedback/backoff policy.
|
||||
|
||||
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`.
|
||||
`release_guard_active` and `release_guard_reference_curvature` expose the
|
||||
independent C0/C1 guard and its retained delayed reference.
|
||||
`feedback_release_tracking_active`, `feedback_release_ceiling` and
|
||||
`feedback_curvature_delta` identify accepted release
|
||||
tracking, the total-heading threshold used to admit new bias, and the
|
||||
measured-curvature change across the response interval (1/m). The tracking
|
||||
flag is true only when the branch accepts a bias change on a new measurement;
|
||||
it is false on repeated measurements. The ceiling/trend fields can describe
|
||||
an evaluated condition even when no increment is accepted.
|
||||
`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
|
||||
During backoff or the independent release guard, `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
|
||||
@@ -231,6 +297,26 @@ 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.
|
||||
v8 success. Guard checks also cover driver reset/history rebuilding,
|
||||
same-direction growth, opposing coefficients, repeated measurements,
|
||||
undertracking and invalid-status inhibition. Tracking checks cover delayed
|
||||
curvature trends and tapered release-entry headroom. Historical v5–v7 replay
|
||||
results remain historical observations.
|
||||
|
||||
The v8 recorded-input fixture contains 15,273 cycles with 4,879 selected
|
||||
evidence samples. Base allocation and output eligibility match v7. In the
|
||||
clean deficient exit, median absolute C1 changes from 0.0665 to 0.0845 rad
|
||||
while C0 stays unchanged. The growth guard also acts while feedback history
|
||||
rebuilds; the largest over-growth witness includes nearby driver input and
|
||||
is excluded from the strict autonomous tracking score. Both good comparison
|
||||
curves in that fixture retain their median requests, and the older large-turn
|
||||
fixtures retain their required command scale.
|
||||
|
||||
On the earlier good drive, one comparison curve retains extra C1 after
|
||||
eligible release tracking: median magnitude changes from 0.121 to 0.128 rad.
|
||||
In its 103–110 s interval, tracking increments occur only while measured
|
||||
turning falls short, with a median current response/request ratio of 0.895.
|
||||
Acquired bias can persist after matching, as with ordinary integral feedback.
|
||||
This collateral command change remains a reason to compare new vehicle logs.
|
||||
Replay fixes recorded motion and planner outputs, so enabled vehicle logs
|
||||
are still required to assess tracking error, oscillation and interventions.
|
||||
|
||||
@@ -55,6 +55,57 @@ class PscmStatus:
|
||||
return None
|
||||
|
||||
|
||||
class ReleaseGuard:
|
||||
"""Keep pose growth from defeating a measured turn release.
|
||||
|
||||
Request history is independent of the integral: driver input resets
|
||||
correction authority, but does not erase valid requests already sent.
|
||||
Coefficient signs describe path geometry, not motor effort. Opposing path
|
||||
terms stay available; only growth in the requested direction is limited.
|
||||
"""
|
||||
def __init__(self, delay):
|
||||
self.delay = delay
|
||||
self.history = deque()
|
||||
self.last_measurement_time = None
|
||||
self.last_pscm_time = None
|
||||
self.direction = 0.
|
||||
self.active = False
|
||||
self.reference_curvature = None
|
||||
|
||||
def update(self, desired, *, yaw_rate, speed, now, measurement_time, heading_horizon, driver_override, pscm_status):
|
||||
self.history.append((now, desired))
|
||||
while len(self.history) > 2 and self.history[1][0] < now - self.delay - .25:
|
||||
self.history.popleft()
|
||||
direction = float(np.sign(desired))
|
||||
status_reason = pscm_status.invalid_reason(now) if pscm_status is not None else 'missing_pscm'
|
||||
fresh_status = (status_reason in (None, 'unavailable_pscm') and
|
||||
(self.last_pscm_time is None or pscm_status.timestamp >= self.last_pscm_time))
|
||||
available = (not driver_override and speed >= FEEDBACK_MIN_SPEED and direction != 0. and
|
||||
fresh_status and status_reason is None and pscm_status.limit < 3)
|
||||
if fresh_status:
|
||||
self.last_pscm_time = pscm_status.timestamp
|
||||
if not available or direction != self.direction:
|
||||
self.active = False
|
||||
self.direction = direction
|
||||
if measurement_time != self.last_measurement_time:
|
||||
self.last_measurement_time = measurement_time
|
||||
reference = next((sample for sample in reversed(self.history) if sample[0] <= measurement_time - self.delay), None)
|
||||
self.reference_curvature = reference[1] if reference is not None else None
|
||||
self.active = False
|
||||
if available and reference is not None:
|
||||
delayed = reference[1]
|
||||
releasing = (abs(delayed) - abs(desired)) * heading_horizon > HEADING_RESOLUTION
|
||||
self.active = (delayed * desired > 0. and releasing and
|
||||
(yaw_rate - speed * desired) * direction > 0. and
|
||||
(yaw_rate - speed * delayed) * direction > 0.)
|
||||
return self.active
|
||||
|
||||
def limit(self, target, previous):
|
||||
if self.active and target * self.direction > 0.:
|
||||
return self.direction * min(target * self.direction, max(previous * self.direction, 0.))
|
||||
return target
|
||||
|
||||
|
||||
class HeadingFeedback:
|
||||
"""Bound a heading correction using measured yaw error, not an EPS gain fit.
|
||||
|
||||
@@ -68,13 +119,18 @@ class HeadingFeedback:
|
||||
|
||||
def reset(self, status='inactive'):
|
||||
self.history = deque()
|
||||
self.response_history = deque()
|
||||
self.bias = 0.
|
||||
self.previous_base = None
|
||||
self.last_measurement_time = self.last_pscm_time = None
|
||||
self.backoff_active = False
|
||||
self.release_command = self.release_reference = 0.
|
||||
self.release_quiet_since = None
|
||||
self.diagnostics = {'heading_bias': 0., 'feedback_status': status, 'feedback_reference_time': None,
|
||||
'feedback_reference_curvature': None, 'feedback_yaw_error': None,
|
||||
'feedback_backoff_active': False, 'feedback_recovery_active': False}
|
||||
'feedback_backoff_active': False, 'feedback_recovery_active': False,
|
||||
'feedback_release_tracking_active': False, 'feedback_release_ceiling': None,
|
||||
'feedback_curvature_delta': None}
|
||||
|
||||
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,13 +156,20 @@ class HeadingFeedback:
|
||||
self.history.popleft()
|
||||
|
||||
status = 'no_new_measurement'
|
||||
recovery_active = False
|
||||
recovery_active = release_tracking_active = False
|
||||
release_ceiling = curvature_delta = None
|
||||
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
|
||||
self.response_history.append((measurement_time, yaw_rate / speed))
|
||||
while len(self.response_history) > 2 and self.response_history[1][0] < target_time - .1:
|
||||
self.response_history.popleft()
|
||||
prior_response = next((sample for sample in reversed(self.response_history) if sample[0] <= target_time), None)
|
||||
if prior_response is not None:
|
||||
curvature_delta = yaw_rate / speed - prior_response[1]
|
||||
# Use the command actually held at the historical instant. Interpolating
|
||||
# toward a later publication would compare against a different request.
|
||||
reference = next((sample for sample in reversed(self.history) if sample[0] <= target_time), None)
|
||||
@@ -118,6 +181,21 @@ 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
|
||||
# A brief quantization-level pause must not acquire a larger ceiling.
|
||||
# A full response interval without release ends the previous episode.
|
||||
if desired * base <= 0.:
|
||||
self.release_command = self.release_reference = 0.
|
||||
self.release_quiet_since = None
|
||||
elif releasing:
|
||||
self.release_quiet_since = None
|
||||
if self.release_reference == 0. and reference_curvature * desired > 0. and desired * base > 0.:
|
||||
self.release_reference = abs(reference_curvature)
|
||||
self.release_command = max(0., math.copysign(1., base) * previous_command)
|
||||
else:
|
||||
if self.release_quiet_since is None:
|
||||
self.release_quiet_since = measurement_time
|
||||
if measurement_time - self.release_quiet_since >= self.delay:
|
||||
self.release_command = self.release_reference = 0.
|
||||
constrained = releasing or pscm_status.limit >= 2
|
||||
heading_before = base + self.bias
|
||||
# Do not brake turn-in merely for exceeding an older, smaller request:
|
||||
@@ -126,7 +204,17 @@ class HeadingFeedback:
|
||||
backoff = constrained and yaw_error * base < 0. and current_yaw_error * base < 0. and heading_before * base > 0.
|
||||
recovering = (releasing and pscm_status.limit < 2 and self.bias * base < 0. and
|
||||
desired * base > 0. and reference_curvature * base > 0. and yaw_error * base > 0. and current_yaw_error * base > 0.)
|
||||
if constrained and not (backoff or recovering):
|
||||
tracking_release = (releasing and pscm_status.limit < 2 and self.bias * base >= 0. and
|
||||
desired * base > 0. and reference_curvature * base > 0. and yaw_error * base > 0. and current_yaw_error * base > 0. and
|
||||
curvature_delta is not None and math.copysign(1., base) * curvature_delta * heading_horizon <= HEADING_RESOLUTION and
|
||||
self.release_reference > 0.)
|
||||
if tracking_release:
|
||||
# Taper only extra correction; preserve the large-turn model base.
|
||||
# The bound comes from earlier commands, not an EPS gain fit.
|
||||
remaining = min(1., abs(desired) / self.release_reference)
|
||||
headroom = max(0., self.release_command - abs(base)) * remaining
|
||||
release_ceiling = abs(base) + headroom
|
||||
if constrained and not (backoff or recovering or tracking_release):
|
||||
status = 'release' if releasing else 'pscm_limit'
|
||||
else:
|
||||
bias_before = self.bias
|
||||
@@ -137,6 +225,11 @@ class HeadingFeedback:
|
||||
# stop at zero bias; recovery cannot create demand beyond the base.
|
||||
increment = float(np.clip(self.tuning.feedback_gain * current_yaw_error * measurement_dt,
|
||||
min(0., -self.bias), max(0., -self.bias)))
|
||||
elif tracking_release:
|
||||
# Include the ceiling in integral admission, so no hidden bias
|
||||
# accumulates behind an unreachable output request.
|
||||
available = max(0., release_ceiling - math.copysign(1., base) * heading_before)
|
||||
increment = math.copysign(1., base) * min(abs(self.tuning.feedback_gain * current_yaw_error * measurement_dt), available)
|
||||
elif backoff:
|
||||
# A release/limit may still reduce an excessive same-direction
|
||||
# heading request. It cannot grow that request or cross through
|
||||
@@ -163,6 +256,9 @@ class HeadingFeedback:
|
||||
elif recovering and self.bias != bias_before:
|
||||
recovery_active = True
|
||||
status = 'release_recovery'
|
||||
elif tracking_release:
|
||||
release_tracking_active = self.bias != bias_before
|
||||
status = 'release_tracking' if release_tracking_active else 'release'
|
||||
self.bias = float(np.clip(self.bias, -.5 - base, .5 - base))
|
||||
target = float(np.clip(base + self.bias, -.5, .5))
|
||||
if self.backoff_active:
|
||||
@@ -174,7 +270,9 @@ class HeadingFeedback:
|
||||
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,
|
||||
'feedback_backoff_active': self.backoff_active, 'feedback_recovery_active': recovery_active}
|
||||
'feedback_backoff_active': self.backoff_active, 'feedback_recovery_active': recovery_active,
|
||||
'feedback_release_tracking_active': release_tracking_active, 'feedback_release_ceiling': release_ceiling,
|
||||
'feedback_curvature_delta': curvature_delta}
|
||||
return target
|
||||
|
||||
|
||||
@@ -253,12 +351,15 @@ class FordVirtualAngleController:
|
||||
def reset(self):
|
||||
self.reference.reset()
|
||||
self.feedback.reset()
|
||||
self.release_guard = ReleaseGuard(self.delay)
|
||||
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': 'model-pose-c0-c1-feedback-v7', 'command': (0., 0., 0., 0.),
|
||||
self.diagnostics = {'status': 'inactive', 'hypothesis': 'model-pose-c0-c1-feedback-v8', 'command': (0., 0., 0., 0.),
|
||||
'release_guard_active': False, 'release_guard_reference_curvature': None,
|
||||
'offset_target_unguarded': 0., 'heading_target_unguarded': 0.,
|
||||
**self.feedback.diagnostics}
|
||||
|
||||
def update(self, model, desired_curvature, *, yaw_rate, speed, now, measurement_time, model_time, reference_time,
|
||||
@@ -317,6 +418,11 @@ class FordVirtualAngleController:
|
||||
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,
|
||||
heading_horizon=heading_horizon, driver_override=driver_override, pscm_status=pscm_status)
|
||||
self.release_guard.update(desired_curvature, yaw_rate=yaw_rate, speed=speed, now=now, measurement_time=measurement_time,
|
||||
heading_horizon=heading_horizon, driver_override=driver_override, pscm_status=pscm_status)
|
||||
unguarded_offset, unguarded_heading = target_offset, target_heading
|
||||
target_offset = self.release_guard.limit(target_offset, self.offset_request)
|
||||
target_heading = self.release_guard.limit(target_heading, self.heading_request)
|
||||
delta_offset = target_offset - self.offset_request
|
||||
delta_heading = target_heading - self.heading_request
|
||||
# A slow C1 transition must not hold a C0 correction after action releases it.
|
||||
@@ -327,8 +433,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': 'model-pose-c0-c1-feedback-v7',
|
||||
self.diagnostics = {'status': 'driver_override' if driver_override else 'active', 'hypothesis': 'model-pose-c0-c1-feedback-v8',
|
||||
'desired_curvature': desired_curvature, 'offset_target': target_offset, 'heading_target': target_heading,
|
||||
'offset_target_unguarded': unguarded_offset, 'heading_target_unguarded': unguarded_heading,
|
||||
'release_guard_active': self.release_guard.active,
|
||||
'release_guard_reference_curvature': self.release_guard.reference_curvature,
|
||||
'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,
|
||||
|
||||
@@ -0,0 +1,156 @@
|
||||
{
|
||||
"description": "Anonymous recorded-input turn-exit regression fixture; command construction only, not simulated vehicle response.",
|
||||
"baseline_revision": "dfcfddb91ce2409511f5b2dbce25d06d5056b3d6",
|
||||
"baseline_hypothesis": "model-pose-c0-c1-feedback-v7",
|
||||
"baseline_source_hashes": {
|
||||
"controller_sha256": "4951a6352d89fcd66277bbfe682bd22e935a31b5a4db33e617ad21189b6705fd",
|
||||
"allocator_sha256": "383538fc7cdae3bc28dffb71fe12ac5f3f9866ffbe6adfb7457f3593e9fc903a"
|
||||
},
|
||||
"fixture_sha256": "87a030c309061b7dc218715d05440c2077e465a8138079b46e8e8cee94201e54",
|
||||
"source_fixture_sha256": "d476110b83dc628ffbd094220e464d6d3114b709bda2977813c3217964d41086",
|
||||
"response_delay": 0.20000000298023224,
|
||||
"publication_latency_estimate_s": 0.0015483515003040793,
|
||||
"samples": 15273,
|
||||
"model_count": 3078,
|
||||
"evidence_samples": 4879,
|
||||
"context_policy": "At least twenty seconds prior context, extended before the last observed reset. Overlapping intervals are merged.",
|
||||
"provenance": "Selected from a recorded drive running the pinned baseline; request, model, driver and PSCM observations stay fixed during replay.",
|
||||
"baseline_policy": "Stored commands, validity and bias exactly match the complete baseline replay on evidence samples. Context outside evidence initializes state and is not an exact-output target.",
|
||||
"compact_full_baseline_evidence_parity": {
|
||||
"commands": {
|
||||
"exact": true,
|
||||
"max_difference": 0.0
|
||||
},
|
||||
"valid": {
|
||||
"exact": true,
|
||||
"max_difference": 0.0
|
||||
},
|
||||
"heading_bias": {
|
||||
"exact": true,
|
||||
"max_difference": 0.0
|
||||
}
|
||||
},
|
||||
"measurement_policy": "Controller computation time is estimated from publication time using the recorded median latency; exact vehicle motion under changed commands is unknown.",
|
||||
"clean_policy": "Every sample from request time minus 0.5 s through plus 0.65 s is active, valid, fresh, unpressed and within 1 Nm raw driver torque. Demand is absolute desired curvature times current speed squared; substantial means at least 0.5 m/s2.",
|
||||
"driver_policy": "All replay inputs retain driver interference; only comparison metrics use the clean mask. History-reset failures intentionally retain nearby driver context.",
|
||||
"coordinates": "Elapsed seconds shifted to the first fixture control cycle; model x/y/heading are vehicle-relative, not global position.",
|
||||
"retained_fields": [
|
||||
"t",
|
||||
"episode",
|
||||
"model_index",
|
||||
"models",
|
||||
"desired_curvature",
|
||||
"yaw_rate",
|
||||
"speed",
|
||||
"measurement_time",
|
||||
"model_time",
|
||||
"reference_time",
|
||||
"active",
|
||||
"valid",
|
||||
"pressed",
|
||||
"steering_torque",
|
||||
"pscm_timestamp",
|
||||
"pscm_valid",
|
||||
"pscm_lateral_state",
|
||||
"pscm_limit",
|
||||
"pscm_capability",
|
||||
"pscm_denied",
|
||||
"clean_rawtorque",
|
||||
"demand",
|
||||
"window_masks",
|
||||
"evidence",
|
||||
"baseline_commands",
|
||||
"baseline_valid",
|
||||
"baseline_heading_base",
|
||||
"baseline_heading_target",
|
||||
"baseline_heading_bias",
|
||||
"baseline_feedback_yaw_error",
|
||||
"baseline_feedback_reference_curvature",
|
||||
"baseline_status",
|
||||
"baseline_offset_target"
|
||||
],
|
||||
"omitted_data": "No route/device identifiers, VIN, GPS, private paths, raw wheel angle, wheel rate, EPS torque, or absolute clock origins.",
|
||||
"baseline_status_meaning": "feedback_status from the pinned baseline",
|
||||
"windows": [
|
||||
{
|
||||
"name": "good_curve_a",
|
||||
"role": "comparison",
|
||||
"range_s": [
|
||||
20.0002130975003,
|
||||
25.0002130975003
|
||||
],
|
||||
"samples": 496,
|
||||
"clean_substantial_samples": 259
|
||||
},
|
||||
{
|
||||
"name": "first_reversal",
|
||||
"role": "reversal",
|
||||
"range_s": [
|
||||
83.0002130975003,
|
||||
92.7002130975003
|
||||
],
|
||||
"samples": 964,
|
||||
"clean_substantial_samples": 167
|
||||
},
|
||||
{
|
||||
"name": "good_curve_b",
|
||||
"role": "comparison",
|
||||
"range_s": [
|
||||
121.0002130975003,
|
||||
128.0002130975003
|
||||
],
|
||||
"samples": 695,
|
||||
"clean_substantial_samples": 308
|
||||
},
|
||||
{
|
||||
"name": "second_reversal",
|
||||
"role": "reversal",
|
||||
"range_s": [
|
||||
133.5002130975003,
|
||||
138.9002130975003
|
||||
],
|
||||
"samples": 537,
|
||||
"clean_substantial_samples": 191
|
||||
},
|
||||
{
|
||||
"name": "large_turn_driver_context_a",
|
||||
"role": "driver_context",
|
||||
"range_s": [
|
||||
150.0002130975003,
|
||||
157.0002130975003
|
||||
],
|
||||
"samples": 695,
|
||||
"clean_substantial_samples": 0
|
||||
},
|
||||
{
|
||||
"name": "over_growth",
|
||||
"role": "over_response",
|
||||
"range_s": [
|
||||
182.0002130975003,
|
||||
191.0002130975003
|
||||
],
|
||||
"samples": 897,
|
||||
"clean_substantial_samples": 66
|
||||
},
|
||||
{
|
||||
"name": "large_turn_driver_context_b",
|
||||
"role": "driver_context",
|
||||
"range_s": [
|
||||
199.0002130975003,
|
||||
205.0002130975003
|
||||
],
|
||||
"samples": 595,
|
||||
"clean_substantial_samples": 281
|
||||
},
|
||||
{
|
||||
"name": "zero_bias_release",
|
||||
"role": "under_response",
|
||||
"range_s": [
|
||||
202.0002130975003,
|
||||
205.0002130975003
|
||||
],
|
||||
"samples": 297,
|
||||
"clean_substantial_samples": 279
|
||||
}
|
||||
]
|
||||
}
|
||||
Binary file not shown.
@@ -62,11 +62,15 @@ class TestFordControlsLogging(unittest.TestCase):
|
||||
self.assertEqual(record['reference_service'], 'modelV2')
|
||||
self.assertEqual(record['reference_mono_time'], 123456789)
|
||||
self.assertEqual(record['status'], controller.diagnostics['status'])
|
||||
self.assertEqual(record['hypothesis'], 'model-pose-c0-c1-feedback-v7')
|
||||
self.assertEqual(record['hypothesis'], 'model-pose-c0-c1-feedback-v8')
|
||||
self.assertEqual(record['command'], list(controller.diagnostics['command']))
|
||||
self.assertIs(record['feedback_backoff_active'], False)
|
||||
self.assertIs(record['feedback_recovery_active'], False)
|
||||
self.assertIs(record['feedback_release_tracking_active'], False)
|
||||
self.assertIsNone(record['feedback_release_ceiling'])
|
||||
self.assertIsNone(record['feedback_curvature_delta'])
|
||||
if active and valid:
|
||||
self.assertIs(record['release_guard_active'], False)
|
||||
self.assertEqual(record['response_delay'], 0.2)
|
||||
self.assertEqual(record['desired_curvature'], 0.01)
|
||||
self.assertEqual(record['measured_curvature'], 0.005)
|
||||
@@ -76,7 +80,10 @@ class TestFordControlsLogging(unittest.TestCase):
|
||||
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_offset_base', 'model_heading_base',
|
||||
'curvature_offset_base', 'curvature_heading_base', 'model_share', 'base_guard')))
|
||||
'curvature_offset_base', 'curvature_heading_base', 'model_share', 'base_guard',
|
||||
'offset_target_unguarded', 'heading_target_unguarded',
|
||||
'release_guard_reference_curvature', 'feedback_release_ceiling',
|
||||
'feedback_curvature_delta')))
|
||||
|
||||
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.),
|
||||
@@ -145,6 +152,60 @@ class TestFordControlsLogging(unittest.TestCase):
|
||||
self.assertIs(record['feedback_backoff_active'], False)
|
||||
self.assertEqual(record['heading_bias'], controller.diagnostics['heading_bias'])
|
||||
|
||||
def test_periodic_diagnostics_expose_release_guard_during_feedback_history_reset(self):
|
||||
for sign in (-1, 1):
|
||||
with self.subTest(sign=sign):
|
||||
controller = FordVirtualAngleController()
|
||||
for i in range(60):
|
||||
now = 1. + i * .01
|
||||
controller.update(circle(sign * .02), sign * .02, yaw_rate=sign * .2, speed=10., now=now, measurement_time=now,
|
||||
model_time=now, reference_time=now, active=True, pscm_status=PscmStatus(now, 2, 0, 2, False))
|
||||
for now, pressed, measurement, expected in ((1.6, True, 1.6, 'driver_override'),
|
||||
(1.61, False, 1.61, 'history'), (1.62, False, 1.61, 'no_new_measurement')):
|
||||
controller.update(circle(sign * .03), sign * .018, yaw_rate=sign * .4, speed=10., now=now, measurement_time=measurement,
|
||||
model_time=now, reference_time=now, active=True, steering_pressed=pressed,
|
||||
pscm_status=PscmStatus(now, 2, 0, 2, False))
|
||||
controls = SimpleNamespace(ford_path_controller=controller, curvature=sign * .04,
|
||||
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)
|
||||
self.assertIs(record['release_guard_active'], not pressed)
|
||||
self.assertAlmostEqual(record['release_guard_reference_curvature'], sign * .02)
|
||||
self.assertEqual(record['heading_bias'], 0.)
|
||||
for field in ('offset_target', 'heading_target'):
|
||||
self.assertEqual(record[field + '_unguarded'], controller.diagnostics[field + '_unguarded'])
|
||||
if pressed:
|
||||
self.assertEqual(record[field], record[field + '_unguarded'])
|
||||
else:
|
||||
self.assertLess(sign * record[field], sign * record[field + '_unguarded'])
|
||||
|
||||
def test_periodic_diagnostics_expose_accepted_release_tracking_and_its_ceiling(self):
|
||||
for sign in (-1, 1):
|
||||
with self.subTest(sign=sign):
|
||||
controller = FordVirtualAngleController()
|
||||
for i in range(60):
|
||||
now = 1. + i * .01
|
||||
controller.update(circle(sign * .04), sign * .04, yaw_rate=sign * .4, speed=10., now=now, measurement_time=now,
|
||||
model_time=now, reference_time=now, active=True, pscm_status=PscmStatus(now, 2, 0, 2, False))
|
||||
for now in (1.6, 1.61):
|
||||
controller.update(circle(sign * .02), sign * .03, yaw_rate=sign * .2, speed=10., now=now, measurement_time=1.6,
|
||||
model_time=now, reference_time=now, active=True, pscm_status=PscmStatus(now, 2, 0, 2, False))
|
||||
controls = SimpleNamespace(ford_path_controller=controller, curvature=sign * .02,
|
||||
sm=SimpleNamespace(logMonoTime={'modelV2': int(now * 1e9), 'carState': 1_600_000_000}))
|
||||
record = self.emit_controls_event('Ford C2-free path tracking', controls)
|
||||
fresh = now == 1.6
|
||||
self.assertEqual(record['feedback_status'], 'release_tracking' if fresh else 'no_new_measurement')
|
||||
self.assertIs(record['feedback_release_tracking_active'], fresh)
|
||||
self.assertIs(record['feedback_recovery_active'], False)
|
||||
self.assertIs(record['release_guard_active'], False)
|
||||
if fresh:
|
||||
self.assertLess(sign * record['feedback_curvature_delta'], 0.)
|
||||
self.assertGreater(sign * record['heading_target'], sign * record['heading_base'])
|
||||
self.assertLessEqual(sign * record['heading_target'], record['feedback_release_ceiling'])
|
||||
else:
|
||||
self.assertIsNone(record['feedback_release_ceiling'])
|
||||
self.assertIsNone(record['feedback_curvature_delta'])
|
||||
|
||||
def test_actual_ford_branch_uses_selected_reference_and_disables_invalid_output(self):
|
||||
source_path = Path(__file__).resolve().parents[1] / 'controlsd.py'
|
||||
source = ast.parse(source_path.read_text())
|
||||
|
||||
@@ -45,12 +45,14 @@ class TestFordHeadingRecovery(unittest.TestCase):
|
||||
self.assertAlmostEqual(feedback.bias, 0.)
|
||||
self.assertAlmostEqual(sign * target, .2)
|
||||
self.assertEqual(feedback.diagnostics['feedback_status'], 'release_recovery')
|
||||
# The request is still releasing, but zero bias cannot become boost.
|
||||
# The recovery update stops exactly at zero. A later new observation
|
||||
# can enter the separately bounded release-tracking policy.
|
||||
target = update(feedback, sign, .66, yaw=0.)
|
||||
self.assertAlmostEqual(feedback.bias, 0.)
|
||||
self.assertAlmostEqual(sign * target, .2)
|
||||
self.assertEqual(feedback.diagnostics['feedback_status'], 'release')
|
||||
self.assertGreater(sign * feedback.bias, 0.)
|
||||
self.assertLessEqual(sign * target, feedback.diagnostics['feedback_release_ceiling'])
|
||||
self.assertEqual(feedback.diagnostics['feedback_status'], 'release_tracking')
|
||||
self.assertFalse(feedback.diagnostics['feedback_recovery_active'])
|
||||
self.assertTrue(feedback.diagnostics['feedback_release_tracking_active'])
|
||||
|
||||
def test_opposing_delayed_request_blocks_recovery_even_with_both_positive_errors(self):
|
||||
for sign in (-1, 1):
|
||||
@@ -88,14 +90,15 @@ class TestFordHeadingRecovery(unittest.TestCase):
|
||||
self.assertAlmostEqual(feedback.bias, retained)
|
||||
self.assertNotEqual(feedback.diagnostics['feedback_status'], 'release_recovery')
|
||||
|
||||
def test_release_does_not_recover_a_same_direction_bias(self):
|
||||
def test_same_direction_bias_uses_release_tracking_instead_of_opposing_bias_recovery(self):
|
||||
for sign in (-1, 1):
|
||||
feedback, previous = acquired_correction(sign, yaw=.2)
|
||||
retained = feedback.bias * .2 / .3
|
||||
self.assertGreater(sign * retained, 0.)
|
||||
update(feedback, sign, .6, previous=previous)
|
||||
self.assertAlmostEqual(feedback.bias, retained)
|
||||
self.assertEqual(feedback.diagnostics['feedback_status'], 'release')
|
||||
self.assertGreater(sign * feedback.bias, sign * retained)
|
||||
self.assertEqual(feedback.diagnostics['feedback_status'], 'release_tracking')
|
||||
self.assertFalse(feedback.diagnostics['feedback_recovery_active'])
|
||||
|
||||
def test_fresh_recovery_clears_backoff_without_reusing_measurements(self):
|
||||
for sign in (-1, 1):
|
||||
|
||||
@@ -88,9 +88,13 @@ class TestFordHeadingRecoveryRoutes(unittest.TestCase):
|
||||
self.assertTrue((self.heading_bias[mask] * self.bias_before_update[mask] >= -1e-12).all())
|
||||
self.assertTrue((abs(self.heading_target[mask]) <= abs(self.heading_base[mask]) + 1e-12).all())
|
||||
|
||||
def test_c0_base_and_output_validity_are_unchanged(self):
|
||||
def test_guard_does_not_add_c0_and_preserves_heading_base_and_validity(self):
|
||||
evidence = self.data['evidence']
|
||||
np.testing.assert_array_equal(self.commands[evidence, 0], self.data['baseline_commands'][evidence, 0])
|
||||
# The release guard now intentionally prevents same-direction C0 growth.
|
||||
# Keep the original fixture's no-extra-demand requirement and its separate
|
||||
# good-curve retention checks, rather than insisting on old excess demand.
|
||||
direction = np.sign(self.data['desired_curvature'][evidence])
|
||||
self.assertTrue(((self.commands[evidence, 0] - self.data['baseline_commands'][evidence, 0]) * direction <= 1e-12).all())
|
||||
np.testing.assert_array_equal(self.heading_base[evidence], self.data['baseline_heading_base'][evidence])
|
||||
np.testing.assert_array_equal(self.gates[evidence], self.data['baseline_valid'][evidence])
|
||||
|
||||
|
||||
@@ -0,0 +1,46 @@
|
||||
"""Request-level turn-exit regressions; these do not simulate EPS response."""
|
||||
import unittest
|
||||
|
||||
from openpilot.selfdrive.controls.lib.ford_virtual_angle import FordVirtualAngleController
|
||||
from openpilot.selfdrive.controls.tests.test_ford_curvature_c0 import step
|
||||
from openpilot.selfdrive.controls.tests.test_ford_heading_recovery import acquired_correction, update
|
||||
from openpilot.selfdrive.controls.tests.test_ford_path_reference import circle
|
||||
from openpilot.selfdrive.controls.lib.ford_virtual_angle import PscmStatus
|
||||
|
||||
|
||||
class TestFordTurnExit(unittest.TestCase):
|
||||
def test_release_can_correct_a_deficit_after_opposing_bias_is_gone(self):
|
||||
for sign in (-1, 1):
|
||||
with self.subTest(sign=sign):
|
||||
feedback, previous = acquired_correction(sign, yaw=.3)
|
||||
self.assertAlmostEqual(feedback.bias, 0.)
|
||||
target = update(feedback, sign, .6, base=.1, desired=.02, yaw=.1, previous=previous)
|
||||
self.assertGreater(sign * target, .1)
|
||||
first_bias = sign * feedback.bias
|
||||
target = update(feedback, sign, .61, base=.0995, desired=.0199, yaw=.1, previous=sign * target)
|
||||
self.assertGreater(sign * feedback.bias, first_bias)
|
||||
self.assertLessEqual(sign * target, previous)
|
||||
|
||||
def test_model_growth_cannot_defeat_release_after_driver_bias_reset(self):
|
||||
for sign in (-1, 1):
|
||||
with self.subTest(sign=sign):
|
||||
controller = FordVirtualAngleController()
|
||||
for i in range(300):
|
||||
now = i * .01
|
||||
step(controller, now, sign * .03, circle(sign * .035), speed=10., yaw_rate=sign * .3,
|
||||
steering_pressed=(i == 299), pscm_status=PscmStatus(now, 2, 0, 2, False))
|
||||
prior_offset, prior_heading = controller.offset_request, controller.heading_request
|
||||
self.assertEqual(controller.feedback.bias, 0.)
|
||||
step(controller, 3., sign * .025, circle(sign * .06), speed=10., yaw_rate=sign * .4,
|
||||
pscm_status=PscmStatus(3., 2, 0, 2, False))
|
||||
self.assertLessEqual(sign * controller.offset_request, sign * prior_offset + 1e-12)
|
||||
self.assertLessEqual(sign * controller.heading_request, sign * prior_heading + 1e-12)
|
||||
# The same strong model pose stays available once new measured motion
|
||||
# shows a deficit. The guard must not impose a fixed geometry cap.
|
||||
step(controller, 3.01, sign * .024, circle(sign * .06), speed=10., yaw_rate=sign * .1,
|
||||
pscm_status=PscmStatus(3.01, 2, 0, 2, False))
|
||||
self.assertGreater(sign * controller.offset_request, sign * prior_offset)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
unittest.main()
|
||||
@@ -0,0 +1,199 @@
|
||||
"""Independent turn-exit guard properties, not a PSCM response simulation."""
|
||||
import unittest
|
||||
|
||||
from openpilot.selfdrive.controls.lib.ford_virtual_angle import FordVirtualAngleController, HeadingFeedback, PathTuning, PscmStatus, ReleaseGuard
|
||||
|
||||
|
||||
AUTO_STATUS = object()
|
||||
|
||||
|
||||
def guard_step(guard, sign, now, *, desired=.02, yaw=.4, speed=10., measurement=None, pscm=AUTO_STATUS, **overrides):
|
||||
inputs = {'yaw_rate': sign * yaw, 'speed': speed, 'now': now, 'measurement_time': now if measurement is None else measurement,
|
||||
'heading_horizon': 10., 'driver_override': False,
|
||||
'pscm_status': PscmStatus(now, 2, 0, 2, False) if pscm is AUTO_STATUS else pscm}
|
||||
inputs.update(overrides)
|
||||
return guard.update(sign * desired, **inputs)
|
||||
|
||||
|
||||
def warm_guard(guard, sign, last_override=False):
|
||||
for i in range(40):
|
||||
guard_step(guard, sign, i * .01, desired=.03, yaw=.3, driver_override=last_override and i == 39)
|
||||
|
||||
|
||||
def feedback_step(feedback, sign, now, *, base=.1, desired=.02, yaw=.1, speed=10., previous=.3, measurement=None, **overrides):
|
||||
inputs = {'yaw_rate': sign * yaw, 'speed': speed, 'now': now, 'measurement_time': now if measurement is None else measurement,
|
||||
'dt': .01, 'previous_command': sign * previous, 'heading_horizon': 10., 'driver_override': False,
|
||||
'pscm_status': PscmStatus(now, 2, 0, 2, False)}
|
||||
inputs.update(overrides)
|
||||
return feedback.update(sign * base, sign * desired, **inputs)
|
||||
|
||||
|
||||
def warm_feedback(sign, speed=10., yaw=.3):
|
||||
feedback = HeadingFeedback(.2, PathTuning())
|
||||
previous = .3
|
||||
for i in range(60):
|
||||
target = feedback_step(feedback, sign, i * .01, base=.3, desired=.03, yaw=yaw, speed=speed, previous=previous)
|
||||
previous = sign * target
|
||||
return feedback, previous
|
||||
|
||||
|
||||
class TestFordReleaseGuard(unittest.TestCase):
|
||||
def test_only_growth_in_the_requested_direction_is_capped(self):
|
||||
for sign in (-1, 1):
|
||||
with self.subTest(sign=sign):
|
||||
guard = ReleaseGuard(.2)
|
||||
warm_guard(guard, sign)
|
||||
self.assertTrue(guard_step(guard, sign, .4))
|
||||
self.assertAlmostEqual(guard.limit(sign * .5, sign * .2), sign * .2)
|
||||
self.assertAlmostEqual(guard.limit(sign * .1, sign * .2), sign * .1)
|
||||
self.assertAlmostEqual(guard.limit(-sign * .5, sign * .2), -sign * .5)
|
||||
self.assertAlmostEqual(guard.limit(sign * .5, -sign * .2), 0.)
|
||||
|
||||
def test_driver_reset_does_not_erase_valid_request_history(self):
|
||||
for sign in (-1, 1):
|
||||
guard = ReleaseGuard(.2)
|
||||
warm_guard(guard, sign, last_override=True)
|
||||
self.assertFalse(guard.active)
|
||||
self.assertTrue(guard_step(guard, sign, .4))
|
||||
self.assertAlmostEqual(guard.reference_curvature, sign * .03)
|
||||
|
||||
def test_repeated_measurement_keeps_guard_but_current_override_disables_it(self):
|
||||
for sign in (-1, 1):
|
||||
guard = ReleaseGuard(.2)
|
||||
warm_guard(guard, sign)
|
||||
self.assertTrue(guard_step(guard, sign, .4))
|
||||
self.assertTrue(guard_step(guard, sign, .41, measurement=.4))
|
||||
self.assertFalse(guard_step(guard, sign, .42, measurement=.4, driver_override=True))
|
||||
self.assertAlmostEqual(guard.limit(sign * .5, sign * .2), sign * .5)
|
||||
self.assertTrue(guard_step(guard, sign, .43))
|
||||
|
||||
def test_status_speed_zero_and_reversal_disable_action(self):
|
||||
cases = [
|
||||
{'pscm': None}, {'pscm': PscmStatus(.1, 2, 0, 2, False)},
|
||||
{'pscm': PscmStatus(.4, 2, 0, 2, False, False)}, {'pscm': PscmStatus(.4, 2, 0, 2, True)},
|
||||
{'pscm': PscmStatus(.4, 1, 0, 2, False)}, {'pscm': PscmStatus(.4, 2, 0, 0, False)},
|
||||
{'pscm': PscmStatus(.4, 2, 3, 2, False)}, {'pscm': PscmStatus(float('nan'), 2, 0, 2, False)},
|
||||
{'speed': 1.99}, {'desired': 0.}, {'desired': -.02},
|
||||
]
|
||||
for sign in (-1, 1):
|
||||
for overrides in cases:
|
||||
with self.subTest(sign=sign, overrides=overrides):
|
||||
guard = ReleaseGuard(.2)
|
||||
warm_guard(guard, sign)
|
||||
self.assertFalse(guard_step(guard, sign, .4, **overrides))
|
||||
self.assertAlmostEqual(guard.limit(sign * .5, sign * .2), sign * .5)
|
||||
|
||||
def test_repeated_backward_status_cannot_restore_guard_authority(self):
|
||||
guard = ReleaseGuard(.2)
|
||||
warm_guard(guard, 1)
|
||||
self.assertTrue(guard_step(guard, 1, .4))
|
||||
self.assertFalse(guard_step(guard, 1, .41, pscm=PscmStatus(.39, 2, 0, 2, False)))
|
||||
self.assertFalse(guard_step(guard, 1, .42, pscm=PscmStatus(.39, 2, 0, 2, False)))
|
||||
self.assertTrue(guard_step(guard, 1, .43, pscm=PscmStatus(.43, 2, 0, 2, False)))
|
||||
|
||||
def test_invalid_future_status_does_not_poison_later_fresh_status(self):
|
||||
guard = ReleaseGuard(.2)
|
||||
warm_guard(guard, 1)
|
||||
self.assertFalse(guard_step(guard, 1, .4, pscm=PscmStatus(10., 2, 0, 2, False)))
|
||||
self.assertTrue(guard_step(guard, 1, .41, pscm=PscmStatus(.41, 2, 0, 2, False)))
|
||||
|
||||
def test_fresh_unavailable_status_prevents_older_in_progress_reactivation(self):
|
||||
for unavailable in (PscmStatus(.41, 1, 0, 2, False), PscmStatus(.41, 2, 0, 2, True), PscmStatus(.41, 2, 0, 0, False)):
|
||||
with self.subTest(unavailable=unavailable):
|
||||
guard = ReleaseGuard(.2)
|
||||
warm_guard(guard, 1)
|
||||
self.assertTrue(guard_step(guard, 1, .4))
|
||||
self.assertFalse(guard_step(guard, 1, .41, pscm=unavailable))
|
||||
self.assertFalse(guard_step(guard, 1, .42, pscm=PscmStatus(.4, 2, 0, 2, False)))
|
||||
self.assertFalse(guard_step(guard, 1, .43, pscm=PscmStatus(.4, 2, 0, 2, False)))
|
||||
self.assertAlmostEqual(guard.limit(.5, .2), .5)
|
||||
self.assertTrue(guard_step(guard, 1, .44, pscm=PscmStatus(.44, 2, 0, 2, False)))
|
||||
|
||||
def test_parent_reset_clears_the_independent_history(self):
|
||||
controller = FordVirtualAngleController()
|
||||
warm_guard(controller.release_guard, 1)
|
||||
self.assertTrue(guard_step(controller.release_guard, 1, .4))
|
||||
controller.reset()
|
||||
self.assertFalse(guard_step(controller.release_guard, 1, .41))
|
||||
self.assertAlmostEqual(controller.release_guard.limit(.5, .2), .5)
|
||||
|
||||
|
||||
class TestFordReleaseTrackingGuards(unittest.TestCase):
|
||||
def test_repeated_measurement_does_not_add_another_release_correction(self):
|
||||
for sign in (-1, 1):
|
||||
feedback, previous = warm_feedback(sign)
|
||||
target = feedback_step(feedback, sign, .6, previous=previous)
|
||||
bias = feedback.bias
|
||||
self.assertGreater(sign * bias, 0.)
|
||||
feedback_step(feedback, sign, .61, previous=sign * target, measurement=.6)
|
||||
self.assertEqual(feedback.bias, bias)
|
||||
self.assertFalse(feedback.diagnostics['feedback_release_tracking_active'])
|
||||
feedback_step(feedback, sign, .62, previous=sign * target)
|
||||
self.assertGreater(sign * feedback.bias, sign * bias)
|
||||
|
||||
def test_response_trend_uses_curvature_despite_opposite_yaw_rate_trend(self):
|
||||
# First case: yaw rises .025->.04, but curvature falls .005->.004.
|
||||
# Second case: yaw falls .05->.04, but curvature rises .005->.008.
|
||||
for sign in (-1, 1):
|
||||
for old_speed, old_yaw, new_speed, allowed in ((5., .025, 10., True), (10., .05, 5., False)):
|
||||
with self.subTest(sign=sign, allowed=allowed):
|
||||
feedback, previous = warm_feedback(sign, speed=old_speed, yaw=old_yaw)
|
||||
retained = feedback.bias * (.1 / .3)
|
||||
feedback_step(feedback, sign, .6, speed=new_speed, yaw=.04, previous=previous)
|
||||
self.assertEqual(feedback.diagnostics['feedback_release_tracking_active'], allowed)
|
||||
if allowed:
|
||||
self.assertGreater(sign * feedback.bias, sign * retained)
|
||||
else:
|
||||
self.assertAlmostEqual(feedback.bias, retained)
|
||||
|
||||
def test_zero_release_headroom_does_not_reduce_base_or_store_boost(self):
|
||||
for sign in (-1, 1):
|
||||
feedback, previous = warm_feedback(sign)
|
||||
target = feedback_step(feedback, sign, .6, base=.4, previous=previous)
|
||||
self.assertAlmostEqual(feedback.bias, 0.)
|
||||
self.assertAlmostEqual(sign * target, .4)
|
||||
self.assertFalse(feedback.diagnostics['feedback_release_tracking_active'])
|
||||
|
||||
def test_release_tracking_needs_current_deficit_and_no_eps_limit(self):
|
||||
for sign in (-1, 1):
|
||||
for overrides in ({'yaw': .25}, {'yaw': .2}, {'pscm_status': PscmStatus(.6, 2, 2, 2, False)}):
|
||||
with self.subTest(sign=sign, overrides=overrides):
|
||||
feedback, previous = warm_feedback(sign)
|
||||
target = feedback_step(feedback, sign, .6, previous=previous, **overrides)
|
||||
self.assertAlmostEqual(feedback.bias, 0.)
|
||||
self.assertAlmostEqual(sign * target, .1)
|
||||
self.assertFalse(feedback.diagnostics['feedback_release_tracking_active'])
|
||||
|
||||
def test_release_tracking_respects_blocked_and_partial_slew_admission(self):
|
||||
for sign in (-1, 1):
|
||||
for partial in (False, True):
|
||||
with self.subTest(sign=sign, partial=partial):
|
||||
feedback, previous = warm_feedback(sign)
|
||||
feedback_step(feedback, sign, .6, previous=previous)
|
||||
retained = feedback.bias * (.09 / .1)
|
||||
heading_before = .09 + sign * retained
|
||||
previous = heading_before - (.0045 if partial else .005)
|
||||
feedback_step(feedback, sign, .61, base=.09, desired=.0199, previous=previous)
|
||||
self.assertAlmostEqual(sign * (feedback.bias - retained), .0005 if partial else 0.)
|
||||
self.assertEqual(feedback.diagnostics['feedback_release_tracking_active'], partial)
|
||||
|
||||
def test_brief_release_pause_cannot_capture_a_larger_entry_command(self):
|
||||
for sign in (-1, 1):
|
||||
with self.subTest(sign=sign):
|
||||
feedback, previous = warm_feedback(sign)
|
||||
feedback_step(feedback, sign, .6, previous=previous)
|
||||
# Hold the smaller request until the delayed reference catches up,
|
||||
# but not for a full response interval after release becomes false.
|
||||
for i in range(61, 84):
|
||||
feedback_step(feedback, sign, i * .01, desired=.02, yaw=.2)
|
||||
feedback_step(feedback, sign, .84, desired=.019, previous=.45)
|
||||
self.assertAlmostEqual(feedback.diagnostics['feedback_release_ceiling'], .1 + (.3 - .1) * (.019 / .03))
|
||||
# A full quiet response interval starts a new independent episode.
|
||||
for i in range(85, 129):
|
||||
feedback_step(feedback, sign, i * .01, desired=.019, yaw=.19)
|
||||
feedback_step(feedback, sign, 1.29, desired=.018, previous=.45)
|
||||
self.assertAlmostEqual(feedback.diagnostics['feedback_release_ceiling'], .1 + (.45 - .1) * (.018 / .019))
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
unittest.main()
|
||||
@@ -0,0 +1,181 @@
|
||||
"""Recorded-input regression checks; changed commands do not predict motion."""
|
||||
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 TestFordTurnExitRoutes(unittest.TestCase):
|
||||
@classmethod
|
||||
def setUpClass(cls):
|
||||
cls.fixture = Path(__file__).parent / 'fixtures/ford_turn_exit_requests.npz'
|
||||
cls.metadata = json.loads(cls.fixture.with_suffix('.json').read_text())
|
||||
cls.data = dict(np.load(cls.fixture, allow_pickle=False))
|
||||
d = cls.data
|
||||
models = [SimpleNamespace(position=SimpleNamespace(x=p[0], y=p[1]), orientation=SimpleNamespace(z=p[2])) for p in d['models']]
|
||||
commands, gates, rows, previous_commands, prior_biases = [], [], [], [], []
|
||||
episode = None
|
||||
for i, now in enumerate(d['t']):
|
||||
if d['episode'][i] != episode:
|
||||
controller = FordVirtualAngleController(response_delay=cls.metadata['response_delay'])
|
||||
episode = d['episode'][i]
|
||||
previous_commands.append([controller.offset_request, controller.heading_request])
|
||||
old_base, old_bias = controller.feedback.previous_base, controller.feedback.bias
|
||||
eps = PscmStatus(float(d['pscm_timestamp'][i]), int(d['pscm_lateral_state'][i]), int(d['pscm_limit'][i]),
|
||||
int(d['pscm_capability'][i]), bool(d['pscm_denied'][i]), bool(d['pscm_valid'][i]))
|
||||
path = controller.update(models[d['model_index'][i]], d['desired_curvature'][i], yaw_rate=d['yaw_rate'][i], speed=d['speed'][i],
|
||||
now=now, measurement_time=d['measurement_time'][i], model_time=d['model_time'][i],
|
||||
reference_time=d['reference_time'][i], active=bool(d['active'][i]), valid=bool(d['valid'][i]),
|
||||
steering_pressed=bool(d['pressed'][i]), steering_torque=d['steering_torque'][i], pscm_status=eps)
|
||||
row = dict(controller.diagnostics)
|
||||
base = row.get('heading_base', 0.)
|
||||
retained = old_bias if old_base is not None and old_base * base >= 0. else 0.
|
||||
if old_base and old_base * base >= 0.:
|
||||
retained *= min(1., abs(base / old_base))
|
||||
prior_biases.append(retained)
|
||||
commands.append([path.path_offset, path.path_angle, path.curvature, path.curvature_rate])
|
||||
gates.append(path.valid)
|
||||
rows.append(row)
|
||||
cls.commands, cls.gates = np.array(commands), np.array(gates)
|
||||
cls.previous_commands, cls.prior_biases = np.array(previous_commands), np.array(prior_biases)
|
||||
cls.status = np.array([row['feedback_status'] for row in rows])
|
||||
for key in ('heading_base', 'heading_bias', 'offset_target', 'heading_target', 'offset_target_unguarded', 'heading_target_unguarded',
|
||||
'feedback_yaw_error', 'feedback_reference_curvature', 'release_guard_reference_curvature',
|
||||
'feedback_release_ceiling', 'feedback_curvature_delta', 'heading_horizon'):
|
||||
setattr(cls, key, np.array([row.get(key, np.nan) for row in rows], dtype=float))
|
||||
cls.guarded = np.array([row.get('release_guard_active', False) for row in rows])
|
||||
cls.tracking = np.array([row.get('feedback_release_tracking_active', False) for row in rows])
|
||||
|
||||
def window(self, name):
|
||||
index = next(i for i, window in enumerate(self.metadata['windows']) if window['name'] == name)
|
||||
return self.data['window_masks'][:, index]
|
||||
|
||||
def test_fixture_provenance_and_minimal_signal_schema(self):
|
||||
self.assertEqual(hashlib.sha256(self.fixture.read_bytes()).hexdigest(), self.metadata['fixture_sha256'])
|
||||
self.assertEqual(self.metadata['baseline_revision'], 'dfcfddb91ce2409511f5b2dbce25d06d5056b3d6')
|
||||
self.assertEqual(self.metadata['baseline_hypothesis'], 'model-pose-c0-c1-feedback-v7')
|
||||
self.assertEqual(set(self.data), set(self.metadata['retained_fields']))
|
||||
self.assertGreater(self.metadata['samples'], 15000)
|
||||
self.assertEqual(len(self.data['t']), self.metadata['samples'])
|
||||
self.assertGreater(int(self.data['evidence'].sum()), 4000)
|
||||
self.assertTrue(np.isfinite(self.data['models']).all())
|
||||
self.assertEqual(self.data['t'][0], 0.)
|
||||
for key in ('wheel_deg', 'wheel_rate', 'eps_torque', 'recorded_wheel_curvature', 'origin_ns', 'publication_time'):
|
||||
self.assertNotIn(key, self.data)
|
||||
for key in ('commands', 'valid', 'heading_bias'):
|
||||
self.assertTrue(self.metadata['compact_full_baseline_evidence_parity'][key]['exact'])
|
||||
for digest in self.metadata['baseline_source_hashes'].values():
|
||||
self.assertRegex(digest, r'^[0-9a-f]{64}$')
|
||||
|
||||
def test_recorded_model_growth_is_guarded_while_feedback_rebuilds_history(self):
|
||||
d = self.data
|
||||
# Select the problem using pinned baseline status and recorded inputs,
|
||||
# not candidate success. Nearby driver input is deliberately retained.
|
||||
selected = self.window('first_reversal') | self.window('second_reversal') | self.window('over_growth')
|
||||
index = np.searchsorted(d['t'], d['measurement_time'] - self.metadata['response_delay'], side='right') - 1
|
||||
safe_index = np.maximum(index, 0)
|
||||
delayed = d['desired_curvature'][safe_index]
|
||||
direction = np.sign(d['desired_curvature'])
|
||||
horizon = np.maximum(7., d['speed'])
|
||||
mask = (selected & (d['baseline_status'] == 'history') & d['baseline_valid'] & (index >= 0) &
|
||||
(d['episode'][safe_index] == d['episode']) & ~d['pressed'] & (abs(d['steering_torque']) <= 1.) &
|
||||
(delayed * d['desired_curvature'] > 0.) & ((abs(delayed) - abs(d['desired_curvature'])) * horizon > .0005) &
|
||||
((d['yaw_rate'] - d['speed'] * delayed) * direction > 0.) &
|
||||
((d['yaw_rate'] - d['speed'] * d['desired_curvature']) * direction > 0.))
|
||||
self.assertGreater(int(mask.sum()), 25)
|
||||
self.assertGreater(int((mask & self.window('over_growth')).sum()), 10)
|
||||
self.assertTrue(self.guarded[mask].all())
|
||||
self.assertTrue((self.status[mask] == 'history').all())
|
||||
np.testing.assert_array_equal(self.heading_bias[mask], 0.)
|
||||
targets = np.column_stack((self.offset_target, self.heading_target))
|
||||
unguarded = np.column_stack((self.offset_target_unguarded, self.heading_target_unguarded))
|
||||
for sign in (-1, 1):
|
||||
case = mask & (direction == sign)
|
||||
self.assertGreater(int(case.sum()), 0)
|
||||
growth_removed = (unguarded[case] - targets[case]) * sign
|
||||
self.assertTrue((growth_removed >= -1e-12).all())
|
||||
self.assertTrue((growth_removed.max(axis=0) > [.01, .0005]).all())
|
||||
|
||||
def test_every_release_guard_ceiling_preserves_opposing_path_terms(self):
|
||||
mask = self.guarded
|
||||
d = self.data
|
||||
self.assertGreater(int(mask.sum()), 100)
|
||||
direction = np.sign(d['desired_curvature'][mask])[:, None]
|
||||
targets = np.column_stack((self.offset_target, self.heading_target))[mask]
|
||||
unguarded = np.column_stack((self.offset_target_unguarded, self.heading_target_unguarded))[mask]
|
||||
previous = self.previous_commands[mask]
|
||||
same_direction = unguarded * direction > 0.
|
||||
self.assertTrue((targets * direction <= np.maximum(previous * direction, 0.) + 1e-12)[same_direction].all())
|
||||
np.testing.assert_array_equal(targets[~same_direction], unguarded[~same_direction])
|
||||
self.assertTrue((~d['pressed'][mask] & (abs(d['steering_torque'][mask]) <= 1.) & (d['pscm_limit'][mask] < 3)).all())
|
||||
|
||||
def test_recorded_zero_bias_release_can_track_with_bounded_new_correction(self):
|
||||
d = self.data
|
||||
base = d['baseline_heading_base']
|
||||
current_error = d['speed'] * d['desired_curvature'] - d['yaw_rate']
|
||||
mask = (self.window('zero_bias_release') & d['clean_rawtorque'] & (d['demand'] >= .5) &
|
||||
(d['baseline_status'] == 'release') & (abs(d['baseline_heading_bias']) <= 1e-9) & (d['pscm_limit'] < 2) &
|
||||
(current_error * base > 0.) & (d['baseline_feedback_yaw_error'] * base > 0.) &
|
||||
(d['desired_curvature'] * base > 0.) & (d['baseline_feedback_reference_curvature'] * base > 0.))
|
||||
self.assertGreater(int(mask.sum()), 100)
|
||||
direction = np.sign(d['desired_curvature'][mask])
|
||||
increase = (self.commands[mask, 1] - d['baseline_commands'][mask, 1]) * direction
|
||||
self.assertGreater(float(np.median(increase)), .005)
|
||||
self.assertGreater(int((mask & self.tracking).sum()), 25)
|
||||
np.testing.assert_array_equal(self.commands[mask, 0], d['baseline_commands'][mask, 0])
|
||||
|
||||
def test_release_tracking_admits_only_eligible_reachable_headroom(self):
|
||||
d, mask = self.data, self.tracking
|
||||
self.assertGreater(int(mask.sum()), 25)
|
||||
base, bias = self.heading_base[mask], self.heading_bias[mask]
|
||||
sign = np.sign(base)
|
||||
self.assertTrue((self.status[mask] == 'release_tracking').all())
|
||||
self.assertTrue((d['pscm_limit'][mask] < 2).all())
|
||||
self.assertTrue((d['pscm_valid'][mask] & self.gates[mask] & ~d['pressed'][mask]).all())
|
||||
self.assertTrue((abs(d['steering_torque'][mask]) <= 1.).all())
|
||||
self.assertTrue((self.prior_biases[mask] * base >= 0.).all())
|
||||
self.assertTrue((self.feedback_yaw_error[mask] * base > 0.).all())
|
||||
current_error = d['speed'][mask] * d['desired_curvature'][mask] - d['yaw_rate'][mask]
|
||||
self.assertTrue((current_error * base > 0.).all())
|
||||
self.assertTrue((d['desired_curvature'][mask] * base > 0.).all())
|
||||
self.assertTrue((self.feedback_reference_curvature[mask] * base > 0.).all())
|
||||
self.assertTrue((sign * self.feedback_curvature_delta[mask] * self.heading_horizon[mask] <= .0005 + 1e-12).all())
|
||||
self.assertTrue(((bias - self.prior_biases[mask]) * sign > 0.).all())
|
||||
self.assertTrue(((base + bias) * sign <= self.feedback_release_ceiling[mask] + 1e-12).all())
|
||||
|
||||
def test_raw_model_bases_and_validity_remain_unchanged_on_evidence(self):
|
||||
d, mask = self.data, self.data['evidence']
|
||||
np.testing.assert_array_equal(self.gates[mask], d['baseline_valid'][mask])
|
||||
valid = mask & self.gates
|
||||
np.testing.assert_array_equal(self.heading_base[valid], d['baseline_heading_base'][valid])
|
||||
np.testing.assert_array_equal(self.offset_target_unguarded[valid], d['baseline_offset_target'][valid])
|
||||
|
||||
def test_preselected_good_curves_retain_command_scale(self):
|
||||
d = self.data
|
||||
for name in ('good_curve_a', 'good_curve_b'):
|
||||
with self.subTest(window=name):
|
||||
mask = self.window(name) & d['clean_rawtorque'] & (d['demand'] >= .5)
|
||||
self.assertGreater(int(mask.sum()), 200)
|
||||
old, new = d['baseline_commands'][mask, :2], self.commands[mask, :2]
|
||||
# These are collateral command bounds, not a new-motion prediction.
|
||||
self.assertTrue((np.median(abs(new), axis=0) >= .95 * np.median(abs(old), axis=0)).all())
|
||||
self.assertTrue((np.median(abs(new), axis=0) <= 1.05 * np.median(abs(old), axis=0)).all())
|
||||
self.assertTrue((np.quantile(abs(new - old), .9, axis=0) <= [.02, .005]).all())
|
||||
|
||||
def test_all_commands_keep_zero_c2_c3_and_existing_field_and_rate_limits(self):
|
||||
d = self.data
|
||||
np.testing.assert_array_equal(self.commands[:, 2:], 0.)
|
||||
self.assertTrue(np.isfinite(self.commands).all())
|
||||
self.assertTrue((abs(self.commands[:, :2]) <= [5.110000001, .500000001]).all())
|
||||
continuous = (d['episode'][1:] == d['episode'][:-1]) & self.gates[1:] & self.gates[:-1]
|
||||
allowed = np.diff(d['t'])[:, None] * [4., .5] + [.01, .0005] + np.array([1e-8, 1e-8])
|
||||
self.assertTrue((abs(np.diff(self.commands[:, :2], axis=0))[continuous] <= allowed[continuous]).all())
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
unittest.main()
|
||||
@@ -2184,7 +2184,7 @@
|
||||
"needs_onroad_cycle": true,
|
||||
"title": "C2-Free Path Tracking (Experimental)",
|
||||
"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, a previous opposing correction can unwind when turning falls below both recent and current requests. When the steering controller reports a reached limit, the 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.",
|
||||
"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, planned-curvature tracking can recover within bounded earlier-command headroom when measured turning falls below both recent and current requests and is no longer increasing. An opposing correction still unwinds only to zero. When turning exceeds both requests, a separate guard prevents model-driven growth of the offset and heading requests, including after driver input resets feedback. The guard preserves opposing centering terms. A reported steering-controller limit still prevents request-increasing correction. 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"
|
||||
|
||||
@@ -15,7 +15,7 @@ sections:
|
||||
needs_onroad_cycle: true
|
||||
title: C2-Free Path Tracking (Experimental)
|
||||
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, a previous opposing correction can unwind when turning falls below both recent and current requests. When the steering controller reports a reached limit, the 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.
|
||||
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, planned-curvature tracking can recover within bounded earlier-command headroom when measured turning falls below both recent and current requests and is no longer increasing. An opposing correction still unwinds only to zero. When turning exceeds both requests, a separate guard prevents model-driven growth of the offset and heading requests, including after driver input resets feedback. The guard preserves opposing centering terms. A reported steering-controller limit still prevents request-increasing correction. 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
|
||||
|
||||
Reference in New Issue
Block a user