Make accel profiles shape MPC cruise response

cereal/custom.capnp

@@ -312,6 +312,8 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
     usableGap @10 :Float32;
     closingSpeed @11 :Float32;
     requiredDecel @12 :Float32;
+    aMaxProfile @13 :Float32;
+    aMaxEffective @14 :Float32;
 
     enum Profile {
       eco @0;

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -313,7 +313,7 @@ class LongitudinalMpc:
     lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau)
     return lead_xv
 
-  def update(self, radarstate, v_cruise, personality=log.LongitudinalPersonality.standard):
+  def update(self, radarstate, v_cruise, personality=log.LongitudinalPersonality.standard, cruise_accel_max: float | None = None):
     t_follow = get_T_FOLLOW(personality)
     v_ego = self.x0[1]
     self.status = radarstate.leadOne.status or radarstate.leadTwo.status
@@ -329,9 +329,15 @@ class LongitudinalMpc:
 
     # Fake an obstacle for cruise, this ensures smooth acceleration to set speed
     # when the leads are no factor.
+    # sunnypilot can optionally shape only this cruise reference; this hook does
+    # not alter lead obstacles, braking constraints, or the MPC jerk cost.
+    if cruise_accel_max is None or not np.isfinite(cruise_accel_max):
+      cruise_accel_max = CRUISE_MAX_ACCEL
+    else:
+      cruise_accel_max = float(np.clip(cruise_accel_max, 0.0, CRUISE_MAX_ACCEL))
     v_lower = v_ego + (T_IDXS * CRUISE_MIN_ACCEL * 1.05)
     # TODO does this make sense when max_a is negative?
-    v_upper = v_ego + (T_IDXS * CRUISE_MAX_ACCEL * 1.05)
+    v_upper = v_ego + (T_IDXS * cruise_accel_max * 1.05)
     v_cruise_clipped = np.clip(v_cruise * np.ones(N+1), v_lower, v_upper)
     cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, t_follow)
 

selfdrive/controls/lib/longitudinal_planner.py

@@ -138,7 +138,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     v_cruise = LongitudinalPlannerSP.update_accel_controller(
       self, sm, v_cruise, engaged=not reset_state, cruise_initialized=v_cruise_initialized, acc_selected=not is_e2e,
       planner_speed=self.v_desired_filter.x, previous_mpc_source=self.mpc.source, previous_should_stop=self.output_should_stop,
-      controller_fault=self.mpc.solution_status != 0,
+      stock_accel_max=accel_clip[1], planner_accel=self.a_desired, controller_fault=self.mpc.solution_status != 0,
     )
 
     if force_slow_decel:
@@ -146,7 +146,10 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
 
     self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality)
     self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
-    self.mpc.update(sm['radarState'], v_cruise, personality=sm['selfdriveState'].personality)
+    self.mpc.update(
+      sm['radarState'], v_cruise, personality=sm['selfdriveState'].personality,
+      cruise_accel_max=self.accel_controller_result.mpc_cruise_accel_max,
+    )
 
     self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)
     self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution)
@@ -180,6 +183,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     for idx in range(2):
       accel_clip[idx] = np.clip(accel_clip[idx], self.prev_accel_clip[idx] - 0.05, self.prev_accel_clip[idx] + 0.05)
     self.output_a_target = np.clip(output_a_target, accel_clip[0], accel_clip[1])
+    self.output_a_target = min(self.output_a_target, self.accel_controller_result.output_accel_max)
     self.prev_accel_clip = accel_clip
 
   def publish(self, sm, pm):

sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py

@@ -7,6 +7,7 @@ import math
 import numpy as np
 
 from cereal import log
+from opendbc.car.interfaces import ACCEL_MAX
 from openpilot.common.realtime import DT_MDL
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import (
   LongitudinalMpc,
@@ -35,16 +36,23 @@ class AccelControllerState(IntEnum):
 @dataclass(frozen=True)
 class ProfileConfig:
   comfort_decel: float
-  release_accel: float
   release_confirm: float
 
 
 PROFILE_CONFIGS = {
-  AccelProfile.eco: ProfileConfig(comfort_decel=0.25, release_accel=0.65, release_confirm=0.50),
-  AccelProfile.normal: ProfileConfig(comfort_decel=0.35, release_accel=0.85, release_confirm=0.35),
-  AccelProfile.sport: ProfileConfig(comfort_decel=0.50, release_accel=1.10, release_confirm=0.20),
+  AccelProfile.eco: ProfileConfig(comfort_decel=0.25, release_confirm=0.50),
+  AccelProfile.normal: ProfileConfig(comfort_decel=0.35, release_confirm=0.35),
+  AccelProfile.sport: ProfileConfig(comfort_decel=0.50, release_confirm=0.20),
 }
 
+ACCEL_PROFILE_MAX_BP = [0.0, 10.0, 25.0, 40.0]
+ACCEL_PROFILE_MAX_V = {
+  AccelProfile.eco: [1.00, 0.75, 0.45, 0.30],
+  AccelProfile.normal: [1.30, 1.00, 0.65, 0.45],
+  AccelProfile.sport: [1.55, 1.15, 0.78, 0.58],
+}
+LAUNCH_DELTA_V = 3.0
+
 CAP_FILTER_FRAMES = 5
 RESTRICT_DEADBAND = 0.15
 RELIEF_DEADBAND = 0.35
@@ -53,6 +61,10 @@ STOP_HOLD_CAP = 0.50
 STOPPED_LEAD_SPEED = 0.30
 STOP_HOLD_EXIT_CAP = 0.80
 STOP_HOLD_EXIT_FRAMES = 4
+CLEAR_ROAD_PROFILE_SPEED = 0.10
+ACCEL_LIMIT_JERK = 1.0
+LAUNCH_ACCEL_JERK = 3.0
+LAUNCH_PACE_RATE = 5.0
 
 
 @dataclass(frozen=True)
@@ -71,7 +83,12 @@ class AccelControllerResult:
   enabled: bool
   active: bool
   shadow_active: bool
+  launching: bool
   profile: AccelProfile
+  profile_accel_max: float
+  effective_accel_max: float
+  output_accel_max: float
+  mpc_cruise_accel_max: float
   state: AccelControllerState
   shadow_state: AccelControllerState
   base_speed: float
@@ -95,6 +112,7 @@ class _PacePath:
   departure_frames: int = 0
   departing_from_stop: bool = False
   stopped_lead_hold: bool = False
+  accel_limit: float | None = None
 
   def reset(self) -> None:
     self.cap_samples = deque([math.inf] * CAP_FILTER_FRAMES, maxlen=CAP_FILTER_FRAMES)
@@ -104,6 +122,7 @@ class _PacePath:
     self.departure_frames = 0
     self.departing_from_stop = False
     self.stopped_lead_hold = False
+    self.accel_limit = None
 
   def update_filter(self, cap: float) -> float:
     self.cap_samples.append(cap)
@@ -115,7 +134,7 @@ class _PacePath:
 
 
 class AccelController:
-  """A comfort-only relative-pace envelope applied before longitudinal MPC."""
+  """A relative-pace governor with a positive-acceleration comfort ceiling."""
 
   def __init__(self, CP, dt: float = DT_MDL):
     if not math.isfinite(dt) or dt <= 0.0:
@@ -133,6 +152,15 @@ class AccelController:
     except (TypeError, ValueError):
       return AccelProfile.normal
 
+  @classmethod
+  def get_profile_accel_max(cls, profile: int | AccelProfile, v_ego: float) -> float:
+    """Return the profile's positive-acceleration ceiling at the current speed."""
+    if not math.isfinite(v_ego):
+      return math.nan
+
+    profile = cls._profile(profile)
+    return float(np.interp(max(v_ego, 0.0), ACCEL_PROFILE_MAX_BP, ACCEL_PROFILE_MAX_V[profile]))
+
   def _delay(self) -> float:
     try:
       return float(self.CP.longitudinalActuatorDelay) + DT_MDL
@@ -219,16 +247,28 @@ class AccelController:
     base_speed: float,
     v_ego: float,
     config: ProfileConfig,
+    accel_rate: float,
     previous_mpc_source,
     planner_speed: float,
     previous_should_stop: bool,
     has_nearly_stopped_lead: bool,
+    launch_delta_v: float,
   ) -> float:
     filtered_cap = path.update_filter(raw_cap)
