From 208ef1d8cdb2bf76b0ecaed2df083bb56e56bb99 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Wed, 11 Nov 2026 11:51:01 -0600 Subject: [PATCH] Keep shared HKG lateral through longitudinal cancel handoffs --- openpilot/selfdrive/car/card.py | 5 +- openpilot/selfdrive/selfdrived/selfdrived.py | 2 + .../aol/tests/test_hkg_cancel_joined.py | 71 ++++++++++++++++--- .../tests/test_hkg_early_long_withdrawal.py | 62 ++++++++++++++++ openpilot/starpilot/aol/transport_pause.py | 5 +- openpilot/starpilot/car/hyundai/aol.py | 20 ++++++ prebuilt.json | 2 +- 7 files changed, 149 insertions(+), 18 deletions(-) diff --git a/openpilot/selfdrive/car/card.py b/openpilot/selfdrive/car/card.py index ae6626f0ea..10dec57a07 100644 --- a/openpilot/selfdrive/car/card.py +++ b/openpilot/selfdrive/car/card.py @@ -60,7 +60,6 @@ from opendbc.car.ford.aol import ( temporary_restriction as ford_temporary_restriction, ) from opendbc.car.hyundai.canfd_angle_aol import native_observation, temporary_restriction, qualified as qualified_angle_aol -from opendbc.car.hyundai.blended_stock_aol import qualified_alpha as qualified_blended_alpha_aol from opendbc.car.hyundai.canfd_owner import config_index as hyundai_canfd_config_index from opendbc.car.hyundai.captured_adrv_startup import CapturedADRVStartup from openpilot.starpilot.car.nissan.aol import ( @@ -616,9 +615,7 @@ class Car: self.aol_card_intent, cancel_source_ns=cancel_source_ns, now_ns=now_ns, producer=getattr(self, 'slc_producer_session', '')) cancel_feedback = self.aol_transport_feedback - if (hkg_cancel_retained and (self.CP.carFingerprint in (HYUNDAI_CAR.KIA_EV9, HYUNDAI_CAR.HYUNDAI_IONIQ_5_PE) or - qualified_blended_alpha_aol(self.CP, marked_only=True)) and - cancel_feedback is not None and not cancel_feedback.longitudinal_cancel_ns): + if hkg_cancel_retained and cancel_feedback is not None and not cancel_feedback.longitudinal_cancel_ns: # The pure guard proved this exact raw CANCEL/FULL join. Bind # its first local receipt for the existing bounded withdrawal handshake; # held samples never renew it or any source/native/output deadline. diff --git a/openpilot/selfdrive/selfdrived/selfdrived.py b/openpilot/selfdrive/selfdrived/selfdrived.py index fdef66f300..3e1e6e1f5e 100755 --- a/openpilot/selfdrive/selfdrived/selfdrived.py +++ b/openpilot/selfdrive/selfdrived/selfdrived.py @@ -1804,6 +1804,7 @@ class SelfdriveD: below_engage_lateral_allowed as volkswagen_below_engage_lateral_allowed) from openpilot.starpilot.aol.dm import lateral_event_context from openpilot.starpilot.car.toyota.legacy_lateral import below_engage_lateral_allowed as toyota_below_engage_lateral_allowed + from openpilot.starpilot.car.hyundai.aol import raw_longitudinal_withdrawal retained_dm_lateral, lateral_events = lateral_event_context(self, CS, intent, native, now_ns, ordinary_lag) reverse_pause = known_reverse_pause(CS, getattr(self, 'car_state_samples', ())) pause_brake_mps = self.aol_settings.pause_brake_mps if self.aol_settings is not None else 0.0 @@ -1811,6 +1812,7 @@ class SelfdriveD: lateral_events = [event for event in lateral_events if event not in (EventName.wrongGear, EventName.reverseGear)] return decide_axes( standard_lateral=self.active, standard_longitudinal=bool(self.enabled and self.CP.openpilotLongitudinalControl and + not raw_longitudinal_withdrawal(self.CP, CS, self.sm, native, session=self.aol_session_id, now_ns=now_ns) and not any(event in LONGITUDINAL_ONLY_EVENTS for event in self.events.names) and not any(sample.seatbeltUnlatched or sample.doorOpen for sample, _ in getattr(self, 'car_state_samples', ()))), intent=intent, native=native, car_state=CS, initialized=self.initialized, diff --git a/openpilot/starpilot/aol/tests/test_hkg_cancel_joined.py b/openpilot/starpilot/aol/tests/test_hkg_cancel_joined.py index d0fa464f86..7406627540 100644 --- a/openpilot/starpilot/aol/tests/test_hkg_cancel_joined.py +++ b/openpilot/starpilot/aol/tests/test_hkg_cancel_joined.py @@ -78,7 +78,8 @@ def _invalid_controls_emits_no_commands(received): return envelope.which() == 'sendcan' and not envelope.valid and len(envelope.sendcan) == 0 -def _exercise_join(identity, alternate, incidental, withdrawal, monkeypatch, *, shared_long=False, two_panda=False, unmapped=False): +def _exercise_join(identity, alternate, incidental, withdrawal, monkeypatch, *, shared_long=False, two_panda=False, + unmapped=False, cancel_health_order=None, main_binding=0): assert not two_panda or (shared_long and not alternate and identity == CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN) offset = 4 if two_panda else 0 bootstrap_refusals = [] @@ -91,7 +92,7 @@ def _exercise_join(identity, alternate, incidental, withdrawal, monkeypatch, *, saved.put_bool('OpenpilotEnabledToggle', True, block=True) saved.put_bool('AlwaysOnLateral', True, block=True) saved.put('LKASButtonControl', 0 if unmapped else AOL_TOGGLE, block=True) - saved.put('MainCruiseButtonControl', 0, block=True) + saved.put('MainCruiseButtonControl', main_binding, block=True) now = [10_000_000_000] holder = None query = None @@ -99,6 +100,9 @@ def _exercise_join(identity, alternate, incidental, withdrawal, monkeypatch, *, saved.put(key, 0, block=True) try: cp = params(identity, alternate=alternate) + if shared_long and identity == CAR.KIA_EV6: + from openpilot.starpilot.tests.test_ev6_startup import params as ev6_startup_params + cp = ev6_startup_params(alpha=True) if shared_long: from types import SimpleNamespace from opendbc.car.hyundai import ecu_startup @@ -142,8 +146,10 @@ def _exercise_join(identity, alternate, incidental, withdrawal, monkeypatch, *, with patch.object(ecu_startup, 'IsoTpParallelQuery', side_effect=query), \ patch.object(card_module.time, 'clock_gettime_ns', side_effect=lambda clock: now[0]), \ patch('opendbc.car.hyundai.torque_ev_startup.time.sleep'): - selected = holder.prepare(cp, CarInterface, (capture if captured else list, sent.extend), requested=True, admission=lambda: True) - assert isinstance(holder.owner, GV70Startup if captured else TorqueEVStartup) + selected = holder.prepare(cp, CarInterface, + (capture if captured or identity == CAR.KIA_EV6 else list, sent.extend), requested=True, admission=lambda: True) + from opendbc.car.hyundai.ev6_startup import EV6Startup + assert isinstance(holder.owner, GV70Startup if captured else EV6Startup if identity == CAR.KIA_EV6 else TorqueEVStartup) assert holder.owner.outcome is Outcome.SENT_UNCONFIRMED ci = CarInterface(selected) card = card_module.Car(CI=ci, RI=RadarInterface(ci.CP), startup_owner=holder.owner) @@ -158,6 +164,7 @@ def _exercise_join(identity, alternate, incidental, withdrawal, monkeypatch, *, # The reusable helper defaults this to None; exercise actual saved mappings. sd.aol_settings = read_settings(saved) assert sd.aol_settings.lkas_action == (0 if unmapped else AOL_TOGGLE) and sd.aol_settings.main_action == 0 + assert sd.aol_settings.main_wheel_action == (main_binding == 10) assert sd.CP.to_dict() == card.CP.to_dict() == controls.CP.to_dict() assert len(card.CP.safetyConfigs) == (2 if two_panda else 1) assert bool(card.CP.openpilotLongitudinalControl) == shared_long @@ -182,6 +189,10 @@ def _exercise_join(identity, alternate, incidental, withdrawal, monkeypatch, *, native.set_alternative_experience(card.CP.alternativeExperience) queued, received, previous_sd = [], {}, {} tick = 0 + held_cancel_wire = None + prior_wire = None + held_button_frames = [] + cancel_hold_ticks = 0 patch_scope = monkeypatch.context() local = patch_scope.__enter__() try: @@ -249,7 +260,7 @@ def _exercise_join(identity, alternate, incidental, withdrawal, monkeypatch, *, local.setattr(card_module, 'hkg_cancel_intent_retained', observe_retention) def step(*, main=0, lkas=0, button=0, engaged=False, withdraw_request=False): - nonlocal tick + nonlocal tick, held_cancel_wire, prior_wire, held_button_frames, cancel_hold_ticks tick += 1 now[0] = source_ns = 10_000_000_000 + tick * 10_000_000 prior_axis = previous_sd.get('aolAxisState') @@ -275,7 +286,14 @@ def _exercise_join(identity, alternate, incidental, withdrawal, monkeypatch, *, rx_invalid = bool(native.safety_test_toyota_rx_summary_invalid()) relay_fault = bool(native.get_relay_malfunction()) assert not relay_fault - batch = messaging.log_from_bytes(card_module.can_list_to_can_capnp(frames)).as_builder() + host_frames = [*held_button_frames, *frames] + held_button_frames = [] + first_cancel = bool(cancel_health_order and button == 4 and held_cancel_wire is None and + any(address == 0x1CF for address, _, _ in frames)) + if first_cancel and cancel_health_order == 'health_first': + held_button_frames = [frame for frame in frames if frame[0] == 0x1CF] + host_frames = [frame for frame in frames if frame[0] != 0x1CF] + batch = messaging.log_from_bytes(card_module.can_list_to_can_capnp(host_frames)).as_builder() batch.logMonoTime = source_ns queued[:] = [batch.to_bytes()] device = messaging.new_message('deviceState', valid=True, logMonoTime=source_ns) @@ -299,9 +317,18 @@ def _exercise_join(identity, alternate, incidental, withdrawal, monkeypatch, *, card.CP.alternativeExperience, False, safetyRxChecksInvalid=auxiliary_rx_invalid), slot) if two_panda else (slot,)) wire = messaging.new_message('aolSafetyWire', 0, valid=True, logMonoTime=source_ns) - wire.aolSafetyWire = encode_safety(SafetyState(1, True, source_ns, source_ns + 30_000_000, + wire.aolSafetyWire = encode_safety(SafetyState(1, True, source_ns, source_ns + (200_000_000 if cancel_health_order else 30_000_000), model, word, bool(mask & 1), bool(mask & 2), bool(actual_request & 1), bool(actual_request & 2), 'authority-panda', card.aol_axis_session(), source_ns, inventory)) + actual_wire = wire + if first_cancel: + assert prior_wire is not None + held_cancel_wire = wire if cancel_health_order == 'mask_first' else prior_wire + cancel_hold_ticks = 10 if cancel_health_order == 'mask_first' else 1 + if cancel_hold_ticks: + wire = held_cancel_wire + cancel_hold_ticks -= 1 + prior_wire = actual_wire messages = [device.as_reader(), pandas.as_reader(), wire.as_reader()] messages += [previous_sd[name] for name in ('selfdriveState', 'aolAxisState', 'onroadEvents') if name in previous_sd] if 'onroadEvents' not in previous_sd: @@ -317,6 +344,9 @@ def _exercise_join(identity, alternate, incidental, withdrawal, monkeypatch, *, output_envelope = received['carOutput'] assert state_envelope.logMonoTime == source_ns and state_envelope.valid == state.canValid assert output_envelope.logMonoTime == source_ns + # Deliver the actual raw health first at this deliberately phased poll. + if first_cancel and cancel_health_order == 'health_first': + sd.sm.update_msgs(source_ns / 1e9, [pandas.as_reader()]) # Exact captured envelopes, including validity and published body. axis, host, command = drive(tick, intent_message=received['aolIntentWire'], car_state_message=state_envelope, car_output_message=output_envelope, @@ -324,7 +354,8 @@ def _exercise_join(identity, alternate, incidental, withdrawal, monkeypatch, *, allowed=bool(mask & 1), long_allowed=bool(mask & 2), native_request_lat=bool(actual_request & 1), native_request_long=bool(actual_request & 2), panda_controls_allowed=native.get_controls_allowed(), panda_fault=rx_invalid, relay_fault=relay_fault, - panda_state_message=pandas if two_panda else None, safety_message=wire if two_panda else None) + panda_state_message=pandas if two_panda or cancel_health_order else None, + safety_message=wire if two_panda or cancel_health_order else None) if sd.conditional_car_state_valid: assert sd.aol_car_state_log_ns == state_envelope.logMonoTime assert controls.sm.logMonoTime['carState'] == state_envelope.logMonoTime @@ -336,7 +367,7 @@ def _exercise_join(identity, alternate, incidental, withdrawal, monkeypatch, *, previous_sd.update(sd.pm.messages) validated = current_native(sd.sm, sd.CP, now_ns=source_ns, axis_session_id=sd.aol_session_id) bootstrap_mismatch = False - if two_panda: + if two_panda or cancel_health_order: source_report = decode_safety(wire.aolSafetyWire) assert source_report is not None bootstrap_mismatch = source_report.axisSessionId != sd.aol_session_id @@ -409,7 +440,14 @@ def _exercise_join(identity, alternate, incidental, withdrawal, monkeypatch, *, ('HKG_INDEPENDENT_LATCH_DROPPED', cancel_diagnostics[-1:]) if two_panda else 'HKG_INDEPENDENT_LATCH_DROPPED') assert native.aol_get_permission_mask() & 1, 'HKG_NATIVE_LATERAL_DROPPED' assert axis.lateralActive and command.latActive, 'HKG_JOINED_LATERAL_DROPPED' - assert not axis.longitudinalActive and not command.longActive and not host.enabled + assert not axis.longitudinalActive and not command.longActive, ( + 'LONG_WITHDRAWAL_NOT_APPLIED', cancel_health_order, now[0], + decode_safety(sd.sm['aolSafetyWire']), sd.sm.logMonoTime['pandaStates'], + [(p.controlsAllowed, p.safetyParam) for p in sd.sm['pandaStates']]) + if cancel_health_order == 'health_first' and held_button_frames: + assert host.enabled and not native.get_controls_allowed() + else: + assert not host.enabled nonzero = False for _ in range(20): state, axis, host, command, steering = step() @@ -424,7 +462,7 @@ def _exercise_join(identity, alternate, incidental, withdrawal, monkeypatch, *, assert steering if two_panda: assert cancel_probe_hits, ('Actual retained CANCEL negatives were not exercised', cancel_diagnostics) - if shared_long and (not two_panda or incidental == 'main'): + if shared_long and identity != CAR.KIA_EV6 and (not two_panda or incidental == 'main'): # MAIN has no ordinary LONG-cancel meaning in this owner. Exercise it # after real CANCEL; never fabricate a MAIN-driven LONG transition. for _ in range(2): @@ -839,3 +877,14 @@ def test_mixed_stock_cruise_off_keeps_only_fresh_permitted_lateral(topology, fau assert not axis.lateralActive and not command.latActive finally: case.doCleanups() + + +@pytest.mark.parametrize('identity', (CAR.KIA_EV6, CAR.HYUNDAI_IONIQ_5, CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN)) +@pytest.mark.parametrize('order', ('mask_first', 'health_first')) +@pytest.mark.parametrize('main_binding', (0, 10)) +def test_cancel_keeps_shared_lateral_with_delayed_native_receipts(identity, order, main_binding, monkeypatch): + if _native_variant() == 'release': + _release_long_denial(alternate=False) + else: + _exercise_join(identity, False, 'cancel', 'toggle-off', monkeypatch, + shared_long=True, cancel_health_order=order, main_binding=main_binding) diff --git a/openpilot/starpilot/aol/tests/test_hkg_early_long_withdrawal.py b/openpilot/starpilot/aol/tests/test_hkg_early_long_withdrawal.py index 66d627feaf..85159b2575 100644 --- a/openpilot/starpilot/aol/tests/test_hkg_early_long_withdrawal.py +++ b/openpilot/starpilot/aol/tests/test_hkg_early_long_withdrawal.py @@ -246,3 +246,65 @@ def test_actual_classic_card_keeps_latch_through_steering_brake_and_cancel(monke assert receive(BASE + 130_000_000, state=sd.CS_prev.as_builder(), axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents']) assert card.aol_card_intent.allowed_latch + + +@pytest.mark.parametrize('fault', ('none', 'lat_denied', 'lat_request_off', 'long_denied', 'long_request_off', + 'old_health', 'raw_enabled', 'native_disabled', 'expired', 'session', 'native_invalid', + 'raw_unseen', 'raw_invalid', 'rx', 'heartbeat', 'hardware', 'native_hardware', + 'wrong_word', 'stock', 'can_invalid', 'eps_fault')) +def test_raw_long_health_handoff_preserves_every_authority_veto(fault): + from dataclasses import asdict + from openpilot.starpilot.aol.tests.test_hkg_shared_cancel_guard import captured_context + from openpilot.starpilot.car.hyundai.aol import raw_longitudinal_withdrawal + + cp, cs, sm, feedback, _, native, now = captured_context() + native = replace(native, observedMonoTime=now - 10_000_000, validUntilMonoTime=now + 190_000_000, + longitudinalAllowed=True, sourcePandaStatesMonoTime=now - 10_000_000, + pandaInventory=(replace(native.pandaInventory[0], controlsAllowed=True),)) + session = feedback.session + if fault in ('lat_denied', 'lat_request_off', 'long_denied', 'long_request_off'): + name = {'lat_denied': 'lateralAllowed', 'lat_request_off': 'requestedLateral', + 'long_denied': 'longitudinalAllowed', 'long_request_off': 'requestedLongitudinal'}[fault] + native = replace(native, **{name: False}) + elif fault == 'old_health': + sm.logMonoTime['pandaStates'] = native.sourcePandaStatesMonoTime + elif fault == 'native_disabled': + native = replace(native, pandaInventory=(replace(native.pandaInventory[0], controlsAllowed=False),)) + elif fault == 'expired': + now += 201_000_000 + elif fault == 'session': + session = 'another-owner' + elif fault == 'native_invalid': + sm.valid['aolSafetyWire'] = False + elif fault == 'raw_unseen': + sm.seen['pandaStates'] = False + elif fault == 'raw_invalid': + sm.valid['pandaStates'] = False + elif fault == 'native_hardware': + native = replace(native, pandaInventory=(replace(native.pandaInventory[0], faults=4),)) + elif fault == 'wrong_word': + cp.safetyConfigs[0].safetyParam ^= 0x80 + elif fault == 'stock': + cp.openpilotLongitudinalControl = False + elif fault == 'can_invalid': + cs.canValid = False + elif fault == 'eps_fault': + cs.steerFaultTemporary = True + if fault in ('raw_enabled', 'rx', 'heartbeat', 'hardware'): + raw = messaging.new_message('pandaStates', 1, valid=True, logMonoTime=sm.logMonoTime['pandaStates']) + raw.pandaStates[0] = sm['pandaStates'][0] + if fault == 'raw_enabled': + raw.pandaStates[0].controlsAllowed = True + elif fault == 'rx': + raw.pandaStates[0].safetyRxChecksInvalid = True + elif fault == 'heartbeat': + raw.pandaStates[0].heartbeatLost = True + else: + raw.pandaStates[0].faults = ['interruptRateCan1'] + sm['pandaStates'] = raw.as_reader().pandaStates + sm['aolSafetyWire'] = encode_safety(native) + sm.logMonoTime['aolSafetyWire'] = native.observedMonoTime + before = asdict(native) + encoded = sm['aolSafetyWire'] + assert raw_longitudinal_withdrawal(cp, cs, sm, native, session=session, now_ns=now) == (fault == 'none') + assert asdict(native) == before and sm['aolSafetyWire'] == encoded diff --git a/openpilot/starpilot/aol/transport_pause.py b/openpilot/starpilot/aol/transport_pause.py index a4e3e33646..7f12feef4d 100644 --- a/openpilot/starpilot/aol/transport_pause.py +++ b/openpilot/starpilot/aol/transport_pause.py @@ -347,7 +347,7 @@ class TransportPauseFeedback: def _native_long_withdrawal_lateral_current(self, sm, cp, cs, axis, now_ns, *, latched, producer): from openpilot.starpilot.nostalgia import physical_cancel from openpilot.starpilot.car.gm.aol import native_accepts_cp - from openpilot.starpilot.car.hyundai.aol import early_native_long_withdrawal_supported, classic_brake_withdrawal + from openpilot.starpilot.car.hyundai.aol import early_native_long_withdrawal_supported, classic_brake_withdrawal, raw_longitudinal_withdrawal gm = cp.brand == 'gm' and native_accepts_cp(cp, int(cp.safetyConfigs[0].safetyModel.raw), int(cp.safetyConfigs[0].safetyParam)) from openpilot.starpilot.car.hyundai.aol import qualified_ccnc_long_aol as ev9_qualified from opendbc.car import structs @@ -404,7 +404,8 @@ class TransportPauseFeedback: all(not slot.controlsAllowed for slot, config in zip(sm['pandaStates'], cp.safetyConfigs, strict=True) if str(config.safetyModel) not in ('silent', 'noOutput'))) if (native is None or not native.pandaInventory or not native.requestedLongitudinal or - native.longitudinalAllowed and not brake_health or + native.longitudinalAllowed and not (brake_health or raw_longitudinal_withdrawal( + cp, cs, sm, native, session=self.session, now_ns=now_ns)) or native.observedMonoTime < self.lateral_native_ns or (native.pandaSerial, tuple(slot.hardwareSerial for slot in native.pandaInventory)) != self.lateral_inventory): return None diff --git a/openpilot/starpilot/car/hyundai/aol.py b/openpilot/starpilot/car/hyundai/aol.py index d1dd500c93..3b04b71360 100644 --- a/openpilot/starpilot/car/hyundai/aol.py +++ b/openpilot/starpilot/car/hyundai/aol.py @@ -333,6 +333,26 @@ def early_native_long_withdrawal_supported(CP) -> bool: CP.carFingerprint == HyundaiCAR.HYUNDAI_PALISADE_2023 and qualified_blended_alpha(CP, marked_only=True))) + +def raw_longitudinal_withdrawal(CP, CS, sm, native, *, session, now_ns) -> bool: + from openpilot.starpilot.aol.transport_pause import independent_lateral_health + + try: + if (CP.brand != 'hyundai' or not CP.openpilotLongitudinalControl or native is None or + not native.requestedLateral or not native.lateralAllowed or not native.requestedLongitudinal or + not native.longitudinalAllowed or not native.pandaInventory or + not 0 < native.sourcePandaStatesMonoTime < int(sm.logMonoTime['pandaStates'])): + return False + index = 0 if len(CP.safetyConfigs) == 1 else config_index(CP) + pandas = sm['pandaStates'] + if (index is None or len(pandas) != len(CP.safetyConfigs) or len(native.pandaInventory) != len(CP.safetyConfigs) or + pandas[index].controlsAllowed or not native.pandaInventory[index].controlsAllowed or + not (early_native_long_withdrawal_supported(CP) or qualified_ccnc_long_aol(CP, marked_only=True))): + return False + return independent_lateral_health(sm, CP, CS, session, now_ns) == native + except (AttributeError, KeyError, IndexError, TypeError, ValueError, OverflowError): + return False + def classic_brake_withdrawal(CP, CS) -> bool: return bool(CP.brand == 'hyundai' and CS.brakePressed and not CS.gasPressed and not CS.regenBraking and CS.cruiseState.available and not CS.doorOpen and not CS.seatbeltUnlatched and not CS.vehicleSensorsInvalid and diff --git a/prebuilt.json b/prebuilt.json index d9ef55c7cd..a58a1cbc82 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "c2b3a94f2ea1020a781a1479e766cc428e991a06ad026b1ef4a9d467f2b681a5", "schema_version": 1} +{"protected_inventory_sha256": "2fec230e89e99e049a0c7888fd333e6ceb7109f7bf9cabf64a438c59abc8156a", "schema_version": 1}