Preserve Ford lateral intent through initial steering readiness

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

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

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
 

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):

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')

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "25faf6e535f235f83abacb8881653c7c61d44228c92ce9006582051448209749", "schema_version": 1}
+{"protected_inventory_sha256": "6b933e1125fdbb850b34511196de860c851ac3c98daa06d11599df138136b820", "schema_version": 1}