StarPilot/selfdrive/controls/controlsd.py

#!/usr/bin/env python3
import math
from numbers import Number

from cereal import car, custom, log
import cereal.messaging as messaging
from openpilot.common.constants import CV
from openpilot.common.params import Params
from openpilot.common.realtime import config_realtime_process, DT_CTRL, Priority, Ratekeeper
from openpilot.common.swaglog import cloudlog

from opendbc.car.car_helpers import interfaces
from opendbc.car.chrysler.values import pacifica_hybrid_aol_stock_acc_mode
from opendbc.car.gm.values import CAR as GM_CAR
from opendbc.car.vehicle_model import VehicleModel
from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature, get_lateral_active
from openpilot.selfdrive.controls.lib.latcontrol import LatControl
from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD
from openpilot.selfdrive.controls.lib.latcontrol_torque import (
  BOLT_2018_2021_STEER_RATIO_TEST_SCALE,
  LatControlTorque,
  get_bolt_2017_steer_ratio_scale,
)
from openpilot.selfdrive.controls.lib.longcontrol import LongControl
from openpilot.selfdrive.car.cruise_state import should_cancel_stock_cruise
from openpilot.selfdrive.modeld.modeld import LAT_SMOOTH_SECONDS
from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose

from openpilot.starpilot.common.starpilot_variables import get_starpilot_toggles
from openpilot.starpilot.controls.lib.neural_network_feedforward import LatControlNNFF

State = log.SelfdriveState.OpenpilotState
LaneChangeState = log.LaneChangeState
LaneChangeDirection = log.LaneChangeDirection

ACTUATOR_FIELDS = tuple(car.CarControl.Actuators.schema.fields.keys())

# After a smoothed lane change ends, ramp the curvature limits back to stock over this
# time so the final recenter correction is shaped instead of stepping through unclamped.
LANE_CHANGE_SMOOTH_RELEASE_T = 2.0

# Floor on the jerk factor when the model is unwinding lane-change curvature (the arrest and
# any correction back toward center). Entry gentleness is comfort, but arrest speed is a
# correctness constraint: a slow symmetric cap lets the car glide past the new lane center.
# 0.6 (≈ pace-5 rate) fully tracks the arrest demand seen in logs while staying 40% below stock.
LANE_CHANGE_ARREST_JERK_FLOOR = 0.6