-    if path.pace is None:
+    just_initialized = path.pace is None
+    if just_initialized:
       path.pace = min(base_speed, v_ego)
       path.state = AccelControllerState.free
 
+    # A clear-road standstill engagement should request motion immediately. A
+    # stopped/previously-stopping lead still goes through stop-hold confirmation.
+    if just_initialized and v_ego < STOP_HOLD_EGO_SPEED and not math.isfinite(raw_cap) and not previous_should_stop:
+      path.pace = min(base_speed, v_ego + launch_delta_v)
+      path.state = AccelControllerState.release
+      path.relief_time = config.release_confirm
+      path.departing_from_stop = True
+      return filtered_cap
+
     # A lower non-controller target is authoritative, and is also the correct seed if it later clears.
     path.pace = min(path.pace, base_speed)
     if self._lead_source(previous_mpc_source) and not math.isfinite(raw_cap) and planner_speed < path.pace:
@@ -237,7 +277,8 @@ class AccelController:
     if v_ego < STOP_HOLD_EGO_SPEED and (filtered_cap < STOP_HOLD_CAP or has_nearly_stopped_lead):
       path.stopped_lead_hold = True
 
-    if v_ego >= STOP_HOLD_EGO_SPEED:
+    clear_road_launch_complete = path.departing_from_stop and not path.stopped_lead_hold and v_ego >= CLEAR_ROAD_PROFILE_SPEED
+    if v_ego >= STOP_HOLD_EGO_SPEED or clear_road_launch_complete:
       path.departing_from_stop = False
       path.stopped_lead_hold = False
 
@@ -252,7 +293,11 @@ class AccelController:
       return filtered_cap
 
     if path.state == AccelControllerState.stopHold:
-      if filtered_cap > STOP_HOLD_EXIT_CAP:
+      # A continuously observed moving lead exits after exactly four raw frames.
+      # Total lead loss still waits for the five-frame median dropout guard first.
+      raw_departure = math.isfinite(raw_cap) and raw_cap > STOP_HOLD_EXIT_CAP and not has_nearly_stopped_lead
+      guarded_lead_loss = not math.isfinite(raw_cap) and filtered_cap > STOP_HOLD_EXIT_CAP
+      if raw_departure or guarded_lead_loss:
         path.departure_frames += 1
       else:
         path.departure_frames = 0
@@ -265,6 +310,8 @@ class AccelController:
       path.relief_time = config.release_confirm
       path.departure_frames = 0
       path.departing_from_stop = True
+      path.pace = min(base_speed, filtered_cap, v_ego + launch_delta_v)
+      return filtered_cap
 
     ceiling = min(base_speed, filtered_cap)
     if ceiling <= path.pace - RESTRICT_DEADBAND:
@@ -282,7 +329,8 @@ class AccelController:
       release_allowed = path.relief_time >= config.release_confirm
 
     if release_allowed:
-      path.pace = min(ceiling, path.pace + config.release_accel * self.dt)
+      pace_rate = LAUNCH_PACE_RATE if path.departing_from_stop else accel_rate
+      path.pace = min(ceiling, path.pace + pace_rate * self.dt)
       path.state = AccelControllerState.release
     elif relief <= RELIEF_DEADBAND:
       path.relief_time = 0.0
@@ -290,9 +338,53 @@ class AccelController:
 
     return filtered_cap
 
