Preserve AOL intent through temporary steering overrides

openpilot/starpilot/aol/tests/test_driver_override_eps_intent.py

@@ -0,0 +1,156 @@
+from contextlib import ExitStack
+
+import pytest
+
+from openpilot.common.prefix import OpenpilotPrefix
+from openpilot.starpilot.aol.tests import test_ioniq_availability as fixture
+
+
+@pytest.mark.parametrize('full_op', [False, True])
+@pytest.mark.parametrize('steering', [False, True])
+def test_temporary_eps_during_driver_override_keeps_existing_latch(full_op, steering):
+  case = fixture.IoniqAvailabilityTests(methodName='runTest')
+  with ExitStack() as cleanup:
+    cleanup.callback(case.doCleanups)
+    cleanup.enter_context(OpenpilotPrefix())
+    sd, _, drive, card, receive = case._ioniq_availability_callers(full_op=full_op)
+    temporary = fixture.car_state()
+    temporary.steerFaultTemporary = True
+    temporary.steeringAngleDeg = 100.
+    for tick in range(2, 8):
+      axis, _, command = drive(tick, observed_state=temporary, steering=steering)
+      assert not command.latActive and not axis.lateralActive
+      assert receive(10_000_000_000 + tick * 10_000_000, axis=sd.pm.messages['aolAxisState'],
+                     event=sd.pm.messages['onroadEvents'], state=temporary), {
+                       'marker': 'TEMP_OVERRIDE_CLEARED_AOL', 'axis': axis.to_dict(),
+                       'reason': card.aol_card_intent.latch_clear_reason,
+                       'events': sd.pm.messages['onroadEvents'].to_dict(),
+                       'feedback': {k: getattr(card.aol_transport_feedback, k) for k in
+                         ('failed', 'pending_event', 'lateral_baseline_ns', 'lateral_inventory', 'signature', 'temporary_eps_event_ns')}}
+      assert card.aol_card_intent.allowed_latch
+
+
+@pytest.mark.parametrize('steering', [False, True])
+def test_g70_stock_temporary_eps_keeps_existing_latch(steering):
+  from opendbc.car.hyundai.values import CAR
+  from opendbc.car.hyundai.classic_scc_aol import qualified
+  from openpilot.starpilot.aol.tests.test_classic_scc_profiles import params
+
+  case = fixture.IoniqAvailabilityTests(methodName='runTest')
+  with ExitStack() as cleanup:
+    cleanup.callback(case.doCleanups)
+    cleanup.enter_context(OpenpilotPrefix())
+    from openpilot.common.params import Params
+    Params().put('LKASButtonControl', 9, block=True)
+    cp = params(CAR.GENESIS_G70, 0x391)
+    assert qualified(cp)
+    sd, _, drive, card, receive = case._ioniq_availability_callers(full_op=False, cp=cp)
+    temporary = fixture.car_state()
+    temporary.steerFaultTemporary = True
+    temporary.steeringAngleDeg = 100.
+    for tick in range(2, 8):
+      axis, _, command = drive(tick, observed_state=temporary, steering=steering)
+      assert not command.latActive and not axis.lateralActive
+      assert receive(10_000_000_000 + tick * 10_000_000, axis=sd.pm.messages['aolAxisState'],
+                     event=sd.pm.messages['onroadEvents'], state=temporary), {
+                       'marker': 'TEMP_OVERRIDE_CLEARED_AOL', 'axis': axis.to_dict(),
+                       'reason': card.aol_card_intent.latch_clear_reason,
+                       'events': sd.pm.messages['onroadEvents'].to_dict(),
+                       'feedback': {k: getattr(card.aol_transport_feedback, k) for k in
+                         ('failed', 'pending_event', 'lateral_baseline_ns', 'lateral_inventory', 'signature', 'temporary_eps_event_ns')}}
+      assert card.aol_card_intent.allowed_latch
+    recovered, _, command = drive(8)
+    assert recovered.lateralActive and command.latActive
+    assert not command.longActive and not sd.enabled
+    assert receive(10_080_000_000, axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents'])
+
+
+@pytest.mark.parametrize('configuration', ['aol', 'combined', 'g70'])
+@pytest.mark.parametrize('order', ['proof_first', 'event_first', 'together'])
+def test_driver_override_eps_entry_warning_and_recovery_delivery_pairs(configuration, order):
+  case = fixture.IoniqAvailabilityTests(methodName='runTest')
+  with ExitStack() as cleanup:
+    cleanup.callback(case.doCleanups)
+    cleanup.enter_context(OpenpilotPrefix())
+    if configuration == 'g70':
+      from openpilot.common.params import Params
+      from openpilot.starpilot.aol.tests.test_classic_scc_profiles import params
+      from opendbc.car.hyundai.values import CAR
+      Params().put('LKASButtonControl', 9, block=True)
+      cp = params(CAR.GENESIS_G70, 0x391)
+      sd, _, drive, card, receive = case._ioniq_availability_callers(full_op=False, cp=cp)
+    else:
+      sd, _, drive, card, receive = case._ioniq_availability_callers(full_op=configuration == 'combined')
+    temporary = fixture.car_state()
+    temporary.steerFaultTemporary = True
+    for tick, steering, fault in ((2, True, True), (3, False, True), (4, False, False)):
+      temporary.steerFaultTemporary = fault
+      axis, _, command = drive(tick, observed_state=temporary, steering=steering)
+      assert bool(axis.lateralActive and command.latActive) == (not fault)
+      proof, event = sd.pm.messages['aolAxisState'], sd.pm.messages['onroadEvents']
+      frames = [{'axis': proof}, {'event': event}] if order == 'proof_first' else (
+        [{'event': event}, {'axis': proof}] if order == 'event_first' else [{'axis': proof, 'event': event}])
+      for index, frame in enumerate(frames):
+        assert receive(10_000_000_000 + tick * 10_000_000 + index * 1_000_000, state=temporary,
+                       **frame), ('TEMP_OVERRIDE_PAIR_CLEARED_AOL', tick, order)
+      assert card.aol_card_intent.allowed_latch
+
+
+@pytest.mark.parametrize('fault', ['unexpected_long', 'invalid_native', 'permanent_eps', 'can_invalid', 'hard_event'])
+def test_g70_override_eps_cannot_hide_other_faults_or_long_authority(fault):
+  from openpilot.cereal import log
+  from openpilot.common.params import Params
+  from openpilot.starpilot.aol.tests.test_classic_scc_profiles import params
+  from opendbc.car.hyundai.values import CAR
+
+  case = fixture.IoniqAvailabilityTests(methodName='runTest')
+  with ExitStack() as cleanup:
+    cleanup.callback(case.doCleanups)
+    cleanup.enter_context(OpenpilotPrefix())
+    Params().put('LKASButtonControl', 9, block=True)
+    sd, _, drive, card, receive = case._ioniq_availability_callers(full_op=False, cp=params(CAR.GENESIS_G70, 0x391))
+    temporary = fixture.car_state()
+    temporary.steerFaultTemporary = True
+    temporary.steerFaultPermanent = fault == 'permanent_eps'
+    temporary.canValid = fault != 'can_invalid'
+    drive(2, observed_state=temporary, steering=True, native_valid=fault != 'invalid_native',
+          native_request_long=fault == 'unexpected_long',
+          critical_event=log.OnroadEvent.EventName.canError if fault == 'hard_event' else None)
+    assert not receive(10_020_000_000, state=temporary, axis=sd.pm.messages['aolAxisState'],
+                       event=sd.pm.messages['onroadEvents'])
+    assert not card.aol_card_intent.allowed_latch
+
+
+@pytest.mark.parametrize('full_op', [False, True])
+@pytest.mark.parametrize('order', ['proof_first', 'event_first'])
+def test_override_eps_missing_pair_cannot_renew_original_deadline(full_op, order):
+  from openpilot.cereal import messaging
+
+  case = fixture.IoniqAvailabilityTests(methodName='runTest')
+  with ExitStack() as cleanup:
+    cleanup.callback(case.doCleanups)
+    cleanup.enter_context(OpenpilotPrefix())
+    sd, _, drive, card, receive = case._ioniq_availability_callers(full_op=full_op)
+    feedback = card.aol_transport_feedback
+    original = (feedback.lateral_baseline_ns, feedback.lateral_source_ns)
+    temporary = fixture.car_state()
+    temporary.steerFaultTemporary = True
+    drive(2, observed_state=temporary, steering=True)
+    proof, event = sd.pm.messages['aolAxisState'], sd.pm.messages['onroadEvents']
+    half = {'axis': proof} if order == 'proof_first' else {'event': event}
+    assert receive(10_020_000_000, state=temporary, **half)
+    original_pending = feedback.pending_event
+
+    def fresh_half(stamp):
+      newer = messaging.new_message('aolAxisState', valid=True, logMonoTime=stamp)
+      newer.aolAxisState = card.sm['aolAxisState']
+      newer.aolAxisState.sequence += 1
+      newer.aolAxisState.observedMonoTime = newer.aolAxisState.sourceCarStateMonoTime = stamp
+      newer.aolAxisState.validUntilMonoTime = stamp + 30_000_000
+      return {**half, 'axis': newer}
+
+    assert receive(10_030_000_000, state=temporary, **fresh_half(10_030_000_000))
+    assert feedback.pending_event == original_pending
+    assert (feedback.lateral_baseline_ns, feedback.lateral_source_ns) == original
+    assert not receive(10_050_000_001, state=temporary, **fresh_half(10_050_000_001))
+    assert not card.aol_card_intent.allowed_latch

openpilot/starpilot/aol/tests/test_ioniq_availability.py

@@ -338,7 +338,7 @@ class IoniqAvailabilityTests(unittest.TestCase):
       return sd.pm.messages['aolAxisState'].aolAxisState, sd.pm.messages['selfdriveState'].selfdriveState, command
     return sd, controls, step
 
-  def _ioniq_availability_callers(self, full_op=True, *, initially_armed=True, stock_angle=False):
+  def _ioniq_availability_callers(self, full_op=True, *, initially_armed=True, stock_angle=False, cp=None):
     from opendbc.car.hyundai.interface import CarInterface
     from opendbc.car.hyundai.radar_interface import RadarInterface
     from openpilot.selfdrive.car.card import Car
@@ -354,18 +354,19 @@ class IoniqAvailabilityTests(unittest.TestCase):
 
     self.addCleanup(restore_simulation)
     os.environ['SIMULATION'] = '0'
-    if stock_angle:
+    supplied_profile = cp is not None
+    if stock_angle and not supplied_profile:
       from opendbc.car.hyundai.tests.test_stock_angle_aol import source_profile
       from opendbc.car.hyundai.values import CAR
       cp = source_profile(CAR.HYUNDAI_IONIQ_5_PE if stock_angle is True else CAR(stock_angle))
-    else:
+    elif not supplied_profile:
       _, cp = candidate(True)
       cp.safetyConfigs[-1].safetyParam |= 0x0800
     sd, controls, step = self._authority_callers(full_op, cp=cp, genuine_events=True)
     card_cp = cp.as_reader().as_builder()
     card = Car(CI=CarInterface(card_cp), RI=RadarInterface(card_cp))
     self.assertEqual(card.CP.to_dict(), sd.CP.to_dict())
-    self.assertEqual(card.CP.safetyConfigs[-1].safetyParam, cp.safetyConfigs[-1].safetyParam if stock_angle else 0x8895)
+    self.assertEqual(card.CP.safetyConfigs[-1].safetyParam, cp.safetyConfigs[-1].safetyParam if stock_angle or supplied_profile else 0x8895)
     self.assertTrue(card.aol_card_intent.explicit_latch)
     card.slc_producer_session = 'authority-card'
     cs = car_state()

openpilot/starpilot/aol/transport_pause.py

@@ -777,12 +777,18 @@ class TransportPauseFeedback:
     previous = self.signature
     pending = bool(self.pending_event)
     all_off = bool(withdrawn and not axis.desiredLongitudinal and not axis.longitudinalActive and str(axis.mode) == 'off')
-    if pending and (not all_off or previous is None or previous[5] != 'none' or
+    event_ns = int(sm.logMonoTime['onroadEvents'])
+    same_longitudinal = bool(previous is not None and
+      (axis.longitudinalActive, axis.desiredLongitudinal) == (previous[10], previous[12]))
+    withdrawal_role = bool(all_off or withdrawn and same_longitudinal and
+      (cs.steeringPressed or self.temporary_eps_event_ns and previous is not None and not previous[9] and not previous[11]))
+    if pending and (not withdrawal_role or previous is None or previous[5] != 'none' or
       previous[0] != self.session or previous[7] != self.session or
-      previous[1] != self.sequence or previous[6] != self.pending_event or
-      previous[9] or previous[10] or previous[11] or previous[12] or
+      previous[1] != self.sequence or previous[6] not in (self.pending_event, self.temporary_eps_event_ns) or
+      previous[9] or previous[11] or not same_longitudinal or
       previous[14] != producer or previous[15] != self.availability_epoch or
-      axis.faultEventMonoTime != self.pending_event or
+      axis.faultEventMonoTime not in (self.pending_event, self.temporary_eps_event_ns) or
+      max(axis.faultEventMonoTime, event_ns) != self.pending_event or
       axis.sourceCarStateMonoTime < previous[8] or stamp < previous[2] or
       not 0 < self.pending_event <= now_ns or now_ns - self.pending_event > AXIS_MAX_AGE_NS):
       return False
@@ -801,9 +807,15 @@ class TransportPauseFeedback:
       (signature == previous or fresh_active_successor))
     if not withdrawn and not held_original:
       return False
-    event_ns = int(sm.logMonoTime['onroadEvents'])
+    stock_user = stock_lateral_user_event(cp)
+    stock_eps_user = bool(self.lateral_only_baseline and cp.pcmCruise and
+      not axis.desiredLongitudinal and not axis.longitudinalActive and not cs.steeringDisengage and
+      cs.cruiseState.available and not cs.cruiseState.enabled and stock_user is not None and
+      any(event.name == stock_user and event.userDisable for event in sm['onroadEvents']))
     # Seatbelt blocks normal LONG, not admitted LAT intent. Keep warnings/native/output checks.
     events = tuple(event for event in sm['onroadEvents'] if not (
+      stock_eps_user and event.name == stock_user and event.userDisable and
+      not event.softDisable and not event.immediateDisable) and not (
       self.lateral_only_baseline and cs.seatbeltUnlatched and not axis.desiredLongitudinal and
       not axis.longitudinalActive and event.name == log.OnroadEvent.EventName.seatbeltNotLatched and
       event.softDisable and event.noEntry and not event.immediateDisable and not event.userDisable))
@@ -815,7 +827,10 @@ class TransportPauseFeedback:
         pending and any(event.userDisable for event in events)):
       return False
     temporary_event = any(event.name in temporary_names for event in events)
