StarPilot/openpilot/selfdrive/controls/lib/longitudinal_planner.py

#!/usr/bin/env python3
import math
import time
from collections.abc import Callable
from dataclasses import replace
import numpy as np

import openpilot.cereal.messaging as messaging
from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
from openpilot.common.constants import CV
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.common.realtime import DT_MDL
from openpilot.selfdrive.modeld.constants import ModelConstants
from openpilot.selfdrive.controls.lib.accel_boost import AccelBoost
from openpilot.starpilot.longitudinal.encouragement import Encouragement
from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import (LongitudinalMpc, LongitudinalPlanSource, get_T_FOLLOW, get_jerk_factor,
                                                                                      qualified_model_leads, ModelLeadHistory, should_trigger_planner_fcw)
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC
from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_accel_from_plan, should_stop
from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET
from openpilot.common.swaglog import cloudlog
from openpilot.starpilot.navigation.intent import cruise_ceiling as navigation_ceiling
from openpilot.starpilot.car.tesla.preap_adaptive import startup_owner as preap_adaptive_owner
from openpilot.starpilot.longitudinal.vehicle_policy import forecast_should_stop, planner_cost_owner, far_follow_owner
from openpilot.starpilot.longitudinal.cruise_ceiling import CruiseCeiling, CurveCeiling, select_cruise_ceiling, select_curve_ceiling
from openpilot.starpilot.longitudinal.profile_runtime import (AppliedProfile, ProfileSmoother, ProfileTuning, SelectedProfileTuning,
                                                             TRAFFIC_CRUISE_BRAKE_MAGNITUDE)
from openpilot.starpilot.longitudinal.slc_coast import braking_lead_relevant
from openpilot.starpilot.longitudinal.throttle_gate import ModelThrottleGate
from openpilot.starpilot.longitudinal.ioniq6_start import eligible as ioniq6_long_eligible
from openpilot.starpilot.longitudinal.lane_change_gap import LaneChangeGap, project as project_lane_gap
from openpilot.starpilot.longitudinal.experimental_release import ConditionalHandoff, ExperimentalRelease, project as project_release
from openpilot.starpilot.longitudinal.force_stop import StopPlan
from openpilot.starpilot.longitudinal.follow_jerk import FollowJerk
from openpilot.starpilot.longitudinal.lead_takeoff import LeadTakeoff, project as project_takeoff
from openpilot.starpilot.longitudinal.lead_behavior import adjust as adjust_lead_behavior
from openpilot.starpilot.longitudinal.hkg_vision_stop import owner_for as vision_stop_owner
from openpilot.starpilot.car.ford.stopped_lead import owner_for as ford_stop_owner
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import STOP_DISTANCE
from openpilot.starpilot.longitudinal.lead_approach import LeadApproach, LeadApproachKey, project as project_approach
from openpilot.starpilot.lateral.lane_change_preferences import LaneChangePolicy

A_CRUISE_MAX_VALS = [1.6, 1.2, 0.8, 0.6]
A_CRUISE_MAX_BP = [0., 10.0, 25., 40.]
J_CRUISE_VALS = [1.6, 1.2, 0.8, 0.6]
A_CRUISE_MIN = -0.5
CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
MIN_ALLOW_THROTTLE_SPEED = 2.5
NATIVE_ALLOW_THROTTLE_PROBABILITY = 0.4
NATIVE_PLAN_INPUTS = ('carControl', 'carState', 'controlsState', 'vehicleParameters',
                      'radarState', 'modelV2', 'selfdriveState')

# Lookup table for turns
_A_TOTAL_MAX_V = [1.7, 3.2]
_A_TOTAL_MAX_BP = [20., 40.]
_TAKEOFF_TOTAL_BP = [0., 2., 5., 20., 40.]
_TAKEOFF_TOTAL_MAX = [3.5, 3.5, 1.7, 1.7, 3.2]

def get_max_accel(v_ego):
  return np.interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS)

def get_coast_accel(pitch):
  return np.sin(pitch) * -5.65 - 0.3  # fitted from data using xx/projects/allow_throttle/compute_coast_accel.py