+  def _update_accel_limit(
+    self,
+    path: _PacePath,
+    stock_accel_max: float,
+    planner_accel: float,
+    profile_accel_max: float,
+  ) -> tuple[float, float]:
+    """Return the raw stock/controller minimum and the controller-owned positive ceiling."""
+    requested_limit = float(np.clip(profile_accel_max, 0.0, ACCEL_MAX))
+
+    if path.state == AccelControllerState.stopHold:
+      path.accel_limit = 0.0
+      # Keep stock MPC warm while the output ceiling prevents an early launch
+      # during the four-frame lead-departure confirmation.
+      return min(stock_accel_max, 0.0), 0.0
+
+    if path.departing_from_stop:
+      # Start commanding acceleration on the first confirmed frame, then open
+      # quickly enough to avoid launch delay without a step in requested accel.
+      previous_limit = path.accel_limit if path.accel_limit is not None else 0.0
+      path.accel_limit = min(requested_limit, previous_limit + LAUNCH_ACCEL_JERK * self.dt)
+      return min(stock_accel_max, path.accel_limit), path.accel_limit
+
+    if path.accel_limit is None:
+      # Avoid a discontinuity when enabling around an already-positive command.
+      # The global OP limit bounds this seed; dynamic stock output constraints
+      # still retain their existing output-side enforcement and slew.
+      path.accel_limit = min(ACCEL_MAX, max(requested_limit, max(0.0, planner_accel)))
+    else:
+      max_step = ACCEL_LIMIT_JERK * self.dt
+      path.accel_limit = float(np.clip(requested_limit, path.accel_limit - max_step, path.accel_limit + max_step))
+
+    effective_limit = min(stock_accel_max, path.accel_limit)
+    return effective_limit, path.accel_limit
+
   @staticmethod
   def _valid_context(
-    base_speed: float, v_ego: float, a_ego: float, planner_speed: float, delay: float, engaged: bool, cruise_initialized: bool, controller_fault: bool
+    base_speed: float,
+    v_ego: float,
+    a_ego: float,
+    planner_speed: float,
+    stock_accel_max: float,
+    planner_accel: float,
+    delay: float,
+    engaged: bool,
+    cruise_initialized: bool,
+    controller_fault: bool,
   ) -> bool:
     return (
       engaged
@@ -302,7 +394,7 @@ class AccelController:
       and v_ego >= 0.0
       and planner_speed >= 0.0
       and delay >= 0.0
-      and all(math.isfinite(value) for value in (base_speed, v_ego, a_ego, planner_speed, delay))
+      and all(math.isfinite(value) for value in (base_speed, v_ego, a_ego, planner_speed, stock_accel_max, planner_accel, delay))
     )
 
   def update(
@@ -320,21 +412,47 @@ class AccelController:
     cruise_initialized: bool,
     previous_mpc_source,
     planner_speed: float,
+    stock_accel_max: float,
+    planner_accel: float,
     previous_should_stop: bool,
     controller_fault: bool = False,
   ) -> AccelControllerResult:
     """Update live and shadow acceleration controllers and return the target and additive telemetry."""
     profile = self._profile(profile)
     config = PROFILE_CONFIGS[profile]
+    profile_accel_max = self.get_profile_accel_max(profile, v_ego)
+    launch_delta_v = LAUNCH_DELTA_V
     delay = self._delay()
-    valid_context = self._valid_context(base_speed, v_ego, a_ego, planner_speed, delay, engaged, cruise_initialized, controller_fault)
+    valid_context = self._valid_context(
+      base_speed,
+      v_ego,
+      a_ego,
+      planner_speed,
+      stock_accel_max,
+      planner_accel,
+      delay,
+      engaged,
+      cruise_initialized,
+      controller_fault,
+    )
 
     envelope = self.calculate_energy_envelope(radar_state, v_ego, a_ego, profile, follow_personality) if valid_context else EnergyEnvelope()
 
     if valid_context:
       shadow_filtered_cap = self._update_path(
-        self.shadow, envelope.cap, base_speed, v_ego, config, previous_mpc_source, planner_speed, previous_should_stop, envelope.has_nearly_stopped_lead
+        self.shadow,
+        envelope.cap,
+        base_speed,
+        v_ego,
+        config,
+        profile_accel_max,
+        previous_mpc_source,
+        planner_speed,
+        previous_should_stop,
+        envelope.has_nearly_stopped_lead,
+        launch_delta_v,
       )
+      self._update_accel_limit(self.shadow, stock_accel_max, planner_accel, profile_accel_max)
       shadow_active = True
     else:
       self.shadow.reset()
@@ -343,23 +461,35 @@ class AccelController:
 
     live_active = valid_context and bool(enabled) and bool(acc_selected)
     if live_active:
-      was_uninitialized = self.live.pace is None
-      was_departing_from_stop = self.live.departing_from_stop
       live_filtered_cap = self._update_path(
-        self.live, envelope.cap, base_speed, v_ego, config, previous_mpc_source, planner_speed, previous_should_stop, envelope.has_nearly_stopped_lead
+        self.live,
+        envelope.cap,
+        base_speed,
+        v_ego,
+        config,
+        profile_accel_max,
+        previous_mpc_source,
+        planner_speed,
+        previous_should_stop,
+        envelope.has_nearly_stopped_lead,
+        launch_delta_v,
       )
