Keep shared HKG lateral through longitudinal cancel handoffs

This commit is contained in:
firestar5683
2026-11-11 11:51:01 -06:00
parent 0408331664
commit 208ef1d8cd
7 changed files with 149 additions and 18 deletions
+1 -4
View File
@@ -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.
@@ -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,
@@ -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)
@@ -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
+3 -2
View File
@@ -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
+20
View File
@@ -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
+1 -1
View File
@@ -1 +1 @@
{"protected_inventory_sha256": "c2b3a94f2ea1020a781a1479e766cc428e991a06ad026b1ef4a9d467f2b681a5", "schema_version": 1}
{"protected_inventory_sha256": "2fec230e89e99e049a0c7888fd333e6ceb7109f7bf9cabf64a438c59abc8156a", "schema_version": 1}