def get_cruise_accel(e2e, v_cruise, v_ego, a_cruise_prev, angle_steers, CP, dt, accel_coast, allow_throttle,
                     profile: AppliedProfile | None = None,
                     base_brake_floor: float = A_CRUISE_MIN, selected_acceleration_max: float | None = None, faster_takeoffs: bool = False,
                     *, a_target_prev: float | None = None):
  max_accel = ACCEL_MAX if e2e else get_max_accel(v_ego)
  if selected_acceleration_max is not None and not e2e:
    max_accel = selected_acceleration_max
  if profile is not None:
    max_accel = min(max_accel, profile.acceleration_max, ACCEL_MAX)

  if selected_acceleration_max is not None:
    max_accel = min(max_accel, selected_acceleration_max, ACCEL_MAX)

  if not e2e:
    a_total_max = np.interp(v_ego, _TAKEOFF_TOTAL_BP, _TAKEOFF_TOTAL_MAX) if faster_takeoffs else np.interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V)
    a_y = v_ego ** 2 * angle_steers * CV.DEG_TO_RAD / (CP.steerRatio * CP.wheelbase)
    a_x_allowed = math.sqrt(max(a_total_max ** 2 - a_y ** 2, 0.))
    max_accel = min(max_accel, a_x_allowed)
    if not allow_throttle:
      clipped_accel_coast = max(accel_coast, ACCEL_MIN)
      coast_limit = np.interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [max_accel, clipped_accel_coast])
      max_accel = min(max_accel, coast_limit)

  cruise_min = max(ACCEL_MIN, -profile.cruise_brake_magnitude) if profile is not None else max(ACCEL_MIN, base_brake_floor)
  target_accel = np.clip(v_cruise - v_ego, cruise_min, max_accel)
  j_cruise = np.interp(v_ego, A_CRUISE_MAX_BP, J_CRUISE_VALS)
  decel_start = a_cruise_prev if a_target_prev is None else min(a_cruise_prev, a_target_prev)
  target_accel = float(np.clip(target_accel, decel_start - j_cruise * dt, a_cruise_prev + j_cruise * dt))

  return target_accel


