Preserve stock lateral intent after temporary steering faults

openpilot/starpilot/aol/tests/test_stock_eps_recovery.py

@@ -0,0 +1,91 @@
+"""Fresh EPS recovery retains established stock LAT intent, never an output grant."""
+
+import pytest
+
+from opendbc.car import structs
+from openpilot.cereal import log
+from openpilot.starpilot.aol.tests.test_stock_location_recovery import established_stock as established_stock
+from openpilot.starpilot.tests.test_ford_reverse_intent import NOW, PRODUCER, change, native_change, sample
+
+
+def recovered_pause(cp, state, feedback):
+  stamp = NOW + 10_000_000
+  state.steerFaultTemporary = True
+  sm, _ = sample(cp, stamp, stamp, 2, active=False, requested=False, allowed=False, event_stamp=stamp)
+
+  def eps_events(peers):
+    event = peers.messages['onroadEvents'].as_builder()
+    event.onroadEvents = [{'name': 'pcmDisable', 'userDisable': True},
+                         {'name': 'steerTempUnavailable', 'softDisable': True}]
+    peers.messages['onroadEvents'] = event.as_reader()
+    change(peers, 'aolAxisState', lambda axis: setattr(axis, 'nativeAcknowledged', False))
+
+  eps_events(sm)
+  assert not feedback.observe(sm, cp, state, stamp, latched=True, intent_producer_session=PRODUCER)
+  assert feedback.temporary_eps_event_ns == stamp
+  state.steerFaultTemporary = False
+  now = NOW + 170_000_000
+  sm, native = sample(cp, now, now, 3, active=False, requested=False, allowed=False, event_stamp=stamp)
+  eps_events(sm)
+  return sm, native, now
+
+
+def test_matched_stock_eps_recovery_preserves_only_armed_intent(established_stock):
+  cp, state, feedback = established_stock
+  sm, native, now = recovered_pause(cp, state, feedback)
+  baseline = (feedback.healthy_feedback_ns, feedback.healthy_native_ns, feedback.lateral_baseline_ns,
+              feedback.lateral_source_ns, feedback.lateral_native_ns)
+  assert not feedback.observe(sm, cp, state, now, latched=True, intent_producer_session=PRODUCER)
+  assert feedback.temporary_host_qualified and not feedback.failed
+  assert baseline == (feedback.healthy_feedback_ns, feedback.healthy_native_ns, feedback.lateral_baseline_ns,
+                      feedback.lateral_source_ns, feedback.lateral_native_ns)
+  assert not native.lateralAllowed and not native.longitudinalAllowed
+
+
+@pytest.mark.parametrize('fault', ('invalid_can', 'permanent_eps', 'main_off', 'cancel', 'reverse', 'brake', 'gas',
+  'native_denial', 'native_invalid', 'wrong_epoch', 'wrong_producer', 'wrong_session', 'expired', 'future_source',
+  'immediate', 'other_user', 'long_desired', 'long_active', 'missing_anchor', 'different_anchor'))
+def test_eps_recovery_never_bypasses_independent_vetoes(established_stock, fault):
+  cp, state, feedback = established_stock
+  sm, native, now = recovered_pause(cp, state, feedback)
+  producer = PRODUCER
+  if fault == 'invalid_can':
+    state.canValid = False
+  elif fault == 'permanent_eps':
+    state.steerFaultPermanent = True
+  elif fault == 'main_off':
+    state.cruiseState.available = False
+  elif fault == 'cancel':
+    state.buttonEvents = [structs.CarState.ButtonEvent(type='cancel', pressed=True)]
+  elif fault == 'reverse':
+    state.gearShifter = 'reverse'
+  elif fault == 'brake':
+    state.brakePressed = True
+  elif fault == 'gas':
+    state.gasPressed = True
+  elif fault == 'native_denial':
+    native_change(sm, native, requestedLateral=True)
+  elif fault == 'native_invalid':
+    sm.valid['aolSafetyWire'] = False
+  elif fault == 'wrong_epoch':
+    change(sm, 'aolAxisState', lambda axis: setattr(axis, 'intentIntegrityEpoch', 2))
+  elif fault == 'wrong_producer':
+    producer = 'other'
+  elif fault == 'wrong_session':
+    change(sm, 'aolAxisState', lambda axis: setattr(axis, 'sessionId', 'other'))
+  elif fault == 'expired':
+    change(sm, 'aolAxisState', lambda axis: setattr(axis, 'validUntilMonoTime', now - 1))
+  elif fault == 'future_source':
+    change(sm, 'aolAxisState', lambda axis: setattr(axis, 'sourceCarStateMonoTime', now + 1))
+  elif fault == 'immediate':
+    change(sm, 'onroadEvents', lambda events: setattr(events[1], 'immediateDisable', True))
+  elif fault == 'other_user':
+    change(sm, 'onroadEvents', lambda events: setattr(events[0], 'name', log.OnroadEvent.EventName.buttonCancel))
+  elif fault in ('long_desired', 'long_active'):
+    change(sm, 'aolAxisState', lambda axis: setattr(axis, 'desiredLongitudinal' if fault == 'long_desired' else 'longitudinalActive', True))
+  elif fault == 'missing_anchor':
+    feedback.temporary_eps_event_ns = 0
+  elif fault == 'different_anchor':
+    change(sm, 'aolAxisState', lambda axis: setattr(axis, 'faultEventMonoTime', NOW + 11_000_000))
+  assert feedback.observe(sm, cp, state, now, latched=True, intent_producer_session=producer)
+  assert not feedback.temporary_host_qualified