# Low-speed turn-intent curvature hold. Approaching a turn with the blinker on, the
# model's time-based plan collapses as the car slows to a stop: desiredCurvature decays
# to zero, the controller actively unwinds the wheel at the intersection, and on
# pull-away it re-winds too late — the car goes wide (pauseturn rlog 2026-07-13).
# The hold ratchets up on the blinker-matching model command below the release speed
# and floors the command magnitude afterwards. Below the hard speed the floor is firm;
# between hard and release speed it decays toward the model's sustained demand, so a
# transient model dip barely sags it while a genuine end-of-turn unwind or an aborted
# turn still drains it in a few seconds. Retention deliberately does NOT depend on the
# blinker (the stalk auto-cancels during the stop in the log), on latActive (lateral
# goes inactive at standstill on torque cars; the wheel parks on rack friction), or on
# steeringPressed (the driver's instinctive grip during the unwind is what let the
# collapse through, and the driver physically overpowers a torque command regardless).
CURVATURE_HOLD_HARD_SPEED = 4.5 * CV.MPH_TO_MS
# Release must sit ABOVE the speed where the model's action wakes up mid-turn. Left
# turns cross the intersection before arcing, so the car reaches ~3.5-4 m/s while the
# action still reads ~0 (left1/left2 rlogs 2026-07-15: a 6 mph release dropped the
# floor mid-turn and visibly unwound the wheel 50 deg before the action woke; rights
# wake at ~2.2 m/s and never showed it). Hold authority above creep speed is bounded
# by the opposite-command release and the decay band, not by this ceiling.
CURVATURE_HOLD_RELEASE_SPEED = 10.0 * CV.MPH_TO_MS
# Pre-wind is a NEAR-STANDSTILL device: winding the wheel is only free when the car
# isn't moving. On rolling slow turns a plan-sourced floor applies the turn's final
# curvature at the entry, starting the arc 4-7 m early — the "turning too much"
# corrections in the stickyright1/2 and left-crossing rlogs (2026-07-16) all trace to
# plan capture while moving. Above this speed only the model's own action can raise
# the hold, rate-limited so a single-frame action spike (left1 rlog 21.9s: -0.157 for
# one model frame) can't get captured and floored for seconds.
CURVATURE_HOLD_PLAN_SOURCE_SPEED = 2.0 * CV.MPH_TO_MS
CURVATURE_HOLD_RATCHET_RATE = 0.04  # 1/m per s, hold growth limit above the plan-source speed
# Once the model's action has sustainably taken over the turn, hand off COMPLETELY:
# clear the hold and don't re-engage until the blinker cycle ends. A floor that chases
# the awake action only distorts the model's entry spiral, mid-turn shape, and exit
# unwind — the sticky right-turn exits were the floor trailing the model's unwind by
# 0.03-0.05 even at the fast decay (stickyright1 41.15s: floor 0.064 vs action 0.014,
# driver correcting +394). The bridge job is done the moment the action is awake.
CURVATURE_HOLD_HANDOFF_FRAC = 0.75
CURVATURE_HOLD_HANDOFF_TIME = 0.3  # s of sustained action >= frac*hold before handoff
CURVATURE_HOLD_DECAY_TAU = 2.0     # s; hold tracks a sustained lower model demand with this time constant
# At a turn exit the model unwinds through small SAME-sign commands (curvature only
# flips negative for the final counter-steer), so the opposite-release fires late and
# the tau-2 decay melts the floor slower than the model's exit ramp — the car keeps
# arcing while the driver hauls the wheel back (tightright3/4 rlogs 2026-07-15, drv
# +500). Turn progress is the discriminator between a mid-turn dip (protect the floor;
# left-turn sags happened at ~20 deg of swept heading) and an exit (drop it; the
# sticks happened past ~80 deg): once the swept heading passes the threshold, the
# decay switches to the fast tau and runs at ANY speed, including below the hard-hold
# speed. Swept resets whenever the hold disengages.
CURVATURE_HOLD_SWEPT_EXIT = 0.9    # rad of heading actually turned (~52 deg)
CURVATURE_HOLD_EXIT_DECAY_TAU = 0.5  # s
CURVATURE_HOLD_STANDSTILL_TIMEOUT = 30.0  # s stopped before the held turn intent is dropped

# The model's time-domain action.desiredCurvature is blind below ~2.5 m/s (0.3 s ahead
# at creep speed is centimeters of road), but the plan's spatial geometry already shows
# the turn at standstill: turn3/turn4 rlogs 2026-07-14 read plan curvature 0.13-0.16
# while the action output sat at 0.005, and the plan value matched the demand the
# action produced once rolling. Feeding it into the turn-hold ratchet lets the wheel
# pre-wind toward the real turn before the car moves. Scaled and capped conservatively:
# a too-high floor turns in tighter than the path (mild at creep lat accel), while a
# too-low one just reduces the head start. The plan flickers straight for ~1-2 s right
# at the standstill->motion transition; the ratchet holds through it by design.
CURVATURE_HOLD_PLAN_LOOKAHEAD_NEAR = 4.0  # m; reads whether the turn starts NOW
CURVATURE_HOLD_PLAN_LOOKAHEAD_FAR = 7.0   # m; reads the turn's curvature
CURVATURE_HOLD_PLAN_SCALE = 0.85
CURVATURE_HOLD_PLAN_CAP = 0.12       # 1/m
# The model counter-steers at every turn exit; an opposite-direction command is the
# "turn is over" signal at any speed. Without this the floor converted the exit unwind
# (-0.076) into a stuck +0.012 for 1.4 s and the driver had to unwind by hand
# (rightturnfail rlog 33.2-34.4s). Deadband rejects the ~0.002 pull-away flickers.
CURVATURE_HOLD_OPPOSITE_RELEASE = 0.01  # 1/m


