mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-10-28 17:13:54 +08:00
Log bounded GM lateral withdrawals after command publication
This commit is contained in:
firestar5683
@@ -31,6 +31,7 @@ from openpilot.starpilot.lateral.pause import LateralPause, read_settings as rea
|
||||
from openpilot.starpilot.lateral.model_turn_assist import ModelTurnAssist, update_twitch_guard
|
||||
from openpilot.starpilot.aol.runtime import (current_axis, current_native, ordinary_lateral_requested, ordinary_axis_acknowledged,
|
||||
refresh_expired_lateral_snapshot)
|
||||
from openpilot.starpilot.aol.transition_diagnostic import TransitionDiagnostic
|
||||
from openpilot.starpilot.aol.vehicle import policy_for as axis_policy_for, ordinary_axis_request_allowed, allow_lateral_onset
|
||||
from openpilot.starpilot.lateral.lane_centering import (
|
||||
LaneCenteringController, LaneCenteringInput, LaneCenteringRequest, LaneCenteringResult,
|
||||
@@ -71,6 +72,7 @@ class Controls:
|
||||
self.params, enabled=self.params.get_bool("OpenpilotEnabledToggle"))
|
||||
self.torque_learning_allowed = learning_allowed(self.params, self.CP, selection=self.lateral_controller_selection)
|
||||
self.aol_replay = feature_enabled(self.params, self.CP, 'aol', os.environ)
|
||||
self.aol_transition_diagnostic = TransitionDiagnostic() if self.aol_replay and self.CP.brand == 'gm' else None
|
||||
self.ordinary_axis_ack_required = axis_policy_for(self.CP).ordinary_axis_ack_required
|
||||
self.longitudinal_inputs = LongitudinalInputs(self.CP, self.params, lambda: self.sm)
|
||||
|
||||
@@ -173,10 +175,17 @@ class Controls:
|
||||
self.LaC.__dict__.pop(name, None)
|
||||
|
||||
def state_control(self, *, lane_centering: LaneCenteringRequest | None = None):
|
||||
diagnostic_observer = getattr(self, 'aol_transition_diagnostic', None)
|
||||
diagnostic_entry = diagnostic_gate = diagnostic_native = None
|
||||
diagnostic_reason = ''
|
||||
diagnostic_initial_lateral = diagnostic_onset = True
|
||||
lateral_inputs_refreshed = False
|
||||
if (self.aol_replay and os.getenv('REPLAY') != '1' and
|
||||
getattr(self, 'aol_previous_lateral_active', False)):
|
||||
lateral_inputs_refreshed = refresh_expired_lateral_snapshot(self.sm, now_ns=time.monotonic_ns())
|
||||
refresh_now_ns = time.monotonic_ns()
|
||||
if diagnostic_observer is not None:
|
||||
diagnostic_entry = diagnostic_observer.capture(self.sm, refresh_now_ns)
|
||||
lateral_inputs_refreshed = refresh_expired_lateral_snapshot(self.sm, now_ns=refresh_now_ns)
|
||||
CS = self.sm['carState']
|
||||
self.flm_native_witness = None
|
||||
if self.lateral_gain_owner is not None:
|
||||
@@ -233,6 +242,13 @@ class Controls:
|
||||
self.CP.openpilotLongitudinalControl and
|
||||
not any(e.overrideLongitudinal for e in self.sm['onroadEvents']))
|
||||
|
||||
if diagnostic_entry is not None:
|
||||
diagnostic_gate = diagnostic_observer.capture(self.sm, now_ns)
|
||||
diagnostic_native = native
|
||||
diagnostic_initial_lateral = bool(CC.latActive)
|
||||
if not diagnostic_initial_lateral:
|
||||
diagnostic_reason = 'lateral_gate'
|
||||
|
||||
elif getattr(self, 'ordinary_axis_ack_required', False):
|
||||
now_ns = int(self.sm.logMonoTime['carState']) if os.getenv('REPLAY') == '1' else time.monotonic_ns()
|
||||
CC.latActive = bool(CC.latActive and CS.canValid and not CS.canTimeout and
|
||||
@@ -256,6 +272,9 @@ class Controls:
|
||||
steering_pressed=bool(CS.steeringPressed),
|
||||
previous_active=bool(getattr(self, 'aol_previous_lateral_active', False)))
|
||||
|
||||
if diagnostic_entry is not None:
|
||||
diagnostic_onset = bool(CC.latActive)
|
||||
|
||||
pause_now_ns = int(self.sm.logMonoTime['carState']) if os.getenv('REPLAY') == '1' else time.monotonic_ns()
|
||||
self.lateral_pause.refresh(pause_now_ns)
|
||||
pause_allowed = True
|
||||
@@ -418,6 +437,23 @@ class Controls:
|
||||
(car.CarParams.SteerControlType.angle, car.CarParams.SteerControlType.curvature) else steer))
|
||||
except Exception:
|
||||
self._discard_global_learning()
|
||||
if diagnostic_entry is not None and diagnostic_observer is not None and not CC.latActive:
|
||||
try:
|
||||
if not diagnostic_reason:
|
||||
diagnostic_reason = ('refreshed_source_invalid' if lateral_inputs_refreshed and (not CS.canValid or CS.canTimeout) else
|
||||
'lateral_fault_latch' if fault_latch is not None and latched else
|
||||
'vehicle_onset_denied' if not diagnostic_onset else
|
||||
'lateral_pause' if not pause_allowed else 'car_state_invalid')
|
||||
diagnostic_observer.finish(diagnostic_entry, diagnostic_gate,
|
||||
now_ns=time.monotonic_ns(), refreshed=lateral_inputs_refreshed, native=diagnostic_native,
|
||||
native_header_ns=self.sm.logMonoTime['aolSafetyWire'],
|
||||
native_flags=(self.sm.seen['aolSafetyWire'], self.sm.valid['aolSafetyWire'], self.sm.alive['aolSafetyWire']),
|
||||
native_raw=self.sm['aolSafetyWire'] if diagnostic_native is None else b'',
|
||||
reason=diagnostic_reason, cs=CS, enabled=CC.enabled, long_active=CC.longActive,
|
||||
axis=axis, standstill=standstill, steer_at_standstill=self.CP.steerAtStandstill)
|
||||
except Exception:
|
||||
diagnostic_observer.disabled = True
|
||||
diagnostic_observer.pending = None
|
||||
self.aol_previous_lateral_active = bool(CC.latActive)
|
||||
return CC, lac_log
|
||||
|
||||
@@ -537,6 +573,9 @@ class Controls:
|
||||
else:
|
||||
self.learning_feedback_prior_publish_ns = 0
|
||||
self.pm.send('starpilotLateralState', feedback)
|
||||
diagnostic_observer = getattr(self, 'aol_transition_diagnostic', None)
|
||||
if diagnostic_observer is not None:
|
||||
diagnostic_observer.emit(cloudlog.event, cc_send)
|
||||
|
||||
def run(self):
|
||||
rk = Ratekeeper(100, print_delay_threshold=None)
|
||||
|
||||
@@ -0,0 +1,475 @@
|
||||
"""Bounded withdrawal observations with declared, public GM fixtures."""
|
||||
|
||||
from unittest.mock import patch
|
||||
import json
|
||||
import os
|
||||
import pytest
|
||||
from openpilot.cereal import messaging, custom
|
||||
from opendbc.car.gm.tests.test_ascm_intercept import params as ascm_params
|
||||
from opendbc.car.gm.values import CAR, GMFlags
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.prefix import OpenpilotPrefix
|
||||
from openpilot.selfdrive.controls import controlsd
|
||||
from openpilot.starpilot.aol.runtime import current_axis, current_native
|
||||
from openpilot.starpilot.lateral.tests.test_lane_runtime import feed
|
||||
|
||||
BASE = 10_000_000_000
|
||||
|
||||
|
||||
def declared_inputs():
|
||||
def state(ns):
|
||||
return {
|
||||
'k': 'carState',
|
||||
'ns': ns,
|
||||
'valid': True,
|
||||
'v': {
|
||||
'canValid': True,
|
||||
'canTimeout': False,
|
||||
'vEgo': 17.63,
|
||||
'vEgoRaw': 17.63,
|
||||
'standstill': False,
|
||||
'gearShifter': 'low',
|
||||
'gasPressed': True,
|
||||
'cruiseState': {'available': True, 'enabled': False},
|
||||
},
|
||||
}
|
||||
|
||||
def axis(ns, sequence, source):
|
||||
return {
|
||||
'k': 'aolAxisState',
|
||||
'ns': ns,
|
||||
'valid': True,
|
||||
'v': {
|
||||
'sessionId': 'diagnostic-session',
|
||||
'sequence': sequence,
|
||||
'sourceCarStateMonoTime': source,
|
||||
'observedMonoTime': ns,
|
||||
'validUntilMonoTime': ns + 30_000_000,
|
||||
'mode': 'lateralOnly',
|
||||
'lateralActive': True,
|
||||
'longitudinalActive': False,
|
||||
'desiredLateral': True,
|
||||
'desiredLongitudinal': False,
|
||||
'qualified': True,
|
||||
'nativeAcknowledged': True,
|
||||
},
|
||||
}
|
||||
|
||||
observed = BASE - 30_000_000
|
||||
native = {
|
||||
'k': 'aolSafetyWire',
|
||||
'ns': observed,
|
||||
'valid': True,
|
||||
'v': {
|
||||
'kind': 1,
|
||||
'version': 2,
|
||||
'protocolVersion': 1,
|
||||
'compatible': True,
|
||||
'observedMonoTime': observed,
|
||||
'validUntilMonoTime': observed + 200_000_000,
|
||||
'safetyModel': 4,
|
||||
'safetyParam': 0x605,
|
||||
'lateralAllowed': True,
|
||||
'longitudinalAllowed': False,
|
||||
'requestedLateral': True,
|
||||
'requestedLongitudinal': False,
|
||||
'pandaSerial': 'diagnostic-panda',
|
||||
'axisSessionId': 'diagnostic-session',
|
||||
'sourcePandaStatesMonoTime': observed,
|
||||
'pandaInventory': [
|
||||
{
|
||||
'slotIndex': 0,
|
||||
'hardwareSerial': 'diagnostic-panda',
|
||||
'safetyModel': 4,
|
||||
'safetyParam': 0x605,
|
||||
'alternativeExperience': 32,
|
||||
'controlsAllowed': False,
|
||||
'safetyRxChecksInvalid': False,
|
||||
'heartbeatLost': False,
|
||||
'faults': 0,
|
||||
}
|
||||
],
|
||||
},
|
||||
}
|
||||
return {
|
||||
'cached': [state(BASE - 5_000_000), axis(BASE - 20_000_000, 1, BASE - 25_000_000), native],
|
||||
'pending': [state(BASE + 15_000_000), axis(BASE + 10_000_000, 2, BASE + 5_000_000), native],
|
||||
'controlledStartNs': BASE,
|
||||
'controlledGuardNs': BASE + 16_000_000,
|
||||
}
|
||||
|
||||
|
||||
DATA = declared_inputs()
|
||||
|
||||
|
||||
def event(row):
|
||||
k = row['k']
|
||||
m = messaging.new_message(k, 0 if k == 'aolSafetyWire' else None, valid=row['valid'], logMonoTime=row['ns'])
|
||||
if k == 'aolSafetyWire':
|
||||
m.aolSafetyWire = custom.AolAxisState.SafetyWire.new_message(**row['v']).to_bytes()
|
||||
else:
|
||||
setattr(m, k, row['v'])
|
||||
return m.as_reader()
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
'schedule',
|
||||
[
|
||||
'no_delay',
|
||||
'cross_before_guard',
|
||||
'expired_at_entry',
|
||||
'cross_no_pending',
|
||||
'cross_withdrawn_pending',
|
||||
'cross_eps_fault_pending',
|
||||
'expired_withdrawn',
|
||||
'expired_eps',
|
||||
'expired_empty',
|
||||
'expired_invalid_native',
|
||||
'expired_context',
|
||||
],
|
||||
)
|
||||
def test_declared_proofs_with_controlled_schedule(schedule):
|
||||
with OpenpilotPrefix(), patch.dict(os.environ, {'SIMULATION': '0', 'AOL_REPLAY_RUNTIME': '1'}):
|
||||
params = Params()
|
||||
params.put_bool('OpenpilotEnabledToggle', True, block=True)
|
||||
params.put_bool('AlwaysOnLateral', True, block=True)
|
||||
cp = ascm_params(CAR.CHEVROLET_VOLT_ASCM, sascm=False, accelerator=False, alpha=False)
|
||||
cp.flags |= int(GMFlags.HAS_BSM)
|
||||
cp.alternativeExperience = 32
|
||||
params.put('CarParams', cp.to_bytes(), block=True)
|
||||
controls = controlsd.Controls()
|
||||
assert controls.aol_replay
|
||||
assert controls.CI.CS.steering_authority.enabled, 'Stock-ASCM refusal protection must stay enabled'
|
||||
start = DATA['controlledStartNs']
|
||||
guard = DATA['controlledGuardNs']
|
||||
now = [start]
|
||||
cached = [event(r) for r in DATA['cached']]
|
||||
pending = [event(r).as_builder() for r in DATA['pending']]
|
||||
if schedule in ('cross_no_pending', 'expired_empty'):
|
||||
pending = []
|
||||
if schedule in ('cross_withdrawn_pending', 'expired_withdrawn'):
|
||||
next(r for r in pending if r.which() == 'aolAxisState').aolAxisState.lateralActive = False
|
||||
if schedule == 'expired_invalid_native':
|
||||
next(r for r in pending if r.which() == 'aolSafetyWire').valid = False
|
||||
if schedule == 'expired_context':
|
||||
axis = next(r for r in pending if r.which() == 'aolAxisState').aolAxisState
|
||||
axis.lateralActive = axis.desiredLateral = False
|
||||
axis.mode = 'off'
|
||||
axis.faultReason = 'availabilityPause'
|
||||
axis.faultEventMonoTime = guard - 1
|
||||
physical = next(r for r in pending if r.which() == 'carState').carState
|
||||
physical.gearShifter = 'reverse'
|
||||
physical.cruiseState.available = physical.cruiseState.enabled = False
|
||||
if schedule in ('cross_eps_fault_pending', 'expired_eps'):
|
||||
next(r for r in pending if r.which() == 'carState').carState.steerFaultTemporary = True
|
||||
with patch.object(controls.sm, 'update_msgs') as capture:
|
||||
feed(controls, start, 1, active=False, enabled=False)
|
||||
frames = capture.call_args.args[1]
|
||||
replaced = {r.which() for r in cached}
|
||||
controls.sm.update_msgs(start / 1e9, [r for r in frames if r.which() not in replaced] + cached)
|
||||
# Actual last-known axis/native proof: active, no physical veto, no ordinary LONG.
|
||||
with patch.object(controlsd.time, 'monotonic_ns', return_value=start):
|
||||
healthy, _ = controls.state_control()
|
||||
assert healthy.latActive and not healthy.longActive
|
||||
cached_deadline = int(controls.sm['aolAxisState'].validUntilMonoTime)
|
||||
assert start < cached_deadline < guard
|
||||
updates = []
|
||||
axis_checks = []
|
||||
native_checks = []
|
||||
|
||||
def receive(timeout):
|
||||
assert timeout == 0
|
||||
updates.append(now[0])
|
||||
controls.sm.update_msgs(now[0] / 1e9, [r.as_reader() for r in pending])
|
||||
|
||||
original = controls.VM.update_params
|
||||
|
||||
def delayed_vm(*args, **kwargs):
|
||||
result = original(*args, **kwargs)
|
||||
if schedule != 'no_delay':
|
||||
now[0] = guard
|
||||
return result
|
||||
|
||||
def observe_axis(sm, *, now_ns):
|
||||
answer = current_axis(sm, now_ns=now_ns)
|
||||
axis_checks.append(
|
||||
{'now': now_ns, 'stamp': int(sm.logMonoTime['aolAxisState']), 'deadline': int(sm['aolAxisState'].validUntilMonoTime), 'accepted': answer is not None}
|
||||
)
|
||||
return answer
|
||||
|
||||
def observe_native(sm, CP, **kwargs):
|
||||
answer = current_native(sm, CP, **kwargs)
|
||||
native_checks.append(answer is not None)
|
||||
return answer
|
||||
|
||||
if schedule.startswith('expired'):
|
||||
now[0] = guard
|
||||
with (
|
||||
patch.object(controls.sm, 'update', side_effect=receive),
|
||||
patch.object(controlsd.time, 'monotonic_ns', side_effect=lambda: now[0]),
|
||||
patch.object(controls.VM, 'update_params', side_effect=delayed_vm),
|
||||
patch.object(controlsd, 'current_axis', side_effect=observe_axis),
|
||||
patch.object(controlsd, 'current_native', side_effect=observe_native),
|
||||
):
|
||||
command, _ = controls.state_control()
|
||||
expected = schedule in ('no_delay', 'expired_at_entry')
|
||||
assert bool(command.latActive) == expected and not command.longActive
|
||||
assert native_checks == [schedule != 'expired_invalid_native']
|
||||
assert len(updates) == int(schedule.startswith('expired'))
|
||||
assert axis_checks[0]['accepted'] == (expected or schedule in ('expired_withdrawn', 'expired_eps', 'expired_invalid_native', 'expired_context'))
|
||||
diagnostic = controls.aol_transition_diagnostic
|
||||
assert (diagnostic.pending is None) == expected
|
||||
if not expected:
|
||||
assert isinstance(diagnostic.pending, tuple)
|
||||
order = []
|
||||
headers = []
|
||||
records = []
|
||||
|
||||
class Publisher:
|
||||
def send(self, name, message):
|
||||
order.append(name)
|
||||
if name == 'carControl':
|
||||
headers.append(int(message.logMonoTime))
|
||||
|
||||
with (
|
||||
patch.object(controls, 'pm', Publisher()),
|
||||
patch.object(controlsd.cloudlog, 'event', side_effect=lambda *a, **kw: (order.append('diagnostic'), records.append(kw['diagnostic']))),
|
||||
patch.object(controlsd.time, 'monotonic_ns', return_value=now[0]),
|
||||
):
|
||||
controls.publish(command, _)
|
||||
record = records[0] if records else None
|
||||
if expected:
|
||||
assert record is None
|
||||
else:
|
||||
assert record is not None
|
||||
assert record['entry']['headerNs'] == DATA['cached'][1]['ns']
|
||||
assert record['entry']['session'] == DATA['cached'][1]['v']['sessionId']
|
||||
assert record['entry']['sequence'] == DATA['cached'][1]['v']['sequence']
|
||||
assert record['entry']['sourceNs'] == DATA['cached'][1]['v']['sourceCarStateMonoTime']
|
||||
assert record['entry']['deadlineNs'] == cached_deadline
|
||||
assert record['native']['accepted'] == (schedule != 'expired_invalid_native')
|
||||
assert record['native']['allowed']
|
||||
if schedule == 'expired_invalid_native':
|
||||
assert not record['native']['valid'] and record['native']['decoded']
|
||||
if schedule == 'expired_invalid_native':
|
||||
assert record['reason'] == 'native_rejected'
|
||||
elif schedule in ('expired_withdrawn', 'expired_context'):
|
||||
assert record['reason'] == 'axis_withdrawn'
|
||||
elif schedule == 'expired_eps':
|
||||
assert record['reason'] == 'steer_temporary_fault'
|
||||
else:
|
||||
assert record['reason'] == 'axis_header_expired'
|
||||
assert record['refreshAttempted'] == schedule.startswith('expired')
|
||||
assert record['entry']['atNs'] < cached_deadline if not schedule.startswith('expired') else record['entry']['atNs'] > cached_deadline
|
||||
assert record['gate']['atNs'] > cached_deadline
|
||||
assert record['entry']['mode'] == 'lateralOnly'
|
||||
assert record['physical']['gear'] == ('reverse' if schedule == 'expired_context' else 'low')
|
||||
assert record['physical']['mainAvailable'] == (schedule != 'expired_context')
|
||||
assert not record['physical']['cruiseEnabled']
|
||||
if schedule == 'expired_context':
|
||||
assert record['gate']['mode'] == 'off'
|
||||
assert record['gate']['faultReason'] == 'availabilityPause'
|
||||
assert record['gate']['faultEventNs'] == guard - 1
|
||||
assert order[:2] == ['controlsState', 'carControl']
|
||||
assert ('diagnostic' in order) == (not expected)
|
||||
if not expected:
|
||||
assert record is not None
|
||||
assert order.index('diagnostic') > order.index('carControl')
|
||||
assert record['controlHeaderNs'] == headers[0]
|
||||
for _ignored in range(20):
|
||||
with patch.object(controlsd.time, 'monotonic_ns', return_value=now[0]):
|
||||
repeated, _ = controls.state_control()
|
||||
assert not repeated.latActive and diagnostic.pending is None
|
||||
assert diagnostic.emitted == 1
|
||||
assert diagnostic.pending is None
|
||||
|
||||
|
||||
def test_diagnostic_rate_lifetime_payload_and_failure_bounds():
|
||||
from types import SimpleNamespace
|
||||
from openpilot.starpilot.aol.transition_diagnostic import TransitionDiagnostic, MAX_EVENTS, MAX_EVENT_BYTES, MIN_EVENT_INTERVAL_NS
|
||||
from openpilot.starpilot.aol.wire import decode_safety
|
||||
|
||||
axis = event(DATA['cached'][1]).aolAxisState
|
||||
snap = (DATA['controlledStartNs'], DATA['cached'][1]['ns'], axis, True, True, True, DATA['cached'][0]['ns'])
|
||||
native = decode_safety(event(DATA['cached'][2]).aolSafetyWire)
|
||||
cs = event(DATA['cached'][0]).carState
|
||||
diag = TransitionDiagnostic()
|
||||
events = []
|
||||
now = DATA['controlledGuardNs']
|
||||
|
||||
def finish(stamp, owner=diag):
|
||||
owner.finish(
|
||||
snap,
|
||||
snap,
|
||||
now_ns=stamp,
|
||||
refreshed=False,
|
||||
native=native,
|
||||
native_header_ns=DATA['cached'][2]['ns'],
|
||||
native_flags=(True, True, True),
|
||||
native_raw=b'',
|
||||
reason='axis_header_expired',
|
||||
cs=cs,
|
||||
enabled=False,
|
||||
long_active=False,
|
||||
)
|
||||
|
||||
finish(now)
|
||||
diag.emit(lambda name, **kw: events.append(kw['diagnostic']))
|
||||
for tick in range(1, 1000):
|
||||
finish(now + tick)
|
||||
diag.emit(lambda name, **kw: events.append(kw['diagnostic']))
|
||||
assert len(events) == 1 and diag.suppressed == 999
|
||||
finish(now + MIN_EVENT_INTERVAL_NS)
|
||||
diag.emit(lambda name, **kw: events.append(kw['diagnostic']))
|
||||
assert events[-1]['suppressed'] == 999
|
||||
for tick in range(2, MAX_EVENTS + 20):
|
||||
finish(now + tick * MIN_EVENT_INTERVAL_NS)
|
||||
diag.emit(lambda name, **kw: events.append(kw['diagnostic']))
|
||||
assert len(events) == MAX_EVENTS and diag.emitted == MAX_EVENTS and diag.pending is None
|
||||
assert all(len(json.dumps(r, separators=(',', ':')).encode()) <= MAX_EVENT_BYTES for r in events)
|
||||
assert diag.capture(None, now) is None
|
||||
broken = TransitionDiagnostic()
|
||||
finish(now, broken)
|
||||
|
||||
def failed_sink(*args, **kwargs):
|
||||
raise RuntimeError('sink unavailable')
|
||||
|
||||
with patch.object(broken, '_format', return_value=events[0]):
|
||||
broken.emit(failed_sink)
|
||||
assert broken.disabled and broken.pending is None
|
||||
overflow = TransitionDiagnostic()
|
||||
finish(now, overflow)
|
||||
with patch.object(overflow, '_format', return_value={'bad': 'x' * (MAX_EVENT_BYTES + 1)}):
|
||||
overflow.emit(failed_sink)
|
||||
assert overflow.disabled and overflow.pending is None
|
||||
unavailable = TransitionDiagnostic()
|
||||
assert unavailable.capture(SimpleNamespace(), now) is None and unavailable.disabled
|
||||
|
||||
|
||||
def initialize_controls():
|
||||
params = Params()
|
||||
params.put_bool('OpenpilotEnabledToggle', True, block=True)
|
||||
params.put_bool('AlwaysOnLateral', True, block=True)
|
||||
cp = ascm_params(CAR.CHEVROLET_VOLT_ASCM, sascm=False, accelerator=False, alpha=False)
|
||||
cp.flags |= int(GMFlags.HAS_BSM)
|
||||
cp.alternativeExperience = 32
|
||||
params.put('CarParams', cp.to_bytes(), block=True)
|
||||
owner = controlsd.Controls()
|
||||
stamp = DATA['controlledStartNs']
|
||||
with patch.object(owner.sm, 'update_msgs') as capture:
|
||||
feed(owner, stamp, 1, active=False, enabled=False)
|
||||
cached = [event(r) for r in DATA['cached']]
|
||||
replaced = {r.which() for r in cached}
|
||||
owner.sm.update_msgs(stamp / 1e9, [r for r in capture.call_args.args[1] if r.which() not in replaced] + cached)
|
||||
return owner
|
||||
|
||||
|
||||
@pytest.mark.parametrize('failure', ['format', 'decode', 'json', 'sink'])
|
||||
def test_diagnostic_failure_occurs_after_outputs_and_cannot_change_command(failure):
|
||||
from types import SimpleNamespace
|
||||
from openpilot.starpilot.aol import transition_diagnostic as observer_module, wire
|
||||
|
||||
with OpenpilotPrefix(), patch.dict(os.environ, {'SIMULATION': '0', 'AOL_REPLAY_RUNTIME': '1'}):
|
||||
owner = initialize_controls()
|
||||
now = DATA['controlledStartNs']
|
||||
with patch.object(controlsd.time, 'monotonic_ns', return_value=now) as clock:
|
||||
command, _ = owner.state_control()
|
||||
assert command.latActive
|
||||
changed = event(DATA['cached'][0]).as_builder()
|
||||
changed.carState.steerFaultTemporary = True
|
||||
changed.carState.gearShifter = 'reverse'
|
||||
changed.carState.cruiseState.available = False
|
||||
changed.carState.cruiseState.enabled = False
|
||||
axis = event(DATA['cached'][1]).as_builder()
|
||||
axis.aolAxisState.faultReason = 'availabilityPause'
|
||||
axis.aolAxisState.faultEventMonoTime = now - 1
|
||||
inputs = [changed.as_reader(), axis.as_reader()]
|
||||
if failure == 'decode':
|
||||
rejected = event(DATA['cached'][2]).as_builder()
|
||||
rejected.valid = False
|
||||
inputs.append(rejected.as_reader())
|
||||
now += 1_000_000
|
||||
clock.return_value = now
|
||||
owner.sm.update_msgs(now / 1e9, inputs)
|
||||
command, state = owner.state_control()
|
||||
assert not command.latActive and isinstance(owner.aol_transition_diagnostic.pending, tuple)
|
||||
original = command.to_dict()
|
||||
order, sent = [], {}
|
||||
|
||||
class Publisher:
|
||||
def send(self, name, message):
|
||||
order.append(name)
|
||||
sent[name] = message.as_reader()
|
||||
|
||||
def broken(*args, **kwargs):
|
||||
assert order[:3] == ['controlsState', 'carControl', 'starpilotLateralState']
|
||||
raise RuntimeError('diagnostic ' + failure)
|
||||
|
||||
# publish normally adds HUD/cruise fields. Compare against that real path,
|
||||
# with the optional observer disabled, rather than a pre-publication body.
|
||||
baseline_command = command.as_reader().as_builder()
|
||||
with patch.object(owner, 'aol_transition_diagnostic', None), patch.object(owner.pm, 'send'):
|
||||
owner.publish(baseline_command, state)
|
||||
target = (
|
||||
patch.object(owner.aol_transition_diagnostic, '_format', side_effect=broken)
|
||||
if failure == 'format'
|
||||
else patch.object(wire, 'decode_safety', side_effect=broken)
|
||||
if failure == 'decode'
|
||||
else patch.object(observer_module, 'json', SimpleNamespace(dumps=broken))
|
||||
if failure == 'json'
|
||||
else patch.object(controlsd.cloudlog, 'event', side_effect=broken)
|
||||
)
|
||||
with patch.object(owner, 'pm', Publisher()), target:
|
||||
owner.publish(command, state)
|
||||
assert sent['carControl'].carControl.to_dict() == baseline_command.to_dict() == command.to_dict()
|
||||
assert command.actuators.to_dict() == original['actuators']
|
||||
assert (command.enabled, command.latActive, command.longActive) == (original['enabled'], original['latActive'], original['longActive'])
|
||||
assert owner.aol_transition_diagnostic.disabled and owner.aol_transition_diagnostic.pending is None
|
||||
|
||||
|
||||
def test_finish_retains_references_without_formatting_decoding_or_reading_fields():
|
||||
from openpilot.starpilot.aol.transition_diagnostic import TransitionDiagnostic
|
||||
|
||||
class Unreadable:
|
||||
def __getattr__(self, name):
|
||||
raise AssertionError('field read before output: ' + name)
|
||||
|
||||
reader = Unreadable()
|
||||
diag = TransitionDiagnostic()
|
||||
snapshot = (1, 1, reader, True, True, True, 1)
|
||||
with patch.object(diag, 'axis_record', side_effect=AssertionError('format before output')):
|
||||
diag.finish(
|
||||
snapshot,
|
||||
snapshot,
|
||||
now_ns=2,
|
||||
refreshed=False,
|
||||
native=reader,
|
||||
native_header_ns=1,
|
||||
native_flags=(True, True, True),
|
||||
native_raw=b'',
|
||||
reason='lateral_gate',
|
||||
cs=reader,
|
||||
enabled=False,
|
||||
long_active=False,
|
||||
axis=reader,
|
||||
)
|
||||
assert isinstance(diag.pending, tuple) and not diag.disabled
|
||||
|
||||
|
||||
@pytest.mark.parametrize('remove_observer', [False, True])
|
||||
def test_optional_observer_access_and_commands_match_disabled_baseline(remove_observer):
|
||||
outputs = []
|
||||
for enabled in (False, True):
|
||||
with OpenpilotPrefix(), patch.dict(os.environ, {'SIMULATION': '0', 'AOL_REPLAY_RUNTIME': '1'}):
|
||||
owner = initialize_controls()
|
||||
if not enabled:
|
||||
if remove_observer:
|
||||
del owner.aol_transition_diagnostic
|
||||
else:
|
||||
owner.aol_transition_diagnostic = None
|
||||
now = DATA['controlledStartNs']
|
||||
with patch.object(controlsd.time, 'monotonic_ns', return_value=now), patch.object(owner.pm, 'send'):
|
||||
command, state = owner.state_control()
|
||||
owner.publish(command, state)
|
||||
outputs.append((command.to_dict(), state.to_dict()))
|
||||
assert outputs[0] == outputs[1]
|
||||
@@ -0,0 +1,265 @@
|
||||
"""Bounded GM AOL withdrawal observations; never an authority input."""
|
||||
|
||||
import json
|
||||
|
||||
from openpilot.starpilot.aol.runtime import AXIS_MAX_AGE_NS
|
||||
|
||||
MAX_EVENTS = 64
|
||||
MIN_EVENT_INTERVAL_NS = 1_000_000_000
|
||||
MAX_EVENT_BYTES = 4096
|
||||
AXIS_SERVICE = 'aolAxisState'
|
||||
|
||||
|
||||
class TransitionDiagnostic:
|
||||
def __init__(self):
|
||||
self.disabled = False
|
||||
self.emitted = 0
|
||||
self.suppressed = 0
|
||||
self.last_event_ns = 0
|
||||
self.pending = None
|
||||
|
||||
def capture(self, sm, now_ns):
|
||||
if self.disabled or self.emitted >= MAX_EVENTS:
|
||||
return None
|
||||
try:
|
||||
# Retain the immutable reader. Do not serialize or copy a proof per tick.
|
||||
return (
|
||||
now_ns,
|
||||
int(sm.logMonoTime[AXIS_SERVICE]),
|
||||
sm[AXIS_SERVICE],
|
||||
bool(sm.seen[AXIS_SERVICE]),
|
||||
bool(sm.valid[AXIS_SERVICE]),
|
||||
bool(sm.alive[AXIS_SERVICE]),
|
||||
int(sm.logMonoTime['carState']),
|
||||
)
|
||||
except Exception:
|
||||
self.disabled = True
|
||||
self.pending = None
|
||||
return None
|
||||
|
||||
@staticmethod
|
||||
def axis_record(snapshot):
|
||||
now, header, axis, seen, valid, alive, source = snapshot
|
||||
return {
|
||||
'atNs': now,
|
||||
'headerNs': header,
|
||||
'carStateHeaderNs': source,
|
||||
'seen': seen,
|
||||
'valid': valid,
|
||||
'alive': alive,
|
||||
'session': str(axis.sessionId)[:64],
|
||||
'sequence': int(axis.sequence),
|
||||
'sourceNs': int(axis.sourceCarStateMonoTime),
|
||||
'observedNs': int(axis.observedMonoTime),
|
||||
'deadlineNs': int(axis.validUntilMonoTime),
|
||||
'qualified': bool(axis.qualified),
|
||||
'desired': bool(axis.desiredLateral),
|
||||
'active': bool(axis.lateralActive),
|
||||
'desiredLongitudinal': bool(axis.desiredLongitudinal),
|
||||
'longitudinalActive': bool(axis.longitudinalActive),
|
||||
'nativeAcknowledged': bool(axis.nativeAcknowledged),
|
||||
'mode': str(axis.mode)[:24],
|
||||
'faultReason': str(axis.faultReason)[:32],
|
||||
'faultEventNs': int(axis.faultEventMonoTime),
|
||||
}
|
||||
|
||||
@staticmethod
|
||||
def axis_rejection(snapshot):
|
||||
now, header, axis, seen, valid, alive, _ = snapshot
|
||||
if not seen:
|
||||
return 'axis_unseen'
|
||||
if not valid:
|
||||
return 'axis_invalid'
|
||||
if not alive:
|
||||
return 'axis_not_alive'
|
||||
if not axis.qualified:
|
||||
return 'axis_unqualified'
|
||||
if not axis.sessionId:
|
||||
return 'axis_session_missing'
|
||||
if header > now:
|
||||
return 'axis_header_future'
|
||||
if now - header > AXIS_MAX_AGE_NS:
|
||||
return 'axis_header_expired'
|
||||
if int(axis.observedMonoTime) > now:
|
||||
return 'axis_observation_future'
|
||||
if now > int(axis.validUntilMonoTime):
|
||||
return 'axis_deadline_expired'
|
||||
return 'axis_rejected_unclassified'
|
||||
|
||||
def gate_reason(self, snapshot, axis, native, cs, standstill, steer_at_standstill):
|
||||
try:
|
||||
if axis is None:
|
||||
return self.axis_rejection(snapshot)
|
||||
if native is None:
|
||||
return 'native_rejected'
|
||||
if not axis.nativeAcknowledged:
|
||||
return 'native_unacknowledged'
|
||||
if not axis.lateralActive:
|
||||
return 'axis_withdrawn'
|
||||
if bool(axis.desiredLateral) != bool(native.requestedLateral):
|
||||
return 'native_request_mismatch'
|
||||
if not native.lateralAllowed:
|
||||
return 'native_denied'
|
||||
if cs.steerFaultTemporary:
|
||||
return 'steer_temporary_fault'
|
||||
if cs.steerFaultPermanent:
|
||||
return 'steer_permanent_fault'
|
||||
if standstill and not steer_at_standstill:
|
||||
return 'below_steering_speed'
|
||||
return 'lateral_gate_rejected_unclassified'
|
||||
except Exception:
|
||||
return 'diagnostic_classification_unavailable'
|
||||
|
||||
def finish(
|
||||
self,
|
||||
entry,
|
||||
gate,
|
||||
*,
|
||||
now_ns,
|
||||
refreshed,
|
||||
native,
|
||||
native_header_ns,
|
||||
native_flags,
|
||||
native_raw,
|
||||
reason,
|
||||
cs,
|
||||
enabled,
|
||||
long_active,
|
||||
axis=None,
|
||||
standstill=False,
|
||||
steer_at_standstill=False,
|
||||
):
|
||||
if self.disabled or self.emitted >= MAX_EVENTS or entry is None or gate is None:
|
||||
return
|
||||
if self.pending is not None or (self.last_event_ns and now_ns - self.last_event_ns < MIN_EVENT_INTERVAL_NS):
|
||||
self.suppressed = min(self.suppressed + 1, 0xFFFFFFFF)
|
||||
return
|
||||
# Keep only bounded immutable readers/references and scalars before output.
|
||||
self.pending = (
|
||||
entry,
|
||||
gate,
|
||||
now_ns,
|
||||
refreshed,
|
||||
native,
|
||||
native_header_ns,
|
||||
native_flags,
|
||||
native_raw if len(native_raw) <= MAX_EVENT_BYTES else b'',
|
||||
reason,
|
||||
cs,
|
||||
enabled,
|
||||
long_active,
|
||||
axis,
|
||||
standstill,
|
||||
steer_at_standstill,
|
||||
)
|
||||
self.last_event_ns = now_ns
|
||||
|
||||
def _format(self, pending):
|
||||
(
|
||||
entry,
|
||||
gate,
|
||||
now_ns,
|
||||
refreshed,
|
||||
native,
|
||||
native_header_ns,
|
||||
native_flags,
|
||||
native_raw,
|
||||
reason,
|
||||
cs,
|
||||
enabled,
|
||||
long_active,
|
||||
axis,
|
||||
standstill,
|
||||
steer_at_standstill,
|
||||
) = pending
|
||||
native_accepted = native
|
||||
accepted = native is not None
|
||||
if native is None and len(native_raw) <= MAX_EVENT_BYTES:
|
||||
from openpilot.starpilot.aol.wire import decode_safety
|
||||
|
||||
native = decode_safety(native_raw)
|
||||
status = {
|
||||
'headerNs': int(native_header_ns),
|
||||
'seen': bool(native_flags[0]),
|
||||
'valid': bool(native_flags[1]),
|
||||
'alive': bool(native_flags[2]),
|
||||
'decoded': native is not None,
|
||||
'accepted': accepted,
|
||||
}
|
||||
if native is not None:
|
||||
status.update(
|
||||
observedNs=int(native.observedMonoTime),
|
||||
deadlineNs=int(native.validUntilMonoTime),
|
||||
session=str(native.axisSessionId)[:64],
|
||||
requested=bool(native.requestedLateral),
|
||||
allowed=bool(native.lateralAllowed),
|
||||
requestedLongitudinal=bool(native.requestedLongitudinal),
|
||||
longitudinalAllowed=bool(native.longitudinalAllowed),
|
||||
model=int(native.safetyModel),
|
||||
param=int(native.safetyParam),
|
||||
)
|
||||
if native is not None:
|
||||
inventory = getattr(native, 'pandaInventory', ())
|
||||
status.update(
|
||||
compatible=bool(native.compatible),
|
||||
protocolVersion=int(native.protocolVersion),
|
||||
inventoryCount=len(inventory),
|
||||
inventory=[
|
||||
{
|
||||
'slot': int(slot.slotIndex),
|
||||
'model': int(slot.safetyModel),
|
||||
'param': int(slot.safetyParam),
|
||||
'ae': int(slot.alternativeExperience),
|
||||
'rxInvalid': bool(slot.safetyRxChecksInvalid),
|
||||
'heartbeatLost': bool(slot.heartbeatLost),
|
||||
'faults': int(slot.faults),
|
||||
'selectedOwner': slot.hardwareSerial == native.pandaSerial,
|
||||
}
|
||||
for slot in inventory[:2]
|
||||
],
|
||||
)
|
||||
return {
|
||||
'version': 1,
|
||||
'clock': 'monotonic',
|
||||
'entry': self.axis_record(entry),
|
||||
'gate': self.axis_record(gate),
|
||||
'finishedNs': now_ns,
|
||||
'refreshAttempted': bool(refreshed),
|
||||
'reason': self.gate_reason(gate, axis, native_accepted, cs, standstill, steer_at_standstill) if reason == 'lateral_gate' else reason,
|
||||
'native': status,
|
||||
'physical': {
|
||||
'canValid': bool(cs.canValid),
|
||||
'canTimeout': bool(cs.canTimeout),
|
||||
'brake': bool(cs.brakePressed),
|
||||
'gas': bool(cs.gasPressed),
|
||||
'paddle': bool(cs.regenBraking),
|
||||
'steering': bool(cs.steeringPressed),
|
||||
'temporaryFault': bool(cs.steerFaultTemporary),
|
||||
'permanentFault': bool(cs.steerFaultPermanent),
|
||||
'standstill': bool(cs.standstill),
|
||||
'speed': float(cs.vEgo),
|
||||
'gear': str(cs.gearShifter)[:24],
|
||||
'mainAvailable': bool(cs.cruiseState.available),
|
||||
'cruiseEnabled': bool(cs.cruiseState.enabled),
|
||||
},
|
||||
'ordinaryEnabled': bool(enabled),
|
||||
'longActive': bool(long_active),
|
||||
'suppressed': self.suppressed,
|
||||
}
|
||||
|
||||
def emit(self, sink, control_message=None):
|
||||
pending, self.pending = self.pending, None
|
||||
if pending is None or self.disabled:
|
||||
return
|
||||
try:
|
||||
record = self._format(pending)
|
||||
record['controlHeaderNs'] = int(control_message.logMonoTime) if control_message is not None else 0
|
||||
# One bounded event per falling edge, rate limited and lifetime capped.
|
||||
if len(json.dumps(record, separators=(',', ':'), allow_nan=False).encode()) > MAX_EVENT_BYTES:
|
||||
self.disabled = True
|
||||
return
|
||||
sink('aol_lateral_withdrawal', diagnostic=record)
|
||||
self.emitted += 1
|
||||
self.suppressed = 0
|
||||
except Exception:
|
||||
self.disabled = True
|
||||
+1
-1
@@ -1 +1 @@
|
||||
{"protected_inventory_sha256": "aa9a61c472d6a9e7f14af2e47842d769b06072b5014e3bbf91df6075c07f6c8a", "schema_version": 1}
|
||||
{"protected_inventory_sha256": "da810d48f3843c49b0a739637637195ec5e29d7c32a8250240f3654a6d04cde2", "schema_version": 1}
|
||||
|
||||
@@ -95,6 +95,7 @@ AOL_LIFECYCLE_TARGETS = (
|
||||
"openpilot/starpilot/aol/tests/test_gm_main_level_contract.py",
|
||||
"openpilot/starpilot/aol/tests/test_gm_sdgm_source_wait.py",
|
||||
"openpilot/starpilot/aol/tests/test_gm_stock_ascm_refusal.py",
|
||||
"openpilot/starpilot/aol/tests/test_transition_diagnostic.py",
|
||||
"openpilot/starpilot/aol/tests/test_gm_ascm_receive_order.py",
|
||||
"openpilot/starpilot/aol/tests/test_gm_main_response.py",
|
||||
"openpilot/starpilot/aol/tests/test_gm_held_native.py",
|
||||
|
||||
Reference in New Issue
Block a user