-      initial_lead_warm = was_uninitialized and v_ego < STOP_HOLD_EGO_SPEED and envelope.selected_lead >= 0
-      stopped_lead_owns_hold = (
-        self.live.state == AccelControllerState.stopHold and self.live.stopped_lead_hold and envelope.selected_lead >= 0
+      effective_accel_max, output_accel_max = self._update_accel_limit(
+        self.live, stock_accel_max, planner_accel, profile_accel_max
       )
-      if initial_lead_warm or (v_ego < STOP_HOLD_EGO_SPEED and (self.live.departing_from_stop or stopped_lead_owns_hold)):
-        # Keep stock MPC warm while a present lead owns a latched stop, or after departure is confirmed.
-        # A dropout immediately pins both target and pace to zero instead of inheriting stale geometry.
+      # Stock cruise shaping during a confirmed stop/departure keeps takeoff
+      # immediate. Once rolling, the profile rate shapes MPC's cruise horizon.
+      mpc_cruise_accel_max = (
+        math.inf if self.live.state == AccelControllerState.stopHold or self.live.departing_from_stop else output_accel_max
+      )
+      if self.live.state == AccelControllerState.stopHold:
+        # Keep stock MPC warm while the zero output ceiling pins a confirmed stop
+        # and confirms four moving-lead frames before allowing takeoff.
+        target_speed = base_speed
+      elif self.live.departing_from_stop and v_ego < STOP_HOLD_EGO_SPEED and envelope.selected_lead >= 0:
+        # A moving lead keeps stock MPC well-conditioned during a confirmed
+        # departure. Clear-road launches retain the bounded live pace below.
         target_speed = base_speed
-      elif was_departing_from_stop and v_ego >= STOP_HOLD_EGO_SPEED:
-        # Resume the confirmed relative envelope once the stock start state has moved the car.
-        self.live.pace = min(base_speed, live_filtered_cap)
-        target_speed = self.live.pace
       else:
         target_speed = min(base_speed, self.live.pace if self.live.pace is not None else base_speed)
     else:
@@ -367,13 +497,21 @@ class AccelController:
       live_filtered_cap = math.inf
       # Preserve the stock target bit-for-bit on every bypass, including stock's own invalid-value handling.
       target_speed = base_speed
+      effective_accel_max = math.inf
+      output_accel_max = math.inf
+      mpc_cruise_accel_max = math.inf
 
     return AccelControllerResult(
       target_speed=target_speed,
       enabled=bool(enabled),
       active=live_active,
       shadow_active=shadow_active,
+      launching=live_active and self.live.departing_from_stop,
       profile=profile,
+      profile_accel_max=profile_accel_max if live_active else math.inf,
+      effective_accel_max=effective_accel_max,
+      output_accel_max=output_accel_max,
+      mpc_cruise_accel_max=mpc_cruise_accel_max,
       state=self.live.state,
       shadow_state=self.shadow.state,
       base_speed=base_speed,

sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py

@@ -6,8 +6,14 @@ import pytest
 
 from cereal import log
 from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.controls.lib.longitudinal_planner import get_max_accel
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalPlanSource, STOP_DISTANCE, get_T_FOLLOW
 from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_controller import (
+  ACCEL_LIMIT_JERK,
+  ACCEL_PROFILE_MAX_BP,
+  ACCEL_PROFILE_MAX_V,
+  LAUNCH_ACCEL_JERK,
+  LAUNCH_DELTA_V,
   AccelController,
   AccelControllerState,
   AccelProfile,
@@ -40,12 +46,148 @@ def update(governor, radar_state=None, **overrides):
     "cruise_initialized": True,
     "previous_mpc_source": LongitudinalPlanSource.cruise,
     "planner_speed": 20.0,
+    "stock_accel_max": 2.0,
+    "planner_accel": 0.0,
     "previous_should_stop": False,
   }
   args.update(overrides)
   return governor.update(radar_state or make_radar(), **args)
 
 
+class TestAccelProfileLimits:
+  @pytest.mark.parametrize("profile", list(AccelProfile))
+  def test_profile_accel_max_matches_lookup_table(self, profile):
+    for speed, expected in zip(ACCEL_PROFILE_MAX_BP, ACCEL_PROFILE_MAX_V[profile], strict=True):
+      assert AccelController.get_profile_accel_max(profile, speed) == expected
+
+  @pytest.mark.parametrize("profile", list(AccelProfile))
+  def test_profile_accel_max_interpolates_and_clamps(self, profile):
+    expected_midpoint = (ACCEL_PROFILE_MAX_V[profile][1] + ACCEL_PROFILE_MAX_V[profile][2]) / 2.0
+
+    assert AccelController.get_profile_accel_max(profile, 17.5) == pytest.approx(expected_midpoint)
+    assert AccelController.get_profile_accel_max(profile, -1.0) == ACCEL_PROFILE_MAX_V[profile][0]
+    assert AccelController.get_profile_accel_max(profile, 50.0) == ACCEL_PROFILE_MAX_V[profile][-1]
+
+  @pytest.mark.parametrize("speed", ACCEL_PROFILE_MAX_BP)
+  def test_profile_accel_max_order_is_distinct(self, speed):
+    limits = [AccelController.get_profile_accel_max(profile, speed) for profile in AccelProfile]
+
+    assert limits[AccelProfile.eco] < limits[AccelProfile.normal] < limits[AccelProfile.sport]
+
+  @pytest.mark.parametrize("profile", list(AccelProfile))
+  def test_profile_table_never_exceeds_stock_speed_limit(self, profile):
+    for step in range(161):
+      speed = step * 0.25
+      assert AccelController.get_profile_accel_max(profile, speed) <= get_max_accel(speed)
+
+  def test_active_result_exposes_profile_accel_max(self):
+    governor = make_governor()
+
+    result = update(governor, profile=AccelProfile.eco, v_ego=17.5, planner_speed=17.5)
+
+    assert result.profile_accel_max == pytest.approx(0.60)
+
+  def test_normal_active_limits_are_bounded_by_stock_and_profile(self):
+    governor = make_governor()
+
+    result = update(governor, profile=AccelProfile.normal, v_ego=10.0, planner_speed=10.0, stock_accel_max=1.40)
+
+    assert result.profile_accel_max == 1.0
+    assert result.effective_accel_max == 1.0
+    assert result.output_accel_max == 1.0
+    assert result.mpc_cruise_accel_max == 1.0
+
+  def test_first_enable_seeds_from_positive_planner_accel_within_stock(self):
+    governor = make_governor()
+
+    first = update(
+      governor, profile=AccelProfile.normal, v_ego=10.0, planner_speed=10.0, stock_accel_max=1.30, planner_accel=1.20
+    )
+    second = update(
+      governor, profile=AccelProfile.normal, v_ego=10.0, planner_speed=10.0, stock_accel_max=1.30, planner_accel=1.20
+    )
+
+    assert first.effective_accel_max == 1.20
+    assert second.effective_accel_max == pytest.approx(first.effective_accel_max - ACCEL_LIMIT_JERK * DT_MDL)
+
+  def test_first_enable_seed_preserves_current_plan_but_effective_limit_stays_stock_bounded(self):
+    governor = make_governor()
+
+    result = update(
+      governor, profile=AccelProfile.normal, v_ego=10.0, planner_speed=10.0, stock_accel_max=1.10, planner_accel=1.80
+    )
+
+    assert result.effective_accel_max == 1.10
+    assert result.output_accel_max == 1.80
+
+  def test_profile_switch_slews_at_one_meter_per_second_cubed(self):
+    governor = make_governor()
+    sport = update(governor, profile=AccelProfile.sport, v_ego=10.0, planner_speed=10.0, stock_accel_max=2.0)
+
+    eco = update(governor, profile=AccelProfile.eco, v_ego=10.0, planner_speed=10.0, stock_accel_max=2.0)
+
+    assert sport.effective_accel_max == 1.15
+    assert eco.effective_accel_max == pytest.approx(sport.effective_accel_max - ACCEL_LIMIT_JERK * DT_MDL)
+    assert eco.effective_accel_max > eco.profile_accel_max
+
+  def test_dynamic_stock_tightening_does_not_enter_controller_comfort_state(self):
+    governor = make_governor()
+    update(governor, profile=AccelProfile.normal, v_ego=10.0, planner_speed=10.0, stock_accel_max=1.40)
+
+    tightened = update(governor, profile=AccelProfile.normal, v_ego=10.0, planner_speed=10.0, stock_accel_max=0.40)
+    released = update(governor, profile=AccelProfile.normal, v_ego=10.0, planner_speed=10.0, stock_accel_max=1.40)
+
+    assert tightened.effective_accel_max == 0.40
+    assert tightened.output_accel_max == 1.0
+    assert released.effective_accel_max == 1.0
+    assert released.output_accel_max == 1.0
+
+  def test_negative_stock_max_never_becomes_positive(self):
+    governor = make_governor()
+
+    result = update(governor, v_ego=10.0, planner_speed=10.0, stock_accel_max=-0.20, planner_accel=1.0)
+
+    assert result.effective_accel_max == -0.20
+    assert result.output_accel_max == 1.0
+
+  def test_profile_tightening_can_converge_below_positive_planner_accel(self):
+    governor = make_governor()
+    update(
+      governor, profile=AccelProfile.sport, v_ego=10.0, planner_speed=10.0, stock_accel_max=2.0, planner_accel=1.15
+    )
+
+    results = [
+      update(governor, profile=AccelProfile.eco, v_ego=10.0, planner_speed=10.0, stock_accel_max=2.0, planner_accel=1.15)
+      for _ in range(8)
+    ]
+
+    assert results[-1].output_accel_max == pytest.approx(ACCEL_PROFILE_MAX_V[AccelProfile.eco][1])
+    assert results[-1].output_accel_max < 1.15
+    assert all(math.isfinite(result.output_accel_max) for result in results)
+
+  @pytest.mark.parametrize("bypass", [{"enabled": False}, {"acc_selected": False}])
+  def test_bypass_does_not_expose_an_active_accel_limit(self, bypass):
+    governor = make_governor()
+
+    result = update(governor, **bypass)
+
+    assert math.isinf(result.profile_accel_max)
+    assert math.isinf(result.effective_accel_max)
+    assert math.isinf(result.output_accel_max)
+    assert math.isinf(result.mpc_cruise_accel_max)
+
+  @pytest.mark.parametrize("invalid", [{"stock_accel_max": math.nan}, {"planner_accel": math.nan}])
+  def test_invalid_accel_input_bypasses_and_resets_limits(self, invalid):
+    governor = make_governor()
+    update(governor)
+
+    result = update(governor, **invalid)
+
+    assert not result.active
+    assert governor.live.accel_limit is None
+    assert math.isinf(result.effective_accel_max)
+
+
 class TestEnergyEnvelope:
   def test_correct_relative_energy_formula_and_lead_selection(self):
     governor = make_governor()
@@ -189,7 +331,8 @@ class TestAccelControllerState:
       assert confirmation_updates < 20
 
     assert confirmation_updates >= 6
-    assert result.live_pace == pytest.approx(held_pace + PROFILE_CONFIGS[AccelProfile.normal].release_accel * DT_MDL)
+    expected_rate = AccelController.get_profile_accel_max(AccelProfile.normal, 20.0)
+    assert result.live_pace == pytest.approx(held_pace + expected_rate * DT_MDL)
 
   def test_live_state_never_adopts_shadow_history(self):
     governor = make_governor()
@@ -229,17 +372,63 @@ class TestAccelControllerState:
       assert result.state == AccelControllerState.stopHold
       assert result.live_pace == 0.0
       assert result.target_speed == stop_args["base_speed"]
+      assert result.effective_accel_max == 0.0
+      assert result.output_accel_max == 0.0
+      assert math.isinf(result.mpc_cruise_accel_max)
 
-    for _ in range(5):
+    for _ in range(3):
       result = update(governor, moving, **stop_args)
       assert result.state == AccelControllerState.stopHold
+      assert not result.launching
       assert result.live_pace == 0.0
       assert result.target_speed == stop_args["base_speed"]
+      assert result.effective_accel_max == 0.0
+      assert result.output_accel_max == 0.0
+      assert math.isinf(result.mpc_cruise_accel_max)
 
     departed = update(governor, moving, **stop_args)
     assert departed.state == AccelControllerState.release
-    assert departed.live_pace == pytest.approx(PROFILE_CONFIGS[AccelProfile.normal].release_accel * DT_MDL)
+    assert departed.launching
+    assert departed.live_pace == pytest.approx(stop_args["v_ego"] + LAUNCH_DELTA_V)
     assert departed.target_speed == stop_args["base_speed"]
+    assert departed.effective_accel_max == pytest.approx(LAUNCH_ACCEL_JERK * DT_MDL)
+    assert departed.output_accel_max == departed.effective_accel_max
+    assert math.isinf(departed.mpc_cruise_accel_max)
+
+  def test_second_nearly_stopped_lead_blocks_departure_confirmation(self):
+    governor = make_governor()
+    stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0))
+    mixed = make_radar(
+      make_lead(status=True, d_rel=20.0, v_lead_k=5.0),
+      make_lead(status=True, d_rel=200.0, v_lead_k=0.0),
+    )
+    args = {"base_speed": 5.0, "v_ego": 0.1, "planner_speed": 0.0}
+    update(governor, stopped, **args)
+
+    results = [update(governor, mixed, **args) for _ in range(5)]
+
+    assert all(result.raw_energy_cap > 0.8 for result in results)
+    assert all(result.state == AccelControllerState.stopHold for result in results)
+    assert all(not result.launching for result in results)
+    assert governor.live.departure_frames == 0
+
+  @pytest.mark.parametrize("profile", list(AccelProfile))
+  def test_confirmed_departure_launch_is_immediate_bounded_and_profiled(self, profile):
+    governor = make_governor()
+    stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0))
+    moving = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=5.0))
+    args = {"base_speed": 5.0, "v_ego": 0.1, "planner_speed": 0.0, "stock_accel_max": 2.0, "profile": profile}
+
+    for _ in range(3):
+      update(governor, stopped, **args)
+    departure = [update(governor, moving, **args) for _ in range(4)]
+
+    assert [result.target_speed for result in departure[:3]] == [args["base_speed"]] * 3
+    expected_launch_pace = min(args["base_speed"], departure[-1].live_filtered_cap, args["v_ego"] + LAUNCH_DELTA_V)
+    assert departure[-1].live_pace == pytest.approx(expected_launch_pace)
+    assert departure[-1].target_speed == args["base_speed"]
+    assert departure[-1].effective_accel_max == pytest.approx(LAUNCH_ACCEL_JERK * DT_MDL)
+    assert departure[-1].output_accel_max == departure[-1].effective_accel_max
 
   def test_stopped_lead_departure_releases_while_mpc_source_remains_lead(self):
     governor = make_governor()
