Receive fresh lateral inputs before withdrawing AOL

openpilot/selfdrive/controls/controlsd.py

@@ -28,7 +28,8 @@ from openpilot.starpilot.feature_runtime import enabled as feature_enabled
 from openpilot.starpilot.vehicle_preferences import VehicleStartupPreferences
 from openpilot.starpilot.lateral.pause import LateralPause, read_settings as read_lateral_pause
 from openpilot.starpilot.lateral.model_turn_assist import ModelTurnAssist, update_twitch_guard
-from openpilot.starpilot.aol.runtime import current_axis, current_native, ordinary_lateral_requested, ordinary_axis_acknowledged
+from openpilot.starpilot.aol.runtime import (current_axis, current_native, ordinary_lateral_requested, ordinary_axis_acknowledged,
+                                            refresh_expired_lateral_snapshot)
 from openpilot.starpilot.aol.vehicle import policy_for as axis_policy_for, ordinary_axis_request_allowed, allow_lateral_onset
 from openpilot.starpilot.lateral.lane_centering import (
   LaneCenteringController, LaneCenteringInput, LaneCenteringRequest, LaneCenteringResult,
@@ -158,6 +159,10 @@ class Controls:
       self.LaC.__dict__.pop(name, None)
 
   def state_control(self, *, lane_centering: LaneCenteringRequest | None = None):
+    lateral_inputs_refreshed = False
+    if (self.aol_replay and os.getenv('REPLAY') != '1' and
+        getattr(self, 'aol_previous_lateral_active', False)):
+      lateral_inputs_refreshed = refresh_expired_lateral_snapshot(self.sm, now_ns=time.monotonic_ns())
     CS = self.sm['carState']
     self.flm_native_witness = None
     if self.lateral_gain_owner is not None:
@@ -221,6 +226,12 @@ class Controls:
                           ordinary_axis_acknowledged(self.sm, self.CP, now_ns=now_ns,
                             on_validated_native=lambda native: setattr(self, 'flm_native_witness', native)))
 
