From 36fe2cf8547c2e9bd2b7bac37b60bf32f9dabaf1 Mon Sep 17 00:00:00 2001 From: Isaac Barham Date: Mon, 21 Sep 2026 14:23:50 -0400 Subject: [PATCH] Ford: add opt-in signaled-turn action preview --- docs/ford_signaled_turn_preview.md | 77 ++++++ openpilot/cereal/custom.capnp | 9 + openpilot/common/params_keys.h | 1 + openpilot/selfdrive/car/card.py | 3 +- openpilot/selfdrive/car/ford_joint_control.py | 26 +- openpilot/selfdrive/car/helpers.py | 2 + .../car/tests/test_ford_turn_preview.py | 238 ++++++++++++++++++ openpilot/selfdrive/controls/controlsd.py | 4 + .../controls/lib/ford_turn_preview.py | 51 ++++ .../tests/test_ford_model_action_selection.py | 2 +- .../selfdrive/controls/controlsd_ext.py | 7 + .../sunnypilot/sunnylink/settings_ui.json | 28 +++ .../settings_ui_src/pages/vehicle.yaml | 17 ++ .../sunnylink/tests/test_settings_schema.py | 11 + 14 files changed, 469 insertions(+), 7 deletions(-) create mode 100644 docs/ford_signaled_turn_preview.md create mode 100644 openpilot/selfdrive/car/tests/test_ford_turn_preview.py create mode 100644 openpilot/selfdrive/controls/lib/ford_turn_preview.py diff --git a/docs/ford_signaled_turn_preview.md b/docs/ford_signaled_turn_preview.md new file mode 100644 index 0000000000..1850553bc7 --- /dev/null +++ b/docs/ford_signaled_turn_preview.md @@ -0,0 +1,77 @@ +# Signaled-turn preview trial + +The coordinated Ford controller can start a large intersection turn late even +when the model geometry already shows the turn. This opt-in trial uses geometry +as a timing cue for the existing action request. It does not replace the action +with path heading or lateral position. + +## Selection + +In sunnylink Ford settings, enable **Signaled-Turn Preview (Experimental)** with +**Selected-Action Path Tracking** and **Coordinated C0/C1 Steering** on, and +**Model Geometry Reference** off. Keep **Large-Turn Entry Assist** on to match the +offline candidate. Apply settings while offroad, then start a new onroad session. +The new parameter, `FordPscmTurnPreview`, defaults off. The existing turn-entry +assist remains a separate option. Turning the master selected-action option off +still selects upstream Ford control. + +## Command change + +`controlsd` samples heading change at 7 and 14 metres of cumulative model-path +distance, once per model message. The cue fades in above 5 degrees at 7 metres +and 20 degrees at 14 metres; both headings must have the same sign. Full weight +requires at least 10 and 30 degrees respectively. This uses path distance even +when forward X folds back during a turn. + +The cue and original model timestamp travel in `carControlSP.fordTurnPreview`. +`card` adds at most 0.20 seconds of the existing filtered action-angle rate, +capped at 30 wheel degrees, to the trimmed inverse target. It requires exactly +one matching driver turn signal and both raw model action and filtered angle +request growing toward the turn. The original action, rate estimate, and trim +error remain unchanged. The existing inverse acceleration allowance, encoder, +CAN bounds, transmit observer, and optional post-encoder assist still apply. +C2 and C3 remain zero. + +Missing, invalid, future or older-than-150-ms geometry removes this addition. +Lane changes, maneuver injection, driver steering input, or a fresh PSCM limit +of 2 or greater suppress it too. These cue failures do not invalidate the base +controller. New model array work stays out of the CAN loop. There are no new +subscriptions, logging frequencies, shared process changes, or correction +integrators. + +## Offline evidence and limits + +The frozen candidate's latest-route left entry first changed commands about +0.60 seconds earlier, but the reconstructed reference reached 90 degrees only +21 ms earlier. Another good left entry reached that reference 239 ms earlier. +The latest-route right entry retained identical packets. Those are estimates +from the older-firmware reconstruction, not measured wheel improvements. + +Nine of ten bookmarked wobble windows retained identical packets. The remaining +window differed by at most 0.02 m C0 and 0.001 rad C1. Two camera-verified ordinary +lane-following sections retained identical packets with the matching-signal +gate. This does not establish unchanged behavior on all ordinary roads. + +Retained command history can affect output after the signal/cue clears. In one +good unwind the reconstructed 30-degree crossing was 0.103 seconds later, with +the 5-degree crossing unchanged. Offline replay holds the measured wheel fixed; +it cannot establish stability, physical smoothness, or successful turn completion. + +The installation checks exercise actual model messages, additive Cap'n Proto +transport, both process startup selectors, card's reader contract, and the real +Ford CAN packer. They compare full command histories with the retained candidate +when enabled and the prior controller when disabled. Route gaps reset only the +offline predictor; production timing-gap behavior is unchanged. + +All 11 retained routes (18f, 190, 183, 185, 17c, 166, 16a, 172, 177, 175, 17a) +matched exactly in both configurations: 1,208,782 controller updates and 116,886 +model messages. The installed helper's heading samples and action-trend signs +also matched the original offline extractor. The relevant controller, +integration, startup, and sunnylink suites passed 410 tests; lint and generated +settings consistency checks passed. + +On the development Mac, the new cue helper's median/p99 update cost was +20.5/37.5 microseconds per new model message; cached calls were 0.125/0.25 +microseconds. A 10,000-message Python allocation check retained 32 additional +bytes after warmup. These are local checks, not device realtime or whole-process +memory validation. diff --git a/openpilot/cereal/custom.capnp b/openpilot/cereal/custom.capnp index e3b3e4bcb3..4af0c9c0c3 100644 --- a/openpilot/cereal/custom.capnp +++ b/openpilot/cereal/custom.capnp @@ -384,6 +384,7 @@ struct CarControlSP @0xa5cd762cd951a455 { leadTwo @3 :LeadData; intelligentCruiseButtonManagement @4 :IntelligentCruiseButtonManagement; fordLateralPath @5 :FordLateralPath; + fordTurnPreview @6 :FordTurnPreview; struct Param { key @0 :Text; @@ -404,6 +405,14 @@ struct CarControlSP @0xa5cd762cd951a455 { } } +struct FordTurnPreview { + valid @0 :Bool; + modelMonoTime @1 :UInt64; + heading7 @2 :Float64; # Heading change at 7 m of path distance, native pinion degrees. + heading14 @3 :Float64; # Heading change at 14 m of path distance, native pinion degrees. + actionRate @4 :Float64; # Negative raw model curvature derivative; only its sign is used. +} + struct FordLateralPath { pathOffset @0 :Float32; # c0 [m] pathAngle @1 :Float32; # c1 [rad] diff --git a/openpilot/common/params_keys.h b/openpilot/common/params_keys.h index b04a866565..3ca8872361 100644 --- a/openpilot/common/params_keys.h +++ b/openpilot/common/params_keys.h @@ -243,6 +243,7 @@ inline static std::unordered_map keys = { {"FordGeometryReference", {PERSISTENT | BACKUP, BOOL, "0"}}, {"FordPscmJointControl", {PERSISTENT | BACKUP, BOOL, "0"}}, {"FordPscmTurnEntryAssist", {PERSISTENT | BACKUP, BOOL, "0"}}, + {"FordPscmTurnPreview", {PERSISTENT | BACKUP, BOOL, "0"}}, {"HyundaiLongitudinalTuning", {PERSISTENT | BACKUP, INT, "0"}}, {"SubaruStopAndGo", {PERSISTENT | BACKUP, BOOL, "0"}}, {"SubaruStopAndGoManualParkingBrake", {PERSISTENT | BACKUP, BOOL, "0"}}, diff --git a/openpilot/selfdrive/car/card.py b/openpilot/selfdrive/car/card.py index 62453b22f4..7bf8e33ef6 100755 --- a/openpilot/selfdrive/car/card.py +++ b/openpilot/selfdrive/car/card.py @@ -291,7 +291,8 @@ class Car: control_sp = convert_carControlSP(CC_SP) if self.ford_joint_control is not None: CC = self.ford_joint_control.prepare(CC, control_sp, CS, now_nanos * 1e-9, - fresh=self.sm.all_checks(['carControl', 'carControlSP']), pscm_status=pscm_status) + fresh=self.sm.all_checks(['carControl', 'carControlSP']), pscm_status=pscm_status, + turn_preview=CC_SP.fordTurnPreview) self.last_actuators_output, can_sends = self.CI.apply(CC, control_sp, now_nanos) self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) if self.ford_joint_control is not None: diff --git a/openpilot/selfdrive/car/ford_joint_control.py b/openpilot/selfdrive/car/ford_joint_control.py index c8a48b3722..e648ac263e 100644 --- a/openpilot/selfdrive/car/ford_joint_control.py +++ b/openpilot/selfdrive/car/ford_joint_control.py @@ -41,11 +41,14 @@ def joint_control_enabled(CP, params): def select_joint_control(CP, params): # No calibration or native-library load on the normal/default path. - return FordJointControl(CP, turn_entry_assist=params.get_bool('FordPscmTurnEntryAssist')) if joint_control_enabled(CP, params) else None + if not joint_control_enabled(CP, params): + return None + return FordJointControl(CP, turn_entry_assist=params.get_bool('FordPscmTurnEntryAssist'), + turn_preview=params.get_bool('FordPscmTurnPreview')) class FordJointControl: - def __init__(self, CP, *, turn_entry_assist=False): + def __init__(self, CP, *, turn_entry_assist=False, turn_preview=False): from opendbc.can import CANParser from opendbc.car.ford.fordcan import CanBus from openpilot.selfdrive.controls.lib.ford_joint.model import MainRequest @@ -53,6 +56,7 @@ class FordJointControl: from openpilot.selfdrive.controls.lib.ford_joint.encoder import PairedRelease self.turn_entry_assist = turn_entry_assist + self.turn_preview = turn_preview self.wheelbase, self.ratio = CP.wheelbase, CP.steerRatio if not all(math.isfinite(v) and v > 0 for v in (self.wheelbase, self.ratio)): raise ValueError('Joint control requires finite positive vehicle geometry') @@ -91,7 +95,7 @@ class FordJointControl: self.angle.step(speed, r['filtered_curvature'], angle, yaw, yaw * speed / 3.6, self.wheelbase, self.ratio) self.last_time = now - def prepare(self, CC, CC_SP, CS, now, *, fresh=True, pscm_status=None): + def prepare(self, CC, CC_SP, CS, now, *, fresh=True, pscm_status=None, turn_preview=None): from openpilot.selfdrive.controls.lib.ford_joint.inverse import C0_BOUND, C1_BOUND, invert_angle, quantize dt = now - self.last_time if self.last_time is not None else 0.0 @@ -153,7 +157,15 @@ class FordJointControl: if not CS.steeringPressed and release and error * self.angle_trim < 0.0: self.angle_trim = 0.0 trimmed_target = target + self.angle_trim - inverse = invert_angle(self.angle, speed, trimmed_target, angle, yaw, yaw * speed / 3.6, self.wheelbase, self.ratio) + entry_lead, cue = 0.0, 0.0 + if self.turn_preview and not CS.steeringPressed and not (status_fresh and pscm_status.limit >= 2): + from openpilot.selfdrive.controls.lib.ford_turn_preview import turn_preview_lead + entry_lead, cue = turn_preview_lead(self.requested_rate, turn_preview, now) + signal = int(CS.leftBlinker) - int(CS.rightBlinker) + if signal * entry_lead <= 0.0: + entry_lead = 0.0 + inverse_target = trimmed_target + entry_lead + inverse = invert_angle(self.angle, speed, inverse_target, angle, yaw, yaw * speed / 3.6, self.wheelbase, self.ratio) # Firmware phase is estimated from elapsed time. Cover the 1/2 firmware # ticks before the next nominal 100 Hz transmit; all phases are tested. ticks = max(1, min(2, int((self.phase + 0.01 + 1e-12) / 0.008))) @@ -173,6 +185,9 @@ class FordJointControl: # added request. record_sent/advance still observe the actual packets; # their retained state can affect later commands after this term clears. details = { + 'geometry_lead': entry_lead, + 'geometry_cue': cue, + 'inverse_target': inverse_target, 'base_wire_command': tuple(map(float, base_command)), 'turn_entry_weight': entry_weight, 'turn_entry_c0': float(command[0] - base_command[0]), @@ -215,7 +230,8 @@ class FordJointControl: cc = CC.as_reader().as_builder() if hasattr(CC, 'as_reader') else CC.as_builder() cc.latActive = active self.diagnostics = { - 'hypothesis': 'ford-joint-turn-entry-v1' if self.turn_entry_assist else 'ford-joint-v24', + 'hypothesis': 'ford-joint-signaled-preview-v1' if self.turn_preview else ('ford-joint-turn-entry-v1' if self.turn_entry_assist else 'ford-joint-v24'), + 'turn_preview_enabled': self.turn_preview, 'turn_entry_enabled': self.turn_entry_assist, 'turn_entry_weight': 0.0, 'turn_entry_c0': 0.0, diff --git a/openpilot/selfdrive/car/helpers.py b/openpilot/selfdrive/car/helpers.py index 6970b65b67..f3b50e4ec3 100644 --- a/openpilot/selfdrive/car/helpers.py +++ b/openpilot/selfdrive/car/helpers.py @@ -54,6 +54,8 @@ def convert_carControlSP(struct: capnp.lib.capnp._DynamicStructReader) -> struct return {k: v for k, v in s.items() if not k.endswith('DEPRECATED')} struct_dict = struct.to_dict() + # Consumed by card's Ford adapter, not by opendbc or the CAN packer. + struct_dict.pop('fordTurnPreview', None) struct_dataclass = structs.CarControlSP(**remove_deprecated({k: v for k, v in struct_dict.items() if not isinstance(k, dict)})) struct_dataclass.mads = structs.ModularAssistiveDrivingSystem(**remove_deprecated(struct_dict.get('mads', {}))) diff --git a/openpilot/selfdrive/car/tests/test_ford_turn_preview.py b/openpilot/selfdrive/car/tests/test_ford_turn_preview.py new file mode 100644 index 0000000000..57c115e50b --- /dev/null +++ b/openpilot/selfdrive/car/tests/test_ford_turn_preview.py @@ -0,0 +1,238 @@ +"""Signaled preview through real model messages, SP transport and Ford packing.""" +import ast +import itertools +from pathlib import Path +from types import SimpleNamespace + +import numpy as np +import pytest + +from openpilot.cereal import custom, log +from openpilot.common.params import Params, ParamKeyFlag +from openpilot.selfdrive.car.ford_joint_control import FordJointControl, select_joint_control +from openpilot.selfdrive.car.helpers import convert_carControlSP +from openpilot.selfdrive.car.tests.test_ford_joint_control import Pipeline, cp +from openpilot.selfdrive.controls.lib.ford_joint.encoder import state +from openpilot.selfdrive.controls.lib.ford_turn_preview import FordTurnPreview, turn_preview_lead +from openpilot.selfdrive.controls.tests.test_ford_model_action_adapter import _method +from openpilot.selfdrive.controls.tests.test_ford_model_action_selection import startup + + +def model(sign=1., action=-.01): + m = log.ModelDataV2.new_message() + station = np.linspace(0, 32, 33) + m.position.x = station.tolist() + m.position.y = np.zeros(33).tolist() + m.orientation.z = (-sign * np.radians(station * 2.2)).tolist() + m.action.desiredCurvature = action + return m + + +def sample(sign=1., stamp=1_010_000_000): + cue = FordTurnPreview() + cue.update(model(sign, 0.), stamp - 50_000_000, True) + data = cue.update(model(sign, -sign * .01), stamp, True) + # Serialize exactly the additive production carrier; use its reader at card. + sp = custom.CarControlSP.new_message(fordTurnPreview=data) + with custom.CarControlSP.from_bytes(sp.to_bytes()) as parsed: + convert_carControlSP(parsed) # The cue is card metadata, not an opendbc field. + return parsed.as_builder().as_reader().fordTurnPreview + + +def pipeline(enabled=True): + p = Pipeline() + p.joint = FordJointControl(cp(), turn_entry_assist=True, turn_preview=enabled) + return p + + +def test_default_off_startup_and_master_fallback(tmp_path): + params = Params(str(tmp_path)) + assert params.get_default_value('FordPscmTurnPreview') is False + assert b'FordPscmTurnPreview' in params.all_keys(ParamKeyFlag.PERSISTENT | ParamKeyFlag.BACKUP) + params.put_bool('FordPscmTurnPreview', True, block=True) + assert select_joint_control(cp(), params) is None + params.put_bool('FordModelActionController', True, block=True) + assert select_joint_control(cp(), params) is None + params.put_bool('FordPscmJointControl', True, block=True) + assert select_joint_control(cp(), params).turn_preview + for key in ('FordGeometryReference', 'JoystickDebugMode'): + params.put_bool(key, True, block=True) + assert select_joint_control(cp(), params) is None + params.put_bool(key, False, block=True) + params.put_bool('FordPscmTurnPreview', False, block=True) + assert not select_joint_control(cp(), params).turn_preview + + +@pytest.mark.parametrize('master,joint,geometry,joystick,preview', list(itertools.product((False, True), repeat=5))) +def test_both_processes_select_preview_with_the_same_gates(master, joint, geometry, joystick, preview): + flags = {'FordModelActionController': master, 'FordPscmJointControl': joint, 'FordGeometryReference': geometry, + 'JoystickDebugMode': joystick, 'FordPscmTurnPreview': preview} + params = SimpleNamespace(get_bool=lambda key: flags.get(key, False)) + selected = select_joint_control(cp(), params) + expected = master and joint and preview and not geometry and not joystick + assert bool(selected and selected.turn_preview) == expected + assert (startup(params=params).ford_turn_preview is not None) == expected + + +def publish_preview(tracker, message, stamp, *, healthy=True, maneuver=False): + root = Path(__file__).resolve().parents[3] + filename = root / 'sunnypilot/selfdrive/controls/controlsd_ext.py' + body = _method(filename, 'ControlsExt', 'state_control_ext').body + nodes = [n for n in body if (isinstance(n, ast.Assign) and ast.unparse(n.targets[0]) == 'turn_preview') or + (isinstance(n, ast.If) and ast.unparse(n.test) == 'turn_preview is not None')] + assert len(nodes) == 2 + + class Subscriptions(dict): + valid = {'modelV2': True, 'lateralManeuverPlan': maneuver} + logMonoTime = {'modelV2': stamp} + + def all_checks(self, services): + return healthy + + sp = custom.CarControlSP.new_message() + sp.fordLateralPath.enabled = sp.fordLateralPath.valid = True + exec(compile(ast.Module(body=nodes, type_ignores=[]), str(filename), 'exec'), + {'self': SimpleNamespace(ford_turn_preview=tracker), 'sm': Subscriptions(modelV2=message), 'CC_SP': sp}) + return sp + + +@pytest.mark.parametrize('healthy,maneuver', [(True, False), (False, False), (True, True)]) +def test_actual_publication_gates_and_original_model_timestamp(healthy, maneuver): + tracker = FordTurnPreview() + publish_preview(tracker, model(action=0.), 1_000_000_000) + sp = publish_preview(tracker, model(), 1_050_000_000, healthy=healthy, maneuver=maneuver) + assert sp.fordTurnPreview.valid == (healthy and not maneuver) + assert sp.fordTurnPreview.modelMonoTime == 1_050_000_000 + assert sp.fordLateralPath.valid + convert_carControlSP(sp.as_reader()) + + +def test_card_consumes_published_preview_and_preserves_reader_contract(): + from opendbc.car import structs + from opendbc.car.interfaces import CarInterfaceBase + from openpilot.selfdrive.car.tests.test_ford_joint_control import controls + + root = Path(__file__).resolve().parents[3] + filename = root / 'selfdrive/car/card.py' + method = _method(filename, 'Car', 'controls_update') + env = {'car': structs.car, 'custom': custom, 'REPLAY': True, 'convert_carControlSP': convert_carControlSP, + 'can_list_to_can_capnp': lambda *args, **kwargs: None} + exec(compile(ast.Module(body=[method], type_ignores=[]), str(filename), 'exec'), env) + p = pipeline() + p.cs.leftBlinker = True + card = SimpleNamespace(initialized_prev=True, ford_joint_control=p.joint, + sm=SimpleNamespace(all_alive=lambda services: True, all_checks=lambda services: True, frame=1), + pm=SimpleNamespace(send=lambda *args: None), + CI=SimpleNamespace(apply=lambda *args: CarInterfaceBase.apply(p.interface, *args))) + tracker = FordTurnPreview() + for stamp, target, raw in ((1_000_000_000, 0., 0.), (1_050_000_000, 100., -.01)): + sp = publish_preview(tracker, model(action=raw), stamp) + with custom.CarControlSP.from_bytes(sp.to_bytes()) as reader: + card.can_log_mono_time = stamp + control, _ = controls(target) + env['controls_update'](card, p.cs, control.as_reader(), reader) + assert p.joint.sent[2] + assert p.joint.diagnostics['geometry_lead'] == 30. + assert card.CC_prev.actuators.as_builder().steeringAngleDeg == 100. + + +@pytest.mark.parametrize('sign', [-1., 1.]) +def test_heading_distance_and_action_sign(sign): + p = sample(sign) + assert p.valid + assert p.heading7 == pytest.approx(sign * 15.4, abs=1e-5) + assert p.heading14 == pytest.approx(sign * 30.8, abs=1e-5) + assert turn_preview_lead(sign * 50., p, 1.01) == (sign * 10., 1.) + assert turn_preview_lead(sign * 1000., p, 1.01) == (sign * 30., 1.) + assert turn_preview_lead(-sign * 50., p, 1.01)[0] == 0. + + +def test_folded_path_uses_arc_distance_and_unwrapped_heading(): + m = model() + angle = np.linspace(0., 3.5, 33) + m.position.x = (6 * np.sin(angle)).tolist() + m.position.y = (6 * (1 - np.cos(angle))).tolist() + m.orientation.z = ((angle + np.pi) % (2 * np.pi) - np.pi).tolist() + p = FordTurnPreview().update(m, 1_000_000_000, True) + assert p['valid'] + assert p['heading14'] < p['heading7'] < -60. # x folds back, distance keeps increasing. + + +@pytest.mark.parametrize('bad', ['invalid', 'short', 'mismatch', 'nan', 'action_nan', 'lane_change']) +def test_invalid_geometry_disables_only_preview(bad): + m = model() + if bad == 'short': + m.position.x = np.linspace(0., 13., 33).tolist() + elif bad == 'mismatch': + m.orientation.z = [0.] + elif bad == 'nan': + m.position.y = [float('nan')] * 33 + elif bad == 'action_nan': + m.action.desiredCurvature = float('nan') + elif bad == 'lane_change': + m.meta.laneChangeState = 'laneChangeStarting' + p = FordTurnPreview().update(m, 1_000_000_000, bad != 'invalid') + assert not p['valid'] + + +def test_duplicate_stale_and_invalid_action_history(): + c = FordTurnPreview() + c.update(model(action=0.), 1_000_000_000, True) + good = c.update(model(), 1_050_000_000, True).copy() + assert good['actionRate'] > 0 + assert c.update(model(action=.1), 1_050_000_000, True) == good + p = sample(stamp=1_050_000_000) + for now in (1.049, 1.201): + assert turn_preview_lead(100., p, now) == (0., 0.) + c.update(model(), 1_100_000_000, False) + assert c.update(model(action=-.02), 1_150_000_000, True)['actionRate'] == 0. + assert c.update(model(action=-.03), 1_400_000_000, True)['actionRate'] == 0. + + +@pytest.mark.parametrize('signal', ['none', 'opposite', 'hazards']) +def test_no_matching_signal_retains_identical_packet_history(signal): + base, trial = pipeline(False), pipeline() + for i in range(500): + now = 1. + i * .01 + target = 70. + 50. * np.sin(i * .02) + for p in (base, trial): + p.cs.leftBlinker = signal == 'hazards' + p.cs.rightBlinker = signal != 'none' + p.tick(now, float(target), turn_preview=sample(stamp=round(now * 1e9))) + assert trial.joint.sent == base.joint.sent + np.testing.assert_array_equal(state(trial.joint.request), state(base.joint.request)) + assert trial.joint.angle_trim == base.joint.angle_trim + + +@pytest.mark.parametrize('sign', [-1., 1.]) +def test_matching_signal_changes_inverse_only_and_retains_bounds(sign): + p = pipeline() + p.cs.leftBlinker, p.cs.rightBlinker = sign > 0, sign < 0 + p.tick(1., 0.) + p.tick(1.01, sign * 100., turn_preview=sample(sign)) + d = p.joint.diagnostics + assert d['geometry_lead'] == sign * 30. + assert d['inverse_target'] == d['trimmed_angle'] + sign * 30. + assert d['requested_angle'] == sign * 100. + assert d['requested_rate'] * sign > 0. + # Raw action relaxing suppresses the extra even while filtered request grows. + p.tick(1.02, sign * 105., turn_preview=sample(-sign, 1_020_000_000)) + assert p.joint.diagnostics['geometry_lead'] == 0. + assert p.joint.sent[2] + + +@pytest.mark.parametrize('gate', ['touch', 'limit', 'denied', 'fault', 'stale', 'inactive', 'missing']) +def test_preview_respects_existing_health_and_override_gates(gate): + p = pipeline() + p.cs.leftBlinker = True + p.tick(1., 0.) + p.cs.steeringPressed = gate == 'touch' + p.cs.steerFaultTemporary = gate == 'fault' + status = SimpleNamespace(valid=True, canMonoTime=1_010_000_000, limit=2 if gate == 'limit' else 0, denied=gate == 'denied') + p.tick(1.01, 120., active=gate != 'inactive', fresh=gate != 'stale', pscm_status=status, + turn_preview=None if gate == 'missing' else sample()) + assert p.joint.diagnostics.get('geometry_lead', 0.) == 0. + if gate in ('touch', 'limit', 'missing'): + assert p.joint.sent[2] + else: + assert p.joint.sent == (0., 0., False) diff --git a/openpilot/selfdrive/controls/controlsd.py b/openpilot/selfdrive/controls/controlsd.py index 540d74138f..7b4ddd2512 100755 --- a/openpilot/selfdrive/controls/controlsd.py +++ b/openpilot/selfdrive/controls/controlsd.py @@ -65,6 +65,10 @@ class Controls(ControlsExt): cloudlog.event("Ford path controller selected", controller=type(self.ford_path_controller).__name__ if self.ford_model_action else "upstream") self.ford_path = FordPath() + self.ford_turn_preview = None + if self.ford_model_action and self.ford_path_controller.joint_control and self.params.get_bool('FordPscmTurnPreview'): + from openpilot.selfdrive.controls.lib.ford_turn_preview import FordTurnPreview + self.ford_turn_preview = FordTurnPreview() self.pose_calibrator = PoseCalibrator() self.calibrated_pose: Pose | None = None diff --git a/openpilot/selfdrive/controls/lib/ford_turn_preview.py b/openpilot/selfdrive/controls/lib/ford_turn_preview.py new file mode 100644 index 0000000000..767aa046f5 --- /dev/null +++ b/openpilot/selfdrive/controls/lib/ford_turn_preview.py @@ -0,0 +1,51 @@ +"""Model geometry gates a bounded action forecast; it is not a steering target.""" +import math + + +class FordTurnPreview: + def __init__(self): + self.previous_time = 0 + self.previous_action = 0.0 + self.previous_valid = False + self.sample = {'valid': False} + + def update(self, model, model_time, valid): + # controlsd runs faster than modeld. Preserve the derivative between model + # messages and do the array work only once per new model timestamp. + if model_time == self.previous_time: + return self.sample + import numpy as np + + action = float(model.action.desiredCurvature) + dt = (model_time - self.previous_time) * 1e-9 + action_valid = valid and math.isfinite(action) + rate = -(action - self.previous_action) / dt if action_valid and self.previous_valid and 0.0 < dt <= 0.15 else 0.0 + self.previous_time, self.previous_action, self.previous_valid = model_time, action, action_valid + self.sample = {'valid': False, 'modelMonoTime': model_time} + if not action_valid or model.meta.laneChangeState != 0: + return self.sample + x, y, heading = (np.asarray(v, dtype=float) for v in (model.position.x, model.position.y, model.orientation.z)) + if len(x) < 2 or len(x) != len(y) or len(x) != len(heading) or not all(np.all(np.isfinite(v)) for v in (x, y, heading)): + return self.sample + station = np.r_[0.0, np.cumsum(np.hypot(np.diff(x), np.diff(y)))] + if station[-1] < 14.0: + return self.sample + heading = np.unwrap(heading) + h7, h14 = (-float(np.degrees(np.interp(d, station, heading) - heading[0])) for d in (7.0, 14.0)) + self.sample.update(valid=True, heading7=h7, heading14=h14, actionRate=rate) + return self.sample + + +def turn_preview_lead(requested_rate, preview, now): + """Return wheel-angle lead and geometry weight in the native pinion sign.""" + if preview is None or not preview.valid or not 0.0 <= now - preview.modelMonoTime * 1e-9 <= 0.15: + return 0.0, 0.0 + h7, h14, raw_rate = preview.heading7, preview.heading14, preview.actionRate + if not all(math.isfinite(v) for v in (h7, h14, raw_rate, requested_rate)) or h7 * h14 <= 0.0: + return 0.0, 0.0 + weight = min(max(0.0, min(1.0, (abs(h7) - 5.0) / 5.0)), max(0.0, min(1.0, (abs(h14) - 20.0) / 10.0))) + direction = math.copysign(1.0, h14) + if direction * raw_rate <= 0.0 or direction * requested_rate <= 0.0: + return 0.0, weight + # Trial choices, not recovered Ford constants: 0.20 s lead, at most 30 deg. + return direction * weight * min(30.0, 0.20 * direction * requested_rate), weight diff --git a/openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py b/openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py index e4469281dc..4d56ed945a 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py +++ b/openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py @@ -30,7 +30,7 @@ def startup(cp=None, params=None): cls = next(n for n in tree.body if isinstance(n, ast.ClassDef) and n.name == 'Controls') body = next(n for n in cls.body if isinstance(n, ast.FunctionDef) and n.name == '__init__').body start = next(i for i, n in enumerate(body) if isinstance(n, ast.Assign) and ast.unparse(n.targets[0]) == 'self.ford_path_controller') - end = next(i for i, n in enumerate(body) if isinstance(n, ast.Assign) and ast.unparse(n.targets[0]) == 'self.ford_path') + end = next(i for i, n in enumerate(body) if isinstance(n, ast.Assign) and ast.unparse(n.targets[0]) == 'self.ford_turn_preview') + 1 if params is None: params = SimpleNamespace(get_bool=lambda key: key == 'FordModelActionController') controls = SimpleNamespace(CP=cp or car_params(), params=params) diff --git a/openpilot/sunnypilot/selfdrive/controls/controlsd_ext.py b/openpilot/sunnypilot/selfdrive/controls/controlsd_ext.py index 7919ef5e6a..63cac01456 100644 --- a/openpilot/sunnypilot/selfdrive/controls/controlsd_ext.py +++ b/openpilot/sunnypilot/selfdrive/controls/controlsd_ext.py @@ -121,6 +121,13 @@ class ControlsExt(ModelStateBase): CC_SP.fordLateralPath.curvature = ford_path.curvature CC_SP.fordLateralPath.curvatureRate = ford_path.curvature_rate + turn_preview = getattr(self, 'ford_turn_preview', None) + if turn_preview is not None: + CC_SP.fordTurnPreview = turn_preview.update(sm['modelV2'], sm.logMonoTime['modelV2'], sm.valid['modelV2']) + # Maneuver injection must retain its own target, independent of the road model. + if not sm.all_checks(['modelV2']) or sm.valid['lateralManeuverPlan']: + CC_SP.fordTurnPreview.valid = False + return CC_SP @staticmethod diff --git a/openpilot/sunnypilot/sunnylink/settings_ui.json b/openpilot/sunnypilot/sunnylink/settings_ui.json index 65e1d66b2d..0c1f325ffb 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui.json +++ b/openpilot/sunnypilot/sunnylink/settings_ui.json @@ -2259,6 +2259,34 @@ "equals": false } ] + }, + { + "key": "FordPscmTurnPreview", + "widget": "toggle", + "needs_onroad_cycle": true, + "title": "Signaled-Turn Preview (Experimental)", + "description": "Advance a growing steering request when the model path and your turn signal agree on a sharp turn ahead.", + "details": "Requires Coordinated C0/C1 Steering and Selected-Action Path Tracking on, with Model Geometry Reference off. Keep Large-Turn Entry Assist on to match the offline trial. Default off. Adds a bounded action forecast while one matching turn signal is on; hazards, lane changes, driver steering input, and reported steering limits suppress it. This does not steer directly from the model path. Offline results showed modest earlier entry and some retained command differences after turns; improved physical tracking is not established. Changes apply after an offroad-to-onroad cycle. Turning Selected-Action Path Tracking off restores upstream Ford control.", + "enablement": [ + { + "type": "offroad_only" + }, + { + "type": "param", + "key": "FordModelActionController", + "equals": true + }, + { + "type": "param", + "key": "FordPscmJointControl", + "equals": true + }, + { + "type": "param", + "key": "FordGeometryReference", + "equals": false + } + ] } ] }, diff --git a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml index 8110d60b36..959072dafd 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml +++ b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml @@ -60,6 +60,23 @@ sections: - type: param key: FordGeometryReference equals: false + - key: FordPscmTurnPreview + widget: toggle + needs_onroad_cycle: true + title: Signaled-Turn Preview (Experimental) + description: Advance a growing steering request when the model path and your turn signal agree on a sharp turn ahead. + details: Requires Coordinated C0/C1 Steering and Selected-Action Path Tracking on, with Model Geometry Reference off. Keep Large-Turn Entry Assist on to match the offline trial. Default off. Adds a bounded action forecast while one matching turn signal is on; hazards, lane changes, driver steering input, and reported steering limits suppress it. This does not steer directly from the model path. Offline results showed modest earlier entry and some retained command differences after turns; improved physical tracking is not established. Changes apply after an offroad-to-onroad cycle. Turning Selected-Action Path Tracking off restores upstream Ford control. + enablement: + - $ref: '#/macros/offroad' + - type: param + key: FordModelActionController + equals: true + - type: param + key: FordPscmJointControl + equals: true + - type: param + key: FordGeometryReference + equals: false - id: hyundai title: Hyundai / Kia / Genesis Settings description: '' diff --git a/openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py b/openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py index 41e77c2929..c8fcabbabf 100644 --- a/openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py +++ b/openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py @@ -332,6 +332,17 @@ class TestKnownVehicleSettings(OpenpilotTestCase): keys = {i["key"] for i in _brand_items(schema["vehicle_settings"].get("hyundai"))} assert "HyundaiLongitudinalTuning" in keys + def test_ford_turn_preview_is_separate_default_off_cycle_only_trial(self, schema): + items = _brand_items(schema["vehicle_settings"].get("ford")) + item = next(item for item in items if item["key"] == "FordPscmTurnPreview") + assert item["widget"] == "toggle" and item["needs_onroad_cycle"] is True + assert item["enablement"] == [{"type": "offroad_only"}, + {"type": "param", "key": "FordModelActionController", "equals": True}, + {"type": "param", "key": "FordPscmJointControl", "equals": True}, + {"type": "param", "key": "FordGeometryReference", "equals": False}] + with tempfile.TemporaryDirectory() as path: + assert Params(path).get_default_value("FordPscmTurnPreview") is False + def test_toyota_has_enforce_stock_and_stop_go(self, schema): keys = {i["key"] for i in _brand_items(schema["vehicle_settings"].get("toyota"))} assert "ToyotaEnforceStockLongitudinal" in keys