Bind SET engagement to the selected speed limit

openpilot/cereal/custom.capnp

@@ -535,6 +535,7 @@ struct SlcCruiseEvent @0xcd96dafb67a082d0 {
     switchbackMode @9;
     wheelAction @10;
     teslaStockCruise @11;
+    engagementAdopt @12;
   }
 
   struct TrafficModeGesture {

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:

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

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

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:

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

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

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

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))