From 3eb7f3fe29862044728a9cd326dd4cd6a7152c7e Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Wed, 4 Nov 2026 15:33:07 -0600 Subject: [PATCH] Preserve Ford lateral intent through initial steering readiness --- openpilot/selfdrive/car/card.py | 13 +- openpilot/starpilot/aol/transport_pause.py | 4 +- openpilot/starpilot/car/ford/aol.py | 43 ++++ .../starpilot/tests/ford_lifecycle_helpers.py | 22 ++- .../tests/test_ford_initial_request.py | 183 ++++++++++++++++++ prebuilt.json | 2 +- 6 files changed, 258 insertions(+), 9 deletions(-) create mode 100644 openpilot/starpilot/tests/test_ford_initial_request.py diff --git a/openpilot/selfdrive/car/card.py b/openpilot/selfdrive/car/card.py index 5e577b02de..0882409393 100644 --- a/openpilot/selfdrive/car/card.py +++ b/openpilot/selfdrive/car/card.py @@ -68,7 +68,8 @@ from openpilot.starpilot.car.nissan.aol import ( ) from openpilot.starpilot.car.hyundai.aol import aol_safety_config as hyundai_aol_safety_config, pending_stock_angle_arm from openpilot.starpilot.car.hyundai.aol import retained_shared_long_cancel_intent as hkg_cancel_intent_retained -from openpilot.starpilot.car.ford.aol import brake_pause_active as ford_brake_pause_active +from openpilot.starpilot.car.ford.aol import (brake_pause_active as ford_brake_pause_active, + awaiting_initial_request as ford_awaiting_initial_request) from openpilot.starpilot.aol.runtime import AXIS_MAX_AGE_NS, current_axis, current_native from openpilot.starpilot.aol.vehicle import create_intent as create_aol_intent, native_latch_rejected, policy_for as aol_policy_for from openpilot.starpilot.aol.wire import IntentState, encode_intent @@ -620,7 +621,12 @@ class Car: angle_arm_pending = pending_stock_angle_arm(self.CP, CS, self.sm, self.aol_card_intent, now_ns=now_ns, producer=self.slc_producer_session, acknowledged_arm_ns=getattr(self, '_angle_aol_acknowledged_arm_ns', 0)) - transport_reset = not (hkg_cancel_retained or rivian_brake_retained or angle_arm_pending) and self.aol_transport_feedback.observe( + ford_initial_wait = bool(not self.aol_transport_feedback.ever_lateral_active and + self.aol_card_intent.allowed_latch and qualified_ford_aol(self.CP, marked_only=True) and + ford_awaiting_initial_request(self.CP, CS, self.sm, + current_native(self.sm, self.CP, now_ns=now_ns, axis_session_id=self.aol_axis_session()), + self.aol_transport_feedback, self.aol_card_intent, now_ns=now_ns, producer=self.slc_producer_session)) + transport_reset = not (hkg_cancel_retained or rivian_brake_retained or angle_arm_pending or ford_initial_wait) and self.aol_transport_feedback.observe( self.sm, self.CP, CS, now_ns, latched=self.aol_card_intent.allowed_latch, cancel_source_ns=cancel_source_ns, brake_source_ns=brake_source_ns, @@ -647,7 +653,7 @@ class Car: not feedback.retention_started_ns and not feedback.retention_qualified and not feedback.retention_pending and not any(e.immediateDisable or e.softDisable or e.userDisable for e in self.sm['onroadEvents'])) fault_active = transport_reset or self.aol_disarming_fault(CS, event_ns, now_ns, - transport_observed_healthy=reuse_transport or angle_arm_pending) + transport_observed_healthy=reuse_transport or angle_arm_pending or ford_initial_wait) angle_aol, ford_aol, honda_aol, mazda_aol, preap_aol, volkswagen_aol = self.aol_permission_owners() nissan_aol = qualified_nissan_aol(self.CP, marked_only=True) rivian_aol = qualified_rivian_aol(self.CP, marked_only=True) @@ -680,6 +686,7 @@ class Car: latched=self.aol_card_intent.allowed_latch, panda_ready=angle_panda_ready, restricted=(getattr(self.aol_transport_feedback, 'ordinary_warning_qualified', False) or + ford_initial_wait and not self.sm['aolAxisState'].desiredLateral or restrict(self.CP, CS) or ((angle_aol or ford_aol or mazda_aol or preap_aol or rivian_aol) and self.aol_card_intent.pause_lateral) or (angle_aol and CS.brakePressed and (not CS.standstill) and (CS.vEgo < self.aol_settings.pause_brake_mps)) or diff --git a/openpilot/starpilot/aol/transport_pause.py b/openpilot/starpilot/aol/transport_pause.py index 3b3e93448d..0f471356d5 100644 --- a/openpilot/starpilot/aol/transport_pause.py +++ b/openpilot/starpilot/aol/transport_pause.py @@ -311,10 +311,8 @@ class TransportPauseFeedback: def _first_ford_gas_lateral_grant(self, sm, cp, cs, axis, now_ns, *, latched, producer): from opendbc.car import structs - from opendbc.car.ford.values import CAR from opendbc.car.ford.aol import qualified as ford_qualified - first_long = (ford_qualified(cp, marked_only=True) and cp.openpilotLongitudinalControl and - cp.carFingerprint == CAR.FORD_MUSTANG_MACH_E_MK1) + first_long = ford_qualified(cp, marked_only=True) and cp.openpilotLongitudinalControl pending_ack = str(axis.mode) == 'off' and not axis.lateralActive if (not first_long or not latched or self.failed or self.paused or self.pending_event or self.retention_started_ns or self.source_retention_started_ns or diff --git a/openpilot/starpilot/car/ford/aol.py b/openpilot/starpilot/car/ford/aol.py index 4910867350..bee7f8375f 100644 --- a/openpilot/starpilot/car/ford/aol.py +++ b/openpilot/starpilot/car/ford/aol.py @@ -27,6 +27,49 @@ def native_latch_rejected(cp, native): return qualified(cp, marked_only=True) and native.requestedLateral and not native.lateralAllowed +def awaiting_initial_request(cp, cs, sm, native, feedback, intent, *, now_ns, producer): + """Retain startup intent while request and first-grant receipts catch up.""" + from openpilot.starpilot.aol.runtime import AXIS_MAX_AGE_NS, current_axis + from openpilot.starpilot.aol.transport_pause import _healthy_transport_native, cp_identity + try: + axis = current_axis(sm, now_ns=now_ns) + if (not qualified(cp, marked_only=True) or not intent.allowed_latch or intent.pause_lateral or + feedback.ever_lateral_active or feedback.failed or feedback.paused or feedback.pending_event or + feedback.cp != cp_identity(cp) or + not cs.canValid or cs.canTimeout or cs.steerFaultTemporary or cs.steerFaultPermanent or + cs.accFaulted or cs.vehicleSensorsInvalid or cs.steeringDisengage or cs.doorOpen or cs.seatbeltUnlatched or + cs.brakePressed or cs.regenBraking or not cs.cruiseState.available or + cs.gearShifter != structs.CarState.GearShifter.drive or + axis is None or str(axis.faultReason) != 'none' or not axis.sessionId or + cs.buttonEvents or axis.sequence <= 0 or axis.sequence < feedback.sequence or + axis.observedMonoTime != int(sm.logMonoTime['aolAxisState']) or + axis.validUntilMonoTime != axis.observedMonoTime + AXIS_MAX_AGE_NS or + axis.faultSessionId != axis.sessionId or axis.sessionId != feedback.session or + not producer or axis.intentProducerSessionId != producer or + feedback.signature is None or feedback.signature[14:16] != (producer, int(axis.intentIntegrityEpoch)) or + axis.sourceCarStateMonoTime < feedback.signature[8] or axis.observedMonoTime < feedback.signature[2] or + not 0 < axis.sourceCarStateMonoTime <= axis.observedMonoTime <= now_ns or + now_ns - axis.sourceCarStateMonoTime > AXIS_MAX_AGE_NS or + axis.lateralActive or + native is None or native.axisSessionId != axis.sessionId or not native.pandaInventory or + bool(native.requestedLateral) != bool(native.lateralAllowed) or + native.lateralAllowed and (axis.desiredLateral or native.observedMonoTime <= axis.observedMonoTime) or + _healthy_transport_native(sm, cp, cs, str(axis.sessionId), now_ns, native, + require_longitudinal=not cs.gasPressed) is None or + feedback.healthy_inventory != (native.pandaSerial, tuple(p.hardwareSerial for p in native.pandaInventory))): + return False + state = sm['selfdriveState'] + state_ns = int(sm.logMonoTime['selfdriveState']) + event_ns = int(sm.logMonoTime['onroadEvents']) + return bool(sm.seen['selfdriveState'] and sm.valid['selfdriveState'] and sm.alive['selfdriveState'] and + state.enabled and state.active and 0 < state_ns <= now_ns and now_ns - state_ns <= AXIS_MAX_AGE_NS and + sm.seen['onroadEvents'] and sm.valid['onroadEvents'] and sm.alive['onroadEvents'] and + 0 < axis.faultEventMonoTime <= event_ns <= now_ns and now_ns - event_ns <= 1_500_000_000 and + not any(e.immediateDisable or e.softDisable or e.userDisable for e in sm['onroadEvents'])) + except (AttributeError, KeyError, TypeError, ValueError, OverflowError): + return False + + def ordinary_axis_request_allowed(cp, cs): return qualified(cp, marked_only=True) and cs.canValid and not cs.canTimeout and not cs.steerFaultPermanent diff --git a/openpilot/starpilot/tests/ford_lifecycle_helpers.py b/openpilot/starpilot/tests/ford_lifecycle_helpers.py index cd27b6944d..26ce2888ff 100644 --- a/openpilot/starpilot/tests/ford_lifecycle_helpers.py +++ b/openpilot/starpilot/tests/ford_lifecycle_helpers.py @@ -282,6 +282,9 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag return control_update(timestamp, snapshots) local.setattr(controls.sm, 'update_msgs', curved_model_update) + probe_setup = getattr(monkeypatch, 'ford_probe_setup', None) + if probe_setup is not None: + probe_setup(host, sd, controls, local) native.safety = ClockMappedSafety() location_unavailable = [False] @@ -372,7 +375,7 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag advance, main=0 if main_off else 5 if engaged else 3, gear=0 if park else 1 if reverse else 3, - mask=request, + mask=0 if getattr(monkeypatch, 'ford_drop_native_requests', False) and not warmup else request, heartbeat=heartbeat and fault != 'heartbeat', speed=(3.1 + .12 * ((steps // 2) % 12) / 11 if getattr(monkeypatch, "ford_speed_window", False) else getattr(joined, 'speed', 15.0)), eps=1 if fault in ('temporary-eps', 'temporary-eps-gas') else 0, @@ -449,7 +452,15 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag # Match pandad's 10Hz native publication, including a pedal-first USB read. # Values are actual native results; retained messages keep their stamps. if getattr(monkeypatch, 'ford_native_10hz', False): - if getattr(monkeypatch, 'ford_native_health_first', False): + if getattr(monkeypatch, 'ford_native_wire_first', False): + if not delayed_native: + delayed_native.update(wire=wire.as_reader(), panda=panda.as_reader(), permission=permission) + elif steps % 10 == 0: + pending_native_wire.update(panda=panda.as_reader()) + delayed_native.update(wire=wire.as_reader(), permission=permission) + elif steps % 10 == 1 and pending_native_wire: + delayed_native.update(panda=pending_native_wire.pop('panda')) + elif getattr(monkeypatch, 'ford_native_health_first', False): if not delayed_native: delayed_native.update(wire=wire.as_reader(), panda=panda.as_reader(), permission=permission) elif steps % 10 == 0: @@ -478,6 +489,9 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag events = [e for e in events if e.which() != service] events.append(route45_old[service]) route45_mixed[0] = True + probe_events = getattr(monkeypatch, 'ford_probe_events', None) + if probe_events is not None: + events = probe_events(events, now[0]) host.sm.update_msgs(now[0] / 1e9, events) state, _ = host.state_update() host.state_publish(state, None) @@ -676,6 +690,10 @@ def run_actual_producer(ford_joined, monkeypatch, *, scenario='reverse', reengag assert 0 <= warmup_waits < 120 and qualified_samples >= 100 assert host.ci_initialized # Read-only witness: real controls_update completed init. assert not host.aol_card_intent.allowed_latch + probe_run = getattr(monkeypatch, 'ford_probe_run', None) + if probe_run is not None: + probe_run(step, host, sd, joined) + return if scenario == 'startup-warning': if getattr(monkeypatch, 'ford_startup_armed', False): for _ in range(40): diff --git a/openpilot/starpilot/tests/test_ford_initial_request.py b/openpilot/starpilot/tests/test_ford_initial_request.py new file mode 100644 index 0000000000..7397363eda --- /dev/null +++ b/openpilot/starpilot/tests/test_ford_initial_request.py @@ -0,0 +1,183 @@ +import pytest +from opendbc.car.ford.values import CAR +from opendbc.car import structs +from openpilot.selfdrive.car import card as card_module +from openpilot.starpilot.tests.ford_lifecycle_helpers import ford_joined as ford_joined, run_actual_producer + + +@pytest.fixture(autouse=True) +def settings(monkeypatch, topology): + monkeypatch.ford_bluepilot = True + monkeypatch.ford_angle = True + monkeypatch.ford_starpilot = True + monkeypatch.ford_pinion = True + monkeypatch.ford_topology = topology + monkeypatch.ford_nonzero = True + monkeypatch.ford_native_10hz = True + + +@pytest.mark.parametrize('identity,alpha,hands_free,topology', [ + ((CAR.FORD_MUSTANG_MACH_E_MK1, 2), True, True, 'automatic'), + ((CAR.FORD_MUSTANG_MACH_E_MK1, 2), True, True, 'manual-multi'), + ((CAR.FORD_MUSTANG_MACH_E_MK1, 2), False, True, 'automatic'), + ((CAR.FORD_EXPLORER_MK6, 0), True, True, 'automatic'), + ((CAR.FORD_F_150_MK14, 2), True, True, 'automatic'), +]) +@pytest.mark.parametrize('scenario,delivery', [ + ('startup', 'paired'), ('startup', 'axis-delayed'), ('startup', 'state-delayed'), + ('brake', 'health-first'), ('brake', 'wire-first'), ('startup', 'native-timeout'), +]) +def test_initial_request_and_longitudinal_withdrawal(ford_joined, monkeypatch, scenario, delivery): + invalid = [False] + injected = [False] + calls = [] + delayed = {} + negative_checked = [False] + if delivery == 'native-timeout': + monkeypatch.ford_drop_native_requests = True + if delivery == 'health-first': + monkeypatch.ford_native_health_first = True + if delivery == 'wire-first': + monkeypatch.ford_native_wire_first = True + + def setup(host, sd, controls, local): + if scenario == 'startup': + sd.big_model_ready_t = card_module.time.monotonic() + update = sd.sm.update_msgs + + def calibration(timestamp, messages): + adjusted = [] + for message in messages: + if message.which() in ('extrinsicsCalibration', 'driverMonitoringState') and invalid[0]: + message = message.as_builder() + message.valid = False + message = message.as_reader() + adjusted.append(message) + if invalid[0] and not injected[0]: + present = {message.which() for message in adjusted} + for service in ('extrinsicsCalibration', 'driverMonitoringState'): + if service not in present: + message = card_module.messaging.new_message(service, valid=False) + setattr(message, service, sd.sm[service]) + adjusted.append(message.as_reader()) + injected[0] = True + return update(timestamp, adjusted) + + local.setattr(sd.sm, 'update_msgs', calibration) + observe = card_module.ford_native_observation + + def traced_native(native, **kwargs): + result = observe(native, **kwargs) + calls.append({'now': kwargs['now_ns'], 'latched': kwargs['latched'], 'pending': kwargs['pending_since_ns'], + 'restricted': kwargs['restricted'], 'result': result, + 'native': None if native is None else [native.requestedLateral, native.lateralAllowed, + native.requestedLongitudinal, native.longitudinalAllowed]}) + return result + + local.setattr(card_module, 'ford_native_observation', traced_native) + pending = getattr(card_module, 'ford_awaiting_initial_request', None) + if pending is not None: + def checked_pending(cp, cs, sm, native, feedback, intent, **kwargs): + accepted = pending(cp, cs, sm, native, feedback, intent, **kwargs) + if accepted and not negative_checked[0]: + from copy import copy + from dataclasses import replace + from types import SimpleNamespace + for fields in ({'canValid': False}, {'canTimeout': True}, {'brakePressed': True}, + {'steerFaultPermanent': True}, {'steerFaultTemporary': True}, {'doorOpen': True}): + state = structs.CarState(**cs.to_dict()) + for key, value in fields.items(): + setattr(state, key, value) + assert not pending(cp, state.as_reader(), sm, native, feedback, intent, **kwargs) + assert not pending(cp, cs, sm, None, feedback, intent, **kwargs) + assert not pending(cp, cs, sm, replace(native, requestedLateral=True), feedback, intent, **kwargs) + assert not pending(cp, cs, sm, replace(native, axisSessionId='other-session'), feedback, intent, **kwargs) + assert not pending(cp, cs, sm, replace(native, pandaInventory=tuple(replace(p, faults=1) for p in native.pandaInventory)), feedback, intent, **kwargs) + assert not pending(cp, cs, sm, native, feedback, SimpleNamespace(allowed_latch=False, pause_lateral=False), **kwargs) + assert not pending(cp, cs, sm, native, feedback, SimpleNamespace(allowed_latch=True, pause_lateral=True), **kwargs) + assert not pending(cp, cs, sm, native, feedback, intent, now_ns=kwargs['now_ns']+201_000_000, producer=kwargs['producer']) + assert not pending(cp, cs, sm, native, feedback, intent, now_ns=kwargs['now_ns'], producer='other-producer') + prior = copy(feedback) + prior.ever_lateral_active = True + assert not pending(cp, cs, sm, native, prior, intent, **kwargs) + other = structs.CarParams(**cp.to_dict()) + other.brand = 'toyota' + assert not pending(other, cs, sm, native, feedback, intent, **kwargs) + class Snapshot: + def __init__(self, service, value): + self.service, self.value = service, value + + def __getitem__(self, service): + return self.value if service == self.service else sm[service] + + def __getattr__(self, name): + return getattr(sm, name) + + from openpilot.cereal import log + event = log.OnroadEvent.new_message(name='controlsMismatch', immediateDisable=True) + assert not pending(cp, cs, Snapshot('onroadEvents', [event.as_reader()]), native, feedback, intent, **kwargs) + for key, value in (('sequence', 0), ('validUntilMonoTime', sm['aolAxisState'].validUntilMonoTime + 1), + ('sourceCarStateMonoTime', feedback.signature[8] - 1)): + axis = sm['aolAxisState'].as_builder() + setattr(axis, key, value) + assert not pending(cp, cs, Snapshot('aolAxisState', axis.as_reader()), native, feedback, intent, **kwargs) + assert pending(cp, cs, sm, native, feedback, intent, **kwargs) + negative_checked[0] = True + return accepted + local.setattr(card_module, 'ford_awaiting_initial_request', checked_pending) + + def events(messages, now): + if scenario != 'startup' or delivery in ('paired', 'native-timeout'): + return messages + name = 'aolAxisState' if delivery == 'axis-delayed' else 'selfdriveState' + result = [] + for message in messages: + if message.which() == name: + previous = delayed.get(name) + delayed[name] = message + if previous is not None: + result.append(previous) + else: + result.append(message) + return result + + def run(step, host, sd, joined): + if scenario == 'startup': + invalid[0] = True + for _ in range(45): + state, axis, sd_state, command = step(engaged=True, fault='gas', steering=True) + assert not command.latActive and not command.longActive + invalid[0] = False + ever_lateral = False + for _ in range(100): + state, axis, sd_state, command = step(engaged=True, fault='gas', steering=True) + ever_lateral |= command.latActive + if delivery == 'native-timeout': + assert not host.aol_card_intent.allowed_latch + assert host.aol_card_intent.latch_clear_reason == 'native_rejection' + expiries = [call for call in calls if call['result'][2]] + assert expiries and 200_000_000 <= expiries[0]['now'] - expiries[0]['pending'] < 220_000_000 + assert not ever_lateral + else: + assert host.aol_card_intent.allowed_latch, ('Readiness wait erased intent', host.aol_card_intent.latch_clear_reason) + assert command.latActive + for _ in range(8): + state, axis, sd_state, command = step(main_off=True) + assert not host.aol_card_intent.allowed_latch + assert not command.latActive and not command.longActive + assert negative_checked[0] + else: + for _ in range(50): + step(engaged=True) + assert host.aol_card_intent.allowed_latch + for _ in range(35): + state, axis, sd_state, command = step(brake=True) + assert host.aol_card_intent.allowed_latch, ('Brake cleared intent', host.aol_card_intent.latch_clear_reason) + assert command.latActive + if state.brakePressed or not joined.last_permission & 2: + assert not command.longActive + + monkeypatch.ford_probe_setup = setup + monkeypatch.ford_probe_events = events + monkeypatch.ford_probe_run = run + run_actual_producer(ford_joined, monkeypatch, scenario='route22') diff --git a/prebuilt.json b/prebuilt.json index 29484611f3..6f4e69b9b4 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "25faf6e535f235f83abacb8881653c7c61d44228c92ce9006582051448209749", "schema_version": 1} +{"protected_inventory_sha256": "6b933e1125fdbb850b34511196de860c851ac3c98daa06d11599df138136b820", "schema_version": 1}