class LongitudinalPlanner:
  def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL, clock_ns: Callable[[], int] = time.monotonic_ns):
    self.CP = CP
    self.mpc = LongitudinalMpc(dt=dt)
    self.model_lead_history = ModelLeadHistory()
    self.fcw = False
    self.dt = dt
    self.clock_ns = clock_ns
    self.allow_throttle = True
    self.accel_boost = AccelBoost()
    self.encouragement = Encouragement()
    self.plan_drive_id = 0
    self.throttle_gate = ModelThrottleGate()
    self.ioniq6_throttle_gate_enabled = ioniq6_long_eligible(CP)
    self.lane_change_gap = LaneChangeGap()
    self.experimental_release = ExperimentalRelease()
    self.lead_approach = LeadApproach(dt, CP.longitudinalActuatorDelay)

    self.v_desired_filter = FirstOrderFilter(init_v, 2.0, self.dt)
    self.a_cruise = init_a
    self.output_a_target = init_a
    self.output_should_stop = False

    self.v_desired_trajectory = np.zeros(CONTROL_N)
    self.a_desired_trajectory = np.zeros(CONTROL_N)
    self.j_desired_trajectory = np.zeros(CONTROL_N)
    self.last_cruise_ceiling_status = "absent"
    self.last_curve_ceiling_status = "absent"
    self.last_curve_ceiling_applied = False
    self.profile_smoother = ProfileSmoother()
    self.last_profile: AppliedProfile | None = None
    self.force_stop_plan = StopPlan()
    self.follow_jerk = FollowJerk(CP)
    self.vehicle_cost_owner = planner_cost_owner(CP)
    self.lead_takeoff = LeadTakeoff()
    self.vehicle_far_follow = far_follow_owner(CP, dt)
    self.vision_stop_buffer = vision_stop_owner(CP)
    self.ford_stop_approach = ford_stop_owner(CP, STOP_DISTANCE - .5)
    self.preap_adaptive = preap_adaptive_owner(CP)

  def update(self, sm, *, cruise_ceiling: CruiseCeiling | None = None,
             profile_tuning: ProfileTuning | None = None, curve_ceiling: CurveCeiling | None = None,
             curve_provider: Callable[[float | None], CurveCeiling | None] | None = None,
             traffic_mode: bool | None = False, lane_change_policy: LaneChangePolicy | None = None,
             global_braking_response: str | None = None, selected_profiles: SelectedProfileTuning | None = None,
             conditional_handoff: ConditionalHandoff | None = None,
             lead_approach_key: LeadApproachKey | None = None,
             force_stop_provider: Callable[[float], StopPlan] | None = None,
             faster_lead_takeoff: bool = False, takeoff_drive_id: int = 0,
             now_ns: int | None = None, drive_id: int = 0, wheel_coast=None):
    if curve_ceiling is not None and curve_provider is not None:
      raise ValueError('choose explicit Curve ceiling or same-cycle provider')
    if self.plan_drive_id != drive_id:
      self.model_lead_history.clear()
    self.plan_drive_id = drive_id
    faster_takeoffs = bool(faster_lead_takeoff)
    sample_now_ns = self.clock_ns() if now_ns is None else now_ns
    if len(sm['carControl'].orientationNED) == 3:
      accel_coast = get_coast_accel(sm['carControl'].orientationNED[1])
    else:
      accel_coast = ACCEL_MAX

    v_ego = sm['carState'].vEgo
    v_cruise_kph = min(sm['carState'].vCruise, V_CRUISE_MAX)
    v_cruise = v_cruise_kph * CV.KPH_TO_MS
    ceiling = select_cruise_ceiling(
      cruise_ceiling, driver_v_cruise_kph=sm['carState'].vCruise, driver_cruise_mps=v_cruise,
      system_long_available=self.CP.openpilotLongitudinalControl,
      long_control_active=sm['controlsState'].longControlState != LongCtrlState.off,
      selfdrive_enabled=sm['selfdriveState'].enabled, force_decel=sm['controlsState'].forceDecel,
    )
    self.last_cruise_ceiling_status = ceiling.status
    if ceiling.effective_mps is not None:
      v_cruise = ceiling.effective_mps
    long_control_off = sm['controlsState'].longControlState == LongCtrlState.off
    profile_eligible = (self.CP.openpilotLongitudinalControl and
                        sm['selfdriveState'].enabled and not long_control_off and sm['carControl'].longActive)
    if profile_eligible:
      self.last_profile = self.profile_smoother.sample(profile_tuning, sm['selfdriveState'].personality, self.dt,
                                                       traffic_mode=traffic_mode)
    else:
      self.profile_smoother.applied = None
      self.last_profile = None

    # Reset current state when not engaged, or user is controlling the speed
    reset_state = long_control_off if self.CP.openpilotLongitudinalControl else not sm['selfdriveState'].enabled
    # PCM cruise speed may be updated a few cycles later, check if initialized
    v_cruise_initialized = sm['carState'].vCruise != V_CRUISE_UNSET
    reset_state = reset_state or not v_cruise_initialized

    throttle_probs = sm['modelV2'].meta.disengagePredictions.gasPressProbs
    throttle_prob = throttle_probs[1] if len(throttle_probs) > 1 else 1.0
    if self.ioniq6_throttle_gate_enabled and sm['carControl'].longActive and not reset_state:
      model_ns = getattr(sm, 'logMonoTime', {}).get('modelV2', 0)
      self.allow_throttle = self.throttle_gate.step(throttle_prob if len(throttle_probs) > 1 else float('nan'),
                                                    v_ego, low_speed_mps=MIN_ALLOW_THROTTLE_SPEED, model_ns=model_ns,
                                                    now_ns=sample_now_ns,
                                                    model_valid=(getattr(sm, 'valid', {}).get('modelV2', False) is True and
                                                                 getattr(sm, 'alive', {}).get('modelV2', False) is True))
    else:
      self.throttle_gate.reset()
      self.allow_throttle = throttle_prob > NATIVE_ALLOW_THROTTLE_PROBABILITY or v_ego <= MIN_ALLOW_THROTTLE_SPEED

    steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['vehicleParameters'].angleOffsetDeg

    if reset_state:
      self.v_desired_filter.x = v_ego
      self.output_a_target = np.clip(sm['carState'].aEgo, ACCEL_MIN, ACCEL_MAX)
      self.a_cruise = self.output_a_target

    # Prevent divergence, smooth in current v_ego
    self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego))

    # No change cost when user is controlling the speed, or when standstill
    prev_accel_constraint = not (reset_state or sm['carState'].standstill)

    if self.CP.openpilotLongitudinalControl:
      self.mpc.set_cur_state(self.v_desired_filter.x, self.output_a_target)
    else:
      self.mpc.set_cur_state(v_ego, sm['carState'].aEgo)
    base_follow = self.last_profile.follow_seconds if self.last_profile is not None else get_T_FOLLOW(sm['selfdriveState'].personality)
    approach_follow = self.lead_approach.step(
      lead_approach_key, project_approach(sm, self.CP, sample_now_ns, lead_approach_key) if lead_approach_key is not None else None,
      base_follow, blocked=reset_state)
    gap_follow = self.lane_change_gap.step(
      lane_change_policy, project_lane_gap(sm, sample_now_ns) if lane_change_policy is not None else None,
      base_follow=base_follow, long_active=bool(self.CP.openpilotLongitudinalControl and sm['carControl'].longActive and
                                                not reset_state and sm['selfdriveState'].enabled and approach_follow is None))
    follow = approach_follow if approach_follow is not None else gap_follow
    selected_follow = follow if follow is not None else base_follow
    self.force_stop_plan = force_stop_provider(selected_follow) if force_stop_provider is not None else StopPlan()
    if (reset_state or not profile_eligible or self.force_stop_plan.model_ns != getattr(sm, 'logMonoTime', {}).get('modelV2') or
        sm['carState'].gasPressed or sm['carState'].brakePressed or sm['controlsState'].forceDecel):
      self.force_stop_plan = StopPlan()
    if self.force_stop_plan.speed_ceiling_mps is not None:
      v_cruise = min(v_cruise, self.force_stop_plan.speed_ceiling_mps)
    follow_scale = self.follow_jerk.sample(sm, self.CP, sample_now_ns, selected_follow, self.force_stop_plan,
                                           active=profile_eligible and not reset_state, drive_id=drive_id)
    accelerating = sm['carState'].aEgo >= 0
    weights = None
    if self.vehicle_cost_owner is not None:
      # Current mode arbitration is retained; do not recreate old one-cycle delay.
      weights = self.vehicle_cost_owner.sample(
        sm, sample_now_ns, active=profile_eligible and not reset_state, drive_id=drive_id,
        mode='blended' if sm['selfdriveState'].experimentalMode else 'acc',
        acceleration_factor=(self.last_profile.acceleration_jerk if accelerating else self.last_profile.deceleration_jerk)
        if self.last_profile is not None else get_jerk_factor(sm['selfdriveState'].personality),
        speed_factor=(self.last_profile.speed_jerk if accelerating else self.last_profile.speed_decrease_jerk)
        if self.last_profile is not None else get_jerk_factor(sm['selfdriveState'].personality),
        danger_factor=self.last_profile.danger_jerk if self.last_profile is not None else get_jerk_factor(sm['selfdriveState'].personality),
        stop_plan=self.force_stop_plan, follow_scale=follow_scale, prev_accel_constraint=prev_accel_constraint)
    if weights is not None:
      self.mpc.set_cost_weights(weights.stage, weights.constraints)
    else:
      self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality,
                           acceleration_jerk=(self.last_profile.acceleration_jerk if accelerating else self.last_profile.deceleration_jerk)
                           if self.last_profile is not None else None,
                           speed_jerk=(self.last_profile.speed_jerk if accelerating else self.last_profile.speed_decrease_jerk)
                           if self.last_profile is not None else None,
                           danger_jerk=self.last_profile.danger_jerk if self.last_profile is not None else None,
                           stop_jerk_scale=self.force_stop_plan.jerk_scale, acceleration_change_scale=follow_scale)
    selected_acceleration = selected_profiles.acceleration_max if profile_eligible and selected_profiles is not None else None
    acceleration_cap = self.last_profile.acceleration_max if self.last_profile is not None else None
    if selected_acceleration is not None:
      acceleration_cap = selected_acceleration
    preap_acceleration_cap = None
    if self.preap_adaptive is not None:
      preap_acceleration_cap = self.preap_adaptive.sample(
        sm, now_ns=sample_now_ns, drive_id=drive_id, upper=ACCEL_MAX if acceleration_cap is None else acceleration_cap,
        follow_seconds=selected_follow, active=profile_eligible and not reset_state)
      if preap_acceleration_cap is not None:
        acceleration_cap = preap_acceleration_cap
    self.mpc.update(sm['radarState'], personality=sm['selfdriveState'].personality,
                    follow_seconds=follow if follow is not None else
                    (self.last_profile.follow_seconds if self.last_profile is not None else None),
                    acceleration_max=acceleration_cap,
                    stop_line_m=self.force_stop_plan.obstacle_m, modelV2=qualified_model_leads(sm, sample_now_ns, self.model_lead_history))

    # MPC has selected the actual follow time for this model cycle. Its solve
    # does not use the Curve cruise cap, so a provider may now evaluate the
    # filtered lead condition without reading a stale saved preference.
    if curve_provider is not None:
      try:
        selected_follow = float(self.mpc.params[0, 4])
        if not math.isfinite(selected_follow) or not 0.5 <= selected_follow <= 3.0:
          selected_follow = None
      except (AttributeError, IndexError, TypeError, ValueError, OverflowError):
        selected_follow = None
      curve_ceiling = curve_provider(selected_follow)
    curve = select_curve_ceiling(
      curve_ceiling, model_ns=getattr(sm, 'logMonoTime', {}).get('modelV2', 0),
      driver_v_cruise_kph=sm['carState'].vCruise, effective_cruise_mps=v_cruise,
      system_long_available=self.CP.openpilotLongitudinalControl,
      long_control_active=sm['controlsState'].longControlState != LongCtrlState.off,
      car_long_active=sm['carControl'].longActive, selfdrive_enabled=sm['selfdriveState'].enabled,
      force_decel=sm['controlsState'].forceDecel, driver_override=sm['carState'].gasPressed or sm['carState'].brakePressed,
    )
    self.last_curve_ceiling_status = curve.status
    self.last_curve_ceiling_applied = False
    if curve.effective_mps is not None:
      v_cruise = curve.effective_mps
    navigation_target = navigation_ceiling(sm, self.CP, sample_now_ns, v_cruise) if profile_eligible else None
    if navigation_target is not None:
      v_cruise = min(v_cruise, navigation_target)
    coast_plan = None
    if wheel_coast is not None:
      owner, wheel_params = wheel_coast
      pulse_lead = any(lead.present and (v_ego - lead.vLead > 0.5 or lead.aLeadK < -0.4 or
                                        lead.dRel < max(18.0, 2.0 * v_ego))
                       for lead in (sm['radarState'].leadOne, sm['radarState'].leadTwo))
      coast_plan = owner.sample(wheel_params, self.CP, sm, now_ns=sample_now_ns, target_mps=v_cruise,
        lead_relevant=bool(pulse_lead), stop_context=bool(sm['carState'].standstill or sm['controlsState'].forceDecel or
        self.force_stop_plan.forcing or sm['modelV2'].action.shouldStop or not self.allow_throttle))
      if coast_plan.speed_ceiling_mps is not None:
        v_cruise = min(v_cruise, coast_plan.speed_ceiling_mps)
    if sm['controlsState'].forceDecel or coast_plan is not None and coast_plan.force_decel:
      v_cruise = 0.0

    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)
    self.j_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC[:-1], self.mpc.j_solution)

    # TODO counter is only needed because radar is glitchy, remove once radar is gone
    self.fcw = self.mpc.crash_cnt > 2 and not sm['carState'].standstill
    if self.CP.carFingerprint == 'SUBARU_CROSSTREK_2025':
      self.fcw = self.fcw and should_trigger_planner_fcw(sm['radarState'].leadOne, float(v_ego))
    if self.fcw:
      cloudlog.info("FCW triggered")

    # Save starting point for next iteration
    a_prev = self.output_a_target

    action_t =  self.CP.longitudinalActuatorDelay + DT_MDL
    output_a_target_mpc = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX,
                                              action_t=action_t)
    output_should_stop_mpc = forecast_should_stop(
      self.CP, self.v_desired_trajectory, CONTROL_N_T_IDX, action_t,
      fallback=should_stop(v_ego, output_a_target_mpc))
    output_a_target_e2e = sm['modelV2'].action.desiredAcceleration
    output_should_stop_e2e = sm['modelV2'].action.shouldStop

    relevant_lead = any(braking_lead_relevant(lead, v_ego) for lead in
                        (sm['radarState'].leadOne, sm['radarState'].leadTwo))
    global_magnitude = ({'standard': 1.2, 'eco': 0.6, 'sport': 2.4}.get(global_braking_response)
                        if type(global_braking_response) is str else None)
    selected_braking = (selected_profiles.cruise_brake_magnitude
                        if profile_eligible and selected_profiles is not None else None)
    if selected_braking is not None:
      global_magnitude = selected_braking  # One resolved response, never multiplied or stacked.
      global_braking_response = selected_profiles.braking_style
    global_selected = (global_magnitude is not None and profile_eligible and (traffic_mode is False or selected_braking is not None) and
                       not sm['controlsState'].forceDecel and not sm['carState'].standstill and
                       not sm['carState'].brakePressed and not sm['carState'].gasPressed and
                       not relevant_lead and not self.force_stop_plan.forcing and not sm['modelV2'].action.shouldStop and
                       not output_should_stop_mpc and
                       (selected_braking is not None or self.last_profile is None or profile_tuning is None or
                        profile_tuning.cruise_brake_magnitude is None))
    global_floor = -global_magnitude if global_selected else A_CRUISE_MIN
    cruise_profile = self.last_profile
    custom_traffic_braking = (traffic_mode is True and profile_eligible and cruise_profile is not None and
                              profile_tuning is not None and profile_tuning.traffic_braking_custom is True and
                              not sm['controlsState'].forceDecel)
    if custom_traffic_braking and (sm['carState'].standstill or sm['radarState'].leadOne.present or
                                   sm['radarState'].leadTwo.present):
      cruise_profile = replace(cruise_profile, cruise_brake_magnitude=TRAFFIC_CRUISE_BRAKE_MAGNITUDE)
      self.profile_smoother.applied = cruise_profile
      self.last_profile = cruise_profile
    if global_selected and cruise_profile is not None:
      cruise_profile = replace(cruise_profile, cruise_brake_magnitude=global_magnitude)
    if sm['controlsState'].forceDecel:
      cruise_profile = None
    elif (ceiling.effective_mps is not None or curve.effective_mps is not None) and cruise_profile is not None:
      # An eligible lower ceiling must retain at least native approach
      # braking even when the saved profile requests a softer cruise response.
      cruise_profile = replace(cruise_profile, cruise_brake_magnitude=max(cruise_profile.cruise_brake_magnitude,
                                                                          abs(A_CRUISE_MIN)))
    if ceiling.effective_mps is not None or curve.effective_mps is not None:
      global_floor = min(global_floor, A_CRUISE_MIN)
    if selected_acceleration is not None and cruise_profile is not None:
      # The selected curve owns acceleration; following/jerks retain their owner.
      cruise_profile = replace(cruise_profile, acceleration_max=selected_acceleration)
    if coast_plan is not None and coast_plan.brake_floor is not None:
      global_floor = coast_plan.brake_floor
      cruise_profile = None
    if preap_acceleration_cap is not None:
      selected_acceleration = (preap_acceleration_cap if selected_acceleration is None else
                               min(selected_acceleration, preap_acceleration_cap))
    self.a_cruise = get_cruise_accel(sm['selfdriveState'].experimentalMode, v_cruise, v_ego,
                                     self.a_cruise, steer_angle_without_offset, self.CP, self.dt,
                                     accel_coast, self.allow_throttle, cruise_profile,
                                     base_brake_floor=global_floor, selected_acceleration_max=selected_acceleration, faster_takeoffs=faster_takeoffs,
                                     a_target_prev=self.output_a_target)
    cruise_should_stop = should_stop(v_ego, self.a_cruise)

    encouraged_model = self.encouragement.update(
      sm, self.CP, now_ns=sample_now_ns, drive_id=drive_id, dt=self.dt, target_speed=v_cruise,
      model_accel=output_a_target_e2e, mpc_accel=output_a_target_mpc, cruise_accel=self.a_cruise,
      lateral_accel=(v_ego ** 2 * steer_angle_without_offset * CV.DEG_TO_RAD / (self.CP.steerRatio * self.CP.wheelbase)
                     if self.CP.steerRatio > 0 and self.CP.wheelbase > 0 else math.inf),
      blocked=bool(reset_state or self.fcw or not self.allow_throttle or self.force_stop_plan.forcing or
                   output_should_stop_mpc or cruise_should_stop or self.mpc.solution_status != 0 or
                   curve.effective_mps is not None or traffic_mode is not False))
    manual_model = self.accel_boost.update(sm, output_a_target_e2e, output_a_target_mpc, self.a_cruise)
    output_a_target_e2e = max(manual_model, encouraged_model)

    candidates = [(output_a_target_mpc, self.mpc.source, output_should_stop_mpc),
                  (self.a_cruise, LongitudinalPlanSource.cruise, cruise_should_stop)]
    if sm['selfdriveState'].experimentalMode:
      candidates.append((output_a_target_e2e, LongitudinalPlanSource.e2e, output_should_stop_e2e))

    mpc_source = self.mpc.source
    output_a_target, self.mpc.source, _ = min(candidates, key=lambda c: c[0])
    # A curve candidate alone cannot own driver feedback. Lead/e2e braking and
    # an equal or lower SLC ceiling retain their own attribution.
    self.last_curve_ceiling_applied = (curve.effective_mps is not None and self.a_cruise < output_a_target_mpc and
                                      (not sm['selfdriveState'].experimentalMode or self.a_cruise < output_a_target_e2e))
    self.output_should_stop = self.force_stop_plan.should_stop or any(should_stop for _, _, should_stop in candidates)
    if conditional_handoff is None:
      self.experimental_release.reset()
    else:
      mpc_lead_index = {LongitudinalPlanSource.lead0: 0, LongitudinalPlanSource.lead1: 1}.get(mpc_source)
      release_cap = self.experimental_release.step(
        conditional_handoff, project_release(sm, self.CP, sample_now_ns, lead_index=mpc_lead_index,
                                             conditional_handoff=conditional_handoff),
        previous_target=a_prev, target=output_a_target, follow_seconds=float(self.mpc.params[0, 4]),
        blocked=bool(reset_state or self.output_should_stop or self.fcw))
      if release_cap is not None:
        output_a_target = min(output_a_target, release_cap)
    output_a_target = adjust_lead_behavior(
      target=output_a_target, sm=sm, cp=self.CP, now_ns=sample_now_ns, key=conditional_handoff,
      follow_seconds=float(self.mpc.params[0, 4]), mpc_target=output_a_target_mpc,
      cruise_target=self.a_cruise, model_target=output_a_target_e2e, stopping=self.output_should_stop,
      force_stop=self.force_stop_plan.forcing, traffic_mode=traffic_mode, accel_min=ACCEL_MIN)
    if faster_lead_takeoff:
      # Dom takeoff bypasses the standstill cruise slew, never its final bounds.
      takeoff_max = ACCEL_MAX if sm['selfdriveState'].experimentalMode else get_max_accel(v_ego)
      if selected_acceleration is not None and not sm['selfdriveState'].experimentalMode:
        takeoff_max = selected_acceleration
      if cruise_profile is not None:
        takeoff_max = min(takeoff_max, cruise_profile.acceleration_max)
      if selected_acceleration is not None:
        takeoff_max = min(takeoff_max, selected_acceleration)
      a_total_max = np.interp(v_ego, _TAKEOFF_TOTAL_BP, _TAKEOFF_TOTAL_MAX) if faster_takeoffs else np.interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V)
      a_y = v_ego ** 2 * steer_angle_without_offset * CV.DEG_TO_RAD / (self.CP.steerRatio * self.CP.wheelbase)
      takeoff_max = min(takeoff_max, math.sqrt(max(a_total_max ** 2 - a_y ** 2, 0.)), max(0., v_cruise - v_ego))
      takeoff_frame = project_takeoff(
        sm, self.CP, now_ns=sample_now_ns, drive_id=takeoff_drive_id, stop_plan=self.force_stop_plan,
        acceleration_max=takeoff_max, mpc_accel=output_a_target_mpc, follow_seconds=float(self.mpc.params[0, 4]),
        blocked=bool(reset_state or self.fcw or not self.allow_throttle or traffic_mode is not False or
                     self.mpc.solution_status != 0))
      output_a_target, self.output_should_stop = self.lead_takeoff.step(
        True, takeoff_frame, output_a_target, self.output_should_stop)
    else:
      self.lead_takeoff.reset()
    if self.vehicle_far_follow is not None:
      output_a_target = self.vehicle_far_follow.sample(
        sm, now_ns=sample_now_ns, drive_id=drive_id, active=profile_eligible and not reset_state,
        target=float(output_a_target), previous_target=float(a_prev), follow_seconds=float(self.mpc.params[0, 4]),
        blocked=bool(self.output_should_stop or self.fcw or self.force_stop_plan.forcing or
                     self.force_stop_plan.approach_distance_m > 0 or self.mpc.solution_status != 0))
    if self.vision_stop_buffer is not None:
      output_a_target, self.output_should_stop = self.vision_stop_buffer.sample(
        sm, now_ns=sample_now_ns, drive_id=drive_id, target=float(output_a_target),
        stopping=self.output_should_stop, accel_min=ACCEL_MIN,
        active=profile_eligible and not reset_state and self.mpc.solution_status == 0)
    if preap_acceleration_cap is not None:
      output_a_target = min(output_a_target, preap_acceleration_cap)
    if self.ford_stop_approach is not None:
      output_a_target, self.output_should_stop = self.ford_stop_approach.sample(
        sm, now_ns=sample_now_ns, drive_id=drive_id, target=float(output_a_target),
        stopping=self.output_should_stop, accel_min=ACCEL_MIN, active=profile_eligible and not reset_state)
    self.output_a_target = np.clip(output_a_target, ACCEL_MIN, ACCEL_MAX)

    self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.output_a_target + a_prev) / 2.0

  def publish(self, sm, pm):
    plan_send = messaging.new_message('longitudinalPlan')

    plan_send.valid = sm.all_checks(NATIVE_PLAN_INPUTS)

    longitudinalPlan = plan_send.longitudinalPlan
    longitudinalPlan.modelMonoTime = sm.logMonoTime['modelV2']
    longitudinalPlan.processingDelay = (plan_send.logMonoTime - sm.logMonoTime['modelV2']) / 1e9
    longitudinalPlan.solverExecutionTime = self.mpc.solve_time

    longitudinalPlan.speeds = self.v_desired_trajectory.tolist()
    longitudinalPlan.accels = self.a_desired_trajectory.tolist()
    longitudinalPlan.jerks = self.j_desired_trajectory.tolist()

    longitudinalPlan.hasLead = sm['radarState'].leadOne.present
    longitudinalPlan.longitudinalPlanSource = self.mpc.source
    longitudinalPlan.fcw = self.fcw

    longitudinalPlan.aTarget = float(self.output_a_target)
    longitudinalPlan.accelBoost = float(self.accel_boost.total_boost)
    longitudinalPlan.shouldStop = bool(self.output_should_stop)
    longitudinalPlan.allowBrake = True
    longitudinalPlan.allowThrottle = bool(self.allow_throttle)

    companion = messaging.new_message('starpilotLongitudinalPlan')
    companion.logMonoTime = plan_send.logMonoTime
    companion.valid = bool(plan_send.valid and 0 < self.plan_drive_id < longitudinalPlan.modelMonoTime <= plan_send.logMonoTime)
    hold = companion.starpilotLongitudinalPlan
    hold.version = 1
    hold.sourcePlanMonoTime = plan_send.logMonoTime
    hold.modelMonoTime = longitudinalPlan.modelMonoTime
    hold.driveStartMonoTime = self.plan_drive_id
    hold.forceStopHolding = bool(self.force_stop_plan.manual_hold)
    pm.send('longitudinalPlan', plan_send)
    pm.send('starpilotLongitudinalPlan', companion)