mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-11-01 03:24:05 +08:00
Reduce idle wheel audits and reconcile late Ioniq inputs
Skip repeated neutral media settings audits only while every gesture tracker is idle. Preserve edge, held-input, epoch, periodic and command-time qualification. For qualified Ioniq 6 output continuity, reconcile sources arriving during car-event work before selected-source decisions. Retain earlier exact intent restrictions and queued cancellation/fault events, process samples once within the existing bound, and preserve the original absolute output deadline. Add production-module regression coverage for late arrivals, source restrictions, GM ordering and wheel remaps. No arming/rearming, alert, freshness-threshold, native, model or dependency changes. Validation: 101 focused cases passed; affected authority, controller, conditional, SelfdriveD/Controls and cross-brand functional checks completed. Baseline fixture/order and timing failures were separately reproduced; full aggregate, interrupted compatibility runs and native-process qualification are not claimed green. No device or road qualification. This is a scoped partial timing repair, not proof that genuine Card producer stalls are eliminated. Build receipt and native artifacts remain unchanged. Manual Fast Update rejects the stale protected-source receipt; the normal updater path is required.
This commit is contained in:
Dom
@@ -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
|
||||
|
||||
@@ -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')
|
||||
@@ -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
|
||||
@@ -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:
|
||||
|
||||
Reference in New Issue
Block a user