diff --git a/openpilot/cereal/custom.capnp b/openpilot/cereal/custom.capnp index 3b937b78e7..12cdbaa7e8 100644 --- a/openpilot/cereal/custom.capnp +++ b/openpilot/cereal/custom.capnp @@ -535,6 +535,7 @@ struct SlcCruiseEvent @0xcd96dafb67a082d0 { switchbackMode @9; wheelAction @10; teslaStockCruise @11; + engagementAdopt @12; } struct TrafficModeGesture { diff --git a/openpilot/selfdrive/car/card.py b/openpilot/selfdrive/car/card.py index af3a89d088..e58b717302 100644 --- a/openpilot/selfdrive/car/card.py +++ b/openpilot/selfdrive/car/card.py @@ -843,9 +843,24 @@ class Car: slc_enabled=getattr(self, 'slc_control_enabled', getattr(self, 'slc_replay', False)), slc_approved=approved) if self.sm['carControl'].enabled and not self.CC_prev.enabled: + initial = None + now_ns = int(self.can_log_mono_time) if REPLAY and hasattr(self, 'can_log_mono_time') else time.monotonic_ns() + if (getattr(self, 'slc_control_enabled', False) and self.CP.openpilotLongitudinalControl and + CS.canValid and not CS.canTimeout and CS.cruiseState.available and + self.sm.valid['carControl'] and self.sm.alive['carControl'] and + 0 < int(self.sm.logMonoTime['carControl']) <= now_ns and + now_ns - int(self.sm.logMonoTime['carControl']) <= slc_physical.STATE_MAX_AGE_NS and + (self.sm['carControl'].longActive or CS.gasPressed)): + evidence = getattr(self.CI.CS, 'dashboard_limit', None) + initial = slc_physical.engagement_state( + self.sm, evidence.observation if evidence is not None else None, self.slc_producer_session, + now_ns, after_ns=getattr(self, 'slc_source_floor_ns', 0)) # Initialization owns this edge; evaluate commands against its final speed. - self.v_cruise_helper.initialize_v_cruise(self.CS_prev, self.experimental_mode, - resume=self.vehicle_startup.consume_cruise_resume(), initial_speed=self.initial_cruise_speed) + adopted = self.v_cruise_helper.initialize_v_cruise( + self.CS_prev, self.experimental_mode, resume=self.vehicle_startup.consume_cruise_resume(), initial_speed=self.initial_cruise_speed, + slc_target_mps=float(initial.presentationAdjustedLimit) if initial is not None else None, is_metric=self.is_metric) + if adopted: + self.slc_receipts.append(('engagementAdopt', initial, 0.0, float(self.v_cruise_helper.v_cruise_kph / 3.6), now_ns)) def aol_cancel_source(self, CS, now_ns: int) -> int: """Bind Cancel to this parsed CAN batch and its local monotonic receipt.""" @@ -1162,9 +1177,11 @@ class Car: record.button = 'accel' else: record.sessionId = str(detail.sessionId) - record.commandId = int(detail.commandId) + record.commandId = 0 if kind == 'engagementAdopt' else int(detail.commandId) record.decisionId = int(detail.decisionId) record.presentationId = int(detail.presentationId) + if kind == 'engagementAdopt': + record.button = 'decel' self.pm.send('slcCruiseEvent', event) if getattr(self, 'conditional_replay', False) and self.manual_receipt is not None: diff --git a/openpilot/selfdrive/car/cruise.py b/openpilot/selfdrive/car/cruise.py index 80873f1245..c632b6dcfd 100644 --- a/openpilot/selfdrive/car/cruise.py +++ b/openpilot/selfdrive/car/cruise.py @@ -300,12 +300,15 @@ class VCruiseHelper: self.toyota.constrain(self) self.toyota.requested = (self.v_cruise_kph, self.v_cruise_cluster_kph) - def initialize_v_cruise(self, CS, experimental_mode: bool, *, resume: bool | None = None, initial_speed: object = V_CRUISE_INITIAL) -> None: - # initializing is handled by the PCM - if self.CP.pcmCruise: - return - - + def initialize_v_cruise(self, CS, experimental_mode: bool, *, resume: bool | None = None, + initial_speed: object = V_CRUISE_INITIAL, slc_target_mps: float | None = None, is_metric: bool = False) -> bool: + """Return whether fresh SET adopts the qualified current SLC limit.""" + if (not self.CP.openpilotLongitudinalControl or type(slc_target_mps) not in (int, float) or + not math.isfinite(slc_target_mps) or not V_CRUISE_MIN / 3.6 - 1e-5 <= slc_target_mps <= V_CRUISE_MAX / 3.6 + 1e-5): + slc_target_mps = None + # PCM initializes when SLC does not own SET. + if self.CP.pcmCruise and (not self.slc_pcm or slc_target_mps is None): + return False # A car-owned acknowledgment can outlive the physical button event. if resume is None: age = monotonic_ns() - self.pending_resume_ns @@ -313,8 +316,17 @@ class VCruiseHelper: b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for b in CS.buttonEvents) self.pending_resume_ns = 0 if resume and self.v_cruise_initialized: - self.v_cruise_kph = self.v_cruise_kph_last + if not self.CP.pcmCruise: + self.v_cruise_kph = self.v_cruise_kph_last + self.v_cruise_cluster_kph = self.v_cruise_kph + return False + elif slc_target_mps is not None: + # Toyota still needs its native capacity/release approval before application. + if self.toyota is None: + self.apply_slc_target(slc_target_mps, is_metric) + return True else: self.v_cruise_kph = int(round(np.clip(max(CS.vEgo * CV.MS_TO_KPH, validated_initial_cruise_speed(initial_speed)), V_CRUISE_INITIAL, V_CRUISE_MAX))) self.v_cruise_cluster_kph = self.v_cruise_kph + return False diff --git a/openpilot/starpilot/speed_limits/README.md b/openpilot/starpilot/speed_limits/README.md index 3fc0ed4bed..4b0ea4aba1 100644 --- a/openpilot/starpilot/speed_limits/README.md +++ b/openpilot/starpilot/speed_limits/README.md @@ -9,6 +9,15 @@ Lead, curve, stop and actuator safety constraints retain their existing owners. ## New limits and adoption +On fresh SET engagement, a currently qualified limit from the selected source +initializes SET with its existing offset and mph/km/h rounding, even with Automatic +adoption off. Sources keep updating while disengaged. This explicit adoption clears +the matching prompt, decline or set-speed override; later limits keep the normal +confirmation policy. RESUME restores the previous SET. Without a qualified current +limit, normal cruise initialization applies. An early Maps proposal does not replace +the current road's limit at engagement. Toyota still requires native cruise capacity +and release approval before applying the target. + A different numeric limit from the selected source starts a new decision, including while a set-speed override is active. A source/road identity change at the same numeric limit does not start another decision. A decline remains effective across diff --git a/openpilot/starpilot/speed_limits/physical_actions.py b/openpilot/starpilot/speed_limits/physical_actions.py index 0c7407a99d..138c5d53f3 100644 --- a/openpilot/starpilot/speed_limits/physical_actions.py +++ b/openpilot/starpilot/speed_limits/physical_actions.py @@ -65,6 +65,22 @@ def state_current(sm, observation, producer_session: str, now_ns: int): return state +def engagement_state(sm, observation, producer_session: str, now_ns: int, *, after_ns: int = 0): + state = _current_state(sm, now_ns) + if (state is None or int(state.frameMonoTime) <= after_ns or int(state.presentationId) <= 0 or + str(state.source) == 'online' or not state.hasPresentationAdjustedLimit): + return None + if str(state.source) == 'dashboard' and ( + observation is None or observation.observed_ns <= after_ns or not source_matches(state, observation, producer_session, now_ns)): + return None + if str(state.source) == 'vision' and not ( + state.sourceProducerSessionId and state.sourceEpisode > 0 and + 0 < state.sourceObservedMonoTime <= now_ns <= state.sourceValidUntilMonoTime): + return None + target = float(state.presentationAdjustedLimit) + return state if finite_speed(target) and V_CRUISE_MIN / 3.6 - 1e-5 <= target <= V_CRUISE_MAX / 3.6 + 1e-5 else None + + def pending_confirmation(sm, now_ns: int, *, long_active: bool, after_ns: int = 0) -> SlcPendingConfirmation | None: if not long_active: diff --git a/openpilot/starpilot/speed_limits/runtime.py b/openpilot/starpilot/speed_limits/runtime.py index 4598025266..ce916e5079 100644 --- a/openpilot/starpilot/speed_limits/runtime.py +++ b/openpilot/starpilot/speed_limits/runtime.py @@ -253,27 +253,38 @@ class Runtime: self.ledger = completed.state return classified if not classified.errors and classified.change is not None else None - def _physical_action(self, event: CruiseEvent, sm, now_ns: int) -> tuple[acc.DriverAction | None, int | None]: - if (event.kind not in ('confirmationAccept', 'confirmationReject') or + def _physical_action(self, event: CruiseEvent, sm, now_ns: int) -> tuple[acc.DriverAction | None, acc.AdoptRequest | None, int | None]: + if (event.kind not in ('confirmationAccept', 'confirmationReject', 'engagementAdopt') or not self._event_current(event, sm, now_ns)): - return None, None + return None, None, None self._mark_event(event) + engagement = event.kind == 'engagementAdopt' pending = self.state.acceptance.pending shown = self.state.acceptance.presentation - if (event.session_id != self.session_id or pending is None or shown is None or + if shown is None and engagement: + shown = self.current_presentation + if (event.session_id != self.session_id or shown is None or event.producer_session != str(sm['slcDashboardObservation'].producerSessionId) or - event.decision_id != pending.decision_id or event.presentation_id != shown.presentation_id or - pending.candidate != shown.candidate or - event.button != ('accel' if event.kind == 'confirmationAccept' else 'decel')): - return None, None + event.presentation_id != shown.presentation_id or event.button != ('accel' if event.kind == 'confirmationAccept' else 'decel')): + return None, None, None + if engagement: + if (event.command_id != 0 or not _number(event.selected_mps, positive=True) or + not _number(float(sm['carState'].vCruise), positive=True) or + abs(float(sm['carState'].vCruise) / 3.6 - event.selected_mps) > 0.1): + return None, None, None + elif pending is None or event.decision_id != pending.decision_id or pending.candidate != shown.candidate: + return None, None, None action_id = self.ledger.last_action_id + 1 - begun = arb.step(self.ledger, arb.BeginAction(self.session_id, action_id, arb.Origin.DRIVER_CRUISE, - f'pending:{pending.decision_id}'), now_ns=now_ns) + context = f'presentation:{shown.presentation_id}' if engagement else f'pending:{pending.decision_id}' + begun = arb.step(self.ledger, arb.BeginAction(self.session_id, action_id, arb.Origin.DRIVER_CRUISE, context), now_ns=now_ns) self.ledger = begun.state if begun.errors or begun.status != 'begun': - return None, None + return None, None, None + if engagement: + return None, acc.AdoptRequest(self.session_id, action_id, shown.presentation_id, shown.candidate), action_id + assert pending is not None kind = acc.ActionKind.ACCEPT if event.kind == 'confirmationAccept' else acc.ActionKind.REJECT - return acc.DriverAction(self.session_id, action_id, pending.decision_id, kind), action_id + return acc.DriverAction(self.session_id, action_id, pending.decision_id, kind), None, action_id def _command_receipt(self, event: CruiseEvent, sm, now_ns: int) -> None: if (event.kind not in ('commandApplied', 'commandRejected') or @@ -390,6 +401,11 @@ class Runtime: ego, bool(car_state.gasPressed) if car_fresh else False, host, action=action, adopt=adopt, classified_cruise=classified, is_metric=self.is_metric, set_speed_ready=self._set_speed_ready(sm)) current = sel.select_limit(observations, self.settings.selection).observation.candidate + shown = self.state.acceptance.presentation + if self.advance.encounter is not None and shown is not None and current != self.last_received: + identity = self.state.acceptance.next_presentation_id + self.state = replace(self.state, acceptance=replace(self.state.acceptance, + presentation=acc.Presentation(identity, shown.candidate), next_presentation_id=identity+1)) if (adopt is not None and self.advance.encounter is not None and adopt.candidate == self.last_received == current): self.advance.use_current(adopt.candidate) @@ -413,11 +429,6 @@ class Runtime: shown = self.current_presentation if shown is None or shown.presentation_id != request.presentation_id: return None, None, None - if request.kind == 'adopt' and self.advance.encounter is not None and self.state.acceptance.pending is None and self.last_received is not None: - # The adopt wheel action still means the displayed current limit. - shown = replace(shown, candidate=self.last_received) - self.state = replace(self.state, acceptance=replace(self.state.acceptance, presentation=shown, - observation=acc.Observation(acc.ObservationKind.VALID, shown.candidate))) action_id = self.ledger.last_action_id + 1 context = self.state.override.context context_id = context.context_id if context is not None else "slc-presentation" @@ -445,7 +456,14 @@ class Runtime: self._command_receipt(cruise_event, sm, now_ns) action, adopt, action_id = self._action(request, now_ns) if cruise_event is not None and request is None: - action, action_id = self._physical_action(cruise_event, sm, now_ns) + action, adopt, action_id = self._physical_action(cruise_event, sm, now_ns) + if (adopt is not None and self.advance.encounter is not None and self.last_received is not None and + (request is None or self.state.acceptance.pending is None)): + # Explicit adoption uses the current road, including SET over an old early prompt. + shown = acc.Presentation(adopt.presentation_id, self.last_received) + adopt = replace(adopt, candidate=shown.candidate) + self.state = replace(self.state, acceptance=replace(self.state.acceptance, presentation=shown, pending=None, timer_running=False, + observation=acc.Observation(acc.ObservationKind.VALID, shown.candidate))) frame = self._frame(sm, CP, now_ns, now_boot_ns, action, adopt, classified) result = comp.step(self.state, frame) self.state = result.state diff --git a/openpilot/starpilot/speed_limits/tests/test_engagement.py b/openpilot/starpilot/speed_limits/tests/test_engagement.py new file mode 100644 index 0000000000..8156be7f64 --- /dev/null +++ b/openpilot/starpilot/speed_limits/tests/test_engagement.py @@ -0,0 +1,337 @@ +"""A qualified current limit is ready before fresh SET, with RESUME preserved.""" +from collections import deque +from dataclasses import replace +from types import SimpleNamespace +from unittest.mock import Mock, patch +import math + +import pytest + +from opendbc.car.dashboard_speed_limit import Status +from opendbc.car.structs import car +from openpilot.cereal import messaging +from openpilot.common.constants import CV +from openpilot.selfdrive.car import card, cruise +from openpilot.selfdrive.controls.plannerd import queue_cruise_event +from openpilot.starpilot.speed_limits import 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.speed_domain import OffsetBand, OffsetSchedule +from openpilot.starpilot.speed_limits.tests import test_runtime_replay as replay_fixture +from openpilot.starpilot.speed_limits.tests.test_maps_runtime import packet +from openpilot.starpilot.speed_limits.vision.observation import MODEL_ID + + +START = 2_000_000_000 + + +def drive(*, metric=False, limit=45., offset=5., confirmation=True): + fixture = replay_fixture.RuntimeReplayTests() + fixture.setUp() + sm, cp = fixture.sm, fixture.cp + cp.pcmCruise = False + sm['carState'].canValid = sm['carState'].cruiseState.available = True + sm['carState'].vCruise = sm['carState'].vCruiseCluster = cruise.V_CRUISE_UNSET + sm['carControl'].enabled = sm['carControl'].longActive = False + units = CV.KPH_TO_MS if metric else CV.MPH_TO_MS + sm['slcDashboardObservation'].speedMps = limit * units + settings = replace(parse({'SpeedLimitController': True, 'SLCPriority1': 'Dashboard', 'SLCConfirmation': confirmation}), + offsets=OffsetSchedule((OffsetBand(0., None, offset * units),))) + runtime = Runtime(settings, is_metric=metric) + state = runtime.step(sm, cp, now_ns=START).message.slcState + sm.data['slcState'] = state + for flags in (sm.valid, sm.alive, sm.updated): + flags['slcState'] = True + sm.logMonoTime['slcState'] = START + return runtime, sm, cp + + +def owner(sm, cp, *, metric=False, resume=None): + instance = card.Car.__new__(card.Car) + instance.CP, instance.sm = cp, sm + instance.CC_prev = car.CarControl(enabled=False) + instance.CS_prev = car.CarState(vEgo=sm['carState'].vEgo) + instance.is_metric, instance.experimental_mode, instance.initial_cruise_speed = metric, False, 40 + instance.slc_control_enabled = instance.slc_replay = True + instance.slc_source_floor_ns = 0 + instance.slc_producer_session, instance.slc_cruise_event_id = 'card-producer', 0 + instance.slc_receipts = [] + instance.v_cruise_helper = cruise.VCruiseHelper(cp) + instance.vehicle_startup = SimpleNamespace(consume_cruise_resume=lambda: resume) + evidence = sm['slcDashboardObservation'] + observation = SimpleNamespace(status=Status.VALID, speed_mps=float(evidence.speedMps), episode=int(evidence.episode), + observed_ns=int(evidence.observedMonoTime), valid_until_ns=int(evidence.validUntilMonoTime)) + instance.CI = SimpleNamespace(CS=SimpleNamespace(dashboard_limit=SimpleNamespace(observation=observation))) + return instance + + +def engage(instance, *, now=START + 10_000_000): + sm = instance.sm + sm.advance(now) + sm['carControl'].enabled = sm['carControl'].longActive = True + cs = car.CarState(canValid=sm['carState'].canValid, canTimeout=sm['carState'].canTimeout, + vEgo=sm['carState'].vEgo, cruiseState={'available': True, 'enabled': True, 'speed': 60 / 3.6, 'speedCluster': 60 / 3.6}) + with patch.object(card.time, 'monotonic_ns', return_value=now), patch.object(cruise, 'monotonic_ns', return_value=now): + instance.update_cruise_speed(cs) + sm['carState'].vCruise = instance.v_cruise_helper.v_cruise_kph + sm['carState'].vCruiseCluster = instance.v_cruise_helper.v_cruise_cluster_kph + return cs + + +def receipt(instance, *, now=START + 10_000_000, event_id=1): + kind, state, previous, selected, observed = instance.slc_receipts[-1] + return CruiseEvent(event_id, observed, previous, selected, 'decel', instance.slc_producer_session, now, + kind, str(state.sessionId), int(state.decisionId), int(state.presentationId)) + + +@pytest.mark.parametrize('confirmation', [False, True]) +@pytest.mark.parametrize('metric,limit,offset', [(False, 45., 5.), (False, 20., -2.), (False, 55., -.4), + (True, 72., 5.), (True, 30., -2.), (True, 72., -.4)]) +def test_prepared_limit_initializes_set_once_and_adopts_it(metric, limit, offset, confirmation): + runtime, sm, cp = drive(metric=metric, limit=limit, offset=offset, confirmation=confirmation) + prepared = sm['slcState'] + assert prepared.observationKind == 'valid' and prepared.hasPresentationAdjustedLimit + assert not sm['carControl'].enabled and prepared.presentationId > 0 + instance = owner(sm, cp, metric=metric) + engage(instance) + expected = cruise.slc_target_kph(float(prepared.presentationAdjustedLimit), metric) + assert instance.v_cruise_helper.v_cruise_kph == expected + assert instance.v_cruise_helper.v_cruise_cluster_kph == expected + adopted = runtime.step(sm, cp, now_ns=START + 10_000_000, cruise_event=receipt(instance)) + assert not adopted.result.errors and adopted.message.slcState.hasAccepted + assert not adopted.message.slcState.hasPending and not adopted.message.slcState.driverOverrideActive + assert adopted.message.slcState.actionStatus == 'adopt' and adopted.command is None + assert runtime.ledger.transactions == () + instance.CC_prev.enabled = True + instance.slc_receipts.clear() + engage(instance, now=START + 20_000_000) + assert instance.v_cruise_helper.v_cruise_kph == expected and instance.slc_receipts == [] + + +@pytest.mark.parametrize('invalid', ['disabled', 'stale', 'unknown', 'stock-long', 'can-fault', 'wrong-episode', 'configuration']) +def test_unavailable_limit_keeps_normal_initialization(invalid): + _, sm, cp = drive() + instance = owner(sm, cp) + if invalid == 'disabled': + instance.slc_control_enabled = False + elif invalid == 'stale': + sm['slcState'].frameMonoTime = START - physical_actions.STATE_MAX_AGE_NS + elif invalid == 'unknown': + sm['slcState'].observationKind = 'unknown' + elif invalid == 'stock-long': + cp.openpilotLongitudinalControl = False + elif invalid == 'can-fault': + sm['carState'].canTimeout = True + elif invalid == 'wrong-episode': + instance.CI.CS.dashboard_limit.observation.episode += 1 + else: + instance.slc_source_floor_ns = START + engage(instance) + assert instance.v_cruise_helper.v_cruise_kph == 72. # Existing vEgo-based initialization. + assert instance.slc_receipts == [] + + +def test_first_engagement_without_limit_keeps_default_floor(): + _, sm, cp = drive() + sm['slcState'].observationKind = 'unknown' + sm['carState'].vEgo = sm['carState'].vEgoCluster = 0. + instance = owner(sm, cp) + engage(instance) + assert instance.v_cruise_helper.v_cruise_kph == cruise.V_CRUISE_INITIAL + assert instance.slc_receipts == [] + + +@pytest.mark.parametrize('resume', [True, None]) +def test_resume_restores_previous_set_instead_of_available_limit(resume): + _, sm, cp = drive() + instance = owner(sm, cp, resume=resume) + instance.v_cruise_helper.v_cruise_kph = instance.v_cruise_helper.v_cruise_cluster_kph = 105. + if resume is None: + instance.CS_prev.buttonEvents = [car.CarState.ButtonEvent(type='accelCruise', pressed=False)] + engage(instance) + assert instance.v_cruise_helper.v_cruise_kph == 105. + assert instance.slc_receipts == [] + + +def test_card_publication_routes_engagement_to_runtime_and_preserves_later_confirmation(): + runtime, sm, cp = drive() + instance = owner(sm, cp) + cs = engage(instance) + instance.can_log_mono_time = START + 10_000_000 + instance.car_params_published, instance.aol_replay, instance.curve_replay = True, False, False + instance.last_actuators_output = car.CarControl.Actuators() + instance.can_rcv_cum_timeout_counter = 0 + instance.rk = SimpleNamespace(remaining=0.) + instance.pm, instance.car_gps_publisher = Mock(), Mock() + sm.frame, sm.all_checks = 1, lambda _: True + cs.vCruise, cs.vCruiseCluster = instance.v_cruise_helper.v_cruise_kph, instance.v_cruise_helper.v_cruise_cluster_kph + with patch.object(card, 'REPLAY', True): + instance.state_publish(cs, None) + published = next(call.args[1] for call in instance.pm.send.call_args_list if call.args[0] == 'slcCruiseEvent') + events = deque() + queue_cruise_event(published.as_reader(), events, deque()) + assert len(events) == 1 and events[0].kind == 'engagementAdopt' + first = runtime.step(sm, cp, now_ns=START + 10_000_000, cruise_event=events.popleft()) + assert first.message.slcState.hasAccepted and first.command is None + sm.advance(START + 50_000_000) + sm['slcDashboardObservation'].speedMps = 55 * CV.MPH_TO_MS + later = runtime.step(sm, cp, now_ns=START + 50_000_000) + assert later.message.slcState.hasPending and later.command is None + + +@pytest.mark.parametrize('change', ['session', 'presentation', 'producer', 'replaced-source', 'changed-set', 'stale', 'replay']) +def test_engagement_receipt_cannot_adopt_unrelated_or_stale_work(change): + runtime, sm, cp = drive() + instance = owner(sm, cp) + engage(instance) + event = receipt(instance) + if change == 'session': + event = replace(event, session_id='old-drive') + elif change == 'presentation': + event = replace(event, presentation_id=event.presentation_id + 1) + elif change == 'producer': + event = replace(event, producer_session='old-card') + elif change == 'replaced-source': + sm['slcDashboardObservation'].episode += 1 + sm['slcDashboardObservation'].observedMonoTime = START + 5_000_000 + elif change == 'changed-set': + sm['carState'].vCruise += 5. + elif change == 'stale': + event = replace(event, observed_ns=START - 100_000_000) + else: + runtime.step(sm, cp, now_ns=START + 10_000_000, cruise_event=event) + output = runtime.step(sm, cp, now_ns=START + 10_000_000, cruise_event=event) + assert output.message.slcState.actionStatus != 'adopt' and output.command is None + assert runtime.ledger.transactions == () + + +@pytest.mark.parametrize('target', [None, True, 0., -1., math.nan, math.inf, 146 / 3.6]) +def test_initializer_rejects_invalid_targets(target): + helper = cruise.VCruiseHelper(car.CarParams(pcmCruise=False, openpilotLongitudinalControl=True)) + assert not helper.initialize_v_cruise(car.CarState(), False, resume=False, slc_target_mps=target) + assert helper.v_cruise_kph == cruise.V_CRUISE_INITIAL + + +def test_set_reengagement_clears_previous_decline(): + runtime, sm, cp = drive() + sm['carControl'].enabled = sm['carControl'].longActive = True + sm.advance(START + 1_000_000) + pending = runtime.step(sm, cp, now_ns=START + 1_000_000).message.slcState + sm.advance(START + 2_000_000) + rejected = runtime.step(sm, cp, now_ns=START + 2_000_000, + request=Action(runtime.session_id, 1, pending.decisionId, pending.presentationId, 'reject')) + assert not rejected.message.slcState.hasPending and not rejected.message.slcState.hasAccepted + sm.data['slcState'] = rejected.message.slcState + instance = owner(sm, cp) + engage(instance) + adopted = runtime.step(sm, cp, now_ns=START + 10_000_000, cruise_event=receipt(instance)) + assert adopted.message.slcState.actionStatus == 'adopt' and adopted.message.slcState.hasAccepted + assert not adopted.message.slcState.hasPending and adopted.command is None + + +def test_set_reengagement_clears_override_even_after_previous_application(): + runtime, sm, cp = drive() + instance = owner(sm, cp) + engage(instance) + runtime.step(sm, cp, now_ns=START + 10_000_000, cruise_event=receipt(instance)) + previous = float(sm['carState'].vCruise) / 3.6 + sm.advance(START + 20_000_000) + sm['carState'].vCruise = sm['carState'].vCruiseCluster = 105. + overridden = runtime.step(sm, cp, now_ns=START + 20_000_000, + cruise_event=CruiseEvent(2, START + 20_000_000, previous, 105 / 3.6, 'accel', 'card-producer', START + 20_000_000)) + assert overridden.message.slcState.driverOverrideActive + sm.data['slcState'] = overridden.message.slcState + instance.CC_prev.enabled = False + instance.slc_receipts.clear() + engage(instance, now=START + 30_000_000) + adopted = runtime.step(sm, cp, now_ns=START + 30_000_000, + cruise_event=receipt(instance, now=START + 30_000_000, event_id=3)) + assert adopted.message.slcState.actionStatus == 'adopt' and not adopted.message.slcState.driverOverrideActive + assert adopted.message.slcState.commandStatus == 'applied' and adopted.command is None + + +@pytest.mark.parametrize('source', ['Maps', 'Vision']) +@pytest.mark.parametrize('dashboard_available', [True, False]) +def test_engagement_uses_existing_priority_and_qualified_secondary_source(source, dashboard_available): + runtime, sm, cp = drive() + boot = START + 4_000_000_000 + settings = replace(runtime.settings, selection=parse({'SLCPriority1': 'Dashboard', 'SLCPriority2': source}).selection) + runtime = Runtime(settings, vision_enabled=True, vision_control_qualified=True) + sm['slcDashboardObservation'].status = 'valid' if dashboard_available else 'unknown' + if source == 'Maps': + road = packet(speedLimit=30., gpsMonoTime=boot - 50_000_000, computedMonoTime=boot) + sm.data['mapdOut'] = road.mapdOut + service = 'mapdOut' + else: + vision = messaging.new_message('slcVisionObservation').slcVisionObservation + vision.vision = {'status': 'valid', 'modelId': MODEL_ID, 'stream': 'road', 'speedMps': 30., 'confidence': .9, + 'supportCount': 2, 'episode': 1, 'producerSessionId': 'vision', 'frameId': 1, + 'observedMonoTime': START - 50_000_000, 'validUntilMonoTime': START + 200_000_000, 'cameraFrameEofBootTime': boot - 50_000_000} + sm.data['slcVisionObservation'] = vision + service = 'slcVisionObservation' + for flags in (sm.valid, sm.alive, sm.updated): + flags[service] = True + sm.logMonoTime[service] = boot if source == 'Maps' else START + prepared = runtime.step(sm, cp, now_ns=START, now_boot_ns=boot).message.slcState + expected_source = 'dashboard' if dashboard_available else ('map' if source == 'Maps' else 'vision') + assert prepared.source == expected_source and prepared.hasPresentationAdjustedLimit + sm.data['slcState'] = prepared + instance = owner(sm, cp) + engage(instance) + expected = cruise.slc_target_kph(float(prepared.presentationAdjustedLimit), False) + assert instance.v_cruise_helper.v_cruise_kph == expected + # ReplaySM.advance uses monotonic stamps for every service; mapd uses boottime. + if source == 'Maps': + sm.logMonoTime['mapdOut'] = boot + adopted = runtime.step(sm, cp, now_ns=START + 10_000_000, now_boot_ns=boot + 10_000_000, cruise_event=receipt(instance)) + assert adopted.message.slcState.acceptedSource == expected_source and not adopted.message.slcState.hasPending + + +@pytest.mark.parametrize('confirmation', [False, True]) +def test_set_engagement_uses_current_limit_over_previous_early_proposal(confirmation): + from openpilot.starpilot.speed_limits.tests.test_advance import Drive + preview = Drive(confirm=confirmation) + output = preview.tick() + assert output.message.slcState.upcomingBoundaryId + preview.sm['carControl'].enabled = preview.sm['carControl'].longActive = False + prepared = preview.tick().message.slcState + sm, cp = preview.sm, preview.cp + sm.data['slcState'] = prepared + sm.valid['slcState'] = sm.alive['slcState'] = True + sm.logMonoTime['slcState'] = preview.now + instance = owner(sm, cp, metric=True) + stamp = preview.now + 10_000_000 + engage(instance, now=stamp) + assert instance.v_cruise_helper.v_cruise_kph == 79. + adopted = preview.tick(at=stamp, event=receipt(instance, now=stamp)) + assert adopted.message.slcState.acceptedSpeedLimit == 20. + assert not adopted.message.slcState.hasPending and not adopted.message.slcState.upcomingBoundaryId + assert adopted.message.slcState.actionStatus == 'adopt' and adopted.command is None + + +@pytest.mark.parametrize('current', [20., 25.]) +def test_delayed_engagement_is_bound_to_current_limit(current): + from openpilot.starpilot.speed_limits.tests.test_advance import Drive + preview = Drive(confirm=True) + assert preview.tick().message.slcState.upcomingBoundaryId + preview.sm['carControl'].enabled = preview.sm['carControl'].longActive = False + prepared = preview.tick().message.slcState + sm, cp = preview.sm, preview.cp + sm.data['slcState'] = prepared + sm.valid['slcState'] = sm.alive['slcState'] = True + sm.logMonoTime['slcState'] = preview.now + instance = owner(sm, cp, metric=True) + stamp = preview.now + 10_000_000 + engage(instance, now=stamp) + event = receipt(instance, now=stamp) + pending = preview.tick(at=stamp + 40_000_000, current=current).message.slcState + delayed = preview.tick(at=stamp + 90_000_000, current=current, event=event) + state = delayed.message.slcState + assert not delayed.result.errors and delayed.command is None + assert sm['carState'].vCruise == 79. and state.acceptedSpeedLimit == 20. + assert (state.actionStatus == 'adopt') == (current == 20.) + assert state.hasPending == (current != 20.) + if current != 20.: + assert state.presentationId != event.presentation_id + assert state.decisionId == pending.decisionId + assert state.pendingSinceMonoTime == pending.pendingSinceMonoTime diff --git a/openpilot/starpilot/speed_limits/tests/test_physical_actions.py b/openpilot/starpilot/speed_limits/tests/test_physical_actions.py index 68b922f7cb..102eb65ca1 100644 --- a/openpilot/starpilot/speed_limits/tests/test_physical_actions.py +++ b/openpilot/starpilot/speed_limits/tests/test_physical_actions.py @@ -61,3 +61,22 @@ def test_preserve_authority_and_configuration_barriers(sm, long_active, after_ns def test_fresh_post_configuration_decision_qualifies(sm): assert physical_actions.pending_confirmation(sm, NOW, long_active=True, after_ns=NOW - 1) is not None + + +@pytest.mark.parametrize('field,value', [ + ('enabled', False), ('displayOnly', True), ('observationKind', 'stale'), ('source', 'online'), + ('presentationId', 0), ('frameMonoTime', NOW - physical_actions.STATE_MAX_AGE_NS - 1), + ('hasPresentationAdjustedLimit', False), ('presentationAdjustedLimit', 0.), + ('presentationAdjustedLimit', math.nan), ('presentationAdjustedLimit', math.inf), + ('presentationAdjustedLimit', 146 / 3.6), ('presentationAdjustedLimit', 7 / 3.6), +]) +def test_engagement_rejects_unqualified_current_target(sm, field, value): + sm['slcState'].hasPresentationAdjustedLimit, sm['slcState'].presentationAdjustedLimit = True, 25. + setattr(sm['slcState'], field, value) + assert physical_actions.engagement_state(sm, None, 'card', NOW) is None + + +@pytest.mark.parametrize('target', [8 / 3.6, 145 / 3.6]) +def test_engagement_accepts_float32_cruise_boundaries(sm, target): + sm['slcState'].hasPresentationAdjustedLimit, sm['slcState'].presentationAdjustedLimit = True, target + assert physical_actions.engagement_state(sm, None, 'card', NOW) is not None diff --git a/openpilot/starpilot/speed_limits/tests/test_toyota_execution.py b/openpilot/starpilot/speed_limits/tests/test_toyota_execution.py index b2de2e40bc..776eb9410b 100644 --- a/openpilot/starpilot/speed_limits/tests/test_toyota_execution.py +++ b/openpilot/starpilot/speed_limits/tests/test_toyota_execution.py @@ -84,6 +84,22 @@ def test_upcoming_approval_keeps_native_capacity_and_release_checks(helper): assert helper.toyota.status == 'ready' and helper.toyota.can_apply(70./3.6) +def test_engagement_adoption_waits_for_native_capacity_and_release(helper): + helper.toyota.engaged = False + update(helper, state(native=60.)) + before = helper.v_cruise_kph + assert helper.initialize_v_cruise(state(), False, resume=False, slc_target_mps=70 / 3.6, is_metric=True) + assert helper.v_cruise_kph == before # Existing Toyota preparation owns application. + approved = approval(70.) + approved.actionStatus, approved.actionSequenceId = 'adopt', 1 + update(helper, state(native=60.), approved) + assert helper.toyota.status == 'raise_dash' and not helper.toyota.can_apply(70 / 3.6) + update(helper, state(native=80., button='accelCruise'), approved) + assert helper.toyota.status == 'release_plus' + update(helper, state(native=80., button='accelCruise', pressed=False), approved) + assert helper.toyota.status == 'ready' and helper.toyota.can_apply(70 / 3.6) + + @pytest.fixture(params=[False, True], ids=['normal-steps', 'swapped-steps']) def swap(helper, tmp_path, request): params = Params(str(tmp_path))