@@ -255,10 +444,11 @@ class TestAccelControllerState:
     for _ in range(3):
       update(governor, stopped, **lead_args)
 
-    departure = [update(governor, moving, **lead_args) for _ in range(6)]
-    assert [result.live_pace for result in departure[:5]] == [0.0] * 5
-    assert departure[5].live_pace > 0.0
-    assert all(result.target_speed == lead_args["base_speed"] for result in departure)
+    departure = [update(governor, moving, **lead_args) for _ in range(4)]
+    assert [result.live_pace for result in departure[:3]] == [0.0] * 3
+    assert departure[3].live_pace > 0.0
+    assert [result.target_speed for result in departure[:3]] == [lead_args["base_speed"]] * 3
+    assert departure[-1].target_speed == lead_args["base_speed"]
     assert len(departure) * DT_MDL < 1.0
 
     continued_release = update(governor, moving, **lead_args)
@@ -280,9 +470,9 @@ class TestAccelControllerState:
     for _ in range(3):
       update(governor, stopped, **stale_stop_args)
 
-    departure = [update(governor, moving, **stale_stop_args) for _ in range(6)]
-    assert [result.live_pace for result in departure[:5]] == [0.0] * 5
-    assert departure[5].live_pace > 0.0
+    departure = [update(governor, moving, **stale_stop_args) for _ in range(4)]
+    assert [result.live_pace for result in departure[:3]] == [0.0] * 3
+    assert departure[3].live_pace > 0.0
     assert len(departure) * DT_MDL < 1.0
 
     continued_paces = [update(governor, moving, **stale_stop_args).live_pace for _ in range(60)]