-    matched_withdrawal = bool(temporary_event and axis.faultEventMonoTime == event_ns <= stamp)
+    override_temporary = bool((cs.steeringPressed or 0 < event_ns == self.temporary_eps_event_ns) and not temporary_event and
+      any(event.name == log.OnroadEvent.EventName.steerOverride for event in events) and
+      not any(event.softDisable or event.userDisable for event in events))
+    matched_withdrawal = bool((temporary_event or override_temporary) and axis.faultEventMonoTime == event_ns <= stamp)
     original_active = bool(previous is not None and previous[0] == self.session and
       previous[7] == self.session and previous[5] == 'none' and
       previous[9] and previous[11] and previous[13] and previous[6] == event_ns and
@@ -823,15 +838,23 @@ class TransportPauseFeedback:
       previous[14] == producer and previous[15] == self.availability_epoch and
       0 < self.lateral_source_ns <= previous[8] <= previous[2] <= now_ns <= previous[4] and
       now_ns - previous[8] <= AXIS_MAX_AGE_NS)
-    pending_withdrawal = bool(all_off and not any(event.softDisable or event.userDisable for event in events) and
+    repeated_join = bool(self.temporary_eps_event_ns and previous is not None and withdrawn and
+      not previous[9] and not previous[11] and same_longitudinal and
+      previous[6] == self.temporary_eps_event_ns and (temporary_event or override_temporary) and
+      axis.faultEventMonoTime != event_ns and
+      min(axis.faultEventMonoTime, event_ns) == self.temporary_eps_event_ns and
+      0 < max(axis.faultEventMonoTime, event_ns) <= now_ns and
+      now_ns - max(axis.faultEventMonoTime, event_ns) <= AXIS_MAX_AGE_NS)
+    pending_withdrawal = bool(withdrawal_role and not any(event.softDisable or event.userDisable for event in events) and
       not temporary_event and 0 < event_ns < axis.faultEventMonoTime <= stamp and
       now_ns - axis.faultEventMonoTime <= AXIS_MAX_AGE_NS and
       (pending or original_active and axis.sequence > self.sequence))
-    if withdrawn and not (matched_withdrawal or pending_withdrawal):
+    if withdrawn and not (matched_withdrawal or pending_withdrawal or repeated_join):
       return False
     if held_original and not (
         axis.faultEventMonoTime == event_ns <= stamp and not any(event.softDisable for event in events) or
-        temporary_event and 0 < axis.faultEventMonoTime < event_ns and now_ns - event_ns <= AXIS_MAX_AGE_NS):
+        (temporary_event or override_temporary) and 0 < axis.faultEventMonoTime < event_ns and
+        now_ns - event_ns <= AXIS_MAX_AGE_NS):
       return False
     # Intent only: temporary EPS still blocks transport/availability health proofs.
     from openpilot.starpilot.car.nissan.aol import temporary_eps_restriction