openpilot/starpilot/aol/tests/test_stock_location_recovery.py

@@ -12,22 +12,36 @@ from opendbc.car.chrysler.interface import CarInterface as ChryslerInterface
 from opendbc.car.chrysler.values import CAR as Chrysler
 from opendbc.car.ford.interface import CarInterface as FordInterface
 from opendbc.car.ford.values import CAR as Ford
+from opendbc.car.hyundai.interface import CarInterface as HyundaiInterface
+from opendbc.car.hyundai.radar_interface import RADAR_START_ADDR
+from opendbc.car.hyundai.values import CAR as Hyundai
 from openpilot.cereal import log
 from openpilot.common.prefix import OpenpilotPrefix
 from openpilot.starpilot.aol.runtime import current_native
+from openpilot.starpilot.aol.vehicle import policy_for
 from openpilot.starpilot.aol.transport_pause import TransportPauseFeedback
 from openpilot.starpilot.tests.test_ford_reverse_intent import NOW, PRODUCER, change, native_change, sample
 
 
 @pytest.fixture(params=(Chrysler.CHRYSLER_PACIFICA_2018, Chrysler.RAM_1500_5TH_GEN,
-                        Ford.FORD_MUSTANG_MACH_E_MK1, Ford.FORD_ESCAPE_MK4))
+                        Ford.FORD_MUSTANG_MACH_E_MK1, Ford.FORD_ESCAPE_MK4, Hyundai.HYUNDAI_SONATA_HYBRID))
 def established_stock(request):
   interface = FordInterface if request.param in (Ford.FORD_MUSTANG_MACH_E_MK1, Ford.FORD_ESCAPE_MK4) else ChryslerInterface
   fp = gen_empty_fingerprint()
   fp[0][0x5A] = 8
+  if request.param == Hyundai.HYUNDAI_SONATA_HYBRID:
+    interface = HyundaiInterface
+    fp[0].update({0x391: 8, 0x38D: 8})
+    fp[2][0x485] = 4
+    fp[1][RADAR_START_ADDR] = 8
   with OpenpilotPrefix():
     cp = interface.get_params(request.param, fp, [], False, False, False)
-  cp.alternativeExperience = 32
+  # Use the same admitted factory policy additions as Card. Wire vectors
+  # remain controlled; the separate actual CAN caller qualifies the native owner.
+  policy = policy_for(cp)
+  cp.safetyConfigs[0].safetyParam |= policy.safety_param_addition
+  cp.alternativeExperience |= policy.alternative_experience_addition
+  assert cp.alternativeExperience == 32
   assert cp.pcmCruise and not cp.openpilotLongitudinalControl
   state = structs.CarState(canValid=True, gearShifter=structs.CarState.GearShifter.drive, vEgo=15.)
   state.cruiseState.available = True

openpilot/starpilot/aol/transport_pause.py

@@ -364,12 +364,12 @@ class TransportPauseFeedback:
     gears = structs.CarState.GearShifter
 
     # Stock PCM off is not the driver's independent AOL toggle. Retain only
-    # previously established intent while the exact location input fault has
+    # previously established intent through a location fault or its matched EPS recovery, after it has
     # withdrawn output; the native/session/source checks below still apply.
     stock_angle = angle_qualified(cp, marked_only=True)
     stock_hkg = stock_angle or stock_canfd_qualified(cp, marked_only=True) or stock_classic_qualified(cp, marked_only=True)
     stock_user = stock_lateral_user_event(cp)
-    stock_location_user = bool(location_fault and (stock_hkg or chrysler_qualified(cp, marked_only=True) or
+    stock_temporary_user = bool((location_fault or eps_recovery) and (stock_hkg or chrysler_qualified(cp, marked_only=True) or
       ford_qualified(cp, marked_only=True)) and users and
       not axis.desiredLongitudinal and not axis.longitudinalActive and
       cs.cruiseState.available and not cs.cruiseState.enabled and
@@ -389,7 +389,7 @@ class TransportPauseFeedback:
         int(getattr(axis, 'intentIntegrityEpoch', 0)) != self.availability_epoch or
         not self.lateral_inventory or not self.lateral_baseline_ns or
         not cs.canValid or cs.canTimeout or cs.steerFaultPermanent or cs.accFaulted or cs.steerFaultTemporary or
-        users and not (pedal or stock_location_user) or str(axis.faultReason) not in ('none', 'critical') or
+        users and not (pedal or stock_temporary_user) or str(axis.faultReason) not in ('none', 'critical') or
         not 0 < self.lateral_source_ns <= axis.sourceCarStateMonoTime <= stamp <= now_ns or
         now_ns - axis.sourceCarStateMonoTime > AXIS_MAX_AGE_NS or
         not sm.seen['onroadEvents'] or not sm.valid['onroadEvents'] or not sm.alive['onroadEvents']):

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "4077b51b4a4637a078e8745ee982bab55424d657542911f3d8c84251a3ab6d9b", "schema_version": 1}
+{"protected_inventory_sha256": "86809b0899e8303a48006b5f534bcb774fc7e2790fdf4d6285fe8150d8eda2fb", "schema_version": 1}