+    if lateral_inputs_refreshed:
+      # A newly received physical veto wins over an older admitted axis. This
+      # LAT-only refresh cannot enable ordinary LONG while normal controls are off.
+      CC.latActive = bool(CC.latActive and CS.canValid and not CS.canTimeout)
+      CC.longActive = bool(CC.longActive and CC.enabled and CS.canValid and not CS.canTimeout)
+
     fault_latch = getattr(self, 'hyundai_lateral_fault', None)
     if fault_latch is not None:
       latched = fault_latch.update(requested=lateral_requested, temporary_fault=bool(CS.steerFaultTemporary),

openpilot/starpilot/aol/runtime.py

@@ -207,6 +207,22 @@ def current_axis(sm, *, now_ns: int):
   return state
 
 
+def refresh_expired_lateral_snapshot(sm, *, now_ns: int) -> bool:
+  """Receive queued AOL inputs once; an expired LAT-only proof never gains time."""
+  axis = sm['aolAxisState']
+  if (sm.seen['aolAxisState'] and sm.valid['aolAxisState'] and sm.alive['aolAxisState'] and
+      axis.qualified and axis.nativeAcknowledged and axis.sessionId and
+      axis.lateralActive and axis.desiredLateral and
+      not axis.longitudinalActive and not axis.desiredLongitudinal and
+      not sm['selfdriveState'].enabled and
+      0 < sm.logMonoTime['aolAxisState'] <= axis.observedMonoTime <= axis.validUntilMonoTime < now_ns):
+    # All normal authority, source, physical and native gates run on the newly
+    # received snapshot. An empty receive leaves the expired proof untouched.
+    sm.update(0)
+    return True
+  return False
+
+
 def monitoring_lateral_engaged(sm, *, now_ns: int) -> bool:
   axis = current_axis(sm, now_ns=now_ns)
   if axis is None or not axis.lateralActive or not axis.nativeAcknowledged:

openpilot/starpilot/aol/tests/test_late_lateral_inputs.py

@@ -0,0 +1,211 @@
+"""A fresh queued proof may replace an expired snapshot; no lease is renewed."""
+
+from unittest.mock import patch
+import pytest
+from opendbc.can import CANPacker
+from opendbc.car import Bus
+from opendbc.car.gm.interface import CarInterface
+from opendbc.car.gm.tests.test_volt_sdgm_control import sdgm_params
+from opendbc.car.gm.values import DBC
+from opendbc.safety.tests.libsafety import libsafety_py as native
+from openpilot.cereal import messaging
+from openpilot.common.params import Params
+from openpilot.common.prefix import OpenpilotPrefix
+from openpilot.selfdrive.controls.controlsd import Controls
+from openpilot.starpilot.aol.tests.test_gm_startup_main import parsed_packets
+from openpilot.starpilot.lateral.tests.test_lane_runtime import feed
+from openpilot.starpilot.aol.wire import SafetyState, encode_safety
+
+BASE = 10_000_000_000
+
+
+@pytest.mark.parametrize('c9', [False, True])
+@pytest.mark.parametrize(
+  'case',
+  [
+    'healthy',
+    'empty',
+    'withdrawn',
+    'temporary_eps',
+    'permanent_eps',
+    'native_denied',
+    'wrong_session',
+    'invalid_axis',
+    'invalid_native',
+    'invalid_state',
+    'future_axis',
+    'stale_axis',
+    'stock_sibling',
+    'live_snapshot',
+    'disabled_long_candidate',
+    'clock_crosses_deadline',
+    'old_long_axis',
+  ],
+)
+def test_queued_fresh_sdgm_axis_is_consumed_before_steering_gate(c9, case):
+  cp = sdgm_params(alpha=case != 'stock_sibling', brake_c9=c9)
+  cp.alternativeExperience |= 32
+  with OpenpilotPrefix():
+    params = Params()
+    params.put_bool('AlwaysOnLateral', True, block=True)
+    params.put('CarParams', cp.to_bytes(), block=True)
+    controls = Controls()
+    ci = CarInterface(cp)
+    packer = CANPacker(DBC[cp.carFingerprint][Bus.pt])
+    safety = native.libsafety
+    safety.init_tests()
+    safety.set_alternative_experience(32)
+    assert safety.set_safety_hooks(4, cp.safetyConfigs[0].safetyParam) == 0
+    for tick in range(101):
+      ns = BASE - 1_000_000_000 + tick * 10_000_000
+      packets = parsed_packets(cp, packer, ns, tick, 24.78, True)
+      cs = ci.update([(ns, packets)])
+      safety.set_timer(ns // 1000)
+      safety.set_aol_test_heartbeat(True)
+      for a, data, bus in packets:
+        safety.safety_fwd_hook(bus, a)
+        assert safety.safety_rx_hook(native.make_CANPacket(a, bus, data))
+      safety.safety_tick()
+      safety.aol_set_host_request(1)
+    assert cs.canValid and not cs.canTimeout and not safety.get_controls_allowed()
+    mask = safety.aol_get_permission_mask()
+    assert mask & 1 and not mask & 2
+
+    def batch(ns):
+      captured = []
+      with patch.object(controls.sm, 'update_msgs', side_effect=lambda stamp, rows: captured.extend(rows)):
+        feed(controls, ns, 5, active=False, enabled=False)
+      state = messaging.new_message('carState', valid=True, logMonoTime=ns)
+      state.carState = cs
+      axis = messaging.new_message('aolAxisState', valid=True, logMonoTime=ns)
+      a = axis.aolAxisState
+      a.qualified = a.nativeAcknowledged = a.desiredLateral = a.lateralActive = True
+      a.sessionId = 'late-sdgm'
+      a.observedMonoTime = ns
+      a.validUntilMonoTime = ns + 30_000_000
+      a.sourceCarStateMonoTime = ns
+      wire = messaging.new_message('aolSafetyWire', 0, valid=True, logMonoTime=ns)
+      wire.aolSafetyWire = encode_safety(
+        SafetyState(
+          1, True, ns, ns + 200_000_000, 4, cp.safetyConfigs[0].safetyParam, bool(mask & 1), bool(mask & 2), True, False, 'native-test-panda', 'late-sdgm'
+        )
+      )
+      return [r for r in captured if r.which() != 'carState'] + [state.as_reader(), axis.as_reader(), wire.as_reader()]
+
+    old = batch(BASE)
+    if case == 'old_long_axis':
+      changed = old[-2].as_builder()
+      changed.aolAxisState.longitudinalActive = True
+      changed.aolAxisState.desiredLongitudinal = True
+      old[-2] = changed.as_reader()
+    controls.sm.update_msgs(BASE / 1e9, old)
+    with patch('openpilot.selfdrive.controls.controlsd.time.monotonic_ns', return_value=BASE):
+      command, _ = controls.state_control()
+    assert command.latActive and not command.longActive
+    original_deadline = controls.sm['aolAxisState'].validUntilMonoTime
+    pending = batch(BASE + 25_000_000)
+    now = BASE + 31_000_000
+    values = {v.which(): v.as_builder() for v in pending}
+    if case == 'withdrawn':
+      values['aolAxisState'].aolAxisState.lateralActive = False
+    elif case == 'temporary_eps':
+      values['carState'].carState.steerFaultTemporary = True
+    elif case == 'permanent_eps':
+      values['carState'].carState.steerFaultPermanent = True
+    elif case == 'invalid_state':
+      values['carState'].carState.canValid = False
+      values['carState'].carState.canTimeout = True
+    elif case == 'wrong_session':
+      values['aolAxisState'].aolAxisState.sessionId = 'replaced-session'
+    elif case == 'invalid_axis':
+      values['aolAxisState'].valid = False
+    elif case == 'invalid_native':
+      values['aolSafetyWire'].valid = False
+    elif case == 'future_axis':
+      values['aolAxisState'].aolAxisState.observedMonoTime = now + 1
+    elif case == 'stale_axis':
+      values['aolAxisState'].aolAxisState.validUntilMonoTime = now - 1
+    elif case == 'native_denied':
+      values['aolSafetyWire'].aolSafetyWire = encode_safety(
+        SafetyState(
+          1, True, BASE + 25_000_000, BASE + 225_000_000, 4, cp.safetyConfigs[0].safetyParam, False, False, True, False, 'native-test-panda', 'late-sdgm'
+        )
+      )
+    elif case == 'live_snapshot':
+      now = BASE + 29_000_000
+    elif case == 'disabled_long_candidate':
+      values['aolAxisState'].aolAxisState.longitudinalActive = True
+      values['aolAxisState'].aolAxisState.desiredLongitudinal = True
+      values['aolSafetyWire'].aolSafetyWire = encode_safety(
+        SafetyState(
+          1, True, BASE + 25_000_000, BASE + 225_000_000, 4, cp.safetyConfigs[0].safetyParam, True, True, True, True, 'native-test-panda', 'late-sdgm'
+        )
+      )
+    pending = [] if case == 'empty' else [v.as_reader() for v in values.values()]
+
+    def receive(timeout):
+      nonlocal now
+      assert timeout == 0
+      controls.sm.update_msgs(now / 1e9, pending)
+      if case == 'clock_crosses_deadline':
+        now = BASE + 60_000_000
+        decision_clock.return_value = now
+
+    with (
+      patch.object(controls.sm, 'update', side_effect=receive) as receive_latest,
+      patch('openpilot.selfdrive.controls.controlsd.time.monotonic_ns', return_value=now) as decision_clock,
+    ):
+      command, _ = controls.state_control()
+    expected = case in ('healthy', 'live_snapshot', 'disabled_long_candidate', 'stock_sibling')
+    assert command.latActive == expected and not command.longActive, (case, command.to_dict())
+    if case in ('live_snapshot', 'old_long_axis'):
+      receive_latest.assert_not_called()
+    else:
+      receive_latest.assert_called_once_with(0)
+    safety.set_timer(now // 1000)
+    safety.set_aol_test_heartbeat(True)
+    safety.aol_set_host_request(int(command.latActive))
+    _, frames = ci.apply(command.as_reader(), now)
+    assert frames
+    for a, data, bus in frames:
+      assert safety.safety_tx_hook(native.make_CANPacket(a, bus, data)), (case, hex(a), bus)
+    assert old[-2].aolAxisState.validUntilMonoTime == original_deadline == BASE + 30_000_000
+
+
+@pytest.mark.parametrize('denied', [False, True])
+def test_ioniq_queued_axis_uses_same_admitted_transport_contract(denied):
+  # Actual Card/SelfdriveD/Controls callers; this fixture supplies native observations,
+  # model readiness and receive transport, rather than exercising an HKG CAN/Panda loop.
+  from contextlib import ExitStack
+  from openpilot.starpilot.aol.tests import test_ioniq_availability as fixture
+
+  case = fixture.IoniqAvailabilityTests(methodName='runTest')
+  with OpenpilotPrefix(), ExitStack() as cleanup:
+    cleanup.callback(case.doCleanups)
+    sd, controls, step, card, _ = case._ioniq_availability_callers(full_op=False)
+    cleanup.callback(card.vehicle_startup.close)
+    old_axis, _, old_command = step(2, native_request_long=False)
+    assert old_axis.lateralActive and old_command.latActive and not old_command.longActive
+    old_deadline = int(old_axis.validUntilMonoTime)
+    pending = []
+    with (
+      patch.object(controls.sm, 'update_msgs', side_effect=lambda stamp, rows: pending.extend(rows)),
+      patch.object(controls, 'state_control', return_value=(old_command, None)),
+      patch.object(controls, 'publish'),
+    ):
+      fresh_axis, _, _ = step(5, offset_ns=-5_000_000, native_request_long=False, allowed=not denied)
+    assert fresh_axis.lateralActive == (not denied)
+    now = 10_051_000_000
+
+    def receive(timeout):
+      assert timeout == 0
+      controls.sm.update_msgs(now / 1e9, pending)
+
+    with (
+      patch.object(controls.sm, 'update', side_effect=receive) as receive_latest,
+      patch('openpilot.selfdrive.controls.controlsd.time.monotonic_ns', return_value=now),
+    ):
+      command, _ = controls.state_control()
+    assert command.latActive == (not denied) and not command.longActive
+    receive_latest.assert_called_once_with(0)
+    assert int(old_axis.validUntilMonoTime) == old_deadline == 10_050_000_000

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "9a2493649474b945d892fae1e9bba846cf1ff7420c07b88e543ac59b436581de", "schema_version": 1}
+{"protected_inventory_sha256": "d70926493e00d0d3341348cac261553859eafb7f19ac0131e397323162ca277a", "schema_version": 1}