def _plan_circle_curvature(xs, ys, lookahead: float) -> float:
  # curvature of the circle through the origin, tangent to the car's heading, passing
  # through the plan point ~lookahead meters ahead: kappa = 2y / (x^2 + y^2)
  px, py = 0.0, 0.0
  for x, y in zip(xs, ys):
    px, py = x, y
    if math.hypot(x, y) >= lookahead:
      break
  d2 = px * px + py * py
  if d2 < 1.0:
    return 0.0
  return 2.0 * py / d2


def get_plan_spatial_curvature(model_v2) -> float:
  # Min-magnitude of a near and a far circle fit. The far probe alone assumes the turn
  # starts immediately, which over-winds wide turns whose arc begins several meters out
  # (wide multi-lane lefts): the near probe reads ~straight there and only grows as the
  # car creeps up to the arc, so the pre-wind self-scales to the turn geometry. Sign
  # disagreement means no coherent turn ahead: contribute nothing.
  xs, ys = model_v2.position.x, model_v2.position.y
  near = _plan_circle_curvature(xs, ys, CURVATURE_HOLD_PLAN_LOOKAHEAD_NEAR)
  far = _plan_circle_curvature(xs, ys, CURVATURE_HOLD_PLAN_LOOKAHEAD_FAR)
  if near * far <= 0.0:
    return 0.0
  return near if abs(near) < abs(far) else far


def get_gm_hud_set_speed(set_speed_ms: float, starpilot_toggles) -> float:
  spoofed_speed = set_speed_ms

  set_speed_offset = float(getattr(starpilot_toggles, "set_speed_offset", 0.0) or 0.0)
  if spoofed_speed > 0 and set_speed_offset > 0:
    spoofed_speed += set_speed_offset * CV.KPH_TO_MS

  return spoofed_speed


def get_torque_control_params(CP, torque_params, starpilot_toggles, use_live_params: bool) -> tuple[float, float, float]:
  torque_tune = CP.lateralTuning.torque
  lat_accel_factor = torque_tune.latAccelFactor
  lat_accel_offset = torque_tune.latAccelOffset
  friction = torque_tune.friction

  use_custom_lat_accel = getattr(starpilot_toggles, "use_custom_latAccelFactor", False)
  use_custom_friction = getattr(starpilot_toggles, "use_custom_friction", False)

  if use_live_params:
    if not use_custom_lat_accel:
      lat_accel_factor = torque_params.latAccelFactorFiltered
      lat_accel_offset = torque_params.latAccelOffsetFiltered
    if not use_custom_friction:
      friction = torque_params.frictionCoefficientFiltered

  if use_custom_lat_accel:
    lat_accel_factor = starpilot_toggles.latAccelFactor
  if use_custom_friction:
    friction = starpilot_toggles.friction

  return lat_accel_factor, lat_accel_offset, friction


