Preserve HKG stock CAN-FD AOL intent during location input loss

openpilot/starpilot/aol/tests/test_stock_location_family_intent.py

@@ -0,0 +1,156 @@
+from contextlib import ExitStack
+from unittest import mock
+
+import pytest
+
+from opendbc.car import gen_empty_fingerprint
+from opendbc.car.hyundai.interface import CarInterface
+from opendbc.car.hyundai.values import CAR
+from openpilot.cereal import log
+from openpilot.common.params import Params
+from openpilot.common.prefix import OpenpilotPrefix
+from openpilot.starpilot.aol.tests.stock_lateral_test_helpers import car_state, hkg_family_case
+
+def stock_family_callers(cleanup, family):
+  cleanup.enter_context(OpenpilotPrefix())
+  saved = Params()
+  saved.put('LKASButtonControl', 9, block=True)
+  saved.put('MainCruiseButtonControl', 0, block=True)
+  for key in ('CancelButtonControl', 'LongCancelButtonControl', 'VeryLongCancelButtonControl'):
+    saved.put(key, 0, block=True)
+  case = hkg_family_case('hkg_canfd', False)
+  cleanup.callback(case.doCleanups)
+  lka = family in ('canfd_lka', 'canfd_lka_alt')
+  fp = gen_empty_fingerprint()
+  fp[1 if lka else 0].update({0x1CF: 8, 0x130: 32, 0x35: 32, 0x100: 32, 0x175: 24, 0xA0: 24, 0xEA: 24, 0x1A0: 32})
+  if lka:
+    fp[2][0x110 if family == 'canfd_lka_alt' else 0x50] = 32 if family == 'canfd_lka_alt' else 16
+  if family == 'canfd_lka_alt':
+    fp[2][0x362] = 32
+  identity = (CAR.KIA_NIRO_EV_2ND_GEN if family == 'canfd_camera' else CAR.HYUNDAI_IONIQ_5_N if family == 'canfd_ccnc_camera' else
+              CAR.KIA_EV6 if lka else CAR.GENESIS_GV70_1ST_GEN)
+  cp = CarInterface.get_params(identity, fp, [], False, False, False)
+  case.family_cp = cp.as_reader()
+  sd, _, drive, card, receive = case.family_callers(False)
+  state = car_state()
+  state.vEgo = state.vEgoRaw = 6.98
+  state.steeringAngleDeg = 65.9
+  return sd, drive, card, receive, state
+
+
+@pytest.mark.parametrize('family', ['canfd_radar', 'canfd_lka', 'canfd_lka_alt', 'canfd_camera', 'canfd_ccnc_camera'])
+@pytest.mark.parametrize('event_first', [False, True])
+def test_existing_stock_hkg_location_and_nominal_pcm_off_keep_established_intent(family, event_first):
+  with ExitStack() as cleanup:
+    sd, drive, card, receive, state = stock_family_callers(cleanup, family)
+    original = sd.sm.update_msgs
+    unavailable = [False]
+
+    def update(timestamp, messages):
+      selected = []
+      for message in messages:
+        if message.which() == 'deviceMotion':
+          message = message.as_builder()
+          message.deviceMotion.inputsOK = not unavailable[0]
+          message = message.as_reader()
+        selected.append(message)
+      return original(timestamp, selected)
+
+    with mock.patch.object(sd.sm, 'update_msgs', side_effect=update):
+      for tick in range(2, 5):
+        axis, _, command = drive(tick, observed_state=state, panda_controls_allowed=False, native_request_long=False)
+        assert axis.lateralActive and command.latActive and not axis.longitudinalActive and not command.longActive
+        assert receive(10_000_000_000 + tick * 10_000_000, state=state,
+                       axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents'])
+      baseline = (card.aol_transport_feedback.lateral_baseline_ns, card.aol_transport_feedback.lateral_native_ns)
+      prior = sd.pm.messages['aolAxisState']
+      unavailable[0] = True
+      axis, _, command = drive(5, observed_state=state, panda_controls_allowed=False, native_request_long=False)
+      assert log.OnroadEvent.EventName.locationdTemporaryError in sd.events.names
+      assert log.OnroadEvent.EventName.pcmDisable in sd.events.names
+      assert not axis.lateralActive and not axis.longitudinalActive and not command.latActive and not command.longActive
+      if event_first:
+        assert receive(10_050_000_000, state=state, axis=prior, event=sd.pm.messages['onroadEvents'])
+      assert receive(10_050_000_001, state=state, axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents'])
+      assert card.aol_card_intent.allowed_latch
+      assert baseline == (card.aol_transport_feedback.lateral_baseline_ns, card.aol_transport_feedback.lateral_native_ns)
+      for tick in range(6, 10):
+        axis, _, command = drive(tick, observed_state=state, panda_controls_allowed=False, native_request_long=False,
+                                 allowed=False, native_request_lat=False)
+        assert not axis.lateralActive and not axis.longitudinalActive and not command.latActive and not command.longActive
+        assert receive(10_000_000_000 + tick * 10_000_000, state=state,
+                       axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents'])
+        assert baseline == (card.aol_transport_feedback.lateral_baseline_ns, card.aol_transport_feedback.lateral_native_ns)
+      unavailable[0] = False
+      for tick in range(10, 12):
+        axis, _, command = drive(tick, observed_state=state, panda_controls_allowed=False, native_request_long=False)
+        assert receive(10_000_000_000 + tick * 10_000_000, state=state,
+                       axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents'])
+        assert not axis.longitudinalActive and not command.longActive and not sd.enabled
+      assert axis.lateralActive and command.latActive
+
+
+@pytest.mark.parametrize('family', ['canfd_radar', 'canfd_lka', 'canfd_lka_alt', 'canfd_camera', 'canfd_ccnc_camera'])
+@pytest.mark.parametrize('fault', ['native_denial', 'native_invalid', 'panda_rx', 'relay', 'permanent_eps',
+  'invalid_can', 'can_timeout', 'door', 'seatbelt', 'brake', 'regen', 'gas', 'physical_cancel', 'reverse', 'unknown_gear',
+  'extra_user', 'hard_event', 'epoch', 'session', 'expired', 'future_source', 'no_baseline'])
+def test_stock_location_never_retains_intent_through_independent_fault(family, fault):
+  from opendbc.car import structs
+
+  with ExitStack() as cleanup:
+    sd, drive, card, receive, state = stock_family_callers(cleanup, family)
+    for tick in range(2, 5):
+      axis, _, command = drive(tick, observed_state=state, panda_controls_allowed=False, native_request_long=False)
+      assert axis.lateralActive and command.latActive
+      assert receive(10_000_000_000 + tick * 10_000_000, state=state,
+                     axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents'])
+    assert card.aol_transport_feedback.lateral_baseline_ns
+    original = sd.sm.update_msgs
+
+    def update(timestamp, messages):
+      selected = []
+      for message in messages:
+        if message.which() == 'deviceMotion':
+          message = message.as_builder()
+          message.deviceMotion.inputsOK = False
+          message = message.as_reader()
+        selected.append(message)
+      return original(timestamp, selected)
+
+    with mock.patch.object(sd.sm, 'update_msgs', side_effect=update):
+      axis, _, command = drive(5, observed_state=state, panda_controls_allowed=False, native_request_long=False,
+        allowed=fault != 'native_denial', native_valid=fault != 'native_invalid',
+        panda_fault=fault == 'panda_rx', relay_fault=fault == 'relay')
+    assert log.OnroadEvent.EventName.locationdTemporaryError in sd.events.names
+    assert log.OnroadEvent.EventName.pcmDisable in sd.events.names
+    assert not axis.lateralActive and not axis.longitudinalActive and not command.latActive and not command.longActive
+    proof = sd.pm.messages['aolAxisState'].as_builder()
+    event = sd.pm.messages['onroadEvents'].as_builder()
+    state.steerFaultPermanent = fault == 'permanent_eps'
+    state.canValid = fault != 'invalid_can'
+    state.canTimeout = fault == 'can_timeout'
+    state.doorOpen = fault == 'door'
+    state.seatbeltUnlatched = fault == 'seatbelt'
+    state.brakePressed = fault == 'brake'
+    state.regenBraking = fault == 'regen'
+    state.gasPressed = fault == 'gas'
+    if fault == 'physical_cancel':
+      state.buttonEvents = [structs.CarState.ButtonEvent(type='cancel', pressed=True)]
+    if fault in ('reverse', 'unknown_gear'):
+      state.gearShifter = 'reverse' if fault == 'reverse' else 'unknown'
+    if fault == 'extra_user':
+      event.onroadEvents = [x.to_dict() for x in event.onroadEvents] + [{'name': 'buttonCancel', 'userDisable': True}]
+    if fault == 'hard_event':
+      event.onroadEvents[0].immediateDisable = True
+    if fault == 'epoch':
+      proof.aolAxisState.intentIntegrityEpoch += 1
+    if fault == 'session':
+      proof.aolAxisState.sessionId = proof.aolAxisState.faultSessionId = 'wrong-owner'
+    if fault == 'expired':
+      proof.aolAxisState.validUntilMonoTime = 10_049_999_999
+    if fault == 'future_source':
+      proof.aolAxisState.sourceCarStateMonoTime = 10_060_000_000
+    if fault == 'no_baseline':
+      card.aol_transport_feedback.lateral_baseline_ns = 0
+    assert not receive(10_050_000_001, state=state, axis=proof.as_reader(), event=event.as_reader())
+    assert not card.aol_card_intent.allowed_latch

openpilot/starpilot/aol/transport_pause.py

@@ -354,18 +354,23 @@ class TransportPauseFeedback:
           (not event.softDisable or event.name == log.OnroadEvent.EventName.steerTempUnavailable) for event in events))
     users = [event for event in events if event.userDisable]
     from opendbc.car.hyundai.canfd_angle_aol import qualified as angle_qualified
+    from opendbc.car.hyundai.canfd_stock_aol import qualified as stock_canfd_qualified
     from opendbc.car import structs
     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
     # withdrawn output; the native/session/source checks below still apply.
-    stock_location_user = bool(location_fault and angle_qualified(cp, marked_only=True) and users and
+    stock_angle = angle_qualified(cp, marked_only=True)
+    stock_hkg = stock_angle or stock_canfd_qualified(cp, marked_only=True)
+    stock_user = stock_lateral_user_event(cp)
+    stock_location_user = bool(location_fault and stock_hkg and users and
       not axis.desiredLongitudinal and not axis.longitudinalActive and
       cs.cruiseState.available and not cs.cruiseState.enabled and
       cs.gearShifter in (gears.drive, gears.low, gears.sport, gears.brake) and
       not cs.doorOpen and not cs.seatbeltUnlatched and not cs.brakePressed and not cs.regenBraking and
-      all(event.name == log.OnroadEvent.EventName.pcmDisable and not event.softDisable and
+      (not cs.gasPressed or stock_angle) and
+      all(event.name == stock_user and not event.softDisable and
           not event.immediateDisable for event in users))
     pedal = bool(cs.brakePressed and users and all(event.name == log.OnroadEvent.EventName.pedalPressed and
       not event.softDisable and not event.immediateDisable for event in users) and not faults)

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "bd40317888464fc6b713be5bcb99feac9c266fc5d8462fe8e0dd68fd09fae2ea", "schema_version": 1}
+{"protected_inventory_sha256": "4f797d9bb255601305a748fb2efbde933d5b33ab00952bf6d17db5aa7e50145e", "schema_version": 1}

tools/ci/run_vehicle_tests.py

@@ -33,6 +33,7 @@ DEBUG_SAFETY_CLASS_TARGETS = {
 
 
 RELEASE_TARGETS = (
+  "openpilot/starpilot/aol/tests/test_stock_location_family_intent.py",
   'opendbc_repo/opendbc/car/hyundai/tests/test_generic_hda2_startup.py',
   'opendbc_repo/opendbc/car/hyundai/tests/test_generic_hda2_startup_guards.py',
   'opendbc_repo/opendbc/car/hyundai/tests/test_generic_hda2_startup_lifecycle.py::test_exact_factory_stock_copy_and_owner_preserve_all_params',