mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-11-08 05:23:41 +08:00
firestarsdog
792 lines
43 KiB
Python
792 lines
43 KiB
Python
"""The live adoption path owns one card set speed, including PCM system long."""
|
||
from types import SimpleNamespace
|
||
from dataclasses import replace
|
||
from unittest.mock import patch
|
||
|
||
import pytest
|
||
|
||
from opendbc.car.dashboard_speed_limit import Status
|
||
from opendbc.car.structs import car
|
||
from openpilot.common.constants import CV
|
||
from openpilot.selfdrive.car.cruise import VCruiseHelper, slc_target_kph
|
||
from openpilot.selfdrive.controls.lib import longitudinal_planner as planner
|
||
from openpilot.starpilot.speed_limits import acceptance as acc, physical_actions
|
||
from openpilot.starpilot.speed_limits.runtime import Action, CruiseEvent, Runtime
|
||
from openpilot.starpilot.speed_limits.runtime_settings import parse
|
||
from openpilot.starpilot.speed_limits.tests.test_runtime_replay import ReplaySM
|
||
from openpilot.starpilot.speed_limits.tests import test_runtime_replay as replay_fixture
|
||
from openpilot.starpilot.speed_limits.speed_domain import OffsetBand, OffsetSchedule
|
||
|
||
START = 2_000_000_000
|
||
|
||
|
||
@pytest.fixture
|
||
def drive():
|
||
fixture = replay_fixture.RuntimeReplayTests()
|
||
fixture.setUp()
|
||
fixture.sm['carState'].vCruise = fixture.sm['carState'].vCruiseCluster = 60.
|
||
return fixture.sm, fixture.cp
|
||
|
||
|
||
def tick(runtime, sm, cp, seconds, **kwargs):
|
||
now = START + round(seconds * 1e9)
|
||
sm.advance(now)
|
||
sm['slcDashboardObservation'].validUntilMonoTime = now + 1_000_000_000
|
||
return runtime.step(sm, cp, now_ns=now, **kwargs)
|
||
|
||
|
||
def command_receipt(runtime, command, seconds, previous, selected, *, applied=True, event_id=1):
|
||
stamp = START + round(seconds * 1e9)
|
||
return CruiseEvent(event_id, stamp, previous / 3.6, selected / 3.6, '', 'card-producer', stamp,
|
||
'commandApplied' if applied else 'commandRejected', runtime.session_id,
|
||
command.decisionId, command.presentationId, command.commandId)
|
||
|
||
|
||
@pytest.mark.parametrize('button,kind', [('accel', 'confirmationAccept'), ('decel', 'confirmationReject')])
|
||
def test_wheel_confirmation_does_not_follow_same_step_source_replacement(drive, button, kind):
|
||
sm, cp = drive
|
||
runtime = Runtime(parse({'SpeedLimitController': True, 'SLCPriority1': 'Dashboard', 'SLCConfirmation': True}))
|
||
pending = tick(runtime, sm, cp, 0).message.slcState
|
||
sm['slcDashboardObservation'].episode += 1
|
||
sm['slcDashboardObservation'].observedMonoTime = START + 40_000_000
|
||
event = CruiseEvent(1, START + 50_000_000, 0.0, 0.0, button, 'card-producer', START + 50_000_000,
|
||
kind, runtime.session_id, pending.decisionId, pending.presentationId)
|
||
output = tick(runtime, sm, cp, .05, cruise_event=event)
|
||
assert output.message.slcState.hasPending and not output.message.slcState.hasAccepted
|
||
assert output.message.slcState.actionStatus == 'different_presentation'
|
||
assert output.command is None and runtime.ledger.transactions == ()
|
||
|
||
|
||
@pytest.mark.parametrize('confirmation', [False, True])
|
||
@pytest.mark.parametrize('metric,limit,offset', [(False, 45., 0.), (False, 45., 5.), (False, 45., -.4),
|
||
(True, 72., 0.), (True, 72., 5.), (True, 72., -.4)])
|
||
def test_matching_set_needs_neither_confirmation_nor_command(drive, confirmation, metric, limit, offset):
|
||
sm, cp = drive
|
||
conversion = CV.KPH_TO_MS if metric else CV.MPH_TO_MS
|
||
settings = replace(parse({'SpeedLimitController': True, 'SLCPriority1': 'Dashboard', 'SLCConfirmation': confirmation}),
|
||
offsets=OffsetSchedule((OffsetBand(0., None, offset * conversion),)))
|
||
runtime = Runtime(settings, is_metric=metric)
|
||
sm['slcDashboardObservation'].speedMps = limit * conversion
|
||
selected = slc_target_kph((limit + offset) * conversion, metric)
|
||
sm['carState'].vCruise = sm['carState'].vCruiseCluster = selected
|
||
selected = float(sm['carState'].vCruise)
|
||
for seconds in (0, .05, 11):
|
||
output = tick(runtime, sm, cp, seconds)
|
||
assert not output.result.errors and not output.message.slcState.hasPending
|
||
assert output.message.slcState.hasAccepted and output.command is None
|
||
assert output.message.slcState.commandStatus == 'applied'
|
||
assert sm['carState'].vCruise == selected
|
||
|
||
|
||
def test_half_unit_offset_uses_the_rounding_card_gets_after_float32_transport(drive):
|
||
sm, cp = drive
|
||
settings = replace(parse({'SpeedLimitController': True, 'SLCPriority1': 'Dashboard', 'SLCConfirmation': True}),
|
||
offsets=OffsetSchedule((OffsetBand(0., None, .5 / 3.6),)))
|
||
runtime = Runtime(settings, is_metric=True)
|
||
sm['slcDashboardObservation'].speedMps = 72 / 3.6
|
||
sm['carState'].vCruise = sm['carState'].vCruiseCluster = 72.
|
||
pending = tick(runtime, sm, cp, 0).message.slcState
|
||
assert pending.hasPending and not pending.hasAccepted
|
||
accepted = tick(runtime, sm, cp, .05,
|
||
request=Action(runtime.session_id, 1, pending.decisionId, pending.presentationId, 'accept'))
|
||
assert accepted.command is not None
|
||
helper = VCruiseHelper(cp)
|
||
helper.apply_slc_target(accepted.command.slcCruiseCommand.targetMps, True)
|
||
assert helper.v_cruise_kph == 73.
|
||
|
||
|
||
@pytest.mark.parametrize('confirmation', [False, True])
|
||
def test_commute_changes_only_set_speed_and_keeps_confirmation_policy(drive, confirmation):
|
||
sm, cp = drive
|
||
runtime = Runtime(parse({'SpeedLimitController': True, 'SLCPriority1': 'Dashboard', 'SLCConfirmation': confirmation}))
|
||
helper = VCruiseHelper(cp)
|
||
selected = slc_target_kph(35 * CV.MPH_TO_MS, False)
|
||
sm['carState'].vCruise = sm['carState'].vCruiseCluster = selected
|
||
for index, mph in enumerate((35, 45, 65, 55, 65, 35, 30)):
|
||
source = sm['slcDashboardObservation']
|
||
source.speedMps = mph * CV.MPH_TO_MS
|
||
source.episode = index + 1
|
||
source.observedMonoTime = START + index * 1_000_000_000
|
||
output = tick(runtime, sm, cp, index)
|
||
if index == 0:
|
||
assert not output.message.slcState.hasPending and output.command is None
|
||
continue
|
||
if confirmation:
|
||
pending = output.message.slcState
|
||
assert pending.hasPending and output.command is None
|
||
assert sm['carState'].vCruise == pytest.approx(selected)
|
||
output = tick(runtime, sm, cp, index + .01,
|
||
request=Action(runtime.session_id, index, pending.decisionId, pending.presentationId, 'accept'))
|
||
assert output.command is not None
|
||
command = output.command.slcCruiseCommand
|
||
helper.apply_slc_target(command.targetMps, False)
|
||
previous, selected = selected, helper.v_cruise_kph
|
||
assert round(selected * CV.KPH_TO_MPH) == mph
|
||
sm['carState'].vCruise = sm['carState'].vCruiseCluster = selected
|
||
applied = tick(runtime, sm, cp, index + .02,
|
||
cruise_event=command_receipt(runtime, command, index + .02, previous, selected, event_id=index))
|
||
assert not applied.message.slcState.hasPending and applied.command is None
|
||
|
||
|
||
@pytest.mark.parametrize('source_name', ['Maps', 'Vision'])
|
||
def test_secondary_source_matching_set_skips_confirmation_and_command(drive, source_name):
|
||
from openpilot.cereal import messaging
|
||
from openpilot.starpilot.speed_limits.selection import Source
|
||
sm, cp = drive
|
||
runtime = Runtime(parse({'SpeedLimitController': True, 'SLCPriority1': source_name, 'SLCConfirmation': True}),
|
||
vision_enabled=True, vision_control_qualified=True)
|
||
sm['carState'].vCruise = sm['carState'].vCruiseCluster = slc_target_kph(65 * CV.MPH_TO_MS, False)
|
||
source = Source.MAP if source_name == 'Maps' else Source.VISION
|
||
observations = dict.fromkeys(Source, acc.Observation(acc.ObservationKind.UNKNOWN))
|
||
observations[source] = acc.Observation(acc.ObservationKind.VALID, acc.Candidate(source.value,
|
||
acc.ObservationIdentity(acc.IdentityKind.GEOGRAPHIC, value='current-road'), 65 * CV.MPH_TO_MS))
|
||
vision = messaging.new_message('slcVisionObservation').slcVisionObservation
|
||
sm.data['slcVisionObservation'] = vision
|
||
with patch.object(runtime, '_observations', return_value=observations):
|
||
aligned = tick(runtime, sm, cp, 0)
|
||
assert aligned.message.slcState.acceptedSource == source.value
|
||
assert not aligned.message.slcState.hasPending and aligned.command is None
|
||
|
||
|
||
@pytest.mark.parametrize('selected', [65., 70.])
|
||
def test_returned_limit_only_prompts_when_it_would_change_set(drive, selected):
|
||
sm, cp = drive
|
||
runtime = Runtime(parse({'SpeedLimitController': True, 'SLCPriority1': 'Dashboard', 'SLCConfirmation': True}))
|
||
sm['slcDashboardObservation'].speedMps = 65 * CV.MPH_TO_MS
|
||
sm['carState'].vCruise = sm['carState'].vCruiseCluster = slc_target_kph(65 * CV.MPH_TO_MS, False)
|
||
first = tick(runtime, sm, cp, 0).message.slcState
|
||
tick(runtime, sm, cp, .05, request=Action(runtime.session_id, 1, 0, first.presentationId, 'adopt'))
|
||
sm['slcDashboardObservation'].speedMps = 55 * CV.MPH_TO_MS
|
||
assert tick(runtime, sm, cp, .1).message.slcState.hasPending
|
||
assert tick(runtime, sm, cp, 10.1).result.acceptance.basis == 'timeout_reject'
|
||
sm['carState'].vCruise = sm['carState'].vCruiseCluster = slc_target_kph(selected * CV.MPH_TO_MS, False)
|
||
sm['slcDashboardObservation'].speedMps = 65 * CV.MPH_TO_MS
|
||
returned = tick(runtime, sm, cp, 10.2)
|
||
assert returned.message.slcState.hasPending == (selected != 65.)
|
||
assert returned.command is None
|
||
|
||
|
||
@pytest.mark.parametrize('action_kind', ['accept', 'reject'])
|
||
@pytest.mark.parametrize('seconds', [.05, 10.])
|
||
def test_presented_driver_choice_wins_when_set_reaches_the_proposal(drive, action_kind, seconds):
|
||
sm, cp = drive
|
||
runtime = Runtime(parse({'SpeedLimitController': True, 'SLCPriority1': 'Dashboard', 'SLCConfirmation': True}))
|
||
sm['slcDashboardObservation'].speedMps = 55 * CV.MPH_TO_MS
|
||
pending = tick(runtime, sm, cp, 0).message.slcState
|
||
selected = slc_target_kph(55 * CV.MPH_TO_MS, False)
|
||
sm['carState'].vCruise = sm['carState'].vCruiseCluster = selected
|
||
output = tick(runtime, sm, cp, seconds,
|
||
request=Action(runtime.session_id, 1, pending.decisionId, pending.presentationId, action_kind))
|
||
assert output.result.acceptance.action_receipt.consumed
|
||
assert output.result.acceptance.basis == ('driver_accept' if action_kind == 'accept' else 'driver_reject')
|
||
assert output.command is None and sm['carState'].vCruise == selected
|
||
held = tick(runtime, sm, cp, seconds + .05)
|
||
assert not held.message.slcState.hasPending and held.command is None
|
||
if action_kind == 'reject':
|
||
assert held.result.acceptance.basis == 'rejected' and not held.message.slcState.hasAccepted
|
||
|
||
|
||
@pytest.mark.parametrize('seconds', [10., 10.001, 11.])
|
||
def test_set_alignment_cannot_accept_an_expired_confirmation(drive, seconds):
|
||
sm, cp = drive
|
||
runtime = Runtime(parse({'SpeedLimitController': True, 'SLCPriority1': 'Dashboard', 'SLCConfirmation': True}))
|
||
sm['slcDashboardObservation'].speedMps = 55 * CV.MPH_TO_MS
|
||
pending = tick(runtime, sm, cp, 0)
|
||
assert pending.message.slcState.hasPending
|
||
sm['carState'].vCruise = sm['carState'].vCruiseCluster = slc_target_kph(55 * CV.MPH_TO_MS, False)
|
||
expired = tick(runtime, sm, cp, seconds)
|
||
assert expired.result.acceptance.basis == 'timeout_reject'
|
||
assert not expired.message.slcState.hasAccepted and not expired.message.slcState.hasPending
|
||
assert expired.command is None
|
||
|
||
|
||
@pytest.mark.parametrize('resolution', ['applied', 'rejected', 'expired'])
|
||
def test_alignment_waits_for_the_previous_command_and_newer_set_evidence(drive, resolution):
|
||
from openpilot.starpilot.speed_limits.selection import Source
|
||
sm, cp = drive
|
||
runtime = Runtime(parse({'SpeedLimitController': True, 'SLCPriority1': 'Maps', 'SLCConfirmation': True}))
|
||
sm['carState'].vCruise = sm['carState'].vCruiseCluster = slc_target_kph(65 * CV.MPH_TO_MS, False)
|
||
previous = sm['carState'].vCruise
|
||
observations = dict.fromkeys(Source, acc.Observation(acc.ObservationKind.UNKNOWN))
|
||
|
||
def limit(mph):
|
||
observations[Source.MAP] = acc.Observation(acc.ObservationKind.VALID, acc.Candidate('map',
|
||
acc.ObservationIdentity(acc.IdentityKind.GEOGRAPHIC, value=f'road-{mph}'), mph * CV.MPH_TO_MS))
|
||
|
||
with patch.object(runtime, '_observations', return_value=observations):
|
||
limit(55)
|
||
pending = tick(runtime, sm, cp, 0).message.slcState
|
||
issued = tick(runtime, sm, cp, .05,
|
||
request=Action(runtime.session_id, 1, pending.decisionId, pending.presentationId, 'accept'))
|
||
command = issued.command.slcCruiseCommand
|
||
limit(65)
|
||
waiting = tick(runtime, sm, cp, .06)
|
||
assert waiting.command is None and waiting.message.slcState.commandStatus == 'issued'
|
||
assert runtime.commanded_acceptance.candidate.speed_mps == pytest.approx(55 * CV.MPH_TO_MS)
|
||
if resolution != 'expired':
|
||
selected = slc_target_kph(command.targetMps, False) if resolution == 'applied' else previous
|
||
sm['carState'].vCruise = sm['carState'].vCruiseCluster = selected
|
||
received = tick(runtime, sm, cp, .07, cruise_event=command_receipt(
|
||
runtime, command, .07, previous, selected, applied=resolution == 'applied'))
|
||
assert received.command is None
|
||
settled = tick(runtime, sm, cp, .08)
|
||
else:
|
||
# An expired command could have applied after this older (still fresh) SET sample.
|
||
now = START + 151_000_000
|
||
sm.advance(now)
|
||
sm.logMonoTime['carState'] = command.expiresMonoTime
|
||
sm.logMonoTime['slcDashboardObservation'] = command.expiresMonoTime
|
||
sm['slcDashboardObservation'].carStateLogMonoTime = command.expiresMonoTime
|
||
stale_set = runtime.step(sm, cp, now_ns=now)
|
||
assert stale_set.command is None and stale_set.message.slcState.commandStatus == 'expired'
|
||
settled = tick(runtime, sm, cp, .16)
|
||
if resolution == 'applied':
|
||
assert settled.message.slcState.hasPending and settled.message.slcState.commandStatus == 'applied'
|
||
assert settled.message.slcState.acceptedSpeedLimit == pytest.approx(55 * CV.MPH_TO_MS)
|
||
else:
|
||
assert not settled.message.slcState.hasPending and settled.command is None
|
||
assert settled.message.slcState.acceptedSpeedLimit == pytest.approx(65 * CV.MPH_TO_MS)
|
||
|
||
|
||
def test_session_reset_does_not_forget_an_in_flight_speed_change(drive):
|
||
sm, cp = drive
|
||
runtime = Runtime(parse({'SpeedLimitController': True, 'SLCPriority1': 'Dashboard', 'SLCConfirmation': True}))
|
||
sm['carState'].vCruise = sm['carState'].vCruiseCluster = slc_target_kph(65 * CV.MPH_TO_MS, False)
|
||
sm['slcDashboardObservation'].speedMps = 55 * CV.MPH_TO_MS
|
||
pending = tick(runtime, sm, cp, 0).message.slcState
|
||
issued = tick(runtime, sm, cp, .05,
|
||
request=Action(runtime.session_id, 1, pending.decisionId, pending.presentationId, 'accept'))
|
||
assert issued.command is not None
|
||
runtime.reset()
|
||
sm['slcDashboardObservation'].speedMps = 65 * CV.MPH_TO_MS
|
||
waiting = tick(runtime, sm, cp, .06)
|
||
assert not waiting.message.slcState.hasAccepted and waiting.command is None
|
||
settled = tick(runtime, sm, cp, .16)
|
||
assert settled.message.slcState.hasAccepted and not settled.message.slcState.hasPending
|
||
assert settled.command is None
|
||
|
||
|
||
@pytest.mark.parametrize('unavailable', ['off', 'lateral', 'stock', 'stale_set', 'stale_ego', 'unset', 'force_decel'])
|
||
def test_set_alignment_requires_current_system_speed_authority(drive, unavailable):
|
||
sm, cp = drive
|
||
runtime = Runtime(parse({'SpeedLimitController': True, 'SLCPriority1': 'Dashboard', 'SLCConfirmation': True}))
|
||
sm['slcDashboardObservation'].speedMps = 65 * CV.MPH_TO_MS
|
||
sm['carState'].vCruise = sm['carState'].vCruiseCluster = slc_target_kph(65 * CV.MPH_TO_MS, False)
|
||
sm.advance(START)
|
||
if unavailable in ('off', 'lateral', 'stock'):
|
||
sm['carControl'].longActive = False
|
||
sm['controlsState'].longControlState = 'off'
|
||
if unavailable == 'stock':
|
||
cp.openpilotLongitudinalControl = False
|
||
sm['carState'].cruiseState.enabled = True
|
||
else:
|
||
sm['carControl'].enabled = sm['selfdriveState'].enabled = False
|
||
sm['carControl'].latActive = unavailable == 'lateral'
|
||
elif unavailable == 'stale_set':
|
||
sm.valid['carState'] = False
|
||
elif unavailable == 'stale_ego':
|
||
sm['carState'].vEgoCluster = float('nan')
|
||
elif unavailable == 'unset':
|
||
sm['carState'].vCruise = sm['carState'].vCruiseCluster = 255.
|
||
else:
|
||
sm['controlsState'].forceDecel = True
|
||
output = runtime.step(sm, cp, now_ns=START)
|
||
assert not output.message.slcState.hasAccepted and output.command is None
|
||
|
||
|
||
def test_reaching_actual_speed_does_not_accept_a_different_set_target(drive):
|
||
sm, cp = drive
|
||
runtime = Runtime(parse({'SpeedLimitController': True, 'SLCPriority1': 'Dashboard', 'SLCConfirmation': True}))
|
||
sm['slcDashboardObservation'].speedMps = 45 * CV.MPH_TO_MS
|
||
sm['carState'].vEgo = sm['carState'].vEgoCluster = 45 * CV.MPH_TO_MS
|
||
sm['carState'].vCruise = sm['carState'].vCruiseCluster = slc_target_kph(65 * CV.MPH_TO_MS, False)
|
||
output = tick(runtime, sm, cp, 0)
|
||
assert output.message.slcState.hasPending and not output.message.slcState.hasAccepted and output.command is None
|
||
|
||
|
||
def test_matching_new_limit_reconciles_an_old_manual_override_without_changing_set(drive):
|
||
sm, cp = drive
|
||
runtime = Runtime(parse({'SpeedLimitController': True, 'SLCPriority1': 'Dashboard', 'SLCConfirmation': True}))
|
||
sm['slcDashboardObservation'].speedMps = 65 * CV.MPH_TO_MS
|
||
baseline = slc_target_kph(65 * CV.MPH_TO_MS, False)
|
||
sm['carState'].vCruise = sm['carState'].vCruiseCluster = baseline
|
||
first = tick(runtime, sm, cp, 0)
|
||
assert not first.message.slcState.hasPending
|
||
selected = slc_target_kph(70 * CV.MPH_TO_MS, False)
|
||
sm['carState'].vCruise = sm['carState'].vCruiseCluster = selected
|
||
selected = float(sm['carState'].vCruise)
|
||
changed = tick(runtime, sm, cp, .05, cruise_event=CruiseEvent(1, START + 50_000_000,
|
||
baseline / 3.6, selected / 3.6, 'accel', 'card-producer', START + 50_000_000))
|
||
assert changed.message.slcState.driverOverrideActive and changed.command is None
|
||
sm['slcDashboardObservation'].speedMps = 70 * CV.MPH_TO_MS
|
||
aligned = tick(runtime, sm, cp, .1)
|
||
assert not aligned.message.slcState.hasPending and not aligned.message.slcState.driverOverrideActive
|
||
assert aligned.command is None and sm['carState'].vCruise == selected
|
||
|
||
|
||
@pytest.mark.parametrize('direction', [-1, 1])
|
||
def test_matching_limit_keeps_wheel_buttons_available_for_normal_set_changes(drive, direction):
|
||
sm, cp = drive
|
||
runtime = Runtime(parse({'SpeedLimitController': True, 'SLCPriority1': 'Dashboard', 'SLCConfirmation': True}))
|
||
sm['slcDashboardObservation'].speedMps = 65 * CV.MPH_TO_MS
|
||
selected = slc_target_kph(65 * CV.MPH_TO_MS, False)
|
||
sm['carState'].vCruise = sm['carState'].vCruiseCluster = selected
|
||
output = tick(runtime, sm, cp, 0)
|
||
pending = physical_actions.pending_confirmation(ReplaySM({'slcState': output.message.slcState}, START), START, long_active=True)
|
||
assert pending is None
|
||
helper = VCruiseHelper(cp)
|
||
helper.v_cruise_kph = selected
|
||
button = 'accelCruise' if direction > 0 else 'decelCruise'
|
||
cs = car.CarState(cruiseState={'available': True}, buttonEvents=[car.CarState.ButtonEvent(type=button, pressed=True)])
|
||
helper.update_v_cruise(cs, True, False, pending, slc_enabled=True)
|
||
assert helper.slc_consumed_button is None
|
||
cs.buttonEvents = [car.CarState.ButtonEvent(type=button, pressed=False)]
|
||
helper.update_v_cruise(cs, True, False, slc_enabled=True)
|
||
assert helper.v_cruise_kph == pytest.approx(round(selected + direction * round(CV.MPH_TO_KPH, 1), 1))
|
||
|
||
|
||
@pytest.mark.parametrize('failure', ['source_refresh', 'expiry'])
|
||
def test_failed_adoption_retries_with_current_evidence_and_stops_after_application(drive, failure):
|
||
sm, cp = drive
|
||
runtime = Runtime(parse({'SpeedLimitController': True, 'SLCPriority1': 'Dashboard', 'SLCConfirmation': True}))
|
||
pending = tick(runtime, sm, cp, 0).message.slcState
|
||
first = tick(runtime, sm, cp, 1, request=Action(runtime.session_id, 17, pending.decisionId, pending.presentationId, 'accept'))
|
||
command = first.command.slcCruiseCommand
|
||
sm['slcDashboardObservation'].observedMonoTime = START + 1_010_000_000
|
||
if failure == 'source_refresh':
|
||
retry = tick(runtime, sm, cp, 1.05, cruise_event=command_receipt(runtime, command, 1.05, 60., 60., applied=False))
|
||
else:
|
||
retry = tick(runtime, sm, cp, 1.15)
|
||
assert retry.command is not None
|
||
current = retry.message.slcState
|
||
retried = retry.command.slcCruiseCommand
|
||
assert retried.commandId > command.commandId
|
||
assert current.commandActionSequenceId == 17
|
||
evidence = sm['slcDashboardObservation']
|
||
observation = SimpleNamespace(status=Status.VALID, speed_mps=evidence.speedMps, episode=evidence.episode,
|
||
observed_ns=evidence.observedMonoTime, valid_until_ns=evidence.validUntilMonoTime)
|
||
assert physical_actions.command_applicable(retried, current, observation, 'card-producer',
|
||
int(retry.message.logMonoTime), 60 / 3.6, long_active=True, button_event=False)
|
||
applied_at = 1.2 if failure == 'expiry' else 1.1
|
||
sm['carState'].vCruise = sm['carState'].vCruiseCluster = 90.
|
||
applied = tick(runtime, sm, cp, applied_at,
|
||
cruise_event=command_receipt(runtime, retried, applied_at, 60., 90., event_id=2))
|
||
assert applied.message.slcState.commandStatus == 'applied' and applied.command is None
|
||
assert tick(runtime, sm, cp, 2).command is None
|
||
|
||
|
||
@pytest.mark.parametrize('change', ['manual', 'unclassified', 'source', 'returned_source', 'disengaged'])
|
||
def test_expired_adoption_does_not_revive_superseded_commands(drive, change):
|
||
sm, cp = drive
|
||
runtime = Runtime(parse({'SpeedLimitController': True, 'SLCPriority1': 'Dashboard'}))
|
||
tick(runtime, sm, cp, 0)
|
||
old_command = tick(runtime, sm, cp, 3).command.slcCruiseCommand
|
||
kwargs = {}
|
||
if change in ('manual', 'unclassified'):
|
||
sm['carState'].vCruise = sm['carState'].vCruiseCluster = 61.
|
||
if change == 'manual':
|
||
kwargs['cruise_event'] = CruiseEvent(1, START + 3_200_000_000, 60 / 3.6, 61 / 3.6,
|
||
'accel', 'card-producer', START + 3_200_000_000)
|
||
elif change == 'source':
|
||
sm['slcDashboardObservation'].speedMps = 20.
|
||
elif change == 'returned_source':
|
||
sm['slcDashboardObservation'].speedMps = 20.
|
||
tick(runtime, sm, cp, 3.05)
|
||
sm['slcDashboardObservation'].speedMps = 25.
|
||
else:
|
||
sm['selfdriveState'].enabled = sm['carControl'].enabled = sm['carControl'].longActive = False
|
||
sm['controlsState'].longControlState = 'off'
|
||
result = tick(runtime, sm, cp, 3.2, **kwargs)
|
||
if change in ('source', 'returned_source'):
|
||
assert result.message.slcState.acceptedMonoTime == START + 3_200_000_000
|
||
assert result.command.slcCruiseCommand.commandId > old_command.commandId
|
||
assert result.command.slcCruiseCommand.targetMps == (20. if change == 'source' else 25.)
|
||
else:
|
||
assert result.command is None
|
||
|
||
|
||
@pytest.mark.parametrize('failure', ['rejected', 'expired'])
|
||
@pytest.mark.parametrize('response', ['reject', 'timeout'])
|
||
def test_returned_limit_cannot_retry_an_older_adoption_after_decline(drive, failure, response):
|
||
sm, cp = drive
|
||
runtime = Runtime(parse({'SpeedLimitController': True, 'SLCPriority1': 'Dashboard', 'SLCConfirmation': True}))
|
||
pending = tick(runtime, sm, cp, 0).message.slcState
|
||
first = tick(runtime, sm, cp, 1, request=Action(runtime.session_id, 1, pending.decisionId, pending.presentationId, 'accept'))
|
||
command = first.command.slcCruiseCommand
|
||
sm['slcDashboardObservation'].speedMps = 20.
|
||
receipt = command_receipt(runtime, command, 1.05, 60., 60., applied=False) if failure == 'rejected' else None
|
||
tick(runtime, sm, cp, 1.05, cruise_event=receipt)
|
||
sm['slcDashboardObservation'].speedMps = 25.
|
||
pending = tick(runtime, sm, cp, 4).message.slcState
|
||
action = Action(runtime.session_id, 2, pending.decisionId, pending.presentationId, 'reject') if response == 'reject' else None
|
||
seconds = 5 if action else 14
|
||
declined = tick(runtime, sm, cp, seconds, request=action)
|
||
assert declined.result.acceptance.basis == ('driver_reject' if action else 'timeout_reject')
|
||
assert declined.command is None
|
||
held = tick(runtime, sm, cp, seconds + .1)
|
||
assert not held.result.errors and not runtime.state.reset_required and held.command is None
|
||
assert sm['carState'].vCruise == 60.
|
||
# An explicit later adoption remains available after declining the new decision.
|
||
current = declined.message.slcState
|
||
readopted = tick(runtime, sm, cp, seconds + .2,
|
||
request=Action(runtime.session_id, 3, 0, current.presentationId, 'adopt'))
|
||
assert readopted.command is not None and readopted.command.slcCruiseCommand.targetMps == 25.
|
||
|
||
|
||
@pytest.mark.parametrize('limit,offset,bounded', [(140., 10., 145.), (10., -5., 8.)])
|
||
@pytest.mark.parametrize('metric', [False, True])
|
||
def test_adoption_and_display_use_existing_cruise_bounds(drive, limit, offset, bounded, metric):
|
||
sm, cp = drive
|
||
sm['slcDashboardObservation'].speedMps = limit / 3.6
|
||
settings = replace(parse({'SpeedLimitController': True, 'SLCPriority1': 'Dashboard'}),
|
||
offsets=OffsetSchedule((OffsetBand(0., None, offset / 3.6),)))
|
||
runtime = Runtime(settings)
|
||
tick(runtime, sm, cp, 0)
|
||
output = tick(runtime, sm, cp, 3)
|
||
assert not output.result.errors and output.command is not None
|
||
command, state = output.command.slcCruiseCommand, output.message.slcState
|
||
assert command.targetMps == pytest.approx(bounded / 3.6)
|
||
assert state.acceptedAdjustedLimit == pytest.approx(bounded / 3.6)
|
||
assert state.offset == pytest.approx((bounded - limit) / 3.6)
|
||
evidence = sm['slcDashboardObservation']
|
||
observation = SimpleNamespace(status=Status.VALID, speed_mps=evidence.speedMps, episode=evidence.episode,
|
||
observed_ns=evidence.observedMonoTime, valid_until_ns=evidence.validUntilMonoTime)
|
||
assert physical_actions.command_applicable(command, state, observation, 'card-producer', START + 3_000_000_000,
|
||
60 / 3.6, long_active=True, button_event=False)
|
||
helper = VCruiseHelper(cp)
|
||
helper.apply_slc_target(command.targetMps, metric)
|
||
assert 8 <= helper.v_cruise_kph <= 145
|
||
|
||
|
||
@pytest.mark.parametrize('metric,display', [(True, 72), (False, 55), (False, 60), (False, 65), (False, 70)])
|
||
@pytest.mark.parametrize('direction', [-1, 1])
|
||
def test_manual_override_compares_against_the_applied_rounded_set_speed(drive, metric, display, direction):
|
||
sm, cp = drive
|
||
cp.pcmCruise = False
|
||
sm['slcDashboardObservation'].speedMps = 45 * CV.MPH_TO_MS if metric else display * CV.MPH_TO_MS
|
||
runtime = Runtime(parse({'SpeedLimitController': True, 'SLCPriority1': 'Dashboard', 'IsMetric': metric}))
|
||
tick(runtime, sm, cp, 0)
|
||
adopted = tick(runtime, sm, cp, 3)
|
||
helper = VCruiseHelper(cp)
|
||
helper.apply_slc_target(adopted.command.slcCruiseCommand.targetMps, metric)
|
||
baseline = helper.v_cruise_kph
|
||
conversion = 1 if metric else CV.KPH_TO_MPH
|
||
assert round(baseline * conversion) == display
|
||
sm['carState'].vCruise = sm['carState'].vCruiseCluster = baseline
|
||
tick(runtime, sm, cp, 3.05, cruise_event=command_receipt(runtime, adopted.command.slcCruiseCommand, 3.05, 60., baseline))
|
||
for event_id, (step, distance) in enumerate(zip((direction, direction, -direction, -direction), (1, 2, 1, 0), strict=True), 2):
|
||
previous = float(helper.v_cruise_kph)
|
||
helper.adjust_v_cruise(car.CarState.ButtonEvent.Type.accelCruise if step > 0 else
|
||
car.CarState.ButtonEvent.Type.decelCruise, sm['carState'], metric)
|
||
chosen = float(helper.v_cruise_kph)
|
||
assert round(chosen * conversion) == display + direction * distance
|
||
sm['carState'].vCruise = sm['carState'].vCruiseCluster = chosen
|
||
seconds = 3.05 + event_id * .05
|
||
stamp = START + round(seconds * 1e9)
|
||
output = tick(runtime, sm, cp, seconds, cruise_event=CruiseEvent(event_id, stamp, previous / 3.6,
|
||
chosen / 3.6, 'accel' if chosen > previous else 'decel', 'card-producer', stamp))
|
||
assert output.result.override.event_receipt.consumed
|
||
assert output.message.slcState.driverOverrideActive == (distance != 0)
|
||
assert output.command is None
|
||
assert helper.v_cruise_kph == pytest.approx(baseline, abs=1e-6)
|
||
|
||
|
||
@pytest.mark.parametrize('fallback', [0, 2])
|
||
def test_source_loss_preserves_latest_received_limit_and_offset_without_actions(drive, fallback):
|
||
from openpilot.starpilot.ui.onroad_state import OnroadState, speed_limit_from_message, slc_controls
|
||
from openpilot.starpilot.ui.presentation import Profile
|
||
from openpilot.starpilot.ui.unified_speed_presentation import resolve_unified_speed
|
||
sm, cp = drive
|
||
settings = replace(parse({'SpeedLimitController': True, 'SLCPriority1': 'Dashboard', 'SLCConfirmation': True, 'SLCFallback': fallback}),
|
||
offsets=OffsetSchedule((OffsetBand(0., None, 2 / 3.6),)))
|
||
runtime = Runtime(settings)
|
||
first = tick(runtime, sm, cp, 0).message.slcState
|
||
tick(runtime, sm, cp, 1, request=Action(runtime.session_id, 1, first.decisionId, first.presentationId, 'accept'))
|
||
sm['carState'].vCruise = sm['carState'].vCruiseCluster = 92.
|
||
sm['slcDashboardObservation'].speedMps = 20.
|
||
newer = tick(runtime, sm, cp, 2).message.slcState
|
||
tick(runtime, sm, cp, 2.1, request=Action(runtime.session_id, 2, newer.decisionId, newer.presentationId, 'reject'))
|
||
sm['slcDashboardObservation'].status = 'unknown'
|
||
lost = tick(runtime, sm, cp, 2.2)
|
||
observation = speed_limit_from_message(lost.message.slcState)
|
||
screen = OnroadState(True, False, 20., 92., observation, metric=True)
|
||
shown = resolve_unified_speed(screen)
|
||
assert (shown.posted_text, shown.offset_text, shown.source, shown.status) == ('72', '+2', 'dashboard', 'Unavailable')
|
||
assert not shown.pending and lost.command is None
|
||
for profile in Profile:
|
||
assert not slc_controls(profile, replace(screen, slc_prompt_open=True))
|
||
runtime.reset()
|
||
reset = tick(runtime, sm, cp, 2.3)
|
||
assert resolve_unified_speed(replace(screen, speed_limit=speed_limit_from_message(reset.message.slcState))).posted_text == '–'
|
||
|
||
|
||
@pytest.mark.parametrize('limit', [12., 25.])
|
||
@pytest.mark.parametrize('pcm', [False, True])
|
||
def test_automatic_adoption_updates_set_speed_up_or_down_immediately(drive, limit, pcm):
|
||
from openpilot.starpilot.ui.onroad_state import OnroadState, speed_limit_from_message
|
||
from openpilot.starpilot.ui.speed_limit_pulse import SpeedLimitPulse
|
||
sm, cp = drive
|
||
cp.pcmCruise = pcm
|
||
sm['slcDashboardObservation'].speedMps = limit
|
||
runtime = Runtime(parse({'SpeedLimitController': True, 'SLCPriority1': 'Dashboard'}))
|
||
adopted = tick(runtime, sm, cp, 0)
|
||
assert adopted.message.slcState.hasAccepted
|
||
assert not adopted.message.slcState.automaticAdoptionPending
|
||
assert adopted.message.slcState.pendingSinceMonoTime == 0
|
||
assert adopted.command is not None
|
||
assert adopted.command.slcCruiseCommand.targetMps == pytest.approx(limit)
|
||
assert adopted.result.ceiling is None and not adopted.message.slcState.hasCeiling
|
||
assert not adopted.message.slcState.hasPending
|
||
state = adopted.message.slcState
|
||
evidence = sm['slcDashboardObservation']
|
||
observation = SimpleNamespace(status=Status.VALID, speed_mps=limit, episode=evidence.episode,
|
||
observed_ns=evidence.observedMonoTime, valid_until_ns=evidence.validUntilMonoTime)
|
||
current = physical_actions.state_current(ReplaySM({'slcState': state}, START), observation, 'card-producer', START)
|
||
assert physical_actions.command_applicable(adopted.command.slcCruiseCommand, current, observation, 'card-producer',
|
||
START, 60 / 3.6, long_active=True, button_event=False)
|
||
helper = VCruiseHelper(cp)
|
||
helper.v_cruise_kph = 60.
|
||
helper.apply_slc_target(limit, True)
|
||
sm['carState'].vCruise = sm['carState'].vCruiseCluster = helper.v_cruise_kph
|
||
assert helper.v_cruise_kph == round(limit * 3.6)
|
||
pulse = SpeedLimitPulse()
|
||
pulse.update(OnroadState(True, False, 20., helper.v_cruise_kph, speed_limit_from_message(state)), START / 1e9)
|
||
assert not pulse.display_pulsing and pulse.start_time == -float('inf')
|
||
assert tick(runtime, sm, cp, 0.05).command is None
|
||
|
||
|
||
@pytest.mark.parametrize('response', ['accept', 'reject', 'timeout'])
|
||
def test_confirmation_outcomes_preserve_set_speed_until_acceptance(drive, response):
|
||
sm, cp = drive
|
||
runtime = Runtime(parse({'SpeedLimitController': True, 'SLCConfirmation': True, 'SLCPriority1': 'Dashboard'}))
|
||
pending = tick(runtime, sm, cp, 0).message.slcState
|
||
assert pending.hasPending and not pending.automaticAdoptionPending
|
||
action = Action(runtime.session_id, 1, pending.decisionId, pending.presentationId, response) if response != 'timeout' else None
|
||
output = tick(runtime, sm, cp, 10 if action is None else 1, request=action)
|
||
assert not output.message.slcState.hasPending
|
||
assert (output.command is not None) == (response == 'accept')
|
||
assert sm['carState'].vCruise == 60.
|
||
if response == 'accept':
|
||
sm['carState'].vCruise = sm['carState'].vCruiseCluster = 90.
|
||
tick(runtime, sm, cp, 1.05, cruise_event=command_receipt(runtime, output.command.slcCruiseCommand, 1.05, 60., 90.))
|
||
assert tick(runtime, sm, cp, 11).command is None
|
||
assert not tick(runtime, sm, cp, 12).message.slcState.hasPending
|
||
|
||
|
||
@pytest.mark.parametrize('selected', [50., 100.])
|
||
def test_new_limits_still_adopt_during_manual_override_in_either_direction(drive, selected):
|
||
sm, cp = drive
|
||
runtime = Runtime(parse({'SpeedLimitController': True, 'SLCPriority1': 'Dashboard'}))
|
||
tick(runtime, sm, cp, 0)
|
||
tick(runtime, sm, cp, 3)
|
||
sm['carState'].vCruise = sm['carState'].vCruiseCluster = selected
|
||
changed = tick(runtime, sm, cp, 3.1, cruise_event=CruiseEvent(1, START + 3_100_000_000, 90 / 3.6,
|
||
selected / 3.6, 'accel' if selected > 90 else 'decel', 'card-producer', START + 3_100_000_000))
|
||
assert changed.message.slcState.driverOverrideActive
|
||
sm['slcDashboardObservation'].speedMps = 18.
|
||
adopted = tick(runtime, sm, cp, 3.2)
|
||
assert not adopted.message.slcState.hasPending
|
||
assert adopted.command.slcCruiseCommand.targetMps == pytest.approx(18.)
|
||
assert not adopted.message.slcState.driverOverrideActive
|
||
|
||
|
||
def test_accelerator_does_not_change_set_speed_or_manual_override_and_release_keeps_profile(drive):
|
||
sm, cp = drive
|
||
runtime = Runtime(parse({'SpeedLimitController': True, 'SLCPriority1': 'Dashboard'}))
|
||
tick(runtime, sm, cp, 0)
|
||
tick(runtime, sm, cp, 3)
|
||
sm['carState'].vCruise = sm['carState'].vCruiseCluster = 90.
|
||
sm['carState'].gasPressed = True
|
||
sm['carState'].vEgo = sm['carState'].vEgoCluster = 30.
|
||
pressed = tick(runtime, sm, cp, 4)
|
||
assert not pressed.message.slcState.driverOverrideActive
|
||
assert sm['carState'].vCruise == 90. and pressed.command is None
|
||
sm['carState'].gasPressed = False
|
||
released = tick(runtime, sm, cp, 5)
|
||
assert not released.message.slcState.driverOverrideActive and released.result.ceiling is None
|
||
native = planner.LongitudinalPlanner(cp)
|
||
with patch.object(planner, 'get_cruise_accel', wraps=planner.get_cruise_accel) as cruise:
|
||
native.update(sm, global_braking_response='eco')
|
||
assert cruise.call_args.args[1] == 25.
|
||
assert cruise.call_args.kwargs['base_brake_floor'] == -.6
|
||
assert native.last_cruise_ceiling_status == 'absent'
|
||
|
||
|
||
def test_pcm_software_set_speed_survives_native_speed_and_consumes_confirmation(drive):
|
||
_, cp = drive
|
||
cp.pcmCruise = True
|
||
helper = VCruiseHelper(cp)
|
||
cs = car.CarState(cruiseState={'available': True, 'enabled': True, 'speed': 60 / 3.6})
|
||
helper.update_v_cruise(cs, True, True, slc_enabled=True)
|
||
helper.apply_slc_target(25., True)
|
||
helper.update_v_cruise(cs, True, True, slc_enabled=True)
|
||
assert helper.v_cruise_kph == helper.v_cruise_cluster_kph == 90.
|
||
from openpilot.selfdrive.car.cruise import SlcPendingConfirmation
|
||
pending = SlcPendingConfirmation('drive', 1, 1)
|
||
cs.buttonEvents = [car.CarState.ButtonEvent(type='decelCruise', pressed=True)]
|
||
helper.update_v_cruise(cs, True, True, pending, slc_enabled=True)
|
||
assert helper.slc_consumed_button.button == 'decel'
|
||
cs.cruiseState.speed = 59 / 3.6
|
||
cs.buttonEvents = [car.CarState.ButtonEvent(type='decelCruise', pressed=False)]
|
||
helper.update_v_cruise(cs, True, True, slc_enabled=True)
|
||
assert helper.v_cruise_kph == 90.
|
||
helper.update_v_cruise(cs, True, True, slc_enabled=False)
|
||
assert helper.v_cruise_kph == pytest.approx(59.)
|
||
|
||
|
||
@pytest.mark.parametrize('pending', [False, True])
|
||
@pytest.mark.parametrize('signal,direction,normal_speed', [
|
||
('CcAslButtnSetIncPress', 'accel', 61.), ('CcAslButtnSetDecPress', 'decel', 59.),
|
||
])
|
||
def test_ford_parser_buttons_adjust_set_speed_or_confirm_without_a_second_step(pending, signal, direction, normal_speed):
|
||
from opendbc.can import CANPacker
|
||
from opendbc.car import Bus, gen_empty_fingerprint
|
||
from opendbc.car.ford.carstate import CarState
|
||
from opendbc.car.ford.interface import CarInterface
|
||
from opendbc.car.ford.values import CAR, DBC
|
||
from openpilot.selfdrive.car.cruise import CRUISE_LONG_PRESS, SlcPendingConfirmation
|
||
|
||
cp = CarInterface.get_params(CAR.FORD_ESCAPE_MK4, gen_empty_fingerprint(), [], True, False, False)
|
||
source = CarState(cp)
|
||
parsers = source.get_can_parsers(cp)
|
||
packer = CANPacker(DBC[cp.carFingerprint][Bus.pt])
|
||
helper = VCruiseHelper(cp)
|
||
choice = SlcPendingConfirmation('drive', 1, 1) if pending else None
|
||
now_ns = START
|
||
|
||
def frame(pressed, decision=None):
|
||
nonlocal now_ns
|
||
parsers[Bus.pt].update([(now_ns, [
|
||
packer.make_can_msg('Steering_Data_FD1', parsers[Bus.pt].bus, {signal: pressed}),
|
||
packer.make_can_msg('EngBrakeData', parsers[Bus.pt].bus, {'CcStat_D_Actl': 4, 'Veh_V_DsplyCcSet': 60}),
|
||
])])
|
||
now_ns += 10_000_000
|
||
helper.update_v_cruise(source.update(parsers), True, True, decision, slc_enabled=True)
|
||
|
||
for _ in range(3):
|
||
frame(0)
|
||
helper.apply_slc_target(60. / 3.6, True)
|
||
frame(1, choice)
|
||
consumed = helper.slc_consumed_button
|
||
assert (consumed is not None) == pending
|
||
if pending:
|
||
assert consumed.pending == choice and consumed.button == direction
|
||
if direction == 'accel':
|
||
helper.apply_slc_target(25., True)
|
||
for _ in range(CRUISE_LONG_PRESS + 1):
|
||
frame(1)
|
||
assert helper.slc_consumed_button is None and helper.slc_cruise_change is None
|
||
frame(0)
|
||
assert helper.v_cruise_kph == (90. if pending and direction == 'accel' else 60. if pending else normal_speed)
|
||
assert (helper.slc_cruise_change is None) == pending
|
||
|
||
|
||
def test_automatic_adoption_does_not_consume_confirmation_actions_and_late_manual_accept_is_declined():
|
||
candidate = acc.Candidate('dashboard', acc.ObservationIdentity(acc.IdentityKind.SESSION_VALUE, session_id='drive'), 20.)
|
||
observation = acc.Observation(acc.ObservationKind.VALID, candidate)
|
||
authority = acc.Authority(acc.Mode.LONGITUDINAL_ONLY, acc.LongitudinalOwner.SYSTEM, False, True, False, False)
|
||
for automatic in (False, True):
|
||
policy = acc.Policy(not automatic, not automatic)
|
||
first = acc.step(acc.new_session('drive'), observation, authority, policy, now_ns=0)
|
||
action = acc.DriverAction('drive', 1, first.state.next_decision_id - 1, acc.ActionKind.ACCEPT)
|
||
output = acc.step(first.state, observation, authority, policy, now_ns=1 if automatic else acc.CONFIRMATION_NS + 1, action=action)
|
||
assert not output.action_receipt.consumed
|
||
assert (output.state.accepted is not None) == automatic
|
||
|
||
|
||
@pytest.mark.parametrize('source_name', ['map', 'vision'])
|
||
def test_map_and_vision_adoption_commands_bind_the_current_source_without_dashboard_limit(drive, source_name):
|
||
from openpilot.cereal import messaging
|
||
from openpilot.starpilot.speed_limits.selection import Source
|
||
sm, cp = drive
|
||
sm['slcDashboardObservation'].status = 'unknown'
|
||
evidence = messaging.new_message('slcVisionObservation').slcVisionObservation
|
||
evidence.vision.producerSessionId = 'vision-producer'
|
||
evidence.vision.episode = 1
|
||
evidence.vision.observedMonoTime = START
|
||
evidence.vision.validUntilMonoTime = START + 10_000_000_000
|
||
sm.data['slcVisionObservation'] = evidence
|
||
source = Source(source_name)
|
||
runtime = Runtime(parse({'SpeedLimitController': True, 'SLCPriority1': 'Maps' if source == Source.MAP else 'Vision'}),
|
||
vision_enabled=True, vision_control_qualified=True)
|
||
candidate = acc.Candidate(source_name, acc.ObservationIdentity(acc.IdentityKind.GEOGRAPHIC, value='qualified-source'), 20.)
|
||
observations = dict.fromkeys(Source, acc.Observation(acc.ObservationKind.ABSENT))
|
||
observations[source] = acc.Observation(acc.ObservationKind.VALID, candidate)
|
||
with patch.object(runtime, '_observations', return_value=observations):
|
||
tick(runtime, sm, cp, 0)
|
||
output = tick(runtime, sm, cp, 3)
|
||
assert output.command is not None, (output.result.errors, output.result.status, output.result.acceptance, runtime.settings.errors)
|
||
command = output.command.slcCruiseCommand
|
||
assert command.source == source_name
|
||
current = physical_actions.state_current(ReplaySM({'slcState': output.message.slcState}, START + 3_000_000_000),
|
||
None, 'card-producer', START + 3_000_000_000)
|
||
assert physical_actions.command_applicable(command, current, None, 'card-producer', START + 3_000_000_000,
|
||
60 / 3.6, long_active=True, button_event=False)
|
||
command.source = 'dashboard'
|
||
assert not physical_actions.command_applicable(command, current, None, 'card-producer', START + 3_000_000_000,
|
||
60 / 3.6, long_active=True, button_event=False)
|
||
|
||
|
||
def test_offset_edit_does_not_cancel_manual_override(drive):
|
||
from openpilot.starpilot.speed_limits.speed_domain import OffsetBand, OffsetSchedule
|
||
sm, cp = drive
|
||
runtime = Runtime(parse({'SpeedLimitController': True, 'SLCPriority1': 'Dashboard'}))
|
||
tick(runtime, sm, cp, 0)
|
||
tick(runtime, sm, cp, 3)
|
||
sm['carState'].vCruise = sm['carState'].vCruiseCluster = 100.
|
||
tick(runtime, sm, cp, 3.1, cruise_event=CruiseEvent(1, START + 3_100_000_000, 90 / 3.6, 100 / 3.6, 'accel',
|
||
'card-producer', START + 3_100_000_000))
|
||
runtime.update_offsets(OffsetSchedule((OffsetBand(0., None, 1.),)))
|
||
output = tick(runtime, sm, cp, 3.2)
|
||
assert output.command is None and output.message.slcState.driverOverrideActive
|
||
assert sm['carState'].vCruise == 100.
|
||
|
||
|
||
@pytest.mark.parametrize('confirmation', [False, True])
|
||
def test_new_limit_can_be_adopted_while_accelerator_temporarily_suspends_longitudinal_output(drive, confirmation):
|
||
sm, cp = drive
|
||
sm['carState'].gasPressed = True
|
||
sm['carControl'].longActive = False
|
||
sm['controlsState'].longControlState = 'off'
|
||
runtime = Runtime(parse({'SpeedLimitController': True, 'SLCPriority1': 'Dashboard', 'SLCConfirmation': confirmation}))
|
||
first = tick(runtime, sm, cp, 0).message.slcState
|
||
action = Action(runtime.session_id, 1, first.decisionId, first.presentationId, 'accept') if confirmation else None
|
||
output = tick(runtime, sm, cp, 1 if confirmation else 3, request=action)
|
||
assert output.command is not None
|
||
assert output.command.slcCruiseCommand.targetMps == 25.
|
||
assert not output.message.slcState.driverOverrideActive
|
||
assert not output.result.acceptance.authority.longitudinal_active
|
||
assert output.result.acceptance.authority.accelerator_override
|
||
assert output.result.ceiling is None
|
||
|
||
|
||
def test_manual_changes_during_source_loss_remain_overrides_after_reconnection(drive):
|
||
sm, cp = drive
|
||
runtime = Runtime(parse({'SpeedLimitController': True, 'SLCPriority1': 'Dashboard', 'SLCFallback': 0}))
|
||
tick(runtime, sm, cp, 0)
|
||
tick(runtime, sm, cp, 3)
|
||
sm['slcDashboardObservation'].status = 'unknown'
|
||
tick(runtime, sm, cp, 3.1)
|
||
sm['carState'].vCruise = sm['carState'].vCruiseCluster = 100.
|
||
tick(runtime, sm, cp, 3.2, cruise_event=CruiseEvent(1, START + 3_200_000_000, 90 / 3.6, 100 / 3.6, 'accel',
|
||
'card-producer', START + 3_200_000_000))
|
||
sm['slcDashboardObservation'].status = 'valid'
|
||
output = tick(runtime, sm, cp, 3.3)
|
||
assert output.message.slcState.driverOverrideActive and output.command is None
|
||
assert sm['carState'].vCruise == 100.
|
||
|
||
|
||
def test_pcm_confirmation_release_is_retired_even_if_slc_is_switched_off(drive):
|
||
from openpilot.selfdrive.car.cruise import SlcPendingConfirmation
|
||
_, cp = drive
|
||
cp.pcmCruise = True
|
||
helper = VCruiseHelper(cp)
|
||
cs = car.CarState(cruiseState={'available': True, 'speed': 20.},
|
||
buttonEvents=[car.CarState.ButtonEvent(type='accelCruise', pressed=True)])
|
||
helper.update_v_cruise(cs, True, True, SlcPendingConfirmation('drive', 1, 1), slc_enabled=True)
|
||
assert helper.slc_suppressed_buttons
|
||
cs.buttonEvents = [car.CarState.ButtonEvent(type='accelCruise', pressed=False)]
|
||
helper.update_v_cruise(cs, True, True, slc_enabled=False)
|
||
assert not helper.slc_suppressed_buttons
|
||
assert not any(helper.button_timers.values())
|