@@ -311,9 +501,10 @@ class TestAccelControllerState:
     assert renewed_stop.state == AccelControllerState.stopHold
     assert renewed_stop.live_pace == 0.0
     assert renewed_stop.target_speed == stale_stop_args["base_speed"]
+    assert renewed_stop.output_accel_max == 0.0
     assert not governor.live.departing_from_stop
 
-  def test_low_speed_moving_lead_only_warms_mpc_on_first_frame(self):
+  def test_low_speed_moving_lead_never_bypasses_bounded_pace(self):
     governor = make_governor()
     noisy_moving_lead = make_radar(make_lead(status=True, d_rel=10.0, v_lead_k=1.5))
 
@@ -322,7 +513,7 @@ class TestAccelControllerState:
 
     assert first.selected_lead == 0
     assert first.live_pace == 0.0
-    assert first.target_speed == first.base_speed
+    assert first.target_speed == first.live_pace
     assert not governor.live.stopped_lead_hold
     assert second.target_speed == second.live_pace
     assert second.target_speed < second.base_speed
@@ -337,10 +528,11 @@ class TestAccelControllerState:
     stopped_evidence = update(governor, stopped, **args)
     repeated_noise = update(governor, noisy_moving_lead, **args)
 
-    assert initial_noise.target_speed == initial_noise.base_speed
+    assert initial_noise.target_speed == initial_noise.live_pace
     assert stopped_evidence.state == AccelControllerState.stopHold
     assert governor.live.stopped_lead_hold
-    assert repeated_noise.target_speed == repeated_noise.base_speed
+    assert stopped_evidence.target_speed == stopped_evidence.base_speed
+    assert repeated_noise.target_speed == args["base_speed"]
 
   def test_later_continuously_moving_lead_does_not_latch_stopped_hold(self):
     governor = make_governor()
@@ -366,19 +558,24 @@ class TestAccelControllerState:
     assert dropout.selected_lead == -1
     assert dropout.state == AccelControllerState.stopHold
     assert dropout.live_pace == 0.0
-    assert dropout.target_speed == dropout.live_pace
+    assert dropout.target_speed == dropout.base_speed
+    assert dropout.output_accel_max == 0.0
     assert governor.live.stopped_lead_hold
 
-  def test_no_lead_start_retains_profile_ramp(self):
+  def test_no_lead_start_launches_immediately_with_profile_limit(self):
     governor = make_governor()
 
     result = update(governor, base_speed=5.0, v_ego=0.1, planner_speed=0.1)
 
     assert result.selected_lead == -1
-    assert result.live_pace == 0.1
+    assert result.launching
+    assert result.live_pace == pytest.approx(0.1 + LAUNCH_DELTA_V)
     assert result.target_speed == result.live_pace
+    assert result.effective_accel_max == pytest.approx(LAUNCH_ACCEL_JERK * DT_MDL)
+    assert result.output_accel_max == result.effective_accel_max
+    assert math.isinf(result.mpc_cruise_accel_max)
 
-  def test_confirmed_departure_seeds_filtered_cap_at_start_threshold(self):
+  def test_confirmed_departure_has_no_later_pace_jump(self):
     governor = make_governor()
     stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0))
     moving = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=5.0))
