diff --git a/docs/ford_virtual_angle_experiment.md b/docs/ford_virtual_angle_experiment.md index c7d73ab22f..ff3ba4ba96 100644 --- a/docs/ford_virtual_angle_experiment.md +++ b/docs/ford_virtual_angle_experiment.md @@ -1,15 +1,18 @@ # Ford C2-free model-pose tracking with measured feedback -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. +Hypothesis `model-pose-c0-c1-feedback-v7` retains v6's model-pose C0/C1 base +and adds narrow recovery of an opposing C1 bias during release. C0, model +geometry, blending, gains, rates and output bounds are unchanged. Recovery +can remove retained opposing bias but cannot create bias beyond zero or +relax the PSCM LimitReached growth restriction. C2/C3 remain zero, and +selected desired curvature remains the measured-yaw feedback target. This is an experimental outer controller around the multivariable PSCM. Its geometry does not define a calibrated C0/C1-to-wheel mapping or an angle servo. Command replay cannot establish the truck's response, closed-loop stability, or an overshoot improvement. +The v7 recovery change has offline validation only; its physical response +is unproven. ## Evidence and scope @@ -74,7 +77,7 @@ The existing pose encoder retains its quantization and field-allocation rules. The residual-curvature lift is geometric, not a claim of EPS equivalence to C2. The inherited pose encoder allocates heading overflow using its asymmetric -limits (+0.5235/−0.5 rad), before v6 applies the symmetric final ±0.5 rad +limits (+0.5235/−0.5 rad), before the symmetric final ±0.5 rad heading bound. On clipped tails, this can leave mirrored C0 requests differing by up to 0.0235 rad × 7 m = 0.1645 m. The favorable comparison anchors lie below that heading cap; full model-base odd symmetry is not claimed. @@ -120,8 +123,26 @@ 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. +carry it through zero. Existing host field and slew limits still apply. + +The new release-recovery exception requires fresh valid PSCM status with +limit below 2, retained bias opposing the base, and both current and delayed +requests aligned with that base. Measured turning must be below both requests +in their direction. It then uses the current yaw deficit × the existing +feedback gain × measurement interval to unwind only the opposing bias toward +zero. The increment is clipped so recovery cannot cross zero bias or create +demand beyond the existing base. Common host anti-windup still limits what +can be accepted. All other constrained cases remain frozen; PSCM limit 2 +never permits this request-increasing recovery. +The no-new-bias restriction applies only to `release_recovery`. Once release +ends, ordinary eligible integration can add correction beyond the base as +before; its existing limits and guards are unchanged. + +`release_recovery` and `feedback_recovery_active=true` indicate that the +recovery branch actually changed bias on that update. If host anti-windup +blocks the entire increment, the status remains `host_limit` and the flag is +false. Recovery is evaluated only on fresh measurements; the flag is false +on repeated-measurement updates and after reset. Diagnostics distinguish `release_backoff` and `pscm_backoff`; a release takes precedence when both conditions apply. While `feedback_backoff_active` is @@ -169,9 +190,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 v6 on Ford CAN FD `FORD_F_150_LIGHTNING_MK1` +requirement. Enabled selects v7 on Ford CAN FD `FORD_F_150_LIGHTNING_MK1` regardless of missing or different EPS firmware-query results. Other platforms -retain their existing controller. V6 takes priority over PSCM Coefficient +retain their existing controller. V7 takes priority over PSCM Coefficient Observer while selected; disabling and cycling offroad/onroad restores the previous selection. Controller selection does not force lateral engagement. @@ -180,7 +201,7 @@ 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 v6. +The 5 Hz `Ford C2-free path tracking` event keeps its name and identifies v7. `model_offset_base` / `model_heading_base` report the already weighted and encoded model contribution; `curvature_offset_base` / `curvature_heading_base` report the residual-curvature contribution. `model_share` and `base_guard` @@ -192,6 +213,11 @@ The event retains source timestamps, measured curvature/yaw, final commands, slew scales, feedback bias/status/history, raw torque and PSCM status/age. `feedback_backoff_active` records the persistent heading ceiling, including cycles whose feedback status is `no_new_measurement`. +`feedback_recovery_active` records an accepted recovery increment on this +update only; it does not persist between measurements. +`feedback_yaw_error` retains its delayed-reference meaning. Recovery instead +uses current error, reconstructed from logged `desired_curvature`, +synchronized car-state speed and `yaw_rate`; those two errors can differ. During backoff, `heading_target` can be lower in the request direction than the bounded sum of `heading_base` and `heading_bias`, because the temporary ceiling is not part of the stored bias. @@ -202,7 +228,9 @@ Validation must cover large recorded maneuvers, flat-model centering, both turn directions, model/action disagreement, share transitions, release and reversal, release/limit backoff without growth or zero crossing, status/driver resets, reference causality, bounds, slew and CAN packing with C2/C3 zero. +Recovery checks cover both directions, stopping at zero bias, repeated +measurements, current-and-delayed agreement, and rejection at PSCM limit 2. Old v3/v4 command-equality expectations do not define -v6 success. Historical v5 replay results remain historical observations. +v7 success. Historical v5/v6 replay results remain historical observations. Replay fixes recorded motion and planner outputs, so enabled vehicle logs are still required to assess tracking error, oscillation and interventions. diff --git a/openpilot/selfdrive/controls/lib/ford_virtual_angle.py b/openpilot/selfdrive/controls/lib/ford_virtual_angle.py index d2e65b807d..f56155c7d7 100644 --- a/openpilot/selfdrive/controls/lib/ford_virtual_angle.py +++ b/openpilot/selfdrive/controls/lib/ford_virtual_angle.py @@ -73,7 +73,8 @@ class HeadingFeedback: 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_backoff_active': False} + 'feedback_reference_curvature': None, 'feedback_yaw_error': None, + 'feedback_backoff_active': False, 'feedback_recovery_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)) @@ -99,6 +100,7 @@ class HeadingFeedback: self.history.popleft() status = 'no_new_measurement' + recovery_active = False reference_time = reference_curvature = yaw_error = None if measurement_time != self.last_measurement_time: self.backoff_active = False @@ -122,11 +124,20 @@ class HeadingFeedback: # 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: + 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): status = 'release' if releasing else 'pscm_limit' else: + bias_before = self.bias increment = self.tuning.feedback_gain * yaw_error * measurement_dt - if backoff: + if recovering: + # Once both references show a shortfall, unwind a previous opposing + # correction during release. Use the current, smaller deficit and + # 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 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 @@ -149,6 +160,9 @@ class HeadingFeedback: if backoff: self.backoff_active = True status = 'release_backoff' if releasing else 'pscm_backoff' + elif recovering and self.bias != bias_before: + recovery_active = True + status = 'release_recovery' self.bias = float(np.clip(self.bias, -.5 - base, .5 - base)) target = float(np.clip(base + self.bias, -.5, .5)) if self.backoff_active: @@ -160,7 +174,7 @@ 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_backoff_active': self.backoff_active, 'feedback_recovery_active': recovery_active} return target @@ -244,7 +258,7 @@ class FordVirtualAngleController: 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-v6', 'command': (0., 0., 0., 0.), + self.diagnostics = {'status': 'inactive', 'hypothesis': 'model-pose-c0-c1-feedback-v7', 'command': (0., 0., 0., 0.), **self.feedback.diagnostics} def update(self, model, desired_curvature, *, yaw_rate, speed, now, measurement_time, model_time, reference_time, @@ -313,7 +327,7 @@ 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-v6', + self.diagnostics = {'status': 'driver_override' if driver_override else 'active', 'hypothesis': 'model-pose-c0-c1-feedback-v7', '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, diff --git a/openpilot/selfdrive/controls/tests/fixtures/ford_heading_recovery_requests.json b/openpilot/selfdrive/controls/tests/fixtures/ford_heading_recovery_requests.json new file mode 100644 index 0000000000..f402155b62 --- /dev/null +++ b/openpilot/selfdrive/controls/tests/fixtures/ford_heading_recovery_requests.json @@ -0,0 +1,40 @@ +{ + "description": "Signal-only v6 turn-exit recovery regression; no location, device identity, or predicted new vehicle response.", + "recorded_controller_revision": "61dac4977bf9c36504398e8a4959dfed79cf6f05", + "baseline_revision": "61dac4977bf9c36504398e8a4959dfed79cf6f05", + "response_delay": 0.20000000298023224, + "samples": 9134, + "models": 1843, + "fixture_sha256": "41d5e3efcee03a9e02fcaf7bf456c050c6a671a5b7f7fc9706ddf4d27bad71b8", + "windows": [ + { + "name": "overturn_then_underturn", + "range_s": [ + 19.99615067150053, + 29.48070058550053 + ], + "samples": 521 + }, + { + "name": "well_tracked_curve_a", + "range_s": [ + 56.19474309950053, + 63.68808603150053 + ], + "samples": 729 + }, + { + "name": "well_tracked_curve_b", + "range_s": [ + 101.19780691350051, + 116.33885445450052 + ], + "samples": 810 + } + ], + "selection": "One previously identified overturn-then-underturn event and two previously reported well-tracked curves; selected before recovery implementation.", + "mask": "Whole t-0.5 through t+0.65 interval active, valid, fresh, unpressed, raw driver torque magnitude <=1 Nm; requested |curvature|*speed\u00b2 >=.5 m/s\u00b2.", + "timing": "Exact consumed model publication; causal CAN/PSCM at estimated control computation time. Subtract observed median computation-to-publication delay; unsampled tick timing remains approximate.", + "context": "At least 20 seconds prior context or the available start, extended before the latest observed reset. Overlapping episodes are merged.", + "coordinates": "Times are local elapsed seconds; models contain only relative position.x/y and orientation.z arrays." +} diff --git a/openpilot/selfdrive/controls/tests/fixtures/ford_heading_recovery_requests.npz b/openpilot/selfdrive/controls/tests/fixtures/ford_heading_recovery_requests.npz new file mode 100644 index 0000000000..30572ccfe0 Binary files /dev/null and b/openpilot/selfdrive/controls/tests/fixtures/ford_heading_recovery_requests.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 43af68e744..9bd5a5956c 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py +++ b/openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py @@ -62,9 +62,10 @@ 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-v6') + self.assertEqual(record['hypothesis'], 'model-pose-c0-c1-feedback-v7') self.assertEqual(record['command'], list(controller.diagnostics['command'])) self.assertIs(record['feedback_backoff_active'], False) + self.assertIs(record['feedback_recovery_active'], False) if active and valid: self.assertEqual(record['response_delay'], 0.2) self.assertEqual(record['desired_curvature'], 0.01) @@ -123,6 +124,27 @@ class TestFordControlsLogging(unittest.TestCase): # The output ceiling is observable separately from the stored integral. self.assertLess(record['heading_target'], record['heading_base'] + record['heading_bias']) + def test_periodic_diagnostics_log_recovery_only_on_accepted_fresh_updates(self): + for limit in (0, 2): + with self.subTest(pscm_limit=limit): + controller = FordVirtualAngleController() + for i in range(50): + now = 1. + i * .01 + controller.update(circle(.02), .02, yaw_rate=.3, speed=10., now=now, measurement_time=now, + model_time=now, reference_time=now, active=True, pscm_status=PscmStatus(now, 2, 0, 2, False)) + self.assertLess(controller.diagnostics['heading_bias'], 0.) + first_status = 'release_recovery' if limit == 0 else 'release' + for now, expected_status in ((1.5, first_status), (1.51, 'no_new_measurement')): + controller.update(circle(.02), .01, yaw_rate=.03, speed=10., now=now, measurement_time=1.5, + model_time=now, reference_time=now, active=True, pscm_status=PscmStatus(now, 2, limit, 2, False)) + controls = SimpleNamespace(ford_path_controller=controller, desired_curvature=.01, curvature=.003, + sm=SimpleNamespace(logMonoTime={'modelV2': int(now * 1e9), 'carState': 1_500_000_000})) + record = self.emit_controls_event('Ford C2-free path tracking', controls) + self.assertEqual(record['feedback_status'], expected_status) + self.assertIs(record['feedback_recovery_active'], limit == 0 and now == 1.5) + self.assertIs(record['feedback_backoff_active'], False) + self.assertEqual(record['heading_bias'], controller.diagnostics['heading_bias']) + 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()) diff --git a/openpilot/selfdrive/controls/tests/test_ford_heading_recovery.py b/openpilot/selfdrive/controls/tests/test_ford_heading_recovery.py new file mode 100644 index 0000000000..e901a2f9c3 --- /dev/null +++ b/openpilot/selfdrive/controls/tests/test_ford_heading_recovery.py @@ -0,0 +1,135 @@ +import unittest + +from openpilot.selfdrive.controls.lib.ford_virtual_angle import HeadingFeedback, PathTuning, PscmStatus + + +def update(feedback, sign, now, *, base=.2, desired=.02, yaw=.1, previous=.2, measurement=None, limit=0, **overrides): + inputs = {'yaw_rate': sign * yaw, 'speed': 10., '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, limit, 2, False)} + inputs.update(overrides) + return feedback.update(sign * base, sign * desired, **inputs) + + +def acquired_correction(sign, yaw=.4, desired=.03): + feedback = HeadingFeedback(.2, PathTuning()) + previous = .3 + for i in range(60): + target = update(feedback, sign, i * .01, base=.3, desired=desired, yaw=yaw, previous=previous) + previous = sign * target + return feedback, previous + + +class TestFordHeadingRecovery(unittest.TestCase): + def test_release_recovers_opposing_bias_using_current_error(self): + for sign in (-1, 1): + with self.subTest(sign=sign): + feedback, previous = acquired_correction(sign) + retained = feedback.bias * .2 / .3 + self.assertLess(sign * retained, -.01) + target = update(feedback, sign, .6, previous=previous) + # Current request needs 0.2 rad/s, delayed request 0.3 rad/s, measured + # yaw is 0.1 rad/s. Use the smaller current deficit, not the old turn. + self.assertAlmostEqual(sign * (feedback.bias - retained), .1 * .01) + self.assertLessEqual(sign * feedback.bias, 0.) + self.assertLessEqual(sign * target, .2) + self.assertEqual(feedback.diagnostics['feedback_status'], 'release_recovery') + self.assertTrue(feedback.diagnostics['feedback_recovery_active']) + + def test_recovery_stops_at_zero_bias_with_batched_measurement(self): + for sign in (-1, 1): + with self.subTest(sign=sign): + feedback, previous = acquired_correction(sign, yaw=.301) + self.assertLess(sign * feedback.bias, 0.) + target = update(feedback, sign, .65, previous=previous, yaw=0.) + 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. + 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.assertFalse(feedback.diagnostics['feedback_recovery_active']) + + def test_opposing_delayed_request_blocks_recovery_even_with_both_positive_errors(self): + for sign in (-1, 1): + feedback, previous = acquired_correction(sign, yaw=-.2, desired=-.03) + retained = feedback.bias * .2 / .3 + self.assertLess(sign * retained, 0.) + update(feedback, sign, .6, previous=previous, yaw=-.5) + self.assertGreater(sign * feedback.diagnostics['feedback_yaw_error'], 0.) + self.assertAlmostEqual(feedback.bias, retained) + self.assertEqual(feedback.diagnostics['feedback_status'], 'release') + self.assertFalse(feedback.diagnostics['feedback_recovery_active']) + + def test_new_base_cannot_fabricate_recovery_beyond_available_slew(self): + for sign in (-1, 1): + for partial in (False, True): + with self.subTest(sign=sign, partial=partial): + feedback, previous = acquired_correction(sign) + bias = feedback.bias + before = .4 + sign * bias + if partial: + previous = before - .0045 # Only .0005 rad of the .001 recovery is deliverable. + target = update(feedback, sign, .6, base=.4, previous=previous) + self.assertAlmostEqual(sign * (feedback.bias - bias), .0005 if partial else 0.) + self.assertLessEqual(sign * target, .4) + self.assertEqual(feedback.diagnostics['feedback_status'], 'release_recovery' if partial else 'host_limit') + self.assertEqual(feedback.diagnostics['feedback_recovery_active'], partial) + + def test_recovery_requires_both_undertracking_errors_and_no_eps_limit(self): + for sign in (-1, 1): + for overrides in ({'yaw': .25}, {'yaw': .2}, {'limit': 2}, {'desired': 0.}, {'desired': -.02}): + with self.subTest(sign=sign, overrides=overrides): + feedback, previous = acquired_correction(sign) + retained = feedback.bias * .2 / .3 + update(feedback, sign, .6, previous=previous, **overrides) + self.assertAlmostEqual(feedback.bias, retained) + self.assertNotEqual(feedback.diagnostics['feedback_status'], 'release_recovery') + + def test_release_does_not_recover_a_same_direction_bias(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') + + def test_fresh_recovery_clears_backoff_without_reusing_measurements(self): + for sign in (-1, 1): + with self.subTest(sign=sign): + feedback, previous = acquired_correction(sign) + target = update(feedback, sign, .6, previous=previous, yaw=.4) + self.assertTrue(feedback.backoff_active) + bias = feedback.bias + # The new current request alone cannot recover from an old observation. + repeated = update(feedback, sign, .61, measurement=.6, previous=sign * target, yaw=.4) + self.assertEqual(feedback.bias, bias) + self.assertTrue(feedback.backoff_active) + self.assertLessEqual(sign * repeated, sign * target) + update(feedback, sign, .62, previous=sign * repeated) + self.assertGreater(sign * feedback.bias, sign * bias) + self.assertFalse(feedback.backoff_active) + self.assertEqual(feedback.diagnostics['feedback_status'], 'release_recovery') + bias = feedback.bias + update(feedback, sign, .63, measurement=.62) + self.assertEqual(feedback.bias, bias) + self.assertEqual(feedback.diagnostics['feedback_status'], 'no_new_measurement') + self.assertFalse(feedback.diagnostics['feedback_recovery_active']) + + def test_driver_or_missing_status_clears_the_correction(self): + for sign in (-1, 1): + for overrides in ({'driver_override': True}, {'pscm_status': None}, + {'pscm_status': PscmStatus(.3, 2, 0, 2, False)}): + with self.subTest(sign=sign, overrides=overrides): + feedback, previous = acquired_correction(sign) + target = update(feedback, sign, .6, previous=previous, **overrides) + self.assertEqual(feedback.bias, 0.) + self.assertAlmostEqual(sign * target, .2) + self.assertNotEqual(feedback.diagnostics['feedback_status'], 'release_recovery') + + +if __name__ == '__main__': + unittest.main() diff --git a/openpilot/selfdrive/controls/tests/test_ford_heading_recovery_routes.py b/openpilot/selfdrive/controls/tests/test_ford_heading_recovery_routes.py new file mode 100644 index 0000000000..4ecc004f45 --- /dev/null +++ b/openpilot/selfdrive/controls/tests/test_ford_heading_recovery_routes.py @@ -0,0 +1,118 @@ +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 TestFordHeadingRecoveryRoutes(unittest.TestCase): + @classmethod + def setUpClass(cls): + cls.fixture = Path(__file__).parent / 'fixtures/ford_heading_recovery_requests.npz' + cls.metadata = json.loads(cls.fixture.with_suffix('.json').read_text()) + cls.data = dict(np.load(cls.fixture)) + 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_episode = None + for i, now in enumerate(d['t']): + if d['episode'][i] != previous_episode: + controller = FordVirtualAngleController(response_delay=cls.metadata['response_delay']) + previous_episode = d['episode'][i] + 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])) + prior_bias, prior_base = controller.feedback.bias, controller.feedback.previous_base + 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_bias = prior_bias if prior_base is not None and prior_base * base >= 0. else 0. + if prior_base and prior_base * base >= 0.: + retained_bias *= min(1., abs(base / prior_base)) + row['bias_before_update'] = retained_bias + 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.status = np.array([row['feedback_status'] for row in rows]) + cls.recovering = np.array([row.get('feedback_recovery_active', False) for row in rows]) + cls.backoff = np.array([row.get('feedback_backoff_active', False) for row in rows]) + for key in ('heading_base', 'heading_target', 'heading_bias', 'bias_before_update', 'feedback_reference_curvature', 'feedback_yaw_error'): + setattr(cls, key, np.array([row.get(key, np.nan) for row in rows], dtype=float)) + + def test_fixture_hash_and_original_controller_provenance(self): + self.assertEqual(hashlib.sha256(self.fixture.read_bytes()).hexdigest(), self.metadata['fixture_sha256']) + self.assertEqual(self.metadata['baseline_revision'], '61dac4977bf9c36504398e8a4959dfed79cf6f05') + self.assertEqual(len(self.metadata['windows']), 3) + self.assertGreater(int(self.data['evidence'].sum()), 1000) + + def test_recorded_turn_exit_releases_opposing_correction(self): + d = self.data + # Select the captured command problem from the old policy, not from the + # candidate result: release was freezing an opposing bias while both + # current and delayed requests still exceeded measured turning. + base, bias = d['baseline_heading_base'], d['baseline_heading_bias'] + current_error = d['desired_curvature'] * d['speed'] - d['yaw_rate'] + delayed = d['baseline_feedback_reference_curvature'] + mask = (d['window_masks'][:, 0] & (d['baseline_status'] == 'release') & (bias * base < 0.) & + (current_error * base > 0.) & (d['baseline_feedback_yaw_error'] * base > 0.) & + (d['desired_curvature'] * base > 0.) & (delayed * base > 0.) & (d['pscm_limit'] < 2)) + self.assertGreater(int(mask.sum()), 100) + along_turn = np.sign(d['desired_curvature'][mask]) + increase = (self.commands[mask, 1] - d['baseline_commands'][mask, 1]) * along_turn + self.assertGreater(float(np.median(increase)), .005) + self.assertGreater(int((self.recovering & mask).sum()), 25) + self.assertLess(float(np.median(abs(self.heading_bias[mask]))), float(np.median(abs(bias[mask]))) - .005) + + def test_recovery_only_cancels_bias_with_both_requests_undertracked(self): + d, mask = self.data, self.recovering + self.assertGreater(int(mask.sum()), 25) + self.assertTrue((self.status[mask] == 'release_recovery').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.assertFalse(self.backoff[mask].any()) + self.assertTrue((self.bias_before_update[mask] * self.heading_base[mask] < 0.).all()) + self.assertTrue((self.feedback_yaw_error[mask] * self.heading_base[mask] > 0.).all()) + current_error = d['speed'] * d['desired_curvature'] - d['yaw_rate'] + self.assertTrue((current_error[mask] * self.heading_base[mask] > 0.).all()) + self.assertTrue((d['desired_curvature'][mask] * self.heading_base[mask] > 0.).all()) + self.assertTrue((self.feedback_reference_curvature[mask] * self.heading_base[mask] > 0.).all()) + self.assertTrue((abs(self.heading_bias[mask]) < abs(self.bias_before_update[mask])).all()) + 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): + evidence = self.data['evidence'] + np.testing.assert_array_equal(self.commands[evidence, 0], self.data['baseline_commands'][evidence, 0]) + 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]) + + def test_well_tracked_curves_keep_command_scale(self): + for index in (1, 2): + with self.subTest(window=self.metadata['windows'][index]['name']): + mask = self.data['window_masks'][:, index] + old, new = self.data['baseline_commands'][mask, 1], self.commands[mask, 1] + # This bounds collateral command change; it cannot guarantee the same + # future vehicle response on a drive with the candidate installed. + self.assertGreaterEqual(float(np.median(abs(new))), .95 * float(np.median(abs(old)))) + self.assertLess(float(np.quantile(abs(new - old), .9)), .02) + + def test_all_fixture_commands_respect_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() diff --git a/openpilot/sunnypilot/sunnylink/settings_ui.json b/openpilot/sunnypilot/sunnylink/settings_ui.json index c0a3428b6a..aff8f36985 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui.json +++ b/openpilot/sunnypilot/sunnylink/settings_ui.json @@ -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 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.", + "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.", "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 599547896a..8a123655d5 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml +++ b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml @@ -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 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. + 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. enablement: - $ref: '#/macros/offroad' - key: FordPscmObserver