Preserve HKG lateral through CANCEL status handoffs

openpilot/starpilot/aol/tests/test_hkg_shared_cancel_guard.py

@@ -256,3 +256,54 @@ def test_simultaneous_cancel_keeps_default_timestamp_side_effects(opted_in, mapp
   owner.update(cs, now_ns=2, fault_active=False)
   assert not owner.allowed_latch
   assert owner._last_latch_edge_ns == (0 if opted_in and mapped else 2)
+
+
+@pytest.mark.parametrize('identity', (CAR.GENESIS_GV60_EV_1ST_GEN, CAR.HYUNDAI_IONIQ_5,
+                                     CAR.HYUNDAI_KONA_EV_2ND_GEN))
+@pytest.mark.parametrize('fault', ('none', 'no_cancel', 'lat_denied', 'rx', 'heartbeat', 'expired', 'same_health'))
+def test_shared_long_cancel_health_can_precede_native_status(identity, fault):
+  from opendbc.car import gen_empty_fingerprint
+  from opendbc.car.hyundai.canfd_stock_aol import qualified_long
+  from openpilot.starpilot.aol.transport_pause import cp_identity
+
+  cp, cs, sm, feedback, owner, native, now = captured_context()
+  if identity == CAR.GENESIS_GV60_EV_1ST_GEN:
+    fingerprint = gen_empty_fingerprint()
+    fingerprint[0][0x1cf] = 8
+    cp = CarInterface.get_params(identity, fingerprint, [], True, False, False)
+    cp.safetyConfigs[0].safetyParam |= 0x800
+    cp.alternativeExperience = 32
+  else:
+    cp.carFingerprint = identity
+  assert qualified_long(cp, marked_only=True)
+  feedback.cp = cp_identity(cp)
+  config = cp.safetyConfigs[0]
+  slot = replace(native.pandaInventory[0], safetyModel=int(config.safetyModel.raw), safetyParam=config.safetyParam,
+                 controlsAllowed=True)
+  native = replace(native, safetyModel=int(config.safetyModel.raw), safetyParam=config.safetyParam,
+                   longitudinalAllowed=True, pandaInventory=(slot,), sourcePandaStatesMonoTime=now)
+  sm['aolSafetyWire'] = encode_safety(native)
+  pandas = messaging.new_message('pandaStates', 1, valid=True, logMonoTime=now)
+  panda = pandas.pandaStates[0]
+  panda.safetyModel, panda.safetyParam, panda.alternativeExperience = config.safetyModel, config.safetyParam, 32
+  panda.controlsAllowed = True
+  sm['pandaStates'] = pandas.as_reader().pandaStates
+  if fault == 'no_cancel':
+    cs.buttonEvents = []
+  assert not feedback.observe(sm, cp, cs, now, latched=True, cancel_source_ns=now,
+                              intent_producer_session='authority-card')
+  assert bool(feedback.longitudinal_cancel_ns) == (fault != 'no_cancel')
+  original_receipt = feedback.longitudinal_cancel_ns
+  cs.buttonEvents = []
+  pandas.pandaStates[0].controlsAllowed = False
+  pandas.pandaStates[0].safetyRxChecksInvalid = fault == 'rx'
+  pandas.pandaStates[0].heartbeatLost = fault == 'heartbeat'
+  sm['pandaStates'] = pandas.as_reader().pandaStates
+  sm.logMonoTime['pandaStates'] = now + (0 if fault == 'same_health' else 10_000_000)
+  if fault == 'lat_denied':
+    sm['aolSafetyWire'] = encode_safety(replace(native, lateralAllowed=False))
+  later = now + (201_000_000 if fault == 'expired' else 10_000_000)
+  retained = feedback._longitudinal_cancel_current(sm, cp, cs, sm['aolAxisState'], later,
+                                                  latched=True, cancel_source_ns=0)
+  assert (retained is not None) == (fault == 'none')
+  assert feedback.longitudinal_cancel_ns == original_receipt

openpilot/starpilot/aol/transport_pause.py

@@ -805,7 +805,7 @@ class TransportPauseFeedback:
     return native
 
   def _longitudinal_cancel_current(self, sm, cp, cs, axis, now_ns, *, latched, cancel_source_ns, withdrawal_required=True) -> SafetyState | None:
-    from opendbc.car.hyundai.ev6_aol import qualified
+    from opendbc.car.hyundai.canfd_stock_aol import qualified_long as qualified
     from openpilot.starpilot.car.hyundai.aol import qualified_ccnc_long_aol as ev9_qualified
     from opendbc.car.structs import car
 
@@ -833,7 +833,7 @@ class TransportPauseFeedback:
       return None
     native = current_native(sm, cp, now_ns=now_ns, axis_session_id=self.session)
     if (native is None or not native.pandaInventory or not native.requestedLateral or not native.lateralAllowed or
-        not native.requestedLongitudinal or withdrawal_required and native.longitudinalAllowed or
+        not native.requestedLongitudinal or
         native.observedMonoTime < self.lateral_native_ns or
         not native_inventory_matches_cp(native, cp) or
         (native.pandaSerial, tuple(p.hardwareSerial for p in native.pandaInventory)) != self.lateral_inventory):
@@ -849,6 +849,12 @@ class TransportPauseFeedback:
       not p.heartbeatLost and not blocking_fault_list(p.faults, cp) and
       (str(c.safetyModel) not in ('silent', 'noOutput') or not p.controlsAllowed)
       for p, c in zip(pandas, cp.safetyConfigs, strict=True))
+    raw_withdrawal = bool(self.longitudinal_cancel_ns and
+      0 < native.sourcePandaStatesMonoTime < stamp and
+      all(not p.controlsAllowed for p, c in zip(pandas, cp.safetyConfigs, strict=True)
+          if str(c.safetyModel) not in ('silent', 'noOutput')))
+    if withdrawal_required and native.longitudinalAllowed and not raw_withdrawal:
+      return None
     return native if current_pandas else None
 
 

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "da07e43b8010c9bfc648527e37a6d3e0e39642ef3763691ba776087c9023d457", "schema_version": 1}
+{"protected_inventory_sha256": "a97a892642f4c96af7393c2e76eb2fe54c3ff73499017d8a5d4db324cda55cd7", "schema_version": 1}