@@ -862,6 +885,7 @@ class TransportPauseFeedback:
       not axis.desiredLongitudinal and not axis.longitudinalActive)
     native = _configured_transport_native(sm, cp, self.session, now_ns, allow_restricted_lateral=restricted_eps)
     if (native is None or not native.pandaInventory or
+        stock_eps_user and (native.requestedLongitudinal or native.longitudinalAllowed) or
         held_original and not (native.requestedLateral and (native.lateralAllowed or
           restricted_eps and not native.requestedLongitudinal and not native.longitudinalAllowed)) or
         axis.longitudinalActive and not native.longitudinalAllowed or
@@ -876,10 +900,10 @@ class TransportPauseFeedback:
         # Prior gas: accepted event receipt, not newer other-socket CS release.
         self.temporary_eps_gas_override = any(
           event.name == log.OnroadEvent.EventName.gasPressedOverride for event in events)
-    if pending_withdrawal:
+    if pending_withdrawal or repeated_join:
       # Exact physical-EPS join: intent only; newer/duplicate proofs cannot reanchor.
       if not pending:
-        self.pending_event = int(axis.faultEventMonoTime)
+        self.pending_event = max(int(axis.faultEventMonoTime), event_ns)
       self.awaiting = True
     elif pending:
       self.pending_event = 0

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "04163c69bd7ecb02ed5ccccc16013a5a02c9f8012643dde3ef27b3c4d0826285", "schema_version": 1}
+{"protected_inventory_sha256": "c6ba3bcb9eb3e9b2e83919129278b90520717744e0702dab7db595863886b88f", "schema_version": 1}