class Controls:
  def __init__(self) -> None:
    self.params = Params()
    cloudlog.info("controlsd is waiting for CarParams")
    self.CP = messaging.log_from_bytes(self.params.get("CarParams", block=True), car.CarParams)
    self.FPCP = messaging.log_from_bytes(self.params.get("StarPilotCarParams", block=True), custom.StarPilotCarParams)
    cloudlog.info("controlsd got CarParams")

    self.CI = interfaces[self.CP.carFingerprint](self.CP, self.FPCP)

    self.sm = messaging.SubMaster(['liveDelay', 'liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState',
                                   'liveCalibration', 'livePose', 'longitudinalPlan', 'lateralManeuverPlan', 'carState', 'carOutput',
                                   'driverMonitoringState', 'onroadEvents', 'driverAssistance'], poll='selfdriveState')
    self.pm = messaging.PubMaster(['carControl', 'controlsState'])

    self.steer_limited_by_safety = False
    self.curvature = 0.0
    self.desired_curvature = 0.0
    self.lc_smooth_release = 0.0
    self.turn_hold_curvature = 0.0
    self.turn_hold_standstill_t = 0.0
    self.turn_hold_swept = 0.0
    self.turn_hold_handoff_t = 0.0
    self.turn_hold_done = False

    self.pose_calibrator = PoseCalibrator()
    self.calibrated_pose: Pose | None = None

    self.LoC = LongControl(self.CP)
    self.VM = VehicleModel(self.CP)
    self.LaC: LatControl
    if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
      self.LaC = LatControlAngle(self.CP, self.CI, DT_CTRL)
    elif self.CP.lateralTuning.which() == 'pid':
      self.LaC = LatControlPID(self.CP, self.CI, DT_CTRL)
    elif self.CP.lateralTuning.which() == 'torque':
      self.LaC = LatControlTorque(self.CP, self.CI, DT_CTRL)

    self.sm = self.sm.extend(['liveDelay', 'starpilotCarState', 'starpilotPlan'])

    self.starpilot_toggles = get_starpilot_toggles()
    self.ecu_disable_failed = False
    self.ecu_disable_failed_checked = not self.CP.openpilotLongitudinalControl

    if self.CP.lateralTuning.which() == "torque" and (self.starpilot_toggles.nnff or self.starpilot_toggles.nnff_lite):
      self.LaC = LatControlNNFF(self.CP, self.CI, DT_CTRL)

  def update(self):
    self.sm.update(15)
    if self.sm.updated["liveCalibration"]:
      self.pose_calibrator.feed_live_calib(self.sm['liveCalibration'])
    if self.sm.updated["livePose"]:
      device_pose = Pose.from_live_pose(self.sm['livePose'])
      self.calibrated_pose = self.pose_calibrator.build_calibrated_pose(device_pose)

    if hasattr(self.LaC, "pid") and self.CP.lateralTuning.which() != "pid":
      self.LaC.pid._k_p = self.starpilot_toggles.steerKp

    if self.sm.updated['liveDelay'] and hasattr(self.LaC, "update_live_delay"):
      self.LaC.update_live_delay(self.sm['liveDelay'].lateralDelay)

    self.starpilot_toggles = get_starpilot_toggles(self.sm)

  def update_ecu_disable_failed(self):
    if self.ecu_disable_failed_checked:
      return

    # ControlsReady is set after CarInterface.init(), where Hyundai ECU disable
    # writes EcuDisableFailed. Once init has completed, the value is stable.
    if self.params.get_bool("ControlsReady"):
      self.ecu_disable_failed = self.params.get_bool("EcuDisableFailed")
      self.ecu_disable_failed_checked = True

  def state_control(self):
    CS = self.sm['carState']

    # Update VehicleModel
    lp = self.sm['liveParameters']
    x = max(lp.stiffnessFactor, 0.1)
    sr = max(lp.steerRatio, 0.1)
    if self.CP.carFingerprint == GM_CAR.CHEVROLET_BOLT_CC_2017:
      sr *= get_bolt_2017_steer_ratio_scale(CS.vEgo)
    elif self.CP.carFingerprint == GM_CAR.CHEVROLET_BOLT_CC_2018_2021:
      sr *= BOLT_2018_2021_STEER_RATIO_TEST_SCALE
    self.VM.update_params(x, sr)

    steer_angle_without_offset = math.radians(CS.steeringAngleDeg - lp.angleOffsetDeg)
    self.curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo, lp.roll)

    # Update Torque Params
    if self.CP.lateralTuning.which() == 'torque':
      torque_params = self.sm['liveTorqueParameters']
      force_auto_tune = getattr(self.starpilot_toggles, "force_auto_tune", False)
      use_live_params = self.sm.all_checks(['liveTorqueParameters']) and (torque_params.useParams or force_auto_tune)
      use_custom_torque_params = (
        getattr(self.starpilot_toggles, "use_custom_latAccelFactor", False) or
        getattr(self.starpilot_toggles, "use_custom_friction", False)
      )
      if use_live_params or use_custom_torque_params:
        lat_accel_factor, lat_accel_offset, friction = get_torque_control_params(self.CP, torque_params, self.starpilot_toggles, use_live_params)
        self.LaC.update_live_torque_params(lat_accel_factor, lat_accel_offset, friction)

    long_plan = self.sm['longitudinalPlan']
    model_v2 = self.sm['modelV2']

    CC = car.CarControl.new_message()
    CC.enabled = self.sm['selfdriveState'].enabled

    # Check which actuators can be enabled
    standstill = abs(CS.vEgo) <= max(self.CP.minSteerSpeed, 0.3) or CS.standstill
    CC.latActive = get_lateral_active(CC.enabled, self.sm['selfdriveState'].active,
                                      self.sm['starpilotCarState'].alwaysOnLateralEnabled,
                                      CS.steerFaultTemporary, CS.steerFaultPermanent,
                                      standstill, self.CP.steerAtStandstill,
                                      self.sm['starpilotPlan'].lateralCheck)
    # EcuDisableFailed is set when car started in READY mode (ECU disable was rejected)
    # Disable longitudinal so stock ACC works instead
    self.update_ecu_disable_failed()
    CC.longActive = CC.enabled and not any(e.overrideLongitudinal for e in self.sm['onroadEvents']) and not self.sm['starpilotCarState'].pauseLongitudinal and self.CP.openpilotLongitudinalControl and not self.ecu_disable_failed

    actuators = CC.actuators
    actuators.longControlState = self.LoC.long_control_state

    # Enable blinkers while lane changing
    if model_v2.meta.laneChangeState != LaneChangeState.off:
      CC.leftBlinker = model_v2.meta.laneChangeDirection == LaneChangeDirection.left
      CC.rightBlinker = model_v2.meta.laneChangeDirection == LaneChangeDirection.right

    if not CC.latActive:
      self.LaC.reset()
    if not CC.longActive:
      self.LoC.reset()

    # accel PID loop
    pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, CS.vCruise * CV.KPH_TO_MS)
    self.LoC.experimental_mode = bool(self.sm['selfdriveState'].experimentalMode)
    actuators.accel = float(min(self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits,
                                                self.starpilot_toggles, has_lead=long_plan.hasLead),
                                self.starpilot_toggles.max_desired_acceleration))

    # Steering PID loop and lateral MPC
    # Reset desired curvature to current to avoid violating the limits on engage
    if self.sm.valid['lateralManeuverPlan']:
      new_desired_curvature = self.sm['lateralManeuverPlan'].desiredCurvature if CC.latActive else self.curvature
    else:
      new_desired_curvature = model_v2.action.desiredCurvature if CC.latActive else self.curvature

    # Low-speed turn-intent hold (see CURVATURE_HOLD_* above). Curvature sign convention
    # here is positive for RIGHT turns (pauseturn log: left turn at +148 deg steering
    # angle logs desiredCurvature -0.07), so the blinker maps right=+1, left=-1.
    blinker_dir = float(CS.rightBlinker) - float(CS.leftBlinker)
    if CS.vEgo >= CURVATURE_HOLD_RELEASE_SPEED:
      self.turn_hold_curvature = 0.0
      self.turn_hold_standstill_t = 0.0
      self.turn_hold_swept = 0.0
      self.turn_hold_handoff_t = 0.0
      self.turn_hold_done = False
    else:
      if self.turn_hold_curvature == 0.0:
        self.turn_hold_swept = 0.0
      else:
        # heading actually swept in the hold's direction: the measure of turn progress
        self.turn_hold_swept += max(CS.vEgo * self.curvature * math.copysign(1.0, self.turn_hold_curvature), 0.0) * DT_CTRL
      turn_exiting = self.turn_hold_swept > CURVATURE_HOLD_SWEPT_EXIT
      if (CS.vEgo > CURVATURE_HOLD_HARD_SPEED or turn_exiting) and CC.latActive and self.turn_hold_curvature != 0.0:
        # Decay toward the model's sustained same-direction demand instead of leaking on
        # wall-clock time: a wall-clock leak drained the floor mid-turn while the model
        # dipped transiently (turnn rlog 40.0-40.5s), while sustained low demand (end of
        # turn, abort) still drains the hold within a couple of time constants.
        hold_dir = math.copysign(1.0, self.turn_hold_curvature)
        model_mag = max(new_desired_curvature * hold_dir, 0.0)
        if model_mag < abs(self.turn_hold_curvature):
          decay_tau = CURVATURE_HOLD_EXIT_DECAY_TAU if turn_exiting else CURVATURE_HOLD_DECAY_TAU
          decayed = abs(self.turn_hold_curvature) + (model_mag - abs(self.turn_hold_curvature)) * (DT_CTRL / decay_tau)
          self.turn_hold_curvature = math.copysign(decayed, self.turn_hold_curvature)
      if CS.vEgo < 0.5:
        self.turn_hold_standstill_t += DT_CTRL
        if self.turn_hold_standstill_t > CURVATURE_HOLD_STANDSTILL_TIMEOUT:
          self.turn_hold_curvature = 0.0
        # a stop resets the turn cycle: the model goes blind again, so a prior handoff
        # must not block the standstill pre-wind (turn4 regression in v9 replay)
        self.turn_hold_done = False
      else:
        self.turn_hold_standstill_t = 0.0
      if CC.latActive and self.turn_hold_curvature != 0.0 and \
         new_desired_curvature * math.copysign(1.0, self.turn_hold_curvature) < -CURVATURE_HOLD_OPPOSITE_RELEASE:
        # model is actively counter-steering: the turn is over, release at any speed
        self.turn_hold_curvature = 0.0
        self.turn_hold_done = True
      if CC.latActive and self.turn_hold_curvature != 0.0 and \
         new_desired_curvature * math.copysign(1.0, self.turn_hold_curvature) >= CURVATURE_HOLD_HANDOFF_FRAC * abs(self.turn_hold_curvature):
        self.turn_hold_handoff_t += DT_CTRL
        if self.turn_hold_handoff_t > CURVATURE_HOLD_HANDOFF_TIME:
          # action has sustainably taken over: hand off completely (see HANDOFF consts)
          self.turn_hold_curvature = 0.0
          self.turn_hold_done = True
      else:
        self.turn_hold_handoff_t = 0.0
      if blinker_dir == 0.0:
        # blinker cycle over: a fresh turn may engage a fresh hold
        self.turn_hold_done = False
      if blinker_dir != 0.0 and not self.turn_hold_done:
        # Ratchet up on the raw model command, never on the floored/measured value, so
        # the hold can't feed itself and defeat the decay. Below the release speed the
        # plan's spatial curvature (see get_plan_spatial_curvature) is the second,
        # earlier-seeing source: it shows the turn at standstill while the action is
        # still blind, letting the pre-wind start before the car moves.
        turn_candidate = new_desired_curvature if CC.latActive else 0.0
        if CC.latActive and CS.vEgo < CURVATURE_HOLD_PLAN_SOURCE_SPEED:
          plan_curvature = get_plan_spatial_curvature(model_v2) * CURVATURE_HOLD_PLAN_SCALE
          plan_curvature = max(min(plan_curvature, CURVATURE_HOLD_PLAN_CAP), -CURVATURE_HOLD_PLAN_CAP)
          if plan_curvature * blinker_dir > turn_candidate * blinker_dir:
            turn_candidate = plan_curvature
        if turn_candidate * blinker_dir > abs(self.turn_hold_curvature):
          new_mag = turn_candidate * blinker_dir
          if CS.vEgo > CURVATURE_HOLD_PLAN_SOURCE_SPEED:
            new_mag = min(new_mag, abs(self.turn_hold_curvature) + CURVATURE_HOLD_RATCHET_RATE * DT_CTRL)
          self.turn_hold_curvature = math.copysign(new_mag, turn_candidate)
        elif self.turn_hold_curvature * blinker_dir < 0.0:
          # blinker flipped to the other side: turn intent changed
          self.turn_hold_curvature = 0.0
      if CC.latActive and self.turn_hold_curvature != 0.0:
        hold_dir = math.copysign(1.0, self.turn_hold_curvature)
        if new_desired_curvature * hold_dir < abs(self.turn_hold_curvature):
          new_desired_curvature = self.turn_hold_curvature

    jerk_factor = 1.0
    if self.starpilot_toggles.lane_change_pace < 10:
      set_jerk = self.starpilot_toggles.lane_change_jerk_factor
      in_lane_change = model_v2.meta.laneChangeState in (LaneChangeState.laneChangeStarting, LaneChangeState.laneChangeFinishing) \
          and CS.vEgo >= self.starpilot_toggles.minimum_lane_change_speed
      # Hold the tight jerk limit for the whole maneuver, then taper back to stock so the
      # model's recenter step and mid-change corrections stay shaped instead of passing
      # through a mostly-relaxed clamp. Only the jerk (curvature rate) is tightened: capping
      # lat accel strangles the end-of-maneuver arrest and lets the car glide past the new
      # lane center before it can build enough counter-curvature.
      if in_lane_change:
        self.lc_smooth_release = LANE_CHANGE_SMOOTH_RELEASE_T
      else:
        self.lc_smooth_release = max(self.lc_smooth_release - DT_CTRL, 0.0)
      if self.lc_smooth_release > 0.0:
        release = 1.0 - self.lc_smooth_release / LANE_CHANGE_SMOOTH_RELEASE_T  # 0 in maneuver → 1 after
        jerk_factor = set_jerk + (1.0 - set_jerk) * release
        # When the model is unwinding curvature (reducing the lane-change curvature magnitude)
        # and the entry cap would make the command lag it, apply the arrest floor so the car
        # can stop on the new lane center. Applies only to the unwind direction; the entry
        # ramp keeps the full pace smoothness. Robust to double lane changes (no baseline).
        model_unwinding = abs(new_desired_curvature) < abs(self.desired_curvature) and \
            math.copysign(1.0, new_desired_curvature - self.desired_curvature) == -math.copysign(1.0, self.desired_curvature) and \
            abs(self.desired_curvature) > 1e-4
        if model_unwinding:
          arrest_floor = LANE_CHANGE_ARREST_JERK_FLOOR + (1.0 - LANE_CHANGE_ARREST_JERK_FLOOR) * release
          jerk_factor = max(jerk_factor, arrest_floor)

    self.desired_curvature, curvature_limited = clip_curvature(CS.vEgo, self.desired_curvature, new_desired_curvature, lp.roll,
                                                               jerk_factor)
    lat_delay = self.sm["liveDelay"].lateralDelay + LAT_SMOOTH_SECONDS

    actuators.curvature = self.desired_curvature
    steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
                                                       self.steer_limited_by_safety, self.desired_curvature,
                                                       curvature_limited, lat_delay,
                                                       self.calibrated_pose,
                                                       self.sm['modelV2'],
                                                       self.starpilot_toggles)
    actuators.torque = float(steer)
    actuators.steeringAngleDeg = float(steeringAngleDeg)

    if len(long_plan.speeds):
      actuators.speed = long_plan.speeds[-1]

    # Ensure no NaNs/Infs
    for p in ACTUATOR_FIELDS:
      attr = getattr(actuators, p)
      if not isinstance(attr, Number):
        continue

      if not math.isfinite(attr):
        cloudlog.error(f"actuators.{p} not finite {actuators.to_dict()}")
        setattr(actuators, p, 0.0)

    return CC, lac_log

  def publish(self, CC, lac_log):
    CS = self.sm['carState']
    long_plan = self.sm['longitudinalPlan']

    # Orientation and angle rates can be useful for carcontroller
    # Only calibrated (car) frame is relevant for the carcontroller
    CC.currentCurvature = self.curvature
    if self.calibrated_pose is not None:
      CC.orientationNED = self.calibrated_pose.orientation.xyz.tolist()
      CC.angularVelocity = self.calibrated_pose.angular_velocity.xyz.tolist()

    CC.cruiseControl.override = CC.enabled and not CC.longActive and self.CP.openpilotLongitudinalControl
    pacifica_hybrid_aol = pacifica_hybrid_aol_stock_acc_mode(
      self.CP.carFingerprint,
      self.CP.pcmCruise,
      CC.enabled,
      self.sm['starpilotCarState'].alwaysOnLateralEnabled,
    )
    cancel_requested = should_cancel_stock_cruise(self.CP, CS.cruiseState.enabled, CC.enabled)
    CC.cruiseControl.cancel = cancel_requested and not pacifica_hybrid_aol

    legacy_resume_hack = False
    if len(long_plan.speeds):
      planned_resume = long_plan.speeds[-1] > 0.1
      # Some stock-ACC SNG hacks still rely on the legacy "planner wants speed"
      # signal rather than longitudinalPlan.shouldStop going false first.
      legacy_resume_hack = planned_resume and (
        (self.CP.brand == "toyota" and self.CP.openpilotLongitudinalControl and not self.CP.autoResumeSng) or
        (self.CP.brand == "gm" and getattr(self.starpilot_toggles, "volt_sng", False))
      )

    CC.cruiseControl.resume = CC.enabled and CS.cruiseState.standstill and (
      not self.sm['longitudinalPlan'].shouldStop or legacy_resume_hack
    )

    hudControl = CC.hudControl
    hud_set_speed = float(CS.vCruiseCluster * CV.KPH_TO_MS)
    gm_dash_spoof_offsets_enabled = (
      self.CP.brand == "gm" and
      self.CP.openpilotLongitudinalControl and
      self.CP.enableGasInterceptorDEPRECATED and
      getattr(self.starpilot_toggles, "gm_pedal_longitudinal", True) and
      not getattr(self.starpilot_toggles, "disable_openpilot_long", False) and
      getattr(self.starpilot_toggles, "gm_dash_spoof_offsets", False)
    )
    if gm_dash_spoof_offsets_enabled:
      hud_set_speed = get_gm_hud_set_speed(hud_set_speed, self.starpilot_toggles)
    hudControl.setSpeed = hud_set_speed
    hudControl.speedVisible = CC.enabled
    hudControl.lanesVisible = CC.enabled
    hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead
    hudControl.leadDistanceBars = self.sm['selfdriveState'].personality.raw + 1
    hudControl.visualAlert = self.sm['selfdriveState'].alertHudVisual

    hudControl.rightLaneVisible = True
    hudControl.leftLaneVisible = True
    if self.sm.valid['driverAssistance']:
      hudControl.leftLaneDepart = self.sm['driverAssistance'].leftLaneDeparture
      hudControl.rightLaneDepart = self.sm['driverAssistance'].rightLaneDeparture

    if self.sm['selfdriveState'].active:
      CO = self.sm['carOutput']
      if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
        self.steer_limited_by_safety = abs(CC.actuators.steeringAngleDeg - CO.actuatorsOutput.steeringAngleDeg) > \
                                              STEER_ANGLE_SATURATION_THRESHOLD
      else:
        self.steer_limited_by_safety = abs(CC.actuators.torque - CO.actuatorsOutput.torque) > 1e-2

    # TODO: both controlsState and carControl valids should be set by
    #       sm.all_checks(), but this creates a circular dependency

    # controlsState
    dat = messaging.new_message('controlsState')
    dat.valid = CS.canValid
    cs = dat.controlsState

    cs.curvature = self.curvature
    cs.longitudinalPlanMonoTime = self.sm.logMonoTime['longitudinalPlan']
    cs.lateralPlanMonoTime = self.sm.logMonoTime['modelV2']
    cs.desiredCurvature = self.desired_curvature
    cs.longControlState = self.LoC.long_control_state
    cs.upAccelCmd = float(self.LoC.pid.p)
    cs.uiAccelCmd = float(self.LoC.pid.i)
    cs.ufAccelCmd = float(self.LoC.pid.f)
    cs.forceDecel = bool(self.sm['driverMonitoringState'].noResponseForceDecel or
                         (self.sm['selfdriveState'].state == State.softDisabling) or self.sm["starpilotCarState"].forceCoast)

    lat_tuning = self.CP.lateralTuning.which()
    if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
      cs.lateralControlState.angleState = lac_log
    elif lat_tuning == 'pid':
      cs.lateralControlState.pidState = lac_log
    elif lat_tuning == 'torque':
      cs.lateralControlState.torqueState = lac_log

    self.pm.send('controlsState', dat)

    # carControl
    cc_send = messaging.new_message('carControl')
    cc_send.valid = CS.canValid
    cc_send.carControl = CC
    self.pm.send('carControl', cc_send)

  def run(self):
    rk = Ratekeeper(100, print_delay_threshold=None)
    while True:
      self.update()
      CC, lac_log = self.state_control()
      self.publish(CC, lac_log)
      rk.monitor_time()


def main():
  config_realtime_process(4, Priority.CTRL_HIGH)
  controls = Controls()
  controls.run()


if __name__ == "__main__":
  main()