@@ -399,9 +596,11 @@ class TestAccelControllerState:
 
     assert not governor.live.departing_from_stop
     assert not governor.live.stopped_lead_hold
-    assert handed_back.live_pace == min(handed_back.base_speed, handed_back.live_filtered_cap)
-    assert handed_back.live_pace > departing.live_pace
+    expected_step = AccelController.get_profile_accel_max(AccelProfile.normal, 0.31) * DT_MDL
+    assert handed_back.live_pace == pytest.approx(departing.live_pace + expected_step)
+    assert handed_back.live_pace < min(handed_back.base_speed, handed_back.live_filtered_cap)
     assert handed_back.target_speed == handed_back.live_pace
+    assert handed_back.mpc_cruise_accel_max == handed_back.output_accel_max
 
   @pytest.mark.parametrize(
     "bypass",
@@ -426,6 +625,9 @@ class TestAccelControllerState:
     assert not result.active
     assert result.state == AccelControllerState.inactive
     assert math.isinf(result.live_pace)
+    assert governor.live.accel_limit is None
+    assert math.isinf(result.effective_accel_max)
+    assert math.isinf(result.output_accel_max)
 
   def test_disabled_acc_mode_keeps_shadow_running(self):
     governor = make_governor()

sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller_interfaces.py

@@ -1,8 +1,11 @@
 from types import SimpleNamespace
 
+import numpy as np
 import pytest
 
-from cereal import custom
+from cereal import custom, messaging
+from opendbc.car.interfaces import ACCEL_MAX, ACCEL_MIN
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc
 from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality import AccelControllerState, AccelProfile
 from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP, LongitudinalPlanSource
 
@@ -20,6 +23,24 @@ def test_legacy_profile_enum_keeps_toyota_importable():
   assert CarState.__module__ == "opendbc.car.toyota.carstate"
 
 
+def test_mpc_profile_shapes_only_the_cruise_reference():
+  radar_state = messaging.new_message('radarState').radarState
+  mpc = LongitudinalMpc()
+  mpc.set_cur_state(10.0, 0.0)
+  mpc.run = lambda: None
+
+  mpc.update(radar_state, 30.0, cruise_accel_max=0.5)
+  shaped_params = mpc.params.copy()
+  mpc.update(radar_state, 30.0)
+  stock_params = mpc.params.copy()
+  mpc.update(radar_state, 30.0, cruise_accel_max=np.inf)
+
+  np.testing.assert_array_equal(shaped_params[:, 0], ACCEL_MIN)
+  np.testing.assert_array_equal(shaped_params[:, 1], ACCEL_MAX)
+  assert np.any(shaped_params[:, 2] < stock_params[:, 2])
+  np.testing.assert_array_equal(mpc.params, stock_params)
+
+
 def test_shadow_target_telemetry_publishes_filtered_cap():
   planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
   planner.source = LongitudinalPlanSource.cruise
@@ -31,6 +52,7 @@ def test_shadow_target_telemetry_publishes_filtered_cap():
     enabled=True,
     active=False,
     shadow_active=True,
+    launching=False,
     profile=AccelProfile.normal,
     state=AccelControllerState.inactive,
     shadow_state=AccelControllerState.restrict,
@@ -43,6 +65,10 @@ def test_shadow_target_telemetry_publishes_filtered_cap():
     usable_gap=30.0,
     closing_speed=5.0,
     required_decel=0.4,
+    profile_accel_max=1.0,
+    effective_accel_max=0.85,
+    output_accel_max=0.85,
+    mpc_cruise_accel_max=0.85,
   )
   planner.scc = SimpleNamespace(
     vision=SimpleNamespace(
@@ -84,3 +110,5 @@ def test_shadow_target_telemetry_publishes_filtered_cap():
   telemetry = sent["longitudinalPlanSP"].longitudinalPlanSP.accelController
   assert telemetry.vTargetShadow == pytest.approx(planner.accel_controller_result.shadow_filtered_cap)
   assert telemetry.vTargetShadow != pytest.approx(planner.accel_controller_result.shadow_pace)
+  assert telemetry.aMaxProfile == pytest.approx(planner.accel_controller_result.profile_accel_max)
+  assert telemetry.aMaxEffective == pytest.approx(planner.accel_controller_result.effective_accel_max)

sunnypilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -40,7 +40,7 @@ class LongitudinalPlannerSP:
     self.accel_controller = AccelController(CP, dt=dt)
     self.accel_controller_result = None
 
-    self._param_read_frames = max(1, int(round(1.0 / dt)))
+    self._param_read_frames = max(1, int(round(0.25 / dt)))
     self._param_frame = 0
     self.accel_personality_enabled = False
     self.accel_personality = int(AccelProfile.normal)
@@ -96,7 +96,7 @@ class LongitudinalPlannerSP:
 
   def update_accel_controller(self, sm: messaging.SubMaster, base_speed: float, engaged: bool, cruise_initialized: bool,
                               acc_selected: bool, planner_speed: float, previous_mpc_source, previous_should_stop: bool,
-                              controller_fault: bool = False) -> float:
+                              stock_accel_max: float, planner_accel: float, controller_fault: bool = False) -> float:
     self.accel_controller_result = self.accel_controller.update(
       sm['radarState'],
       base_speed=base_speed,
@@ -110,6 +110,8 @@ class LongitudinalPlannerSP:
       cruise_initialized=cruise_initialized,
       previous_mpc_source=previous_mpc_source,
       planner_speed=planner_speed,
+      stock_accel_max=stock_accel_max,
+      planner_accel=planner_accel,
       previous_should_stop=previous_should_stop,
       controller_fault=controller_fault,
     )
@@ -155,6 +157,8 @@ class LongitudinalPlannerSP:
       accel_controller.usableGap = float(result.usable_gap)
       accel_controller.closingSpeed = float(result.closing_speed)
       accel_controller.requiredDecel = float(result.required_decel)
+      accel_controller.aMaxProfile = float(result.profile_accel_max)
+      accel_controller.aMaxEffective = float(result.effective_accel_max)
 
     # Smart Cruise Control
     smartCruiseControl = longitudinalPlanSP.smartCruiseControl

sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py

@@ -22,9 +22,13 @@ class ClosedLoopTrace:
   source: list
   active: np.ndarray
   shadow_active: np.ndarray
+  launching: np.ndarray
   pace: np.ndarray
   filtered_cap: np.ndarray
   selected_lead: np.ndarray
+  profile_accel_max: np.ndarray
+  effective_accel_max: np.ndarray
+  output_accel_max: np.ndarray
   solver_failures: int
 
 
@@ -75,9 +79,13 @@ def _run(
         result["fcw"],
         controller.active,
         controller.shadow_active,
+        controller.launching,
         controller.live_pace,
         controller.live_filtered_cap,
         controller.selected_lead,
+        controller.profile_accel_max,
+        controller.effective_accel_max,
+        controller.output_accel_max,
       )
     )
     sources.append(result["mpc_source"])
@@ -95,9 +103,13 @@ def _run(
     source=sources,
     active=data[:, 8].astype(bool),
     shadow_active=data[:, 9].astype(bool),
-    pace=data[:, 10],
-    filtered_cap=data[:, 11],
-    selected_lead=data[:, 12].astype(int),
+    launching=data[:, 10].astype(bool),
+    pace=data[:, 11],
+    filtered_cap=data[:, 12],
+    selected_lead=data[:, 13].astype(int),
+    profile_accel_max=data[:, 14],
+    effective_accel_max=data[:, 15],
+    output_accel_max=data[:, 16],
     solver_failures=solver_failures,
   )
 
@@ -170,6 +182,19 @@ def test_non_actuating_modes_are_bit_exact(plant_kwargs, expect_shadow_active):
     assert not shadow.shadow_active.any()
 
 
+def test_disabled_profiles_are_bit_exact_in_engaged_acc():
+  common = dict(duration=2.0, controller_enabled=False, lead_relevancy=True, speed=20.0, distance_lead=70.0, v_lead=14.0)
+  traces = [_run(profile=profile, **common) for profile in range(3)]
+
+  for trace in traces[1:]:
+    np.testing.assert_allclose(trace.a_target, traces[0].a_target, atol=1e-6, rtol=0.0)
+    np.testing.assert_array_equal(trace.should_stop, traces[0].should_stop)
+    np.testing.assert_array_equal(trace.fcw, traces[0].fcw)
+    assert trace.source == traces[0].source
+  assert all(not trace.active.any() for trace in traces)
+  assert all(np.isinf(trace.output_accel_max).all() for trace in traces)
+
+
 def test_dec_radar_lead_selects_acc_and_standstill_uses_shadow_only():
   blended = _run(
     duration=2.0,
@@ -295,10 +320,6 @@ def test_alternating_full_lead_range_glitch_has_bounded_jerk_and_no_reversal():
     assert not np.any(disturbance[positive[0] + 1:] < -0.2)
 
 
-@pytest.mark.xfail(
-  reason="opt-in validation: raw stock lead MPC dominates a zero cruise cap during repeated slow-lead stop/go",
-  strict=True,
-)
 def test_repeated_slow_lead_stop_go_has_no_post_settle_reversal():
   def lead_speed(current_time: float) -> float:
     return float(0.1 * (1.0 - np.cos(np.pi * current_time)))
@@ -318,7 +339,7 @@ def test_repeated_slow_lead_stop_go_has_no_post_settle_reversal():
   settled = trace.time >= 4.0
   assert trace.active[settled].all()
   assert np.all(trace.pace[settled] == 0.0)
-  assert np.any(trace.a_target[settled] > 0.2)
+  assert np.max(trace.a_target[settled]) <= 0.2
   assert not _has_propulsion_brake_reversal(trace, after=4.0)
 
 
@@ -392,6 +413,7 @@ def test_stopped_lead_noise_requires_four_departure_frames_and_launches_within_o
   record_property("predeparture_peak_command", float(np.max(trace.a_target[before_departure])))
   record_property("first_three_departure_frames_peak_command", float(np.max(trace.a_target[first_three_departure_frames])))
   assert np.max(trace.speed[first_three_departure_frames]) < 1e-3
+  assert not trace.launching[first_three_departure_frames].any()
 
   launched = np.flatnonzero((trace.time >= departure_time) & (trace.speed > 0.05))
   assert len(launched)
@@ -427,6 +449,43 @@ def test_stop_hold_two_frame_total_lead_dropout_cannot_launch():
   assert not _has_propulsion_brake_reversal(trace, after=0.5)
 
 
+def test_clear_road_launch_is_immediate_bounded_and_profiles_feel_distinct():
+  common = dict(
+    duration=6.0,
+    controller_enabled=True,
+    lead_relevancy=False,
+    speed=0.0,
+    v_cruise=15.0,
+    actuator_delay=0.15,
+    actuator_lag=0.20,
+  )
+  traces = [_run(profile=profile, **common) for profile in range(3)]
+
+  onset_times = []
+  movement_times = []
+  for trace in traces:
+    positive = np.flatnonzero(trace.a_target > 0.05)
+    moving = np.flatnonzero(trace.speed > 0.01)
+    assert len(positive)
+    assert len(moving)
+    onset_times.append(float(trace.time[positive[0]]))
+    movement_times.append(float(trace.time[moving[0]]))
+    assert np.all(trace.a_target <= trace.output_accel_max + 1e-6)
+    assert trace.solver_failures == 0
+
+  assert max(onset_times) - min(onset_times) <= DT_MDL
+  assert max(onset_times) <= 4 * DT_MDL
+  assert max(movement_times) <= 1.0
+
+  for sample_time in (2.0,):
+    realized = [float(trace.acceleration[np.searchsorted(trace.time, sample_time)]) for trace in traces]
+    assert realized[0] < realized[1] < realized[2], (sample_time, realized)
+  final_speeds = [trace.speed[-1] for trace in traces]
+  assert final_speeds[0] < final_speeds[1] < final_speeds[2]
+  assert final_speeds[1] - final_speeds[0] > 0.5
+  assert final_speeds[2] - final_speeds[1] > 0.4
+
+
 @pytest.mark.parametrize(
   ("actuator_delay", "actuator_lag", "current_tn_jerk_p95"),
   [