diff --git a/openpilot/selfdrive/selfdrived/selfdrived.py b/openpilot/selfdrive/selfdrived/selfdrived.py index cf2c3ccd33..357e2d3380 100755 --- a/openpilot/selfdrive/selfdrived/selfdrived.py +++ b/openpilot/selfdrive/selfdrived/selfdrived.py @@ -5,6 +5,7 @@ import time import threading import uuid from copy import copy +from dataclasses import replace from collections import deque import openpilot.cereal.messaging as messaging @@ -33,6 +34,7 @@ from openpilot.selfdrive.selfdrived.state import StateMachine from opendbc.car.gm.aol import native_bootstrap_supported from opendbc.car.gm.steering_authority import monitored_profile from openpilot.starpilot.car.gm.steering_companion import SteeringCompanion +from openpilot.starpilot.car.hyundai.aol import qualified_ioniq6 from opendbc.car.gm.values import GMFlags, is_volt_ascm_longitudinal from openpilot.starpilot.aol.intent import read_settings from openpilot.starpilot.aol.intent_companion import IntentCompanion @@ -341,6 +343,8 @@ class SelfdriveD: """Compute onroadEvents from carState""" self.events.clear() + self.car_event_sample_count = 0 + self.aol_reconciled_sources = () self.tesla_stock_alerts = [] self.nostalgia_paddle_cancel = False action_owner = getattr(self, 'controller_action_owner', None) @@ -508,18 +512,51 @@ class SelfdriveD: hardware_events_added = len(self.events) - num_events CS = self.refresh_event_car_state(CS) - self.event_car_state = CS - self._update_steering_authority(CS) - samples = getattr(self, 'car_state_samples', ()) or ((CS, getattr(self, 'conditional_car_state_valid', False)),) + input_proof = getattr(self, 'aol_step_input_proof', None) + reconcile_input = bool(input_proof is not None and not REPLAY and self.aol_replay and self.axis_transport_required and + self.CP.openpilotLongitudinalControl and qualified_ioniq6(self.CP) and + input_proof[0] == self.aol_session_id and + (input_proof[8] or input_proof[9]) and time.monotonic_ns() <= input_proof[3]) + if not reconcile_input: + # Other vehicle owners retain their original ordering. In particular, + # GM's exact steering companion must not lose an earlier source's fault + # when a newer carState arrives during car-event processing. + self._update_steering_authority(CS) previous = self.CS_prev - for sample, envelope_valid in samples: - self._update_car_event_sample(sample, previous) - if len(samples) > 1: + retained_source = (getattr(self, 'car_state_retained_source_ns', 0) + if not getattr(self, 'car_state_samples', ()) else 0) + consumed = 0 + envelope_errors = [] + while True: + received = getattr(self, 'car_state_samples', ()) + samples = received[consumed:] if received else ((CS, getattr(self, 'conditional_car_state_valid', False)),) + for sample, envelope_valid in samples: + self._update_car_event_sample(sample, previous) + self.car_event_sample_count += 1 if sample.canTimeout: - self.events.add(EventName.canBusMissing) - elif not envelope_valid or not sample.canValid: - self.events.add(EventName.canError) - previous = sample + envelope_errors.append(EventName.canBusMissing) + elif (received and not envelope_valid) or not sample.canValid: + envelope_errors.append(EventName.canError) + previous = sample + consumed = len(received) + if not reconcile_input: + break + # Input can arrive while car-event owners run. Reconcile it before the + # selected-source owners and authority/state-machine decisions. The drain + # retains one total 32-envelope bound and the original output deadline; + # the cursor ensures no stateful car-event owner sees a sample twice. + CS = self.refresh_event_car_state(CS) + if len(getattr(self, 'car_state_samples', ())) <= consumed: + break + self.aol_reconciled_sources = ((retained_source,) if retained_source else ()) + tuple( + int(sample.logMonoTime) for sample in self.car_state_envelopes) + samples = getattr(self, 'car_state_samples', ()) or ((CS, getattr(self, 'conditional_car_state_valid', False)),) + if self.car_event_sample_count > 1: + for event in envelope_errors: + self.events.add(event) + self.event_car_state = CS + if reconcile_input: + self._update_steering_authority(CS) if any(physical_cancel(sample) for sample, _ in samples): self.nostalgia_paddle_cancel = False if CS.canValid: @@ -864,6 +901,38 @@ class SelfdriveD: CS = latest.carState return CS + def reconciled_intent(self, intent, previous, pairs, epoch, now_ns): + sources = getattr(self, 'aol_reconciled_sources', ()) + if not sources or intent is None: + return intent + companion = self.aol_intent_companion + proof = getattr(self, 'aol_step_input_proof', None) + if (proof is None or proof[0] != self.aol_session_id or companion is None or + epoch != proof[5] or companion.integrity_epoch != epoch or + intent.producerSessionId != proof[4]): + return None + for source_ns in sources: + pair = pairs.get(source_ns) + if (pair is None and previous is not None and source_ns == proof[1] and + previous.carStateLogMonoTime == source_ns and previous.producerSessionId == proof[4]): + pair = (source_ns, True, previous) + if pair is None or not pair[1] or pair[2] is None: + return None + restriction = pair[2] + if (pair[0] != source_ns or restriction.carStateLogMonoTime != source_ns or + not restriction.settingsQualified or restriction.producerSessionId != intent.producerSessionId or + not 0 < source_ns <= now_ns or not 0 < restriction.observedMonoTime <= now_ns <= restriction.validUntilMonoTime or + not 0 < restriction.validUntilMonoTime - restriction.observedMonoTime <= SAFETY_MAX_AGE_NS): + return None + # Earlier exact companions only veto the normally validated fresh pair. + # Their clocks never renew a source or supply an output lease. + intent = replace(intent, allowedLatch=intent.allowedLatch and restriction.allowedLatch, + lateralArmed=intent.lateralArmed and restriction.lateralArmed, + pauseLateral=intent.pauseLateral or restriction.pauseLateral, + pauseLongitudinal=intent.pauseLongitudinal or restriction.pauseLongitudinal, + optionalSetRelease=intent.optionalSetRelease and restriction.optionalSetRelease) + return intent + def update_alerts(self, CS): clear_event_types = set() if ET.WARNING not in self.state_machine.current_alert_types: @@ -1340,6 +1409,8 @@ class SelfdriveD: previous_fault_reason = getattr(self, 'aol_fault_reason', 'none') self.aol_step_input_proof = getattr(self, 'aol_published_input_proof', None) CS = self.data_sample() + self.car_event_sample_count = 0 + self.aol_reconciled_sources = () self.event_car_state = CS self.event_input_refresh_pending = True try: @@ -1347,7 +1418,7 @@ class SelfdriveD: finally: self.event_input_refresh_pending = False CS = self.event_car_state - if len(getattr(self, 'car_state_samples', ())) > 1 and any(self.events.contains(kind) for kind in ( + if max(self.car_event_sample_count, len(getattr(self, 'car_state_samples', ()))) > 1 and any(self.events.contains(kind) for kind in ( ET.USER_DISABLE, ET.SOFT_DISABLE, ET.IMMEDIATE_DISABLE)): # A queued enable cannot override a cancellation or pedal edge in this batch. self.events.events = [event for event in self.events.names if ET.ENABLE not in self.events.event_rules.get(event, {})] @@ -1365,6 +1436,8 @@ class SelfdriveD: self.aol_decision_native = native if self.aol_replay: previous_intent = getattr(self, 'aol_last_intent', None) + reconciled_pairs = dict(intent_companion.samples) if intent_companion is not None and self.aol_reconciled_sources else {} + reconciled_epoch = intent_companion.integrity_epoch if intent_companion is not None else -1 source_pair = intent_companion.samples.get(self.aol_car_state_log_ns) if intent_companion is not None else None verified = intent_companion.verified_sm_projection if intent_companion is not None else None if (intent_companion is not None and source_pair is None and verified is not None and @@ -1384,6 +1457,7 @@ class SelfdriveD: intent = current_intent(self.sm, car_state_ns=self.aol_car_state_log_ns, now_ns=now_ns, previous=getattr(self, 'aol_last_intent', None), companion=intent_companion, drain=False) if intent_drained else None + intent = self.reconciled_intent(intent, previous_intent, reconciled_pairs, reconciled_epoch, now_ns) self.aol_last_intent = intent if self.sm['driverMonitoringState'].alertLevel == AlertLevel.three or self.sm['driverMonitoringState'].lockout: self.aol_dm_lateral_inhibit = True diff --git a/openpilot/starpilot/aol/tests/test_event_input_reconciliation.py b/openpilot/starpilot/aol/tests/test_event_input_reconciliation.py new file mode 100644 index 0000000000..771e132081 --- /dev/null +++ b/openpilot/starpilot/aol/tests/test_event_input_reconciliation.py @@ -0,0 +1,245 @@ +from collections import deque +from contextlib import ExitStack +from unittest import mock + +import pytest + +from openpilot.cereal import log, messaging +from openpilot.common.prefix import OpenpilotPrefix +from opendbc.car.structs import car +from openpilot.starpilot.aol.tests import test_ioniq_availability as fixture +from openpilot.starpilot.aol.tests.test_carstate_queue import message +from openpilot.starpilot.aol.tests.test_input_deadline_continuity import BASE, intent + + +@pytest.mark.parametrize('timeout', [False, True]) +@pytest.mark.parametrize('full_op', [False, True]) +@pytest.mark.parametrize('pulse', ['healthy', 'cancel_then_enable', 'brake_then_clear', 'permanent_eps', 'invalid_then_valid']) +def test_input_arriving_during_car_event_work_is_reconciled_once(full_op, pulse, timeout): + case = fixture.IoniqAvailabilityTests(methodName='runTest') + with ExitStack() as cleanup: + cleanup.callback(case.doCleanups) + with OpenpilotPrefix(): + sd, _, step, _, _ = case._ioniq_availability_callers(full_op=full_op) + first, _, command = step(2, offset_ns=3_720_071, state_stamp=BASE, intent_message=intent(BASE, 3), gas=not full_op) + assert first.lateralActive and command.latActive and first.longitudinalActive == full_op + deadline = BASE + 33_720_071 + sample_age = 30_100_000 if timeout else 27_000_000 + now = [BASE + sample_age] + sd.car_state_poller = mock.Mock() + sd.car_state_poller.poll.return_value = [] + fresh = BASE + 28_254_175 + queue, wires = deque(), deque() + original_receive = messaging.recv_one_or_none + original_car_update = sd._update_car_event_sample + injected = False + + def receive(sock): + if sock is sd.car_state_sock: + return queue.popleft() if queue else None + if sock is sd.aol_intent_companion.sock: + return wires.popleft() if wires else None + return original_receive(sock) + + def car_update(sample, previous): + nonlocal injected + result = original_car_update(sample, previous) + if not injected: + injected = True + late = fixture.car_state() + late.cruiseState.available = True + late.gasPressed = not full_op + late.vEgo = 25. + if pulse == 'cancel_then_enable': + late.buttonEvents = [car.CarState.ButtonEvent(type=car.CarState.ButtonEvent.Type.cancel, pressed=True)] + elif pulse == 'brake_then_clear': + late.brakePressed = True + elif pulse == 'permanent_eps': + late.steerFaultPermanent = True + queue.append(message(fresh, late, valid=pulse != 'invalid_then_valid')) + wires.append(intent(fresh, 4, latched=pulse != 'cancel_then_enable').as_reader()) + if pulse in ('cancel_then_enable', 'brake_then_clear', 'invalid_then_valid'): + cleared = fixture.car_state() + cleared.cruiseState.available = True + cleared.gasPressed = not full_op + cleared.vEgo = 26. + cleared.buttonEnable = pulse == 'cancel_then_enable' + queue.append(message(fresh + 100_000, cleared)) + wires.append(intent(fresh + 100_000, 5).as_reader()) + now[0] = BASE + 30_253_011 + return result + + with mock.patch('openpilot.selfdrive.selfdrived.selfdrived.time.monotonic_ns', side_effect=lambda: now[0]), \ + mock.patch('openpilot.selfdrive.selfdrived.selfdrived.messaging.recv_one_or_none', side_effect=receive), \ + mock.patch.object(sd, '_update_car_event_sample', side_effect=car_update) as car_events, \ + mock.patch.object(sd.state_machine, 'update', wraps=sd.state_machine.update) as transition, \ + mock.patch.object(sd.excessive_actuation_check, 'update', wraps=sd.excessive_actuation_check.update) as excessive: + axis, _, command = step(4, timeout=timeout, offset_ns=sample_age - 20_000_000, state_stamp=BASE, + intent_message=intent(BASE, 3), gas=not full_op) + assert now[0] < deadline and not queue + assert transition.call_count == 1 + assert excessive.call_count == (1 if sd.calibrated_pose is not None else 0) + assert car_events.call_count == (3 if pulse.endswith(('enable', 'clear', 'valid')) else 2) + if pulse == 'healthy': + assert axis.sourceCarStateMonoTime == fresh and sd.aol_last_intent.carStateLogMonoTime == fresh + assert axis.lateralActive and command.latActive + assert axis.longitudinalActive == full_op and command.longActive == full_op + assert sd.enabled == full_op and not sd.aol_authority_lost + else: + assert not axis.longitudinalActive and not command.longActive and not sd.enabled + if pulse != 'brake_then_clear': + assert not axis.lateralActive and not command.latActive + if pulse == 'cancel_then_enable': + assert log.OnroadEvent.EventName.buttonCancel in sd.events.names + assert not sd.events.contains(fixture.ET.ENABLE) + if pulse == 'brake_then_clear': + assert log.OnroadEvent.EventName.pedalPressed in sd.events.names + if pulse == 'invalid_then_valid': + assert axis.sourceCarStateMonoTime == 0 and not sd.conditional_car_state_valid + + +def changed_intent(stamp, sequence, **changes): + from dataclasses import replace + from openpilot.starpilot.aol.wire import decode_intent, encode_intent + wire = intent(stamp, sequence) + decoded = decode_intent(wire.aolIntentWire) + assert decoded is not None + wire.aolIntentWire = encode_intent(replace(decoded, **changes)) + return wire + + +@pytest.mark.parametrize('restriction', ['pauseLongitudinal', 'pauseLateral', 'allowedLatch', 'lateralArmed', + 'settingsQualified', 'missing', 'producer', 'session', 'conflict', 'future', 'intermediate']) +def test_reconciliation_preserves_every_exact_intent_restriction(restriction): + case = fixture.IoniqAvailabilityTests(methodName='runTest') + with ExitStack() as cleanup: + cleanup.callback(case.doCleanups) + with OpenpilotPrefix(): + full_op = restriction not in ('allowedLatch', 'lateralArmed') + sd, _, step, _, _ = case._ioniq_availability_callers(full_op=full_op) + step(2, offset_ns=3_720_071, state_stamp=BASE, intent_message=intent(BASE, 3)) + a, b, c = BASE + 20_000_000, BASE + 25_000_000, BASE + 26_000_000 + changes = ({restriction: restriction.startswith('pause')} if restriction in + ('pauseLongitudinal', 'pauseLateral', 'allowedLatch', 'lateralArmed', 'settingsQualified') else {}) + if restriction == 'lateralArmed': + changes['allowedLatch'] = False + if restriction == 'future': + changes['observedMonoTime'] = BASE + 40_000_000 + original_intent = changed_intent(a, 4, **changes) + fresh_intent = changed_intent(b, 5, **({'producerSessionId': 'replacement-card'} if restriction == 'producer' else {})) + if restriction == 'intermediate': + fresh_intent = changed_intent(b, 5, pauseLongitudinal=True) + latest = intent(c, 6) if restriction == 'intermediate' else fresh_intent + wires = deque([] if restriction == 'missing' else [original_intent.as_reader()]) + if restriction == 'conflict': + wires.append(changed_intent(a, 4, pauseLongitudinal=True).as_reader()) + wires.append(fresh_intent.as_reader()) + if restriction == 'intermediate': + wires.append(latest.as_reader()) + queue = deque() + now = [BASE + 27_000_000] + original_receive, original_car_update, original_health = messaging.recv_one_or_none, sd._update_car_event_sample, sd.update_process_health + injected = False + + def health(): + result = original_health() + sd.sm.update_msgs(now[0] / 1e9, [latest.as_reader()]) + return result + + def receive(sock): + if sock is sd.car_state_sock: + return queue.popleft() if queue else None + if sock is sd.aol_intent_companion.sock: + return wires.popleft() if wires else None + return original_receive(sock) + + def car_update(sample, previous): + nonlocal injected + result = original_car_update(sample, previous) + if not injected: + injected = True + if restriction == 'session': + sd.aol_session_id = 'replacement-selfdrived' + for stamp in ((b, c) if restriction == 'intermediate' else (b,)): + late = fixture.car_state() + late.cruiseState.available = True + queue.append(message(stamp, late)) + now[0] = BASE + 29_000_000 + return result + + with mock.patch('openpilot.selfdrive.selfdrived.selfdrived.time.monotonic_ns', side_effect=lambda: now[0]), \ + mock.patch('openpilot.selfdrive.selfdrived.selfdrived.messaging.recv_one_or_none', side_effect=receive), \ + mock.patch.object(sd, '_update_car_event_sample', side_effect=car_update), \ + mock.patch.object(sd, 'update_process_health', side_effect=health): + axis, _, command = step(4, offset_ns=7_000_000, state_stamp=a, intent_message=original_intent) + assert not queue + if restriction in ('pauseLongitudinal', 'intermediate'): + assert sd.aol_last_intent is not None and sd.aol_last_intent.pauseLongitudinal + assert axis.lateralActive and command.latActive and not axis.longitudinalActive and not command.longActive + elif restriction == 'pauseLateral': + assert sd.aol_last_intent is not None and sd.aol_last_intent.pauseLateral + assert not axis.lateralActive and not command.latActive and axis.longitudinalActive and command.longActive + elif restriction in ('allowedLatch', 'lateralArmed'): + assert sd.aol_last_intent is not None and not getattr(sd.aol_last_intent, restriction) + assert not axis.lateralActive and not command.latActive + else: + assert sd.aol_last_intent is None + assert not axis.lateralActive and not axis.longitudinalActive and not command.latActive and not command.longActive + + +@pytest.mark.parametrize('companion_case', ['healthy', 'unavailable', 'missing', 'invalid']) +def test_gm_companion_ordering_is_unchanged_by_ioniq_reconciliation(companion_case, monkeypatch): + from opendbc.car.gm.tests.test_bolt_volt_configurations import ordinary_params + from opendbc.car.gm.values import CAR + from openpilot.starpilot.car.gm.steering_companion import SteeringCompanion + monkeypatch.setenv('SIMULATION', '0') + case = fixture.IoniqAvailabilityTests(methodName='runTest') + with ExitStack() as cleanup: + cleanup.callback(case.doCleanups) + with OpenpilotPrefix(): + cp = ordinary_params(CAR.CHEVROLET_VOLT_ASCM, alpha=True, sascm=True, radar=True) + sd, _, step = case._authority_callers(True, cp=cp, genuine_events=True) + sd.steering_companion = SteeringCompanion() + assert step(2, state_stamp=BASE, intent_message=intent(BASE, 3))[2].longActive + a, b = BASE + 20_000_000, BASE + 25_000_000 + now = [BASE + 27_000_000] + queue, wires = deque(), deque([intent(a, 4).as_reader()]) + original_receive, original_car_update, original_health = messaging.recv_one_or_none, sd._update_car_event_sample, sd.update_process_health + + def health(): + result = original_health() + for stamp in (a, b): + companion = messaging.new_message('starpilotCarState', valid=True, logMonoTime=stamp) + companion.starpilotCarState.sourceCarStateMonoTime = stamp + companion.starpilotCarState.lateralAuthorityUnavailable = stamp == a and companion_case == 'unavailable' + if stamp == a and companion_case != 'missing': + sd.steering_companion._observe(stamp, companion_case != 'invalid', companion.starpilotCarState) + if stamp == b: + sd.sm.update_msgs(now[0] / 1e9, [companion.as_reader()]) + return result + + def receive(sock): + if sock is sd.car_state_sock: + return queue.popleft() if queue else None + if sock is sd.aol_intent_companion.sock: + return wires.popleft() if wires else None + return original_receive(sock) + + def car_update(sample, previous): + result = original_car_update(sample, previous) + late = fixture.car_state() + late.cruiseState.available = True + queue.append(message(b, late)) + wires.append(intent(b, 5).as_reader()) + now[0] = BASE + 29_000_000 + return result + + with mock.patch('openpilot.selfdrive.selfdrived.selfdrived.time.monotonic_ns', side_effect=lambda: now[0]), \ + mock.patch('openpilot.selfdrive.selfdrived.selfdrived.messaging.recv_one_or_none', side_effect=receive), \ + mock.patch.object(sd, '_update_car_event_sample', side_effect=car_update), \ + mock.patch.object(sd, 'update_process_health', side_effect=health): + axis, _, command = step(4, offset_ns=7_000_000, state_stamp=a, intent_message=intent(a, 4)) + assert len(queue) == 1 and axis.sourceCarStateMonoTime == a + assert command.latActive == (companion_case == 'healthy') + assert command.longActive == (companion_case == 'healthy') + assert (log.OnroadEvent.EventName.controlsMismatch in sd.events.names) == (companion_case != 'healthy') diff --git a/openpilot/starpilot/controllers/tests/test_wheel_media_audit.py b/openpilot/starpilot/controllers/tests/test_wheel_media_audit.py new file mode 100644 index 0000000000..c5a0ee7bb2 --- /dev/null +++ b/openpilot/starpilot/controllers/tests/test_wheel_media_audit.py @@ -0,0 +1,184 @@ +from pathlib import Path +from unittest import mock + +import pytest + +from opendbc.car.hyundai.ioniq6_media import Ioniq6MediaButtons +from opendbc.car.hyundai.tests.test_ioniq6_media import packet +from openpilot.starpilot.conditional_mode.manual import MEDIA_LONG_NS +from openpilot.starpilot.controllers import wheel_actions +from openpilot.starpilot.controllers.tests.test_wheel_actions import DRIVE, NOW, context as context, state + + +def observe(owner, source, params, cp, stamp, *, mode=False, custom=False, media_packet=True): + media = source.update([packet(stamp, mode=mode, custom=custom) if media_packet else (stamp, [])]) + return owner.observe(params, cp, state(), now_ns=stamp, drive_id=DRIVE, media=media) + + +def test_neutral_media_packets_do_not_scan_fourteen_params_each_time(context): + params, cp = context + owner, source = wheel_actions.WheelPublisher(params), Ioniq6MediaButtons(1) + with mock.patch.object(wheel_actions, 'read_saved', wraps=wheel_actions.read_saved) as reads: + for frame in range(200): + commands = observe(owner, source, params, cp, NOW + frame * 10_000_000, media_packet=frame % 20 == 0) + assert not any(action for _, action in commands) + # The one-second audit remains; the ten 5 Hz neutral packets are not edges. + assert reads.call_count == 2 * len(wheel_actions.KEYS) + + +@pytest.mark.parametrize('custom,key', [(False, 'ModeButtonControl'), (True, 'StarButtonControl')]) +def test_real_media_edges_refresh_mapping_and_emit_release_once(context, custom, key): + params, cp = context + params.put(key, 8, block=True) + owner, source = wheel_actions.WheelPublisher(params), Ioniq6MediaButtons(1) + observe(owner, source, params, cp, NOW) + params.put(key, 11, block=True) + with mock.patch.object(wheel_actions, 'capture', wraps=wheel_actions.capture) as capture: + assert not any(action for _, action in observe(owner, source, params, cp, NOW + 100_000_000, + mode=not custom, custom=custom)) + assert capture.call_count == 1 + # A changed mapping cancels the current gesture and requires new neutral. + assert not any(action for _, action in observe(owner, source, params, cp, NOW + 200_000_000)) + assert capture.call_count == 2 + assert not any(action for _, action in observe(owner, source, params, cp, NOW + 300_000_000)) + assert capture.call_count == 2 + observe(owner, source, params, cp, NOW + 400_000_000, mode=not custom, custom=custom) + assert observe(owner, source, params, cp, NOW + 500_000_000) == ((key, 11),) + assert capture.call_count == 4 + + +def test_transient_mapping_change_during_hold_cancels_the_gesture(context): + params, cp = context + params.put('ModeButtonControl', 8, block=True) + owner, source = wheel_actions.WheelPublisher(params), Ioniq6MediaButtons(1) + observe(owner, source, params, cp, NOW) + observe(owner, source, params, cp, NOW + 10_000_000, mode=True) + params.put('ModeButtonControl', 11, block=True) + observe(owner, source, params, cp, NOW + 20_000_000, mode=True) + params.put('ModeButtonControl', 8, block=True) + observe(owner, source, params, cp, NOW + 30_000_000, mode=True) + assert not any(action for _, action in observe(owner, source, params, cp, NOW + 40_000_000)) + + +def test_invalid_media_and_new_epoch_cannot_release_an_old_gesture(context): + params, cp = context + params.put('ModeButtonControl', 8, block=True) + owner, source = wheel_actions.WheelPublisher(params), Ioniq6MediaButtons(1) + observe(owner, source, params, cp, NOW) + observe(owner, source, params, cp, NOW + 100_000_000, mode=True) + invalid = source.update([]) + assert not invalid.valid + assert not any(action for _, action in owner.observe(params, cp, state(), now_ns=NOW + 110_000_000, + drive_id=DRIVE, media=invalid)) + # A held-at-restart input cannot finish the pre-fault gesture. + assert not any(action for _, action in observe(owner, source, params, cp, NOW + 200_000_000, mode=True)) + assert not any(action for _, action in observe(owner, source, params, cp, NOW + 300_000_000)) + observe(owner, source, params, cp, NOW + 400_000_000, mode=True) + assert observe(owner, source, params, cp, NOW + 500_000_000) == (('ModeButtonControl', 8),) + + + +def test_press_release_in_one_media_batch_is_not_lost(context): + params, cp = context + params.put('ModeButtonControl', 8, block=True) + owner, source = wheel_actions.WheelPublisher(params), Ioniq6MediaButtons(1) + observe(owner, source, params, cp, NOW) + batch = source.update([packet(NOW + 100_000_000, mode=True), packet(NOW + 200_000_000)]) + assert owner.observe(params, cp, state(), now_ns=NOW + 200_000_000, drive_id=DRIVE, media=batch) == (('ModeButtonControl', 8),) + + +@pytest.mark.parametrize('long_press', [False, True]) +def test_invalid_saved_mapping_cannot_emit_media_command(context, long_press): + params, cp = context + key = 'LongModeButtonControl' if long_press else 'ModeButtonControl' + params.put(key, 8, block=True) + owner, source = wheel_actions.WheelPublisher(params), Ioniq6MediaButtons(1) + observe(owner, source, params, cp, NOW) + pressed = NOW + 10_000_000 + observe(owner, source, params, cp, pressed, mode=True) + Path(params.get_param_path(key)).write_bytes(b'invalid') + stop = MEDIA_LONG_NS if long_press else 100_000_000 + for elapsed in range(10_000_000, stop + 1, 10_000_000): + commands = observe(owner, source, params, cp, pressed + elapsed, mode=long_press or elapsed < stop) + assert not any(action for _, action in commands) + + +def test_off_to_on_mapping_during_hold_cannot_authorize_that_gesture(context): + params, cp = context + params.put('LongModeButtonControl', 0, block=True) + owner, source = wheel_actions.WheelPublisher(params), Ioniq6MediaButtons(1) + observe(owner, source, params, cp, NOW) + pressed = NOW + 10_000_000 + observe(owner, source, params, cp, pressed, mode=True) + params.put('LongModeButtonControl', 8, block=True) + for elapsed in range(10_000_000, MEDIA_LONG_NS + 20_000_000, 10_000_000): + assert not any(action for _, action in observe(owner, source, params, cp, pressed + elapsed, mode=True)) + assert not any(action for _, action in observe(owner, source, params, cp, pressed + MEDIA_LONG_NS + 20_000_000)) + + +def test_one_second_audit_detects_map_change_without_edge(context): + params, cp = context + owner, source = wheel_actions.WheelPublisher(params), Ioniq6MediaButtons(1) + observe(owner, source, params, cp, NOW) + params.put('ModeButtonControl', 8, block=True) + for frame in range(1, 101): + assert not any(action for _, action in observe(owner, source, params, cp, NOW + frame * 10_000_000, + media_packet=frame % 20 == 0)) + assert owner.mapping is not None + assert owner.mapping['ModeButtonControl'] == 8 + assert owner.checked_ns == NOW + 1_000_000_000 + + +def test_new_drive_while_held_cannot_release_previous_gesture(context): + params, cp = context + params.put('ModeButtonControl', 8, block=True) + owner, source = wheel_actions.WheelPublisher(params), Ioniq6MediaButtons(1) + observe(owner, source, params, cp, NOW) + observe(owner, source, params, cp, NOW + 100_000_000, mode=True) + for offset, held in ((200_000_000, True), (300_000_000, False)): + media = source.update([packet(NOW + offset, mode=held)]) + assert not any(action for _, action in owner.observe(params, cp, state(), now_ns=NOW + offset, + drive_id=DRIVE + 1, media=media)) + + +def test_no_media_still_audits_on_car_button_edges(context): + params, cp = context + owner = wheel_actions.WheelPublisher(params) + owner.observe(params, cp, state(), now_ns=NOW, drive_id=DRIVE) + with mock.patch.object(wheel_actions, 'capture', wraps=wheel_actions.capture) as capture: + owner.observe(params, cp, state(True), now_ns=NOW + 10_000_000, drive_id=DRIVE) + assert owner.observe(params, cp, state(False), now_ns=NOW + 20_000_000, drive_id=DRIVE) == (('DistanceButtonControl', 1),) + assert capture.call_count == 2 + + +@pytest.mark.parametrize('key,button', [('DistanceButtonControl', 'gapAdjustCruise'), + ('CancelButtonControl', 'cancel'), ('MainCruiseButtonControl', 'mainCruise')]) +def test_neutral_media_retains_audits_during_conventional_button_hold(context, key, button): + from opendbc.car.structs import car + params, cp = context + # Main's sole supported command is adopt-speed-limit. + original, changed = (10, 0) if key == 'MainCruiseButtonControl' else (8, 11) + params.put(key, original, block=True) + owner, source = wheel_actions.WheelPublisher(params), Ioniq6MediaButtons(1) + observe(owner, source, params, cp, NOW) + button_type = getattr(car.CarState.ButtonEvent.Type, button) + owner.observe(params, cp, state(True, button=button_type), now_ns=NOW + 10_000_000, drive_id=DRIVE) + params.put(key, changed, block=True) + observe(owner, source, params, cp, NOW + 20_000_000) + params.put(key, original, block=True) + observe(owner, source, params, cp, NOW + 30_000_000) + commands = owner.observe(params, cp, state(False, button=button_type), now_ns=NOW + 40_000_000, drive_id=DRIVE) + assert not any(action for _, action in commands) + + +def test_valid_empty_media_never_turns_into_a_per_frame_audit(context): + from opendbc.car.hyundai.ioniq6_media import MediaObservation + params, cp = context + owner = wheel_actions.WheelPublisher(params) + with mock.patch.object(wheel_actions, 'capture', wraps=wheel_actions.capture) as capture: + for frame in range(200): + now = NOW + frame * 10_000_000 + media = MediaObservation((), now, frame // 20, True) + assert not any(action for _, action in owner.observe(params, cp, state(), now_ns=now, drive_id=DRIVE, media=media)) + assert capture.call_count == 2 + assert owner.media_sample_state is None diff --git a/openpilot/starpilot/controllers/wheel_actions.py b/openpilot/starpilot/controllers/wheel_actions.py index e8212d72e7..01cf825022 100644 --- a/openpilot/starpilot/controllers/wheel_actions.py +++ b/openpilot/starpilot/controllers/wheel_actions.py @@ -78,15 +78,42 @@ class WheelPublisher: self.mapping = None self.last_heartbeat = 0 self.checked_ns = 0 + self.media_sample_state = None self.suppress_distance_release = False initial = capture(params) if params is not None else None self.axis_keys = {key for key, action in (initial or {}).items() if action in (3, 4, 9)} self.initial_axis_read = initial is not None + def media_audit_needed(self, media): + if media is None: + return False + if not media.valid: + self.media_sample_state = None + return bool(media.samples) + if not media.samples: + return False + previous = self.media_sample_state + changed = previous is None or previous[0] != media.source_epoch + for sample in media.samples: + current = (media.source_epoch, sample.mode_held, sample.custom_held) + # Keep auditing a held gesture: an assignment can change and return to + # its original value before release, which must still cancel that press. + changed |= current != previous or sample.mode_held or sample.custom_held + previous = current + self.media_sample_state = previous + # Neutral media packets also used to audit assignments during conventional + # button holds. Preserve those revocations until every gesture is idle. + held = any(tracker.distance_held or tracker.media_held is not None + for tracker in (self.tracker, self.cancel_tracker, self.main_tracker)) + return changed or bool(media.samples and held) + def observe(self, params, cp, state, *, now_ns, drive_id, media=None, blocked_keys=frozenset()): self.suppress_distance_release = any(event.type == structs.CarState.ButtonEvent.Type.gapAdjustCruise and not event.pressed for event in state.buttonEvents) - edge = bool(state.buttonEvents) or bool(media is not None and media.samples) + # Repeated neutral packets are not button edges. Keep held-input audits, + # the periodic audit, and the final read before emitted commands below. + media_edge = self.media_audit_needed(media) + edge = bool(state.buttonEvents) or media_edge audit = self.mapping is None or drive_id != self.drive or edge or now_ns - self.checked_ns >= 1_000_000_000 mapping = capture(params) if audit else self.mapping if audit: