sunnypilot v2026.10.10-4923

version: sunnypilot v2026.003.000 (feature-branch) date: 2026-10-10T17:48:13 master commit: 20d1722968
2026-11-08 11:53:47 +08:00 · 2026-10-10 17:48:13 +00:00
122 changed files with 10142 additions and 3795 deletions
@@ -166,3 +166,5 @@ class CarControlSP:
@auto_dataclass
 class CarStateSP:
  speedLimit: float = auto_field()
+  engineOff: bool = auto_field()
+  engineRpm: float = auto_field()
@@ -11,14 +11,17 @@ from opendbc.car.toyota import toyotacan
 from opendbc.car.toyota.values import CAR, NO_STOP_TIMER_CAR, TSS2_CAR, \
                                        CarControllerParams, ToyotaFlags
 from opendbc.can import CANPacker
-
+from opendbc.sunnypilot.car.toyota.auto_brake_hold import AutoBrakeHoldCarController
+from opendbc.sunnypilot.car.toyota.enhanced_bsm import EnhancedBsmCarController
 from opendbc.sunnypilot.car.toyota.gas_interceptor import GasInterceptorCarController
 from opendbc.sunnypilot.car.toyota.values import ToyotaFlagsSP
+from opendbc.sunnypilot.car.toyota.brake_onset import BrakeOnsetShaper, BrakeCommandCorrections, EngageOnsetShaper

 Ecu = structs.CarParams.Ecu
 LongCtrlState = structs.CarControl.Actuators.LongControlState
 SteerControlType = structs.CarParams.SteerControlType
 VisualAlert = structs.CarControl.HUDControl.VisualAlert
+GearShifter = structs.CarState.GearShifter

 # The up limit allows the brakes/gas to unwind quickly leaving a stop,
 # the down limit roughly matches the rate of ACCEL_NET, reducing PCM compensation windup
@@ -36,11 +39,20 @@ MAX_STEER_RATE_FRAMES = 17  # tx control frames needed before torque can be cut
 # EPS allows user torque above threshold for 50 frames before permanently faulting
 MAX_USER_TORQUE = 500

+CRUISE_CANCEL_DELAY_FRAMES = 10

-def get_long_tune(CP, params):
+def get_long_tune(CP, CP_SP, params):
  if CP.flags & ToyotaFlags.TSS2:
-    kiBP = [2., 5.]
-    kiV = [0.5, 0.25]
+    if CP_SP.flags & ToyotaFlagsSP.TSS2_LONG_TUNING:
+      #kiBP = [0.,   2.0,  9.0,  14.,  20.,  27.]
+      #kiV =  [0.25, 0.25, 0.15, 0.12, 0.12, 0.12]
+      #kiBP= [0.,  1.0,  2.0,   3.0,   4.0,   5.0,   7.,  20.,  27.,  36.]
+      #kiV  = [0.31, 0.32, 0.301, 0.280,  0.259,  0.226, 0.15, 0.15, 0.101, 0.10]
+      kiBP= [0.,   0.3,   5.0,   12.,  27.,  36.]
+      kiV  = [0.50, 0.52, 0.25,  0.23, 0.10, 0.09]
+    else:
+      kiBP = [2., 5.]
+      kiV = [0.5, 0.25]
  else:
    kiBP = [0., 5., 35.]
    kiV = [3.6, 2.4, 1.5]
@@ -63,15 +75,21 @@ class CarController(CarControllerBase, GasInterceptorCarController):
    self.permit_braking = True
    self.steer_rate_counter = 0
    self.distance_button = 0
+    self.cancel_counter = 0

    # *** start long control state ***
-    self.long_pid = get_long_tune(self.CP, self.params)
-    self.aego = FirstOrderFilter(0.0, 0.25, DT_CTRL * 3)
+    self.long_pid = get_long_tune(self.CP, self.CP_SP, self.params)
+    self.aego = FirstOrderFilter(0.0, 0.45, DT_CTRL * 3)
    self.pitch = FirstOrderFilter(0, 0.5, DT_CTRL)
    self.pitch_hp = HighPassFilter(0.0, 0.25, 1.5, DT_CTRL)

    self.accel = 0
    self.prev_accel = 0
+    # sunnypilot: ToyotaBrakeOnset gates the whole brake_onset module (onset + engage onset + command corrections)
+    self.brake_onset_enabled = bool(CP_SP.flags & ToyotaFlagsSP.SP_BRAKE_ONSET.value)
+    self.brake_onset = BrakeOnsetShaper(DT_CTRL * 3, -ACCEL_WINDDOWN_LIMIT / (DT_CTRL * 3))
+    self.engage_onset = EngageOnsetShaper(DT_CTRL * 3, ACCEL_WINDUP_LIMIT / (DT_CTRL * 3))
+    self.brake_corrections = BrakeCommandCorrections(DT_CTRL * 3)  # sunnypilot: see brake_onset.py
    # *** end long control state ***

    self.packer = CANPacker(dbc_names[Bus.pt])
@@ -81,11 +99,20 @@ class CarController(CarControllerBase, GasInterceptorCarController):
    self.secoc_acc_message_counter = 0
    self.secoc_prev_reset_counter = 0

+    self.enhanced_bsm = EnhancedBsmCarController(CP, CP_SP)
+    self.auto_brake_hold = AutoBrakeHoldCarController(CP, CP_SP)
+
+    self._auto_lock_speed = 0.0
+
+    self._auto_lock_once = False
+    self._gear_prev = GearShifter.park
+
  def update(self, CC, CC_SP, CS, now_nanos):
    actuators = CC.actuators
    stopping = actuators.longControlState == LongCtrlState.stopping
    hud_control = CC.hudControl
-    pcm_cancel_cmd = CC.cruiseControl.cancel
+    self.cancel_counter = self.cancel_counter + 1 if CC.cruiseControl.cancel else 0
+    pcm_cancel_cmd = self.cancel_counter > CRUISE_CANCEL_DELAY_FRAMES
    lat_active = CC.latActive and abs(CS.out.steeringTorque) < MAX_USER_TORQUE

    if len(CC.orientationNED) == 3:
@@ -196,6 +223,9 @@ class CarController(CarControllerBase, GasInterceptorCarController):

    self.last_standstill = CS.out.standstill

+    if self.auto_brake_hold.enabled:
+      can_sends.extend(self.auto_brake_hold.update(CS, self.frame, self.packer))
+
    # handle UI messages
    fcw_alert = hud_control.visualAlert == VisualAlert.fcw
    steer_alert = hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw)
@@ -213,8 +243,22 @@ class CarController(CarControllerBase, GasInterceptorCarController):

        # internal PCM gas command can get stuck unwinding from negative accel so we apply a generous rate limit
        pcm_accel_cmd = actuators.accel
-        if CC.longActive:
-          pcm_accel_cmd = rate_limit(pcm_accel_cmd, self.prev_accel, ACCEL_WINDDOWN_LIMIT, ACCEL_WINDUP_LIMIT)
+        if self.brake_onset_enabled:
+          self.engage_onset.cruise_state(CS.out.cruiseState.enabled)  # sunnypilot hook
+        if CC.longActive and self.brake_onset_enabled:
+          self.prev_accel = self.engage_onset.start_accel(self.prev_accel, CS.out.aEgo, CS.out.vEgo)  # sunnypilot hook
+          # a hard request bypasses the soft brake onset, except in the first 0.6 s after engaging (FCW always does)
+          urgent = self.brake_onset.is_urgent(pcm_accel_cmd, False) and not self.engage_onset.in_engage_window
+          winddown_step = self.brake_onset.down_step(pcm_accel_cmd, self.prev_accel, bypass=fcw_alert, v_ego=CS.out.vEgo,
+                                                     urgent=urgent, t_engaged=self.engage_onset.t_since_engage,
+                                                     a_ego=CS.out.aEgo, brake_force=CS.brake_force)
+          windup_step = self.engage_onset.up_step(True)
+          pcm_accel_cmd = rate_limit(pcm_accel_cmd, self.prev_accel, winddown_step, windup_step)
+        elif CC.longActive:
+          pcm_accel_cmd = rate_limit(pcm_accel_cmd, self.prev_accel, ACCEL_WINDDOWN_LIMIT, ACCEL_WINDUP_LIMIT)  # sunnypilot: stock path
+        else:
+          self.brake_onset.reset()
+          self.engage_onset.reset()
        self.prev_accel = pcm_accel_cmd

        # calculate amount of acceleration PCM should apply to reach target, given pitch.
@@ -250,12 +294,19 @@ class CarController(CarControllerBase, GasInterceptorCarController):
                                               -MAX_PITCH_COMPENSATION, MAX_PITCH_COMPENSATION))
            pcm_accel_cmd += pitch_compensation

+          # sunnypilot: bleed and freeze the PID integrator at the end of the stop
+          freeze_i = self.brake_onset_enabled and self.brake_corrections.condition_pid(self.long_pid, self.prev_accel, CS.out.vEgo)
          pcm_accel_cmd = self.long_pid.update(error_future,
                                               speed=CS.out.vEgo,
                                               feedforward=pcm_accel_cmd,
-                                               freeze_integrator=actuators.longControlState != LongCtrlState.pid)
+                                               freeze_integrator=actuators.longControlState != LongCtrlState.pid or freeze_i)
+          if self.brake_onset_enabled:
+            pcm_accel_cmd = self.brake_corrections.apply(pcm_accel_cmd, self.prev_accel, a_ego_future, CS.out.vEgo, stopping,
+                                                        fcw_alert, a_ego=a_ego_blended,
+                                                        relaxed=hud_control.leadDistanceBars == 3)  # sunnypilot hook
        else:
          self.long_pid.reset()
+          self.brake_corrections.reset()  # sunnypilot hook

        # Along with rate limiting positive jerk above, this greatly improves gas response time
        # Consider the net acceleration request that the PCM should be applying (pitch included)
@@ -319,6 +370,9 @@ class CarController(CarControllerBase, GasInterceptorCarController):
    if self.frame % 20 == 0 and self.CP.flags & ToyotaFlags.DISABLE_RADAR.value:
      can_sends.append(make_tester_present_msg(0x750, 0, 0xF))

+    if self.enhanced_bsm.enabled:
+      can_sends.extend(self.enhanced_bsm.update(CS, self.frame))
+
    new_actuators = actuators.as_builder()
    new_actuators.torque = apply_torque / self.params.STEER_MAX
    new_actuators.torqueOutputCan = apply_torque
@@ -1,6 +1,9 @@
 import copy
+from enum import IntEnum
+import importlib

 from opendbc.can import CANDefine, CANParser
+from opendbc.can.dbc import DBC as DBCParser
 from opendbc.car import Bus, DT_CTRL, create_button_events, structs
 from opendbc.car.common.conversions import Conversions as CV
 from opendbc.car.common.filter_simple import FirstOrderFilter
@@ -8,11 +11,34 @@ from opendbc.car.interfaces import CarStateBase
 from opendbc.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, \
                                                  TSS2_CAR, EPS_SCALE
 from opendbc.sunnypilot.car.toyota.carstate_ext import CarStateExt
+from opendbc.sunnypilot.car.toyota.enhanced_bsm import EnhancedBsmCarState
 from opendbc.sunnypilot.car.toyota.values import ToyotaFlagsSP

 ButtonType = structs.CarState.ButtonEvent.Type
 SteerControlType = structs.CarParams.SteerControlType

+
+class AccelPersonality(IntEnum):
+  eco = 0
+  normal = 1
+  sport = 2
+
+
+def get_accel_personality(sport_mode: int, eco_mode: int) -> AccelPersonality:
+  if sport_mode == 1:
+    return AccelPersonality.sport
+  if eco_mode == 1:
+    return AccelPersonality.eco
+  return AccelPersonality.normal
+
+
+def get_host_params():
+  """Return sunnypilot's Params store when opendbc is embedded in openpilot."""
+  try:
+    return importlib.import_module("openpilot.common.params").Params()
+  except ModuleNotFoundError:
+    return None
+
 # These steering fault definitions seem to be common across LKA (torque) and LTA (angle):
 # - high steer rate fault: goes to 21 or 25 for 1 frame, then 9 for 2 seconds
 # - lka/lta msg drop out: goes to 9 then 11 for a combined total of 2 seconds, then 3.
@@ -55,6 +81,21 @@ class CarState(CarStateBase, CarStateExt):
    self.gvc = 0.0
    self.secoc_synchronization = None

+    self.enhanced_bsm = EnhancedBsmCarState(CP, CP_SP)
+
+    if CP_SP.flags & ToyotaFlagsSP.SP_AUTO_BRAKE_HOLD:
+      self.pre_collision_2 = {}
+    self.brake_force = float('nan')
+    self._host_params = get_host_params()
+    self.toyota_drive_mode = self._host_params is not None and self._host_params.get_bool('ToyotaDriveMode')
+    self._drive_mode_signals_checked = False
+    self._sport_signal_available = False
+    self._eco_signal_available = False
+    self._prev_accel_profile = None
+    self._accel_profile_init = False
+
+    self.frame = 0
+
  def update(self, can_parsers) -> tuple[structs.CarState, structs.CarStateSP]:
    cp = can_parsers[Bus.pt]
    cp_cam = can_parsers[Bus.cam]
@@ -72,18 +113,47 @@ class CarState(CarStateBase, CarStateExt):
    ret.parkingBrake = cp.vl["BODY_CONTROL_STATE"]["PARKING_BRAKE"] == 1

    ret.brakePressed = cp.vl["BRAKE_MODULE"]["BRAKE_PRESSED"] != 0
-    ret.brakeHoldActive = cp.vl["ESP_CONTROL"]["BRAKE_HOLD_ACTIVE"] == 1
+    #ret.brakeHoldActive = cp.vl["ESP_CONTROL"]["BRAKE_HOLD_ACTIVE"] == 1

    if self.CP.flags & ToyotaFlags.SECOC.value:
      self.secoc_synchronization = copy.copy(cp.vl["SECOC_SYNCHRONIZATION"])
      ret.gasPressed = cp.vl["GAS_PEDAL"]["GAS_PEDAL_USER"] > 0
      can_gear = int(cp.vl["GEAR_PACKET_HYBRID"]["GEAR"])
    else:
-      ret.gasPressed = cp.vl["PCM_CRUISE"]["GAS_RELEASED"] == 0
+      ret.gasPressed = cp.vl["PCM_CRUISE"]["GAS_RELEASED"] == 0  # TODO: these also have GAS_PEDAL, come back and unify
      can_gear = int(cp.vl["GEAR_PACKET"]["GEAR"])
      if not self.CP.flags & ToyotaFlags.DISABLE_RADAR.value:
        ret.stockAeb = bool(cp_acc.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"] and cp_acc.vl["PRE_COLLISION"]["FORCE"] < -1e-5)

+    if self.toyota_drive_mode: # and not self.CP.flags & ToyotaFlags.SECOC.value:
+      sport_signal = 'SPORT_ON_2' if self.CP.carFingerprint in (CAR.TOYOTA_RAV4_TSS2, CAR.LEXUS_ES_TSS2,
+                                                                CAR.TOYOTA_HIGHLANDER_TSS2) else 'SPORT_ON'
+
+      if not self._drive_mode_signals_checked:
+        self._drive_mode_signals_checked = True
+        try:
+          sport_mode = cp.vl["GEAR_PACKET"][sport_signal]
+          self._sport_signal_available = True
+        except KeyError:
+          sport_mode = 0
+          self._sport_signal_available = False
+        try:
+          eco_mode = cp.vl["GEAR_PACKET"]['ECON_ON']
+          self._eco_signal_available = True
+        except KeyError:
+          eco_mode = 0
+          self._eco_signal_available = False
+      else:
+        sport_mode = cp.vl["GEAR_PACKET"][sport_signal] if self._sport_signal_available else 0
+        eco_mode = cp.vl["GEAR_PACKET"]['ECON_ON'] if self._eco_signal_available else 0
+
+      accel_profile = get_accel_personality(sport_mode, eco_mode)
+
+      if not self._accel_profile_init or accel_profile != self._prev_accel_profile:
+        self._host_params.put('AccelPersonality', int(accel_profile))
+        self._accel_profile_init = True
+        self._prev_accel_profile = accel_profile
+
    self.parse_wheel_speeds(ret,
      cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FL"],
      cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FR"],
@@ -212,6 +282,16 @@ class CarState(CarStateBase, CarStateExt):

    ret.buttonEvents = buttonEvents

+    if self.enhanced_bsm.enabled and self.frame > 199:
+      ret.leftBlindspot, ret.rightBlindspot = self.enhanced_bsm.update(cp, self.frame)
+
+    if self.CP_SP.flags & ToyotaFlagsSP.SP_AUTO_BRAKE_HOLD:
+      self.pre_collision_2 = copy.copy(cp_cam.vl["PRE_COLLISION_2"])
+      if "BRAKE" in cp.vl:
+        self.brake_force = float(cp.vl["BRAKE"]["BRAKE_FORCE"])
+
+    self.frame += 1
+
    CarStateExt.update(self, ret, ret_sp, can_parsers)

    return ret, ret_sp
@@ -221,6 +301,11 @@ class CarState(CarStateBase, CarStateExt):
    pt_messages = [
      ("BLINKERS_STATE", float('nan')),
    ]
+    if CP.flags & ToyotaFlags.HYBRID:
+      pt_messages.append(("ENGINE_RPM", float('nan')))
+
+    if CP_SP.flags & ToyotaFlagsSP.SP_AUTO_BRAKE_HOLD and "BRAKE" in DBCParser(DBC[CP.carFingerprint][Bus.pt]).name_to_msg:
+      pt_messages.append(("BRAKE", float('nan')))

    cam_messages = [
      ("RSA1", 0),
@@ -3,7 +3,7 @@ from opendbc.car.toyota.carstate import CarState
 from opendbc.car.toyota.carcontroller import CarController
 from opendbc.car.toyota.radar_interface import RadarInterface
 from opendbc.car.toyota.values import Ecu, CAR, DBC, ToyotaFlags, CarControllerParams, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, \
-                                                  MIN_ACC_SPEED, EPS_SCALE, NO_STOP_TIMER_CAR, ToyotaSafetyFlags, UNSUPPORTED_DSU_CAR
+                                                  MIN_ACC_SPEED, EPS_SCALE, NO_STOP_TIMER_CAR, ToyotaSafetyFlags, UNSUPPORTED_DSU_CAR, SECOC_CAR
 from opendbc.car.disable_ecu import disable_ecu
 from opendbc.car.interfaces import CarInterfaceBase
 from opendbc.sunnypilot.car.toyota.values import ToyotaFlagsSP, ToyotaSafetyFlagsSP
@@ -117,6 +117,7 @@ class CarInterface(CarInterfaceBase):

    if ret.flags & ToyotaFlags.TSS2:
      ret.flags |= ToyotaFlags.RAISED_ACCEL_LIMIT.value
+      #ret.stopAccel = -0.02

      # Hybrids have much quicker longitudinal actuator response
      if ret.flags & ToyotaFlags.HYBRID.value:
@@ -130,6 +131,9 @@ class CarInterface(CarInterfaceBase):
    if candidate in UNSUPPORTED_DSU_CAR:
      ret.safetyParam |= ToyotaSafetyFlagsSP.UNSUPPORTED_DSU

+    if candidate == CAR.TOYOTA_PRIUS_TSS2:
+      ret.flags |= ToyotaFlagsSP.SP_NEED_DEBUG_BSM.value
+
    # Detect smartDSU, which intercepts ACC_CMD from the DSU (or radar) allowing openpilot to send it
    # 0x2AA is sent by a similar device which intercepts the radar instead of DSU on NO_DSU_CARs
    if 0x2FF in fingerprint[0] or (0x2AA in fingerprint[0] and candidate in NO_DSU_CAR):
@@ -1,4 +1,5 @@
 from opendbc.car.structs import CarParams
+from opendbc.car.can_definitions import CanData

 SteerControlType = CarParams.SteerControlType

@@ -164,3 +165,47 @@ def toyota_checksum(address: int, sig, d: bytearray) -> int:
  for i in range(len(d) - 1):
    s += d[i]
  return s & 0xFF
+
+def create_set_bsm_debug_mode(lr_blindspot, enabled):
+  dat = b"\x02\x10\x60\x00\x00\x00\x00" if enabled else b"\x02\x10\x01\x00\x00\x00\x00"
+  dat = lr_blindspot + dat
+
+  return CanData(0x750, dat, 0)
+
+
+def create_bsm_polling_status(lr_blindspot):
+  return CanData(0x750, lr_blindspot + b"\x02\x21\x69\x00\x00\x00\x00", 0)
+
+
+# auto brake hold
+def create_brake_hold_command(packer, frame, pre_collision_2, brake_hold_active):
+  # forward PRE_COLLISION_2 when auto brake hold is not active
+  values = {s: pre_collision_2[s] for s in [
+    "DSS1GDRV",
+    "DS1STAT2",
+    "DS1STBK2",
+    "PCSWAR",
+    "PCSALM",
+    "PCSOPR",
+    "PCSABK",
+    "PBATRGR",
+    "PPTRGR",
+    "IBTRGR",
+    "CLEXTRGR",
+    "IRLT_REQ",
+    "BRKHLD",
+    "AVSTRGR",
+    "VGRSTRGR",
+    "PREFILL",
+    "PBRTRGR",
+    "PCSDIS",
+    "PBPREPMP",
+  ]}
+
+  if brake_hold_active:
+    values = {
+      "DSS1GDRV": 0x3FF,
+      "PBRTRGR": frame % 730 < 727,  # cut actuation for 3 frames
+    }
+
+  return packer.make_can_msg("PRE_COLLISION_2", 0, values)
@@ -0,0 +1,79 @@
+BO_ 1880 DEBUG: 8 XXX
+ SG_ BLINDSPOTSIDE : 7|8@0+ (1,0) [0|255] "" XXX
+ SG_ BLINDSPOT : 38|1@0+ (1,0) [0|15] "" XXX
+ SG_ BLINDSPOTD1 : 47|8@0+ (1,0) [0|255] "" XXX
+ SG_ BLINDSPOTD2 : 55|8@0+ (1,0) [0|255] "" XXX
+
+BO_ 836 PRE_COLLISION_2: 8 DSU
+ SG_ DSS1GDRV : 7|10@0- (0.1,0) [0|0] "m/s^2" Vector__XXX
+ SG_ DS1STAT2 : 13|3@0+ (1,0) [0|0] "" Vector__XXX
+ SG_ DS1STBK2 : 10|3@0+ (1,0) [0|0] "" Vector__XXX
+ SG_ PCSWAR : 18|1@0+ (1,0) [0|0] "" FCM
+ SG_ PCSALM : 17|1@0+ (1,0) [0|0] "" FCM
+ SG_ PCSOPR : 16|1@0+ (1,0) [0|0] "" Vector__XXX
+ SG_ PCSABK : 31|1@0+ (1,0) [0|0] "" Vector__XXX
+ SG_ PBATRGR : 30|2@0+ (1,0) [0|0] "" Vector__XXX
+ SG_ PPTRGR : 28|1@0+ (1,0) [0|0] "" FCM
+ SG_ IBTRGR : 27|1@0+ (1,0) [0|0] "" FCM
+ SG_ CLEXTRGR : 26|1@0+ (1,0) [0|0] "" Vector__XXX
+ SG_ IRLT_REQ : 25|2@0+ (1,0) [0|0] "" Vector__XXX
+ SG_ BRKHLD : 37|1@0+ (1,0) [0|0] "" Vector__XXX
+ SG_ AVSTRGR : 36|1@0+ (1,0) [0|0] "" SCS
+ SG_ VGRSTRGR : 35|2@0+ (1,0) [0|0] "" Vector__XXX
+ SG_ PREFILL : 33|1@0+ (1,0) [0|0] "" Vector__XXX
+ SG_ PBRTRGR : 32|1@0+ (1,0) [0|0] "" Vector__XXX
+ SG_ PCSDIS : 43|1@0+ (1,0) [0|0] "" Vector__XXX
+ SG_ PBPREPMP : 40|1@0+ (1,0) [0|0] "" Vector__XXX
+ SG_ CHECKSUM : 63|8@0+ (1,0) [0|0] "" XXX
+
+BO_ 713 HYBRID_POWERTRAIN: 8 XXX
+ SG_ COAST_FUEL_CUT : 5|1@0+ (1,0) [0|1] "" XXX
+ SG_ HYBRID_DRIVE_STATE : 11|4@0+ (1,0) [0|15] "" XXX
+
+BO_ 814 BRAKE_HYDRAULIC: 8 XXX
+ SG_ BRAKE_PRESSURE : 15|8@0+ (1,0) [0|255] "" XXX
+ SG_ BRAKE_PRESSURE_COPY1 : 31|8@0+ (1,0) [0|255] "" XXX
+ SG_ BRAKE_PRESSURE_COPY2 : 47|8@0+ (1,0) [0|255] "" XXX
+
+BO_ 971 MOTOR_SPEED_CLUSTER: 7 XXX
+ SG_ CLUSTER_SPEED : 55|8@0+ (1,-21) [0|255] "km/h" XXX
+
+BO_ 896 HYBRID_BATTERY: 8 XXX
+ SG_ HV_SOC_PCT : 55|8@0+ (1,0) [0|100] "%" XXX
+
+BO_ 1654 HV_POWER_INTEGRATOR: 8 XXX
+ SG_ HV_POWER_ACCUM : 39|8@0+ (1,0) [0|255] "" XXX
+
+BO_ 975 HV_INVERTER: 5 XXX
+ SG_ HV_VOLTAGE_LO : 23|8@0+ (1,0) [0|255] "" XXX
+ SG_ HV_CURRENT_LO : 39|8@0+ (1,0) [0|255] "" XXX
+
+BO_ 955 BRAKE_REDUNDANT: 8 XXX
+ SG_ BRAKE_PRESSED_REDUNDANT : 0|1@0+ (1,0) [0|1] "" XXX
+
+BO_ 918 VEHICLE_DRIVE_MODE: 8 XXX
+ SG_ DRIVE_MODE_STATE : 7|8@0+ (1,0) [0|255] "" XXX
+
+CM_ "Sunnypilot Prius TSS2 reverse-engineered signals — route 550a71ee4c7a7fbe/0000038f--6b48497966, 2026-05-19. Confidence: high unless noted.";
+CM_ BO_ 713 "Hybrid powertrain status. 100Hz on bus 0.";
+CM_ SG_ 713 COAST_FUEL_CUT "byte0 bit5. 1 when accelerator lifted at speed -> ICE fuel cut / decel-fuel-cut active. P(=1|coast)=0.95, P(=1|accel)=0.008, P(=1|brake)=0.0. Use: lift detection for accel feedforward, EV-only mode hint.";
+CM_ SG_ 713 HYBRID_DRIVE_STATE "byte1 lower-nibble (bits 8-11). Enum (validated): 4=brake/regen-active, 5-8=coast/cruise levels, 10/12=ICE running active, 11=idle-stop. Not a scalar magnitude. Use: distinguish ICE-on vs EV for accel command shaping.";
+CM_ BO_ 814 "Brake hydraulic. 5Hz on bus 0. Bytes 0,2,4,6 are status/flag bytes (not yet decoded). Saturates 255 under hard brake.";
+CM_ SG_ 814 BRAKE_PRESSURE "byte1. Master cylinder pressure proxy. 0 during coast, 33-45 mean during brake, 254-255 max. NOT same as BRAKE_MODULE.BRAKE_PRESSURE.";
+CM_ SG_ 814 BRAKE_PRESSURE_COPY1 "byte3. Identical to BRAKE_PRESSURE 90% of brake samples. Redundant copy (CRC/safety).";
+CM_ SG_ 814 BRAKE_PRESSURE_COPY2 "byte5. Identical to BRAKE_PRESSURE 90% of brake samples. Redundant copy.";
+CM_ BO_ 971 "Cluster speedometer feed. 10Hz on bus 0.";
+CM_ SG_ 971 CLUSTER_SPEED "byte6. Encoded as raw_byte = v_kph + 21. After (1,-21) factor/offset: km/h matching dashboard speedometer. Corr 0.998 w/ WHEEL_SPEEDS. ~5km/h higher than wheel mean = cluster display bias.";
+CM_ BO_ 896 "HV battery status. 5Hz on bus 0.";
+CM_ SG_ 896 HV_SOC_PCT "byte6. HV battery state-of-charge percent. Validated range 53-62% on route 0000038f (Prius hybrid SoC normal band 40-80%). Use: energy-aware planner — reduce regen demand at high SoC (>70%), lift accel demand at low SoC (<50%).";
+CM_ BO_ 1654 "HV power integrator. 1Hz on bus 0. Slow drift signal, semantic not fully confirmed.";
+CM_ SG_ 1654 HV_POWER_ACCUM "byte4. Slow-drifting integer 0-59, independent of HV_SOC_PCT. Hypothesis: HV power flow accumulator or charging-cycle counter. Needs more routes.";
+CM_ BO_ 975 "HV inverter telemetry. 10Hz on bus 0. 5-byte message.";
+CM_ SG_ 975 HV_VOLTAGE_LO "byte2. Sequential integer 40-53 range. Hypothesis: HV bus voltage low-byte scaled. Moderate corr w/ brake (regen-loaded voltage).";
+CM_ SG_ 975 HV_CURRENT_LO "byte4. Sequential integer 17-36 range. JUMPS -16 at brake-release (regen current cutoff signature). Use: real-time regen current readback for brake-blend tuning.";
+CM_ BO_ 955 "Brake redundant signal. 10Hz on bus 0.";
+CM_ SG_ 955 BRAKE_PRESSED_REDUNDANT "byte0 bit0. 99.9% agreement with BRAKE_MODULE.BRAKE_PRESSED. Safety-redundant copy. Use: cross-check brake state for fault detection.";
+CM_ BO_ 918 "Vehicle drive mode state. 5Hz on bus 0.";
+CM_ SG_ 918 DRIVE_MODE_STATE "byte0. 3 states observed: 185=parked-with-brake, 189=ready-to-drive, 191=ready-no-brake. Transitions on GEAR shifts + brake. Use: more reliable drive-state machine than gear+brake combo.";
+VAL_ 713 HYBRID_DRIVE_STATE 4 "brake_regen_active" 5 "coast_5" 6 "coast_6" 7 "coast_7" 8 "coast_cruise" 10 "ice_active_10" 11 "idle_stop" 12 "ice_active_12";
+VAL_ 918 DRIVE_MODE_STATE 185 "parked_brake" 189 "ready" 191 "ready_no_brake";
@@ -234,6 +234,7 @@ BO_ 956 GEAR_PACKET: 8 XXX
 SG_ SPORT_GEAR_ON : 33|1@0+ (1,0) [0|1] "" XXX
 SG_ SPORT_GEAR : 38|3@0+ (1,0) [0|7] "" XXX
 SG_ ECON_ON : 40|1@0+ (1,0) [0|1] "" XXX
+ SG_ SPORT_ON_2 : 55|1@0+ (1,0) [0|1] "" XXX
 SG_ B_GEAR_ENGAGED : 41|1@0+ (1,0) [0|1] "" XXX
 SG_ DRIVE_ENGAGED : 47|1@0+ (1,0) [0|1] "" XXX

@@ -545,6 +546,7 @@ VAL_ 956 GEAR 0 "D" 1 "S" 8 "N" 16 "R" 32 "P";
 VAL_ 956 SPORT_GEAR_ON 0 "off" 1 "on";
 VAL_ 956 SPORT_GEAR 1 "S1" 2 "S2" 3 "S3" 4 "S4" 5 "S5" 6 "S6";
 VAL_ 956 ECON_ON 0 "off" 1 "on";
+VAL_ 956 SPORT_ON_2 0 "off" 1 "on";
 VAL_ 956 B_GEAR_ENGAGED 0 "off" 1 "on";
 VAL_ 956 DRIVE_ENGAGED 0 "off" 1 "on";
 VAL_ 1005 REVERSE_CAMERA_GUIDELINES 3 "No guidelines" 2 "Static guidelines" 1 "Active guidelines";
@@ -35,14 +35,6 @@ BO_ 740 STEERING_LKA: 5 XXX
 SG_ STEER_TORQUE_CMD : 15|16@0- (1,0) [0|65535] "" XXX
 SG_ CHECKSUM : 39|8@0+ (1,0) [0|255] "" XXX

-BO_ 836 PRE_COLLISION_2: 8 DSU
- SG_ DSS1GDRV : 7|10@0- (0.1,0) [0|0] "m/s^2" Vector__XXX
- SG_ PCSALM : 17|1@0+ (1,0) [0|0] "" FCM
- SG_ IBTRGR : 27|1@0+ (1,0) [0|0] "" FCM
- SG_ PBATRGR : 30|2@0+ (1,0) [0|0] "" Vector__XXX
- SG_ PREFILL : 33|1@0+ (1,0) [0|0] "" Vector__XXX
- SG_ AVSTRGR : 36|1@0+ (1,0) [0|0] "" SCS
- SG_ CHECKSUM : 63|8@0+ (1,0) [0|0] "" XXX

 CM_ SG_ 466 NEUTRAL_FORCE "force in newtons the engine/electric motors are applying without any acceleration commands or user input";
 CM_ SG_ 466 ACC_BRAKING "whether brakes are being actuated from ACC command";
@@ -1,6 +1,7 @@
 CM_ "IMPORT _community.dbc";
 CM_ "IMPORT _toyota_2017.dbc";
 CM_ "IMPORT _toyota_adas_standard.dbc";
+CM_ "IMPORT _sp_debug_toyota.dbc";

 BO_ 548 BRAKE_MODULE: 8 XXX
 SG_ BRAKE_PRESSURE : 43|12@0+ (1,0) [0|4047] "" XXX
@@ -1,6 +1,7 @@
 CM_ "IMPORT _community.dbc";
 CM_ "IMPORT _toyota_2017.dbc";
 CM_ "IMPORT _toyota_adas_standard.dbc";
+CM_ "IMPORT _sp_debug_toyota.dbc";

 BO_ 401 STEERING_LTA: 8 XXX
 SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
@@ -1,6 +1,7 @@
 CM_ "IMPORT _community.dbc";
 CM_ "IMPORT _toyota_2017.dbc";
 CM_ "IMPORT _toyota_adas_standard.dbc";
+CM_ "IMPORT _sp_debug_toyota.dbc";

 BO_ 550 BRAKE_MODULE: 8 XXX
 SG_ BRAKE_PRESSURE : 0|9@0+ (1,0) [0|511] "" XXX
@@ -4,7 +4,7 @@

 // Stock longitudinal
 #define TOYOTA_BASE_TX_MSGS \
-  {0x191, 0, 8, .check_relay = true}, {0x412, 0, 8, .check_relay = true}, {0x1D2, 0, 8, .check_relay = false},  /* LKAS + LTA + PCM cancel cmd */  \
+  {0x191, 0, 8, .check_relay = true}, {0x412, 0, 8, .check_relay = true}, {0x1D2, 0, 8, .check_relay = false}, {0x750, 0, 8, .check_relay = false}, /* LKAS + LTA + PCM cancel cmd */  \

 #define TOYOTA_COMMON_TX_MSGS \
  TOYOTA_BASE_TX_MSGS \
@@ -69,6 +69,7 @@ static bool toyota_secoc = false;
 static bool toyota_alt_brake = false;
 static bool toyota_stock_longitudinal = false;
 static bool toyota_lta = false;
+static bool toyota_cruise_engaged = false;  // SP: PCM_CRUISE.CRUISE_ACTIVE, narrows the auto brake hold AEB window below
 static int toyota_dbc_eps_torque_factor = 100;   // conversion factor for STEER_TORQUE_EPS in %: see dbc file

 static uint32_t toyota_compute_checksum(const CANPacket_t *msg) {
@@ -151,6 +152,7 @@ static void toyota_rx_hook(const CANPacket_t *msg) {
      if (msg->addr == 0x176U) {
        bool cruise_engaged = GET_BIT(msg, 5U);  // PCM_CRUISE.CRUISE_ACTIVE
        pcm_cruise_check(cruise_engaged);
+        toyota_cruise_engaged = cruise_engaged;
      }
      if (msg->addr == 0x116U) {
        gas_pressed = msg->data[1] != 0U;  // GAS_PEDAL.GAS_PEDAL_USER
@@ -162,6 +164,7 @@ static void toyota_rx_hook(const CANPacket_t *msg) {
      if (msg->addr == 0x1D2U) {
        bool cruise_engaged = GET_BIT(msg, 5U);  // PCM_CRUISE.CRUISE_ACTIVE
        pcm_cruise_check(cruise_engaged);
+        toyota_cruise_engaged = cruise_engaged;

        if (!enable_gas_interceptor) {
          gas_pressed = !GET_BIT(msg, 4U);  // PCM_CRUISE.GAS_RELEASED
@@ -386,13 +389,35 @@ static bool toyota_tx_hook(const CANPacket_t *msg) {
        tx = false;
      }
    }
+
+    // SP: auto brake hold https://github.com/AlexandreSato
+    if ((msg->addr == 0x344U) && (alternative_experience & ALT_EXP_ALLOW_AEB)) {
+      if (vehicle_moving || gas_pressed || !acc_main_on || toyota_cruise_engaged) {
+        tx = false;
+      }
+    }
  }

  // UDS: Only tester present ("\x0F\x02\x3E\x00\x00\x00\x00\x00") allowed on diagnostics address
  if (msg->addr == 0x750U) {
    // this address is sub-addressed. only allow tester present to radar (0xF)
    bool invalid_uds_msg = (GET_BYTES(msg, 0, 4) != 0x003E020FU) || (GET_BYTES(msg, 4, 4) != 0x0U);
-    if (invalid_uds_msg) {
+    // SP: Secret sauce from dp. (ask @rav4kumar prior to modifying)
+    // Enhanced BSM
+    bool sp_valid_uds_msgs = ((GET_BYTES(msg, 0, 4) == 0x01100241U) ||  // disable left BSM debug
+                              (GET_BYTES(msg, 0, 4) == 0x60100241U) ||  // enable left BSM debug
+                              (GET_BYTES(msg, 0, 4) == 0x69210241U) ||  // poll left BSM status
+                              (GET_BYTES(msg, 0, 4) == 0x01100242U) ||  // disable right BSM debug
+                              (GET_BYTES(msg, 0, 4) == 0x60100242U) ||  // enable right BSM debug
+                              (GET_BYTES(msg, 0, 4) == 0x69210242U))    // poll right BSM status
+                              && (GET_BYTES(msg, 4, 4) == 0x0U);
+
+    sp_valid_uds_msgs |= (GET_BYTES(msg, 0, 4) == 0x11300540U) &&       // automatic door locking and unlocking
+                         ((GET_BYTES(msg, 4, 4) == 0x00004000U) ||      // unlock
+                          (GET_BYTES(msg, 4, 4) == 0x00008000U));       // lock
+
+    bool valid_tester_present = !invalid_uds_msg && !toyota_stock_longitudinal && !toyota_secoc;
+    if (!valid_tester_present && !sp_valid_uds_msgs) {
      tx = false;
    }
  }
@@ -566,10 +591,27 @@ static safety_config toyota_init(uint16_t param) {
  return ret;
 }

+static bool toyota_fwd_hook(int bus_num, int addr) {
+  bool block_msg = false;
+  if (bus_num == 2) {
+    // SP: block AEB when auto brake hold is active, unblock AEB when auto brake hold is not active.
+    // Narrowed to match auto brake hold's own precondition (cruise must be off) - previously this
+    // blocked native AEB forwarding, forcing a slower software relay, any time the car was simply
+    // stopped with the gas released and ACC main on, even while cruise was actively engaged and
+    // auto brake hold couldn't be active at all.
+    bool is_aeb_msg = (addr == 0x344);
+    block_msg = (is_aeb_msg && (alternative_experience & ALT_EXP_ALLOW_AEB) && !vehicle_moving && !gas_pressed && acc_main_on &&
+                 !toyota_cruise_engaged);
+  }
+
+  return block_msg;
+}
+
 const safety_hooks toyota_hooks = {
  .init = toyota_init,
  .rx = toyota_rx_hook,
  .tx = toyota_tx_hook,
+  .fwd = toyota_fwd_hook,
  .get_checksum = toyota_get_checksum,
  .compute_checksum = toyota_compute_checksum,
  .get_quality_flag_valid = toyota_get_quality_flag_valid,
@@ -101,6 +101,29 @@ class TestToyotaSafetyBase(common.CarSafetyTest, common.LongitudinalAccelSafetyT
      tester_present = libsafety_py.make_CANPacket(0x750, 0, msg)
      self.assertEqual(should_tx and ecu_disabled and not stock_longitudinal, self._tx(tester_present))

+  def test_enhanced_bsm(self):
+    # SP: enable/disable/poll left+right blind spot debug mode, sent to the radar diagnostic address
+    valid_msgs = [
+      b"\x41\x02\x10\x60\x00\x00\x00\x00",  # enable left
+      b"\x41\x02\x10\x01\x00\x00\x00\x00",  # disable left
+      b"\x41\x02\x21\x69\x00\x00\x00\x00",  # poll left
+      b"\x42\x02\x10\x60\x00\x00\x00\x00",  # enable right
+      b"\x42\x02\x10\x01\x00\x00\x00\x00",  # disable right
+      b"\x42\x02\x21\x69\x00\x00\x00\x00",  # poll right
+    ]
+    for msg in valid_msgs:
+      pkt = libsafety_py.make_CANPacket(0x750, 0, msg)
+      self.assertTrue(self._tx(pkt), msg.hex())
+
+    invalid_msgs = [
+      b"\x41\x02\x10\x61\x00\x00\x00\x00",  # wrong subfunction
+      b"\x43\x02\x10\x60\x00\x00\x00\x00",  # wrong sub-address (not left/right)
+      b"\x41\x02\x10\x60\x01\x00\x00\x00",  # non-zero trailing bytes
+    ]
+    for msg in invalid_msgs:
+      pkt = libsafety_py.make_CANPacket(0x750, 0, msg)
+      self.assertFalse(self._tx(pkt), msg.hex())
+
  def test_block_aeb(self, stock_longitudinal: bool = False):
    for controls_allowed in (True, False):
      for bad in (True, False):
@@ -14,7 +14,7 @@ from opendbc.car import structs
 from opendbc.car.can_definitions import CanRecvCallable, CanSendCallable
 from opendbc.car.hyundai.values import HyundaiFlags
 from opendbc.car.subaru.values import SubaruFlags
-from opendbc.car.toyota.values import ToyotaSafetyFlags
+from opendbc.car.toyota.values import RADAR_ACC_CAR, SECOC_CAR, TSS2_CAR, ToyotaSafetyFlags
 from opendbc.sunnypilot.car.hyundai.enable_radar_tracks import enable_radar_tracks as hyundai_enable_radar_tracks
 from opendbc.sunnypilot.car.hyundai.longitudinal.helpers import LongitudinalTuningType
 from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP
@@ -157,6 +157,10 @@ def _initialize_toyota(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params
  if CP.brand == 'toyota':
    toyota_stock_long = int(params_dict.get("ToyotaEnforceStockLongitudinal", 0)) == 1
    toyota_stop_and_go_hack = int(params_dict.get("ToyotaStopAndGoHack", 0)) == 1
+    toyota_tss2_long_tuning = int(params_dict.get("ToyotaTSS2Long", 0)) == 1
+    toyota_enhanced_bsm = int(params_dict.get("ToyotaEnhancedBsm", 0)) == 1
+    toyota_auto_brake_hold = int(params_dict.get("ToyotaAutoHold", 0)) == 1
+    toyota_brake_onset = int(params_dict.get("ToyotaBrakeOnset", 0)) == 1

    if toyota_stock_long:
      CP_SP.flags |= ToyotaFlagsSP.STOCK_LONGITUDINAL.value
@@ -166,3 +170,15 @@ def _initialize_toyota(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params

    if toyota_stop_and_go_hack and CP.openpilotLongitudinalControl:
      CP_SP.flags |= ToyotaFlagsSP.STOP_AND_GO_HACK.value
+
+    if toyota_tss2_long_tuning:
+      CP_SP.flags |= ToyotaFlagsSP.TSS2_LONG_TUNING.value
+
+    if toyota_enhanced_bsm and CP.carFingerprint in (TSS2_CAR - SECOC_CAR):
+      CP_SP.flags |= ToyotaFlagsSP.SP_ENHANCED_BSM.value
+
+    if toyota_auto_brake_hold and CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR - SECOC_CAR):
+      CP_SP.flags |= ToyotaFlagsSP.SP_AUTO_BRAKE_HOLD.value
+
+    if toyota_brake_onset:
+      CP_SP.flags |= ToyotaFlagsSP.SP_BRAKE_ONSET.value
@@ -0,0 +1,92 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+
+import math
+from opendbc.car import structs
+from opendbc.car.toyota import toyotacan
+from opendbc.sunnypilot.car.toyota.values import ToyotaFlagsSP
+
+GearShifter = structs.CarState.GearShifter
+
+# frames of confirmed hold-eligible standstill required before engaging
+BRAKE_HOLD_ALLOWED_TIMER = 100
+BRAKE_HOLD_MIN_FORCE = 1400.0 #n
+BRAKE_HOLD_LIGHT_TIMER = 250
+
+DISALLOWED_GEARS = (GearShifter.park, GearShifter.reverse)
+
+# PRE_COLLISION_2 fields that go high when the camera's own PCS/AEB is genuinely intervening this
+# frame (PCSALM mirrors PRECOLLISION_ACTIVE; IBTRGR/PBATRGR/PREFILL/AVSTRGR/PBRTRGR/PPTRGR are its
+# actuation triggers - see create_pcs_commands for the same field set on the stock-DSU PCS path).
+# Deliberately over-inclusive: a false positive here just means we pass a quiescent frame through
+# instead of holding it, never the other way around, so err toward checking more fields, not fewer.
+PCS_TRIGGER_FIELDS = ("PCSALM", "IBTRGR", "PBATRGR", "PREFILL", "AVSTRGR", "PBRTRGR", "PPTRGR")
+
+
+def pcs_is_active(pre_collision_2: dict) -> bool:
+  return any(pre_collision_2.get(field, 0) for field in PCS_TRIGGER_FIELDS) or pre_collision_2.get("DSS1GDRV", 0) != 0
+
+
+class AutoBrakeHold:
+  def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP):
+    self.CP = CP
+    self.CP_SP = CP_SP
+
+  @property
+  def enabled(self):
+    return bool(self.CP_SP.flags & ToyotaFlagsSP.SP_AUTO_BRAKE_HOLD)
+
+
+# Auto Brake Hold (@AlexandreSato, @rav4kumar): holds the car at a stop with cruise off by
+# overriding PRE_COLLISION_2 - the only channel on this platform that can command the brake
+# independent of ACC engagement, since PCS/AEB is an always-on active safety system by design.
+# Yields to any genuine PCS activation this frame - the real message is only ever overridden while
+# it's quiescent - and releases for the rest of the current standstill episode on a brake press,
+# rather than for a single frame.
+class AutoBrakeHoldCarController(AutoBrakeHold):
+  def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP):
+    super().__init__(CP, CP_SP)
+
+    self.active = False
+    self._counter = 0
+    self._released = False
+    self._armed = False
+    self._firm_frame = 0
+    self._prev_brake_pressed = False
+
+  def update(self, CS: structs.CarState, frame: int, packer) -> list:
+    relay_blocked = (CS.out.standstill and CS.out.cruiseState.available and not CS.out.cruiseState.enabled and
+                      not CS.out.gasPressed)
+    hold_allowed = relay_blocked and CS.out.gearShifter not in DISALLOWED_GEARS
+
+    if hold_allowed:
+      # only a fresh press releases hold - the press that caused the stop is already reflected in
+      # _prev_brake_pressed by the time standstill is reached, so it doesn't count as a release
+      if CS.out.brakePressed and not self._prev_brake_pressed:
+        self._released = True
+      force = getattr(CS, "brake_force", float("nan"))
+      self._counter += 1
+      if not self._armed and (math.isnan(force) or force >= BRAKE_HOLD_MIN_FORCE):
+        self._armed = True
+        self._firm_frame = self._counter
+      firm_ready = self._armed and self._counter - self._firm_frame >= BRAKE_HOLD_ALLOWED_TIMER
+      held_long = self._counter > BRAKE_HOLD_LIGHT_TIMER
+      self.active = (firm_ready or held_long) and not self._released
+    else:
+      self._counter = 0
+      self.active = False
+      self._released = False
+      self._armed = False
+
+    self._prev_brake_pressed = CS.out.brakePressed
+
+    can_sends = []
+    if relay_blocked and frame % 2 == 0:
+      override = self.active and not pcs_is_active(CS.pre_collision_2)
+      can_sends.append(toyotacan.create_brake_hold_command(packer, frame, CS.pre_collision_2, override))
+
+    return can_sends
@@ -0,0 +1,416 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+import math
+import numpy as np
+
+ONSET_T_BP = [0.0, 0.15, 0.6]  # s
+# Speed schedule of the brake onset. Below 10 km/h (creep follow: the lead moves a little, the car catches up and
+# stops again) the Altis PCM bites hard on a modest request (route 000000ed 04:05:41: output -0.42 -> -1.25 m/s^2,
+# 1400 N; five ACC re-stops on 10-03/04 all -0.9..-1.25), so at creep speeds the onset uses the same numbers as the
+# engage schedule (driver 2026-10-04 "yes, apply it"): 0.5 m/s^3 for 0.2 s, stock by 0.7 s; from 20 km/h the normal
+# 1.0 m/s^3 for 0.1 s, stock by 0.4 s (driver 2026-10-03), interpolated between.
+# Driver 2026-10-04 after route 000000f2: "the brake after creeping forward again improved but is still not soft
+# enough, softer again" -> below 10 km/h 0.3 m/s^3 for 0.3 s (-0.09 m/s^2 at 0.3 s), stock by 0.9 s.
+# Driver 2026-10-05: "at 50 km/h the brake still feels heavy and there was plenty of distance; every brake should start
+# extremely light, and if that needs distance, start earlier" -> the creep-speed numbers (0.3 m/s^3 for 0.3 s, stock
+# by 0.9 s) now apply at every speed up to 60 km/h, fading to the highway onset (1.0 m/s^3, 0.1 s, stock by 0.4 s) at
+# 80 km/h, which stays as it was. The planner's light-onset margin (long_mpc) supplies the extra distance.
+# Driver 2026-10-05, same drive: "at 40-50 km/h the build from light to firm is still not smooth, a bit abrupt" -> the
+# jerk the onset ramps UP TO is also scheduled: 2.0 m/s^3 up to 60 km/h (was the stock 4.0 from 0.9 s on), stock at 80.
+ONSET_V_BP = [16.7, 22.2]  # m/s (60, 80 km/h)
+ONSET_T1_V = [0.3, 0.1]
+ONSET_T3_V = [0.9, 0.4]
+ONSET_J1_V = [0.3, 1.0]  # m/s^3 during the first phase
+ONSET_J3_V = [2.0, 4.0]  # m/s^3 the ramp ends at (the shaper never exceeds the stock limit)
+ONSET_J_DOWN = [1.0, 1.0, 4.0]  # m/s^3 (reference shape; the first two entries follow ONSET_J1_V)
+# Engaging ACC (SET-) while creeping up to a lead (driver 2026-10-04: "the car used to brake hard the moment I press
+# SET-; the first ~0.2 s must be a light touch, then blend into the decel the speed needs"). Route 000000ee 11:48:07:
+# engaged at 9 km/h 4 m behind a 4 km/h lead, the request went 0 -> -0.42 in 0.2 s and -1.6 by 0.8 s, the car hit
+# -2.7 m/s^2 (3280 N). While the engage window is open the down jerk is capped on time SINCE ENGAGING: 0.5 m/s^3 for
+# the first 0.2 s (-0.1 m/s^2 at 0.2 s), then rising to stock by 0.7 s; the planner's full request still arrives, just
+# later. The normal onset schedule applies on top (the stricter of the two wins).
+ENGAGE_BRAKE_T_BP = [0.0, 0.2, 0.7]  # s since engaging
+ENGAGE_BRAKE_J_DOWN = [0.5, 0.5, 4.0]  # m/s^3
+# Owner 2026-10-06: only a plan beyond -3.0 (a genuine emergency) bypasses the soft onset; -2.0 .. -3.0 keep it (at
+# 40 km/h -2.5 is reached slowly in ~1.8 s). History: -1.5 until 2026-10-03, -2.0, then -3.5 (= never) on 2026-10-05.
+HARD_BRAKE_ACCEL = -3.0
+URGENT_T = 0.1  # s
+URGENT_J = 1.0  # m/s^3
+URGENT_T_RAMP = 0.1  # s
+
+
+# Creep-follow gas release, with the creep-follow stop (see
+# stopping_controller.py). Route 0000010d 23:56:59: after a +0.5 launch the plan turned to braking within 0.2 s, but the
+# soft onset above treated the drop from +0.5 as the start of a brake (0.3 m/s^3) and the command stayed positive for
+# ~0.7 s while the car kept accelerating into a 2 m gap; then the brake bit 0 -> 1640 N. Taking away gas is not
+# braking: below CREEP_GAS_V the positive part of the command is now taken away at the stock rate; the soft onset
+# starts from zero, where the braking starts.
+CREEP_GAS_V = 8.0 / 3.6  # m/s
+
+
+# Friction entry (hybrid). Owner 2026-10-09: "no heavy-press jolt at the start of a brake - this is what I most want
+# gone; even -2.6 is fine if it is reached smoothly". On this hybrid the first ~-0.6 m/s^2 of a brake is regen; past it
+# the PCM brings in the friction brake (BRAKE 0xA6 BRAKE_FORCE), and how hard that bites depends on how fast the
+# command is moving at that moment. 27 engaged onsets with a friction entry, routes 00000109-00000119 (onset_bite.py):
+#   command rate at friction entry -1.91 / -1.05 / -0.44 m/s^3 -> delivered jerk -3.70 / -2.22 / -1.22 m/s^3,
+#   friction force rate 3067 / 1067 / 533 N/s (r = 0.78; gap command-delivered at entry r = 0.80).
+# The onset schedule above opens to 2.0 m/s^3 by 0.9 s - right when the friction usually enters (median 0.9 s), e.g.
+# route 00000119 09:23:02 (46 km/h, lead braking to a stop): command -0.9 -> -1.66 smoothly, the car sat at the regen
+# limit (-0.55), then the friction came in 680 -> 1520 N and the car went -0.76 -> -1.98 m/s^2 in 0.5 s.
+# So while the command is more than ENTRY_GAP beyond what the car delivers and the friction force is still below
+# ENTRY_FORCE_N, the command moves at most ENTRY_J: the friction enters slowly and the car catches up. After the entry
+# (force established or ENTRY_T_MAX) the command closes the remaining gap at CATCHUP_J, then the normal limits apply.
+# Not for FCW / urgent (< HARD_BRAKE_ACCEL) requests, below ENTRY_V_MIN (creep follow has its own onset), or without
+# measurements (a_ego None).
+ENTRY_GAP = 0.25        # m/s^2 the command may lead the delivered decel before the entry limit applies
+ENTRY_FORCE_N = 600.0   # N: friction established
+ENTRY_J = 0.5           # m/s^3 while the friction enters
+ENTRY_T_MAX = 1.0       # s per brake onset
+CATCHUP_J = 1.5         # m/s^3 after the entry until the command reaches the request
+ENTRY_V_MIN = 10.0 / 3.6  # m/s
+
+
+class BrakeOnsetShaper:
+  def __init__(self, dt: float, stock_down_jerk: float):
+    self.dt = dt
+    self.stock_down_jerk = stock_down_jerk
+    self.t_onset = 0.0
+    self.t_entry = 0.0      # time spent limited by the friction entry in this brake
+    self.catchup = False    # after an entry: close the gap gently
+    self.friction_in = False
+
+  def reset(self) -> None:
+    self.t_onset = 0.0
+    self.t_entry = 0.0
+    self.catchup = False
+    self.friction_in = False
+
+  def friction_entry_step(self, accel_request: float, prev_accel: float, v_ego: float, a_ego: float | None,
+                          brake_force: float) -> float | None:
+    """largest down step (> 0, m/s^2 per step) while the friction brake enters, None = no limit"""
+    if a_ego is None or v_ego < ENTRY_V_MIN or prev_accel > -0.1:
+      self.t_entry, self.catchup, self.friction_in = 0.0, False, False
+      return None
+    if math.isfinite(brake_force) and brake_force >= ENTRY_FORCE_N:
+      self.friction_in = True
+    if accel_request >= prev_accel:
+      self.catchup = False
+      return None
+    leading = prev_accel - a_ego < -ENTRY_GAP
+    if leading and not self.friction_in and self.t_entry < ENTRY_T_MAX:
+      self.t_entry += self.dt
+      self.catchup = True
+      return ENTRY_J * self.dt
+    if self.catchup:
+      return CATCHUP_J * self.dt
+    return None
+
+  @staticmethod
+  def schedule_t(v_ego: float) -> list[float]:
+    t1 = float(np.interp(v_ego, ONSET_V_BP, ONSET_T1_V))
+    t3 = float(np.interp(v_ego, ONSET_V_BP, ONSET_T3_V))
+    return [0.0, t1, t3]
+
+  @staticmethod
+  def schedule_j(v_ego: float) -> list[float]:
+    j1 = float(np.interp(v_ego, ONSET_V_BP, ONSET_J1_V))
+    j3 = float(np.interp(v_ego, ONSET_V_BP, ONSET_J3_V))
+    return [j1, j1, j3]
+
+  def down_step(self, accel_request: float, prev_accel: float, bypass: bool = False, v_ego: float = 30.0,
+                urgent: bool = False, t_engaged: float | None = None, a_ego: float | None = None,
+                brake_force: float = float('nan')) -> float:
+    if bypass:
+      self.t_onset = 0.0
+      self.t_entry, self.catchup = 0.0, False
+      return -self.stock_down_jerk * self.dt
+    entry = None if urgent else self.friction_entry_step(accel_request, prev_accel, v_ego, a_ego, brake_force)
+    step = self._down_step(accel_request, prev_accel, v_ego, urgent, t_engaged)
+    return step if entry is None else -min(-step, entry)
+
+  def _down_step(self, accel_request: float, prev_accel: float, v_ego: float, urgent: bool,
+                 t_engaged: float | None) -> float:
+    if prev_accel > 0.0 and v_ego < CREEP_GAS_V and accel_request < prev_accel:
+      self.t_onset = 0.0
+      return -min(self.stock_down_jerk * self.dt, prev_accel - max(accel_request, 0.0))
+
+    if urgent:
+      t_bp, j_bp = [0.0, URGENT_T, URGENT_T + max(URGENT_T_RAMP, self.dt)], [URGENT_J, URGENT_J, self.stock_down_jerk]
+    else:
+      t_bp, j_bp = self.schedule_t(v_ego), self.schedule_j(v_ego)
+    gentlest_step = -ONSET_J_DOWN[0] * self.dt
+    onset = (accel_request - prev_accel) < gentlest_step - 1e-9
+    if onset:
+      j_down = float(np.interp(self.t_onset, t_bp, j_bp))
+      self.t_onset = min(self.t_onset + self.dt, t_bp[-1])
+    else:
+      j_down = self.stock_down_jerk
+      self.t_onset = max(self.t_onset - self.dt, 0.0)
+    if t_engaged is not None and t_engaged < ENGAGE_BRAKE_T_BP[-1]:
+      j_down = min(j_down, float(np.interp(t_engaged, ENGAGE_BRAKE_T_BP, ENGAGE_BRAKE_J_DOWN)))
+    return -min(j_down, self.stock_down_jerk) * self.dt
+
+  def is_urgent(self, accel_request: float, fcw: bool) -> bool:
+    return fcw or accel_request < HARD_BRAKE_ACCEL
+
+ENGAGE_T_BP = [0.0, 0.1, 0.6]  # s; the window is still used to keep the soft BRAKE onset for a hard request after engaging
+# Driver 2026-10-04: "if Kumar's base added a soft start again, remove it - I want an immediate response, not a lurch,
+# following the eco/normal/sport profile". The gas-side engage ramp (0.25 -> 0.6 -> 4.0 m/s^3 over 0.6 s) is gone:
+# the up jerk after engaging is the stock windup limit; the accel profile sets how much.
+ENGAGE_J_UP = [4.0, 4.0, 4.0]  # m/s^3
+# Resume while moving. Owner 2026-10-08: "when resuming the
+# speed (RES), can the first 0.4 s accelerate more gently?". Routes 0000010e-00000112, eight RES engagements at 15-61
+# km/h: the car was coasting at -0.3..-0.5 m/s^2 (regen), the command started from 0 and reached +0.3..+0.5 within
+# 0.4-0.6 s, so in the first 0.4 s the regen vanished AND the gas came in (aEgo -0.43 -> +0.19 in 0.4 s at 23:09:07,
+# +0.12 -> +0.64 in 0.6 s at 17:43:01, ~1.5-2 m/s^3). Now, when engaging above RESUME_V_MIN, the command starts from
+# the coasting decel (aEgo, clipped to RESUME_START_MIN..0) instead of 0, and rises at RESUME_J_UP: 0.6 m/s^3 for the
+# first 0.4 s, opening to the stock limit by 0.8 s. Replays of the logged plans: the command meets the stock one by
+# 0.6-0.8 s, identical afterwards (the eco/normal/sport profile is untouched). Standstill launches are not affected.
+RESUME_V_MIN = 15.0 / 3.6  # m/s
+RESUME_T_BP = [0.0, 0.4, 0.8]  # s since engaging
+RESUME_J_UP = [0.6, 0.6, 4.0]  # m/s^3
+RESUME_START_MIN = -0.6  # m/s^2
+RESUME_EDGE_T = 0.5  # s: the engagement must follow the cruise switching on (not a gas-override release)
+
+
+class EngageOnsetShaper:
+  def __init__(self, dt: float, stock_up_jerk: float):
+    self.dt = dt
+    self.stock_up_jerk = stock_up_jerk
+    self.t_engaged: float | None = None
+    self.resume = False
+    self.cruise_on_t = 1e9  # s since the PCM cruise went from off to on (RES / SET)
+    self.prev_cruise = False
+
+  def reset(self) -> None:
+    self.t_engaged = None
+    self.resume = False
+
+  def cruise_state(self, cruise_enabled: bool) -> None:
+    """called every frame: tracks RES / SET (cruise off -> on), so a gas-override release is not a resume"""
+    self.cruise_on_t = 0.0 if (cruise_enabled and not self.prev_cruise) else self.cruise_on_t + self.dt
+    self.prev_cruise = cruise_enabled
+
+  def start_accel(self, prev_accel: float, a_ego: float, v_ego: float) -> float:
+    """on the first engaged frame of a resume while moving, the command starts from the coasting decel"""
+    if self.t_engaged is None and v_ego > RESUME_V_MIN and self.cruise_on_t < RESUME_EDGE_T:
+      self.resume = True
+      return float(np.clip(a_ego, RESUME_START_MIN, 0.0))
+    return prev_accel
+
+  @property
+  def in_engage_window(self) -> bool:
+    return self.t_engaged is None or self.t_engaged < ENGAGE_T_BP[-1]
+
+  @property
+  def t_since_engage(self) -> float | None:
+    """seconds since engaging while the engage brake schedule still applies, else None"""
+    t = 0.0 if self.t_engaged is None else self.t_engaged
+    return t if t < ENGAGE_BRAKE_T_BP[-1] else None
+
+  def up_step(self, active: bool) -> float:
+    if not active:
+      self.t_engaged = None
+      return self.stock_up_jerk * self.dt
+    if self.t_engaged is None:
+      self.t_engaged = 0.0
+    else:
+      self.t_engaged += self.dt
+    if self.resume:
+      if self.t_engaged < RESUME_T_BP[-1]:
+        return min(float(np.interp(self.t_engaged, RESUME_T_BP, RESUME_J_UP)), self.stock_up_jerk) * self.dt
+      self.resume = False
+    if self.t_engaged >= ENGAGE_T_BP[-1]:
+      return self.stock_up_jerk * self.dt
+    j_up = float(np.interp(self.t_engaged, ENGAGE_T_BP, ENGAGE_J_UP))
+    return min(j_up, self.stock_up_jerk) * self.dt
+
+# Hard-brake overshoot limit (owner 2026-10-06, route 00000100 19:41:42-45: the lead braked from 44 km/h at -3.3 m/s^2;
+# openpilot asked for -3.49 at most, but the PCM with the hybrid's regen delivered -4.3 m/s^2 for 0.8 s (BRAKE_FORCE
+# 4880 N), ~30% more than asked - the last, heaviest part of the "sudden brake" feel. The stock PID only lifted the
+# command ~0.3 above the request.) Once the request is a hard brake, the car's own deceleration is watched: when it
+# runs OVERSHOOT_DEADBAND past the request (predicted a moment ahead, as the PID does), the command is lifted by
+# OVERSHOOT_GAIN times the excess, at most OVERSHOOT_MAX, so the car settles near what openpilot asked for. The request
+# itself is never weakened: the limit only takes back braking the car delivered ON TOP of it (an MPC replay of 19:41
+# with the asked-for -3.5 ends 4.9 m behind the stopped lead). It never lifts the command above OVERSHOOT_CMD_CEIL.
+OVERSHOOT_ACTIVE_ACCEL = -2.5  # m/s^2: the limit works only while the request is harder than this
+OVERSHOOT_DEADBAND = 0.2  # m/s^2 of overshoot that is left alone
+OVERSHOOT_GAIN = 0.8
+OVERSHOOT_MAX = 1.0  # m/s^2 largest lift
+OVERSHOOT_RATE_UP = 3.0  # m/s^3 how fast the lift may grow
+OVERSHOOT_RATE_DOWN = 2.0  # m/s^3 how fast it is given back (also when the hard request ends)
+OVERSHOOT_CMD_CEIL = -1.5  # m/s^2: the lifted command always stays a firm brake
+# Moderate band - Corolla Altis Hybrid only (owner 2026-10-07, route 0000010e 17:38:41: "at 50 km/h the braking started
+# soft, but the speed dropped too fast, still a bit fierce"). A stopped queue appeared 74 m ahead after a lane change;
+# openpilot asked for at most -1.60 m/s^2, the car delivered -1.82..-1.95 (regen on top). Routes 00000100..0000010e:
+# above 20 km/h the Altis delivers 0.1-0.4 m/s^2 more than a -1.0..-2.0 request (e.g. 0000010d 15-25 km/h: asked
+# -1.68, got -2.05). So above OVERSHOOT_MOD_V the limit also works for requests harder than OVERSHOOT_MOD_ACCEL, with
+# a smaller deadband, and the lifted command never gets lighter than OVERSHOOT_MOD_KEEP of the command (the request
+# is never weakened by more than a quarter). The hard band above is unchanged for every car.
+OVERSHOOT_MOD_ACCEL = -1.0  # m/s^2
+OVERSHOOT_MOD_V = 12.0 / 3.6  # m/s (0000010d 23:54:58: asked -1.72 at 17-23 km/h, delivered -1.96..-2.19)
+OVERSHOOT_MOD_DEADBAND = 0.15  # m/s^2
+OVERSHOOT_MOD_KEEP = 0.75
+# The moderate band works as a slow trim, not a fast limiter: a replay of 17:38:36-38 with the hard band's logic
+# (predicted aEgo, 3.0 / 2.0 m/s^3) pulsed the command by 0.2-0.3 m/s^2 for 0.2 s at a time on the noisy aEgo (-1.2 ..
+# -1.8 at a steady request), and the stock PID was already lifting the command (-1.6 asked, ~-1.1 sent). So the band
+# uses the overshoot of the measured aEgo, low-passed over OVERSHOOT_MOD_TAU, and moves at OVERSHOOT_MOD_RATE.
+# Relaxed personality only (owner 2026-10-08). A process_replay of 14 Aggressive-mode segments (routes fe/100/101/104/
+# 107/109) found the band acting mostly in CLOSE following: 0000109 23:19:44, lead 16 m away and still braking, request
+# -1.99/-1.75 at 26-21 km/h, the car delivered -2.73/-2.27 and the band lifted the command by up to 0.35 - taking away a
+# margin the close gap needs (open loop, no planner reaction: +6 m travelled). Tonight's Relaxed drives (routes
+# 00000112-00000116) never triggered it. Owner: "keep D in Relaxed". The carcontroller passes
+# hud_control.leadDistanceBars == 3 (controlsd: personality + 1; relaxed = 2 -> 3 bars).
+# OFF since 2026-10-08 (owner: "so D never acted in Relaxed last night? then switch it off and try"): routes
+# 00000112-00000116 never triggered it, and in Aggressive close following it removes margin. Kept in the code; set True
+# to bring it back (Relaxed only).
+OVERSHOOT_MOD_ENABLED = False
+OVERSHOOT_MOD_TAU = 0.5  # s
+OVERSHOOT_MOD_RATE = 0.5  # m/s^3
+
+
+class BrakeOvershootLimiter:
+  def __init__(self, dt: float, moderate: bool = False):
+    self.dt = dt
+    self.moderate = moderate  # also the moderate band (OVERSHOOT_MOD_*)
+    self.lift = 0.0
+    self.in_moderate = False
+    self.mod_excess = 0.0
+
+  def reset(self) -> None:
+    self.lift = 0.0
+    self.in_moderate = False
+    self.mod_excess = 0.0
+
+  def update(self, accel_request: float, a_ego_future: float, active: bool = True, v_ego: float = 0.0,
+             a_ego: float | None = None, moderate_allowed: bool = True) -> float:
+    """returns the lift (>= 0, m/s^2) to add to the command"""
+    target = 0.0
+    hard = accel_request < OVERSHOOT_ACTIVE_ACCEL
+    self.in_moderate = (self.moderate and moderate_allowed and not hard and accel_request < OVERSHOOT_MOD_ACCEL and
+                        v_ego > OVERSHOOT_MOD_V)
+    if self.in_moderate:
+      measured = a_ego_future if a_ego is None else a_ego
+      alpha = self.dt / (OVERSHOOT_MOD_TAU + self.dt)
+      self.mod_excess += alpha * ((accel_request - measured) - self.mod_excess)
+    else:
+      self.mod_excess = 0.0
+    if active and hard:
+      excess = accel_request - a_ego_future - OVERSHOOT_DEADBAND  # > 0: decelerating harder than asked
+      target = float(np.clip(OVERSHOOT_GAIN * excess, 0.0, OVERSHOOT_MAX))
+    elif active and self.in_moderate:
+      target = float(np.clip(OVERSHOOT_GAIN * (self.mod_excess - OVERSHOOT_MOD_DEADBAND), 0.0, OVERSHOOT_MAX))
+    if self.in_moderate and not hard:
+      step = OVERSHOOT_MOD_RATE * self.dt
+      self.lift = float(np.clip(target, self.lift - step, self.lift + step))
+    else:
+      self.lift = float(np.clip(target, self.lift - OVERSHOOT_RATE_DOWN * self.dt, self.lift + OVERSHOOT_RATE_UP * self.dt))
+    return self.lift
+
+  def apply(self, accel_cmd: float) -> float:
+    if self.lift <= 0.0:
+      return accel_cmd
+    ceiling = OVERSHOOT_MOD_KEEP * accel_cmd if self.in_moderate else OVERSHOOT_CMD_CEIL
+    return min(accel_cmd + self.lift, max(accel_cmd, ceiling))
+
+
+# Low-speed regen hand-over feed-forward. History: route 0000010d 23:54:58-23:55:03 - between 20 and 9 km/h the car cut its own
+# friction brake (BRAKE 0xA6 BRAKE_FORCE 1640 -> 440 N) and delivered ~70% of the request, felt as "brakes, goes light,
+# brakes again". A feedback compensation (shortfall x 0.8 past a 0.2 deadband, 1.0 m/s^3) was the first fix; its
+# first drive, route 0000010e 2026-10-07, showed it acting only in 0.1-0.25 m/s^2 pulses that never filled the gap
+# (owner: "the stops are sometimes soft, sometimes still not smooth; several stops still braked twice").
+# Measured on routes 00000100..0000010e (engaged stops, request / delivered, m/s^2): the request eases smoothly, but the
+# delivered decel DIPS at 15-9 km/h (10e: -0.84 asked, -0.61 delivered; 109: -0.87 / -0.69; 107: -1.04 / -0.81) and
+# comes back ON TOP of the request at 7-1 km/h (10e: -0.47 / -0.60; 109: -0.54 / -0.80), the second part pushed by the
+# PID integrator that wound up during the dip (command - request -0.10..-0.17 there). So, instead of feedback:
+#  - a FEED-FORWARD extra brake by speed, the measured dip: HANDOVER_EXTRA_MAX (or HANDOVER_EXTRA_FRAC of a lighter
+#    request) at 10-14 km/h, fading to 0 at 6 and 17 km/h, changing at HANDOVER_RATE, only while braking harder than
+#    HANDOVER_MIN_REQUEST;
+#  - below PID_HOLD_V, while the request brakes, the PID integrator is frozen and bled toward zero (a positive
+#    integral, left over from a launch, faster), so the catch-up no longer adds braking at the end of the stop.
+# Expected delivered decel on 10e (by speed band 15-12/12-9/9-7/7-5/5-3/3-1 km/h): -0.86/-0.81/-0.75/-0.75/-0.63/-0.48,
+# continuous easing, instead of -0.66/-0.61/-0.75/-0.75/-0.71/-0.60.
+HANDOVER_V_BP = [6.0 / 3.6, 8.0 / 3.6, 10.0 / 3.6, 14.0 / 3.6, 17.0 / 3.6]  # m/s
+HANDOVER_V_W = [0.0, 0.5, 1.0, 1.0, 0.0]
+HANDOVER_MIN_REQUEST = -0.3  # m/s^2
+HANDOVER_EXTRA_MAX = 0.2  # m/s^2
+HANDOVER_EXTRA_FRAC = 0.25  # of the request, for lighter requests
+HANDOVER_RATE = 1.0  # m/s^3
+# The dip is a moderate-brake effect: at a firm request the regen gives MORE, not less (0000010d 15-12 km/h: asked
+# -1.46, delivered -1.72), so the extra fades out between these requests (full at -1.0 and lighter, none at -1.5).
+HANDOVER_REQ_BP = [-1.5, -1.0]  # m/s^2
+HANDOVER_REQ_W = [0.0, 1.0]
+# Owner 2026-10-10 (route 0000011c 00:19:10, Aggressive stop 1.6 m behind the lead): "the end of the stop slides too
+# much, the last two metres feel too light". With the PID held below PID_HOLD_V nothing makes up the under-delivery
+# below the hand-over band; asked / delivered by band (m/s^2), three stops of 11c (00:18:07 / 00:19:10 / 00:20:58):
+#   8-7 km/h -0.75/-0.62  -0.89/-0.58  -0.77/-0.54     6-5 km/h -0.63/-0.45  -0.75/-0.50  -0.66/-0.49
+#   5-4 km/h -0.60/-0.40  -0.67/-0.28  -0.64/-0.35     4-3 km/h -0.58/-0.62  -0.62/-0.45  -0.61/-0.57
+#   3-2 km/h on target or above. Brake force 650-850 N at 6-4 km/h, below what beats the hybrid creep torque.
+# So a second, low-speed feed-forward: LOW_EXTRA at LOW_EXTRA_V_BP (0.25 at 5-6.5 km/h, gone by 3 km/h, led by about
+# 0.5 km/h for the ~0.3 s actuator lag), added to the hand-over extra, same request gate and rate.
+LOW_EXTRA_V_BP = [3.0 / 3.6, 4.0 / 3.6, 5.0 / 3.6, 6.5 / 3.6, 8.5 / 3.6]  # m/s
+LOW_EXTRA = [0.0, 0.15, 0.25, 0.25, 0.0]  # m/s^2
+PID_HOLD_V = 9.0 / 3.6  # m/s
+PID_BLEED_NEG = 0.5  # m/s^2 per s: a braking integral fades this fast
+PID_BLEED_POS = 2.0  # m/s^2 per s: a gas integral (left from the launch) fades this fast
+
+class BrakeHandoverFeedforward:
+  def __init__(self, dt: float):
+    self.dt = dt
+    self.extra = 0.0
+
+  def reset(self) -> None:
+    self.extra = 0.0
+
+  def update(self, accel_request: float, v_ego: float, active: bool = True) -> float:
+    """returns the extra braking (>= 0, m/s^2) to subtract from the command"""
+    target = 0.0
+    if active and accel_request < HANDOVER_MIN_REQUEST:
+      w = float(np.interp(v_ego, HANDOVER_V_BP, HANDOVER_V_W)) * float(np.interp(accel_request, HANDOVER_REQ_BP, HANDOVER_REQ_W))
+      target = w * min(HANDOVER_EXTRA_MAX, HANDOVER_EXTRA_FRAC * -accel_request)
+      target += float(np.interp(v_ego, LOW_EXTRA_V_BP, LOW_EXTRA)) * float(np.interp(accel_request, HANDOVER_REQ_BP, HANDOVER_REQ_W))
+    step = HANDOVER_RATE * self.dt
+    self.extra = float(np.clip(target, self.extra - step, self.extra + step))
+    return self.extra
+
+  def apply(self, accel_cmd: float) -> float:
+    return accel_cmd - self.extra
+
+  def condition_pid(self, pid, accel_request: float, v_ego: float) -> bool:
+    """below PID_HOLD_V while braking: bleed the integrator toward zero; returns True = freeze it this step"""
+    if v_ego >= PID_HOLD_V or accel_request >= 0.0:
+      return False
+    if pid.i > 0.0:
+      pid.i = max(0.0, pid.i - PID_BLEED_POS * self.dt)
+    else:
+      pid.i = min(0.0, pid.i + PID_BLEED_NEG * self.dt)
+    return True
+
+
+class BrakeCommandCorrections:
+  """tnpb2 corrections to the final braking command."""
+  def __init__(self, dt: float):
+    self.overshoot = BrakeOvershootLimiter(dt, moderate=OVERSHOOT_MOD_ENABLED)
+    self.handover = BrakeHandoverFeedforward(dt)
+
+  def reset(self) -> None:
+    self.overshoot.reset()
+    self.handover.reset()
+
+  def apply(self, accel_cmd: float, accel_request: float, a_ego_future: float, v_ego: float, stopping: bool, fcw: bool,
+            a_ego: float | None = None, relaxed: bool = True) -> float:
+    # take back braking the car delivers beyond a hard request (never under FCW); the moderate band only in Relaxed
+    self.overshoot.update(accel_request, a_ego_future, active=not stopping and not fcw, v_ego=v_ego, a_ego=a_ego,
+                          moderate_allowed=relaxed)
+    accel_cmd = self.overshoot.apply(accel_cmd)
+    # feed-forward the braking the car does not deliver in the low-speed regen hand-over
+    self.handover.update(accel_request, v_ego, active=not fcw)
+    return self.handover.apply(accel_cmd)
+
+  def condition_pid(self, pid, accel_request: float, v_ego: float) -> bool:
+    """called before the stock PID update; True = freeze the integrator this step"""
+    return self.handover.condition_pid(pid, accel_request, v_ego)
@@ -11,8 +11,11 @@ from opendbc.car import Bus, structs
 from opendbc.car.carlog import carlog
 from opendbc.can.parser import CANParser
 from opendbc.car.common.conversions import Conversions as CV
+from opendbc.car.toyota.values import ToyotaFlags
 from opendbc.sunnypilot.car.toyota.values import ToyotaFlagsSP

+ENGINE_RUNNING_RPM = 300.0
+
 TRAFFIC_SIGNAL_MAP = {
  1: "kph",
  36: "mph",
@@ -168,3 +171,7 @@ class CarStateExt:
    # Update traffic signals and speed limit
    self.update_traffic_signals(cp_cam)
    ret_sp.speedLimit = self.calculate_speed_limit()
+
+    if self.CP.flags & ToyotaFlags.HYBRID and "ENGINE_RPM" in cp.vl:
+      ret_sp.engineRpm = float(cp.vl["ENGINE_RPM"]["RPM"])
+      ret_sp.engineOff = ret_sp.engineRpm < ENGINE_RUNNING_RPM and not bool(cp.vl["ENGINE_RPM"]["ENGINE_RUNNING"])
@@ -0,0 +1,126 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+
+from opendbc.car import structs
+from opendbc.car.can_definitions import CanData
+from opendbc.car.toyota import toyotacan
+from opendbc.sunnypilot.car.toyota.values import ToyotaFlagsSP
+
+LEFT_BLINDSPOT = b"\x41"
+RIGHT_BLINDSPOT = b"\x42"
+LEFT_SIDE = LEFT_BLINDSPOT[0]
+RIGHT_SIDE = RIGHT_BLINDSPOT[0]
+
+# BLINDSPOTD1/D2 aren't real distances despite the DBC name: verified against 3 real routes, values
+# fall into 3 clean bands - idle (0), a small transitional-noise band (seen 1-31, rare: single-digit
+# occurrence counts, present during side/state transitions), and two real zone codes (~46-47 and
+# ~50-53, consistent across routes and cars - likely mirroring stock BSM's own ADJACENT/APPROACHING
+# split). A plain nonzero check lets the noise band through as false "occupied" hits; gate on the
+# real-zone floor instead - comfortably above every noise value seen, comfortably below neither zone code.
+BLINDSPOT_NOISE_FLOOR = 35
+
+# DEBUG also carries a 1-bit BLINDSPOT flag (byte 4) that isn't read here. Checked against the same
+# route: stayed 0 across all frames, including every confirmed real detection - not a usable signal.
+
+
+class EnhancedBsm:
+  def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP):
+    self.CP = CP
+    self.CP_SP = CP_SP
+
+  @property
+  def enabled(self):
+    return bool(self.CP_SP.flags & ToyotaFlagsSP.SP_ENHANCED_BSM)
+
+
+class _BsmSideState:
+  def __init__(self):
+    self.blindspot = False
+    self.counter = 0
+
+  def update(self, distance_1, distance_2):
+    # every fresh matching-side reading directly reflects the current occupancy check - this is not
+    # a latch. A drop to an idle/noise reading on this exact side clears it immediately, same as the
+    # original. The counter is purely a silence timeout for when this side stops responding entirely,
+    # not a "hold the last detection for a while" mechanism.
+    self.blindspot = distance_1 > BLINDSPOT_NOISE_FLOOR or distance_2 > BLINDSPOT_NOISE_FLOOR
+    self.counter = 100
+
+  def decay(self):
+    self.counter = max(0, self.counter - 1)
+    if self.counter == 0:
+      self.blindspot = False
+
+
+# Enhanced BSM (@arne182, @rav4kumar)
+class EnhancedBsmCarState(EnhancedBsm):
+  def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP):
+    super().__init__(CP, CP_SP)
+
+    self._sides = {LEFT_SIDE: _BsmSideState(), RIGHT_SIDE: _BsmSideState()}
+
+  def update(self, cp, frame: int) -> tuple[bool, bool]:
+    # Let's keep all the commented out code for easy debug purposes in the future.
+    distance_1 = cp.vl["DEBUG"].get('BLINDSPOTD1')
+    distance_2 = cp.vl["DEBUG"].get('BLINDSPOTD2')
+    side = cp.vl["DEBUG"].get('BLINDSPOTSIDE')
+
+    if all(val is not None for val in [distance_1, distance_2, side]) and side in self._sides:
+      self._sides[side].update(distance_1, distance_2)
+
+    for side_state in self._sides.values():
+      side_state.decay()
+
+    return self._sides[LEFT_SIDE].blindspot, self._sides[RIGHT_SIDE].blindspot
+
+
+class _BsmSideController:
+  def __init__(self, addr_byte: bytes):
+    self.addr_byte = addr_byte
+    self.debug_enabled = False
+    self.last_poll_frame = 0
+
+  def update(self, frame: int, poll_phase: int, e_bsm_rate: int, always_on: bool, vego_ok: bool) -> list[CanData]:
+    can_sends = []
+
+    if not self.debug_enabled:
+      if always_on or vego_ok:  # eagle eye camera will stop working if bsm is switched on under 6m/s
+        can_sends.append(toyotacan.create_set_bsm_debug_mode(self.addr_byte, True))
+        self.debug_enabled = True
+        self.last_poll_frame = frame  # give the poll loop a fresh baseline so the stale-poll disable check below can't fire before the first real poll
+    else:
+      # no periodic re-assert: re-sending DiagnosticSessionControl(extendedSession) while already in that
+      # session appears to make the ECU intermittently drop its own detection state (confirmed against a
+      # real route - two reasserts landed inside an 8s window where the ECU went silent on that side).
+      # send it once and leave it alone, matching the original, proven-stable behavior.
+      if not always_on and frame - self.last_poll_frame > 50:
+        can_sends.append(toyotacan.create_set_bsm_debug_mode(self.addr_byte, False))
+        self.debug_enabled = False
+
+      if frame % e_bsm_rate == poll_phase:
+        can_sends.append(toyotacan.create_bsm_polling_status(self.addr_byte))
+        self.last_poll_frame = frame
+
+    return can_sends
+
+
+# Enhanced BSM (@arne182, @rav4kumar)
+class EnhancedBsmCarController(EnhancedBsm):
+  def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP):
+    super().__init__(CP, CP_SP)
+
+    self._left = _BsmSideController(LEFT_BLINDSPOT)
+    self._right = _BsmSideController(RIGHT_BLINDSPOT)
+
+  def update(self, CS: structs.CarState, frame: int, e_bsm_rate: int = 20, always_on: bool = True) -> list[CanData]:
+    if frame <= 200:
+      return []
+
+    vego_ok = CS.out.vEgo > 6
+    can_sends = self._left.update(frame, 0, e_bsm_rate, always_on, vego_ok)
+    can_sends += self._right.update(frame, e_bsm_rate // 2, e_bsm_rate, always_on, vego_ok)
+    return can_sends
@@ -0,0 +1,231 @@
+import unittest
+from unittest.mock import patch
+
+from opendbc.car import structs
+from opendbc.sunnypilot.car.toyota.auto_brake_hold import AutoBrakeHoldCarController, BRAKE_HOLD_ALLOWED_TIMER, pcs_is_active
+from opendbc.sunnypilot.car.toyota.values import ToyotaFlagsSP
+
+GearShifter = structs.CarState.GearShifter
+
+
+def make_car_params_sp(enabled: bool = True) -> structs.CarParamsSP:
+  cp_sp = structs.CarParamsSP()
+  cp_sp.flags = ToyotaFlagsSP.SP_AUTO_BRAKE_HOLD if enabled else 0
+  return cp_sp
+
+
+class FakeCarState:
+  def __init__(self, standstill=True, cruise_enabled=False, cruise_available=True, gas_pressed=False,
+               gear=GearShifter.drive, brake_pressed=False, pre_collision_2=None):
+    self.out = structs.CarState()
+    self.out.standstill = standstill
+    self.out.cruiseState.enabled = cruise_enabled
+    self.out.cruiseState.available = cruise_available
+    self.out.gasPressed = gas_pressed
+    self.out.gearShifter = gear
+    self.out.brakePressed = brake_pressed
+    self.pre_collision_2 = pre_collision_2 if pre_collision_2 is not None else {}
+
+
+@patch("opendbc.sunnypilot.car.toyota.auto_brake_hold.toyotacan.create_brake_hold_command")
+class TestAutoBrakeHoldCarController(unittest.TestCase):
+  def _make(self):
+    return AutoBrakeHoldCarController(structs.CarParams(), make_car_params_sp())
+
+  def test_enabled_reflects_flag(self, mock_create):
+    for value in range(256):
+      with self.subTest(flags=value):
+        cp_sp = structs.CarParamsSP()
+        cp_sp.flags = value
+        ctrl = AutoBrakeHoldCarController(structs.CarParams(), cp_sp)
+        self.assertEqual(ctrl.enabled, bool(value & ToyotaFlagsSP.SP_AUTO_BRAKE_HOLD))
+
+  def test_does_not_engage_before_timer(self, mock_create):
+    ctrl = self._make()
+    cs = FakeCarState(brake_pressed=False)
+    for i in range(BRAKE_HOLD_ALLOWED_TIMER):
+      ctrl.update(cs, i, None)
+      self.assertFalse(ctrl.active)
+
+  def test_engages_after_timer_once_brake_released(self, mock_create):
+    ctrl = self._make()
+    cs = FakeCarState(brake_pressed=False)
+    for i in range(BRAKE_HOLD_ALLOWED_TIMER + 1):
+      ctrl.update(cs, i, None)
+    self.assertTrue(ctrl.active)
+
+  def test_brake_still_down_from_the_stop_does_not_block_engagement(self, mock_create):
+    # the driver's foot is normally still on the brake on the very frame standstill is reached -
+    # that must not count as a "fresh press" release, or the feature could never engage
+    ctrl = self._make()
+    # decelerating into the stop with the brake held continuously
+    cs = FakeCarState(standstill=False, brake_pressed=True)
+    for i in range(30):
+      ctrl.update(cs, i, None)
+    # reaches standstill, foot stays down for a while, then lifts
+    cs.out.standstill = True
+    for i in range(30, 50):
+      ctrl.update(cs, i, None)
+      self.assertFalse(ctrl.active, "must not engage while the original stopping press is still held")
+    cs.out.brakePressed = False
+    for i in range(50, 50 + BRAKE_HOLD_ALLOWED_TIMER + 1):
+      ctrl.update(cs, i, None)
+    self.assertTrue(ctrl.active, "should engage once the stopping press is released and the timer elapses")
+
+  def test_fresh_brake_press_mid_hold_releases_and_stays_released(self, mock_create):
+    ctrl = self._make()
+    cs = FakeCarState(brake_pressed=False)
+    frame = 0
+    for _ in range(BRAKE_HOLD_ALLOWED_TIMER + 1):
+      ctrl.update(cs, frame, None)
+      frame += 1
+    self.assertTrue(ctrl.active)
+
+    cs.out.brakePressed = True
+    ctrl.update(cs, frame, None)
+    frame += 1
+    self.assertFalse(ctrl.active, "a fresh press should release immediately")
+
+    for _ in range(20):
+      ctrl.update(cs, frame, None)
+      frame += 1
+      self.assertFalse(ctrl.active, "must stay released for the rest of the episode while continuously held")
+
+    cs.out.brakePressed = False
+    for _ in range(20):
+      ctrl.update(cs, frame, None)
+      frame += 1
+      self.assertFalse(ctrl.active, "must stay released for the rest of the episode even after lifting off again")
+
+  def test_drive_off_and_restop_rearms(self, mock_create):
+    ctrl = self._make()
+    cs = FakeCarState(brake_pressed=False)
+    frame = 0
+    for _ in range(BRAKE_HOLD_ALLOWED_TIMER + 1):
+      ctrl.update(cs, frame, None)
+      frame += 1
+    cs.out.brakePressed = True
+    ctrl.update(cs, frame, None)
+    frame += 1
+    self.assertFalse(ctrl.active)
+
+    # drives off - leaves the standstill episode entirely
+    cs.out.standstill = False
+    ctrl.update(cs, frame, None)
+    frame += 1
+
+    # stops again
+    cs.out.standstill = True
+    cs.out.brakePressed = False
+    for _ in range(BRAKE_HOLD_ALLOWED_TIMER + 1):
+      ctrl.update(cs, frame, None)
+      frame += 1
+    self.assertTrue(ctrl.active, "should re-arm and engage again at the next stop")
+
+  def test_cruise_engaged_blocks_hold(self, mock_create):
+    ctrl = self._make()
+    cs = FakeCarState(cruise_enabled=True, brake_pressed=False)
+    for i in range(BRAKE_HOLD_ALLOWED_TIMER + 1):
+      ctrl.update(cs, i, None)
+    self.assertFalse(ctrl.active, "must not hold while ACC is engaged - that's the point of the constraint")
+
+  def test_gas_pressed_blocks_hold(self, mock_create):
+    ctrl = self._make()
+    cs = FakeCarState(gas_pressed=True, brake_pressed=False)
+    for i in range(BRAKE_HOLD_ALLOWED_TIMER + 1):
+      ctrl.update(cs, i, None)
+    self.assertFalse(ctrl.active)
+
+  def test_park_and_reverse_block_hold(self, mock_create):
+    for gear in (GearShifter.park, GearShifter.reverse):
+      with self.subTest(gear=gear):
+        ctrl = self._make()
+        cs = FakeCarState(gear=gear, brake_pressed=False)
+        for i in range(BRAKE_HOLD_ALLOWED_TIMER + 1):
+          ctrl.update(cs, i, None)
+        self.assertFalse(ctrl.active)
+
+  def test_yields_to_live_pcs_without_dropping_active_state(self, mock_create):
+    ctrl = self._make()
+    cs = FakeCarState(brake_pressed=False)
+    frame = 0
+    for _ in range(BRAKE_HOLD_ALLOWED_TIMER + 1):
+      ctrl.update(cs, frame, None)
+      frame += 1
+    self.assertTrue(ctrl.active)
+    mock_create.reset_mock()
+
+    # a real PCS event shows up on the live signal
+    cs.pre_collision_2 = {"PCSALM": 1}
+    # advance to the next even frame the message is actually built on
+    while frame % 2 != 0:
+      frame += 1
+    ctrl.update(cs, frame, None)
+    override_arg = mock_create.call_args.args[-1]
+    self.assertTrue(ctrl.active, "internal hold state should not be cleared by a live PCS event")
+    self.assertFalse(override_arg, "must not override PRE_COLLISION_2 while PCS is genuinely active")
+
+    # once PCS goes quiet again, override resumes on our own signal, not stale PCS state
+    frame += 2
+    cs.pre_collision_2 = {}
+    ctrl.update(cs, frame, None)
+    override_arg = mock_create.call_args.args[-1]
+    self.assertTrue(override_arg)
+
+  def test_message_only_built_every_other_frame(self, mock_create):
+    ctrl = self._make()
+    cs = FakeCarState(brake_pressed=False)
+    for i in range(10):
+      ctrl.update(cs, i, None)
+    self.assertEqual(mock_create.call_count, 5)
+
+  def test_no_message_sent_outside_relay_blocked_window(self, mock_create):
+    # outside relay_blocked, toyota_fwd_hook lets the real PRE_COLLISION_2 relay through on its own -
+    # our passthrough copy would just be redundant traffic panda rejects, so it must not be sent at all
+    cases = [
+      dict(cruise_enabled=True),
+      dict(gas_pressed=True),
+      dict(cruise_available=False),
+    ]
+    for kwargs in cases:
+      with self.subTest(kwargs=kwargs):
+        ctrl = self._make()
+        cs = FakeCarState(brake_pressed=False, **kwargs)
+        for i in range(10):
+          ctrl.update(cs, i, None)
+        mock_create.assert_not_called()
+
+  def test_message_still_sent_in_park_or_reverse(self, mock_create):
+    # relay_blocked has no gear check, matching toyota_fwd_hook - park/reverse must not open a gap
+    # in the PRE_COLLISION_2 relay even though hold can never engage there (this was a real
+    # regression: gating the send on hold_allowed's gear check left bus 0 with nothing at this
+    # address while parked, which is what tripped a genuine PCS dash fault on-road)
+    for gear in (GearShifter.park, GearShifter.reverse):
+      with self.subTest(gear=gear):
+        ctrl = self._make()
+        cs = FakeCarState(gear=gear, brake_pressed=False)
+        for i in range(BRAKE_HOLD_ALLOWED_TIMER + 1):
+          ctrl.update(cs, i, None)
+        self.assertFalse(ctrl.active, "must never actually hold in park/reverse")
+        self.assertGreater(mock_create.call_count, 0, "must still relay PRE_COLLISION_2 in park/reverse")
+        override_arg = mock_create.call_args.args[-1]
+        self.assertFalse(override_arg, "never override while gear disallows an actual hold")
+
+
+class TestPcsIsActive(unittest.TestCase):
+  def test_all_zero_is_not_active(self):
+    self.assertFalse(pcs_is_active({}))
+    self.assertFalse(pcs_is_active({"PCSALM": 0, "DSS1GDRV": 0}))
+
+  def test_any_trigger_field_is_active(self):
+    for field in ("PCSALM", "IBTRGR", "PBATRGR", "PREFILL", "AVSTRGR", "PBRTRGR", "PPTRGR"):
+      with self.subTest(field=field):
+        self.assertTrue(pcs_is_active({field: 1}))
+
+  def test_nonzero_force_signal_is_active(self):
+    self.assertTrue(pcs_is_active({"DSS1GDRV": -5}))
+    self.assertFalse(pcs_is_active({"DSS1GDRV": 0}))
+
+
+if __name__ == "__main__":
+  unittest.main()
@@ -0,0 +1,171 @@
+import numpy as np
+
+from opendbc.car import rate_limit, DT_CTRL
+from opendbc.sunnypilot.car.toyota.brake_onset import BrakeOnsetShaper, ONSET_J_DOWN, HARD_BRAKE_ACCEL
+
+DT = DT_CTRL * 3
+STOCK_J = 4.0
+UP_STEP = 4.0 * DT
+
+
+def run(requests, a0=0.0, fcw=False):
+  shaper = BrakeOnsetShaper(DT, STOCK_J)
+  a = a0
+  out = []
+  for req in requests:
+    step = shaper.down_step(req, a, bypass=shaper.is_urgent(req, fcw))
+    a = rate_limit(req, a, step, UP_STEP)
+    out.append(a)
+  return np.array(out)
+
+
+def at(out, t):
+  return out[int(round(t / DT)) - 1]
+
+
+class TestBrakeOnset:
+  def test_step_brake_eases_in_over_the_first_second(self):
+    out = run([-1.5] * 60)
+    assert at(out, 0.09) > -0.06  # first 0.15 s: barely anything
+    assert at(out, 0.21) > -0.12  # 0.2 s: still very light
+    assert -0.45 < at(out, 0.36) < -0.10  # ~0.35 s: building slowly
+    assert -0.60 < at(out, 0.60) < -0.15  # 0.6 s: still gentle, about a fifth of the way
+    assert -1.10 < at(out, 1.00) < -0.55  # 1.0 s: about half way, the ramp is now running up
+    assert np.isclose(at(out, 1.5), -1.5, atol=1e-6)  # settled once the ramp reaches the stock limit
+
+  def test_jerk_follows_the_schedule_and_never_exceeds_stock(self):
+    out = run([-1.9] * 60, a0=1.0)  # large step, still above the hard-brake bypass
+    jerks = np.diff(np.concatenate([[1.0], out])) / DT
+    assert jerks.min() >= -STOCK_J - 1e-6
+    assert jerks[0] >= -ONSET_J_DOWN[0] - 1e-6
+    assert jerks[int(0.09 / DT)] >= -ONSET_J_DOWN[1] - 1e-6
+    # the blend is one straight ramp: jerk keeps growing monotonically until the stock limit
+    ramp = -jerks[int(0.1 / DT):int(0.6 / DT)]
+    assert np.all(np.diff(ramp) >= -1e-6)
+
+  def test_lead_switch_during_a_settled_brake_gets_the_same_soft_onset(self):
+    reqs = [-0.6] * 70 + [-1.8] * 60  # long enough for the schedule to wind fully back before the switch
+    out = run(reqs)
+    t_switch = 70 * DT
+    assert np.isclose(at(out, t_switch), -0.6, atol=1e-6)
+    assert at(out, t_switch + 0.09) > -0.6 - 0.05
+    assert at(out, t_switch + 0.21) > -0.6 - 0.25
+    assert np.isclose(out[-1], -1.8, atol=1e-6)
+
+  def test_hard_brake_request_bypasses_the_schedule(self):
+    out = run([HARD_BRAKE_ACCEL - 0.5] * 20)
+    assert np.isclose(out[0], -STOCK_J * DT, atol=1e-6)
+    assert at(out, 0.3) <= -1.1
+
+  def test_fcw_bypasses_the_schedule(self):
+    out = run([-1.5] * 20, fcw=True)
+    assert np.isclose(out[0], -STOCK_J * DT, atol=1e-6)
+
+  def test_release_is_untouched(self):
+    out = run([1.0] * 20, a0=-1.5)
+    assert np.isclose(out[0], -1.5 + UP_STEP, atol=1e-6)
+
+  def test_gentle_request_passes_straight_through(self):
+    reqs = list(np.linspace(0.0, -0.2, 40))  # ~0.17 m/s^3, below the gentlest onset limit (0.25)
+    out = run(reqs)
+    assert np.allclose(out, reqs, atol=1e-9)
+
+  def test_short_pause_does_not_fully_rearm(self):
+    shaper = BrakeOnsetShaper(DT, STOCK_J)
+    a = 0.0
+    for _ in range(int(0.3 / DT)):
+      a = rate_limit(-1.5, a, shaper.down_step(-1.5, a), UP_STEP)
+    t_after_ramp = shaper.t_onset
+    for _ in range(2):  # hold for two frames, request steady
+      shaper.down_step(a, a)
+    assert 0.0 < shaper.t_onset < t_after_ramp
+    for _ in range(int(1.0 / DT)):  # a full second of settled brake winds it back completely
+      shaper.down_step(a, a)
+    assert shaper.t_onset == 0.0
+
+
+class TestEngageBrake:
+  """2026-10-04: SET- while creeping up to a lead: the first 0.2 s after engaging is a light touch."""
+  def test_first_0_2_s_after_engaging_is_light_then_blends_to_stock(self):
+    from opendbc.sunnypilot.car.toyota.brake_onset import BrakeOnsetShaper, EngageOnsetShaper, ENGAGE_BRAKE_T_BP
+    dt = 0.03
+    shaper, engage = BrakeOnsetShaper(dt, 4.0), EngageOnsetShaper(dt, 4.0)
+    a, t, trace = 0.0, 0.0, {}
+    while t < 1.6:
+      engage.up_step(True)
+      step = shaper.down_step(-1.65, a, v_ego=2.5, t_engaged=engage.t_since_engage)
+      a = max(-1.65, a + step)
+      t += dt
+      for mark in (0.2, 0.5, 1.5):
+        if mark not in trace and t >= mark:
+          trace[mark] = a
+    assert trace[0.2] > -0.12          # light first touch (at creep speed the creep onset schedule is the stricter one)
+    assert -1.0 < trace[0.5] < -0.15   # blending in
+    assert trace[1.5] < -1.4           # the planner's request has arrived (end jerk 2.0 m/s^3 below 60 km/h)
+    assert ENGAGE_BRAKE_T_BP[-1] <= 1.0
+
+
+class TestCreepOnset:
+  """2026-10-04: a brake onset at creep speed (re-stop behind a lead) is as light as the engage schedule."""
+  def test_below_10kph_first_0_2_s_is_light(self):
+    from opendbc.sunnypilot.car.toyota.brake_onset import BrakeOnsetShaper
+    dt = 0.03
+    for v, light_limit in ((1.0, -0.15), (30.0, -0.45)):   # creep: barely anything by 0.2 s; highway: the normal onset
+      shaper = BrakeOnsetShaper(dt, 4.0)
+      a, t, at_02 = 0.0, 0.0, None
+      while t < 1.2:
+        a = max(-1.0, a + shaper.down_step(-1.0, a, v_ego=v))
+        t += dt
+        if at_02 is None and t >= 0.2:
+          at_02 = a
+      assert at_02 > light_limit, (v, at_02)
+      assert a < -0.9, (v, a)   # the full request arrives within the window at both speeds
+
+
+class TestCreepGasRelease:
+  """route 0000010d 23:56:59: the drop from a +0.5 launch request was shaped like a brake onset"""
+  def _shaper(self):
+    from opendbc.sunnypilot.car.toyota.brake_onset import BrakeOnsetShaper
+    return BrakeOnsetShaper(DT_CTRL * 3, 4.0)
+
+  def test_positive_part_goes_at_the_stock_rate_below_8kph(self):
+    sh = self._shaper()
+    step = sh.down_step(-0.24, 0.5, v_ego=2.0 / 3.6)
+    assert abs(step - (-4.0 * DT_CTRL * 3)) < 1e-9
+    step = sh.down_step(-0.24, 0.05, v_ego=2.0 / 3.6)
+    assert abs(step - (-0.05)) < 1e-9            # stops at zero; the braking starts from there
+
+  def test_faster_keeps_the_soft_onset(self):
+    step = self._shaper().down_step(-0.24, 0.5, v_ego=20.0 / 3.6)
+    assert step > -4.0 * DT_CTRL * 3 + 1e-9
+
+
+class TestResumeSoftStart:
+  """RES while coasting at 46 km/h: start from the coasting decel, 0.6 m/s^3 for 0.4 s, stock by 0.8 s"""
+  def _run(self, a_ego=-0.45, v=46.0, req=0.4, edge=True, n=40):
+    from opendbc.sunnypilot.car.toyota.brake_onset import EngageOnsetShaper
+    sh = EngageOnsetShaper(DT, 4.0)
+    sh.cruise_state(False)
+    sh.cruise_state(True if edge else False)
+    if not edge:
+      for _ in range(30):
+        sh.cruise_state(True)
+    prev, out = 0.0, []
+    for _ in range(n):
+      prev = sh.start_accel(prev, a_ego, v / 3.6)
+      prev = rate_limit(req, prev, -4.0 * DT, sh.up_step(True))
+      out.append(prev)
+    return np.array(out)
+
+  def test_gentle_first_0_4s_then_stock(self):
+    out = self._run()
+    assert abs(out[0] - (-0.45 + 0.6 * DT)) < 1e-9          # starts from the coasting decel
+    assert at(out, 0.39) < -0.45 + 0.6 * 0.4 + 0.02         # 0.6 m/s^3 for the first 0.4 s
+    assert np.isclose(out[-1], 0.4)                          # reaches the plan
+
+  def test_not_at_low_speed_or_a_gas_override_release(self):
+    assert self._run(v=10.0)[0] == 4.0 * DT
+    assert self._run(edge=False)[0] == 4.0 * DT
+
+  def test_a_positive_a_ego_starts_from_zero(self):
+    assert abs(self._run(a_ego=0.3)[0] - 0.6 * DT) < 1e-9
@@ -0,0 +1,105 @@
+import unittest
+
+from opendbc.sunnypilot.car.toyota.brake_onset import (BrakeOvershootLimiter, OVERSHOOT_CMD_CEIL, OVERSHOOT_DEADBAND,
+                                                       OVERSHOOT_MAX, OVERSHOOT_MOD_DEADBAND, OVERSHOOT_MOD_KEEP,
+                                                       OVERSHOOT_MOD_RATE)
+
+DT = 0.03
+
+
+class TestBrakeOvershootLimiter(unittest.TestCase):
+  def run_steady(self, req, a_ego, n=200, active=True):
+    lim = BrakeOvershootLimiter(DT)
+    for _ in range(n):
+      lim.update(req, a_ego, active)
+    return lim
+
+  def test_moderate_braking_is_untouched(self):
+    self.assertEqual(self.run_steady(-2.0, -3.0).lift, 0.0)
+
+  def test_overshoot_inside_the_deadband_is_untouched(self):
+    self.assertEqual(self.run_steady(-3.4, -3.4 - OVERSHOOT_DEADBAND + 0.01).lift, 0.0)
+
+  def test_19_41_overshoot_is_taken_back(self):
+    lim = self.run_steady(-3.4, -4.3)
+    self.assertAlmostEqual(lim.lift, 0.8 * (0.9 - OVERSHOOT_DEADBAND), places=6)
+    self.assertAlmostEqual(lim.apply(-3.4), -3.4 + lim.lift, places=6)
+
+  def test_under_braking_never_lifts(self):
+    self.assertEqual(self.run_steady(-3.4, -2.5).lift, 0.0)
+
+  def test_lift_is_capped_and_command_stays_a_firm_brake(self):
+    lim = self.run_steady(-2.6, -6.0)
+    self.assertEqual(lim.lift, OVERSHOOT_MAX)
+    self.assertAlmostEqual(lim.apply(-2.6), -2.6 + OVERSHOOT_MAX)
+    self.assertEqual(lim.apply(-2.2), OVERSHOOT_CMD_CEIL)   # a PID-lightened command is not lifted past a firm brake
+
+  def test_lift_grows_and_fades_gradually(self):
+    lim = BrakeOvershootLimiter(DT)
+    prev = 0.0
+    for _ in range(10):
+      lift = lim.update(-3.4, -5.0)
+      self.assertLessEqual(lift - prev, 3.0 * DT + 1e-9)
+      prev = lift
+    for _ in range(10):
+      lift = lim.update(-1.0, -1.0)   # the hard request ends
+      self.assertLessEqual(prev - lift, 2.0 * DT + 1e-9)
+      prev = lift
+
+  def test_inactive_gives_it_back(self):
+    lim = self.run_steady(-3.4, -4.3)
+    for _ in range(100):
+      lim.update(-3.4, -4.3, active=False)
+    self.assertEqual(lim.lift, 0.0)
+
+
+class TestModerateBand(unittest.TestCase):
+  """Altis Hybrid only: route 0000010e 17:38:41, asked -1.60 at 40-50 km/h, the car delivered -1.95."""
+  def run_steady(self, req, a_ego, v_kph, moderate=True, n=200):
+    lim = BrakeOvershootLimiter(DT, moderate=moderate)
+    for _ in range(n):
+      lim.update(req, a_ego, True, v_ego=v_kph / 3.6)
+    return lim
+
+  def test_17_38_41_overshoot_is_taken_back(self):
+    lim = self.run_steady(-1.60, -1.95, 45.0)
+    self.assertAlmostEqual(lim.lift, 0.8 * (0.35 - OVERSHOOT_MOD_DEADBAND), places=4)  # low-pass converged
+    self.assertAlmostEqual(lim.apply(-1.60), -1.60 + lim.lift, places=6)
+
+  def test_never_lighter_than_three_quarters_of_the_command(self):
+    lim = self.run_steady(-1.2, -3.0, 45.0)
+    self.assertAlmostEqual(lim.apply(-1.2), OVERSHOOT_MOD_KEEP * -1.2, places=6)
+
+  def test_off_for_other_cars_low_speed_and_light_requests(self):
+    self.assertEqual(self.run_steady(-1.60, -1.95, 45.0, moderate=False).lift, 0.0)
+    self.assertEqual(self.run_steady(-1.60, -1.95, 10.0).lift, 0.0)
+    self.assertEqual(self.run_steady(-0.9, -1.4, 45.0).lift, 0.0)
+    self.assertEqual(self.run_steady(-1.60, -1.70, 45.0).lift, 0.0)  # inside the deadband
+
+  def test_hard_band_unchanged_with_moderate_on(self):
+    lim = self.run_steady(-3.4, -4.3, 45.0)
+    self.assertAlmostEqual(lim.lift, 0.8 * (0.9 - OVERSHOOT_DEADBAND), places=6)
+    self.assertAlmostEqual(lim.apply(-3.4), -3.4 + lim.lift, places=6)
+
+  def test_moderate_band_is_a_slow_smooth_trim(self):
+    lim = BrakeOvershootLimiter(DT, moderate=True)
+    prev = 0.0
+    for k in range(100):
+      a = -1.95 if k % 2 else -1.45   # noisy aEgo around -1.70 at a steady -1.60 request
+      lift = lim.update(-1.60, a, True, v_ego=45 / 3.6, a_ego=a)
+      self.assertLessEqual(abs(lift - prev), OVERSHOOT_MOD_RATE * DT + 1e-9)
+      prev = lift
+    self.assertEqual(lift, 0.0)   # averaged overshoot 0.10 is inside the deadband
+
+  def test_moderate_band_only_in_relaxed(self):
+    lim = BrakeOvershootLimiter(DT, moderate=True)
+    for _ in range(200):
+      lim.update(-1.60, -1.95, True, v_ego=45 / 3.6, moderate_allowed=False)
+    self.assertEqual(lim.lift, 0.0)
+    for _ in range(200):
+      lim.update(-3.4, -4.3, True, v_ego=45 / 3.6, moderate_allowed=False)   # the hard band in every personality
+    self.assertGreater(lim.lift, 0.5)
+
+
+if __name__ == "__main__":
+  unittest.main()
@@ -0,0 +1,66 @@
+import unittest
+from types import SimpleNamespace
+
+from opendbc.sunnypilot.car.toyota.brake_onset import (BrakeHandoverFeedforward, HANDOVER_EXTRA_MAX,
+                                                       HANDOVER_EXTRA_FRAC, HANDOVER_RATE, PID_BLEED_NEG, PID_BLEED_POS)
+
+DT = 0.03
+class TestHandoverFeedforward(unittest.TestCase):
+  def run_steady(self, req, v_kph, n=200):
+    c = BrakeHandoverFeedforward(DT)
+    for _ in range(n):
+      c.update(req, v_kph / 3.6)
+    return c
+
+  def test_full_extra_in_the_dip(self):
+    c = self.run_steady(-0.84, 12.0)   # route 0000010e: asked -0.84 at 12-9 km/h, the car gave -0.61
+    self.assertAlmostEqual(c.extra, HANDOVER_EXTRA_MAX, places=9)
+    self.assertAlmostEqual(c.apply(-0.84), -0.84 - HANDOVER_EXTRA_MAX, places=9)
+
+  def test_lighter_request_gets_a_quarter(self):
+    self.assertAlmostEqual(self.run_steady(-0.5, 12.0).extra, HANDOVER_EXTRA_FRAC * 0.5, places=9)
+
+  def test_fades_at_the_edges(self):
+    self.assertAlmostEqual(self.run_steady(-1.0, 8.0).extra, 0.5 * HANDOVER_EXTRA_MAX, places=6)
+    self.assertEqual(self.run_steady(-1.0, 5.0).extra, 0.0)
+    self.assertEqual(self.run_steady(-1.0, 20.0).extra, 0.0)
+    self.assertEqual(self.run_steady(-0.25, 12.0).extra, 0.0)
+
+  def test_fades_out_for_a_firm_request(self):
+    self.assertEqual(self.run_steady(-1.5, 12.0).extra, 0.0)   # 0000010d: at -1.46 the car delivered -1.72
+    self.assertAlmostEqual(self.run_steady(-1.25, 12.0).extra, 0.5 * HANDOVER_EXTRA_MAX, places=6)
+
+  def test_gradual(self):
+    c = BrakeHandoverFeedforward(DT)
+    prev = 0.0
+    for _ in range(50):
+      e = c.update(-1.0, 12 / 3.6)
+      self.assertLessEqual(e - prev, HANDOVER_RATE * DT + 1e-9)
+      prev = e
+
+class TestPidHold(unittest.TestCase):
+  def test_braking_integral_bleeds_and_freezes_below_9kph(self):
+    c = BrakeHandoverFeedforward(DT)
+    pid = SimpleNamespace(i=-0.15)
+    self.assertTrue(c.condition_pid(pid, -0.6, 5 / 3.6))
+    self.assertAlmostEqual(pid.i, -0.15 + PID_BLEED_NEG * DT, places=9)
+    for _ in range(100):
+      c.condition_pid(pid, -0.6, 5 / 3.6)
+    self.assertEqual(pid.i, 0.0)
+
+  def test_gas_integral_left_from_a_launch_bleeds_fast(self):
+    c = BrakeHandoverFeedforward(DT)
+    pid = SimpleNamespace(i=0.3)
+    c.condition_pid(pid, -0.2, 3 / 3.6)
+    self.assertAlmostEqual(pid.i, 0.3 - PID_BLEED_POS * DT, places=9)
+
+  def test_untouched_above_9kph_or_when_not_braking(self):
+    c = BrakeHandoverFeedforward(DT)
+    pid = SimpleNamespace(i=-0.15)
+    self.assertFalse(c.condition_pid(pid, -0.6, 12 / 3.6))
+    self.assertFalse(c.condition_pid(pid, 0.3, 3 / 3.6))
+    self.assertEqual(pid.i, -0.15)
+
+
+if __name__ == "__main__":
+  unittest.main()
@@ -0,0 +1,15 @@
+import unittest
+
+from opendbc.car.toyota.carstate import AccelPersonality, get_accel_personality
+
+
+class TestDriveMode(unittest.TestCase):
+  def test_acceleration_profile_mapping(self):
+    self.assertEqual(get_accel_personality(0, 0), AccelPersonality.normal)
+    self.assertEqual(get_accel_personality(0, 1), AccelPersonality.eco)
+    self.assertEqual(get_accel_personality(1, 0), AccelPersonality.sport)
+    self.assertEqual(get_accel_personality(1, 1), AccelPersonality.sport)
+
+
+if __name__ == "__main__":
+  unittest.main()
@@ -0,0 +1,57 @@
+import unittest
+from types import SimpleNamespace
+
+from opendbc.car import structs
+from opendbc.car.toyota.carcontroller import get_long_tune
+from opendbc.car.toyota.values import CAR, ToyotaFlags
+from opendbc.sunnypilot.car.interfaces import _initialize_toyota
+from opendbc.sunnypilot.car.toyota.values import ToyotaFlagsSP
+
+
+def build_params(*, tss2=True):
+  flags = ToyotaFlags.TSS2.value if tss2 else 0
+  CP = structs.CarParams(
+    brand="toyota",
+    carFingerprint=str(CAR.TOYOTA_COROLLA_TSS2),
+    flags=flags,
+  )
+  return CP, structs.CarParamsSP()
+
+
+class TestTss2LongTuning(unittest.TestCase):
+  def test_param_handoff(self):
+    for enabled in (False, True):
+      with self.subTest(enabled=enabled):
+        CP, CP_SP = build_params()
+        _initialize_toyota(CP, CP_SP, {"ToyotaTSS2Long": int(enabled)})
+        self.assertEqual(bool(CP_SP.flags & ToyotaFlagsSP.TSS2_LONG_TUNING), enabled)
+
+  def test_tss2_tune_selection(self):
+    controller_params = SimpleNamespace(ACCEL_MAX=2.0, ACCEL_MIN=-3.5)
+    CP, CP_SP = build_params()
+
+    stock_tune = get_long_tune(CP, CP_SP, controller_params)
+    stock_tune.speed = 2.0
+    self.assertEqual(stock_tune.k_i, 0.5)
+    stock_tune.speed = 5.0
+    self.assertEqual(stock_tune.k_i, 0.25)
+
+    CP_SP.flags |= ToyotaFlagsSP.TSS2_LONG_TUNING.value
+    custom_tune = get_long_tune(CP, CP_SP, controller_params)
+    custom_tune.speed = 0.0
+    self.assertEqual(custom_tune.k_i, 0.30)
+    custom_tune.speed = 5.0
+    self.assertEqual(custom_tune.k_i, 0.28)
+
+  def test_non_tss2_ignores_custom_tune_flag(self):
+    controller_params = SimpleNamespace(ACCEL_MAX=2.0, ACCEL_MIN=-3.5)
+    CP, CP_SP = build_params(tss2=False)
+    CP_SP.flags |= ToyotaFlagsSP.TSS2_LONG_TUNING.value
+
+    standard_tune = get_long_tune(CP, CP_SP, controller_params)
+    standard_tune.speed = 0.0
+    self.assertEqual(standard_tune.k_i, 3.6)
+
+
+if __name__ == "__main__":
+  unittest.main()
@@ -14,6 +14,11 @@ class ToyotaFlagsSP(IntFlag):
  ZSS = 4
  STOCK_LONGITUDINAL = 8
  STOP_AND_GO_HACK = 16
+  SP_ENHANCED_BSM = 32
+  SP_NEED_DEBUG_BSM = 64
+  SP_AUTO_BRAKE_HOLD = 128
+  SP_BRAKE_ONSET = 256
+  TSS2_LONG_TUNING = 512


 class ToyotaSafetyFlagsSP:
@@ -204,11 +204,16 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
  aTarget @5 :Float32;
  events @6 :List(OnroadEventSP.Event);
  e2eAlerts @7 :E2eAlerts;
+  accelController @8 :AccelController;

  struct DynamicExperimentalControl {
    state @0 :DynamicExperimentalControlState;
    enabled @1 :Bool;
    active @2 :Bool;
+    decelIntent @3 :Float32;
+    curveDetected @4 :Bool;
+    wantBlended @5 :Bool;
+    leadVeto @6 :Bool;

    enum DynamicExperimentalControlState {
      acc @0;
@@ -306,6 +311,17 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
    greenLightAlert @0 :Bool;
    leadDepartAlert @1 :Bool;
  }
+
+  struct AccelController {
+    enabled @0 :Bool;
+    active @1 :Bool;
+    profile @2 :Profile;
+    enum Profile {
+      eco @0;
+      normal @1;
+      sport @2;
+    }
+  }
 }

 struct OnroadEventSP @0xda96579883444c35 {
@@ -447,6 +463,8 @@ struct BackupManagerSP @0xf98d843bfd7004a3 {

 struct CarStateSP @0xb86e6369214c01c8 {
  speedLimit @0 :Float32;
+  engineOff @1 :Bool;
+  engineRpm @2 :Float32;
 }

 struct LiveMapDataSP @0xf416ec09499d9d19 {
@@ -1897,34 +1897,36 @@ const ::capnp::_::RawSchema s_e60821c0505ad473 = {
  4, 12, i_e60821c0505ad473, nullptr, nullptr, { &s_e60821c0505ad473, nullptr, nullptr, 0, 0, nullptr }, false
 };
 #endif  // !CAPNP_LITE
-static const ::capnp::_::AlignedData<172> b_f35cc4560bbf6ec2 = {
+static const ::capnp::_::AlignedData<192> b_f35cc4560bbf6ec2 = {
  {   0,   0,   0,   0,   5,   0,   6,   0,
    194, 110, 191,  11,  86, 196,  92, 243,
     13,   0,   0,   0,   1,   0,   2,   0,
     89,  10,  85,  29, 102, 186,  38, 181,
-      5,   0,   7,   0,   0,   0,   0,   0,
+      6,   0,   7,   0,   0,   0,   0,   0,
      0,   0,   0,   0,   0,   0,   0,   0,
     21,   0,   0,   0,   2,   1,   0,   0,
-     33,   0,   0,   0,  87,   0,   0,   0,
+     33,   0,   0,   0, 103,   0,   0,   0,
      0,   0,   0,   0,   0,   0,   0,   0,
-    125,   0,   0,   0, 199,   1,   0,   0,
+    141,   0,   0,   0, 255,   1,   0,   0,
      0,   0,   0,   0,   0,   0,   0,   0,
      0,   0,   0,   0,   0,   0,   0,   0,
     99, 117, 115, 116, 111, 109,  46,  99,
     97, 112, 110, 112,  58,  76, 111, 110,
    103, 105, 116, 117, 100, 105, 110,  97,
    108,  80, 108,  97, 110,  83,  80,   0,
-     20,   0,   0,   0,   1,   0,   1,   0,
+     24,   0,   0,   0,   1,   0,   1,   0,
    119, 191,  14,  16, 174,  91, 106, 188,
-     33,   0,   0,   0, 218,   0,   0,   0,
+     41,   0,   0,   0, 218,   0,   0,   0,
    163,  23,  92,  92,  71, 153, 141, 193,
-     41,   0,   0,   0, 154,   0,   0,   0,
+     49,   0,   0,   0, 154,   0,   0,   0,
    187,  34, 168, 255,  81, 184, 158, 154,
-     45,   0,   0,   0,  90,   0,   0,   0,
+     53,   0,   0,   0,  90,   0,   0,   0,
    196, 110, 217,  86,   5,  68,  71, 173,
-     45,   0,   0,   0, 186,   0,   0,   0,
+     53,   0,   0,   0, 186,   0,   0,   0,
    254,  40, 174,  34, 232, 188, 103, 165,
-     49,   0,   0,   0,  82,   0,   0,   0,
+     57,   0,   0,   0,  82,   0,   0,   0,
+    161, 196, 243,   6, 208, 214, 220, 136,
+     57,   0,   0,   0, 130,   0,   0,   0,
     68, 121, 110,  97, 109, 105,  99,  69,
    120, 112, 101, 114, 105, 109, 101, 110,
    116,  97, 108,  67, 111, 110, 116, 114,
@@ -1939,63 +1941,72 @@ static const ::capnp::_::AlignedData<172> b_f35cc4560bbf6ec2 = {
     83, 111, 117, 114,  99, 101,   0,   0,
     69,  50, 101,  65, 108, 101, 114, 116,
    115,   0,   0,   0,   0,   0,   0,   0,
-     32,   0,   0,   0,   3,   0,   4,   0,
+     65,  99,  99, 101, 108,  67, 111, 110,
+    116, 114, 111, 108, 108, 101, 114,   0,
+     36,   0,   0,   0,   3,   0,   4,   0,
      0,   0,   0,   0,   0,   0,   0,   0,
      0,   0,   1,   0,   0,   0,   0,   0,
      0,   0,   0,   0,   0,   0,   0,   0,
-    209,   0,   0,   0,  34,   0,   0,   0,
+    237,   0,   0,   0,  34,   0,   0,   0,
      0,   0,   0,   0,   0,   0,   0,   0,
-    204,   0,   0,   0,   3,   0,   1,   0,
-    216,   0,   0,   0,   2,   0,   1,   0,
+    232,   0,   0,   0,   3,   0,   1,   0,
+    244,   0,   0,   0,   2,   0,   1,   0,
      1,   0,   0,   0,   0,   0,   0,   0,
      0,   0,   1,   0,   1,   0,   0,   0,
      0,   0,   0,   0,   0,   0,   0,   0,
-    213,   0,   0,   0, 186,   0,   0,   0,
-      0,   0,   0,   0,   0,   0,   0,   0,
-    216,   0,   0,   0,   3,   0,   1,   0,
-    228,   0,   0,   0,   2,   0,   1,   0,
-      2,   0,   0,   0,   1,   0,   0,   0,
-      0,   0,   1,   0,   2,   0,   0,   0,
-      0,   0,   0,   0,   0,   0,   0,   0,
-    225,   0,   0,   0, 154,   0,   0,   0,
-      0,   0,   0,   0,   0,   0,   0,   0,
-    228,   0,   0,   0,   3,   0,   1,   0,
-    240,   0,   0,   0,   2,   0,   1,   0,
-      3,   0,   0,   0,   2,   0,   0,   0,
-      0,   0,   1,   0,   3,   0,   0,   0,
-      0,   0,   0,   0,   0,   0,   0,   0,
-    237,   0,   0,   0,  90,   0,   0,   0,
-      0,   0,   0,   0,   0,   0,   0,   0,
-    236,   0,   0,   0,   3,   0,   1,   0,
-    248,   0,   0,   0,   2,   0,   1,   0,
-      4,   0,   0,   0,   1,   0,   0,   0,
-      0,   0,   1,   0,   4,   0,   0,   0,
-      0,   0,   0,   0,   0,   0,   0,   0,
-    245,   0,   0,   0,  66,   0,   0,   0,
-      0,   0,   0,   0,   0,   0,   0,   0,
-    240,   0,   0,   0,   3,   0,   1,   0,
-    252,   0,   0,   0,   2,   0,   1,   0,
-      5,   0,   0,   0,   2,   0,   0,   0,
-      0,   0,   1,   0,   5,   0,   0,   0,
-      0,   0,   0,   0,   0,   0,   0,   0,
-    249,   0,   0,   0,  66,   0,   0,   0,
+    241,   0,   0,   0, 186,   0,   0,   0,
      0,   0,   0,   0,   0,   0,   0,   0,
    244,   0,   0,   0,   3,   0,   1,   0,
      0,   1,   0,   0,   2,   0,   1,   0,
-      6,   0,   0,   0,   3,   0,   0,   0,
-      0,   0,   1,   0,   6,   0,   0,   0,
+      2,   0,   0,   0,   1,   0,   0,   0,
+      0,   0,   1,   0,   2,   0,   0,   0,
      0,   0,   0,   0,   0,   0,   0,   0,
-    253,   0,   0,   0,  58,   0,   0,   0,
+    253,   0,   0,   0, 154,   0,   0,   0,
      0,   0,   0,   0,   0,   0,   0,   0,
-    248,   0,   0,   0,   3,   0,   1,   0,
+      0,   1,   0,   0,   3,   0,   1,   0,
+     12,   1,   0,   0,   2,   0,   1,   0,
+      3,   0,   0,   0,   2,   0,   0,   0,
+      0,   0,   1,   0,   3,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+      9,   1,   0,   0,  90,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+      8,   1,   0,   0,   3,   0,   1,   0,
     20,   1,   0,   0,   2,   0,   1,   0,
-      7,   0,   0,   0,   4,   0,   0,   0,
-      0,   0,   1,   0,   7,   0,   0,   0,
+      4,   0,   0,   0,   1,   0,   0,   0,
+      0,   0,   1,   0,   4,   0,   0,   0,
      0,   0,   0,   0,   0,   0,   0,   0,
-     17,   1,   0,   0,  82,   0,   0,   0,
+     17,   1,   0,   0,  66,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+     12,   1,   0,   0,   3,   0,   1,   0,
+     24,   1,   0,   0,   2,   0,   1,   0,
+      5,   0,   0,   0,   2,   0,   0,   0,
+      0,   0,   1,   0,   5,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+     21,   1,   0,   0,  66,   0,   0,   0,
      0,   0,   0,   0,   0,   0,   0,   0,
     16,   1,   0,   0,   3,   0,   1,   0,
     28,   1,   0,   0,   2,   0,   1,   0,
+      6,   0,   0,   0,   3,   0,   0,   0,
+      0,   0,   1,   0,   6,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+     25,   1,   0,   0,  58,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+     20,   1,   0,   0,   3,   0,   1,   0,
+     48,   1,   0,   0,   2,   0,   1,   0,
+      7,   0,   0,   0,   4,   0,   0,   0,
+      0,   0,   1,   0,   7,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+     45,   1,   0,   0,  82,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+     44,   1,   0,   0,   3,   0,   1,   0,
+     56,   1,   0,   0,   2,   0,   1,   0,
+      8,   0,   0,   0,   5,   0,   0,   0,
+      0,   0,   1,   0,   8,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+     53,   1,   0,   0, 130,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+     52,   1,   0,   0,   3,   0,   1,   0,
+     64,   1,   0,   0,   2,   0,   1,   0,
    100, 101,  99,   0,   0,   0,   0,   0,
     16,   0,   0,   0,   0,   0,   0,   0,
    119, 191,  14,  16, 174,  91, 106, 188,
@@ -2067,6 +2078,15 @@ static const ::capnp::_::AlignedData<172> b_f35cc4560bbf6ec2 = {
    254,  40, 174,  34, 232, 188, 103, 165,
      0,   0,   0,   0,   0,   0,   0,   0,
      0,   0,   0,   0,   0,   0,   0,   0,
+     16,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+     97,  99,  99, 101, 108,  67, 111, 110,
+    116, 114, 111, 108, 108, 101, 114,   0,
+     16,   0,   0,   0,   0,   0,   0,   0,
+    161, 196, 243,   6, 208, 214, 220, 136,
+      0,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
     16,   0,   0,   0,   0,   0,   0,   0,
      0,   0,   0,   0,   0,   0,   0,   0,
      0,   0,   0,   0,   0,   0,   0,   0, }
@@ -2074,6 +2094,7 @@ static const ::capnp::_::AlignedData<172> b_f35cc4560bbf6ec2 = {
 ::capnp::word const* const bp_f35cc4560bbf6ec2 = b_f35cc4560bbf6ec2.words;
 #if !CAPNP_LITE
 static const ::capnp::_::RawSchema* const d_f35cc4560bbf6ec2[] = {
+  &s_88dcd6d006f3c4a1,
  &s_9a9eb851ffa822bb,
  &s_a567bce822ae28fe,
  &s_ad47440556d96ec4,
@@ -2081,14 +2102,14 @@ static const ::capnp::_::RawSchema* const d_f35cc4560bbf6ec2[] = {
  &s_c18d99475c5c17a3,
  &s_f6e831752fcdf793,
 };
-static const uint16_t m_f35cc4560bbf6ec2[] = {5, 0, 7, 6, 1, 2, 3, 4};
-static const uint16_t i_f35cc4560bbf6ec2[] = {0, 1, 2, 3, 4, 5, 6, 7};
+static const uint16_t m_f35cc4560bbf6ec2[] = {5, 8, 0, 7, 6, 1, 2, 3, 4};
+static const uint16_t i_f35cc4560bbf6ec2[] = {0, 1, 2, 3, 4, 5, 6, 7, 8};
 const ::capnp::_::RawSchema s_f35cc4560bbf6ec2 = {
-  0xf35cc4560bbf6ec2, b_f35cc4560bbf6ec2.words, 172, d_f35cc4560bbf6ec2, m_f35cc4560bbf6ec2,
-  6, 8, i_f35cc4560bbf6ec2, nullptr, nullptr, { &s_f35cc4560bbf6ec2, nullptr, nullptr, 0, 0, nullptr }, false
+  0xf35cc4560bbf6ec2, b_f35cc4560bbf6ec2.words, 192, d_f35cc4560bbf6ec2, m_f35cc4560bbf6ec2,
+  7, 9, i_f35cc4560bbf6ec2, nullptr, nullptr, { &s_f35cc4560bbf6ec2, nullptr, nullptr, 0, 0, nullptr }, false
 };
 #endif  // !CAPNP_LITE
-static const ::capnp::_::AlignedData<73> b_bc6a5bae100ebf77 = {
+static const ::capnp::_::AlignedData<137> b_bc6a5bae100ebf77 = {
  {   0,   0,   0,   0,   5,   0,   6,   0,
    119, 191,  14,  16, 174,  91, 106, 188,
     32,   0,   0,   0,   1,   0,   1,   0,
@@ -2098,7 +2119,7 @@ static const ::capnp::_::AlignedData<73> b_bc6a5bae100ebf77 = {
     21,   0,   0,   0, 218,   1,   0,   0,
     49,   0,   0,   0,  23,   0,   0,   0,
      0,   0,   0,   0,   0,   0,   0,   0,
-     69,   0,   0,   0, 175,   0,   0,   0,
+     69,   0,   0,   0, 143,   1,   0,   0,
      0,   0,   0,   0,   0,   0,   0,   0,
      0,   0,   0,   0,   0,   0,   0,   0,
     99, 117, 115, 116, 111, 109,  46,  99,
@@ -2116,28 +2137,56 @@ static const ::capnp::_::AlignedData<73> b_bc6a5bae100ebf77 = {
    120, 112, 101, 114, 105, 109, 101, 110,
    116,  97, 108,  67, 111, 110, 116, 114,
    111, 108,  83, 116,  97, 116, 101,   0,
-     12,   0,   0,   0,   3,   0,   4,   0,
+     28,   0,   0,   0,   3,   0,   4,   0,
      0,   0,   0,   0,   0,   0,   0,   0,
      0,   0,   1,   0,   0,   0,   0,   0,
      0,   0,   0,   0,   0,   0,   0,   0,
-     69,   0,   0,   0,  50,   0,   0,   0,
+    181,   0,   0,   0,  50,   0,   0,   0,
      0,   0,   0,   0,   0,   0,   0,   0,
-     64,   0,   0,   0,   3,   0,   1,   0,
-     76,   0,   0,   0,   2,   0,   1,   0,
+    176,   0,   0,   0,   3,   0,   1,   0,
+    188,   0,   0,   0,   2,   0,   1,   0,
      1,   0,   0,   0,  16,   0,   0,   0,
      0,   0,   1,   0,   1,   0,   0,   0,
      0,   0,   0,   0,   0,   0,   0,   0,
-     73,   0,   0,   0,  66,   0,   0,   0,
+    185,   0,   0,   0,  66,   0,   0,   0,
      0,   0,   0,   0,   0,   0,   0,   0,
-     68,   0,   0,   0,   3,   0,   1,   0,
-     80,   0,   0,   0,   2,   0,   1,   0,
+    180,   0,   0,   0,   3,   0,   1,   0,
+    192,   0,   0,   0,   2,   0,   1,   0,
      2,   0,   0,   0,  17,   0,   0,   0,
      0,   0,   1,   0,   2,   0,   0,   0,
      0,   0,   0,   0,   0,   0,   0,   0,
-     77,   0,   0,   0,  58,   0,   0,   0,
+    189,   0,   0,   0,  58,   0,   0,   0,
      0,   0,   0,   0,   0,   0,   0,   0,
-     72,   0,   0,   0,   3,   0,   1,   0,
-     84,   0,   0,   0,   2,   0,   1,   0,
+    184,   0,   0,   0,   3,   0,   1,   0,
+    196,   0,   0,   0,   2,   0,   1,   0,
+      3,   0,   0,   0,   1,   0,   0,   0,
+      0,   0,   1,   0,   3,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+    193,   0,   0,   0,  98,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+    192,   0,   0,   0,   3,   0,   1,   0,
+    204,   0,   0,   0,   2,   0,   1,   0,
+      4,   0,   0,   0,  18,   0,   0,   0,
+      0,   0,   1,   0,   4,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+    201,   0,   0,   0, 114,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+    200,   0,   0,   0,   3,   0,   1,   0,
+    212,   0,   0,   0,   2,   0,   1,   0,
+      5,   0,   0,   0,  19,   0,   0,   0,
+      0,   0,   1,   0,   5,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+    209,   0,   0,   0,  98,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+    208,   0,   0,   0,   3,   0,   1,   0,
+    220,   0,   0,   0,   2,   0,   1,   0,
+      6,   0,   0,   0,  20,   0,   0,   0,
+      0,   0,   1,   0,   6,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+    217,   0,   0,   0,  74,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+    216,   0,   0,   0,   3,   0,   1,   0,
+    228,   0,   0,   0,   2,   0,   1,   0,
    115, 116,  97, 116, 101,   0,   0,   0,
     15,   0,   0,   0,   0,   0,   0,   0,
     55,  64, 112, 160, 167, 242, 246, 216,
@@ -2155,6 +2204,42 @@ static const ::capnp::_::AlignedData<73> b_bc6a5bae100ebf77 = {
      0,   0,   0,   0,   0,   0,   0,   0,
      0,   0,   0,   0,   0,   0,   0,   0,
     97,  99, 116, 105, 118, 101,   0,   0,
+      1,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+      1,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+    100, 101,  99, 101, 108,  73, 110, 116,
+    101, 110, 116,   0,   0,   0,   0,   0,
+     10,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+     10,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+     99, 117, 114, 118, 101,  68, 101, 116,
+    101,  99, 116, 101, 100,   0,   0,   0,
+      1,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+      1,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+    119,  97, 110, 116,  66, 108, 101, 110,
+    100, 101, 100,   0,   0,   0,   0,   0,
+      1,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+      1,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+    108, 101,  97, 100,  86, 101, 116, 111,
+      0,   0,   0,   0,   0,   0,   0,   0,
      1,   0,   0,   0,   0,   0,   0,   0,
      0,   0,   0,   0,   0,   0,   0,   0,
      0,   0,   0,   0,   0,   0,   0,   0,
@@ -2168,11 +2253,11 @@ static const ::capnp::_::AlignedData<73> b_bc6a5bae100ebf77 = {
 static const ::capnp::_::RawSchema* const d_bc6a5bae100ebf77[] = {
  &s_d8f6f2a7a0704037,
 };
-static const uint16_t m_bc6a5bae100ebf77[] = {2, 1, 0};
-static const uint16_t i_bc6a5bae100ebf77[] = {0, 1, 2};
+static const uint16_t m_bc6a5bae100ebf77[] = {2, 4, 3, 1, 6, 0, 5};
+static const uint16_t i_bc6a5bae100ebf77[] = {0, 1, 2, 3, 4, 5, 6};
 const ::capnp::_::RawSchema s_bc6a5bae100ebf77 = {
-  0xbc6a5bae100ebf77, b_bc6a5bae100ebf77.words, 73, d_bc6a5bae100ebf77, m_bc6a5bae100ebf77,
-  1, 3, i_bc6a5bae100ebf77, nullptr, nullptr, { &s_bc6a5bae100ebf77, nullptr, nullptr, 0, 0, nullptr }, false
+  0xbc6a5bae100ebf77, b_bc6a5bae100ebf77.words, 137, d_bc6a5bae100ebf77, m_bc6a5bae100ebf77,
+  1, 7, i_bc6a5bae100ebf77, nullptr, nullptr, { &s_bc6a5bae100ebf77, nullptr, nullptr, 0, 0, nullptr }, false
 };
 #endif  // !CAPNP_LITE
 static const ::capnp::_::AlignedData<34> b_d8f6f2a7a0704037 = {
@@ -3257,6 +3342,132 @@ const ::capnp::_::RawSchema s_a567bce822ae28fe = {
  0, 2, i_a567bce822ae28fe, nullptr, nullptr, { &s_a567bce822ae28fe, nullptr, nullptr, 0, 0, nullptr }, false
 };
 #endif  // !CAPNP_LITE
+static const ::capnp::_::AlignedData<68> b_88dcd6d006f3c4a1 = {
+  {   0,   0,   0,   0,   5,   0,   6,   0,
+    161, 196, 243,   6, 208, 214, 220, 136,
+     32,   0,   0,   0,   1,   0,   1,   0,
+    194, 110, 191,  11,  86, 196,  92, 243,
+      0,   0,   7,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+     21,   0,   0,   0, 130,   1,   0,   0,
+     41,   0,   0,   0,  23,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+     49,   0,   0,   0, 175,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+     99, 117, 115, 116, 111, 109,  46,  99,
+     97, 112, 110, 112,  58,  76, 111, 110,
+    103, 105, 116, 117, 100, 105, 110,  97,
+    108,  80, 108,  97, 110,  83,  80,  46,
+     65,  99,  99, 101, 108,  67, 111, 110,
+    116, 114, 111, 108, 108, 101, 114,   0,
+      4,   0,   0,   0,   1,   0,   1,   0,
+    166,  80,  70, 176,  68, 149, 159, 140,
+      1,   0,   0,   0,  66,   0,   0,   0,
+     80, 114, 111, 102, 105, 108, 101,   0,
+     12,   0,   0,   0,   3,   0,   4,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   1,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+     69,   0,   0,   0,  66,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+     64,   0,   0,   0,   3,   0,   1,   0,
+     76,   0,   0,   0,   2,   0,   1,   0,
+      1,   0,   0,   0,   1,   0,   0,   0,
+      0,   0,   1,   0,   1,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+     73,   0,   0,   0,  58,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+     68,   0,   0,   0,   3,   0,   1,   0,
+     80,   0,   0,   0,   2,   0,   1,   0,
+      2,   0,   0,   0,   1,   0,   0,   0,
+      0,   0,   1,   0,   2,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+     77,   0,   0,   0,  66,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+     72,   0,   0,   0,   3,   0,   1,   0,
+     84,   0,   0,   0,   2,   0,   1,   0,
+    101, 110,  97,  98, 108, 101, 100,   0,
+      1,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+      1,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+     97,  99, 116, 105, 118, 101,   0,   0,
+      1,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+      1,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+    112, 114, 111, 102, 105, 108, 101,   0,
+     15,   0,   0,   0,   0,   0,   0,   0,
+    166,  80,  70, 176,  68, 149, 159, 140,
+      0,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+     15,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0, }
+};
+::capnp::word const* const bp_88dcd6d006f3c4a1 = b_88dcd6d006f3c4a1.words;
+#if !CAPNP_LITE
+static const ::capnp::_::RawSchema* const d_88dcd6d006f3c4a1[] = {
+  &s_8c9f9544b04650a6,
+};
+static const uint16_t m_88dcd6d006f3c4a1[] = {1, 0, 2};
+static const uint16_t i_88dcd6d006f3c4a1[] = {0, 1, 2};
+const ::capnp::_::RawSchema s_88dcd6d006f3c4a1 = {
+  0x88dcd6d006f3c4a1, b_88dcd6d006f3c4a1.words, 68, d_88dcd6d006f3c4a1, m_88dcd6d006f3c4a1,
+  1, 3, i_88dcd6d006f3c4a1, nullptr, nullptr, { &s_88dcd6d006f3c4a1, nullptr, nullptr, 0, 0, nullptr }, false
+};
+#endif  // !CAPNP_LITE
+static const ::capnp::_::AlignedData<33> b_8c9f9544b04650a6 = {
+  {   0,   0,   0,   0,   5,   0,   6,   0,
+    166,  80,  70, 176,  68, 149, 159, 140,
+     48,   0,   0,   0,   2,   0,   0,   0,
+    161, 196, 243,   6, 208, 214, 220, 136,
+      0,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+     21,   0,   0,   0, 194,   1,   0,   0,
+     45,   0,   0,   0,   7,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+     41,   0,   0,   0,  79,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+     99, 117, 115, 116, 111, 109,  46,  99,
+     97, 112, 110, 112,  58,  76, 111, 110,
+    103, 105, 116, 117, 100, 105, 110,  97,
+    108,  80, 108,  97, 110,  83,  80,  46,
+     65,  99,  99, 101, 108,  67, 111, 110,
+    116, 114, 111, 108, 108, 101, 114,  46,
+     80, 114, 111, 102, 105, 108, 101,   0,
+      0,   0,   0,   0,   1,   0,   1,   0,
+     12,   0,   0,   0,   1,   0,   2,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+     29,   0,   0,   0,  34,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+      1,   0,   0,   0,   0,   0,   0,   0,
+     21,   0,   0,   0,  58,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+      2,   0,   0,   0,   0,   0,   0,   0,
+     13,   0,   0,   0,  50,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+    101,  99, 111,   0,   0,   0,   0,   0,
+    110, 111, 114, 109,  97, 108,   0,   0,
+    115, 112, 111, 114, 116,   0,   0,   0, }
+};
+::capnp::word const* const bp_8c9f9544b04650a6 = b_8c9f9544b04650a6.words;
+#if !CAPNP_LITE
+static const uint16_t m_8c9f9544b04650a6[] = {0, 1, 2};
+const ::capnp::_::RawSchema s_8c9f9544b04650a6 = {
+  0x8c9f9544b04650a6, b_8c9f9544b04650a6.words, 33, nullptr, m_8c9f9544b04650a6,
+  0, 3, nullptr, nullptr, nullptr, { &s_8c9f9544b04650a6, nullptr, nullptr, 0, 0, nullptr }, false
+};
+#endif  // !CAPNP_LITE
+CAPNP_DEFINE_ENUM(Profile_8c9f9544b04650a6, 8c9f9544b04650a6);
 static const ::capnp::_::AlignedData<44> b_da96579883444c35 = {
  {   0,   0,   0,   0,   5,   0,   6,   0,
     53,  76,  68, 131, 152,  87, 150, 218,
@@ -4812,33 +5023,65 @@ const ::capnp::_::RawSchema s_9e62278160b7df26 = {
  2, 8, i_9e62278160b7df26, nullptr, nullptr, { &s_9e62278160b7df26, nullptr, nullptr, 0, 0, nullptr }, false
 };
 #endif  // !CAPNP_LITE
-static const ::capnp::_::AlignedData<33> b_b86e6369214c01c8 = {
+static const ::capnp::_::AlignedData<65> b_b86e6369214c01c8 = {
  {   0,   0,   0,   0,   5,   0,   6,   0,
    200,   1,  76,  33, 105,  99, 110, 184,
-     13,   0,   0,   0,   1,   0,   1,   0,
+     13,   0,   0,   0,   1,   0,   2,   0,
     89,  10,  85,  29, 102, 186,  38, 181,
      0,   0,   7,   0,   0,   0,   0,   0,
      0,   0,   0,   0,   0,   0,   0,   0,
     21,   0,   0,   0, 194,   0,   0,   0,
     29,   0,   0,   0,   7,   0,   0,   0,
      0,   0,   0,   0,   0,   0,   0,   0,
-     25,   0,   0,   0,  63,   0,   0,   0,
+     25,   0,   0,   0, 175,   0,   0,   0,
      0,   0,   0,   0,   0,   0,   0,   0,
      0,   0,   0,   0,   0,   0,   0,   0,
     99, 117, 115, 116, 111, 109,  46,  99,
     97, 112, 110, 112,  58,  67,  97, 114,
     83, 116,  97, 116, 101,  83,  80,   0,
      0,   0,   0,   0,   1,   0,   1,   0,
-      4,   0,   0,   0,   3,   0,   4,   0,
+     12,   0,   0,   0,   3,   0,   4,   0,
      0,   0,   0,   0,   0,   0,   0,   0,
      0,   0,   1,   0,   0,   0,   0,   0,
      0,   0,   0,   0,   0,   0,   0,   0,
-     13,   0,   0,   0,  90,   0,   0,   0,
+     69,   0,   0,   0,  90,   0,   0,   0,
      0,   0,   0,   0,   0,   0,   0,   0,
-     12,   0,   0,   0,   3,   0,   1,   0,
-     24,   0,   0,   0,   2,   0,   1,   0,
+     68,   0,   0,   0,   3,   0,   1,   0,
+     80,   0,   0,   0,   2,   0,   1,   0,
+      1,   0,   0,   0,  32,   0,   0,   0,
+      0,   0,   1,   0,   1,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+     77,   0,   0,   0,  82,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+     76,   0,   0,   0,   3,   0,   1,   0,
+     88,   0,   0,   0,   2,   0,   1,   0,
+      2,   0,   0,   0,   2,   0,   0,   0,
+      0,   0,   1,   0,   2,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+     85,   0,   0,   0,  82,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+     84,   0,   0,   0,   3,   0,   1,   0,
+     96,   0,   0,   0,   2,   0,   1,   0,
    115, 112, 101, 101, 100,  76, 105, 109,
    105, 116,   0,   0,   0,   0,   0,   0,
+     10,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+     10,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+    101, 110, 103, 105, 110, 101,  79, 102,
+    102,   0,   0,   0,   0,   0,   0,   0,
+      1,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+      1,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+      0,   0,   0,   0,   0,   0,   0,   0,
+    101, 110, 103, 105, 110, 101,  82, 112,
+    109,   0,   0,   0,   0,   0,   0,   0,
     10,   0,   0,   0,   0,   0,   0,   0,
      0,   0,   0,   0,   0,   0,   0,   0,
      0,   0,   0,   0,   0,   0,   0,   0,
@@ -4849,11 +5092,11 @@ static const ::capnp::_::AlignedData<33> b_b86e6369214c01c8 = {
 };
 ::capnp::word const* const bp_b86e6369214c01c8 = b_b86e6369214c01c8.words;
 #if !CAPNP_LITE
-static const uint16_t m_b86e6369214c01c8[] = {0};
-static const uint16_t i_b86e6369214c01c8[] = {0};
+static const uint16_t m_b86e6369214c01c8[] = {1, 2, 0};
+static const uint16_t i_b86e6369214c01c8[] = {0, 1, 2};
 const ::capnp::_::RawSchema s_b86e6369214c01c8 = {
-  0xb86e6369214c01c8, b_b86e6369214c01c8.words, 33, nullptr, m_b86e6369214c01c8,
-  0, 1, i_b86e6369214c01c8, nullptr, nullptr, { &s_b86e6369214c01c8, nullptr, nullptr, 0, 0, nullptr }, false
+  0xb86e6369214c01c8, b_b86e6369214c01c8.words, 65, nullptr, m_b86e6369214c01c8,
+  0, 3, i_b86e6369214c01c8, nullptr, nullptr, { &s_b86e6369214c01c8, nullptr, nullptr, 0, 0, nullptr }, false
 };
 #endif  // !CAPNP_LITE
 static const ::capnp::_::AlignedData<116> b_f416ec09499d9d19 = {
@@ -5635,6 +5878,18 @@ constexpr ::capnp::_::RawSchema const* LongitudinalPlanSP::E2eAlerts::_capnpPriv
 #endif  // !CAPNP_NEED_REDUNDANT_CONSTEXPR_DECL
 #endif  // !CAPNP_LITE

+// LongitudinalPlanSP::AccelController
+#if CAPNP_NEED_REDUNDANT_CONSTEXPR_DECL
+constexpr uint16_t LongitudinalPlanSP::AccelController::_capnpPrivate::dataWordSize;
+constexpr uint16_t LongitudinalPlanSP::AccelController::_capnpPrivate::pointerCount;
+#endif  // !CAPNP_NEED_REDUNDANT_CONSTEXPR_DECL
+#if !CAPNP_LITE
+#if CAPNP_NEED_REDUNDANT_CONSTEXPR_DECL
+constexpr ::capnp::Kind LongitudinalPlanSP::AccelController::_capnpPrivate::kind;
+constexpr ::capnp::_::RawSchema const* LongitudinalPlanSP::AccelController::_capnpPrivate::schema;
+#endif  // !CAPNP_NEED_REDUNDANT_CONSTEXPR_DECL
+#endif  // !CAPNP_LITE
+
 // OnroadEventSP
 #if CAPNP_NEED_REDUNDANT_CONSTEXPR_DECL
 constexpr uint16_t OnroadEventSP::_capnpPrivate::dataWordSize;
@@ -178,6 +178,14 @@ enum class LongitudinalPlanSource_ad47440556d96ec4: uint16_t {
 };
 CAPNP_DECLARE_ENUM(LongitudinalPlanSource, ad47440556d96ec4);
 CAPNP_DECLARE_SCHEMA(a567bce822ae28fe);
+CAPNP_DECLARE_SCHEMA(88dcd6d006f3c4a1);
+CAPNP_DECLARE_SCHEMA(8c9f9544b04650a6);
+enum class Profile_8c9f9544b04650a6: uint16_t {
+  ECO,
+  NORMAL,
+  SPORT,
+};
+CAPNP_DECLARE_ENUM(Profile, 8c9f9544b04650a6);
 CAPNP_DECLARE_SCHEMA(da96579883444c35);
 CAPNP_DECLARE_SCHEMA(f6e831752fcdf793);
 CAPNP_DECLARE_SCHEMA(b8007ed8a646b5e6);
@@ -477,9 +485,10 @@ struct LongitudinalPlanSP {
  typedef ::capnp::schemas::LongitudinalPlanSource_ad47440556d96ec4 LongitudinalPlanSource;

  struct E2eAlerts;
+  struct AccelController;

  struct _capnpPrivate {
-    CAPNP_DECLARE_STRUCT_HEADER(f35cc4560bbf6ec2, 2, 5)
+    CAPNP_DECLARE_STRUCT_HEADER(f35cc4560bbf6ec2, 2, 6)
    #if !CAPNP_LITE
    static constexpr ::capnp::_::RawBrandedSchema const* brand() { return &schema->defaultBrand; }
    #endif  // !CAPNP_LITE
@@ -620,6 +629,23 @@ struct LongitudinalPlanSP::E2eAlerts {
  };
 };

+struct LongitudinalPlanSP::AccelController {
+  AccelController() = delete;
+
+  class Reader;
+  class Builder;
+  class Pipeline;
+  typedef ::capnp::schemas::Profile_8c9f9544b04650a6 Profile;
+
+
+  struct _capnpPrivate {
+    CAPNP_DECLARE_STRUCT_HEADER(88dcd6d006f3c4a1, 1, 0)
+    #if !CAPNP_LITE
+    static constexpr ::capnp::_::RawBrandedSchema const* brand() { return &schema->defaultBrand; }
+    #endif  // !CAPNP_LITE
+  };
+};
+
 struct OnroadEventSP {
  OnroadEventSP() = delete;

@@ -806,7 +832,7 @@ struct CarStateSP {
  class Pipeline;

  struct _capnpPrivate {
-    CAPNP_DECLARE_STRUCT_HEADER(b86e6369214c01c8, 1, 0)
+    CAPNP_DECLARE_STRUCT_HEADER(b86e6369214c01c8, 2, 0)
    #if !CAPNP_LITE
    static constexpr ::capnp::_::RawBrandedSchema const* brand() { return &schema->defaultBrand; }
    #endif  // !CAPNP_LITE
@@ -2300,6 +2326,9 @@ public:
  inline bool hasE2eAlerts() const;
  inline  ::cereal::LongitudinalPlanSP::E2eAlerts::Reader getE2eAlerts() const;

+  inline bool hasAccelController() const;
+  inline  ::cereal::LongitudinalPlanSP::AccelController::Reader getAccelController() const;
+
 private:
  ::capnp::_::StructReader _reader;
  template <typename, ::capnp::Kind>
@@ -2372,6 +2401,13 @@ public:
  inline void adoptE2eAlerts(::capnp::Orphan< ::cereal::LongitudinalPlanSP::E2eAlerts>&& value);
  inline ::capnp::Orphan< ::cereal::LongitudinalPlanSP::E2eAlerts> disownE2eAlerts();

+  inline bool hasAccelController();
+  inline  ::cereal::LongitudinalPlanSP::AccelController::Builder getAccelController();
+  inline void setAccelController( ::cereal::LongitudinalPlanSP::AccelController::Reader value);
+  inline  ::cereal::LongitudinalPlanSP::AccelController::Builder initAccelController();
+  inline void adoptAccelController(::capnp::Orphan< ::cereal::LongitudinalPlanSP::AccelController>&& value);
+  inline ::capnp::Orphan< ::cereal::LongitudinalPlanSP::AccelController> disownAccelController();
+
 private:
  ::capnp::_::StructBuilder _builder;
  template <typename, ::capnp::Kind>
@@ -2394,6 +2430,7 @@ public:
  inline  ::cereal::LongitudinalPlanSP::SmartCruiseControl::Pipeline getSmartCruiseControl();
  inline  ::cereal::LongitudinalPlanSP::SpeedLimit::Pipeline getSpeedLimit();
  inline  ::cereal::LongitudinalPlanSP::E2eAlerts::Pipeline getE2eAlerts();
+  inline  ::cereal::LongitudinalPlanSP::AccelController::Pipeline getAccelController();
 private:
  ::capnp::AnyPointer::Pipeline _typeless;
  friend class ::capnp::PipelineHook;
@@ -2425,6 +2462,14 @@ public:

  inline bool getActive() const;

+  inline float getDecelIntent() const;
+
+  inline bool getCurveDetected() const;
+
+  inline bool getWantBlended() const;
+
+  inline bool getLeadVeto() const;
+
 private:
  ::capnp::_::StructReader _reader;
  template <typename, ::capnp::Kind>
@@ -2462,6 +2507,18 @@ public:
  inline bool getActive();
  inline void setActive(bool value);

+  inline float getDecelIntent();
+  inline void setDecelIntent(float value);
+
+  inline bool getCurveDetected();
+  inline void setCurveDetected(bool value);
+
+  inline bool getWantBlended();
+  inline void setWantBlended(bool value);
+
+  inline bool getLeadVeto();
+  inline void setLeadVeto(bool value);
+
 private:
  ::capnp::_::StructBuilder _builder;
  template <typename, ::capnp::Kind>
@@ -3169,6 +3226,92 @@ private:
 };
 #endif  // !CAPNP_LITE

+class LongitudinalPlanSP::AccelController::Reader {
+public:
+  typedef AccelController Reads;
+
+  Reader() = default;
+  inline explicit Reader(::capnp::_::StructReader base): _reader(base) {}
+
+  inline ::capnp::MessageSize totalSize() const {
+    return _reader.totalSize().asPublic();
+  }
+
+#if !CAPNP_LITE
+  inline ::kj::StringTree toString() const {
+    return ::capnp::_::structString(_reader, *_capnpPrivate::brand());
+  }
+#endif  // !CAPNP_LITE
+
+  inline bool getEnabled() const;
+
+  inline bool getActive() const;
+
+  inline  ::cereal::LongitudinalPlanSP::AccelController::Profile getProfile() const;
+
+private:
+  ::capnp::_::StructReader _reader;
+  template <typename, ::capnp::Kind>
+  friend struct ::capnp::ToDynamic_;
+  template <typename, ::capnp::Kind>
+  friend struct ::capnp::_::PointerHelpers;
+  template <typename, ::capnp::Kind>
+  friend struct ::capnp::List;
+  friend class ::capnp::MessageBuilder;
+  friend class ::capnp::Orphanage;
+};
+
+class LongitudinalPlanSP::AccelController::Builder {
+public:
+  typedef AccelController Builds;
+
+  Builder() = delete;  // Deleted to discourage incorrect usage.
+                       // You can explicitly initialize to nullptr instead.
+  inline Builder(decltype(nullptr)) {}
+  inline explicit Builder(::capnp::_::StructBuilder base): _builder(base) {}
+  inline operator Reader() const { return Reader(_builder.asReader()); }
+  inline Reader asReader() const { return *this; }
+
+  inline ::capnp::MessageSize totalSize() const { return asReader().totalSize(); }
+#if !CAPNP_LITE
+  inline ::kj::StringTree toString() const { return asReader().toString(); }
+#endif  // !CAPNP_LITE
+
+  inline bool getEnabled();
+  inline void setEnabled(bool value);
+
+  inline bool getActive();
+  inline void setActive(bool value);
+
+  inline  ::cereal::LongitudinalPlanSP::AccelController::Profile getProfile();
+  inline void setProfile( ::cereal::LongitudinalPlanSP::AccelController::Profile value);
+
+private:
+  ::capnp::_::StructBuilder _builder;
+  template <typename, ::capnp::Kind>
+  friend struct ::capnp::ToDynamic_;
+  friend class ::capnp::Orphanage;
+  template <typename, ::capnp::Kind>
+  friend struct ::capnp::_::PointerHelpers;
+};
+
+#if !CAPNP_LITE
+class LongitudinalPlanSP::AccelController::Pipeline {
+public:
+  typedef AccelController Pipelines;
+
+  inline Pipeline(decltype(nullptr)): _typeless(nullptr) {}
+  inline explicit Pipeline(::capnp::AnyPointer::Pipeline&& typeless)
+      : _typeless(kj::mv(typeless)) {}
+
+private:
+  ::capnp::AnyPointer::Pipeline _typeless;
+  friend class ::capnp::PipelineHook;
+  template <typename, ::capnp::Kind>
+  friend struct ::capnp::ToDynamic_;
+};
+#endif  // !CAPNP_LITE
+
 class OnroadEventSP::Reader {
 public:
  typedef OnroadEventSP Reads;
@@ -4378,6 +4521,10 @@ public:

  inline float getSpeedLimit() const;

+  inline bool getEngineOff() const;
+
+  inline float getEngineRpm() const;
+
 private:
  ::capnp::_::StructReader _reader;
  template <typename, ::capnp::Kind>
@@ -4409,6 +4556,12 @@ public:
  inline float getSpeedLimit();
  inline void setSpeedLimit(float value);

+  inline bool getEngineOff();
+  inline void setEngineOff(bool value);
+
+  inline float getEngineRpm();
+  inline void setEngineRpm(float value);
+
 private:
  ::capnp::_::StructBuilder _builder;
  template <typename, ::capnp::Kind>
@@ -6883,6 +7036,45 @@ inline ::capnp::Orphan< ::cereal::LongitudinalPlanSP::E2eAlerts> LongitudinalPla
      ::capnp::bounded<4>() * ::capnp::POINTERS));
 }

+inline bool LongitudinalPlanSP::Reader::hasAccelController() const {
+  return !_reader.getPointerField(
+      ::capnp::bounded<5>() * ::capnp::POINTERS).isNull();
+}
+inline bool LongitudinalPlanSP::Builder::hasAccelController() {
+  return !_builder.getPointerField(
+      ::capnp::bounded<5>() * ::capnp::POINTERS).isNull();
+}
+inline  ::cereal::LongitudinalPlanSP::AccelController::Reader LongitudinalPlanSP::Reader::getAccelController() const {
+  return ::capnp::_::PointerHelpers< ::cereal::LongitudinalPlanSP::AccelController>::get(_reader.getPointerField(
+      ::capnp::bounded<5>() * ::capnp::POINTERS));
+}
+inline  ::cereal::LongitudinalPlanSP::AccelController::Builder LongitudinalPlanSP::Builder::getAccelController() {
+  return ::capnp::_::PointerHelpers< ::cereal::LongitudinalPlanSP::AccelController>::get(_builder.getPointerField(
+      ::capnp::bounded<5>() * ::capnp::POINTERS));
+}
+#if !CAPNP_LITE
+inline  ::cereal::LongitudinalPlanSP::AccelController::Pipeline LongitudinalPlanSP::Pipeline::getAccelController() {
+  return  ::cereal::LongitudinalPlanSP::AccelController::Pipeline(_typeless.getPointerField(5));
+}
+#endif  // !CAPNP_LITE
+inline void LongitudinalPlanSP::Builder::setAccelController( ::cereal::LongitudinalPlanSP::AccelController::Reader value) {
+  ::capnp::_::PointerHelpers< ::cereal::LongitudinalPlanSP::AccelController>::set(_builder.getPointerField(
+      ::capnp::bounded<5>() * ::capnp::POINTERS), value);
+}
+inline  ::cereal::LongitudinalPlanSP::AccelController::Builder LongitudinalPlanSP::Builder::initAccelController() {
+  return ::capnp::_::PointerHelpers< ::cereal::LongitudinalPlanSP::AccelController>::init(_builder.getPointerField(
+      ::capnp::bounded<5>() * ::capnp::POINTERS));
+}
+inline void LongitudinalPlanSP::Builder::adoptAccelController(
+    ::capnp::Orphan< ::cereal::LongitudinalPlanSP::AccelController>&& value) {
+  ::capnp::_::PointerHelpers< ::cereal::LongitudinalPlanSP::AccelController>::adopt(_builder.getPointerField(
+      ::capnp::bounded<5>() * ::capnp::POINTERS), kj::mv(value));
+}
+inline ::capnp::Orphan< ::cereal::LongitudinalPlanSP::AccelController> LongitudinalPlanSP::Builder::disownAccelController() {
+  return ::capnp::_::PointerHelpers< ::cereal::LongitudinalPlanSP::AccelController>::disown(_builder.getPointerField(
+      ::capnp::bounded<5>() * ::capnp::POINTERS));
+}
+
 inline  ::cereal::LongitudinalPlanSP::DynamicExperimentalControl::DynamicExperimentalControlState LongitudinalPlanSP::DynamicExperimentalControl::Reader::getState() const {
  return _reader.getDataField< ::cereal::LongitudinalPlanSP::DynamicExperimentalControl::DynamicExperimentalControlState>(
      ::capnp::bounded<0>() * ::capnp::ELEMENTS);
@@ -6925,6 +7117,62 @@ inline void LongitudinalPlanSP::DynamicExperimentalControl::Builder::setActive(b
      ::capnp::bounded<17>() * ::capnp::ELEMENTS, value);
 }

+inline float LongitudinalPlanSP::DynamicExperimentalControl::Reader::getDecelIntent() const {
+  return _reader.getDataField<float>(
+      ::capnp::bounded<1>() * ::capnp::ELEMENTS);
+}
+
+inline float LongitudinalPlanSP::DynamicExperimentalControl::Builder::getDecelIntent() {
+  return _builder.getDataField<float>(
+      ::capnp::bounded<1>() * ::capnp::ELEMENTS);
+}
+inline void LongitudinalPlanSP::DynamicExperimentalControl::Builder::setDecelIntent(float value) {
+  _builder.setDataField<float>(
+      ::capnp::bounded<1>() * ::capnp::ELEMENTS, value);
+}
+
+inline bool LongitudinalPlanSP::DynamicExperimentalControl::Reader::getCurveDetected() const {
+  return _reader.getDataField<bool>(
+      ::capnp::bounded<18>() * ::capnp::ELEMENTS);
+}
+
+inline bool LongitudinalPlanSP::DynamicExperimentalControl::Builder::getCurveDetected() {
+  return _builder.getDataField<bool>(
+      ::capnp::bounded<18>() * ::capnp::ELEMENTS);
+}
+inline void LongitudinalPlanSP::DynamicExperimentalControl::Builder::setCurveDetected(bool value) {
+  _builder.setDataField<bool>(
+      ::capnp::bounded<18>() * ::capnp::ELEMENTS, value);
+}
+
+inline bool LongitudinalPlanSP::DynamicExperimentalControl::Reader::getWantBlended() const {
+  return _reader.getDataField<bool>(
+      ::capnp::bounded<19>() * ::capnp::ELEMENTS);
+}
+
+inline bool LongitudinalPlanSP::DynamicExperimentalControl::Builder::getWantBlended() {
+  return _builder.getDataField<bool>(
+      ::capnp::bounded<19>() * ::capnp::ELEMENTS);
+}
+inline void LongitudinalPlanSP::DynamicExperimentalControl::Builder::setWantBlended(bool value) {
+  _builder.setDataField<bool>(
+      ::capnp::bounded<19>() * ::capnp::ELEMENTS, value);
+}
+
+inline bool LongitudinalPlanSP::DynamicExperimentalControl::Reader::getLeadVeto() const {
+  return _reader.getDataField<bool>(
+      ::capnp::bounded<20>() * ::capnp::ELEMENTS);
+}
+
+inline bool LongitudinalPlanSP::DynamicExperimentalControl::Builder::getLeadVeto() {
+  return _builder.getDataField<bool>(
+      ::capnp::bounded<20>() * ::capnp::ELEMENTS);
+}
+inline void LongitudinalPlanSP::DynamicExperimentalControl::Builder::setLeadVeto(bool value) {
+  _builder.setDataField<bool>(
+      ::capnp::bounded<20>() * ::capnp::ELEMENTS, value);
+}
+
 inline bool LongitudinalPlanSP::SmartCruiseControl::Reader::hasVision() const {
  return !_reader.getPointerField(
      ::capnp::bounded<0>() * ::capnp::POINTERS).isNull();
@@ -7473,6 +7721,48 @@ inline void LongitudinalPlanSP::E2eAlerts::Builder::setLeadDepartAlert(bool valu
      ::capnp::bounded<1>() * ::capnp::ELEMENTS, value);
 }

+inline bool LongitudinalPlanSP::AccelController::Reader::getEnabled() const {
+  return _reader.getDataField<bool>(
+      ::capnp::bounded<0>() * ::capnp::ELEMENTS);
+}
+
+inline bool LongitudinalPlanSP::AccelController::Builder::getEnabled() {
+  return _builder.getDataField<bool>(
+      ::capnp::bounded<0>() * ::capnp::ELEMENTS);
+}
+inline void LongitudinalPlanSP::AccelController::Builder::setEnabled(bool value) {
+  _builder.setDataField<bool>(
+      ::capnp::bounded<0>() * ::capnp::ELEMENTS, value);
+}
+
+inline bool LongitudinalPlanSP::AccelController::Reader::getActive() const {
+  return _reader.getDataField<bool>(
+      ::capnp::bounded<1>() * ::capnp::ELEMENTS);
+}
+
+inline bool LongitudinalPlanSP::AccelController::Builder::getActive() {
+  return _builder.getDataField<bool>(
+      ::capnp::bounded<1>() * ::capnp::ELEMENTS);
+}
+inline void LongitudinalPlanSP::AccelController::Builder::setActive(bool value) {
+  _builder.setDataField<bool>(
+      ::capnp::bounded<1>() * ::capnp::ELEMENTS, value);
+}
+
+inline  ::cereal::LongitudinalPlanSP::AccelController::Profile LongitudinalPlanSP::AccelController::Reader::getProfile() const {
+  return _reader.getDataField< ::cereal::LongitudinalPlanSP::AccelController::Profile>(
+      ::capnp::bounded<1>() * ::capnp::ELEMENTS);
+}
+
+inline  ::cereal::LongitudinalPlanSP::AccelController::Profile LongitudinalPlanSP::AccelController::Builder::getProfile() {
+  return _builder.getDataField< ::cereal::LongitudinalPlanSP::AccelController::Profile>(
+      ::capnp::bounded<1>() * ::capnp::ELEMENTS);
+}
+inline void LongitudinalPlanSP::AccelController::Builder::setProfile( ::cereal::LongitudinalPlanSP::AccelController::Profile value) {
+  _builder.setDataField< ::cereal::LongitudinalPlanSP::AccelController::Profile>(
+      ::capnp::bounded<1>() * ::capnp::ELEMENTS, value);
+}
+
 inline bool OnroadEventSP::Reader::hasEvents() const {
  return !_reader.getPointerField(
      ::capnp::bounded<0>() * ::capnp::POINTERS).isNull();
@@ -8807,6 +9097,34 @@ inline void CarStateSP::Builder::setSpeedLimit(float value) {
      ::capnp::bounded<0>() * ::capnp::ELEMENTS, value);
 }

+inline bool CarStateSP::Reader::getEngineOff() const {
+  return _reader.getDataField<bool>(
+      ::capnp::bounded<32>() * ::capnp::ELEMENTS);
+}
+
+inline bool CarStateSP::Builder::getEngineOff() {
+  return _builder.getDataField<bool>(
+      ::capnp::bounded<32>() * ::capnp::ELEMENTS);
+}
+inline void CarStateSP::Builder::setEngineOff(bool value) {
+  _builder.setDataField<bool>(
+      ::capnp::bounded<32>() * ::capnp::ELEMENTS, value);
+}
+
+inline float CarStateSP::Reader::getEngineRpm() const {
+  return _reader.getDataField<float>(
+      ::capnp::bounded<2>() * ::capnp::ELEMENTS);
+}
+
+inline float CarStateSP::Builder::getEngineRpm() {
+  return _builder.getDataField<float>(
+      ::capnp::bounded<2>() * ::capnp::ELEMENTS);
+}
+inline void CarStateSP::Builder::setEngineRpm(float value) {
+  _builder.setDataField<float>(
+      ::capnp::bounded<2>() * ::capnp::ELEMENTS, value);
+}
+
 inline bool LiveMapDataSP::Reader::getSpeedLimitValid() const {
  return _reader.getDataField<bool>(
      ::capnp::bounded<0>() * ::capnp::ELEMENTS);
@@ -192,8 +192,16 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
    {"ShowAdvancedControls", {PERSISTENT | BACKUP, BOOL, "0"}},
    {"ShowTurnSignals", {PERSISTENT | BACKUP, BOOL, "0"}},
    {"StandstillTimer", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"SunnypilotStoppingController", {PERSISTENT | BACKUP, BOOL, "0"}},
    {"TrueVEgoUI", {PERSISTENT | BACKUP, BOOL, "0"}},

+    // toyota specific params
+    {"ToyotaAutoHold", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"ToyotaBrakeOnset", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"ToyotaEnhancedBsm", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"ToyotaTSS2Long", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"ToyotaDriveMode", {PERSISTENT | BACKUP, BOOL, "0"}},
+
    // MADS params
    {"Mads", {PERSISTENT | BACKUP, BOOL, "1"}},
    {"MadsMainCruiseAllowed", {PERSISTENT | BACKUP, BOOL, "1"}},
@@ -243,6 +251,10 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
    {"DynamicExperimentalControl", {PERSISTENT | BACKUP, BOOL, "0"}},
    {"BlindSpot", {PERSISTENT | BACKUP, BOOL, "0"}},

+    // Accel Controller profiles (Eco / Normal / Sport)
+    {"AccelPersonalityEnabled", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"AccelPersonality", {PERSISTENT | BACKUP, INT, "1"}},
+
    // sunnypilot model params
    {"CameraOffset", {PERSISTENT | BACKUP, FLOAT, "0.0"}},
    {"LagdToggle", {PERSISTENT | BACKUP, BOOL, "1"}},
@@ -117,12 +117,16 @@ class TestParams(OpenpilotTestCase):
  def test_params_default_value(self):
    self.params.remove("LanguageSetting")
    self.params.remove("LongitudinalPersonality")
+    self.params.remove("AccelPersonalityEnabled")
+    self.params.remove("AccelPersonality")
    self.params.remove("LiveParametersV2")

    assert self.params.get("LanguageSetting") is None
    assert self.params.get("LanguageSetting", return_default=False) is None
    assert isinstance(self.params.get("LanguageSetting", return_default=True), str)
    assert isinstance(self.params.get("LongitudinalPersonality", return_default=True), int)
+    assert self.params.get("AccelPersonalityEnabled", return_default=True) is False
+    assert self.params.get("AccelPersonality", return_default=True) == 1
    assert self.params.get("LiveParametersV2") is None
    assert self.params.get("LiveParametersV2", return_default=True) is None

@@ -11,13 +11,13 @@ from opendbc.car.structs import car
 from openpilot.common.params import Params
 from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper
 from openpilot.common.swaglog import cloudlog, ForwardingHandler
-
 from opendbc.car import DT_CTRL, structs
 from opendbc.car.can_definitions import CanData, CanRecvCallable, CanSendCallable
 from opendbc.car.carlog import carlog
 from opendbc.car.fw_versions import ObdCallback
 from opendbc.car.car_helpers import get_car, interfaces
 from opendbc.car.interfaces import CarInterfaceBase, RadarInterfaceBase
+from opendbc.safety import ALTERNATIVE_EXPERIENCE
 from openpilot.selfdrive.pandad import can_capnp_to_list, can_list_to_can_capnp
 from openpilot.selfdrive.car.cruise import VCruiseHelper
 from openpilot.selfdrive.car.helpers import convert_carControlSP, convert_to_capnp
@@ -123,6 +123,9 @@ class Car:
      self.RI = RI

    self.CP.alternativeExperience = 0
+    if self.params.get_bool("ToyotaAutoHold"):
+      self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ALLOW_AEB
+
    # mads
    set_alternative_experience(self.CP, self.CP_SP, self.params)
    set_car_specific_params(self.CP, self.CP_SP, self.params)
@@ -19,6 +19,7 @@ IMPERIAL_INCREMENT = round(CV.MPH_TO_KPH, 1)  # round here to avoid rounding err
 ButtonEvent = car.CarState.ButtonEvent
 ButtonType = car.CarState.ButtonEvent.Type
 CRUISE_LONG_PRESS = 50
+TOYOTA_VIRTUAL_CRUISE_LONG_PRESS = 65
 CRUISE_NEAREST_FUNC = {
  ButtonType.accelCruise: math.ceil,
  ButtonType.decelCruise: math.floor,
@@ -43,6 +44,30 @@ class VCruiseHelper(VCruiseHelperSP):
  def v_cruise_initialized(self):
    return self.v_cruise_kph != V_CRUISE_UNSET

+  @property
+  def software_pcm_cruise_speed(self) -> bool:
+    return self.CP.brand == "toyota" and self.CP.pcmCruise and self.CP.openpilotLongitudinalControl and not self.CP_SP.pcmCruiseSpeed
+
+  @property
+  def cruise_long_press_frames(self) -> int:
+    return TOYOTA_VIRTUAL_CRUISE_LONG_PRESS if self.software_pcm_cruise_speed else CRUISE_LONG_PRESS
+
+  @property
+  def software_pcm_cruise_initialized(self) -> bool:
+    return 0 < self.v_cruise_kph < V_CRUISE_UNSET and 0 < self.v_cruise_cluster_kph < V_CRUISE_UNSET
+
+  def _apply_software_pcm_cruise_delta(self, delta_kph: float, is_metric: bool) -> None:
+    """Move Toyota's planner/display targets together while respecting both targets' bounds."""
+    cluster_min_kph = self.v_cruise_min if is_metric else self.v_cruise_min * CV.MPH_TO_KPH
+    min_delta = max(V_CRUISE_MIN - self.v_cruise_kph, cluster_min_kph - self.v_cruise_cluster_kph)
+    max_delta = min(V_CRUISE_MAX - self.v_cruise_kph, V_CRUISE_MAX - self.v_cruise_cluster_kph)
+    if delta_kph > 0:
+      applied_delta = min(delta_kph, max(0., max_delta))
+    else:
+      applied_delta = max(delta_kph, min(0., min_delta))
+    self.v_cruise_kph = round(self.v_cruise_kph + applied_delta, 1)
+    self.v_cruise_cluster_kph = round(self.v_cruise_cluster_kph + applied_delta, 1)
+
  def update_v_cruise(self, CS, enabled, is_metric):
    self.v_cruise_kph_last = self.v_cruise_kph

@@ -51,11 +76,21 @@ class VCruiseHelper(VCruiseHelperSP):
    _enabled = self.update_enabled_state(CS, enabled)

    if CS.cruiseState.available:
-      if not self.CP.pcmCruise or (not self.CP_SP.pcmCruiseSpeed and _enabled):
+      software_pcm_enabled = not self.CP_SP.pcmCruiseSpeed and _enabled
+      if self.software_pcm_cruise_speed:
+        software_pcm_enabled = software_pcm_enabled and self.software_pcm_cruise_initialized
+
+      if not self.CP.pcmCruise or software_pcm_enabled:
        # if stock cruise is completely disabled, then we can use our own set speed logic
        self._update_v_cruise_non_pcm(CS, _enabled, is_metric)
+        v_cruise_kph_before_sla = self.v_cruise_kph
        self.update_speed_limit_assist_v_cruise_non_pcm()
-        self.v_cruise_cluster_kph = self.v_cruise_kph
+        if self.software_pcm_cruise_speed:
+          sla_delta_kph = self.v_cruise_kph - v_cruise_kph_before_sla
+          self.v_cruise_kph = v_cruise_kph_before_sla
+          self._apply_software_pcm_cruise_delta(sla_delta_kph, is_metric)
+        else:
+          self.v_cruise_cluster_kph = self.v_cruise_kph
      else:
        self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
        self.v_cruise_cluster_kph = CS.cruiseState.speedCluster * CV.MS_TO_KPH
@@ -85,13 +120,13 @@ class VCruiseHelper(VCruiseHelperSP):

    for b in CS.buttonEvents:
      if b.type.raw in self.button_timers and not b.pressed:
-        if self.button_timers[b.type.raw] > CRUISE_LONG_PRESS:
+        if self.button_timers[b.type.raw] > self.cruise_long_press_frames:
          return  # end long press
        button_type = b.type.raw
        break
    else:
      for k, timer in self.button_timers.items():
-        if timer and timer % CRUISE_LONG_PRESS == 0:
+        if timer and timer % self.cruise_long_press_frames == 0:
          button_type = k
          long_press = True
          break
@@ -115,10 +150,26 @@ class VCruiseHelper(VCruiseHelperSP):
      return

    long_press, v_cruise_delta = VCruiseHelperSP.update_v_cruise_delta(self, long_press, v_cruise_delta)
-    if long_press and self.v_cruise_kph % v_cruise_delta != 0:  # partial interval
-      self.v_cruise_kph = CRUISE_NEAREST_FUNC[button_type](self.v_cruise_kph / v_cruise_delta) * v_cruise_delta
+    # Toyota's canonical PCM set speed and displayed cluster set speed can differ. In
+    # software-owned PCM mode, round the value the driver sees and apply the same delta
+    # to both targets so the planner/cluster calibration offset remains intact.
+    v_cruise_reference = self.v_cruise_cluster_kph if self.software_pcm_cruise_speed else self.v_cruise_kph
+    if long_press and v_cruise_reference % v_cruise_delta != 0:  # partial interval
+      v_cruise_reference_new = CRUISE_NEAREST_FUNC[button_type](v_cruise_reference / v_cruise_delta) * v_cruise_delta
    else:
-      self.v_cruise_kph += v_cruise_delta * CRUISE_INTERVAL_SIGN[button_type]
+      v_cruise_reference_new = v_cruise_reference + v_cruise_delta * CRUISE_INTERVAL_SIGN[button_type]
+
+    if self.software_pcm_cruise_speed:
+      delta_kph = v_cruise_reference_new - v_cruise_reference
+
+      # If SET is pressed while overriding, do not lower the target below the current speed.
+      if CS.gasPressed and button_type in (ButtonType.decelCruise, ButtonType.setCruise):
+        delta_kph = max(delta_kph, CS.vEgo * CV.MS_TO_KPH - self.v_cruise_kph)
+
+      self._apply_software_pcm_cruise_delta(delta_kph, is_metric)
+      return
+
+    self.v_cruise_kph += v_cruise_reference_new - v_cruise_reference

    # If set is pressed while overriding, clip cruise speed to minimum of vEgo
    if CS.gasPressed and button_type in (ButtonType.decelCruise, ButtonType.setCruise):
@@ -127,6 +178,12 @@ class VCruiseHelper(VCruiseHelperSP):
    self.v_cruise_kph = np.clip(round(self.v_cruise_kph, 1), self.v_cruise_min, V_CRUISE_MAX)

  def update_button_timers(self, CS, enabled):
+    if self.software_pcm_cruise_speed and (not enabled or not CS.cruiseState.available or not self.software_pcm_cruise_initialized):
+      for k in self.button_timers:
+        self.button_timers[k] = 0
+        self.button_change_states[k] = {"standstill": False, "enabled": False}
+      return
+
    # increment timer for buttons still pressed
    for k in self.button_timers:
      if self.button_timers[k] > 0:
@@ -136,6 +136,7 @@ class Controls(ControlsExt):
    # accel PID loop
    pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, self.CP_SP, CS.vEgo, CS.vCruise * CV.KPH_TO_MS)
    actuators.accel = float(self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits))
+    self.long_accel_sp(actuators, CS, long_plan, pid_accel_limits)  # sunnypilot hook

    # Steering PID loop and lateral MPC
    # Reset desired curvature to current to avoid violating the limits on engage
@@ -4,7 +4,6 @@ from openpilot.common.realtime import DT_CTRL
 from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
 from openpilot.common.pid import PIDController
 from openpilot.selfdrive.modeld.constants import ModelConstants
-from openpilot.sunnypilot.selfdrive.controls.lib.longcontrol import LongControlSP

 CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]

@@ -40,8 +39,7 @@ def long_control_state_trans(CP_SP, active, long_control_state,

  return long_control_state

-
-class LongControl(LongControlSP):
+class LongControl:
  def __init__(self, CP, CP_SP):
    self.CP = CP
    self.CP_SP = CP_SP
@@ -70,7 +68,7 @@ class LongControl(LongControlSP):
      if output_accel > self.CP.stopAccel:
        output_accel = min(output_accel, 0.0)
        # TODO: can we just go straight to stopAccel?
-        output_accel -= self.stopping_decel_rate(CS.vEgo) * DT_CTRL
+        output_accel -= 1.0 * DT_CTRL  # m/s^2/s while trying to stop
      self.reset()

    else:  # LongCtrlState.pid
@@ -35,9 +35,12 @@ def get_max_accel(v_ego):
 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):
-  max_accel = ACCEL_MAX if e2e else get_max_accel(v_ego)
-
+def get_cruise_accel(e2e, v_cruise, v_ego, a_cruise_prev, angle_steers, CP, dt, accel_coast, allow_throttle,
+                      max_accel_override=None):
+  if max_accel_override is not None:
+    max_accel = max_accel_override
+  else:
+    max_accel = ACCEL_MAX if e2e else get_max_accel(v_ego)
  if not e2e:
    a_total_max = 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)
@@ -84,7 +87,8 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
    v_ego = sm['carState'].vEgo
    v_cruise_kph = min(sm['carState'].vCruise, V_CRUISE_MAX)
    v_cruise = v_cruise_kph * CV.KPH_TO_MS
-    if sm['controlsState'].forceDecel:
+    force_decel = sm['controlsState'].forceDecel
+    if force_decel:
      v_cruise = 0.0

    long_control_off = sm['controlsState'].longControlState == LongCtrlState.off
@@ -118,6 +122,7 @@ 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.output_a_target)
    self.mpc.update(sm['radarState'], personality=sm['selfdriveState'].personality)
+    self.update_dec(sm)

    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)
@@ -140,9 +145,17 @@ class LongitudinalPlanner(LongitudinalPlannerSP):

    is_e2e = self.is_e2e(sm)

-    self.a_cruise = get_cruise_accel(is_e2e, v_cruise, v_ego,
-                                     self.a_cruise, steer_angle_without_offset, self.CP, self.dt,
-                                     accel_coast, self.allow_throttle)
+    max_accel_override = self.get_max_accel_override(v_ego, sm['carStateSP'].engineOff)
+    v_cruise = self.get_cruise_target_override(v_ego, v_cruise, force_decel, accel_coast if accel_coast < 0.0 else None)
+    a_cruise_prev = self.a_cruise
+    gated_cruise = get_cruise_accel(is_e2e, v_cruise, v_ego, a_cruise_prev, steer_angle_without_offset,
+                                    self.CP, self.dt, accel_coast, self.allow_throttle, max_accel_override)
+    ungated_cruise = get_cruise_accel(is_e2e, v_cruise, v_ego, a_cruise_prev, steer_angle_without_offset,
+                                      self.CP, self.dt, accel_coast, True, max_accel_override)
+    self.a_cruise = self.arbitrate_cruise_candidate(
+      sm, gated_cruise, ungated_cruise, output_a_target_mpc, self.mpc.source,
+      allow_throttle=self.allow_throttle, e2e=is_e2e, force_decel=force_decel,
+    )
    cruise_should_stop = should_stop(v_ego, self.a_cruise)

    candidates = [(output_a_target_mpc, self.mpc.source, output_should_stop_mpc),
@@ -153,6 +166,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
    output_a_target, self.mpc.source, _ = min(candidates, key=lambda c: c[0])
    self.output_should_stop = any(should_stop for _, _, should_stop in candidates)
    self.output_a_target = np.clip(output_a_target, ACCEL_MIN, ACCEL_MAX)
+    self.accel_controller_active = self.is_accel_controller_active(force_decel)

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

@@ -1,10 +1,6 @@
-from types import SimpleNamespace
-from typing import cast
-
 from openpilot.common.test import OpenpilotTestCase
 from openpilot.cereal import custom
 from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState, long_control_state_trans
-from openpilot.sunnypilot.selfdrive.controls.lib.longcontrol import LongControlSP


 class TestLongControlStateTransition(OpenpilotTestCase):
@@ -46,21 +42,3 @@ class TestLongControlStateTransition(OpenpilotTestCase):
    next_state = long_control_state_trans(CP_SP, active, current_state,
                             should_stop=False, brake_pressed=False, cruise_standstill=False)
    assert next_state == LongCtrlState.pid
-
-
-class TestStoppingHold:
-  def test_weak_brake_does_not_hold_stopping(self):
-    control = SimpleNamespace(last_output_accel=-0.109)
-    car_state = SimpleNamespace(vEgo=0.277, aEgo=-0.406)
-    assert not LongControlSP.should_hold_stopping(cast(LongControlSP, control), car_state, -0.072)
-
-  def test_sufficient_brake_holds_stopping(self):
-    control = SimpleNamespace(last_output_accel=-0.543)
-    car_state = SimpleNamespace(vEgo=0.209, aEgo=-0.534)
-    assert LongControlSP.should_hold_stopping(cast(LongControlSP, control), car_state, -0.469)
-
-  def test_stopping_decel_rate_is_smooth_while_rolling(self):
-    assert LongControlSP.stopping_decel_rate(0.1) == 0.3
-
-  def test_stopping_decel_rate_builds_holding_brake_at_stop(self):
-    assert LongControlSP.stopping_decel_rate(0.0) == 2.0
@@ -45,326 +45,326 @@ const static double MAHA_THRESH_31 = 3.8414588206941227;
 *                                                                            *
 *                         This file is part of 'ekf'                         *
 ******************************************************************************/
-void err_fun(double *nom_x, double *delta_x, double *out_205475353292099530) {
-   out_205475353292099530[0] = delta_x[0] + nom_x[0];
-   out_205475353292099530[1] = delta_x[1] + nom_x[1];
-   out_205475353292099530[2] = delta_x[2] + nom_x[2];
-   out_205475353292099530[3] = delta_x[3] + nom_x[3];
-   out_205475353292099530[4] = delta_x[4] + nom_x[4];
-   out_205475353292099530[5] = delta_x[5] + nom_x[5];
-   out_205475353292099530[6] = delta_x[6] + nom_x[6];
-   out_205475353292099530[7] = delta_x[7] + nom_x[7];
-   out_205475353292099530[8] = delta_x[8] + nom_x[8];
+void err_fun(double *nom_x, double *delta_x, double *out_5070392900769873758) {
+   out_5070392900769873758[0] = delta_x[0] + nom_x[0];
+   out_5070392900769873758[1] = delta_x[1] + nom_x[1];
+   out_5070392900769873758[2] = delta_x[2] + nom_x[2];
+   out_5070392900769873758[3] = delta_x[3] + nom_x[3];
+   out_5070392900769873758[4] = delta_x[4] + nom_x[4];
+   out_5070392900769873758[5] = delta_x[5] + nom_x[5];
+   out_5070392900769873758[6] = delta_x[6] + nom_x[6];
+   out_5070392900769873758[7] = delta_x[7] + nom_x[7];
+   out_5070392900769873758[8] = delta_x[8] + nom_x[8];
 }
-void inv_err_fun(double *nom_x, double *true_x, double *out_6859088956493527341) {
-   out_6859088956493527341[0] = -nom_x[0] + true_x[0];
-   out_6859088956493527341[1] = -nom_x[1] + true_x[1];
-   out_6859088956493527341[2] = -nom_x[2] + true_x[2];
-   out_6859088956493527341[3] = -nom_x[3] + true_x[3];
-   out_6859088956493527341[4] = -nom_x[4] + true_x[4];
-   out_6859088956493527341[5] = -nom_x[5] + true_x[5];
-   out_6859088956493527341[6] = -nom_x[6] + true_x[6];
-   out_6859088956493527341[7] = -nom_x[7] + true_x[7];
-   out_6859088956493527341[8] = -nom_x[8] + true_x[8];
+void inv_err_fun(double *nom_x, double *true_x, double *out_3742760733154016051) {
+   out_3742760733154016051[0] = -nom_x[0] + true_x[0];
+   out_3742760733154016051[1] = -nom_x[1] + true_x[1];
+   out_3742760733154016051[2] = -nom_x[2] + true_x[2];
+   out_3742760733154016051[3] = -nom_x[3] + true_x[3];
+   out_3742760733154016051[4] = -nom_x[4] + true_x[4];
+   out_3742760733154016051[5] = -nom_x[5] + true_x[5];
+   out_3742760733154016051[6] = -nom_x[6] + true_x[6];
+   out_3742760733154016051[7] = -nom_x[7] + true_x[7];
+   out_3742760733154016051[8] = -nom_x[8] + true_x[8];
 }
-void H_mod_fun(double *state, double *out_8201686483145055601) {
-   out_8201686483145055601[0] = 1.0;
-   out_8201686483145055601[1] = 0.0;
-   out_8201686483145055601[2] = 0.0;
-   out_8201686483145055601[3] = 0.0;
-   out_8201686483145055601[4] = 0.0;
-   out_8201686483145055601[5] = 0.0;
-   out_8201686483145055601[6] = 0.0;
-   out_8201686483145055601[7] = 0.0;
-   out_8201686483145055601[8] = 0.0;
-   out_8201686483145055601[9] = 0.0;
-   out_8201686483145055601[10] = 1.0;
-   out_8201686483145055601[11] = 0.0;
-   out_8201686483145055601[12] = 0.0;
-   out_8201686483145055601[13] = 0.0;
-   out_8201686483145055601[14] = 0.0;
-   out_8201686483145055601[15] = 0.0;
-   out_8201686483145055601[16] = 0.0;
-   out_8201686483145055601[17] = 0.0;
-   out_8201686483145055601[18] = 0.0;
-   out_8201686483145055601[19] = 0.0;
-   out_8201686483145055601[20] = 1.0;
-   out_8201686483145055601[21] = 0.0;
-   out_8201686483145055601[22] = 0.0;
-   out_8201686483145055601[23] = 0.0;
-   out_8201686483145055601[24] = 0.0;
-   out_8201686483145055601[25] = 0.0;
-   out_8201686483145055601[26] = 0.0;
-   out_8201686483145055601[27] = 0.0;
-   out_8201686483145055601[28] = 0.0;
-   out_8201686483145055601[29] = 0.0;
-   out_8201686483145055601[30] = 1.0;
-   out_8201686483145055601[31] = 0.0;
-   out_8201686483145055601[32] = 0.0;
-   out_8201686483145055601[33] = 0.0;
-   out_8201686483145055601[34] = 0.0;
-   out_8201686483145055601[35] = 0.0;
-   out_8201686483145055601[36] = 0.0;
-   out_8201686483145055601[37] = 0.0;
-   out_8201686483145055601[38] = 0.0;
-   out_8201686483145055601[39] = 0.0;
-   out_8201686483145055601[40] = 1.0;
-   out_8201686483145055601[41] = 0.0;
-   out_8201686483145055601[42] = 0.0;
-   out_8201686483145055601[43] = 0.0;
-   out_8201686483145055601[44] = 0.0;
-   out_8201686483145055601[45] = 0.0;
-   out_8201686483145055601[46] = 0.0;
-   out_8201686483145055601[47] = 0.0;
-   out_8201686483145055601[48] = 0.0;
-   out_8201686483145055601[49] = 0.0;
-   out_8201686483145055601[50] = 1.0;
-   out_8201686483145055601[51] = 0.0;
-   out_8201686483145055601[52] = 0.0;
-   out_8201686483145055601[53] = 0.0;
-   out_8201686483145055601[54] = 0.0;
-   out_8201686483145055601[55] = 0.0;
-   out_8201686483145055601[56] = 0.0;
-   out_8201686483145055601[57] = 0.0;
-   out_8201686483145055601[58] = 0.0;
-   out_8201686483145055601[59] = 0.0;
-   out_8201686483145055601[60] = 1.0;
-   out_8201686483145055601[61] = 0.0;
-   out_8201686483145055601[62] = 0.0;
-   out_8201686483145055601[63] = 0.0;
-   out_8201686483145055601[64] = 0.0;
-   out_8201686483145055601[65] = 0.0;
-   out_8201686483145055601[66] = 0.0;
-   out_8201686483145055601[67] = 0.0;
-   out_8201686483145055601[68] = 0.0;
-   out_8201686483145055601[69] = 0.0;
-   out_8201686483145055601[70] = 1.0;
-   out_8201686483145055601[71] = 0.0;
-   out_8201686483145055601[72] = 0.0;
-   out_8201686483145055601[73] = 0.0;
-   out_8201686483145055601[74] = 0.0;
-   out_8201686483145055601[75] = 0.0;
-   out_8201686483145055601[76] = 0.0;
-   out_8201686483145055601[77] = 0.0;
-   out_8201686483145055601[78] = 0.0;
-   out_8201686483145055601[79] = 0.0;
-   out_8201686483145055601[80] = 1.0;
+void H_mod_fun(double *state, double *out_5758456641177717920) {
+   out_5758456641177717920[0] = 1.0;
+   out_5758456641177717920[1] = 0.0;
+   out_5758456641177717920[2] = 0.0;
+   out_5758456641177717920[3] = 0.0;
+   out_5758456641177717920[4] = 0.0;
+   out_5758456641177717920[5] = 0.0;
+   out_5758456641177717920[6] = 0.0;
+   out_5758456641177717920[7] = 0.0;
+   out_5758456641177717920[8] = 0.0;
+   out_5758456641177717920[9] = 0.0;
+   out_5758456641177717920[10] = 1.0;
+   out_5758456641177717920[11] = 0.0;
+   out_5758456641177717920[12] = 0.0;
+   out_5758456641177717920[13] = 0.0;
+   out_5758456641177717920[14] = 0.0;
+   out_5758456641177717920[15] = 0.0;
+   out_5758456641177717920[16] = 0.0;
+   out_5758456641177717920[17] = 0.0;
+   out_5758456641177717920[18] = 0.0;
+   out_5758456641177717920[19] = 0.0;
+   out_5758456641177717920[20] = 1.0;
+   out_5758456641177717920[21] = 0.0;
+   out_5758456641177717920[22] = 0.0;
+   out_5758456641177717920[23] = 0.0;
+   out_5758456641177717920[24] = 0.0;
+   out_5758456641177717920[25] = 0.0;
+   out_5758456641177717920[26] = 0.0;
+   out_5758456641177717920[27] = 0.0;
+   out_5758456641177717920[28] = 0.0;
+   out_5758456641177717920[29] = 0.0;
+   out_5758456641177717920[30] = 1.0;
+   out_5758456641177717920[31] = 0.0;
+   out_5758456641177717920[32] = 0.0;
+   out_5758456641177717920[33] = 0.0;
+   out_5758456641177717920[34] = 0.0;
+   out_5758456641177717920[35] = 0.0;
+   out_5758456641177717920[36] = 0.0;
+   out_5758456641177717920[37] = 0.0;
+   out_5758456641177717920[38] = 0.0;
+   out_5758456641177717920[39] = 0.0;
+   out_5758456641177717920[40] = 1.0;
+   out_5758456641177717920[41] = 0.0;
+   out_5758456641177717920[42] = 0.0;
+   out_5758456641177717920[43] = 0.0;
+   out_5758456641177717920[44] = 0.0;
+   out_5758456641177717920[45] = 0.0;
+   out_5758456641177717920[46] = 0.0;
+   out_5758456641177717920[47] = 0.0;
+   out_5758456641177717920[48] = 0.0;
+   out_5758456641177717920[49] = 0.0;
+   out_5758456641177717920[50] = 1.0;
+   out_5758456641177717920[51] = 0.0;
+   out_5758456641177717920[52] = 0.0;
+   out_5758456641177717920[53] = 0.0;
+   out_5758456641177717920[54] = 0.0;
+   out_5758456641177717920[55] = 0.0;
+   out_5758456641177717920[56] = 0.0;
+   out_5758456641177717920[57] = 0.0;
+   out_5758456641177717920[58] = 0.0;
+   out_5758456641177717920[59] = 0.0;
+   out_5758456641177717920[60] = 1.0;
+   out_5758456641177717920[61] = 0.0;
+   out_5758456641177717920[62] = 0.0;
+   out_5758456641177717920[63] = 0.0;
+   out_5758456641177717920[64] = 0.0;
+   out_5758456641177717920[65] = 0.0;
+   out_5758456641177717920[66] = 0.0;
+   out_5758456641177717920[67] = 0.0;
+   out_5758456641177717920[68] = 0.0;
+   out_5758456641177717920[69] = 0.0;
+   out_5758456641177717920[70] = 1.0;
+   out_5758456641177717920[71] = 0.0;
+   out_5758456641177717920[72] = 0.0;
+   out_5758456641177717920[73] = 0.0;
+   out_5758456641177717920[74] = 0.0;
+   out_5758456641177717920[75] = 0.0;
+   out_5758456641177717920[76] = 0.0;
+   out_5758456641177717920[77] = 0.0;
+   out_5758456641177717920[78] = 0.0;
+   out_5758456641177717920[79] = 0.0;
+   out_5758456641177717920[80] = 1.0;
 }
-void f_fun(double *state, double dt, double *out_2204957494938772746) {
-   out_2204957494938772746[0] = state[0];
-   out_2204957494938772746[1] = state[1];
-   out_2204957494938772746[2] = state[2];
-   out_2204957494938772746[3] = state[3];
-   out_2204957494938772746[4] = state[4];
-   out_2204957494938772746[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8100000000000005*state[8] + stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*state[1]) + (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*state[4])) + state[5];
-   out_2204957494938772746[6] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*state[4])) + state[6];
-   out_2204957494938772746[7] = state[7];
-   out_2204957494938772746[8] = state[8];
+void f_fun(double *state, double dt, double *out_730595102125265078) {
+   out_730595102125265078[0] = state[0];
+   out_730595102125265078[1] = state[1];
+   out_730595102125265078[2] = state[2];
+   out_730595102125265078[3] = state[3];
+   out_730595102125265078[4] = state[4];
+   out_730595102125265078[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8100000000000005*state[8] + stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*state[1]) + (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*state[4])) + state[5];
+   out_730595102125265078[6] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*state[4])) + state[6];
+   out_730595102125265078[7] = state[7];
+   out_730595102125265078[8] = state[8];
 }
-void F_fun(double *state, double dt, double *out_2327939745960551937) {
-   out_2327939745960551937[0] = 1;
-   out_2327939745960551937[1] = 0;
-   out_2327939745960551937[2] = 0;
-   out_2327939745960551937[3] = 0;
-   out_2327939745960551937[4] = 0;
-   out_2327939745960551937[5] = 0;
-   out_2327939745960551937[6] = 0;
-   out_2327939745960551937[7] = 0;
-   out_2327939745960551937[8] = 0;
-   out_2327939745960551937[9] = 0;
-   out_2327939745960551937[10] = 1;
-   out_2327939745960551937[11] = 0;
-   out_2327939745960551937[12] = 0;
-   out_2327939745960551937[13] = 0;
-   out_2327939745960551937[14] = 0;
-   out_2327939745960551937[15] = 0;
-   out_2327939745960551937[16] = 0;
-   out_2327939745960551937[17] = 0;
-   out_2327939745960551937[18] = 0;
-   out_2327939745960551937[19] = 0;
-   out_2327939745960551937[20] = 1;
-   out_2327939745960551937[21] = 0;
-   out_2327939745960551937[22] = 0;
-   out_2327939745960551937[23] = 0;
-   out_2327939745960551937[24] = 0;
-   out_2327939745960551937[25] = 0;
-   out_2327939745960551937[26] = 0;
-   out_2327939745960551937[27] = 0;
-   out_2327939745960551937[28] = 0;
-   out_2327939745960551937[29] = 0;
-   out_2327939745960551937[30] = 1;
-   out_2327939745960551937[31] = 0;
-   out_2327939745960551937[32] = 0;
-   out_2327939745960551937[33] = 0;
-   out_2327939745960551937[34] = 0;
-   out_2327939745960551937[35] = 0;
-   out_2327939745960551937[36] = 0;
-   out_2327939745960551937[37] = 0;
-   out_2327939745960551937[38] = 0;
-   out_2327939745960551937[39] = 0;
-   out_2327939745960551937[40] = 1;
-   out_2327939745960551937[41] = 0;
-   out_2327939745960551937[42] = 0;
-   out_2327939745960551937[43] = 0;
-   out_2327939745960551937[44] = 0;
-   out_2327939745960551937[45] = dt*(stiffness_front*(-state[2] - state[3] + state[7])/(mass*state[1]) + (-stiffness_front - stiffness_rear)*state[5]/(mass*state[4]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[6]/(mass*state[4]));
-   out_2327939745960551937[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
-   out_2327939745960551937[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
-   out_2327939745960551937[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
-   out_2327939745960551937[49] = dt*((-1 - (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*pow(state[4], 2)))*state[6] - (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*pow(state[4], 2)));
-   out_2327939745960551937[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
-   out_2327939745960551937[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
-   out_2327939745960551937[52] = dt*stiffness_front*state[0]/(mass*state[1]);
-   out_2327939745960551937[53] = -9.8100000000000005*dt;
-   out_2327939745960551937[54] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front - pow(center_to_rear, 2)*stiffness_rear)*state[6]/(rotational_inertia*state[4]));
-   out_2327939745960551937[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
-   out_2327939745960551937[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
-   out_2327939745960551937[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
-   out_2327939745960551937[58] = dt*(-(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*pow(state[4], 2)) - (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*pow(state[4], 2)));
-   out_2327939745960551937[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
-   out_2327939745960551937[60] = dt*(-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])/(rotational_inertia*state[4]) + 1;
-   out_2327939745960551937[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
-   out_2327939745960551937[62] = 0;
-   out_2327939745960551937[63] = 0;
-   out_2327939745960551937[64] = 0;
-   out_2327939745960551937[65] = 0;
-   out_2327939745960551937[66] = 0;
-   out_2327939745960551937[67] = 0;
-   out_2327939745960551937[68] = 0;
-   out_2327939745960551937[69] = 0;
-   out_2327939745960551937[70] = 1;
-   out_2327939745960551937[71] = 0;
-   out_2327939745960551937[72] = 0;
-   out_2327939745960551937[73] = 0;
-   out_2327939745960551937[74] = 0;
-   out_2327939745960551937[75] = 0;
-   out_2327939745960551937[76] = 0;
-   out_2327939745960551937[77] = 0;
-   out_2327939745960551937[78] = 0;
-   out_2327939745960551937[79] = 0;
-   out_2327939745960551937[80] = 1;
+void F_fun(double *state, double dt, double *out_7198115545780111359) {
+   out_7198115545780111359[0] = 1;
+   out_7198115545780111359[1] = 0;
+   out_7198115545780111359[2] = 0;
+   out_7198115545780111359[3] = 0;
+   out_7198115545780111359[4] = 0;
+   out_7198115545780111359[5] = 0;
+   out_7198115545780111359[6] = 0;
+   out_7198115545780111359[7] = 0;
+   out_7198115545780111359[8] = 0;
+   out_7198115545780111359[9] = 0;
+   out_7198115545780111359[10] = 1;
+   out_7198115545780111359[11] = 0;
+   out_7198115545780111359[12] = 0;
+   out_7198115545780111359[13] = 0;
+   out_7198115545780111359[14] = 0;
+   out_7198115545780111359[15] = 0;
+   out_7198115545780111359[16] = 0;
+   out_7198115545780111359[17] = 0;
+   out_7198115545780111359[18] = 0;
+   out_7198115545780111359[19] = 0;
+   out_7198115545780111359[20] = 1;
+   out_7198115545780111359[21] = 0;
+   out_7198115545780111359[22] = 0;
+   out_7198115545780111359[23] = 0;
+   out_7198115545780111359[24] = 0;
+   out_7198115545780111359[25] = 0;
+   out_7198115545780111359[26] = 0;
+   out_7198115545780111359[27] = 0;
+   out_7198115545780111359[28] = 0;
+   out_7198115545780111359[29] = 0;
+   out_7198115545780111359[30] = 1;
+   out_7198115545780111359[31] = 0;
+   out_7198115545780111359[32] = 0;
+   out_7198115545780111359[33] = 0;
+   out_7198115545780111359[34] = 0;
+   out_7198115545780111359[35] = 0;
+   out_7198115545780111359[36] = 0;
+   out_7198115545780111359[37] = 0;
+   out_7198115545780111359[38] = 0;
+   out_7198115545780111359[39] = 0;
+   out_7198115545780111359[40] = 1;
+   out_7198115545780111359[41] = 0;
+   out_7198115545780111359[42] = 0;
+   out_7198115545780111359[43] = 0;
+   out_7198115545780111359[44] = 0;
+   out_7198115545780111359[45] = dt*(stiffness_front*(-state[2] - state[3] + state[7])/(mass*state[1]) + (-stiffness_front - stiffness_rear)*state[5]/(mass*state[4]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[6]/(mass*state[4]));
+   out_7198115545780111359[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
+   out_7198115545780111359[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
+   out_7198115545780111359[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
+   out_7198115545780111359[49] = dt*((-1 - (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*pow(state[4], 2)))*state[6] - (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*pow(state[4], 2)));
+   out_7198115545780111359[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
+   out_7198115545780111359[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
+   out_7198115545780111359[52] = dt*stiffness_front*state[0]/(mass*state[1]);
+   out_7198115545780111359[53] = -9.8100000000000005*dt;
+   out_7198115545780111359[54] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front - pow(center_to_rear, 2)*stiffness_rear)*state[6]/(rotational_inertia*state[4]));
+   out_7198115545780111359[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
+   out_7198115545780111359[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+   out_7198115545780111359[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+   out_7198115545780111359[58] = dt*(-(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*pow(state[4], 2)) - (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*pow(state[4], 2)));
+   out_7198115545780111359[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
+   out_7198115545780111359[60] = dt*(-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])/(rotational_inertia*state[4]) + 1;
+   out_7198115545780111359[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+   out_7198115545780111359[62] = 0;
+   out_7198115545780111359[63] = 0;
+   out_7198115545780111359[64] = 0;
+   out_7198115545780111359[65] = 0;
+   out_7198115545780111359[66] = 0;
+   out_7198115545780111359[67] = 0;
+   out_7198115545780111359[68] = 0;
+   out_7198115545780111359[69] = 0;
+   out_7198115545780111359[70] = 1;
+   out_7198115545780111359[71] = 0;
+   out_7198115545780111359[72] = 0;
+   out_7198115545780111359[73] = 0;
+   out_7198115545780111359[74] = 0;
+   out_7198115545780111359[75] = 0;
+   out_7198115545780111359[76] = 0;
+   out_7198115545780111359[77] = 0;
+   out_7198115545780111359[78] = 0;
+   out_7198115545780111359[79] = 0;
+   out_7198115545780111359[80] = 1;
 }
-void h_25(double *state, double *unused, double *out_8734450240234959528) {
-   out_8734450240234959528[0] = state[6];
+void h_25(double *state, double *unused, double *out_5441109048213736058) {
+   out_5441109048213736058[0] = state[6];
 }
-void H_25(double *state, double *unused, double *out_6133161197384342845) {
-   out_6133161197384342845[0] = 0;
-   out_6133161197384342845[1] = 0;
-   out_6133161197384342845[2] = 0;
-   out_6133161197384342845[3] = 0;
-   out_6133161197384342845[4] = 0;
-   out_6133161197384342845[5] = 0;
-   out_6133161197384342845[6] = 1;
-   out_6133161197384342845[7] = 0;
-   out_6133161197384342845[8] = 0;
+void H_25(double *state, double *unused, double *out_4766949399557973891) {
+   out_4766949399557973891[0] = 0;
+   out_4766949399557973891[1] = 0;
+   out_4766949399557973891[2] = 0;
+   out_4766949399557973891[3] = 0;
+   out_4766949399557973891[4] = 0;
+   out_4766949399557973891[5] = 0;
+   out_4766949399557973891[6] = 1;
+   out_4766949399557973891[7] = 0;
+   out_4766949399557973891[8] = 0;
 }
-void h_24(double *state, double *unused, double *out_8806581475742121932) {
-   out_8806581475742121932[0] = state[4];
-   out_8806581475742121932[1] = state[5];
+void h_24(double *state, double *unused, double *out_2797667639827089179) {
+   out_2797667639827089179[0] = state[4];
+   out_2797667639827089179[1] = state[5];
 }
-void H_24(double *state, double *unused, double *out_5738011069733690670) {
-   out_5738011069733690670[0] = 0;
-   out_5738011069733690670[1] = 0;
-   out_5738011069733690670[2] = 0;
-   out_5738011069733690670[3] = 0;
-   out_5738011069733690670[4] = 1;
-   out_5738011069733690670[5] = 0;
-   out_5738011069733690670[6] = 0;
-   out_5738011069733690670[7] = 0;
-   out_5738011069733690670[8] = 0;
-   out_5738011069733690670[9] = 0;
-   out_5738011069733690670[10] = 0;
-   out_5738011069733690670[11] = 0;
-   out_5738011069733690670[12] = 0;
-   out_5738011069733690670[13] = 0;
-   out_5738011069733690670[14] = 1;
-   out_5738011069733690670[15] = 0;
-   out_5738011069733690670[16] = 0;
-   out_5738011069733690670[17] = 0;
+void H_24(double *state, double *unused, double *out_4461115786511221334) {
+   out_4461115786511221334[0] = 0;
+   out_4461115786511221334[1] = 0;
+   out_4461115786511221334[2] = 0;
+   out_4461115786511221334[3] = 0;
+   out_4461115786511221334[4] = 1;
+   out_4461115786511221334[5] = 0;
+   out_4461115786511221334[6] = 0;
+   out_4461115786511221334[7] = 0;
+   out_4461115786511221334[8] = 0;
+   out_4461115786511221334[9] = 0;
+   out_4461115786511221334[10] = 0;
+   out_4461115786511221334[11] = 0;
+   out_4461115786511221334[12] = 0;
+   out_4461115786511221334[13] = 0;
+   out_4461115786511221334[14] = 1;
+   out_4461115786511221334[15] = 0;
+   out_4461115786511221334[16] = 0;
+   out_4461115786511221334[17] = 0;
 }
-void h_30(double *state, double *unused, double *out_971184983414287087) {
-   out_971184983414287087[0] = state[4];
+void h_30(double *state, double *unused, double *out_1188271489450022901) {
+   out_1188271489450022901[0] = state[4];
 }
-void H_30(double *state, double *unused, double *out_5396892534833592016) {
-   out_5396892534833592016[0] = 0;
-   out_5396892534833592016[1] = 0;
-   out_5396892534833592016[2] = 0;
-   out_5396892534833592016[3] = 0;
-   out_5396892534833592016[4] = 1;
-   out_5396892534833592016[5] = 0;
-   out_5396892534833592016[6] = 0;
-   out_5396892534833592016[7] = 0;
-   out_5396892534833592016[8] = 0;
+void H_30(double *state, double *unused, double *out_9152098344023969527) {
+   out_9152098344023969527[0] = 0;
+   out_9152098344023969527[1] = 0;
+   out_9152098344023969527[2] = 0;
+   out_9152098344023969527[3] = 0;
+   out_9152098344023969527[4] = 1;
+   out_9152098344023969527[5] = 0;
+   out_9152098344023969527[6] = 0;
+   out_9152098344023969527[7] = 0;
+   out_9152098344023969527[8] = 0;
 }
-void h_26(double *state, double *unused, double *out_1416855028350640224) {
-   out_1416855028350640224[0] = state[7];
+void h_26(double *state, double *unused, double *out_6530138725336053464) {
+   out_6530138725336053464[0] = state[7];
 }
-void H_26(double *state, double *unused, double *out_2391657878510286621) {
-   out_2391657878510286621[0] = 0;
-   out_2391657878510286621[1] = 0;
-   out_2391657878510286621[2] = 0;
-   out_2391657878510286621[3] = 0;
-   out_2391657878510286621[4] = 0;
-   out_2391657878510286621[5] = 0;
-   out_2391657878510286621[6] = 0;
-   out_2391657878510286621[7] = 1;
-   out_2391657878510286621[8] = 0;
+void H_26(double *state, double *unused, double *out_8508452718432030115) {
+   out_8508452718432030115[0] = 0;
+   out_8508452718432030115[1] = 0;
+   out_8508452718432030115[2] = 0;
+   out_8508452718432030115[3] = 0;
+   out_8508452718432030115[4] = 0;
+   out_8508452718432030115[5] = 0;
+   out_8508452718432030115[6] = 0;
+   out_8508452718432030115[7] = 1;
+   out_8508452718432030115[8] = 0;
 }
-void h_27(double *state, double *unused, double *out_5511166866350839857) {
-   out_5511166866350839857[0] = state[3];
+void h_27(double *state, double *unused, double *out_8003961757834969913) {
+   out_8003961757834969913[0] = state[3];
 }
-void H_27(double *state, double *unused, double *out_7571655846634016927) {
-   out_7571655846634016927[0] = 0;
-   out_7571655846634016927[1] = 0;
-   out_7571655846634016927[2] = 0;
-   out_7571655846634016927[3] = 1;
-   out_7571655846634016927[4] = 0;
-   out_7571655846634016927[5] = 0;
-   out_7571655846634016927[6] = 0;
-   out_7571655846634016927[7] = 0;
-   out_7571655846634016927[8] = 0;
+void H_27(double *state, double *unused, double *out_6977335032223544616) {
+   out_6977335032223544616[0] = 0;
+   out_6977335032223544616[1] = 0;
+   out_6977335032223544616[2] = 0;
+   out_6977335032223544616[3] = 1;
+   out_6977335032223544616[4] = 0;
+   out_6977335032223544616[5] = 0;
+   out_6977335032223544616[6] = 0;
+   out_6977335032223544616[7] = 0;
+   out_6977335032223544616[8] = 0;
 }
-void h_29(double *state, double *unused, double *out_5235972804066333968) {
-   out_5235972804066333968[0] = state[1];
+void h_29(double *state, double *unused, double *out_1416133831358674750) {
+   out_1416133831358674750[0] = state[1];
 }
-void H_29(double *state, double *unused, double *out_9161725500205983656) {
-   out_9161725500205983656[0] = 0;
-   out_9161725500205983656[1] = 1;
-   out_9161725500205983656[2] = 0;
-   out_9161725500205983656[3] = 0;
-   out_9161725500205983656[4] = 0;
-   out_9161725500205983656[5] = 0;
-   out_9161725500205983656[6] = 0;
-   out_9161725500205983656[7] = 0;
-   out_9161725500205983656[8] = 0;
+void H_29(double *state, double *unused, double *out_8784414385371189905) {
+   out_8784414385371189905[0] = 0;
+   out_8784414385371189905[1] = 1;
+   out_8784414385371189905[2] = 0;
+   out_8784414385371189905[3] = 0;
+   out_8784414385371189905[4] = 0;
+   out_8784414385371189905[5] = 0;
+   out_8784414385371189905[6] = 0;
+   out_8784414385371189905[7] = 0;
+   out_8784414385371189905[8] = 0;
 }
-void h_28(double *state, double *unused, double *out_7891338238985058206) {
-   out_7891338238985058206[0] = state[0];
+void h_28(double *state, double *unused, double *out_7400645873158492396) {
+   out_7400645873158492396[0] = state[0];
 }
-void H_28(double *state, double *unused, double *out_4079326483136453082) {
-   out_4079326483136453082[0] = 1;
-   out_4079326483136453082[1] = 0;
-   out_4079326483136453082[2] = 0;
-   out_4079326483136453082[3] = 0;
-   out_4079326483136453082[4] = 0;
-   out_4079326483136453082[5] = 0;
-   out_4079326483136453082[6] = 0;
-   out_4079326483136453082[7] = 0;
-   out_4079326483136453082[8] = 0;
+void H_28(double *state, double *unused, double *out_6820784113805863654) {
+   out_6820784113805863654[0] = 1;
+   out_6820784113805863654[1] = 0;
+   out_6820784113805863654[2] = 0;
+   out_6820784113805863654[3] = 0;
+   out_6820784113805863654[4] = 0;
+   out_6820784113805863654[5] = 0;
+   out_6820784113805863654[6] = 0;
+   out_6820784113805863654[7] = 0;
+   out_6820784113805863654[8] = 0;
 }
-void h_31(double *state, double *unused, double *out_8694811794428211892) {
-   out_8694811794428211892[0] = state[8];
+void h_31(double *state, double *unused, double *out_1803659519959713708) {
+   out_1803659519959713708[0] = state[8];
 }
-void H_31(double *state, double *unused, double *out_6163807159261303273) {
-   out_6163807159261303273[0] = 0;
-   out_6163807159261303273[1] = 0;
-   out_6163807159261303273[2] = 0;
-   out_6163807159261303273[3] = 0;
-   out_6163807159261303273[4] = 0;
-   out_6163807159261303273[5] = 0;
-   out_6163807159261303273[6] = 0;
-   out_6163807159261303273[7] = 0;
-   out_6163807159261303273[8] = 1;
+void H_31(double *state, double *unused, double *out_9134660820665381591) {
+   out_9134660820665381591[0] = 0;
+   out_9134660820665381591[1] = 0;
+   out_9134660820665381591[2] = 0;
+   out_9134660820665381591[3] = 0;
+   out_9134660820665381591[4] = 0;
+   out_9134660820665381591[5] = 0;
+   out_9134660820665381591[6] = 0;
+   out_9134660820665381591[7] = 0;
+   out_9134660820665381591[8] = 1;
 }
 #include <eigen3/Eigen/Dense>
 #include <iostream>
@@ -518,68 +518,68 @@ void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, doubl
 void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
  update<1, 3, 0>(in_x, in_P, h_31, H_31, NULL, in_z, in_R, in_ea, MAHA_THRESH_31);
 }
-void car_err_fun(double *nom_x, double *delta_x, double *out_205475353292099530) {
-  err_fun(nom_x, delta_x, out_205475353292099530);
+void car_err_fun(double *nom_x, double *delta_x, double *out_5070392900769873758) {
+  err_fun(nom_x, delta_x, out_5070392900769873758);
 }
-void car_inv_err_fun(double *nom_x, double *true_x, double *out_6859088956493527341) {
-  inv_err_fun(nom_x, true_x, out_6859088956493527341);
+void car_inv_err_fun(double *nom_x, double *true_x, double *out_3742760733154016051) {
+  inv_err_fun(nom_x, true_x, out_3742760733154016051);
 }
-void car_H_mod_fun(double *state, double *out_8201686483145055601) {
-  H_mod_fun(state, out_8201686483145055601);
+void car_H_mod_fun(double *state, double *out_5758456641177717920) {
+  H_mod_fun(state, out_5758456641177717920);
 }
-void car_f_fun(double *state, double dt, double *out_2204957494938772746) {
-  f_fun(state,  dt, out_2204957494938772746);
+void car_f_fun(double *state, double dt, double *out_730595102125265078) {
+  f_fun(state,  dt, out_730595102125265078);
 }
-void car_F_fun(double *state, double dt, double *out_2327939745960551937) {
-  F_fun(state,  dt, out_2327939745960551937);
+void car_F_fun(double *state, double dt, double *out_7198115545780111359) {
+  F_fun(state,  dt, out_7198115545780111359);
 }
-void car_h_25(double *state, double *unused, double *out_8734450240234959528) {
-  h_25(state, unused, out_8734450240234959528);
+void car_h_25(double *state, double *unused, double *out_5441109048213736058) {
+  h_25(state, unused, out_5441109048213736058);
 }
-void car_H_25(double *state, double *unused, double *out_6133161197384342845) {
-  H_25(state, unused, out_6133161197384342845);
+void car_H_25(double *state, double *unused, double *out_4766949399557973891) {
+  H_25(state, unused, out_4766949399557973891);
 }
-void car_h_24(double *state, double *unused, double *out_8806581475742121932) {
-  h_24(state, unused, out_8806581475742121932);
+void car_h_24(double *state, double *unused, double *out_2797667639827089179) {
+  h_24(state, unused, out_2797667639827089179);
 }
-void car_H_24(double *state, double *unused, double *out_5738011069733690670) {
-  H_24(state, unused, out_5738011069733690670);
+void car_H_24(double *state, double *unused, double *out_4461115786511221334) {
+  H_24(state, unused, out_4461115786511221334);
 }
-void car_h_30(double *state, double *unused, double *out_971184983414287087) {
-  h_30(state, unused, out_971184983414287087);
+void car_h_30(double *state, double *unused, double *out_1188271489450022901) {
+  h_30(state, unused, out_1188271489450022901);
 }
-void car_H_30(double *state, double *unused, double *out_5396892534833592016) {
-  H_30(state, unused, out_5396892534833592016);
+void car_H_30(double *state, double *unused, double *out_9152098344023969527) {
+  H_30(state, unused, out_9152098344023969527);
 }
-void car_h_26(double *state, double *unused, double *out_1416855028350640224) {
-  h_26(state, unused, out_1416855028350640224);
+void car_h_26(double *state, double *unused, double *out_6530138725336053464) {
+  h_26(state, unused, out_6530138725336053464);
 }
-void car_H_26(double *state, double *unused, double *out_2391657878510286621) {
-  H_26(state, unused, out_2391657878510286621);
+void car_H_26(double *state, double *unused, double *out_8508452718432030115) {
+  H_26(state, unused, out_8508452718432030115);
 }
-void car_h_27(double *state, double *unused, double *out_5511166866350839857) {
-  h_27(state, unused, out_5511166866350839857);
+void car_h_27(double *state, double *unused, double *out_8003961757834969913) {
+  h_27(state, unused, out_8003961757834969913);
 }
-void car_H_27(double *state, double *unused, double *out_7571655846634016927) {
-  H_27(state, unused, out_7571655846634016927);
+void car_H_27(double *state, double *unused, double *out_6977335032223544616) {
+  H_27(state, unused, out_6977335032223544616);
 }
-void car_h_29(double *state, double *unused, double *out_5235972804066333968) {
-  h_29(state, unused, out_5235972804066333968);
+void car_h_29(double *state, double *unused, double *out_1416133831358674750) {
+  h_29(state, unused, out_1416133831358674750);
 }
-void car_H_29(double *state, double *unused, double *out_9161725500205983656) {
-  H_29(state, unused, out_9161725500205983656);
+void car_H_29(double *state, double *unused, double *out_8784414385371189905) {
+  H_29(state, unused, out_8784414385371189905);
 }
-void car_h_28(double *state, double *unused, double *out_7891338238985058206) {
-  h_28(state, unused, out_7891338238985058206);
+void car_h_28(double *state, double *unused, double *out_7400645873158492396) {
+  h_28(state, unused, out_7400645873158492396);
 }
-void car_H_28(double *state, double *unused, double *out_4079326483136453082) {
-  H_28(state, unused, out_4079326483136453082);
+void car_H_28(double *state, double *unused, double *out_6820784113805863654) {
+  H_28(state, unused, out_6820784113805863654);
 }
-void car_h_31(double *state, double *unused, double *out_8694811794428211892) {
-  h_31(state, unused, out_8694811794428211892);
+void car_h_31(double *state, double *unused, double *out_1803659519959713708) {
+  h_31(state, unused, out_1803659519959713708);
 }
-void car_H_31(double *state, double *unused, double *out_6163807159261303273) {
-  H_31(state, unused, out_6163807159261303273);
+void car_H_31(double *state, double *unused, double *out_9134660820665381591) {
+  H_31(state, unused, out_9134660820665381591);
 }
 void car_predict(double *in_x, double *in_P, double *in_Q, double dt) {
  predict(in_x, in_P, in_Q, dt);
@@ -9,27 +9,27 @@ void car_update_27(double *in_x, double *in_P, double *in_z, double *in_R, doubl
 void car_update_29(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
-void car_err_fun(double *nom_x, double *delta_x, double *out_205475353292099530);
-void car_inv_err_fun(double *nom_x, double *true_x, double *out_6859088956493527341);
-void car_H_mod_fun(double *state, double *out_8201686483145055601);
-void car_f_fun(double *state, double dt, double *out_2204957494938772746);
-void car_F_fun(double *state, double dt, double *out_2327939745960551937);
-void car_h_25(double *state, double *unused, double *out_8734450240234959528);
-void car_H_25(double *state, double *unused, double *out_6133161197384342845);
-void car_h_24(double *state, double *unused, double *out_8806581475742121932);
-void car_H_24(double *state, double *unused, double *out_5738011069733690670);
-void car_h_30(double *state, double *unused, double *out_971184983414287087);
-void car_H_30(double *state, double *unused, double *out_5396892534833592016);
-void car_h_26(double *state, double *unused, double *out_1416855028350640224);
-void car_H_26(double *state, double *unused, double *out_2391657878510286621);
-void car_h_27(double *state, double *unused, double *out_5511166866350839857);
-void car_H_27(double *state, double *unused, double *out_7571655846634016927);
-void car_h_29(double *state, double *unused, double *out_5235972804066333968);
-void car_H_29(double *state, double *unused, double *out_9161725500205983656);
-void car_h_28(double *state, double *unused, double *out_7891338238985058206);
-void car_H_28(double *state, double *unused, double *out_4079326483136453082);
-void car_h_31(double *state, double *unused, double *out_8694811794428211892);
-void car_H_31(double *state, double *unused, double *out_6163807159261303273);
+void car_err_fun(double *nom_x, double *delta_x, double *out_5070392900769873758);
+void car_inv_err_fun(double *nom_x, double *true_x, double *out_3742760733154016051);
+void car_H_mod_fun(double *state, double *out_5758456641177717920);
+void car_f_fun(double *state, double dt, double *out_730595102125265078);
+void car_F_fun(double *state, double dt, double *out_7198115545780111359);
+void car_h_25(double *state, double *unused, double *out_5441109048213736058);
+void car_H_25(double *state, double *unused, double *out_4766949399557973891);
+void car_h_24(double *state, double *unused, double *out_2797667639827089179);
+void car_H_24(double *state, double *unused, double *out_4461115786511221334);
+void car_h_30(double *state, double *unused, double *out_1188271489450022901);
+void car_H_30(double *state, double *unused, double *out_9152098344023969527);
+void car_h_26(double *state, double *unused, double *out_6530138725336053464);
+void car_H_26(double *state, double *unused, double *out_8508452718432030115);
+void car_h_27(double *state, double *unused, double *out_8003961757834969913);
+void car_H_27(double *state, double *unused, double *out_6977335032223544616);
+void car_h_29(double *state, double *unused, double *out_1416133831358674750);
+void car_H_29(double *state, double *unused, double *out_8784414385371189905);
+void car_h_28(double *state, double *unused, double *out_7400645873158492396);
+void car_H_28(double *state, double *unused, double *out_6820784113805863654);
+void car_h_31(double *state, double *unused, double *out_1803659519959713708);
+void car_H_31(double *state, double *unused, double *out_9134660820665381591);
 void car_predict(double *in_x, double *in_P, double *in_Q, double dt);
 void car_set_mass(double x);
 void car_set_rotational_inertia(double x);
@@ -5,18 +5,18 @@ void pose_update_4(double *in_x, double *in_P, double *in_z, double *in_R, doubl
 void pose_update_10(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void pose_update_13(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void pose_update_14(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
-void pose_err_fun(double *nom_x, double *delta_x, double *out_7491504487925958906);
-void pose_inv_err_fun(double *nom_x, double *true_x, double *out_8173082431785614969);
-void pose_H_mod_fun(double *state, double *out_3955896825213980969);
-void pose_f_fun(double *state, double dt, double *out_6226431844178190638);
-void pose_F_fun(double *state, double dt, double *out_5590858509180719260);
-void pose_h_4(double *state, double *unused, double *out_8311116516369893334);
-void pose_H_4(double *state, double *unused, double *out_1196621559825099766);
-void pose_h_10(double *state, double *unused, double *out_8024633733467632830);
-void pose_H_10(double *state, double *unused, double *out_9211636060607930666);
-void pose_h_13(double *state, double *unused, double *out_5246243153983473200);
-void pose_H_13(double *state, double *unused, double *out_4408895385157432567);
-void pose_h_14(double *state, double *unused, double *out_2666716766060469740);
-void pose_H_14(double *state, double *unused, double *out_5159862416164584295);
+void pose_err_fun(double *nom_x, double *delta_x, double *out_8299703332077589328);
+void pose_inv_err_fun(double *nom_x, double *true_x, double *out_4608058237383429265);
+void pose_H_mod_fun(double *state, double *out_7885453398379249088);
+void pose_f_fun(double *state, double dt, double *out_1688144950946873165);
+void pose_F_fun(double *state, double dt, double *out_4851283621730691038);
+void pose_h_4(double *state, double *unused, double *out_2329133011647763937);
+void pose_H_4(double *state, double *unused, double *out_8639614400433961695);
+void pose_h_10(double *state, double *unused, double *out_3294648635274985008);
+void pose_H_10(double *state, double *unused, double *out_3780608178453222524);
+void pose_h_13(double *state, double *unused, double *out_8905113933939181200);
+void pose_H_13(double *state, double *unused, double *out_6594855847943257120);
+void pose_h_14(double *state, double *unused, double *out_8324071568749841126);
+void pose_H_14(double *state, double *unused, double *out_5843888816936105392);
 void pose_predict(double *in_x, double *in_P, double *in_Q, double dt);
 }
@@ -29,12 +29,6 @@ enum SpiError {

 const unsigned int SPI_ACK_TIMEOUT = 500; // milliseconds
 const std::string SPI_DEVICE = "/dev/spidev0.0";
-// TODO: fix SPI turnaround synchronization at the protocol level.
-static uint64_t spi_last_bus_activity_ns = 0;  // protected by hw_lock
-
-static void wait_for_spi_turnaround(uint64_t start_ns) {
-  while ((nanos_since_boot() - start_ns) < 400000) {}
-}

 class LockEx {
 public:
@@ -325,8 +319,6 @@ int PandaSpiHandle::spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx
  assert(tx_len < SPI_BUF_SIZE);
  assert(max_rx_len < SPI_BUF_SIZE);

-  wait_for_spi_turnaround(spi_last_bus_activity_ns);
-
  xfer_count++;
  header = {
    .sync = SPI_SYNC,
@@ -355,7 +347,6 @@ int PandaSpiHandle::spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx
  if (ret < 0) {
    goto fail;
  }
-  wait_for_spi_turnaround(nanos_since_boot());

  // Send data
  if (tx_data != NULL) {
@@ -398,7 +389,6 @@ int PandaSpiHandle::spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx
    memcpy(rx_data, rx_buf + 3, rx_data_len);
  }

-  spi_last_bus_activity_ns = nanos_since_boot();
  return rx_data_len;

 fail:
@@ -413,7 +403,6 @@ fail:
    }
  }

-  spi_last_bus_activity_ns = nanos_since_boot();
  if (ret >= 0) ret = -1;
  return ret;
 }
@@ -11,6 +11,15 @@ from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPl
 from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU


+class PlannerSM(dict):
+  def __init__(self, radar_frame: int, services: dict):
+    super().__init__(services)
+    self.frame = radar_frame
+    self.logMonoTime = {"radarState": radar_frame}
+    self.valid = {"radarState": True}
+    self.alive = {"radarState": True}
+
+
 class Plant:
  messaging_initialized = False

@@ -132,7 +141,7 @@ class Plant:
    car_control.carControl.orientationNED = [0., float(pitch), 0.]

    # ******** get controlsState messages for plotting ***
-    sm = {'radarState': radar.radarState,
+    sm = PlannerSM(self.rk.frame, {'radarState': radar.radarState,
          'carState': car_state.carState,
          'carControl': car_control.carControl,
          'controlsState': control.controlsState,
@@ -141,7 +150,7 @@ class Plant:
          'modelV2': model.modelV2,
          'carStateSP': car_state_sp.carStateSP,
          'liveMapDataSP': live_map_data_sp.liveMapDataSP,
-          'gpsLocation': gps_data.gpsLocation}
+          'gpsLocation': gps_data.gpsLocation})
    self.planner.update(sm)
    self.acceleration = self.planner.output_a_target
    if self.planner.output_should_stop:
@@ -27,6 +27,12 @@ DESCRIPTIONS = {
    "In relaxed mode sunnypilot will stay further away from lead cars. On supported cars, you can cycle through these personalities with " +
    "your steering wheel distance button."
  ),
+  "AccelPersonalityEnabled": tr_noop(
+    "Lets you choose how sunnypilot starts, catches up, and settles at the cruise speed. Emergency braking and stopping are unchanged."
+  ),
+  "AccelPersonality": tr_noop(
+    "Eco is gentlest, Normal balances a prompt start with smooth catch-up, and Sport is more responsive."
+  ),
  "IsLdwEnabled": tr_noop(
    "Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line " +
    "without a turn signal activated while driving over 31 mph (50 km/h)."
@@ -106,6 +112,24 @@ class TogglesLayout(Widget):
      icon="speed_limit.png"
    )

+    self._accel_controller_enabled = toggle_item(
+      lambda: tr("Enable Accel Controller"),
+      lambda: tr(DESCRIPTIONS["AccelPersonalityEnabled"]),
+      self._params.get_bool("AccelPersonalityEnabled"),
+      callback=self._set_accel_controller_enabled,
+      icon="speed_limit.png",
+    )
+
+    self._accel_personality_setting = multiple_button_item(
+      lambda: tr("Acceleration Profile"),
+      lambda: tr(DESCRIPTIONS["AccelPersonality"]),
+      buttons=[lambda: tr("Eco"), lambda: tr("Normal"), lambda: tr("Sport")],
+      button_width=300,
+      callback=self._set_accel_personality,
+      selected_index=self._params.get("AccelPersonality", return_default=True),
+      icon="speed_limit.png"
+    )
+
    self._toggles = {}
    self._locked_toggles = set()
    for param, (title, desc, icon, needs_restart) in self._toggle_defs.items():
@@ -135,9 +159,11 @@ class TogglesLayout(Widget):

      self._toggles[param] = toggle

-      # insert longitudinal personality after NDOG toggle
+      # insert longitudinal personality and Accel Controller settings after NDOG toggle
      if param == "DisengageOnAccelerator":
        self._toggles["LongitudinalPersonality"] = self._long_personality_setting
+        self._toggles["AccelPersonalityEnabled"] = self._accel_controller_enabled
+        self._toggles["AccelPersonality"] = self._accel_personality_setting

    self._update_experimental_mode_icon()
    self._scroller = Scroller(list(self._toggles.values()), line_separator=True, spacing=0)
@@ -158,6 +184,7 @@ class TogglesLayout(Widget):

  def _update_toggles(self):
    ui_state.update_params()
+    accel_controller_enabled = self._params.get_bool("AccelPersonalityEnabled")

    e2e_description = tr(
      "sunnypilot defaults to driving in chill mode. Experimental mode enables alpha-level features that aren't ready for chill mode. " +
@@ -176,11 +203,15 @@ class TogglesLayout(Widget):
        self._toggles["ExperimentalMode"].action_item.set_enabled(True)
        self._toggles["ExperimentalMode"].set_description(e2e_description)
        self._long_personality_setting.action_item.set_enabled(True)
+        self._accel_controller_enabled.action_item.set_enabled(True)
+        self._accel_personality_setting.action_item.set_enabled(True)
      else:
        # no long for now
        self._toggles["ExperimentalMode"].action_item.set_enabled(False)
        self._toggles["ExperimentalMode"].action_item.set_state(False)
        self._long_personality_setting.action_item.set_enabled(False)
+        self._accel_controller_enabled.action_item.set_enabled(False)
+        self._accel_personality_setting.action_item.set_enabled(False)
        self._params.remove("ExperimentalMode")

        unavailable = tr("Experimental mode is currently unavailable on this car since the car's stock ACC is used for longitudinal control.")
@@ -203,6 +234,8 @@ class TogglesLayout(Widget):
    # refresh toggles from params to mirror external changes
    for param in self._toggle_defs:
      self._toggles[param].action_item.set_state(self._params.get_bool(param))
+    self._accel_controller_enabled.action_item.set_state(accel_controller_enabled)
+    self._accel_personality_setting.action_item.set_selected_button(self._params.get("AccelPersonality", return_default=True))

    # these toggles need restart, block while engaged
    for toggle_def in self._toggle_defs:
@@ -247,3 +280,9 @@ class TogglesLayout(Widget):

  def _set_longitudinal_personality(self, button_index: int):
    self._params.put("LongitudinalPersonality", button_index, block=True)
+
+  def _set_accel_personality(self, button_index: int):
+    self._params.put("AccelPersonality", button_index, block=True)
+
+  def _set_accel_controller_enabled(self, state: bool):
+    self._params.put_bool("AccelPersonalityEnabled", state, block=True)
@@ -14,6 +14,7 @@ from openpilot.system.ui.lib.application import gui_app
 if gui_app.sunnypilot_ui():
  from openpilot.selfdrive.ui.sunnypilot.mici.layouts.settings import SettingsLayoutSP as SettingsLayout
  from openpilot.selfdrive.ui.sunnypilot.mici.layouts.home import MiciHomeLayoutSP as MiciHomeLayout
+  from openpilot.selfdrive.ui.sunnypilot.mici.layouts.onroad import OnroadViewContainerSP as AugmentedRoadView

 ONROAD_DELAY = 2.5  # seconds

@@ -72,6 +73,9 @@ class MiciMainLayout(Scroller):
    # For scroll_to
    return self._body_onroad_layout if ui_state.is_body else self._car_onroad_layout

+  def _should_auto_scroll_to_onroad(self) -> bool:
+    return True
+
  def _setup_callbacks(self):
    self._home_layout.set_callbacks(
      on_settings=lambda: gui_app.push_widget(self._settings_layout),
@@ -122,13 +126,15 @@ class MiciMainLayout(Scroller):

    # FIXME: these two pops can interrupt user interacting in the settings
    if self._onroad_time_delay is not None and rl.get_time() - self._onroad_time_delay >= ONROAD_DELAY:
-      gui_app.pop_widgets_to(self, lambda: self._scroll_to(self._onroad_layout))
+      if not gui_app.sunnypilot_ui() or self._should_auto_scroll_to_onroad():
+        gui_app.pop_widgets_to(self, lambda: self._scroll_to(self._onroad_layout))
      self._onroad_time_delay = None

    # When car leaves standstill, pop nav stack and scroll to onroad
    CS = ui_state.sm["carState"]
    if not CS.standstill and self._prev_standstill:
-      gui_app.pop_widgets_to(self, lambda: self._scroll_to(self._onroad_layout))
+      if not gui_app.sunnypilot_ui() or self._should_auto_scroll_to_onroad():
+        gui_app.pop_widgets_to(self, lambda: self._scroll_to(self._onroad_layout))
    self._prev_standstill = CS.standstill

  def _on_interactive_timeout(self):
@@ -42,6 +42,8 @@ class TogglesLayoutMici(NavScroller):
    super().__init__()

    self._personality_toggle = BigMultiParamToggle("driving personality", "LongitudinalPersonality", ["aggressive", "standard", "relaxed"])
+    self._accel_controller_enabled = BigParamControl("enable accel controller", "AccelPersonalityEnabled")
+    self._accel_personality_toggle = BigMultiParamToggle("acceleration profile", "AccelPersonality", ["eco", "normal", "sport"])
    self._experimental_btn = BigToggle("experimental mode", initial_state=ui_state.params.get_bool("ExperimentalMode"),
                                       toggle_callback=self._on_experimental_mode)
    is_metric_toggle = BigParamControl("use metric units", "IsMetric")
@@ -53,6 +55,8 @@ class TogglesLayoutMici(NavScroller):

    self._scroller.add_widgets([
      self._personality_toggle,
+      self._accel_controller_enabled,
+      self._accel_personality_toggle,
      self._experimental_btn,
      is_metric_toggle,
      ldw_toggle,
@@ -65,6 +69,7 @@ class TogglesLayoutMici(NavScroller):
    # Toggle lists
    self._refresh_toggles = (
      ("ExperimentalMode", self._experimental_btn),
+      ("AccelPersonalityEnabled", self._accel_controller_enabled),
      ("IsMetric", is_metric_toggle),
      ("IsLdwEnabled", ldw_toggle),
      ("AlwaysOnDM", always_on_dm_toggle),
@@ -104,17 +109,23 @@ class TogglesLayoutMici(NavScroller):
      if ui_state.has_longitudinal_control:
        self._experimental_btn.set_visible(True)
        self._personality_toggle.set_visible(True)
+        self._accel_controller_enabled.set_visible(True)
+        self._accel_personality_toggle.set_visible(True)
      else:
        # no long for now
        self._experimental_btn.set_visible(False)
        self._experimental_btn.set_checked(False)
        self._personality_toggle.set_visible(False)
+        self._accel_controller_enabled.set_visible(False)
+        self._accel_personality_toggle.set_visible(False)
        ui_state.params.remove("ExperimentalMode")

    # Refresh toggles from params to mirror external changes
    for key, item in self._refresh_toggles:
      item.set_checked(ui_state.params.get_bool(key))

+    self._accel_personality_toggle.refresh()
+
  def _on_experimental_mode(self, state: bool):
    if state and not ui_state.params.get_bool("ExperimentalModeConfirmed"):
      # Don't show enabled state until confirm
@@ -154,8 +154,8 @@ class ModelRenderer(Widget, ModelRendererSP):
      self._draw_lane_lines()
      self._draw_path(sm)

-    # if render_lead_indicator and radar_state:
-    #   self._draw_lead_indicator()
+    if render_lead_indicator and radar_state:
+      self._draw_lead_indicator()

  def _update_raw_points(self, model):
    """Update raw 3D points from model data"""
@@ -383,13 +383,18 @@ class BigMultiParamToggle(BigMultiToggle):
    self._load_value()

  def _load_value(self):
-    self.set_value(self._options[self._params.get(self._param) or 0])
+    value = self._params.get(self._param, return_default=True)
+    index = value if isinstance(value, int) else 0
+    self.set_value(self._options[max(0, min(index, len(self._options) - 1))])

  def _handle_mouse_release(self, mouse_pos: MousePos):
    super()._handle_mouse_release(mouse_pos)
    new_idx = self._options.index(self.value)
    self._params.put(self._param, new_idx)

+  def refresh(self):
+    self._load_value()
+

 class BigParamControl(BigToggle):
  def __init__(self, text: str, param: str, toggle_callback: Callable | None = None):
@@ -143,7 +143,8 @@ class CruiseLayout(Widget):
          self.icbm_toggle.show_description(True)

      if has_long or has_icbm:
-        self.custom_acc_toggle.action_item.set_enabled(((has_long and not ui_state.CP.pcmCruise) or has_icbm) and ui_state.is_offroad())
+        software_cruise_speed = has_long and (not ui_state.CP.pcmCruise or not ui_state.CP_SP.pcmCruiseSpeed)
+        self.custom_acc_toggle.action_item.set_enabled((software_cruise_speed or has_icbm) and ui_state.is_offroad())
        self.dec_toggle.action_item.set_enabled(has_long)
        self.scc_v_toggle.action_item.set_enabled(True)
        self.scc_m_toggle.action_item.set_enabled(True)
@@ -169,7 +170,7 @@ class CruiseLayout(Widget):
      show_custom_acc_desc = True
    else:
      if has_long or has_icbm:
-        if has_long and ui_state.CP.pcmCruise:
+        if has_long and ui_state.CP.pcmCruise and ui_state.CP_SP.pcmCruiseSpeed:
          new_custom_acc_desc = tr(ACC_PCMCRUISE_DISABLED_DESCRIPTION)
          show_custom_acc_desc = True
        else:
@@ -23,7 +23,7 @@ DESCRIPTIONS = {
  'stop_and_go_hack': tr_noop(
    'sunnypilot will allow some Toyota/Lexus cars to auto resume during stop and go traffic. ' +
    'This feature is only applicable to certain models that are able to use longitudinal control. This is an alpha feature. Use at your own risk.'
-  )
+  ),
 }


@@ -0,0 +1,19 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+
+from openpilot.selfdrive.ui.mici.layouts.main import MiciMainLayout
+from openpilot.selfdrive.ui.sunnypilot.mici.widgets.scroll_panel_sp import GuiScrollPanel2SP
+
+
+class MiciMainLayoutSP(MiciMainLayout):
+  def __init__(self):
+    super().__init__()
+    scroller = self._scroller
+    scroller.scroll_panel = GuiScrollPanel2SP(scroller._horizontal, handle_out_of_bounds=not scroller._snap_items)
+
+  def _should_auto_scroll_to_onroad(self) -> bool:
+    return not self._onroad_layout.is_on_info_panel()
@@ -0,0 +1,64 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+from collections.abc import Callable
+import pyray as rl
+from openpilot.system.ui.lib.application import gui_app
+from openpilot.selfdrive.ui.sunnypilot.mici.widgets.scroller_sp import ScrollerSP
+from openpilot.selfdrive.ui.sunnypilot.mici.onroad.augmented_road_view import AugmentedRoadViewSP
+from openpilot.selfdrive.ui.sunnypilot.mici.layouts.onroad_info_panel import OnroadInfoPanel
+
+CONFIDENCE_BALL_VISIBLE_RATIO = 0.4
+HORIZONTAL_SETTLE_PX = 5
+HORIZONTAL_RESET_RATIO = 0.5
+
+
+class OnroadViewContainerSP(ScrollerSP):
+  def __init__(self, bookmark_callback=None):
+    super().__init__(horizontal=False, snap_items=True, spacing=0, pad=0, scroll_indicator=False, edge_shadows=False)
+    self.road_view = AugmentedRoadViewSP(bookmark_callback=bookmark_callback)
+    self.onroad_info_panel = OnroadInfoPanel(bookmark_callback=bookmark_callback)
+
+    self._scroller.add_widgets([
+      self.road_view,
+      self.onroad_info_panel,
+    ])
+    self._scroller.set_reset_scroll_at_show(False)
+    self._scroller.set_scrolling_enabled(lambda: abs(self.rect.x) < HORIZONTAL_SETTLE_PX)
+
+    for child in (self.road_view, self.onroad_info_panel):
+      inner_touch_valid = child._touch_valid_callback
+      child.set_touch_valid_callback(
+        lambda inner=inner_touch_valid: self._touch_valid() and (inner() if inner else True)
+      )
+
+  def set_rect(self, rect: rl.Rectangle):
+    super().set_rect(rect)
+    self.road_view.set_rect(rect)
+    self.onroad_info_panel.set_rect(rect)
+    return self
+
+  def is_swiping_left(self) -> bool:
+    return self.road_view.is_swiping_left() or self.onroad_info_panel.is_swiping_left()
+
+  def set_click_callback(self, click_callback: Callable[[], None] | None) -> None:
+    self.road_view.set_click_callback(click_callback)
+    self.onroad_info_panel.set_click_callback(click_callback)
+
+  def is_on_info_panel(self) -> bool:
+    """True when scrolled past halfway toward onroad_info_panel (used by main layout
+    to skip auto-pop-back-to-camera while user is reading the info panel)."""
+    return abs(self._scroller.scroll_panel.get_offset()) > self._rect.height / 2
+
+  def _render(self, rect: rl.Rectangle):
+    if abs(self.rect.x) > gui_app.width * HORIZONTAL_RESET_RATIO:
+      self._scroller.scroll_panel.set_offset(0)
+
+    vertical_offset = self._scroller.scroll_panel.get_offset()
+    show_ball = abs(vertical_offset) < rect.height * CONFIDENCE_BALL_VISIBLE_RATIO
+    self.road_view.set_show_confidence_ball(show_ball)
+
+    super()._render(rect)
@@ -0,0 +1,403 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+
+import pyray as rl
+from dataclasses import dataclass, field
+from openpilot.common.constants import CV
+from openpilot.common.filter_simple import FirstOrderFilter
+from openpilot.selfdrive.ui.ui_state import ui_state
+from openpilot.system.ui.lib.application import gui_app, FontWeight, MousePos
+from openpilot.system.ui.lib.multilang import tr
+from openpilot.system.ui.lib.text_measure import measure_text_cached
+from openpilot.system.ui.widgets import Widget
+from openpilot.selfdrive.ui.mici.onroad.alert_renderer import AlertRenderer
+from openpilot.selfdrive.ui.mici.onroad.augmented_road_view import BookmarkIcon
+
+METER_TO_KM = 0.001
+METER_TO_MILE = 0.000621371
+
+CONTENT_MARGIN = 16
+SPEED_LIMIT_SIGN_WIDTH = 146
+VIENNA_SIGN_SIZE = 146
+MUTCD_SIGN_HEIGHT = 178
+OFFSET_BADGE_SIZE = 50
+OFFSET_BADGE_PANEL_PADDING = 4
+MUTCD_OFFSET_SIGN_Y_SHIFT = 6
+VIENNA_BADGE_X_RATIO = 0.80
+VIENNA_BADGE_UPCOMING_X_RATIO = 0.70
+VIENNA_BADGE_Y_RATIO = -0.82
+UPCOMING_SIGN_SIZE_RATIO = 0.76
+UPCOMING_SIGN_OVERLAP_RATIO = 0.05
+UNIT_FONT_SIZE = 40
+SPEED_FONT_SIZE = 114
+ROAD_FONT_SIZE = 32
+SCC_TAG_WIDTH = 78
+SCC_TAG_HEIGHT = 30
+SCC_TAG_GAP = 5
+COLUMN_GAP = 12
+
+
+@dataclass(frozen=True)
+class OnroadInfoPanelColors:
+  white: rl.Color = rl.WHITE
+  black: rl.Color = rl.BLACK
+  red: rl.Color = field(default_factory=lambda: rl.Color(255, 0, 0, 255))
+  green: rl.Color = field(default_factory=lambda: rl.Color(0, 255, 0, 255))
+  grey: rl.Color = field(default_factory=lambda: rl.Color(190, 195, 190, 255))
+  light_grey: rl.Color = field(default_factory=lambda: rl.Color(200, 200, 200, 255))
+  dark_grey: rl.Color = field(default_factory=lambda: rl.Color(100, 100, 100, 255))
+  bg_dark: rl.Color = field(default_factory=lambda: rl.Color(0, 0, 0, 255))
+  card_bg: rl.Color = field(default_factory=lambda: rl.Color(50, 50, 50, 200))
+  badge_bg: rl.Color = field(default_factory=lambda: rl.Color(60, 60, 60, 255))
+
+
+COLORS = OnroadInfoPanelColors()
+
+
+class OnroadInfoPanel(Widget):
+  def __init__(self, bookmark_callback=None):
+    super().__init__()
+    self.speed_limit: float = 0.0
+    self.speed_limit_valid: bool = False
+    self.speed_limit_offset: float = 0.0
+    self.next_speed_limit: float = 0.0
+    self.next_speed_limit_distance: float = 0.0
+    self.road_name: str = ""
+    self.current_speed: float = 0.0
+    self.set_speed: float = 0.0
+    self.cruise_enabled: bool = False
+
+    self._sign_slide: float = 0.0
+
+    self._font_bold: rl.Font = gui_app.font(FontWeight.BOLD)
+    self._font_semi_bold: rl.Font = gui_app.font(FontWeight.SEMI_BOLD)
+    self._font_medium: rl.Font = gui_app.font(FontWeight.MEDIUM)
+
+    self._marquee_offset: float = 0.0
+    self._marquee_direction: int = 1
+    self._marquee_pause_timer: float = 0.0
+    self._marquee_speed: float = 40.0
+    self._marquee_pause_duration: float = 1.5
+
+    self._alert_renderer = AlertRenderer()
+    self._alert_alpha_filter = FirstOrderFilter(0, 0.05, 1 / gui_app.target_fps)
+
+    self._bookmark_icon = BookmarkIcon(bookmark_callback)
+
+  def is_swiping_left(self) -> bool:
+    return self._bookmark_icon.is_swiping_left()
+
+  def _handle_mouse_release(self, mouse_pos: MousePos) -> None:
+    # Mirror stock AugmentedRoadView: suppress click while bookmark gesture active
+    if not self._bookmark_icon.interacting():
+      super()._handle_mouse_release(mouse_pos)
+
+  def _update_state(self) -> None:
+    sm = ui_state.sm
+    speed_conv = CV.MS_TO_KPH if ui_state.is_metric else CV.MS_TO_MPH
+
+    if sm.valid["longitudinalPlanSP"]:
+      lp_sp = sm["longitudinalPlanSP"]
+      resolver = lp_sp.speedLimit.resolver
+      self.speed_limit = resolver.speedLimit * speed_conv
+      self.speed_limit_valid = resolver.speedLimitValid
+      self.speed_limit_offset = resolver.speedLimitOffset * speed_conv
+
+    if sm.valid["liveMapDataSP"]:
+      lmd = sm["liveMapDataSP"]
+      self.next_speed_limit = lmd.speedLimitAhead * speed_conv
+      self.next_speed_limit_distance = lmd.speedLimitAheadDistance
+      self.road_name = lmd.roadName
+
+    if sm.updated["carState"]:
+      self.current_speed = sm["carState"].vEgo * speed_conv
+
+    if sm.valid["carState"] and sm.valid["controlsState"]:
+      self.cruise_enabled = sm["carState"].cruiseState.enabled
+      v_cruise_cluster = sm["carState"].vCruiseCluster
+      set_speed_kph = sm["controlsState"].vCruiseDEPRECATED if v_cruise_cluster == 0.0 else v_cruise_cluster
+      self.set_speed = set_speed_kph * (METER_TO_MILE / METER_TO_KM) if not ui_state.is_metric else set_speed_kph
+
+  def _render(self, rect: rl.Rectangle) -> None:
+    self._update_state()
+
+    rl.draw_rectangle(int(rect.x), int(rect.y), int(rect.width), int(rect.height), COLORS.bg_dark)
+
+    left_x = rect.x + CONTENT_MARGIN
+
+    if self.cruise_enabled:
+      unit = tr("MAX")
+      display_speed = self.set_speed
+    else:
+      unit = tr("km/h") if ui_state.is_metric else tr("MPH")
+      display_speed = self.current_speed
+
+    display_speed_text = str(round(display_speed))
+    if self.speed_limit_valid and display_speed > self.speed_limit:
+      speed_color = COLORS.red
+    else:
+      speed_color = COLORS.white
+
+    sign_width = min(SPEED_LIMIT_SIGN_WIDTH, rect.width * 0.30)
+    sign_height = VIENNA_SIGN_SIZE if ui_state.is_metric else MUTCD_SIGN_HEIGHT
+
+    has_upcoming_limit = self.next_speed_limit > 0 and self.next_speed_limit != self.speed_limit
+    target_sign_slide = 1.0 if has_upcoming_limit else 0.0
+    slide_speed = 3.0 * rl.get_frame_time()
+    if self._sign_slide < target_sign_slide:
+      self._sign_slide = min(self._sign_slide + slide_speed, target_sign_slide)
+    elif self._sign_slide > target_sign_slide:
+      self._sign_slide = max(self._sign_slide - slide_speed, target_sign_slide)
+
+    upcoming_width = int(sign_width * UPCOMING_SIGN_SIZE_RATIO)
+    upcoming_height = int(sign_height * UPCOMING_SIGN_SIZE_RATIO)
+    upcoming_reserved_width = int(upcoming_width * 0.85) + 5
+    sign_x_without_upcoming = rect.x + rect.width - sign_width - CONTENT_MARGIN
+    sign_x_with_upcoming = rect.x + rect.width - sign_width - CONTENT_MARGIN - upcoming_reserved_width
+    sign_x = sign_x_without_upcoming + (sign_x_with_upcoming - sign_x_without_upcoming) * self._sign_slide
+    sign_y = rect.y + (rect.height - sign_height) / 2
+    if not ui_state.is_metric and self.speed_limit_offset != 0 and self.speed_limit_valid:
+      sign_y += MUTCD_OFFSET_SIGN_Y_SHIFT
+
+    readout_right = sign_x - COLUMN_GAP
+    readout_width = max(1, readout_right - left_x)
+    road_y = rect.y + rect.height - 44
+
+    unit_font_size = self._fit_font_size(self._font_semi_bold, unit, readout_width, 46, UNIT_FONT_SIZE, 28)
+    speed_font_size = self._fit_font_size(self._font_bold, display_speed_text, readout_width, road_y - (rect.y + 54) - 8,
+                                          SPEED_FONT_SIZE, 76)
+    speed_size = measure_text_cached(self._font_bold, display_speed_text, speed_font_size)
+    speed_y = min(rect.y + 54, road_y - speed_size.y - 8)
+    unit_y = max(rect.y + 14, speed_y - unit_font_size - 6)
+
+    rl.draw_text_ex(self._font_semi_bold, unit, rl.Vector2(left_x, unit_y), unit_font_size, 0, COLORS.grey)
+    rl.draw_text_ex(self._font_bold, display_speed_text, rl.Vector2(left_x, speed_y), speed_font_size, 0, speed_color)
+    self._draw_road_name(left_x, road_y, readout_width)
+
+    if has_upcoming_limit and self._sign_slide > 0.01:
+      upcoming_speed_text = str(round(self.next_speed_limit))
+      distance_text = self._format_distance(self.next_speed_limit_distance)
+      upcoming_x = sign_x + sign_width - int(upcoming_width * UPCOMING_SIGN_OVERLAP_RATIO)
+      upcoming_y = sign_y + (sign_height - upcoming_height) / 2
+
+      upcoming_speed_color = COLORS.black
+      if ui_state.is_metric:
+        self._draw_vienna_sign(upcoming_x, upcoming_y, upcoming_width, upcoming_height, upcoming_speed_text, upcoming_speed_color, is_upcoming=True)
+      else:
+        self._draw_mutcd_sign(upcoming_x, upcoming_y, upcoming_width, upcoming_height, upcoming_speed_text, upcoming_speed_color, is_upcoming=True)
+
+      distance_font_size = self._fit_font_size(self._font_medium, distance_text, upcoming_width, 30, 24, 16)
+      distance_size = measure_text_cached(self._font_medium, distance_text, distance_font_size)
+      rl.draw_text_ex(self._font_medium, distance_text, rl.Vector2(upcoming_x + upcoming_width / 2 - distance_size.x / 2, upcoming_y + upcoming_height),
+                      distance_font_size, 0, COLORS.grey)
+
+    self._draw_speed_limit_sign(sign_x, sign_y, sign_width, sign_height)
+
+    if self.speed_limit_offset != 0 and self.speed_limit_valid:
+      offset_text = str(abs(round(self.speed_limit_offset)))
+      badge_size = OFFSET_BADGE_SIZE
+      badge_rect = self._offset_badge_rect(rect, sign_x, sign_y, sign_width, sign_height, badge_size, has_upcoming_limit)
+
+      if ui_state.is_metric:
+        badge_radius = badge_size / 2
+        badge_center_x = badge_rect.x + badge_radius
+        badge_center_y = badge_rect.y + badge_radius
+        rl.draw_circle(int(badge_center_x), int(badge_center_y), badge_radius + 2, COLORS.dark_grey)
+        rl.draw_circle(int(badge_center_x), int(badge_center_y), badge_radius, COLORS.badge_bg)
+        self._draw_text_centered_fit(self._font_bold, offset_text, 32, rl.Vector2(badge_center_x, badge_center_y), COLORS.white,
+                                     badge_size - 10, badge_size - 8, min_size=24)
+      else:
+        rl.draw_rectangle_rounded(badge_rect, 0.25, 10, COLORS.badge_bg)
+        rl.draw_rectangle_rounded_lines_ex(badge_rect, 0.25, 10, 2, COLORS.dark_grey)
+        self._draw_text_centered_fit(self._font_bold, offset_text, 32, rl.Vector2(badge_rect.x + badge_size / 2, badge_rect.y + badge_size / 2),
+                                     COLORS.white, badge_size - 10, badge_size - 8, min_size=24)
+
+    scc_tag_x = min(left_x + speed_size.x + COLUMN_GAP, readout_right - SCC_TAG_WIDTH)
+    scc_tag_y = speed_y + (speed_size.y - (SCC_TAG_HEIGHT * 2 + SCC_TAG_GAP)) / 2
+    if scc_tag_x >= left_x + speed_size.x + 8:
+      self._draw_scc_icons(scc_tag_x, scc_tag_y, readout_right)
+
+    self._bookmark_icon.render(rect)
+
+    if ui_state.started:
+      alert_obj, no_alert = self._alert_renderer.will_render()
+      self._alert_alpha_filter.update(0 if no_alert else 1)
+      alpha = self._alert_alpha_filter.x
+      if alpha > 0.01:
+        rl.draw_rectangle(int(rect.x), int(rect.y), int(rect.width), int(rect.height), rl.Color(0, 0, 0, int(150 * alpha)))
+      self._alert_renderer.render(rect)
+
+  def _draw_scc_icons(self, x: float, y: float, right_limit: float) -> None:
+    sm = ui_state.sm
+    if not sm.valid["longitudinalPlanSP"]:
+      return
+    scc = sm["longitudinalPlanSP"].smartCruiseControl
+
+    drawn = 0
+
+    for label, active in [("SCC-V", scc.vision.active), ("SCC-M", scc.map.active)]:
+      if not active:
+        continue
+      tag_x = x
+      if tag_x + SCC_TAG_WIDTH > right_limit:
+        return
+      tag_y = y + drawn * (SCC_TAG_HEIGHT + SCC_TAG_GAP)
+      rl.draw_rectangle_rounded(rl.Rectangle(tag_x, tag_y, SCC_TAG_WIDTH, SCC_TAG_HEIGHT), 0.3, 10, COLORS.green)
+      self._draw_text_centered_fit(self._font_bold, label, 18, rl.Vector2(tag_x + SCC_TAG_WIDTH / 2, tag_y + SCC_TAG_HEIGHT / 2), COLORS.black,
+                                   SCC_TAG_WIDTH - 10, SCC_TAG_HEIGHT - 4, min_size=14)
+      drawn += 1
+
+  def _draw_speed_limit_sign(self, x: float, y: float, sign_width: float, sign_height: float) -> None:
+    speed_str = str(round(self.speed_limit)) if self.speed_limit_valid and self.speed_limit > 0 else "--"
+    speed_color = COLORS.black if not self.speed_limit_valid or self.current_speed <= self.speed_limit else COLORS.red
+
+    if ui_state.is_metric:
+      self._draw_vienna_sign(x, y, sign_width, sign_height, speed_str, speed_color, is_upcoming=False)
+    else:
+      self._draw_mutcd_sign(x, y, sign_width, sign_height, speed_str, speed_color, is_upcoming=False)
+
+  def _draw_road_name(self, x: float, y: float, width: float) -> None:
+    if width <= 0:
+      return
+
+    road_display = self.road_name if self.road_name else "--"
+    font_size = self._fit_font_size(self._font_semi_bold, road_display, width, 38, ROAD_FONT_SIZE, 28)
+    road_size = measure_text_cached(self._font_semi_bold, road_display, font_size)
+    text_width = road_size.x
+
+    if text_width <= width:
+      self._marquee_offset = 0.0
+      self._marquee_direction = 1
+      self._marquee_pause_timer = 0.0
+      rl.draw_text_ex(self._font_semi_bold, road_display, rl.Vector2(x, y), font_size, 0, COLORS.white)
+    else:
+      overflow = text_width - width
+      dt = rl.get_frame_time()
+
+      if self._marquee_pause_timer > 0:
+        self._marquee_pause_timer -= dt
+      else:
+        self._marquee_offset += self._marquee_direction * self._marquee_speed * dt
+
+        if self._marquee_offset >= overflow:
+          self._marquee_offset = overflow
+          self._marquee_direction = -1
+          self._marquee_pause_timer = self._marquee_pause_duration
+        elif self._marquee_offset <= 0:
+          self._marquee_offset = 0
+          self._marquee_direction = 1
+          self._marquee_pause_timer = self._marquee_pause_duration
+
+      rl.begin_scissor_mode(int(x), int(y), int(width), int(road_size.y + 4))
+      text_pos = rl.Vector2(x - self._marquee_offset, y)
+      rl.draw_text_ex(self._font_semi_bold, road_display, text_pos, font_size, 0, COLORS.white)
+      rl.end_scissor_mode()
+
+  def _draw_vienna_sign(self, x: float, y: float, width: float, height: float, speed_str: str, speed_color: rl.Color, is_upcoming: bool = False) -> None:
+    center = rl.Vector2(x + width / 2, y + height / 2)
+    outer_radius = min(width, height) / 2
+
+    rl.draw_circle_v(center, outer_radius, COLORS.white)
+    ring_width = outer_radius * 0.18
+    rl.draw_ring(center, outer_radius - ring_width, outer_radius, 0, 360, 36, COLORS.red)
+
+    font_size = outer_radius * (0.7 if len(speed_str) >= 3 else 0.9)
+    self._draw_text_centered_fit(self._font_bold, speed_str, int(font_size), center, speed_color, width * 0.72, height * 0.50, min_size=24)
+
+  def _draw_mutcd_sign(self, x: float, y: float, width: float, height: float, speed_str: str, speed_color: rl.Color, is_upcoming: bool = False) -> None:
+    sign_rect = rl.Rectangle(x, y, width, height)
+    rl.draw_rectangle_rounded(sign_rect, 0.35, 10, COLORS.white)
+
+    inset = max(4, width * 0.05)
+    inner_rect = rl.Rectangle(x + inset, y + inset, width - inset * 2, height - inset * 2)
+    outer_radius = 0.35 * width / 2.0
+    inner_radius = outer_radius - inset
+    inner_roundness = inner_radius / (inner_rect.width / 2.0)
+    rl.draw_rectangle_rounded_lines_ex(inner_rect, inner_roundness, 10, 3, COLORS.black)
+
+    mid_x = x + width / 2
+    label_size = max(18, int(width * 0.26))
+    if is_upcoming:
+      self._draw_text_centered_fit(self._font_bold, tr("AHEAD"), int(width * 0.34), rl.Vector2(mid_x, y + height * 0.28), COLORS.black,
+                                   width * 0.94, height * 0.32, min_size=20)
+    else:
+      self._draw_text_centered_fit(self._font_bold, tr("SPEED"), label_size, rl.Vector2(mid_x, y + height * 0.20), COLORS.black,
+                                   width * 0.84, height * 0.24, min_size=16)
+      self._draw_text_centered_fit(self._font_bold, tr("LIMIT"), label_size, rl.Vector2(mid_x, y + height * 0.40), COLORS.black,
+                                   width * 0.84, height * 0.24, min_size=16)
+
+    speed_font_size = int(width * 0.60) if len(speed_str) >= 3 else int(width * 0.72)
+    self._draw_text_centered_fit(self._font_bold, speed_str, speed_font_size, rl.Vector2(mid_x, y + height * 0.72), speed_color,
+                                 width * 0.90, height * 0.52, min_size=32)
+
+  def _draw_text_centered(self, font, text, size, pos_center, color):
+    sz = measure_text_cached(font, text, size)
+    rl.draw_text_ex(font, text, rl.Vector2(pos_center.x - sz.x / 2, pos_center.y - sz.y / 2), size, 0, color)
+
+  def _draw_text_centered_fit(self, font, text, size, pos_center, color, max_width: float, max_height: float, min_size: int = 10):
+    size = self._fit_font_size(font, text, max_width, max_height, size, min_size)
+    self._draw_text_centered(font, text, size, pos_center, color)
+
+  def _fit_font_size(self, font, text: str, max_width: float, max_height: float, max_size: int | float, min_size: int) -> int:
+    size = int(max_size)
+    while size > min_size:
+      text_size = measure_text_cached(font, text, size)
+      if text_size.x <= max_width and text_size.y <= max_height:
+        return size
+      size -= 2
+    return min_size
+
+  def _offset_badge_rect(self, panel_rect: rl.Rectangle, sign_x: float, sign_y: float, sign_width: float, sign_height: float,
+                         badge_size: float, has_upcoming_limit: bool) -> rl.Rectangle:
+    if ui_state.is_metric:
+      radius = min(sign_width, sign_height) / 2
+      center_x = sign_x + sign_width / 2
+      center_y = sign_y + sign_height / 2
+      badge_x_ratio = VIENNA_BADGE_UPCOMING_X_RATIO if has_upcoming_limit else VIENNA_BADGE_X_RATIO
+      badge_center_x = center_x + radius * badge_x_ratio
+      badge_center_y = center_y + radius * VIENNA_BADGE_Y_RATIO
+      badge_x = badge_center_x - badge_size / 2
+      badge_y = badge_center_y - badge_size / 2
+    else:
+      badge_x = sign_x + sign_width - badge_size * 0.45
+      badge_y = sign_y - badge_size * 0.75
+
+    return rl.Rectangle(
+      self._clamp(
+        badge_x,
+        panel_rect.x + OFFSET_BADGE_PANEL_PADDING,
+        panel_rect.x + panel_rect.width - badge_size - OFFSET_BADGE_PANEL_PADDING,
+      ),
+      self._clamp(
+        badge_y,
+        panel_rect.y + OFFSET_BADGE_PANEL_PADDING,
+        panel_rect.y + panel_rect.height - badge_size - OFFSET_BADGE_PANEL_PADDING,
+      ),
+      badge_size,
+      badge_size,
+    )
+
+  @staticmethod
+  def _clamp(value: float, min_value: float, max_value: float) -> float:
+    return max(min_value, min(max_value, value))
+
+  def _format_distance(self, distance: float) -> str:
+    if ui_state.is_metric:
+      if distance < 50:
+        return tr("Near")
+      if distance >= 1000:
+        return f"{distance * METER_TO_KM:.1f}" + tr("km")
+      if distance < 200:
+        rounded = max(10, int(distance / 10) * 10)
+      else:
+        rounded = int(distance / 100) * 100
+      return str(rounded) + tr("m")
+    else:
+      distance_mi = distance * METER_TO_MILE
+      if distance_mi < 0.1:
+        return tr("Near")
+      return f"{distance_mi:.1f}" + tr("mi")
@@ -0,0 +1,29 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+
+from openpilot.selfdrive.ui.mici.onroad.augmented_road_view import AugmentedRoadView
+
+
+class _SuppressedConfidenceBall:
+  def render(self, *_):
+    pass
+
+
+class AugmentedRoadViewSP(AugmentedRoadView):
+  def __init__(self, **kwargs):
+    super().__init__(**kwargs)
+    self._show_confidence_ball: bool = True
+    self._real_confidence_ball = self._confidence_ball
+    self._confidence_ball = _SuppressedConfidenceBall()
+
+  def set_show_confidence_ball(self, show: bool) -> None:
+    self._show_confidence_ball = show
+
+  def _render(self, _) -> None:
+    super()._render(_)
+    if self._show_confidence_ball:
+      self._real_confidence_ball.render(self.rect)
@@ -0,0 +1,83 @@
+import pyray as rl
+
+from openpilot.common.test import OpenpilotTestCase
+from openpilot.system.ui.lib.application import MouseEvent, MousePos, gui_app
+from openpilot.system.ui.lib.scroll_panel2 import ScrollState
+from openpilot.system.ui.widgets import Widget
+from openpilot.system.ui.widgets import scroller as scroller_mod
+from openpilot.selfdrive.ui.sunnypilot.mici.widgets.scroll_panel_sp import GuiScrollPanel2SP
+
+
+class DummyScrollIndicator:
+  def update(self, *_) -> None:
+    pass
+
+  def render(self) -> None:
+    pass
+
+
+class DummyWidget(Widget):
+  def __init__(self, rect: rl.Rectangle):
+    super().__init__()
+    self.set_rect(rect)
+
+  def _render(self, _) -> None:
+    pass
+
+
+def _mouse_event(x: float, y: float, *, pressed: bool = False, released: bool = False,
+                 down: bool = True, t: float = 0.0) -> MouseEvent:
+  return MouseEvent(MousePos(x, y), 0, pressed, released, down, t)
+
+
+class TestScrollerSP(OpenpilotTestCase):
+  def test_vertical_snap_items_are_supported(self, monkeypatch):
+    monkeypatch.setattr(scroller_mod, "ScrollIndicator", DummyScrollIndicator)
+
+    scroller = scroller_mod._Scroller([], horizontal=False, snap_items=True, scroll_indicator=False)
+    scroller.set_rect(rl.Rectangle(0, 0, 100, 100))
+    scroller.scroll_panel.set_offset(-60)
+
+    captured_snap_target = None
+
+    def update(_, __, snap_target=None):
+      nonlocal captured_snap_target
+      captured_snap_target = snap_target
+      return scroller.scroll_panel.get_offset()
+
+    monkeypatch.setattr(scroller.scroll_panel, "update", update)
+
+    visible_items: list[Widget] = [
+      DummyWidget(rl.Rectangle(0, -60, 100, 100)),
+      DummyWidget(rl.Rectangle(0, 40, 100, 100)),
+    ]
+    scroller._get_scroll(visible_items, 200)
+
+    assert captured_snap_target == -100
+
+  def test_scroll_panel_sp_rejects_orthogonal_drags(self, monkeypatch):
+    panel = GuiScrollPanel2SP(horizontal=True)
+    bounds = rl.Rectangle(0, 0, 100, 100)
+
+    monkeypatch.setattr(gui_app, "_mouse_events", [_mouse_event(10, 10, pressed=True, t=1.0)])
+    panel.update(bounds, 200)
+    assert panel.state == ScrollState.PRESSED
+
+    monkeypatch.setattr(gui_app, "_mouse_events", [_mouse_event(23, 60, t=1.1)])
+    panel.update(bounds, 200)
+
+    assert panel.state == ScrollState.STEADY
+    assert panel.get_offset() == 0
+
+  def test_scroll_panel_sp_can_disable_out_of_bounds_handling(self, monkeypatch):
+    panel = GuiScrollPanel2SP(horizontal=False, handle_out_of_bounds=False)
+    bounds = rl.Rectangle(0, 0, 100, 100)
+    monkeypatch.setattr(gui_app, "_mouse_events", [])
+
+    panel.set_offset(20)
+    panel.update(bounds, 200)
+    assert panel.get_offset() == 0
+
+    panel.set_offset(-150)
+    panel.update(bounds, 200)
+    assert panel.get_offset() == -100
@@ -0,0 +1,33 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+
+import pyray as rl
+from openpilot.system.ui.lib.application import MouseEvent
+from openpilot.system.ui.lib.scroll_panel2 import GuiScrollPanel2, ScrollState
+
+
+class GuiScrollPanel2SP(GuiScrollPanel2):
+  """Scroll panel behavior for nested Mici pagers."""
+
+  def __init__(self, horizontal: bool = True, handle_out_of_bounds: bool = True) -> None:
+    super().__init__(horizontal, handle_out_of_bounds=handle_out_of_bounds)
+
+  def _handle_mouse_event(self, mouse_event: MouseEvent, bounds: rl.Rectangle, bounds_size: float,
+                          content_size: float) -> None:
+    state_before_update = self._state
+    super()._handle_mouse_event(mouse_event, bounds, bounds_size, content_size)
+
+    if self._state == ScrollState.MANUAL_SCROLL and state_before_update == ScrollState.PRESSED and \
+       self._initial_click_event is not None:
+      drag_x = abs(mouse_event.pos.x - self._initial_click_event.pos.x)
+      drag_y = abs(mouse_event.pos.y - self._initial_click_event.pos.y)
+      primary_drag = drag_x if self._horizontal else drag_y
+      cross_drag = drag_y if self._horizontal else drag_x
+      if cross_drag > primary_drag:
+        self._state = ScrollState.STEADY
+        self._velocity = 0.0
+        self._velocity_buffer.clear()
@@ -0,0 +1,16 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+
+from openpilot.system.ui.widgets.scroller import Scroller
+from openpilot.selfdrive.ui.sunnypilot.mici.widgets.scroll_panel_sp import GuiScrollPanel2SP
+
+
+class ScrollerSP(Scroller):
+  def __init__(self, **kwargs):
+    super().__init__(**kwargs)
+    inner = self._scroller
+    inner.scroll_panel = GuiScrollPanel2SP(inner._horizontal, handle_out_of_bounds=not inner._snap_items)
@@ -10,6 +10,9 @@ from openpilot.selfdrive.ui.layouts.main import MainLayout
 from openpilot.selfdrive.ui.mici.layouts.main import MiciMainLayout
 from openpilot.selfdrive.ui.ui_state import ui_state

+if gui_app.sunnypilot_ui():
+  from openpilot.selfdrive.ui.sunnypilot.mici.layouts.main import MiciMainLayoutSP as MiciMainLayout
+
 BIG_UI = gui_app.big_ui()


@@ -1 +1 @@
-#define SUNNYPILOT_VERSION "2026.09.16-4895"
+#define SUNNYPILOT_VERSION "2026.10.10-4923"
@@ -115,7 +115,7 @@ class IntelligentCruiseButtonManagement:
    self.is_ready = ready and not button_pressed

  def run(self, CS: car.CarState, CC: car.CarControl, LP_SP: custom.LongitudinalPlanSP, is_metric: bool) -> None:
-    if self.CP_SP.pcmCruiseSpeed:
+    if self.CP_SP.pcmCruiseSpeed or not self.CP_SP.intelligentCruiseButtonManagementAvailable:
      return

    self.is_metric = is_metric
@@ -136,6 +136,10 @@ def initialize_params(params) -> list[dict[str, Any]]:
  keys.extend([
    "ToyotaEnforceStockLongitudinal",
    "ToyotaStopAndGoHack",
+    "ToyotaTSS2Long",
+    "ToyotaEnhancedBsm",
+    "ToyotaAutoHold",
+    "ToyotaBrakeOnset",
  ])

  return [{k: params.get(k, return_default=True)} for k in keys]
@@ -1,14 +1,26 @@
+from opendbc.can.parser import CANParser
+from opendbc.car import create_button_events
 from opendbc.car.structs import car
+from opendbc.car.toyota.carstate import get_virtual_cruise_button, VIRTUAL_CRUISE_BUTTONS
+from openpilot.cereal import custom
 from openpilot.common.constants import CV
 from openpilot.common.parameterized import parameterized, parameterized_class
 from openpilot.common.params import Params
-from openpilot.selfdrive.car.cruise import V_CRUISE_INITIAL
+from openpilot.common.test import OpenpilotTestCase
+from openpilot.selfdrive.car.cruise import TOYOTA_VIRTUAL_CRUISE_LONG_PRESS, VCruiseHelper, V_CRUISE_INITIAL, V_CRUISE_UNSET
 from openpilot.selfdrive.car.tests.test_cruise_speed import TestVCruiseHelper
+from openpilot.sunnypilot.selfdrive.car.interfaces import initialize_params

 ButtonEvent = car.CarState.ButtonEvent
 ButtonType = car.CarState.ButtonEvent.Type


+class TestToyotaParamsHandoff(OpenpilotTestCase):
+  def test_tss2_long_tuning_param_is_forwarded_to_opendbc(self):
+    keys = {next(iter(entry)) for entry in initialize_params(Params())}
+    assert "ToyotaTSS2Long" in keys
+
+
 # TODO: test pcmCruise and pcmCruiseSpeed
@parameterized_class(('pcm_cruise', 'pcm_cruise_speed'), [(False, True)])
 class TestCustomAccIncrements(TestVCruiseHelper):
@@ -114,8 +126,8 @@ class TestCustomAccIncrements(TestVCruiseHelper):
  def test_rounding_behavior(self):
    """Test rounding behavior for 5 and 10 increments"""
    test_cases = [
-      (47, 5, 50),   # 47 -> 50 (round up to next 5)
-      (45, 5, 50),   # 45 -> 50 (already at 5, increment by 5)
+      (47, 5, 50),  # 47 -> 50 (round up to next 5)
+      (45, 5, 50),  # 45 -> 50 (already at 5, increment by 5)
      (43, 10, 50),  # 43 -> 50 (round up to next 10)
      (40, 10, 50),  # 40 -> 50 (already at 10, increment by 10)
    ]
@@ -146,3 +158,302 @@ class TestCustomAccIncrements(TestVCruiseHelper):
    initial_speed = self.v_cruise_helper.v_cruise_kph
    self.press_button_long(ButtonType.accelCruise)
    assert self.v_cruise_helper.v_cruise_kph == initial_speed + 10  # Should fallback to 10
+
+
+class TestToyotaVirtualCruiseSpeed(OpenpilotTestCase):
+  def setup_method(self):
+    self.params = Params()
+    self.params.put_bool("CustomAccIncrementsEnabled", True, block=True)
+    self.params.put("CustomAccShortPressIncrement", 5, block=True)
+    self.params.put("CustomAccLongPressIncrement", 5, block=True)
+
+    CP = car.CarParams(brand="toyota", pcmCruise=True, openpilotLongitudinalControl=True)
+    CP_SP = custom.CarParamsSP(pcmCruiseSpeed=False)
+    self.v_cruise_helper = VCruiseHelper(CP, CP_SP)
+    self.v_cruise_helper.read_custom_set_speed_params()
+    self.route_parser = CANParser("toyota_nodsu_pt_generated", [("CLUTCH", 16)], 0)
+    self.route_button = 0
+
+  @staticmethod
+  def car_state(canonical_kph, cluster_kph, *, available=True, standstill=False, gas_pressed=False, v_ego_kph=0.0, button_events=None):
+    CS = car.CarState(
+      gasPressed=gas_pressed,
+      vEgo=v_ego_kph * CV.KPH_TO_MS,
+      cruiseState={
+        "available": available,
+        "speed": canonical_kph * CV.KPH_TO_MS,
+        "speedCluster": cluster_kph * CV.KPH_TO_MS,
+        "standstill": standstill,
+      },
+    )
+    CS.buttonEvents = button_events or []
+    return CS
+
+  def seed_enabled(self, canonical_kph, cluster_kph, *, is_metric=True):
+    CS = self.car_state(canonical_kph, cluster_kph)
+    self.v_cruise_helper.update_v_cruise(CS, enabled=False, is_metric=is_metric)
+    self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=is_metric)
+    self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=is_metric)
+    assert self.v_cruise_helper.v_cruise_kph == canonical_kph
+    assert self.v_cruise_helper.v_cruise_cluster_kph == cluster_kph
+
+  def press(self, button_type, canonical_kph, cluster_kph, hold_frames=0, *, standstill=False, gas_pressed=False, v_ego_kph=0.0, is_metric=True):
+    pressed = [ButtonEvent(type=button_type, pressed=True)]
+    self.v_cruise_helper.update_v_cruise(
+      self.car_state(canonical_kph, cluster_kph, standstill=standstill, gas_pressed=gas_pressed, v_ego_kph=v_ego_kph, button_events=pressed),
+      enabled=True,
+      is_metric=is_metric,
+    )
+    for _ in range(hold_frames):
+      self.v_cruise_helper.update_v_cruise(
+        self.car_state(canonical_kph, cluster_kph, standstill=standstill, gas_pressed=gas_pressed, v_ego_kph=v_ego_kph),
+        enabled=True,
+        is_metric=is_metric,
+      )
+    released = [ButtonEvent(type=button_type, pressed=False)]
+    self.v_cruise_helper.update_v_cruise(
+      self.car_state(canonical_kph, cluster_kph, standstill=standstill, gas_pressed=gas_pressed, v_ego_kph=v_ego_kph, button_events=released),
+      enabled=True,
+      is_metric=is_metric,
+    )
+
+  def set_increments(self, short_increment, long_increment):
+    self.params.put("CustomAccShortPressIncrement", short_increment, block=True)
+    self.params.put("CustomAccLongPressIncrement", long_increment, block=True)
+    self.v_cruise_helper.read_custom_set_speed_params()
+
+  def assert_kph_almost_equal(self, actual, expected):
+    self.assertAlmostEqual(actual, expected, delta=abs(expected) * 1e-6)
+
+  def route_button_events(self, payload):
+    self.route_parser.update((1, [(0x361, bytes.fromhex(payload), 0)]))
+    current = get_virtual_cruise_button(
+      self.route_parser.vl["CLUTCH"]["CRUISE_RES"],
+      self.route_parser.vl["CLUTCH"]["CRUISE_SET"],
+    )
+    events = create_button_events(current, self.route_button, VIRTUAL_CRUISE_BUTTONS)
+    self.route_button = current
+    return events
+
+  def test_short_press_rounds_display_target_and_preserves_offset(self):
+    self.seed_enabled(27, 31)
+    self.press(ButtonType.accelCruise, 28, 32)
+
+    assert self.v_cruise_helper.v_cruise_kph == 31
+    assert self.v_cruise_helper.v_cruise_cluster_kph == 35
+
+  def test_decel_at_display_minimum_does_not_increase_target(self):
+    self.seed_enabled(26, 30)
+    self.press(ButtonType.decelCruise, 25, 29)
+
+    assert self.v_cruise_helper.v_cruise_kph == 26
+    assert self.v_cruise_helper.v_cruise_cluster_kph == 30
+
+  @parameterized.expand((52, TOYOTA_VIRTUAL_CRUISE_LONG_PRESS - 1))
+  def test_route_length_short_press_is_not_a_long_press(self, hold_frames):
+    self.set_increments(short_increment=2, long_increment=5)
+    self.seed_enabled(27, 31)
+    self.press(ButtonType.accelCruise, 28, 32, hold_frames=hold_frames)
+
+    assert self.v_cruise_helper.v_cruise_kph == 29
+    assert self.v_cruise_helper.v_cruise_cluster_kph == 33
+
+  def test_toyota_long_press_uses_route_validated_cadence_and_suppresses_release(self):
+    self.set_increments(short_increment=2, long_increment=5)
+    self.seed_enabled(27, 31)
+
+    pressed = [ButtonEvent(type=ButtonType.accelCruise, pressed=True)]
+    self.v_cruise_helper.update_v_cruise(self.car_state(31, 35, button_events=pressed), enabled=True, is_metric=True)
+    for _ in range(TOYOTA_VIRTUAL_CRUISE_LONG_PRESS):
+      self.v_cruise_helper.update_v_cruise(self.car_state(31, 35), enabled=True, is_metric=True)
+
+    assert self.v_cruise_helper.v_cruise_kph == 31
+    assert self.v_cruise_helper.v_cruise_cluster_kph == 35
+
+    released = [ButtonEvent(type=ButtonType.accelCruise, pressed=False)]
+    self.v_cruise_helper.update_v_cruise(self.car_state(31, 35, button_events=released), enabled=True, is_metric=True)
+    assert self.v_cruise_helper.v_cruise_kph == 31
+    assert self.v_cruise_helper.v_cruise_cluster_kph == 35
+
+  def test_route_4_32_second_hold_repeats_six_times(self):
+    self.seed_enabled(26, 30)
+    self.press(ButtonType.accelCruise, 30, 34, hold_frames=432)
+
+    assert self.v_cruise_helper.v_cruise_kph == 56
+    assert self.v_cruise_helper.v_cruise_cluster_kph == 60
+
+  def test_maximum_boundary_caps_pair_and_preserves_offset(self):
+    self.seed_enabled(141, 145)
+    self.press(ButtonType.accelCruise, 142, 146)
+
+    assert self.v_cruise_helper.v_cruise_kph == 141
+    assert self.v_cruise_helper.v_cruise_cluster_kph == 145
+
+    self.press(ButtonType.accelCruise, 143, 147)
+    assert self.v_cruise_helper.v_cruise_kph == 141
+    assert self.v_cruise_helper.v_cruise_cluster_kph == 145
+
+  @parameterized.expand(
+    (
+      (25, 29, ButtonType.decelCruise),
+      (141, 147, ButtonType.accelCruise),
+    )
+  )
+  def test_out_of_range_raw_pair_is_not_moved_in_opposite_direction(self, canonical_kph, cluster_kph, button_type):
+    self.seed_enabled(canonical_kph, cluster_kph)
+    self.press(button_type, canonical_kph, cluster_kph)
+
+    assert self.v_cruise_helper.v_cruise_kph == canonical_kph
+    assert self.v_cruise_helper.v_cruise_cluster_kph == cluster_kph
+
+  def test_imperial_increment_preserves_canonical_cluster_pair(self):
+    self.seed_enabled(45, 50, is_metric=False)
+    self.press(ButtonType.accelCruise, 46, 51, is_metric=False)
+
+    assert self.v_cruise_helper.v_cruise_kph == 51
+    assert self.v_cruise_helper.v_cruise_cluster_kph == 56
+
+  def test_engagement_button_held_does_not_change_target(self):
+    initial = self.car_state(27, 31)
+    self.v_cruise_helper.update_v_cruise(initial, enabled=False, is_metric=True)
+
+    pressed = [ButtonEvent(type=ButtonType.decelCruise, pressed=True)]
+    self.v_cruise_helper.update_v_cruise(self.car_state(27, 31, button_events=pressed), enabled=False, is_metric=True)
+    for _ in range(TOYOTA_VIRTUAL_CRUISE_LONG_PRESS + 10):
+      self.v_cruise_helper.update_v_cruise(self.car_state(28, 32), enabled=True, is_metric=True)
+
+    released = [ButtonEvent(type=ButtonType.decelCruise, pressed=False)]
+    self.v_cruise_helper.update_v_cruise(self.car_state(28, 32, button_events=released), enabled=True, is_metric=True)
+    self.v_cruise_helper.update_v_cruise(self.car_state(28, 32), enabled=True, is_metric=True)
+
+    assert self.v_cruise_helper.v_cruise_kph == 28
+    assert self.v_cruise_helper.v_cruise_cluster_kph == 32
+
+  def test_delayed_pcm_target_seeds_before_software_ownership(self):
+    invalid = self.car_state(0, 0)
+    self.v_cruise_helper.update_v_cruise(invalid, enabled=False, is_metric=True)
+
+    release = [ButtonEvent(type=ButtonType.decelCruise, pressed=False)]
+    for _ in range(4):
+      self.v_cruise_helper.update_v_cruise(self.car_state(0, 0, button_events=release), enabled=True, is_metric=True)
+      assert self.v_cruise_helper.v_cruise_kph == V_CRUISE_UNSET
+      assert self.v_cruise_helper.v_cruise_cluster_kph == V_CRUISE_UNSET
+
+    self.v_cruise_helper.update_v_cruise(self.car_state(27, 31), enabled=True, is_metric=True)
+    self.assert_kph_almost_equal(self.v_cruise_helper.v_cruise_kph, 27)
+    self.assert_kph_almost_equal(self.v_cruise_helper.v_cruise_cluster_kph, 31)
+
+    self.v_cruise_helper.update_v_cruise(self.car_state(28, 32), enabled=True, is_metric=True)
+    self.assert_kph_almost_equal(self.v_cruise_helper.v_cruise_kph, 27)
+    self.assert_kph_almost_equal(self.v_cruise_helper.v_cruise_cluster_kph, 31)
+
+  def test_route_payload_short_press_drives_virtual_target(self):
+    self.seed_enabled(27, 31)
+
+    pressed = self.route_button_events("a61a0000561a1a81")
+    self.v_cruise_helper.update_v_cruise(self.car_state(27, 31, button_events=pressed), enabled=True, is_metric=True)
+    for _ in range(52):
+      self.v_cruise_helper.update_v_cruise(self.car_state(28, 32), enabled=True, is_metric=True)
+
+    released = self.route_button_events("861a0000561b1a81")
+    self.v_cruise_helper.update_v_cruise(self.car_state(28, 32, button_events=released), enabled=True, is_metric=True)
+    assert self.v_cruise_helper.v_cruise_kph == 31
+    assert self.v_cruise_helper.v_cruise_cluster_kph == 35
+
+  def test_prius_route_payload_short_set_drives_virtual_target(self):
+    self.seed_enabled(31, 35)
+
+    pressed = self.route_button_events("965f000056666585")
+    self.v_cruise_helper.update_v_cruise(self.car_state(31, 35, button_events=pressed), enabled=True, is_metric=True)
+    for _ in range(45):
+      self.v_cruise_helper.update_v_cruise(self.car_state(30, 34), enabled=True, is_metric=True)
+
+    released = self.route_button_events("865f000056666585")
+    self.v_cruise_helper.update_v_cruise(self.car_state(30, 34, button_events=released), enabled=True, is_metric=True)
+    assert self.v_cruise_helper.v_cruise_kph == 26
+    assert self.v_cruise_helper.v_cruise_cluster_kph == 30
+
+  def test_prius_route_payload_standstill_res_does_not_change_target(self):
+    self.seed_enabled(27, 31)
+
+    pressed = self.route_button_events("a61b0000561c1c80")
+    self.v_cruise_helper.update_v_cruise(
+      self.car_state(27, 31, standstill=True, button_events=pressed),
+      enabled=True,
+      is_metric=True,
+    )
+    for _ in range(TOYOTA_VIRTUAL_CRUISE_LONG_PRESS):
+      self.v_cruise_helper.update_v_cruise(self.car_state(27, 31, standstill=True), enabled=True, is_metric=True)
+
+    released = self.route_button_events("865f000056666585")
+    self.v_cruise_helper.update_v_cruise(
+      self.car_state(27, 31, standstill=True, button_events=released),
+      enabled=True,
+      is_metric=True,
+    )
+    assert self.v_cruise_helper.v_cruise_kph == 27
+    assert self.v_cruise_helper.v_cruise_cluster_kph == 31
+
+  def test_route_payload_disengage_mid_hold_clears_pending_action(self):
+    self.seed_enabled(27, 31)
+
+    pressed = self.route_button_events("a61a0000561a1a81")
+    self.v_cruise_helper.update_v_cruise(self.car_state(27, 31, button_events=pressed), enabled=True, is_metric=True)
+    for _ in range(30):
+      self.v_cruise_helper.update_v_cruise(self.car_state(28, 32), enabled=True, is_metric=True)
+
+    self.v_cruise_helper.update_v_cruise(self.car_state(28, 32), enabled=False, is_metric=True)
+    released = self.route_button_events("861a0000561b1a81")
+    self.v_cruise_helper.update_v_cruise(self.car_state(28, 32, available=False, button_events=released), enabled=False, is_metric=True)
+
+    self.v_cruise_helper.update_v_cruise(self.car_state(28, 32), enabled=False, is_metric=True)
+    self.v_cruise_helper.update_v_cruise(self.car_state(28, 32), enabled=True, is_metric=True)
+    self.v_cruise_helper.update_v_cruise(self.car_state(28, 32), enabled=True, is_metric=True)
+    self.assert_kph_almost_equal(self.v_cruise_helper.v_cruise_kph, 28)
+    self.assert_kph_almost_equal(self.v_cruise_helper.v_cruise_cluster_kph, 32)
+
+  def test_standstill_resume_does_not_change_target(self):
+    self.seed_enabled(27, 31)
+    self.press(ButtonType.accelCruise, 27, 31, standstill=True)
+
+    assert self.v_cruise_helper.v_cruise_kph == 27
+    assert self.v_cruise_helper.v_cruise_cluster_kph == 31
+
+  def test_disengagement_discards_virtual_target_and_reseeds_raw_pair(self):
+    self.seed_enabled(27, 31)
+    self.press(ButtonType.accelCruise, 28, 32)
+    assert self.v_cruise_helper.v_cruise_kph == 31
+
+    raw = self.car_state(28, 32)
+    self.v_cruise_helper.update_v_cruise(raw, enabled=False, is_metric=True)
+    self.assert_kph_almost_equal(self.v_cruise_helper.v_cruise_kph, 28)
+    self.assert_kph_almost_equal(self.v_cruise_helper.v_cruise_cluster_kph, 32)
+
+    self.v_cruise_helper.update_v_cruise(raw, enabled=True, is_metric=True)
+    self.v_cruise_helper.update_v_cruise(raw, enabled=True, is_metric=True)
+    self.assert_kph_almost_equal(self.v_cruise_helper.v_cruise_kph, 28)
+    self.assert_kph_almost_equal(self.v_cruise_helper.v_cruise_cluster_kph, 32)
+
+  def test_unavailable_and_mads_handback_discard_virtual_target(self):
+    self.seed_enabled(27, 31)
+    self.press(ButtonType.accelCruise, 28, 32)
+    assert self.v_cruise_helper.v_cruise_kph == 31
+
+    self.v_cruise_helper.update_v_cruise(self.car_state(28, 32), enabled=False, is_metric=True)
+    self.assert_kph_almost_equal(self.v_cruise_helper.v_cruise_kph, 28)
+    self.assert_kph_almost_equal(self.v_cruise_helper.v_cruise_cluster_kph, 32)
+
+    self.v_cruise_helper.update_v_cruise(self.car_state(0, 0, available=False), enabled=False, is_metric=True)
+    assert self.v_cruise_helper.v_cruise_kph == V_CRUISE_UNSET
+    assert self.v_cruise_helper.v_cruise_cluster_kph == V_CRUISE_UNSET
+
+    self.v_cruise_helper.update_v_cruise(self.car_state(29, 33), enabled=False, is_metric=True)
+    self.assert_kph_almost_equal(self.v_cruise_helper.v_cruise_kph, 29)
+    self.assert_kph_almost_equal(self.v_cruise_helper.v_cruise_cluster_kph, 33)
+
+  def test_set_during_gas_override_clips_target_to_ego_speed(self):
+    self.seed_enabled(27, 31)
+    self.press(ButtonType.decelCruise, 26, 30, gas_pressed=True, v_ego_kph=50)
+
+    assert self.v_cruise_helper.v_cruise_kph == 50
+    assert self.v_cruise_helper.v_cruise_cluster_kph == 54
@@ -12,11 +12,13 @@ from openpilot.cereal import log, custom
 from opendbc.car import structs
 from openpilot.common.params import Params
 from openpilot.common.swaglog import cloudlog
+from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
 from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
 from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
 from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
 from openpilot.sunnypilot.selfdrive.controls.lib.blinker_pause_lateral import BlinkerPauseLateral
 from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_torque_v0 import LatControlTorque as LatControlTorqueV0
+from openpilot.sunnypilot.selfdrive.controls.lib.stopping_controller import StoppingController


 class ControlsExt(ModelStateBase):
@@ -31,6 +33,11 @@ class ControlsExt(ModelStateBase):
    self.CP_SP = messaging.log_from_bytes(params.get("CarParamsSP", block=True), custom.CarParamsSP)
    cloudlog.info("controlsd_ext got CarParamsSP")

+    self.stopping_controller = StoppingController(self.CP.stopAccel) if params.get_bool("SunnypilotStoppingController") else None
+    # the stock LongControl.update() has already run by the time long_accel_sp() sees it, so carry the pair it was
+    # handed last frame; a fresh LongControl starts at (off, 0.0)
+    self._stopping_prev = (LongCtrlState.off, 0.0)
+
    self.sm_services_ext = ['radarState', 'selfdriveStateSP']
    self.pm_services_ext = ['carControlSP']

@@ -47,6 +54,27 @@ class ControlsExt(ModelStateBase):
    else:
      return lac

+  def long_accel_sp(self, actuators, CS, long_plan, accel_limits: tuple[float, float]) -> None:
+    """sunnypilot: terminal-stop policy applied after the stock LongControl.update() (see stopping_controller.py)."""
+    stock_state = self.LoC.long_control_state
+    stock_accel = actuators.accel
+    prev_state, prev_accel = self._stopping_prev
+    self._stopping_prev = (stock_state, stock_accel)
+
+    if self.stopping_controller is None:
+      return
+
+    state, accel = self.stopping_controller.update(
+      prev_state, stock_state, CS, long_plan.aTarget, prev_accel, stock_accel, accel_limits, long_plan.hasLead,
+      pitch=self.calibrated_pose.orientation.pitch if self.calibrated_pose is not None else None,
+      a_long=self.calibrated_pose.acceleration.x if self.calibrated_pose is not None else None)
+    if state != stock_state:
+      self.LoC.reset()
+    self.LoC.long_control_state = state
+    self.LoC.last_output_accel = accel
+    self._stopping_prev = (state, accel)
+    actuators.accel = float(accel)
+
  def get_params_sp(self, sm: messaging.SubMaster) -> None:
    if time.monotonic() - self._param_update_time > PARAMS_UPDATE_PERIOD:
      self.blinker_pause_lateral.get_params()
@@ -0,0 +1,128 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+
+import numpy as np
+
+from openpilot.cereal import custom
+from openpilot.common.params import Params
+from openpilot.sunnypilot import get_sanitize_int_param
+
+AccelProfile = custom.LongitudinalPlanSP.AccelController.Profile
+
+MAX_ACCEL_BREAKPOINTS = [0., 3., 12,  24., 36.]  # m/s
+MAX_ACCEL_PROFILES = {
+  AccelProfile.eco:    [1.75, 1.50, 0.40, 0.15, 0.12],
+  AccelProfile.normal: [1.95, 1.80, 0.70, 0.36, 0.25],
+  AccelProfile.sport:  [2.00, 2.00, 1.20, 0.70, 0.50],
+}
+# Eco while the hybrid's engine is OFF (battery-first mode, ported from tncr18): keep wheel power m*a*v + road load under
+# the ~11-14 kW at which a Corolla hybrid starts its engine (30 Corolla Cross rlogs, threshold not SOC dependent):
+# 60 km/h 0.25, 70 km/h 0.16, 80 km/h 0.12. Launch values are the Altis eco ones. Once the engine runs there is nothing
+# to save, so the normal eco line takes over until it stops again.
+ECO_ENGINE_OFF_BP = [0., 3., 12., 16.67, 19.44, 22.22, 24., 36.]  # m/s
+# The first two rows follow the eco line above: battery-first may never launch harder than eco. They still held the
+# pre-retune eco values (1.85 / 1.55) in the source this was ported from, which put engine-off above eco at 0-3 m/s.
+ECO_ENGINE_OFF_MAX_ACCEL = [1.75, 1.50, 0.35, 0.25, 0.16, 0.12, 0.08, 0.06]
+# Cruise deceleration to a lowered set speed (no lead). Normal/sport: one CONSTANT decel latched when the gap opens
+# (gap / response time, never gentler than CRUISE_DECEL_ACCEL) and held, so the car slows on a straight line instead of
+# the proportional gap/time law, which braked hardest at the start and dragged a long tail; more presses = firmer.
+# Eco (driver request 2026-09-25/26): never hurry to the new set speed however far it is or however many presses.
+# Above 70 km/h: pure coasting - the target decel IS the planner's pitch-aware coast accel (get_coast_accel), so the
+# PCM neither adds throttle nor regen; the speed table above 70 is only the fallback when no coast value is given
+# (road-load estimate of a 1500 kg Corolla Cross: -0.22 at 70, -0.30 at 90, -0.40 at 120). Below 60 km/h -0.40
+# (regen), blended into coasting between 60 and 70 as the speed falls. Whether -0.40 is too firm is for the road test.
+CRUISE_DECEL_RESPONSE_TIME = {  # seconds to close the gap (normal/sport)
+  AccelProfile.normal: 3.5,
+  AccelProfile.sport: 3.0,
+}
+CRUISE_DECEL_ACCEL = {  # m/s^2; gentlest constant decel (normal/sport)
+  AccelProfile.normal: -0.50,
+  AccelProfile.sport: -0.65,
+}
+ECO_CRUISE_DECEL_BP = [0., 16.67, 19.44, 25.0, 33.3]   # m/s (0, 60, 70, 90, 120 km/h)
+ECO_CRUISE_DECEL_V = [-0.40, -0.40, -0.22, -0.30, -0.40]  # m/s^2; the >= 70 km/h part is only the FALLBACK coast estimate
+ECO_COAST_BLEND_BP = [16.67, 19.44]  # m/s: -0.40 at 60 km/h blends into pure coasting by 70 km/h
+ECO_COAST_MIN, ECO_COAST_MAX = -1.2, -0.05  # m/s^2; sanity clip on the planner's pitch-aware coast accel
+# Eco lead pull-away boost (driver 2026-10-01 night): below 80 km/h, when a detected lead is pulling away (gap opening),
+# eco may accelerate a little harder for a while so the car keeps up, eased in and out; with no lead nothing changes.
+# The boost scales with how fast the gap opens, fades out from 70 to 80 km/h, and never lifts eco above the normal line.
+ECO_BOOST_DV_BP = [0.3, 2.5]     # m/s, lead speed minus ego speed
+ECO_BOOST_V = [0.0, 0.35]        # m/s^2 added to the eco limit
+ECO_BOOST_FADE_BP = [19.44, 22.22]  # m/s (70 -> 80 km/h): full -> none
+ECO_BOOST_D_MAX = 80.0           # m, only a lead this close counts
+ECO_BOOST_PROB_MIN = 0.5
+ECO_BOOST_RISE = 0.25            # m/s^3, how fast the boost builds
+ECO_BOOST_FALL = 0.5             # m/s^3, how fast it fades
+ECO_BOOST_DT = 0.05              # s, planner step
+CRUISE_DECEL_TAPER_TIME = 1.0  # s; inside |decel| * this of the target the decel eases off proportionally (no overshoot)
+
+
+class AccelController:
+  def __init__(self):
+    self.params = Params()
+    self._cruise_decel: float | None = None
+    self._cruise_decel_target: float | None = None
+    self._boost = 0.0
+    self.update()
+
+  def update(self) -> None:
+    self._profile = get_sanitize_int_param("AccelPersonality", AccelProfile.eco, AccelProfile.sport, self.params)
+    self._enabled = self.params.get_bool("AccelPersonalityEnabled")
+
+  @property
+  def profile(self) -> int:
+    return self._profile
+
+  def is_enabled(self) -> bool:
+    return self._enabled
+
+  def get_max_accel(self, v_ego: float, engine_off: bool = False, lead=None) -> float:
+    if engine_off and self._profile == AccelProfile.eco:
+      base = float(np.interp(max(0.0, v_ego), ECO_ENGINE_OFF_BP, ECO_ENGINE_OFF_MAX_ACCEL))
+    else:
+      base = float(np.interp(max(0.0, v_ego), MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[self._profile]))
+    if self._profile != AccelProfile.eco:
+      self._boost = 0.0
+      return base
+    return base + self._eco_lead_boost(v_ego, base, lead)
+
+  def _eco_lead_boost(self, v_ego: float, base: float, lead) -> float:
+    target = 0.0
+    if lead is not None and lead.present and lead.modelProb > ECO_BOOST_PROB_MIN and lead.dRel < ECO_BOOST_D_MAX:
+      target = float(np.interp(lead.vLead - v_ego, ECO_BOOST_DV_BP, ECO_BOOST_V))
+      target *= float(np.interp(v_ego, ECO_BOOST_FADE_BP, [1.0, 0.0]))
+      normal = float(np.interp(max(0.0, v_ego), MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[AccelProfile.normal]))
+      target = float(np.clip(target, 0.0, max(normal - base, 0.0)))
+    step = (ECO_BOOST_RISE if target > self._boost else ECO_BOOST_FALL) * ECO_BOOST_DT
+    self._boost = float(np.clip(target, self._boost - step, self._boost + step))
+    return self._boost
+
+  def get_cruise_target(self, v_ego: float, v_target: float, accel_coast: float | None = None) -> float:
+    if not np.isfinite(v_target) or v_target <= 0.0 or v_target >= v_ego:
+      self._cruise_decel = None
+      self._cruise_decel_target = None
+      return v_target
+
+    target_delta = v_target - v_ego
+    if self._profile == AccelProfile.eco:
+      # speed-scheduled, independent of the gap and of how many presses opened it
+      decel = float(np.interp(v_ego, ECO_CRUISE_DECEL_BP, ECO_CRUISE_DECEL_V))
+      if accel_coast is not None and np.isfinite(accel_coast) and accel_coast < 0.0:
+        # >= 70 km/h: coast exactly (pitch-aware); 60-70 km/h: blend from the low-speed value into coasting
+        coast = float(np.clip(accel_coast, ECO_COAST_MIN, ECO_COAST_MAX))
+        w = float(np.interp(v_ego, ECO_COAST_BLEND_BP, [0.0, 1.0]))
+        decel = (1.0 - w) * ECO_CRUISE_DECEL_V[1] + w * coast
+    else:
+      if self._cruise_decel is None or self._cruise_decel_target != v_target:
+        # a new (or changed) gap: latch one constant decel for this episode
+        self._cruise_decel = min(CRUISE_DECEL_ACCEL[self._profile], target_delta / CRUISE_DECEL_RESPONSE_TIME[self._profile])
+        self._cruise_decel_target = v_target
+      decel = self._cruise_decel
+
+    # taper only once the target is close (both terms are negative; max = the gentler one) so the speed lands cleanly
+    decel = max(decel, target_delta / CRUISE_DECEL_TAPER_TIME)
+    return float(v_ego + decel)
@@ -0,0 +1,142 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+
+from types import SimpleNamespace
+
+import numpy as np
+
+from opendbc.car.interfaces import ACCEL_MAX
+from openpilot.common.params import Params
+from openpilot.common.test import OpenpilotTestCase
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import (
+  AccelController, AccelProfile, CRUISE_DECEL_ACCEL, CRUISE_DECEL_RESPONSE_TIME, ECO_CRUISE_DECEL_BP,
+  ECO_CRUISE_DECEL_V, ECO_ENGINE_OFF_MAX_ACCEL, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES,
+)
+
+
+class TestAccelController(OpenpilotTestCase):
+  def setUp(self):
+    self.params = Params()
+    self.params.put_bool("AccelPersonalityEnabled", True, block=True)
+    self.params.put("AccelPersonality", AccelProfile.normal, block=True)
+
+  def set_profile(self, profile: int) -> AccelController:
+    self.params.put("AccelPersonality", profile, block=True)
+    return AccelController()
+
+  def test_table_breakpoints(self):
+    for profile, values in MAX_ACCEL_PROFILES.items():
+      controller = self.set_profile(profile)
+      for speed, expected in zip(MAX_ACCEL_BREAKPOINTS, values, strict=True):
+        assert controller.get_max_accel(speed) == expected
+
+  def test_profiles_are_ordered_monotonic_and_bounded(self):
+    controllers = {
+      profile: self.set_profile(profile)
+      for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport)
+    }
+    previous = {profile: float("inf") for profile in controllers}
+
+    for speed in np.linspace(0.0, 55.0, 551):
+      values = {profile: controller.get_max_accel(speed) for profile, controller in controllers.items()}
+      assert 0.0 <= values[AccelProfile.eco] <= values[AccelProfile.normal] <= values[AccelProfile.sport] <= ACCEL_MAX
+      for profile, value in values.items():
+        assert value <= previous[profile]
+        previous[profile] = value
+
+  def test_engine_off_line_only_lowers_eco(self):
+    speeds = np.linspace(0.0, 40.0, 401)
+    eco = self.set_profile(AccelProfile.eco)
+    for speed in speeds:
+      assert eco.get_max_accel(speed, engine_off=True) <= eco.get_max_accel(speed) + 1e-12
+    for speed, expected in zip(MAX_ACCEL_BREAKPOINTS, ECO_ENGINE_OFF_MAX_ACCEL, strict=True):
+      assert eco.get_max_accel(speed, engine_off=True) == expected
+
+    for profile in (AccelProfile.normal, AccelProfile.sport):
+      controller = self.set_profile(profile)
+      for speed in speeds:
+        assert controller.get_max_accel(speed, engine_off=True) == controller.get_max_accel(speed)
+
+  def test_engine_off_eco_line_respects_hybrid_power_budget(self):
+    controller = self.set_profile(AccelProfile.eco)
+    for kph, budget_kw in ((60, 11.5), (70, 11.5), (80, 13.5)):
+      speed = kph / 3.6
+      road_load = (0.010 * 1500 * 9.81 + 0.5 * 1.2 * 0.82 * speed * speed) * speed
+      wheel_power = 1500 * controller.get_max_accel(speed, engine_off=True) * speed + road_load
+      assert wheel_power <= budget_kw * 1e3, kph
+
+  def test_lead_boost_is_eco_only_and_absent_without_a_lead(self):
+    # the eco lead pull-away boost: no lead is the plain table lookup, and normal/sport ignore the lead entirely
+    eco = self.set_profile(AccelProfile.eco)
+    assert eco.get_max_accel(1.0, lead=None) == eco.get_max_accel(1.0)
+    assert eco.get_max_accel(20.0, lead=None) == eco.get_max_accel(20.0)
+
+    normal = self.set_profile(AccelProfile.normal)
+    lead = SimpleNamespace(present=True, modelProb=0.9, dRel=30.0, vLead=23.0)
+    assert normal.get_max_accel(20.0, lead=lead) == normal.get_max_accel(20.0)
+
+  def test_sport_uses_openpilot_accel_max_at_launch(self):
+    controller = self.set_profile(AccelProfile.sport)
+    assert controller.get_max_accel(0.0) == ACCEL_MAX
+
+  def test_negative_speed_uses_standstill_value(self):
+    controller = self.set_profile(AccelProfile.sport)
+    assert controller.get_max_accel(-1.0) == MAX_ACCEL_PROFILES[AccelProfile.sport][0]
+
+  def test_eco_cruise_decel_is_speed_scheduled_not_gap_scheduled(self):
+    controller = self.set_profile(AccelProfile.eco)
+    a_small = controller.get_cruise_target(22.0, 20.0) - 22.0
+    a_big = controller.get_cruise_target(22.0, 12.0) - 22.0
+    assert np.isclose(a_small, a_big)
+    assert np.isclose(controller.get_cruise_target(19.44, 10.0) - 19.44, -0.22)
+    assert np.isclose(controller.get_cruise_target(15.0, 10.0) - 15.0, -0.40)
+    assert -0.40 < controller.get_cruise_target(18.0, 10.0) - 18.0 < -0.22
+    # with the planner's coast accel available, >= 70 km/h coasts exactly (pitch-aware), 60-70 blends into it
+    assert np.isclose(controller.get_cruise_target(22.0, 10.0, accel_coast=-0.27) - 22.0, -0.27)
+    assert np.isclose(controller.get_cruise_target(15.0, 10.0, accel_coast=-0.27) - 15.0, -0.40)
+    assert -0.40 < controller.get_cruise_target(18.0, 10.0, accel_coast=-0.27) - 18.0 < -0.27
+    # normal ignores the coast value
+    normal = self.set_profile(AccelProfile.normal)
+    assert normal.get_cruise_target(22.0, 20.0, accel_coast=-0.27) == normal.get_cruise_target(22.0, 20.0)
+  def test_normal_and_sport_cruise_decel_latch_then_taper(self):
+    for profile in (AccelProfile.normal, AccelProfile.sport):
+      controller = self.set_profile(profile)
+      v_target = 10.0
+      a1 = controller.get_cruise_target(14.0, v_target) - 14.0
+      a2 = controller.get_cruise_target(13.0, v_target) - 13.0
+      expected = min(CRUISE_DECEL_ACCEL[profile], (v_target - 14.0) / CRUISE_DECEL_RESPONSE_TIME[profile])
+      assert a1 < 0 and np.isclose(a1, a2)
+      assert np.isclose(a1, expected)
+
+      a_close = controller.get_cruise_target(10.2, v_target) - 10.2
+      assert a1 < a_close < 0
+      assert controller.get_cruise_target(10.0, v_target) == v_target
+      # A new, lower target re-latches a firmer deceleration.
+      a_new_target = controller.get_cruise_target(14.0, v_target - 2.0) - 14.0
+      assert a_new_target <= a1
+      assert CRUISE_DECEL_RESPONSE_TIME[profile] >= 3.0
+
+  def test_non_decel_cruise_requests_clear_latched_deceleration(self):
+    controller = self.set_profile(AccelProfile.normal)
+    controller.get_cruise_target(14.0, 10.0)
+    assert controller.get_cruise_target(10.0, 10.0) == 10.0
+    assert np.isclose(controller.get_cruise_target(14.0, 10.0) - 14.0,
+                      (10.0 - 14.0) / CRUISE_DECEL_RESPONSE_TIME[AccelProfile.normal])
+
+  def test_eco_decel_table_is_valid(self):
+    assert len(ECO_CRUISE_DECEL_BP) == len(ECO_CRUISE_DECEL_V)
+    assert all(decel < 0.0 for decel in ECO_CRUISE_DECEL_V)
+
+  def test_params_refresh(self):
+    controller = self.set_profile(AccelProfile.normal)
+    self.params.put("AccelPersonality", AccelProfile.sport, block=True)
+    controller.update()
+    assert controller.profile == AccelProfile.sport
+
+    self.params.put_bool("AccelPersonalityEnabled", False, block=True)
+    controller.update()
+    assert not controller.is_enabled()
@@ -0,0 +1,237 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+
+from collections.abc import Callable
+
+import numpy as np
+
+from openpilot.common.constants import CV
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+from openpilot.common.test import OpenpilotTestCase
+from openpilot.selfdrive.controls.lib.drive_helpers import should_stop
+from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MAX_BP, J_CRUISE_VALS, get_cruise_accel
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalPlanSource, T_IDXS as T_IDXS_MPC
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import (
+  AccelController, AccelProfile, ECO_CRUISE_DECEL_BP, ECO_CRUISE_DECEL_V, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES,
+)
+from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PRIUS_TSS2_ROUTE_MODEL, PlantSP
+
+
+class CarParams:
+  steerRatio = 15.0
+  wheelbase = 2.7
+
+
+def _set_mpc_acceleration(plant: PlantSP, acceleration: float = 2.0) -> None:
+  def update(_radar_state, **_kwargs):
+    mpc = plant.planner.mpc
+    mpc.source = LongitudinalPlanSource.lead0
+    mpc.v_solution[:] = mpc.x0[1] + acceleration * T_IDXS_MPC
+    mpc.a_solution.fill(acceleration)
+    mpc.j_solution.fill(0.0)
+
+  plant.planner.mpc.update = update
+
+
+def run_profile(profile: int, *, enabled: bool = True, speed: float = 0.0, v_cruise: float = 30.0,
+                v_cruise_fn: Callable[[int], float] | None = None, e2e: bool = False, steps: int = 120,
+                speed_noise: float = 0.0, seed: int = 0):
+  params = Params()
+  params.put_bool("AccelPersonalityEnabled", enabled, block=True)
+  params.put("AccelPersonality", profile, block=True)
+  controller = AccelController()
+  rng = np.random.default_rng(seed)
+
+  accel = 0.0
+  rows = []
+  for frame in range(steps):
+    target_speed = v_cruise if v_cruise_fn is None else v_cruise_fn(frame)
+    measured = speed + (float(rng.normal(0.0, speed_noise)) if speed_noise else 0.0)
+    accel = get_cruise_accel(e2e, target_speed, measured, accel, 0.0, CarParams(), DT_MDL, 2.0, True)
+    if accel > 0.0:
+      accel = min(accel, controller.get_max_accel(measured))
+    speed = max(0.0, speed + accel * DT_MDL)
+    rows.append((speed, accel, should_stop(speed, accel)))
+  return rows
+
+
+def run_vehicle_profile(profile: int, duration: float = 80.0, enabled: bool = True, speed: float = 0.0,
+                        v_cruise_fn: Callable[[float], float] | None = None):
+  params = Params()
+  params.put_bool("AccelPersonalityEnabled", enabled, block=True)
+  params.put("AccelPersonality", profile, block=True)
+
+  plant = PlantSP(speed=speed, actuator_model=PRIUS_TSS2_ROUTE_MODEL, run_long_control=True)
+  _set_mpc_acceleration(plant)
+  rows = []
+  while plant.current_time < duration:
+    v_cruise = 25.0 if v_cruise_fn is None else v_cruise_fn(plant.current_time)
+    result = plant.step(v_cruise=v_cruise)
+    rows.append((plant.current_time, result["speed"], result["a_target"], result["actuator_command"], result["acceleration"]))
+  return np.asarray(rows)
+
+
+class TestAccelControllerClosedLoop(OpenpilotTestCase):
+  def test_profiles_are_immediate_smooth_and_clearly_distinct(self):
+    traces = {profile: run_vehicle_profile(profile) for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport)}
+    stock = run_vehicle_profile(AccelProfile.normal, enabled=False)
+
+    def crossing(trace, speed):
+      return float(trace[np.flatnonzero(trace[:, 1] >= speed)[0], 0])
+
+    time_to_20 = {profile: crossing(trace, 20.0 * CV.MPH_TO_MS) for profile, trace in traces.items()}
+    time_to_50 = {profile: crossing(trace, 50.0 * CV.MPH_TO_MS) for profile, trace in traces.items()}
+    first_motion = {profile: int(np.flatnonzero(trace[:, 1] > 0.01)[0]) for profile, trace in traces.items()}
+
+    self.assertEqual(len(set(first_motion.values())), 1)
+    self.assertTrue(all(trace[0, 2] > 0.0 and trace[1, 3] > 0.0 for trace in traces.values()))
+    self.assertLess(time_to_20[AccelProfile.eco], 10.0)
+    self.assertLess(time_to_50[AccelProfile.eco], 65.0)
+    self.assertGreaterEqual(time_to_20[AccelProfile.eco] - time_to_20[AccelProfile.normal], 0.5)
+    self.assertGreaterEqual(time_to_20[AccelProfile.normal] - time_to_20[AccelProfile.sport], 0.5)
+    self.assertGreaterEqual(time_to_50[AccelProfile.eco] - time_to_50[AccelProfile.normal], 2.0)
+    self.assertGreaterEqual(time_to_50[AccelProfile.normal] - time_to_50[AccelProfile.sport], 3.0)
+
+    stock_peak_jerk = float(np.max(np.abs(np.diff(stock[:, 3])) / DT_MDL))
+    for profile, trace in traces.items():
+      command_jerk = np.abs(np.diff(trace[:, 3])) / DT_MDL
+      self.assertLessEqual(float(np.max(command_jerk)), stock_peak_jerk + 1e-9, profile)
+
+      settled = np.flatnonzero(trace[:, 1] >= 25.0 - 0.15)
+      self.assertGreater(len(settled), 0)
+      settled_trace = trace[settled[0]:]
+      self.assertGreaterEqual(float(np.min(settled_trace[:, 3])), -0.05)
+      self.assertLessEqual(float(np.max(trace[:, 1])), 25.0 + 1e-9)
+
+  def test_blended_positive_model_request_uses_profile_cruise_cap(self):
+    params = Params()
+    params.put_bool("DynamicExperimentalControl", False, block=True)
+    params.put_bool("AccelPersonalityEnabled", True, block=True)
+    params.put("AccelPersonality", AccelProfile.eco, block=True)
+
+    def request_acceleration(_current_time: float, _speed: float, _acceleration: float) -> tuple[float, bool]:
+      return 2.0, False
+
+    plant = PlantSP(speed=15.0, e2e=True, model_action_fn=request_acceleration)
+    _set_mpc_acceleration(plant)
+    results = [plant.step(v_cruise=35.0) for _ in range(20)]
+    settled = results[-1]
+    eco_limit = 0.85 * float(np.interp(settled["published_v_ego"], MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[AccelProfile.eco]))
+
+    self.assertTrue(settled["controller_active"])
+    self.assertEqual(settled["mpc_source"], LongitudinalPlanSource.cruise)
+    self.assertAlmostEqual(settled["a_target"], eco_limit, delta=0.01)
+    self.assertLess(settled["a_target"], settled["model_action"]["desiredAcceleration"])
+
+  def test_normal_launch_is_faster_than_eco(self):
+    eco = run_profile(AccelProfile.eco, speed=4.0, steps=120)
+    normal = run_profile(AccelProfile.normal, speed=4.0, steps=120)
+    self.assertGreater(normal[-1][0], eco[-1][0])
+
+  def test_zero_speed_stop_request_is_unchanged(self):
+    for e2e in (False, True):
+      stock = run_profile(AccelProfile.normal, enabled=False, speed=20.0, v_cruise=0.0, e2e=e2e, steps=100)
+      for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport):
+        self.assertEqual(run_profile(profile, speed=20.0, v_cruise=0.0, e2e=e2e, steps=100), stock)
+
+  def test_eco_cruise_decel_uses_gentle_speed_schedule(self):
+    params = Params()
+    params.put_bool("AccelPersonalityEnabled", True, block=True)
+    params.put("AccelPersonality", AccelProfile.eco, block=True)
+    controller = AccelController()
+
+    speeds = (15.0, 18.0, 19.44, 25.0, 33.3)
+    decels = [controller.get_cruise_target(speed, speed - 10.0) - speed for speed in speeds]
+    expected = np.interp(speeds, ECO_CRUISE_DECEL_BP, ECO_CRUISE_DECEL_V)
+    np.testing.assert_allclose(decels, expected)
+    self.assertTrue(all(decel > -0.5 for decel in decels))
+
+  def test_normal_cruise_decel_remains_gradual(self):
+    params = Params()
+    params.put_bool("AccelPersonalityEnabled", True, block=True)
+    params.put("AccelPersonality", AccelProfile.normal, block=True)
+    controller = AccelController()
+    target = 25.0 - 5.0 * CV.MPH_TO_MS
+    stock = get_cruise_accel(False, target, 25.0, 0.0, 0.0, CarParams(), DT_MDL, 2.0, True)
+    shaped = controller.get_cruise_target(25.0, target)
+    self.assertLess(shaped, 25.0)
+    self.assertGreater(shaped, target)
+    self.assertLess(shaped - 25.0, 0.0)
+    self.assertLess(shaped - 25.0, -0.3)
+    self.assertGreater(abs(stock), 0.0)
+
+  def test_blended_launch_respects_profiles(self):
+    traces = {
+      profile: run_profile(profile, v_cruise=8.0, e2e=True, steps=180)
+      for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport)
+    }
+    time_to_five = {
+      profile: next(frame for frame, row in enumerate(rows) if row[0] >= 5.0) * DT_MDL
+      for profile, rows in traces.items()
+    }
+
+    self.assertLess(time_to_five[AccelProfile.sport], time_to_five[AccelProfile.normal])
+    self.assertLess(time_to_five[AccelProfile.normal], time_to_five[AccelProfile.eco])
+
+  def test_launch_ordering_without_departure_delay(self):
+    stock = run_profile(AccelProfile.normal, enabled=False, v_cruise=8.0, steps=160)
+    traces = {
+      profile: run_profile(profile, v_cruise=8.0, steps=160)
+      for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport)
+    }
+    first_motion = {
+      profile: next(frame for frame, row in enumerate(rows) if row[0] > 0.01)
+      for profile, rows in traces.items()
+    }
+    time_to_five = {
+      profile: next(frame for frame, row in enumerate(rows) if row[0] >= 5.0) * DT_MDL
+      for profile, rows in traces.items()
+    }
+    stock_first_motion = next(frame for frame, row in enumerate(stock) if row[0] > 0.01)
+
+    self.assertEqual(len(set(first_motion.values())), 1)
+    self.assertTrue(all(frame == stock_first_motion for frame in first_motion.values()))
+    self.assertGreaterEqual(time_to_five[AccelProfile.eco] - time_to_five[AccelProfile.normal], 0.1)
+    self.assertGreaterEqual(time_to_five[AccelProfile.normal], time_to_five[AccelProfile.sport])
+
+  def test_road_speed_catchup_stays_useful(self):
+    traces = {
+      profile: run_profile(profile, speed=20.0, v_cruise=30.0, steps=100)
+      for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport)
+    }
+    gains = {profile: rows[-1][0] - 20.0 for profile, rows in traces.items()}
+    self.assertGreater(gains[AccelProfile.normal] - gains[AccelProfile.eco], 0.05)
+    self.assertGreater(gains[AccelProfile.sport] - gains[AccelProfile.normal], 0.1)
+
+  def test_full_catchup_trace_respects_stock_jerk(self):
+    for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport):
+      rows = run_profile(profile, v_cruise=30.0, steps=300)
+      previous_speed = 0.0
+      previous_accel = 0.0
+      for speed, accel, _should_stop in rows:
+        jerk_step = float(np.interp(previous_speed, A_CRUISE_MAX_BP, J_CRUISE_VALS)) * DT_MDL
+        self.assertLessEqual(abs(accel - previous_accel), jerk_step + 1e-12)
+        previous_speed = speed
+        previous_accel = accel
+
+  def test_stop_release_frame_is_profile_independent(self):
+    def target_speed(frame: int) -> float:
+      return 0.0 if frame < 20 else 8.0
+
+    traces = {
+      profile: run_profile(profile, v_cruise_fn=target_speed, steps=80)
+      for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport)
+    }
+    release_frames = {
+      profile: next(frame for frame, row in enumerate(rows) if frame >= 20 and not row[2])
+      for profile, rows in traces.items()
+    }
+    stock = run_profile(AccelProfile.normal, enabled=False, v_cruise_fn=target_speed, steps=80)
+    stock_release_frame = next(frame for frame, row in enumerate(stock) if frame >= 20 and not row[2])
+    self.assertEqual(len(set(release_frames.values())), 1)
+    self.assertTrue(all(frame == stock_release_frame for frame in release_frames.values()))
@@ -0,0 +1,129 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+
+import numpy as np
+
+from openpilot.common.params import Params
+from openpilot.common.test import OpenpilotTestCase
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import (
+  AccelController, AccelProfile, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES,
+)
+
+
+class TestEcoLeadBoost(OpenpilotTestCase):
+  """Eco lead pull-away boost (driver 2026-10-01 night)."""
+
+  @staticmethod
+  def _ac(profile):
+    from types import SimpleNamespace
+    ac = AccelController.__new__(AccelController)
+    ac._profile, ac._enabled, ac._boost = profile, True, 0.0
+    ac._cruise_decel = ac._cruise_decel_target = None
+    return ac, SimpleNamespace
+
+  def test_no_lead_unchanged(self):
+    ac, _ = self._ac(AccelProfile.eco)
+    v = 15.0
+    base = float(np.interp(v, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[AccelProfile.eco]))
+    for _ in range(100):
+      assert ac.get_max_accel(v, False, None) == base
+
+  def test_pulling_away_lead_boosts_smoothly_within_normal(self):
+    ac, NS = self._ac(AccelProfile.eco)
+    v = 15.0
+    lead = NS(present=True, modelProb=0.9, dRel=30.0, vLead=v + 3.0)
+    base = float(np.interp(v, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[AccelProfile.eco]))
+    normal = float(np.interp(v, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[AccelProfile.normal]))
+    out = [ac.get_max_accel(v, False, lead) for _ in range(60)]
+    steps = np.diff([base] + out)
+    assert max(steps) <= 0.25 * 0.05 + 1e-9          # eased in
+    assert out[-1] > base + 0.2 and out[-1] <= normal + 1e-9
+    lead.present = False
+    out2 = [ac.get_max_accel(v, False, lead) for _ in range(60)]
+    assert min(np.diff([out[-1]] + out2)) >= -0.5 * 0.05 - 1e-9   # eased out
+    assert abs(out2[-1] - base) < 1e-9
+
+  def test_no_boost_at_80_kph_or_for_a_closing_lead(self):
+    ac, NS = self._ac(AccelProfile.eco)
+    v = 80 / 3.6
+    lead = NS(present=True, modelProb=0.9, dRel=30.0, vLead=v + 3.0)
+    base = float(np.interp(v, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[AccelProfile.eco]))
+    assert abs([ac.get_max_accel(v, False, lead) for _ in range(40)][-1] - base) < 1e-9
+    ac, NS = self._ac(AccelProfile.eco)
+    v = 15.0
+    lead = NS(present=True, modelProb=0.9, dRel=30.0, vLead=v - 1.0)
+    base = float(np.interp(v, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[AccelProfile.eco]))
+    assert abs([ac.get_max_accel(v, False, lead) for _ in range(40)][-1] - base) < 1e-9
+
+
+class _Lead:
+  def __init__(self, present=True, modelProb=0.9, dRel=30.0, vLead=18.0):
+    self.present, self.modelProb, self.dRel, self.vLead = present, modelProb, dRel, vLead
+
+
+class _Radar:
+  def __init__(self, lead):
+    self.leadOne = lead
+
+
+class _Sink:
+  def clear(self):
+    pass
+
+  def update(self, *args, **kwargs):
+    pass
+
+
+class TestPlannerLeadWiring(OpenpilotTestCase):
+  """The boost needs a lead, so the planner has to carry radarState.leadOne into get_max_accel_override."""
+
+  def setUp(self):
+    self.params = Params()
+    self.params.put_bool("AccelPersonalityEnabled", True, block=True)
+    self.params.put("AccelPersonality", AccelProfile.eco, block=True)
+
+  @staticmethod
+  def _planner():
+    from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP
+
+    planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
+    planner.accel_controller = AccelController()
+    planner.accel_controller_active = False
+    planner.lead_one = None
+    return planner
+
+  @staticmethod
+  def _settle(planner, v_ego, lead=None, n=60):
+    return [planner.get_max_accel_override(v_ego, lead=lead) for _ in range(n)][-1]
+
+  def test_update_stores_the_radar_lead(self):
+    planner = self._planner()
+    planner.events_sp = _Sink()
+    planner.e2e_alerts_helper = _Sink()
+    lead = _Lead()
+    planner.update({"radarState": _Radar(lead)})
+    assert planner.lead_one is lead
+
+  def test_stored_lead_feeds_the_boost_and_an_explicit_lead_wins(self):
+    v = 15.0
+    lead = _Lead(vLead=v + 3.0)
+    planner = self._planner()
+    planner.lead_one = lead
+    boosted = self._settle(planner, v)
+
+    planner.accel_controller._boost = 0.0
+    planner.lead_one = _Lead(present=False)
+    base = self._settle(planner, v)
+    assert boosted > base + 0.2
+
+    planner.accel_controller._boost = 0.0
+    explicit = self._settle(planner, v, lead=lead)
+    assert abs(explicit - boosted) < 1e-9
+
+  def test_disabled_accel_controller_returns_no_override(self):
+    self.params.put_bool("AccelPersonalityEnabled", False, block=True)
+    assert self._planner().get_max_accel_override(15.0, lead=_Lead()) is None
@@ -1,17 +0,0 @@
-class WMACConstants:
-  # Lead detection parameters
-  LEAD_WINDOW_SIZE = 6  # Stable detection window
-  LEAD_PROB = 0.45  # Balanced threshold for lead detection
-
-  # Slow down detection parameters
-  SLOW_DOWN_WINDOW_SIZE = 5  # Responsive but stable
-  SLOW_DOWN_PROB = 0.3  # Balanced threshold for slow down scenarios
-
-  # Optimized slow down distance curve - smooth and progressive
-  SLOW_DOWN_BP = [0., 10., 20., 30., 40., 50., 55., 60.]
-  SLOW_DOWN_DIST = [32., 46., 64., 86., 108., 130., 145., 165.]
-
-  # Slowness detection parameters
-  SLOWNESS_WINDOW_SIZE = 10  # Stable slowness detection
-  SLOWNESS_PROB = 0.55  # Clear threshold for slowness
-  SLOWNESS_CRUISE_OFFSET = 1.025  # Conservative cruise speed offset
@@ -4,192 +4,120 @@ Copyright (c) 2021-, rav4kumar, sunnypilot, and a number of other contributors.
 This file is part of sunnypilot and is licensed under the MIT License.
 See the LICENSE.md file in the root directory for more details.
 """
-# Version = 2025-6-30
+from dataclasses import dataclass
+from typing import Literal
+
+import numpy as np

 from openpilot.cereal import messaging
 from opendbc.car import structs
-from numpy import interp
 from openpilot.common.params import Params
-from openpilot.common.realtime import DT_MDL
-from openpilot.sunnypilot.selfdrive.controls.lib.dec.constants import WMACConstants
-from typing import Literal
+from openpilot.selfdrive.modeld.constants import ModelConstants

-# d-e2e, from modeldata.h
-TRAJECTORY_SIZE = 33
-SET_MODE_TIMEOUT = 15
-
-# Define the valid mode types
 ModeType = Literal['acc', 'blended']

+_DECEL_LOOKAHEAD_MIN_T = 1.0
+_DECEL_LOOKAHEAD_MAX_T = 6.0
+_T_IDXS = np.array(ModelConstants.T_IDXS)
+_DECEL_IDX = np.where((_T_IDXS >= _DECEL_LOOKAHEAD_MIN_T) & (_T_IDXS <= _DECEL_LOOKAHEAD_MAX_T))[0]
+_DECEL_INV_T = 1.0 / _T_IDXS[_DECEL_IDX]

-class SmoothKalmanFilter:
-  """Enhanced Kalman filter with smoothing for stable decision making."""
+DECEL_INTENT_A_HINT = 0.35
+DECEL_INTENT_A_FULL = 1.30
+DECEL_INTENT_TRIGGER = 0.5
+DECEL_INTENT_CURVE_OVERRIDE = 0.9

-  def __init__(self, initial_value=0, measurement_noise=0.1, process_noise=0.01,
-               alpha=1.0, smoothing_factor=0.85):
-    self.x = initial_value
-    self.P = 1.0
-    self.R = measurement_noise
-    self.Q = process_noise
-    self.alpha = alpha
-    self.smoothing_factor = smoothing_factor
-    self.initialized = False
-    self.history = []
-    self.max_history = 10
-    self.confidence = 0.0
+CURVE_Y_MAX = 5.0

-  def add_data(self, measurement):
-    if len(self.history) >= self.max_history:
-      self.history.pop(0)
-    self.history.append(measurement)
+LEAD_FUTURE_PROB_VANISH = 0.35
+LEAD_VETO_CONFIRM_FRAMES = 4

-    if not self.initialized:
-      self.x = measurement
-      self.initialized = True
-      self.confidence = 0.1
-      return
+MODEL_DROP_TRUST_FULL = 5.0
+MODEL_DROP_TRUST_NONE = 30.0
+MODEL_TRUST_MIN = 0.5

-    self.P = self.alpha * self.P + self.Q
+CREEP_SPEED_ENTER = 2.0
+CREEP_SPEED_EXIT = 3.0

-    K = self.P / (self.P + self.R)
-    effective_K = K * (1.0 - self.smoothing_factor) + self.smoothing_factor * 0.1
+ENTER_FRAMES = 3
+EXIT_FRAMES = 16
+MIN_BLENDED_FRAMES = 20

-    innovation = measurement - self.x
-    self.x = self.x + effective_K * innovation
-    self.P = (1 - effective_K) * self.P
-
-    if abs(innovation) < 0.1:
-      self.confidence = min(1.0, self.confidence + 0.05)
-    else:
-      self.confidence = max(0.1, self.confidence - 0.02)
-
-  def get_value(self):
-    return self.x if self.initialized else None
-
-  def get_confidence(self):
-    return self.confidence
-
-  def reset_data(self):
-    self.initialized = False
-    self.history = []
-    self.confidence = 0.0
+PARAM_READ_FRAMES = 5


-class ModeTransitionManager:
-  """Manages smooth transitions between driving modes with hysteresis."""
+@dataclass
+class DecSignals:
+  decel_intent: float = 0.0
+  curve_detected: bool = False
+  model_trust: float = 1.0
+  creeping: bool = False

+
+def should_blend(s: DecSignals) -> bool:
+  degraded = s.model_trust < MODEL_TRUST_MIN
+  curve_gate = s.decel_intent >= DECEL_INTENT_CURVE_OVERRIDE or not s.curve_detected
+  slowdown_detected = not degraded and s.decel_intent >= DECEL_INTENT_TRIGGER and curve_gate
+  return slowdown_detected or s.creeping
+
+
+class ModeHysteresis:
  def __init__(self):
-    self.current_mode: ModeType = 'acc'
-    self.mode_confidence = {'acc': 1.0, 'blended': 0.0}
-    self.transition_timeout = 0
-    self.min_mode_duration = 10
-    self.mode_duration = 0
-    self.emergency_override = False
+    self.mode: ModeType = 'acc'
+    self.above = 0
+    self.below = 0
+    self.blended_frames = 0

-  def request_mode(self, mode: ModeType, confidence: float = 1.0, emergency: bool = False):
-    # Emergency override for critical situations (stops, collisions)
-    if emergency:
-      self.emergency_override = True
-      self.current_mode = mode
-      self.transition_timeout = SET_MODE_TIMEOUT
-      self.mode_duration = 0
-      return
+  def update(self, want_blended: bool, override: bool, veto: bool) -> ModeType:
+    self.above = self.above + 1 if want_blended else 0
+    self.below = 0 if want_blended else self.below + 1

-    self.mode_confidence[mode] = min(1.0, self.mode_confidence[mode] + 0.1 * confidence)
-    for m in self.mode_confidence:
-      if m != mode:
-        self.mode_confidence[m] = max(0.0, self.mode_confidence[m] - 0.05)
+    if override:
+      self.mode, self.blended_frames = 'blended', 0
+    elif veto:
+      self.mode = 'acc'
+    elif self.mode == 'acc':
+      if self.above >= ENTER_FRAMES:
+        self.mode, self.blended_frames = 'blended', 0
+    else:
+      self.blended_frames += 1
+      if self.blended_frames >= MIN_BLENDED_FRAMES and self.below >= EXIT_FRAMES:
+        self.mode = 'acc'
+    return self.mode

-    # Require minimum duration in current mode (unless emergency)
-    if self.mode_duration < self.min_mode_duration and not self.emergency_override:
-      return
-
-    # Hysteresis: higher threshold for mode changes
-    confidence_threshold = 0.6 if mode != self.current_mode else 0.3  # Lower threshold for faster response
-
-    if self.mode_confidence[mode] > confidence_threshold:
-      if mode != self.current_mode and self.transition_timeout == 0:
-        self.transition_timeout = SET_MODE_TIMEOUT
-        self.current_mode = mode
-        self.mode_duration = 0
-
-  def update(self):
-    if self.transition_timeout > 0:
-      self.transition_timeout -= 1
-    self.mode_duration += 1
-
-    # Reset emergency override after some time
-    if self.emergency_override and self.mode_duration > 20:
-      self.emergency_override = False
-
-    # Gradual confidence decay
-    for mode in self.mode_confidence:
-      self.mode_confidence[mode] *= 0.98
-
-  def get_mode(self) -> ModeType:
-    return self.current_mode
+  def reset(self) -> None:
+    self.mode = 'acc'
+    self.above = 0
+    self.below = 0
+    self.blended_frames = 0


 class DynamicExperimentalController:
  def __init__(self, CP: structs.CarParams, mpc, params=None):
-    self._CP = CP
    self._mpc = mpc
    self._params = params or Params()
    self._enabled: bool = self._params.get_bool("DynamicExperimentalControl")
    self._active: bool = False
    self._frame: int = 0
-    self._urgency = 0.0

-    self._mode_manager = ModeTransitionManager()
+    self._hysteresis = ModeHysteresis()
+    self._creeping = False
+    self._lead_veto_frames = 0

-    # Smooth filters for stable decision making with faster response for critical scenarios
-    self._lead_filter = SmoothKalmanFilter(
-      measurement_noise=0.15,
-      process_noise=0.05,
-      alpha=1.02,
-      smoothing_factor=0.8
-    )
+    self.signals = DecSignals()
+    self.want_blended = False
+    self.lead_veto = False

-    self._slow_down_filter = SmoothKalmanFilter(
-      measurement_noise=0.1,
-      process_noise=0.1,
-      alpha=1.05,
-      smoothing_factor=0.7
-    )
-
-    self._slowness_filter = SmoothKalmanFilter(
-      measurement_noise=0.1,
-      process_noise=0.06,
-      alpha=1.015,
-      smoothing_factor=0.92
-    )
-
-    self._mpc_fcw_filter = SmoothKalmanFilter(
-      measurement_noise=0.2,
-      process_noise=0.1,
-      alpha=1.1,
-      smoothing_factor=0.5
-    )
-    self._has_lead_filtered = False
-    self._has_slow_down = False
-    self._has_slowness = False
-    self._has_mpc_fcw = False
-    self._v_ego_kph = 0.0
-    self._v_cruise_kph = 0.0
-    self._has_standstill = False
-    self._mpc_fcw_crash_cnt = 0
-    self._standstill_count = 0
-    # debug
-    self._endpoint_x = float('inf')
-    self._expected_distance = 0.0
-    self._trajectory_valid = False
+  def _update_creeping(self, v_ego: float) -> bool:
+    self._creeping = v_ego < CREEP_SPEED_EXIT if self._creeping else v_ego <= CREEP_SPEED_ENTER
+    return self._creeping

  def _read_params(self) -> None:
-    if self._frame % int(1. / DT_MDL) == 0:
+    if self._frame % PARAM_READ_FRAMES == 0:
      self._enabled = self._params.get_bool("DynamicExperimentalControl")

  def mode(self) -> str:
-    return self._mode_manager.get_mode()
+    return self._hysteresis.mode

  def enabled(self) -> bool:
    return self._enabled
@@ -197,192 +125,77 @@ class DynamicExperimentalController:
  def active(self) -> bool:
    return self._active

-  def set_mpc_fcw_crash_cnt(self) -> None:
-    """Set MPC FCW crash count"""
-    self._mpc_fcw_crash_cnt = self._mpc.crash_cnt
+  @staticmethod
+  def _decel_intent(md) -> float:
+    v = np.asarray(md.velocity.x)
+    if len(v) != len(_T_IDXS):
+      return 0.0
+    a_req = float(np.min((v[_DECEL_IDX] - v[0]) * _DECEL_INV_T))
+    return float(np.interp(-a_req, [DECEL_INTENT_A_HINT, DECEL_INTENT_A_FULL], [0.0, 1.0]))

-  def _update_calculations(self, sm: messaging.SubMaster) -> None:
-    car_state = sm['carState']
-    lead_one = sm['radarState'].leadOne
-    md = sm['modelV2']
+  @staticmethod
+  def _curve_detected(md) -> bool:
+    y = md.position.y
+    if len(y) < 1:
+      return False
+    return abs(y[-1]) >= CURVE_Y_MAX

-    self._v_ego_kph = car_state.vEgo * 3.6
-    self._v_cruise_kph = car_state.vCruise
-    self._has_standstill = car_state.standstill
+  @staticmethod
+  def _model_trust(md) -> float:
+    if len(md.velocity.x) != len(_T_IDXS):
+      return 0.0
+    return float(np.interp(md.frameDropPerc, [MODEL_DROP_TRUST_FULL, MODEL_DROP_TRUST_NONE], [1.0, 0.0]))

-    # standstill detection
-    if self._has_standstill:
-      self._standstill_count = min(20, self._standstill_count + 1)
-    else:
-      self._standstill_count = max(0, self._standstill_count - 1)
+  @staticmethod
+  def _lead_veto(radar_state, md) -> bool:
+    lead_one, lead_two = radar_state.leadOne, radar_state.leadTwo
+    lead_now = lead_one.present or lead_two.present
+    probs = md.leadsV3
+    future = min(probs[1].prob, probs[2].prob) if len(probs) >= 3 else 1.0
+    return bool(lead_now and future > LEAD_FUTURE_PROB_VANISH)

-    # Lead detection
-    self._lead_filter.add_data(float(lead_one.present))
-    lead_value = self._lead_filter.get_value() or 0.0
-    self._has_lead_filtered = lead_value > WMACConstants.LEAD_PROB
+  def _update_lead_veto(self, raw_veto: bool, lead_present: bool, urgent_override: bool) -> bool:
+    if raw_veto:
+      self._lead_veto_frames = min(self._lead_veto_frames + 1, LEAD_VETO_CONFIRM_FRAMES)
+      return self.lead_veto or self._lead_veto_frames >= LEAD_VETO_CONFIRM_FRAMES

-    # MPC FCW detection
-    fcw_filtered_value = self._mpc_fcw_filter.get_value() or 0.0
-    self._mpc_fcw_filter.add_data(float(self._mpc_fcw_crash_cnt > 0))
-    self._has_mpc_fcw = fcw_filtered_value > 0.5
+    if not lead_present or urgent_override or not self.lead_veto:
+      self._lead_veto_frames = 0
+      return False

-    # Slow down detection
-    self._calculate_slow_down(md)
-
-    # Slowness detection
-    if not (self._standstill_count > 5) and not self._has_slow_down:
-      current_slowness = float(self._v_ego_kph <= (self._v_cruise_kph * WMACConstants.SLOWNESS_CRUISE_OFFSET))
-      self._slowness_filter.add_data(current_slowness)
-      slowness_value = self._slowness_filter.get_value() or 0.0
-
-      # Hysteresis for slowness
-      threshold = WMACConstants.SLOWNESS_PROB * (0.8 if self._has_slowness else 1.1)
-      self._has_slowness = slowness_value > threshold
-
-  def _calculate_slow_down(self, md):
-    """Calculate urgency based on trajectory endpoint vs expected distance."""
-
-    # Reset to safe defaults
-    urgency = 0.0
-    self._endpoint_x = float('inf')
-    self._trajectory_valid = False
-
-    #Require exact trajectory size
-    position_valid = len(md.position.x) == TRAJECTORY_SIZE
-    orientation_valid = len(md.orientation.x) == TRAJECTORY_SIZE
-
-    if not (position_valid and orientation_valid):
-      # Invalid trajectory - this itself might indicate a stop scenario
-      # Apply moderate urgency for incomplete trajectories at speed
-      if self._v_ego_kph > 20.0:
-        urgency = 0.3
-
-      self._slow_down_filter.add_data(urgency)
-      urgency_filtered = self._slow_down_filter.get_value() or 0.0
-      self._has_slow_down = urgency_filtered > WMACConstants.SLOW_DOWN_PROB
-      self._urgency = urgency_filtered
-      return
-
-    # We have a valid full trajectory
-    self._trajectory_valid = True
-
-    # Use the exact endpoint (33rd point, index 32)
-    endpoint_x = md.position.x[TRAJECTORY_SIZE - 1]
-    self._endpoint_x = endpoint_x
-
-    # Get expected distance based on current speed using tuned constants
-    expected_distance = interp(self._v_ego_kph,
-                               WMACConstants.SLOW_DOWN_BP,
-                               WMACConstants.SLOW_DOWN_DIST)
-    self._expected_distance = expected_distance
-
-    # Calculate urgency based on trajectory shortage
-    if endpoint_x < expected_distance:
-      shortage = expected_distance - endpoint_x
-      shortage_ratio = shortage / expected_distance
-
-      # Base urgency on shortage ratio
-      urgency = min(1.0, shortage_ratio * 2.0)
-
-      # Increase urgency for very short trajectories (imminent stops)
-      critical_distance = expected_distance * 0.3
-      if endpoint_x < critical_distance:
-        urgency = min(1.0, urgency * 2.0)
-
-      # Speed-based urgency adjustment
-      if self._v_ego_kph > 25.0:
-        speed_factor = 1.0 + (self._v_ego_kph - 25.0) / 80.0
-        urgency = min(1.0, urgency * speed_factor)
-
-    # Apply filtering but with less smoothing for stops
-    self._slow_down_filter.add_data(urgency)
-    urgency_filtered = self._slow_down_filter.get_value() or 0.0
-
-    # Update state with lower threshold for better stop detection
-    self._has_slow_down = urgency_filtered > (WMACConstants.SLOW_DOWN_PROB * 0.8)
-    self._urgency = urgency_filtered
-
-  def _radarless_mode(self) -> None:
-    """Radarless mode decision logic with emergency handling."""
-
-    # EMERGENCY: MPC FCW - immediate blended mode
-    if self._has_mpc_fcw:
-      self._mode_manager.request_mode('blended', confidence=1.0, emergency=True)
-      return
-
-    # Standstill: use blended
-    if self._standstill_count > 3:
-      self._mode_manager.request_mode('blended', confidence=0.9)
-      return
-
-    # Slow down scenarios: emergency for high urgency, normal for lower urgency
-    if self._has_slow_down:
-      if self._urgency > 0.7:
-        # Emergency: immediate blended mode for high urgency stops
-        self._mode_manager.request_mode('blended', confidence=1.0, emergency=True)
-      else:
-        # Normal: blended with urgency-based confidence
-        confidence = min(1.0, self._urgency * 1.5)
-        self._mode_manager.request_mode('blended', confidence=confidence)
-      return
-
-    # Driving slow: use ACC (but not if actively slowing down)
-    if self._has_slowness and not self._has_slow_down:
-      self._mode_manager.request_mode('acc', confidence=0.8)
-      return
-
-    # Default: ACC
-    self._mode_manager.request_mode('acc', confidence=0.7)
-
-  def _radar_mode(self) -> None:
-    """Radar mode with emergency handling."""
-
-    # EMERGENCY: MPC FCW - immediate blended mode
-    if self._has_mpc_fcw:
-      self._mode_manager.request_mode('blended', confidence=1.0, emergency=True)
-      return
-
-    # If lead detected and not in standstill: always use ACC
-    if self._has_lead_filtered and not (self._standstill_count > 3):
-      self._mode_manager.request_mode('acc', confidence=1.0)
-      return
-
-    # Slow down scenarios: emergency for high urgency, normal for lower urgency
-    if self._has_slow_down:
-      if self._urgency > 0.7:
-        # Emergency: immediate blended mode for high urgency stops
-        self._mode_manager.request_mode('blended', confidence=1.0, emergency=True)
-      else:
-        # Normal: blended with urgency-based confidence
-        confidence = min(1.0, self._urgency * 1.3)
-        self._mode_manager.request_mode('blended', confidence=confidence)
-      return
-
-    # Standstill: use blended
-    if self._standstill_count > 3:
-      self._mode_manager.request_mode('blended', confidence=0.9)
-      return
-
-    # Driving slow: use ACC (but not if actively slowing down)
-    if self._has_slowness and not self._has_slow_down:
-      self._mode_manager.request_mode('acc', confidence=0.8)
-      return
-
-    # Default: ACC
-    self._mode_manager.request_mode('acc', confidence=0.7)
+    self._lead_veto_frames = max(self._lead_veto_frames - 1, 0)
+    return self._lead_veto_frames > 0

  def update(self, sm: messaging.SubMaster) -> None:
    self._read_params()

-    self.set_mpc_fcw_crash_cnt()
+    car_state = sm['carState']
+    md = sm['modelV2']
+    radar_state = sm['radarState']

-    self._update_calculations(sm)
+    is_creeping = self._update_creeping(car_state.vEgo)
+    lead_present = radar_state.leadOne.present or radar_state.leadTwo.present
+    self.signals = DecSignals(
+      decel_intent=self._decel_intent(md),
+      curve_detected=self._curve_detected(md),
+      model_trust=self._model_trust(md),
+      creeping=is_creeping and not lead_present,
+    )
+    self.want_blended = should_blend(self.signals)

-    if self._CP.radarUnavailable:
-      self._radarless_mode()
+    crash_override = self._mpc.crash_cnt >= 1
+    hard_brake_override = bool(md.meta.hardBrakePredicted)
+    strong_stop = self.signals.model_trust >= MODEL_TRUST_MIN and self.signals.decel_intent >= DECEL_INTENT_CURVE_OVERRIDE
+    raw_lead_veto = self._lead_veto(radar_state, md)
+    urgent_release = (crash_override or hard_brake_override or strong_stop) and not raw_lead_veto
+    self.lead_veto = self._update_lead_veto(raw_lead_veto, lead_present, urgent_release)
+
+    override = (crash_override or hard_brake_override) and not self.lead_veto
+
+    if self._enabled:
+      self._hysteresis.update(self.want_blended, override, self.lead_veto)
    else:
-      self._radar_mode()
+      self._hysteresis.reset()

-    self._mode_manager.update()
    self._active = sm['selfdriveState'].experimentalMode and self._enabled
    self._frame += 1
@@ -1,91 +1,465 @@
+import numpy as np
+
+from openpilot.cereal import messaging
+from opendbc.car import structs
 from openpilot.common.test import OpenpilotTestCase
-from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
+from openpilot.selfdrive.modeld.constants import ModelConstants
+from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import (
+  DecSignals,
+  DynamicExperimentalController,
+  ModeHysteresis,
+  should_blend,
+  ENTER_FRAMES,
+  LEAD_VETO_CONFIRM_FRAMES,
+  MIN_BLENDED_FRAMES,
+)

-class MockLeadOne:
-  def __init__(self, present=0.0):
-    self.present = present
+T_IDXS = np.array(ModelConstants.T_IDXS)

-class MockRadarState:
-  def __init__(self, present=0.0):
-    self.leadOne = MockLeadOne(present=present)
-
-class MockCarState:
-  def __init__(self, vEgo=0.0, vCruise=0.0, standstill=False):
-    self.vEgo = vEgo
-    self.vCruise = vCruise
-    self.standstill = standstill
-
-class MockModelData:
-  def __init__(self, valid=True):
-    size = 33 if valid else 10  # incomplete if invalid
-    self.position = type("Pos", (), {"x": [0.0] * size})()
-    self.orientation = type("Ori", (), {"x": [0.0] * size})()
-
-class MockSelfDriveState:
-  def __init__(self, experimentalMode=False):
-    self.experimentalMode = experimentalMode

 class MockParams:
+  def __init__(self, enabled=True):
+    self._enabled = enabled
+
  def get_bool(self, name):
-    return True
+    return self._enabled

-def default_sm():
-  sm = {
-    'carState': MockCarState(vEgo=10.0, vCruise=20.0),
-    'radarState': MockRadarState(present=1.0),
-    'modelV2': MockModelData(valid=True),
-    'selfdriveState': MockSelfDriveState(experimentalMode=True),
+
+class MockMpc:
+  def __init__(self, crash_cnt=0):
+    self.crash_cnt = crash_cnt
+
+
+def flat_velocity(v):
+  return [float(v)] * len(T_IDXS)
+
+
+def decel_velocity(v0, a):
+  return [float(max(0.0, v0 + a * t)) for t in T_IDXS]
+
+
+def make_car_state(v_ego=10.0, v_cruise=20.0):
+  msg = messaging.new_message('carState')
+  msg.carState.vEgo = v_ego
+  msg.carState.vCruise = v_cruise
+  return msg.carState.as_reader()
+
+
+def make_selfdrive_state(experimental_mode=True):
+  msg = messaging.new_message('selfdriveState')
+  msg.selfdriveState.experimentalMode = experimental_mode
+  return msg.selfdriveState.as_reader()
+
+
+def make_radar_state(lead_present=False, lead_radar=False, lead_two_present=False):
+  msg = messaging.new_message('radarState')
+  msg.radarState.leadOne.present = lead_present
+  msg.radarState.leadOne.radar = lead_radar
+  msg.radarState.leadTwo.present = lead_two_present
+  return msg.radarState.as_reader()
+
+
+def make_model_v2(velocity=None, position_y=None, hard_brake=False, lead_probs=None, frame_drop_perc=0.0):
+  msg = messaging.new_message('modelV2')
+  msg.modelV2.velocity.x = velocity if velocity is not None else flat_velocity(0.0)
+  msg.modelV2.position.y = position_y if position_y is not None else [0.0] * len(T_IDXS)
+  msg.modelV2.frameDropPerc = frame_drop_perc
+  msg.modelV2.meta.hardBrakePredicted = hard_brake
+  if lead_probs is not None:
+    msg.modelV2.init('leadsV3', 3)
+    for i, (prob, prob_time) in enumerate(zip(lead_probs, (0.0, 2.0, 4.0), strict=True)):
+      msg.modelV2.leadsV3[i].prob = prob
+      msg.modelV2.leadsV3[i].probTime = prob_time
+  return msg.modelV2.as_reader()
+
+
+def make_sm(v_ego=10.0, v_cruise=20.0, velocity=None, position_y=None, hard_brake=False,
+            lead_present=False, lead_radar=False, lead_two_present=False, lead_probs=None,
+            frame_drop_perc=0.0, experimental_mode=True):
+  return {
+    'carState': make_car_state(v_ego, v_cruise),
+    'radarState': make_radar_state(lead_present, lead_radar, lead_two_present),
+    'modelV2': make_model_v2(velocity, position_y, hard_brake, lead_probs, frame_drop_perc),
+    'selfdriveState': make_selfdrive_state(experimental_mode),
  }
-  return sm

-def mock_cp():
-  class CP:
-    radarUnavailable = False
-  return CP()

-def mock_mpc():
-  class MPC:
-    crash_cnt = 0
-  return MPC()
+def make_controller(cp=None, mpc=None, enabled=True):
+  return DynamicExperimentalController(cp or structs.CarParams(), mpc or MockMpc(), params=MockParams(enabled))

-# Fake Kalman Filter that always returns a given value
-class FakeKalman:
-  def __init__(self, value=1.0):
-    self.value = value
-  def add_data(self, v): pass
-  def get_value(self): return self.value
-  def get_confidence(self): return 1.0
-  def reset_data(self): pass

 class TestDynamicExperimentalController(OpenpilotTestCase):
-  def test_initial_mode_is_acc(self, mock_cp, mock_mpc):
-    controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  def test_initial_mode_is_acc(self):
+    controller = make_controller()
    assert controller.mode() == "acc"

-  def test_standstill_triggers_blended(self, mock_cp, mock_mpc, default_sm):
-    controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-    default_sm['carState'].standstill = True
+  def test_flat_plan_never_blends_at_any_speed(self):
+    for v_ego in (2.5, 5.6, 8.3, 13.9, 22.2, 30.6):
+      controller = make_controller()
+      sm = make_sm(v_ego=v_ego, velocity=flat_velocity(v_ego))
+      for _ in range(100):
+        controller.update(sm)
+      assert controller.mode() == "acc", f"false blend on a flat plan at v_ego={v_ego}"
+
+  def test_highway_slowdown_without_lead_blends(self):
+    v0 = 110 / 3.6
+    a = (70 / 3.6 - v0) / 6.0
+    controller = make_controller()
+    sm = make_sm(v_ego=v0, velocity=decel_velocity(v0, a))
    for _ in range(10):
-      controller.update(default_sm)
+      controller.update(sm)
    assert controller.mode() == "blended"

-  def test_emergency_blended_on_fcw(self, mock_cp, mock_mpc, default_sm):
-    controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-    mock_mpc.crash_cnt = 1  # simulate FCW
-    for _ in range(2):
-      controller.update(default_sm)
+  def test_curve_exclusion_prevents_false_blend(self):
+    controller = make_controller()
+    sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -1.0), position_y=[6.0] * len(T_IDXS))
+    for _ in range(30):
+      controller.update(sm)
+    assert controller.mode() == "acc"
+
+  def test_curve_does_not_override_saturated_decel_intent(self):
+    controller = make_controller()
+    sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0), position_y=[6.0] * len(T_IDXS))
+    for _ in range(10):
+      controller.update(sm)
    assert controller.mode() == "blended"

-  def test_radarless_slowdown_triggers_blended(self, mock_cp, mock_mpc, default_sm):
-    mock_cp.radarUnavailable = True
-    controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-
-    # Force conditions to simulate slowdown
-    controller._slow_down_filter = FakeKalman(value=1.0)  # ty: ignore[invalid-assignment]
-    controller._v_ego_kph = 35.0
-    default_sm['modelV2'] = MockModelData(valid=False)  # Incomplete trajectory
-
-    for _ in range(3):
-      controller.update(default_sm)
+  def test_persistent_lead_forces_acc_even_with_strong_model_signal(self):
+    for lead_radar in (True, False):
+      controller = make_controller()
+      sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0),
+                   lead_present=True, lead_radar=lead_radar, lead_probs=[1.0, 1.0, 1.0])
+      for _ in range(LEAD_VETO_CONFIRM_FRAMES - 1):
+        controller.update(sm)
+        assert not controller.lead_veto
+      for _ in range(60):
+        controller.update(sm)
+        assert controller.lead_veto
+        assert controller.mode() == "acc"

+  def test_single_frame_lead_veto_pulse_does_not_leave_blended(self):
+    controller = make_controller()
+    no_lead_sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0))
+    lead_sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0),
+                      lead_present=True, lead_probs=[1.0, 1.0, 1.0])
+    for _ in range(ENTER_FRAMES):
+      controller.update(no_lead_sm)
    assert controller.mode() == "blended"
+
+    controller.update(lead_sm)
+    assert controller.mode() == "blended"
+    assert not controller.lead_veto
+
+    controller.update(no_lead_sm)
+    assert controller.mode() == "blended"
+    assert not controller.lead_veto
+
+  def test_three_frame_lead_veto_pulse_does_not_leave_blended(self):
+    controller = make_controller()
+    no_lead_sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0))
+    lead_sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0),
+                      lead_present=True, lead_probs=[1.0, 1.0, 1.0])
+    for _ in range(ENTER_FRAMES):
+      controller.update(no_lead_sm)
+    assert controller.mode() == "blended"
+
+    for _ in range(LEAD_VETO_CONFIRM_FRAMES - 1):
+      controller.update(lead_sm)
+      assert controller.mode() == "blended"
+      assert not controller.lead_veto
+
+    controller.update(no_lead_sm)
+    assert controller.mode() == "blended"
+    assert not controller.lead_veto
+
+  def test_persistent_lead_veto_forces_acc_after_confirmation(self):
+    controller = make_controller()
+    no_lead_sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0))
+    lead_sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0),
+                      lead_present=True, lead_probs=[1.0, 1.0, 1.0])
+    for _ in range(ENTER_FRAMES):
+      controller.update(no_lead_sm)
+    assert controller.mode() == "blended"
+
+    for _ in range(LEAD_VETO_CONFIRM_FRAMES - 1):
+      controller.update(lead_sm)
+      assert controller.mode() == "blended"
+      assert not controller.lead_veto
+
+    controller.update(lead_sm)
+    assert controller.mode() == "acc"
+    assert controller.lead_veto
+
+    controller.update(no_lead_sm)
+    assert controller.mode() == "blended"
+    assert not controller.lead_veto
+
+  def test_veto_releases_without_rebuild_lag(self):
+    controller = make_controller()
+    lead_sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0),
+                       lead_present=True, lead_probs=[1.0, 1.0, 1.0])
+    for _ in range(30):
+      controller.update(lead_sm)
+    assert controller.mode() == "acc"
+    assert controller.lead_veto
+
+    no_lead_sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0), lead_present=False)
+    for _ in range(ENTER_FRAMES + 2):
+      controller.update(no_lead_sm)
+      if controller.mode() == "blended":
+        break
+    assert controller.mode() == "blended"
+
+  def test_lead_gone_with_no_underlying_slowdown_stays_acc(self):
+    controller = make_controller()
+    lead_sm = make_sm(v_ego=20.0, velocity=flat_velocity(20.0), lead_present=True, lead_probs=[1.0, 1.0, 1.0])
+    for _ in range(30):
+      controller.update(lead_sm)
+    assert controller.mode() == "acc"
+
+    no_lead_sm = make_sm(v_ego=20.0, velocity=flat_velocity(20.0), lead_present=False)
+    for _ in range(20):
+      controller.update(no_lead_sm)
+    assert controller.mode() == "acc"
+
+  def test_creep_does_not_release_lead_veto(self):
+    controller = make_controller()
+    sm = make_sm(v_ego=1.0, velocity=flat_velocity(1.0), lead_present=True, lead_probs=[1.0, 1.0, 1.0])
+    for _ in range(10):
+      controller.update(sm)
+    assert controller.mode() == "acc"
+    assert controller.lead_veto
+
+  def test_lead_prevents_creep_only_blending_when_model_probability_drops(self):
+    controller = make_controller()
+    confirmed_sm = make_sm(v_ego=1.0, velocity=flat_velocity(1.0), lead_present=True, lead_probs=[1.0, 1.0, 1.0])
+    for _ in range(LEAD_VETO_CONFIRM_FRAMES):
+      controller.update(confirmed_sm)
+    assert controller.lead_veto
+
+    low_probability_sm = make_sm(v_ego=1.0, velocity=flat_velocity(1.0), lead_present=True, lead_probs=[1.0, 0.1, 0.1])
+    for _ in range(LEAD_VETO_CONFIRM_FRAMES - 1):
+      controller.update(low_probability_sm)
+      assert controller.lead_veto
+      assert controller.mode() == "acc"
+
+    for _ in range(20):
+      controller.update(low_probability_sm)
+      assert not controller.lead_veto
+      assert not controller.signals.creeping
+      assert controller.mode() == "acc"
+
+    no_lead_sm = make_sm(v_ego=1.0, velocity=flat_velocity(1.0), lead_present=False, lead_probs=[1.0, 0.1, 0.1])
+    for _ in range(ENTER_FRAMES):
+      controller.update(no_lead_sm)
+    assert controller.mode() == "blended"
+
+  def test_confirmed_lead_veto_ignores_short_future_probability_dropout(self):
+    controller = make_controller()
+    confirmed_sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -1.0),
+                           lead_present=True, lead_probs=[1.0, 1.0, 1.0])
+    for _ in range(LEAD_VETO_CONFIRM_FRAMES):
+      controller.update(confirmed_sm)
+    assert controller.lead_veto
+
+    dropout_sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -1.0),
+                         lead_present=True, lead_probs=[1.0, 0.1, 0.1])
+    for _ in range(LEAD_VETO_CONFIRM_FRAMES - 1):
+      controller.update(dropout_sm)
+      assert controller.lead_veto
+      assert controller.mode() == "acc"
+
+    controller.update(confirmed_sm)
+    assert controller.lead_veto
+    assert controller.mode() == "acc"
+
+  def test_urgent_override_bypasses_confirmed_veto_release(self):
+    controller = make_controller()
+    confirmed_sm = make_sm(v_ego=20.0, velocity=flat_velocity(20.0),
+                           lead_present=True, lead_probs=[1.0, 1.0, 1.0])
+    for _ in range(LEAD_VETO_CONFIRM_FRAMES):
+      controller.update(confirmed_sm)
+    assert controller.lead_veto
+
+    hard_brake_sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0), hard_brake=True,
+                            lead_present=True, lead_probs=[1.0, 0.1, 0.1])
+    controller.update(hard_brake_sm)
+    assert not controller.lead_veto
+    assert controller.mode() == "blended"
+
+  def test_trusted_strong_stop_bypasses_confirmed_veto_release(self):
+    controller = make_controller()
+    confirmed_sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0),
+                           lead_present=True, lead_probs=[1.0, 1.0, 1.0])
+    for _ in range(LEAD_VETO_CONFIRM_FRAMES):
+      controller.update(confirmed_sm)
+    assert controller.lead_veto
+    assert controller.mode() == "acc"
+
+    departing_lead_sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0),
+                                lead_present=True, lead_probs=[1.0, 0.1, 0.1])
+    controller.update(departing_lead_sm)
+    assert not controller.lead_veto
+    assert controller.mode() == "blended"
+
+  def test_degraded_strong_stop_does_not_bypass_confirmed_veto_release(self):
+    controller = make_controller()
+    confirmed_sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0),
+                           lead_present=True, lead_probs=[1.0, 1.0, 1.0])
+    for _ in range(LEAD_VETO_CONFIRM_FRAMES):
+      controller.update(confirmed_sm)
+    assert controller.lead_veto
+
+    degraded_sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0), lead_present=True,
+                          lead_probs=[1.0, 0.1, 0.1], frame_drop_perc=60.0)
+    for _ in range(LEAD_VETO_CONFIRM_FRAMES - 1):
+      controller.update(degraded_sm)
+      assert controller.lead_veto
+      assert controller.mode() == "acc"
+
+  def test_model_slowdown_still_blends_while_creeping_with_a_lead(self):
+    controller = make_controller()
+    sm = make_sm(v_ego=1.0, velocity=decel_velocity(1.0, -1.0), lead_present=True, lead_probs=[1.0, 0.1, 0.1])
+    for _ in range(ENTER_FRAMES):
+      controller.update(sm)
+    assert not controller.lead_veto
+    assert controller.mode() == "blended"
+
+  def test_hard_brake_still_blends_while_creeping_with_a_lead(self):
+    controller = make_controller()
+    sm = make_sm(v_ego=1.0, velocity=flat_velocity(1.0), hard_brake=True,
+                 lead_present=True, lead_probs=[1.0, 0.1, 0.1])
+    controller.update(sm)
+    assert not controller.lead_veto
+    assert controller.mode() == "blended"
+
+  def test_creep_hysteresis_band_without_lead(self):
+    controller = make_controller()
+    controller.update(make_sm(v_ego=1.5, velocity=flat_velocity(1.5)))
+    assert controller.signals.creeping
+
+    controller.update(make_sm(v_ego=2.5, velocity=flat_velocity(2.5)))
+    assert controller.signals.creeping, "a small excursion above CREEP_SPEED_ENTER should not exit creeping"
+
+    controller.update(make_sm(v_ego=5.0, velocity=flat_velocity(5.0)))
+    assert not controller.signals.creeping, "should exit creeping once genuinely above CREEP_SPEED_EXIT"
+
+  def test_crash_cnt_override_inert_while_lead_present(self):
+    mpc = MockMpc(crash_cnt=0)
+    controller = make_controller(mpc=mpc)
+    sm = make_sm(v_ego=20.0, velocity=flat_velocity(20.0), lead_present=True, lead_probs=[1.0, 1.0, 1.0])
+    for _ in range(30):
+      controller.update(sm)
+    assert controller.mode() == "acc"
+
+    mpc.crash_cnt = 1
+    controller.update(sm)
+    assert controller.mode() == "acc"
+
+  def test_crash_cnt_blends_within_one_frame_without_lead(self):
+    mpc = MockMpc(crash_cnt=1)
+    controller = make_controller(mpc=mpc)
+    sm = make_sm(v_ego=20.0, velocity=flat_velocity(20.0), lead_present=False)
+    controller.update(sm)
+    assert controller.mode() == "blended"
+
+  def test_hard_brake_predicted_blends_within_one_frame_without_lead(self):
+    controller = make_controller()
+    sm = make_sm(v_ego=20.0, velocity=flat_velocity(20.0), hard_brake=True, lead_present=False)
+    controller.update(sm)
+    assert controller.mode() == "blended"
+
+  def test_confirmed_lead_veto_suppresses_hard_brake_override(self):
+    controller = make_controller()
+    sm = make_sm(v_ego=20.0, velocity=flat_velocity(20.0), hard_brake=True,
+                 lead_present=True, lead_probs=[1.0, 1.0, 1.0])
+    for _ in range(LEAD_VETO_CONFIRM_FRAMES - 1):
+      controller.update(sm)
+      assert controller.mode() == "blended"
+      assert not controller.lead_veto
+    controller.update(sm)
+    assert controller.mode() == "acc"
+    assert controller.lead_veto
+
+  def test_degraded_model_does_not_blend(self):
+    controller = make_controller()
+    sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -3.0), frame_drop_perc=60.0)
+    for _ in range(30):
+      controller.update(sm)
+    assert controller.mode() == "acc"
+
+  def test_short_plan_arrays_do_not_blend(self):
+    controller = make_controller()
+    sm = make_sm(v_ego=20.0, velocity=[20.0] * 5)
+    for _ in range(30):
+      controller.update(sm)
+    assert controller.mode() == "acc"
+
+  def test_disabled_param_holds_acc(self):
+    controller = make_controller(enabled=False)
+    sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -3.0))
+    for _ in range(30):
+      controller.update(sm)
+    assert controller.mode() == "acc"
+
+
+class TestModeHysteresis(OpenpilotTestCase):
+  def test_entry_requires_enter_frames(self):
+    h = ModeHysteresis()
+    for _ in range(ENTER_FRAMES - 1):
+      assert h.update(want_blended=True, override=False, veto=False) == "acc"
+    assert h.update(want_blended=True, override=False, veto=False) == "blended"
+
+  def test_override_beats_veto(self):
+    h = ModeHysteresis()
+    assert h.update(want_blended=False, override=True, veto=True) == "blended"
+
+  def test_veto_forces_acc_even_when_reason_active(self):
+    h = ModeHysteresis()
+    for _ in range(ENTER_FRAMES + 5):
+      assert h.update(want_blended=True, override=False, veto=True) == "acc"
+
+  def test_counter_accumulates_under_veto_then_releases_instantly(self):
+    h = ModeHysteresis()
+    for _ in range(ENTER_FRAMES + 5):
+      h.update(want_blended=True, override=False, veto=True)
+    assert h.mode == "acc"
+    assert h.update(want_blended=True, override=False, veto=False) == "blended"
+
+  def test_exit_requires_min_dwell_and_sustained_absence(self):
+    h = ModeHysteresis()
+    for _ in range(ENTER_FRAMES):
+      h.update(want_blended=True, override=False, veto=False)
+    assert h.mode == "blended"
+    for _ in range(MIN_BLENDED_FRAMES - 1):
+      assert h.update(want_blended=False, override=False, veto=False) == "blended"
+    assert h.update(want_blended=False, override=False, veto=False) == "acc"
+
+  def test_no_flapping_on_alternating_reason(self):
+    h = ModeHysteresis()
+    changes = 0
+    prev = h.mode
+    for i in range(200):
+      mode = h.update(want_blended=i % 2 == 0, override=False, veto=False)
+      changes += mode != prev
+      prev = mode
+    assert changes == 0
+
+
+class TestShouldBlend(OpenpilotTestCase):
+  def test_slowdown_detected_triggers(self):
+    assert should_blend(DecSignals(decel_intent=1.0))
+    assert not should_blend(DecSignals(decel_intent=0.0))
+
+  def test_curve_exclusion_suppresses_slowdown(self):
+    assert not should_blend(DecSignals(decel_intent=0.7, curve_detected=True))
+
+  def test_curve_exclusion_does_not_override_saturated_decel_intent(self):
+    assert should_blend(DecSignals(decel_intent=1.0, curve_detected=True))
+
+  def test_degraded_model_suppresses_model_based_reasons(self):
+    s = DecSignals(decel_intent=1.0, model_trust=0.0)
+    assert not should_blend(s)
+
+  def test_creep_bypasses_everything(self):
+    assert should_blend(DecSignals(model_trust=0.0, creeping=True))
@@ -1,25 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-
-import numpy as np
-
-STOPPING_DISTANCE = 1.5
-STOPPED_SPEED = 0.02
-STOPPING_TIME = 2.5
-MIN_STOPPING_HOLD_ACCEL = -0.2
-STOPPING_DECEL_RATE = 0.3
-STOPPED_DECEL_RATE = 0.05
-
-
-class LongControlSP:
-  def should_hold_stopping(self, CS, a_target: float) -> bool:
-    return (self.last_output_accel <= MIN_STOPPING_HOLD_ACCEL and a_target >= self.last_output_accel and CS.vEgo > STOPPED_SPEED and CS.aEgo < 0.0
-            and CS.vEgo <= -CS.aEgo * STOPPING_TIME and CS.vEgo ** 2 <= -2.0 * CS.aEgo * STOPPING_DISTANCE)
-
-  @staticmethod
-  def stopping_decel_rate(v_ego: float) -> float:
-    return float(np.interp(v_ego, [0.0, 0.5, 1.5], [STOPPED_DECEL_RATE, 0.15, STOPPING_DECEL_RATE]))
@@ -5,10 +5,13 @@ This file is part of sunnypilot and is licensed under the MIT License.
 See the LICENSE.md file in the root directory for more details.
 """

-from openpilot.cereal import messaging, custom
+import math
+
+from openpilot.cereal import messaging, custom, log
 from opendbc.car import structs
 from openpilot.common.constants import CV
 from openpilot.selfdrive.car.cruise import V_CRUISE_MAX
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController
 from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
 from openpilot.sunnypilot.selfdrive.controls.lib.e2e_alerts_helper import E2EAlertsHelper
 from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.smart_cruise_control import SmartCruiseControl
@@ -19,12 +22,17 @@ from openpilot.sunnypilot.models.helpers import get_active_bundle

 DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState
 LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource
+MpcPlanSource = log.LongitudinalPlan.LongitudinalPlanSource
+
+E2E_BRAKE_HOLD_ACCEL = -0.2  # m/s^2


 class LongitudinalPlannerSP:
  def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP, mpc):
+    self.accel_controller = AccelController()
+    self.accel_controller_active = False
+    self.lead_one = None
    self.events_sp = EventsSP()
-    self.resolver = SpeedLimitResolver()
    self.dec = DynamicExperimentalController(CP, mpc)
    self.scc = SmartCruiseControl()
    self.resolver = SpeedLimitResolver()
@@ -38,10 +46,49 @@ class LongitudinalPlannerSP:

  def is_e2e(self, sm: messaging.SubMaster) -> bool:
    experimental_mode = sm['selfdriveState'].experimentalMode
-    if not self.dec.active():
-      return experimental_mode
+    if not experimental_mode:
+      return False

-    return experimental_mode and self.dec.mode() == "blended"
+    if not self.dec.active() or self.dec.mode() == "blended":
+      return True
+
+    if self.mpc.source == MpcPlanSource.e2e and sm['modelV2'].action.desiredAcceleration < E2E_BRAKE_HOLD_ACCEL:
+      return True
+
+    return False
+
+  def get_max_accel_override(self, v_ego: float, engine_off: bool = False, lead=None) -> float | None:
+    if not self.accel_controller.is_enabled():
+      return None
+
+    # the eco lead pull-away boost needs the lead; the stock planner does not pass it, so update() keeps it
+    return self.accel_controller.get_max_accel(v_ego, engine_off, lead if lead is not None else self.lead_one)
+
+  def get_cruise_target_override(self, v_ego: float, v_target: float, force_decel: bool, accel_coast: float | None = None) -> float:
+    if not self.accel_controller.is_enabled() or force_decel or self.source != LongitudinalPlanSource.cruise:
+      return v_target
+
+    return self.accel_controller.get_cruise_target(v_ego, v_target, accel_coast)
+
+  def is_accel_controller_active(self, force_decel: bool) -> bool:
+    return bool(self.accel_controller.is_enabled() and not force_decel and
+                self.mpc.source == MpcPlanSource.cruise)
+
+  def _has_valid_selected_lead(self, sm: messaging.SubMaster, source: MpcPlanSource) -> bool:
+    radar_valid = sm.valid.get('radarState', False) and getattr(sm, 'alive', {}).get('radarState', False)
+    return radar_valid and ((source == MpcPlanSource.lead0 and sm['radarState'].leadOne.present) or
+                            (source == MpcPlanSource.lead1 and sm['radarState'].leadTwo.present))
+
+  def arbitrate_cruise_candidate(self, sm: messaging.SubMaster, gated: float, ungated: float,
+                                 mpc_accel: float, mpc_source: MpcPlanSource, *, allow_throttle: bool,
+                                 e2e: bool, force_decel: bool) -> float:
+    finite = all(math.isfinite(value) for value in (gated, ungated, mpc_accel))
+    coast_gate_changed_source = gated < mpc_accel <= ungated
+    if (finite and not allow_throttle and not e2e and not force_decel
+        and self._has_valid_selected_lead(sm, mpc_source) and coast_gate_changed_source):
+      return ungated
+
+    return gated

  def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]:
    CS = sm['carState']
@@ -74,10 +121,14 @@ class LongitudinalPlannerSP:
    return self.output_v_target, self.output_a_target

  def update(self, sm: messaging.SubMaster) -> None:
+    self.accel_controller.update()
+    self.lead_one = sm['radarState'].leadOne
    self.events_sp.clear()
-    self.dec.update(sm)
    self.e2e_alerts_helper.update(sm, self.events_sp)

+  def update_dec(self, sm: messaging.SubMaster) -> None:
+    self.dec.update(sm)
+
  def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
    plan_sp_send = messaging.new_message('longitudinalPlanSP')

@@ -94,6 +145,15 @@ class LongitudinalPlannerSP:
    dec.state = DecState.blended if self.dec.mode() == 'blended' else DecState.acc
    dec.enabled = self.dec.enabled()
    dec.active = self.dec.active()
+    dec.decelIntent = float(self.dec.signals.decel_intent)
+    dec.curveDetected = bool(self.dec.signals.curve_detected)
+    dec.wantBlended = bool(self.dec.want_blended)
+    dec.leadVeto = bool(self.dec.lead_veto)
+
+    accel_controller = longitudinalPlanSP.accelController
+    accel_controller.enabled = bool(self.accel_controller.is_enabled())
+    accel_controller.active = bool(self.accel_controller_active)
+    accel_controller.profile = int(self.accel_controller.profile)

    # Smart Cruise Control
    smartCruiseControl = longitudinalPlanSP.smartCruiseControl
@@ -4,6 +4,8 @@ Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
 This file is part of sunnypilot and is licensed under the MIT License.
 See the LICENSE.md file in the root directory for more details.
 """
+
+from types import SimpleNamespace
 from typing import Any

 import numpy as np
@@ -15,8 +17,23 @@ from openpilot.common.params import Params
 from openpilot.common.realtime import DT_MDL
 from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
 from openpilot.selfdrive.modeld.constants import ModelConstants
+from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP, LongitudinalPlanSource
 from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control import MIN_V
-from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.vision_controller import SmartCruiseControlVision, _ENTERING_PRED_LAT_ACC_TH
+
+from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.vision_controller import (
+  _A_LAT_REG_MAX,
+  _BELOW_EGO_TARGET_RELEASE_RATE,
+  _ENTERING_PRED_LAT_ACC_TH,
+  _MIN_ACTIVATION_SPEED,
+  _RELIEF_CONFIRMATION_FRAMES,
+  _TARGET_RELEASE_CONFIRMATION_FRAMES,
+  _TARGET_RELEASE_RATE,
+  _TARGET_TIGHTEN_CONFIRMATION_FRAMES,
+  _TARGET_TIGHTEN_RATE,
+  _TURNING_LAT_ACC_TH,
+  _URGENT_PRED_LAT_ACC_TH,
+  SmartCruiseControlVision,
+)
 from openpilot.common.test import OpenpilotTestCase

 VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.VisionState
@@ -107,7 +124,6 @@ def generate_controlsState():


 class TestSmartCruiseControlVision(OpenpilotTestCase):
-
  def setup_method(self):
    self.params = Params()
    self.reset_params()
@@ -121,36 +137,377 @@ class TestSmartCruiseControlVision(OpenpilotTestCase):
  def reset_params(self):
    self.params.put_bool("SmartCruiseControlVision", True, block=True)

+  def assert_approx(self, actual, expected):
+    self.assertAlmostEqual(actual, expected, delta=max(1e-12, abs(expected) * 1e-6))
+
+  def set_lat_accels(self, current: float, predicted: float, v_ego: float = 20.0, model_speed: float = 20.0) -> None:
+    self.sm['controlsState'].curvature = current / v_ego**2
+    self.sm['modelV2'].velocity.x = [model_speed] * len(ModelConstants.T_IDXS)
+    self.sm['modelV2'].orientationRate.z = [predicted / model_speed] * len(ModelConstants.T_IDXS)
+
+  def update_lat_accels(
+    self, current: float, predicted: float, cruise: float = 30.0, a_ego: float = 0.0, v_ego: float = 20.0, model_speed: float = 20.0
+  ) -> None:
+    self.set_lat_accels(current, predicted, v_ego, model_speed)
+    self.scc_v.update(self.sm, True, False, v_ego, a_ego, cruise)
+
+  def enter_curve(self, predicted: float = 2.2) -> None:
+    self.update_lat_accels(0.5, predicted)
+    self.update_lat_accels(0.5, predicted)
+    assert self.scc_v.state == VisionState.entering
+
  def test_initial_state(self):
    assert self.scc_v.state == VisionState.disabled
    assert not self.scc_v.is_active
    assert self.scc_v.output_v_target == V_CRUISE_UNSET
-    assert self.scc_v.output_a_target == 0.
+    assert self.scc_v.output_a_target == 0.0

  def test_system_disabled(self):
    self.params.put_bool("SmartCruiseControlVision", False, block=True)
    self.scc_v.enabled = self.params.get_bool("SmartCruiseControlVision")

-    for _ in range(int(10. / DT_MDL)):
-      self.scc_v.update(self.sm, True, False, 0., 0., 0.)
+    for _ in range(int(10.0 / DT_MDL)):
+      self.scc_v.update(self.sm, True, False, 0.0, 0.0, 0.0)
    assert self.scc_v.state == VisionState.disabled
    assert not self.scc_v.is_active

  def test_disabled(self):
-    for _ in range(int(10. / DT_MDL)):
-      self.scc_v.update(self.sm, False, False, 0., 0., 0.)
+    for _ in range(int(10.0 / DT_MDL)):
+      self.scc_v.update(self.sm, False, False, 0.0, 0.0, 0.0)
    assert self.scc_v.state == VisionState.disabled

  def test_transition_disabled_to_enabled(self):
-    for _ in range(int(10. / DT_MDL)):
-      self.scc_v.update(self.sm, True, False, 0., 0., 0.)
+    for _ in range(int(10.0 / DT_MDL)):
+      self.scc_v.update(self.sm, True, False, 0.0, 0.0, 0.0)
    assert self.scc_v.state == VisionState.enabled

-  @parameterized.expand([
+  def test_unconfirmed_release_holds_but_urgent_reentry_tightens(self):
+    self.enter_curve()
+    targets = [self.scc_v.output_v_target]
+
+    self.update_lat_accels(2.0, 2.2, a_ego=-0.8)
+    assert self.scc_v.state == VisionState.turning
+    assert self.scc_v.output_a_target == -0.8
+    turning_demand = self.scc_v._v_demand()
+    targets.append(self.scc_v.output_v_target)
+
+    self.update_lat_accels(1.2, 1.2, a_ego=0.3)
+    assert self.scc_v.state == VisionState.leaving
+    assert self.scc_v.output_a_target == 0.3
+    targets.append(self.scc_v.output_v_target)
+
+    self.update_lat_accels(1.0, 3.0, a_ego=-1.2)
+    assert self.scc_v.state == VisionState.entering
+    assert self.scc_v.output_a_target == -1.2
+    reentry_demand = self.scc_v._v_demand()
+    targets.append(self.scc_v.output_v_target)
+
+    entering, turning, leaving, reentering = targets
+    assert turning < entering
+    self.assert_approx(turning, turning_demand)
+    self.assert_approx(leaving, turning)
+    assert reentering < leaving
+    self.assert_approx(reentering, reentry_demand)
+
+  def test_new_curve_interrupts_confirmed_release_immediately(self):
+    self.enter_curve()
+    for _ in range(_RELIEF_CONFIRMATION_FRAMES + 1):
+      self.update_lat_accels(0.8, 0.8)
+    releasing_v_target = self.scc_v.output_v_target
+    assert self.scc_v.state == VisionState.leaving
+
+    self.update_lat_accels(0.8, 3.0, a_ego=-0.7)
+    assert self.scc_v.state == VisionState.entering
+    assert self.scc_v.output_v_target < releasing_v_target
+    assert self.scc_v.output_a_target == -0.7
+
+  @parameterized.expand([(-2.0,), (-0.5,), (0.0,), (0.8,)])
+  def test_planner_acceleration_passes_through_exactly(self, planner_accel):
+    self.enter_curve()
+    self.update_lat_accels(0.5, 2.2, a_ego=planner_accel)
+    assert self.scc_v.output_a_target == planner_accel
+
+  def test_planner_acceleration_passes_through_all_states(self):
+    cases = (
+      (False, False, 0.5, 2.2, -0.2, VisionState.disabled),
+      (True, False, 0.5, 0.8, 0.1, VisionState.enabled),
+      (True, False, 0.5, 2.2, -0.4, VisionState.entering),
+      (True, False, 2.0, 2.2, -0.8, VisionState.turning),
+      (True, False, 1.2, 1.2, 0.3, VisionState.leaving),
+      (True, True, 1.2, 1.2, 0.6, VisionState.overriding),
+    )
+    for long_enabled, override, current, predicted, planner_accel, state in cases:
+      self.set_lat_accels(current, predicted)
+      self.scc_v.update(self.sm, long_enabled, override, 20.0, planner_accel, 30.0)
+      assert self.scc_v.state == state
+      assert self.scc_v.output_a_target == planner_accel
+
+  def test_jitter_requires_confirmed_relief_then_releases_smoothly(self):
+    self.enter_curve()
+    previous_v_target = self.scc_v.output_v_target
+
+    for frame in range(_RELIEF_CONFIRMATION_FRAMES * 2):
+      self.update_lat_accels(1.0, 1.05 if frame % 2 == 0 else 1.15)
+      assert self.scc_v.state == VisionState.entering
+      assert self.scc_v.output_v_target >= previous_v_target
+      assert self.scc_v.output_v_target - previous_v_target <= _BELOW_EGO_TARGET_RELEASE_RATE * DT_MDL + 1e-9
+      previous_v_target = self.scc_v.output_v_target
+
+    for _ in range(_RELIEF_CONFIRMATION_FRAMES):
+      self.update_lat_accels(1.15, 0.8)
+      assert self.scc_v.state == VisionState.entering
+      assert 0.0 <= self.scc_v.output_v_target - previous_v_target <= _BELOW_EGO_TARGET_RELEASE_RATE * DT_MDL + 1e-9
+      previous_v_target = self.scc_v.output_v_target
+
+    release_cruise = 30.0
+    for _ in range(_RELIEF_CONFIRMATION_FRAMES - 1):
+      self.update_lat_accels(0.8, 0.8, release_cruise)
+      assert self.scc_v.state == VisionState.entering
+      assert 0.0 <= self.scc_v.output_v_target - previous_v_target <= _BELOW_EGO_TARGET_RELEASE_RATE * DT_MDL + 1e-9
+      previous_v_target = self.scc_v.output_v_target
+
+    active_v_targets = [previous_v_target]
+    for _ in range(int((release_cruise - previous_v_target) / (_TARGET_RELEASE_RATE * DT_MDL)) + 10):
+      self.update_lat_accels(0.8, 0.8, release_cruise)
+      if not self.scc_v.is_active:
+        break
+      assert self.scc_v.state == VisionState.leaving
+      assert self.scc_v.output_v_target != V_CRUISE_UNSET
+      active_v_targets.append(self.scc_v.output_v_target)
+
+    assert self.scc_v.state == VisionState.enabled
+    assert self.scc_v.output_v_target == V_CRUISE_UNSET
+    self.assert_approx(active_v_targets[-1], release_cruise)
+    assert np.all((np.diff(active_v_targets) >= 0.0) & (np.diff(active_v_targets) <= _BELOW_EGO_TARGET_RELEASE_RATE * DT_MDL + 1e-9))
+
+  def test_target_release_waits_for_relief_above_ego_speed(self):
+    self.enter_curve()
+    held_v_target = self.scc_v.output_v_target
+    self.assert_approx(held_v_target, self.scc_v.v_ego)
+
+    for _ in range(_RELIEF_CONFIRMATION_FRAMES + _TARGET_RELEASE_CONFIRMATION_FRAMES - 2):
+      self.update_lat_accels(0.8, 0.8)
+      self.assert_approx(self.scc_v.output_v_target, held_v_target)
+
+    self.update_lat_accels(0.8, 0.8)
+    rise = self.scc_v.output_v_target - held_v_target
+    assert 0.0 < rise <= _TARGET_RELEASE_RATE * DT_MDL + 1e-9
+
+  def test_curve_target_is_independent_of_ego_speed(self):
+    model_speed = 24.0
+    predicted_yaw_rate = 0.12
+    predicted_lat_accel = model_speed * predicted_yaw_rate
+    expected_v_target = (_A_LAT_REG_MAX / (predicted_yaw_rate / model_speed)) ** 0.5
+    targets = []
+
+    for v_ego in (18.0, 28.0):
+      controller = SmartCruiseControlVision()
+      self.set_lat_accels(0.5, predicted_lat_accel, v_ego, model_speed)
+      controller.update(self.sm, True, False, v_ego, 0.0, 30.0)
+      controller.update(self.sm, True, False, v_ego, 0.0, 30.0)
+      assert controller.state == VisionState.entering
+      targets.append(controller.v_target)
+
+    self.assert_approx(targets[0], expected_v_target)
+    self.assert_approx(targets[1], expected_v_target)
+
+  def test_curve_target_respects_minimum_speed_floor(self):
+    model_speed = 10.0
+    predicted_yaw_rate = 2.0
+    self.set_lat_accels(0.5, model_speed * predicted_yaw_rate, model_speed=model_speed)
+    self.scc_v.update(self.sm, True, False, 20.0, 0.0, 30.0)
+    self.scc_v.update(self.sm, True, False, 20.0, 0.0, 30.0)
+
+    assert self.scc_v.state == VisionState.entering
+    assert self.scc_v.v_target < MIN_V
+    self.assert_approx(self.scc_v.output_v_target, MIN_V)
+
+  @parameterized.expand(
+    [([], []), ([np.nan] * len(ModelConstants.T_IDXS), [np.nan] * len(ModelConstants.T_IDXS)), ([20.0] * 5, [0.1] * 3)],
+    names=["velocities", "yaw_rates"],
+  )
+  def test_model_vector_edges_remain_finite(self, velocities, yaw_rates):
+    self.sm['modelV2'].velocity.x = velocities
+    self.sm['modelV2'].orientationRate.z = yaw_rates
+    self.scc_v.update(self.sm, True, False, 20.0, 0.0, 30.0)
+    self.scc_v.update(self.sm, True, False, 20.0, 0.0, 30.0)
+
+    assert all(
+      np.isfinite(value)
+      for value in (
+        self.scc_v.current_lat_acc,
+        self.scc_v.max_pred_lat_acc,
+        self.scc_v.v_target,
+        self.scc_v.output_v_target,
+        self.scc_v.output_a_target,
+      )
+    )
+
+  @parameterized.expand([(5.75,), (9.9,), (_MIN_ACTIVATION_SPEED,)])
+  def test_vision_control_does_not_steal_launch(self, launch_speed):
+    self.set_lat_accels(0.5, 3.0, launch_speed)
+    self.scc_v.update(self.sm, True, False, launch_speed, 0.0, 30.0)
+    self.scc_v.update(self.sm, True, False, launch_speed, 0.0, 30.0)
+
+    assert launch_speed <= _MIN_ACTIVATION_SPEED
+    assert self.scc_v.state == VisionState.enabled
+    assert not self.scc_v.is_active
+    assert self.scc_v.output_v_target == V_CRUISE_UNSET
+
+  def test_vision_control_can_activate_above_launch_range(self):
+    speed = _MIN_ACTIVATION_SPEED + 0.01
+    self.set_lat_accels(0.5, 3.0, speed)
+    self.scc_v.update(self.sm, True, False, speed, 0.0, 30.0)
+    self.scc_v.update(self.sm, True, False, speed, 0.0, 30.0)
+
+    assert self.scc_v.state == VisionState.entering
+    assert self.scc_v.is_active
+
+  def test_nonurgent_activation_has_no_target_cliff(self):
+    v_ego = _MIN_ACTIVATION_SPEED + 0.01
+    model_speed = 8.0
+    self.update_lat_accels(0.5, 2.0, v_ego=v_ego, model_speed=model_speed)
+    self.update_lat_accels(0.5, 2.0, v_ego=v_ego, model_speed=model_speed)
+
+    self.assert_approx(self.scc_v.v_target, 8.0)
+    self.assert_approx(self.scc_v.output_v_target, v_ego)
+
+  def test_nonurgent_tightening_is_confirmed_and_rate_limited(self):
+    self.enter_curve()
+    initial_v_target = self.scc_v.output_v_target
+
+    for _ in range(_TARGET_TIGHTEN_CONFIRMATION_FRAMES - 1):
+      self.update_lat_accels(0.5, 2.8)
+      self.assert_approx(self.scc_v.output_v_target, initial_v_target)
+
+    self.update_lat_accels(0.5, 2.8)
+    drop = initial_v_target - self.scc_v.output_v_target
+    assert 0.0 < drop <= _TARGET_TIGHTEN_RATE * DT_MDL + 1e-9
+
+  def test_one_frame_curve_prediction_does_not_pulse_target(self):
+    self.enter_curve()
+    for _ in range(10):
+      self.update_lat_accels(0.5, 2.2)
+    stable_v_target = self.scc_v.output_v_target
+
+    self.update_lat_accels(0.5, 2.8)
+    self.assert_approx(self.scc_v.output_v_target, stable_v_target)
+    self.update_lat_accels(0.5, 2.2)
+
+    self.assert_approx(self.scc_v.output_v_target, stable_v_target)
+
+  def test_one_frame_release_does_not_reverse_target(self):
+    self.enter_curve(_URGENT_PRED_LAT_ACC_TH)
+    stable_v_target = self.scc_v.output_v_target
+
+    self.update_lat_accels(0.5, 2.2)
+    self.assert_approx(self.scc_v.output_v_target, stable_v_target)
+    self.update_lat_accels(0.5, _URGENT_PRED_LAT_ACC_TH)
+
+    self.assert_approx(self.scc_v.output_v_target, stable_v_target)
+
+  def test_urgent_predicted_curve_is_not_delayed(self):
+    self.enter_curve()
+    self.update_lat_accels(0.5, _URGENT_PRED_LAT_ACC_TH)
+
+    self.assert_approx(self.scc_v.output_v_target, self.scc_v._v_demand())
+
+  def test_current_curve_is_not_delayed(self):
+    self.enter_curve()
+    self.update_lat_accels(_TURNING_LAT_ACC_TH, 2.8)
+
+    self.assert_approx(self.scc_v.output_v_target, self.scc_v._v_demand())
+
+  def test_sequential_curve_confirms_release_and_tightens_urgently(self):
+    self.enter_curve(3.0)
+    for _ in range(20):
+      self.update_lat_accels(0.5, 3.0)
+    restrictive_v_target = self.scc_v.output_v_target
+
+    self.update_lat_accels(0.5, 1.4, a_ego=0.4)
+    assert self.scc_v.state == VisionState.entering
+    self.assert_approx(self.scc_v.output_v_target, restrictive_v_target)
+    assert self.scc_v.output_a_target == 0.4
+
+    for _ in range(_TARGET_RELEASE_CONFIRMATION_FRAMES - 2):
+      self.update_lat_accels(0.5, 1.4)
+      self.assert_approx(self.scc_v.output_v_target, restrictive_v_target)
+
+    self.update_lat_accels(0.5, 1.4)
+    released_v_target = self.scc_v.output_v_target
+    assert 0.0 < released_v_target - restrictive_v_target <= _BELOW_EGO_TARGET_RELEASE_RATE * DT_MDL + 1e-9
+
+    self.update_lat_accels(0.5, 3.0, a_ego=-0.6)
+    assert self.scc_v.state == VisionState.entering
+    self.assert_approx(self.scc_v.output_v_target, restrictive_v_target)
+    assert self.scc_v.output_a_target == -0.6
+
+    for _ in range(4):
+      self.update_lat_accels(0.5, 1.4)
+      self.assert_approx(self.scc_v.output_v_target, restrictive_v_target)
+      self.update_lat_accels(0.5, 3.0)
+      self.assert_approx(self.scc_v.output_v_target, restrictive_v_target)
+
+  def test_acceleration_is_continuous_through_planner_arbitration(self):
+    car_control = messaging.new_message('carControl')
+    car_control.carControl.enabled = True
+    car_control.carControl.cruiseControl.override = False
+    self.sm['carControl'] = car_control.carControl
+    self.sm['carState'].vCruiseCluster = 108.0
+
+    planner: Any = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
+    planner.scc = SimpleNamespace(
+      vision=self.scc_v,
+      map=SimpleNamespace(output_v_target=V_CRUISE_UNSET, output_a_target=0.0),
+      update=lambda sm, enabled, override, v_ego, a_ego, v_cruise: self.scc_v.update(sm, enabled, override, v_ego, a_ego, v_cruise),
+    )
+    planner.resolver = SimpleNamespace(
+      speed_limit_valid=False,
+      speed_limit_last_valid=False,
+      speed_limit=0.0,
+      speed_limit_final_last=0.0,
+      distance=0.0,
+      update=lambda _v_ego, _sm: None,
+    )
+    planner.sla = SimpleNamespace(
+      output_v_target=V_CRUISE_UNSET,
+      output_a_target=0.0,
+      update=lambda *_args: None,
+    )
+    planner.events_sp = SimpleNamespace()
+
+    self.set_lat_accels(0.5, 2.2)
+    planner.update_targets(self.sm, 20.0, -0.8, 30.0)
+    planner.update_targets(self.sm, 20.0, -0.8, 30.0)
+    assert planner.source == LongitudinalPlanSource.sccVision
+    assert planner.output_a_target == -0.8
+
+    for planner_accel in (-2.0, 0.5, -0.2):
+      planner.update_targets(self.sm, 20.0, planner_accel, 30.0)
+      assert planner.source == LongitudinalPlanSource.sccVision
+      assert planner.output_a_target == planner_accel
+
+    self.set_lat_accels(0.8, 0.8)
+    for _ in range(int(30.0 / (_TARGET_RELEASE_RATE * DT_MDL)) + 10):
+      planner.update_targets(self.sm, 20.0, 0.4, 30.0)
+      assert planner.output_a_target == 0.4
+      if planner.source == LongitudinalPlanSource.cruise:
+        break
+    else:
+      self.fail("SCC Vision did not release to cruise")
+
+    planner.update_targets(self.sm, 20.0, 0.4, 30.0)
+    assert self.scc_v.state == VisionState.enabled
+    assert planner.source == LongitudinalPlanSource.cruise
+
+  @parameterized.expand(
+    [
      ("p97_just_above_threshold", True),
      ("single_spike_filtered", False),
      ("persistent_high_values", True),
-    ], names=["case", "should_enter"])
+    ],
+    names=["case", "should_enter"],
+  )
  def test_max_pred_lat_acc_uses_p97_and_threshold(self, case, should_enter):
    n = len(ModelConstants.T_IDXS)
    th = float(_ENTERING_PRED_LAT_ACC_TH)
@@ -0,0 +1,110 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+
+import gc
+from contextlib import ExitStack
+from unittest import mock
+
+import numpy as np
+
+from openpilot.common.test import OpenpilotTestCase
+from openpilot.selfdrive.test.longitudinal_maneuvers.plant import Plant
+from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanSource
+from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.vision_controller import _A_LAT_REG_MAX
+
+
+def _run_constant_curve(*, scc_enabled: bool, cruise: float, duration: float = 70.0) -> dict[str, np.ndarray]:
+  gc.collect()
+  curvature = 0.005
+  plant = Plant(lead_relevancy=False, speed=30.0)
+  planner = plant.planner
+  planner.dec._enabled = False
+  planner.scc.map.enabled = False
+  planner.scc.vision.enabled = scc_enabled
+  solver_failures = 0
+
+  with ExitStack() as patches:
+    patches.enter_context(mock.patch.object(planner.dec, "_read_params", return_value=None))
+    patches.enter_context(mock.patch.object(planner.scc.map, "update_params", return_value=None))
+    patches.enter_context(mock.patch.object(planner.scc.vision, "_update_params", return_value=None))
+
+    original_mpc_reset = planner.mpc.reset
+
+    def record_mpc_reset(*args, **kwargs):
+      nonlocal solver_failures
+      solver_failures += int(planner.mpc.solution_status != 0)
+      return original_mpc_reset(*args, **kwargs)
+
+    patches.enter_context(mock.patch.object(planner.mpc, "reset", side_effect=record_mpc_reset))
+
+    if scc_enabled:
+      original_update_calculations = planner.scc.vision._update_calculations
+
+      def inject_constant_curvature(sm):
+        velocities = np.asarray(sm['modelV2'].velocity.x, dtype=float)
+        sm['modelV2'].orientationRate.z = (curvature * velocities).tolist()
+        sm['controlsState'].curvature = curvature
+        original_update_calculations(sm)
+
+      patches.enter_context(mock.patch.object(planner.scc.vision, "_update_calculations", side_effect=inject_constant_curvature))
+
+    original_update = planner.update
+
+    def enable_longitudinal(sm):
+      sm['carControl'].enabled = True
+      sm['carControl'].longActive = True
+      original_update(sm)
+
+    patches.enter_context(mock.patch.object(planner, "update", side_effect=enable_longitudinal))
+    rows = []
+    while plant.current_time < duration:
+      output = plant.step(v_cruise=cruise)
+      rows.append(
+        (
+          plant.current_time,
+          output['speed'],
+          output['should_stop'],
+          planner.scc.vision.is_active,
+          planner.source == LongitudinalPlanSource.sccVision,
+          planner.scc.vision.output_v_target,
+        )
+      )
+
+  data = np.asarray(rows, dtype=float)
+  gc.collect()
+  return {
+    'time': data[:, 0],
+    'speed': data[:, 1],
+    'should_stop': data[:, 2],
+    'active': data[:, 3],
+    'scc_source': data[:, 4],
+    'target': data[:, 5],
+    'solver_failures': np.asarray(solver_failures),
+  }
+
+
+class TestVisionControllerClosedLoop(OpenpilotTestCase):
+  def test_constant_curve_recovers_like_stock_speed_cap(self):
+    target = (_A_LAT_REG_MAX / 0.005) ** 0.5
+    scc = _run_constant_curve(scc_enabled=True, cruise=30.0)
+    stock = _run_constant_curve(scc_enabled=False, cruise=target)
+    scc_final = scc['speed'][scc['time'] >= 60.0]
+    stock_final = stock['speed'][stock['time'] >= 60.0]
+
+    # The generated solver can report platform-specific failures for the
+    # synthetic no-lead plant. The feature must not make that stock baseline
+    # worse; requiring an absolute zero would hide a harness difference as a
+    # controller regression.
+    assert scc['solver_failures'] <= stock['solver_failures']
+    assert not scc['should_stop'].any()
+    assert np.all(scc['active'][scc['time'] >= 60.0])
+    assert np.all(scc['scc_source'][scc['time'] >= 60.0])
+    assert np.allclose(scc['target'][scc['time'] >= 60.0], target)
+    assert scc_final.min() >= target - 1.0
+    assert abs(scc_final.mean() - stock_final.mean()) < 0.5
+    assert abs(scc_final.min() - stock_final.min()) < 1.0
+    assert abs(scc_final.max() - stock_final.max()) < 1.0
@@ -23,25 +23,21 @@ _ENTERING_PRED_LAT_ACC_TH = 1.3  # Predicted Lat Acc threshold to trigger enteri
 _ABORT_ENTERING_PRED_LAT_ACC_TH = 1.1  # Predicted Lat Acc threshold to abort entering state if speed drops.

 _TURNING_LAT_ACC_TH = 1.6  # Lat Acc threshold to trigger turning state.
+_URGENT_PRED_LAT_ACC_TH = 3.  # Predicted Lat Acc threshold that requires an immediate speed reduction.

 _LEAVING_LAT_ACC_TH = 1.3  # Lat Acc threshold to trigger leaving turn state.
 _FINISH_LAT_ACC_TH = 1.1  # Lat Acc threshold to trigger the end of the turn cycle.

 _A_LAT_REG_MAX = 2.  # Maximum lateral acceleration

-_NO_OVERSHOOT_TIME_HORIZON = 4.  # s. Time to use for velocity desired based on a_target when not overshooting.
-
-# Lookup table for the minimum smooth deceleration during the ENTERING state
-# depending on the actual maximum absolute lateral acceleration predicted on the turn ahead.
-_ENTERING_SMOOTH_DECEL_V = [-0.2, -1.]  # min decel value allowed on ENTERING state
-_ENTERING_SMOOTH_DECEL_BP = [1.3, 3.]  # absolute value of lat acc ahead
-
-# Lookup table for the acceleration for the TURNING state
-# depending on the current lateral acceleration of the vehicle.
-_TURNING_ACC_V = [0.5, 0., -0.4]  # acc value
-_TURNING_ACC_BP = [1.5, 2.3, 3.]  # absolute value of current lat acc
-
-_LEAVING_ACC = 0.5  # Conformable acceleration to regain speed while leaving a turn.
+_RELIEF_CONFIRMATION_FRAMES = max(1, int(round(0.5 / DT_MDL)))
+_TARGET_TIGHTEN_CONFIRMATION_FRAMES = max(1, int(round(0.1 / DT_MDL)))
+_TARGET_RELEASE_CONFIRMATION_FRAMES = max(1, int(round(0.15 / DT_MDL)))
+_TARGET_TIGHTEN_RATE = 5.  # m/s^2
+_TARGET_RELEASE_RATE = 1.  # m/s^2
+_BELOW_EGO_TARGET_RELEASE_RATE = 3.  # m/s^2
+_MIN_PRED_SPEED = 1.  # m/s
+_MIN_ACTIVATION_SPEED = 10.  # m/s


 class SmartCruiseControlVision:
@@ -65,14 +61,62 @@ class SmartCruiseControlVision:
    self.state = VisionState.disabled
    self.current_lat_acc = 0.
    self.max_pred_lat_acc = 0.
+    self.relief_frames = 0
+    self.tighten_frames = 0
+    self.release_frames = 0
+
+  def _v_demand(self) -> float:
+    return max(MIN_V, min(self.v_target, self.v_cruise_setpoint))
+
+  def _curve_is_urgent(self) -> bool:
+    return self.current_lat_acc >= _TURNING_LAT_ACC_TH or self.max_pred_lat_acc >= _URGENT_PRED_LAT_ACC_TH
+
+  def _filtered_v_target(self) -> float:
+    demand = self._v_demand()
+
+    if self.output_v_target == V_CRUISE_UNSET:
+      self.tighten_frames = 0
+      self.release_frames = 0
+      if self._curve_is_urgent():
+        return demand
+      return max(demand, min(self.v_ego, self.v_cruise_setpoint))
+
+    if demand < self.output_v_target:
+      self.release_frames = 0
+      if self._curve_is_urgent():
+        self.tighten_frames = 0
+        return demand
+
+      self.tighten_frames += 1
+      if self.tighten_frames < _TARGET_TIGHTEN_CONFIRMATION_FRAMES:
+        return self.output_v_target
+      return max(demand, self.output_v_target - _TARGET_TIGHTEN_RATE * DT_MDL)
+
+    self.tighten_frames = 0
+    releasing_brake = self.output_v_target < min(self.v_ego, demand)
+    if not releasing_brake and self.relief_frames < _RELIEF_CONFIRMATION_FRAMES:
+      self.release_frames = 0
+      return self.output_v_target
+
+    if demand > self.output_v_target:
+      self.release_frames += 1
+      if self.release_frames < _TARGET_RELEASE_CONFIRMATION_FRAMES:
+        return self.output_v_target
+    else:
+      self.release_frames = 0
+
+    release_rate = _BELOW_EGO_TARGET_RELEASE_RATE if releasing_brake else _TARGET_RELEASE_RATE
+    return min(demand, self.output_v_target + release_rate * DT_MDL)

  def get_a_target_from_control(self) -> float:
-    return self.a_target
+    return self.a_ego

  def get_v_target_from_control(self) -> float:
    if self.is_active:
-      return max(self.v_target, MIN_V) + self.a_target * _NO_OVERSHOOT_TIME_HORIZON
+      return self._filtered_v_target()

+    self.tighten_frames = 0
+    self.release_frames = 0
    return V_CRUISE_UNSET

  def _update_params(self) -> None:
@@ -82,25 +126,27 @@ class SmartCruiseControlVision:
  def _update_calculations(self, sm: messaging.SubMaster) -> None:
    if not self.long_enabled:
      return
-    else:
-      rate_plan = np.array(np.abs(sm['modelV2'].orientationRate.z))
-      vel_plan = np.array(sm['modelV2'].velocity.x)

-      self.current_lat_acc = self.v_ego ** 2 * abs(sm['controlsState'].curvature)
+    rate_plan = np.asarray(np.abs(sm['modelV2'].orientationRate.z), dtype=float)
+    vel_plan = np.asarray(sm['modelV2'].velocity.x, dtype=float)
+    size = min(len(rate_plan), len(vel_plan))
+    rate_plan, vel_plan = rate_plan[:size], vel_plan[:size]
+    valid = np.isfinite(rate_plan) & np.isfinite(vel_plan) & (vel_plan >= _MIN_PRED_SPEED)

-      # get the maximum lat accel from the model
-      predicted_lat_accels = rate_plan * vel_plan
-      self.max_pred_lat_acc = np.percentile(predicted_lat_accels, 97)
-
-      # get the maximum curve based on the current velocity
-      v_ego = max(self.v_ego, 0.1)  # ensure a value greater than 0 for calculations
-      max_curve = self.max_pred_lat_acc / (v_ego**2)
-
-      # Get the target velocity for the maximum curve
-      self.v_target = (_A_LAT_REG_MAX / max_curve) ** 0.5
+    self.current_lat_acc = self.v_ego ** 2 * abs(sm['controlsState'].curvature)
+    self.max_pred_lat_acc = 0.
+    self.v_target = V_CRUISE_UNSET
+    if np.any(valid):
+      self.max_pred_lat_acc = float(np.percentile(rate_plan[valid] * vel_plan[valid], 97))
+      max_pred_curvature = float(np.percentile(rate_plan[valid] / vel_plan[valid], 97))
+      if max_pred_curvature > 0.:
+        self.v_target = min(float((_A_LAT_REG_MAX / max_pred_curvature) ** 0.5), V_CRUISE_UNSET)

  def _update_state_machine(self) -> tuple[bool, bool]:
    # ENABLED, ENTERING, TURNING, LEAVING, OVERRIDING
+    relief = self.current_lat_acc < _FINISH_LAT_ACC_TH and self.max_pred_lat_acc < _ABORT_ENTERING_PRED_LAT_ACC_TH
+    self.relief_frames = self.relief_frames + 1 if self.state in ACTIVE_STATES and relief else 0
+
    if self.state != VisionState.disabled:
      # longitudinal and feature disable always have priority in a non-disabled state
      if not self.long_enabled or not self.enabled:
@@ -112,7 +158,7 @@ class SmartCruiseControlVision:
        # ENABLED
        if self.state == VisionState.enabled:
          # Do not enter a turn control cycle if the speed is low.
-          if self.v_ego <= MIN_V:
+          if self.v_ego <= _MIN_ACTIVATION_SPEED:
            pass
          # If significant lateral acceleration is predicted ahead, then move to Entering turn state.
          elif self.max_pred_lat_acc >= _ENTERING_PRED_LAT_ACC_TH:
@@ -128,23 +174,26 @@ class SmartCruiseControlVision:
          # Transition to Turning if current lateral acceleration is over the threshold.
          if self.current_lat_acc >= _TURNING_LAT_ACC_TH:
            self.state = VisionState.turning
-          # Abort if the predicted lateral acceleration drops
-          elif self.max_pred_lat_acc < _ABORT_ENTERING_PRED_LAT_ACC_TH:
-            self.state = VisionState.enabled
+          # Begin releasing only after both current and predicted lateral acceleration stay clear.
+          elif self.relief_frames >= _RELIEF_CONFIRMATION_FRAMES:
+            self.state = VisionState.leaving

        # TURNING
        elif self.state == VisionState.turning:
-          # Transition to Leaving if current lateral acceleration drops below a threshold.
+          # Transition out of Turning if current lateral acceleration drops below a threshold.
          if self.current_lat_acc <= _LEAVING_LAT_ACC_TH:
-            self.state = VisionState.leaving
+            self.state = VisionState.entering if self.max_pred_lat_acc >= _ENTERING_PRED_LAT_ACC_TH else VisionState.leaving

        # LEAVING
        elif self.state == VisionState.leaving:
          # Transition back to Turning if current lateral acceleration goes back over the threshold.
          if self.current_lat_acc >= _TURNING_LAT_ACC_TH:
            self.state = VisionState.turning
-          # Finish if current lateral acceleration goes below a threshold.
-          elif self.current_lat_acc < _FINISH_LAT_ACC_TH:
+          # Start a new turn cycle immediately if another curve is predicted.
+          elif self.max_pred_lat_acc >= _ENTERING_PRED_LAT_ACC_TH:
+            self.state = VisionState.entering
+          # Finish after confirmed relief and a gradual release to the cruise setpoint.
+          elif self.relief_frames >= _RELIEF_CONFIRMATION_FRAMES and self.output_v_target >= self.v_cruise_setpoint:
            self.state = VisionState.enabled

    # DISABLED
@@ -157,32 +206,11 @@ class SmartCruiseControlVision:

    enabled = self.state in ENABLED_STATES
    active = self.state in ACTIVE_STATES
+    if not active:
+      self.relief_frames = 0

    return enabled, active

-  def _update_solution(self) -> float:
-    # DISABLED, ENABLED, OVERRIDING
-    if self.state not in ACTIVE_STATES:
-      # when not overshooting, calculate v_turn as the speed at the prediction horizon when following
-      # the smooth deceleration.
-      a_target = self.a_ego
-    # ENTERING
-    elif self.state == VisionState.entering:
-      # when not overshooting, target a smooth deceleration in preparation for a sharp turn to come.
-      a_target = np.interp(self.max_pred_lat_acc, _ENTERING_SMOOTH_DECEL_BP, _ENTERING_SMOOTH_DECEL_V)
-    # TURNING
-    elif self.state == VisionState.turning:
-      # When turning, we provide a target acceleration that is comfortable for the lateral acceleration felt.
-      a_target = np.interp(self.current_lat_acc, _TURNING_ACC_BP, _TURNING_ACC_V)
-    # LEAVING
-    elif self.state == VisionState.leaving:
-      # When leaving, we provide a comfortable acceleration to regain speed.
-      a_target = _LEAVING_ACC
-    else:
-      raise NotImplementedError(f"SCC-V state not supported: {self.state}")
-
-    return a_target
-
  def update(self, sm: messaging.SubMaster, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float,
             v_cruise_setpoint: float) -> None:
    self.long_enabled = long_enabled
@@ -195,7 +223,7 @@ class SmartCruiseControlVision:
    self._update_calculations(sm)

    self.is_enabled, self.is_active = self._update_state_machine()
-    self.a_target = self._update_solution()
+    self.a_target = self.a_ego

    self.output_v_target = self.get_v_target_from_control()
    self.output_a_target = self.get_a_target_from_control()
@@ -0,0 +1,338 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+
+import math
+
+import numpy as np
+
+from openpilot.common.realtime import DT_CTRL
+from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
+
+
+# Confirmed stop. Owner 2026-10-08: "can the brake be added 0.2 s
+# after the stop, but only when it is 100% certain the car has stopped?". The wheel-pulse standstill flag alone is not
+# certain: on 2 of 30 engaged stops (route 00000100 18:59:09, 19:37:50) the pulse counter still stepped 0.06 s after
+# it (~0.2 m). The body's own signature is: while the car still rolls it decelerates; at the real stop the deceleration
+# collapses (the body pitches back). Using BOTH - the flag AND the longitudinal acceleration (calibrated pose) risen
+# STOP_CONFIRM_RISE above the braking level it had above STOP_LEVEL_V - no wheel pulse followed in any of the 30 stops
+# (routes fe..118). The hold then starts STOP_CONFIRM_DELAY later instead of after the 1.0 s hold delay; without a
+# confirmation (a very light stop, no pitch-back) the 1.0 s delay applies as before.
+STOP_LEVEL_V = 0.4  # m/s (~1.4 km/h): above this the braking level is tracked
+STOP_LEVEL_TAU = 0.2  # s
+STOP_CONFIRM_RISE = 0.2  # m/s^2
+STOP_CONFIRM_DELAY = 0.2  # s
+
+
+class StopConfirm:
+  def __init__(self):
+    self.reset()
+
+  def reset(self):
+    self.level: float | None = None
+    self.collapsed = False
+    self.confirmed_t = -1.0
+
+  def update(self, standstill: bool, v_ego: float, a_long: float | None) -> bool:
+    """returns True once the stop is confirmed and STOP_CONFIRM_DELAY has passed"""
+    if a_long is None or not math.isfinite(a_long):
+      self.reset()
+      return False
+    if v_ego > STOP_LEVEL_V and not standstill:
+      alpha = DT_CTRL / (STOP_LEVEL_TAU + DT_CTRL)
+      self.level = a_long if self.level is None else self.level + alpha * (a_long - self.level)
+      self.collapsed = False
+      self.confirmed_t = -1.0
+      return False
+    if self.level is not None and a_long >= self.level + STOP_CONFIRM_RISE:
+      self.collapsed = True
+    if standstill and self.collapsed:
+      self.confirmed_t = 0.0 if self.confirmed_t < 0.0 else self.confirmed_t + DT_CTRL
+    elif not standstill:
+      self.confirmed_t = -1.0
+    return self.confirmed_t >= STOP_CONFIRM_DELAY - 1e-9
+
+
+class StoppingController:
+  """Optional terminal-stop policy applied after stock LongControl.update()."""
+
+  STOPPING_DECEL_RATE = 0.3  # m/s^2/s
+  # once the wheels really stand (wheel pulse) and the hold delay has passed, press on firmly to stopAccel: the driver
+  # presses to 1000-2400 N within 1-2 s of stopping (owner 2026-10-02: the car must not be able to move again, also on
+  # a slope). 1.0 m/s^2/s reaches the default -2.0 (~2900 N) in ~1.9 s; the old 0.5 took ~3.7 s.
+  STANDSTILL_HOLD_RATE = 1.0  # m/s^2/s
+  # The standstill flag is the real wheel stop (wheel pulse counter). Driver 2026-10-05, after a slope stop at 15:53
+  # where "at the last moment the hold locked early with a jerk": "let the car sit at -0.25 a while longer, then add the
+  # parking force" -> 1.0 s at the end value before the ramp to stopAccel (was 0.2 s; the 10-03 "never more than 0.2 s"
+  # was about the auto hold under a pedal, not this ramp). If the car creeps on a slope during that second the creep
+  # path below raises the request on its own (CREEP_RATE_GROWTH), so the wait costs nothing on the flat and is bounded
+  # on a hill.
+  STANDSTILL_HOLD_DELAY_LEAD = 1.0  # s
+  STANDSTILL_HOLD_DELAY_NO_LEAD = 1.0  # s
+  STOPPING_FREEZE_MAX = 2.0  # s
+  STOPPING_EXIT_DEBOUNCE = 0.2  # s
+  # Engaging at a standstill (driver 2026-10-06, route 0000010a 23:58:49: "pulling away with no lead, just before the car
+  # really moves there is a very short hard brake, then it goes"). The car stood in the auto hold (1360 N); engaging
+  # dropped the hold to 0 N, the plan's first frame still said stop, so the stopping state started the standstill hold
+  # ramp at once (it had stood long past the hold delay) - -0.21 m/s^2 within 0.2 s, which the PCM turned into a
+  # 1600 N brake pulse (IMU +0.6 m/s^2 jolt) before the launch 0.4 s later. For ENGAGE_STANDSTILL_GRACE after engaging
+  # at a standstill with no lead the request stays at zero instead: the launch follows straight away; if the plan
+  # really wants to stay stopped, the hold ramp starts after the grace.
+  ENGAGE_STANDSTILL_GRACE = 0.6  # s
+  STOPPING_FOLLOW_MIN = -0.10  # m/s^2
+  STOPPING_FOLLOW_RATE = 2.0  # m/s^3
+  CREEP_V_MIN = 0.03  # m/s
+  CREEP_RATE_GROWTH = 1.5  # m/s^2/s per second of creep
+  CREEP_RATE_MAX = 1.0  # m/s^2/s
+
+  # The end of the stop. First road test of the template rule (route 000000ec, 2026-10-03, four op stops): holding
+  # END_REQUEST -0.15 (~660 N) from 3 km/h was a STEP - the plan was at -0.63..-0.78 (1160-1560 N) when the car crossed
+  # 3 km/h and the request fell to -0.15 within 0.3 s; the force dropped to 280-520 N, the car sped UP again (+0.1..+0.3
+  # m/s^2, 1.7 -> 2.5 km/h) on its creep torque and then needed 3.4-4.3 s and 760-1000 N (-0.2..-0.25 m/s^2) to stop.
+  # The driver felt exactly that: "at 3 km/h the brake suddenly lightens - wrong - there must be a smooth hand-over".
+  # The driver's own hard stop that night (23:45:04, 36 km/h, 4880 N peak) still ended smoothly because the pedal came
+  # off CONTINUOUSLY: -aEgo ~ 1.6 + 0.25 v (r 0.86), -3.4 m/s^2 at 27 km/h, -2.6 at 10, -1.4 at 3, -1.0 at 1 km/h -
+  # the decel falls roughly linearly with speed to the wheel stop, never a step, and does not go light at the end.
+  # So below BLEND_V the request follows a line in speed from the request it had at BLEND_V (a_entry) down to
+  # END_REQUEST at 0 km/h: blend(v) = END_REQUEST + (a_entry - END_REQUEST) * (v / BLEND_V) ** BLEND_POW. It is a FLOOR
+  # on the plan: the firmer of the plan and the line is sent, so the planner's own curve still wins while it is braking
+  # harder (a late-seen lead), and the line takes over only where the planner eases off. A plan that wants to go
+  # (a_target > 0, the lead moved off) releases it; after standstill the hold ramp to stopAccel applies unchanged.
+  # END_REQUEST -0.50 (~1080 N by BF ~ 480 + 1200 * |req|) is the lightest request that still decelerates under ACC:
+  # on this route 760-1000 N gave only -0.2..-0.25 m/s^2 (the pedal is not pressed, so the hybrid keeps its creep
+  # torque), while -0.63..-0.78 gave -0.6..-1.07 at 3 km/h. The driver's template ends at 0.5-0.7 m/s^2.
+  # Driver 2026-10-03 afternoon, after the -0.50 / -0.40 / -0.35 flat ends: "from 5 km/h to 0 use a formula: -0.5 at
+  # 5 km/h, -0.3 at 0 km/h, interpolated - firmer at 5 km/h, then easing as the car comes to rest". So the line is
+  # now piecewise linear through (BLEND_V, a_entry) -> (END_V_HI, END_HI) -> (0, END_LO); a_entry is at least END_HI.
+  BLEND_V = 10.0 / 3.6  # m/s: the line starts here, at the request the car had at that moment
+  END_V_HI = 5.0 / 3.6  # m/s
+  END_HI = -0.65  # m/s^2 at END_V_HI (~1200 N); -0.60 -> -0.65 2026-10-08, see END_CURVE_A
+  END_LO = -0.30  # m/s^2 at 0-1 km/h; driver 2026-10-04: -0.30 "closer to perfect", -0.25 the threshold; 2026-10-06 back to -0.30
+  # Driver 2026-10-04 (route 000000f2): "between 5 and 0 km/h not linear but parabolic: steeper from 5 to 2, flat from
+  # 2 to 0" -> x^2 parabola -0.50/-0.41/-0.34/-0.29/-0.26/-0.25 ("closer to perfect"). An S-shaped table
+  # (-0.50/-0.47/-0.40/-0.29/-0.25/-0.24) was "not better" and reverted. Then the driver gave his own points, close to
+  # the parabola but a touch lighter in the middle: 4 km/h -0.40, 3 -0.33, 2 -0.27, 1 -0.25, 0 -0.25. Held as a table,
+  # linearly interpolated at the actual speed every 10 ms (continuous curve).
+  END_CURVE_V = [0.0, 1.0 / 3.6, 2.0 / 3.6, 3.0 / 3.6, 4.0 / 3.6, 5.0 / 3.6]  # m/s
+  # Driver 2026-10-06 (route 00000100, seven manual stops 19:45-19:59, averages 5->2 km/h 0.83, 2->0.3 km/h 0.45 m/s^2;
+  # the lightest steady one, 19:56:19, 0.56 / 0.37): "the manual stops are still a touch heavy; give 5-2 km/h a little
+  # more, but already be light at 1 km/h, or reaching -0.25 only at 0 km/h may still nod; keep 1-0 as it was". So a
+  # parabola with its flat top at 5 km/h (END_HI) that bends down to END_LO by 1 km/h, then flat END_LO to the stop:
+  # a = END_LO + (END_HI - END_LO) * (1 - ((5 - v) / 4)^2) -> -0.60/-0.58/-0.51/-0.40/-0.25/-0.25 at 5..0 km/h.
+  # Driver 2026-10-06 night: "the end of the stop is not ideal, make 1-0 -0.3": END_LO -0.30, same parabola above it
+  # -> -0.60/-0.58/-0.53/-0.43/-0.30/-0.30.
+  # Driver 2026-10-08 (routes 00000112-00000116): "just before the stop, around 4-2 km/h, the brake feels released a little
+  # too much". Since the low-speed PID hold (c14c0dbae1) the integrator no longer adds its catch-up there, and the car
+  # delivered -0.44 / -0.55 / -0.49 / -0.45 at 5-4 / 4-3 / 3-2 / 2-1 km/h (BRAKE 0xA6 870-1010 N) against ~-0.7 / -0.7 /
+  # -0.6 before. Driver chose the table 5 km/h -0.65, 4 -0.65, 3 -0.60, 2 -0.48, 1-0 -0.30 unchanged (expected delivered
+  # ~-0.61 / -0.55 / -0.48 at 4-3 / 3-2 / 2-1 km/h). Was -0.60/-0.58/-0.53/-0.43/-0.30/-0.30.
+  END_CURVE_A = [END_LO, END_LO, -0.48, -0.60, -0.65, END_HI]  # m/s^2
+  END_PLAN_MIN = -0.25  # m/s^2: the plan must be braking this much at BLEND_V to count as stopping (a crawl-follow hovers near 0)
+  END_RATE = 2.0  # m/s^3: how fast the request may move toward the line
+
+  # Leaving the standstill hold (driver 2026-10-04, route 000000fa 14:43:32: "the brake release at launch is a jerk, as
+  # if let go suddenly; make it linear and slow"). There the request went from the -2.0 hold (3960 N) to +0.7 in 0.5 s
+  # at the Toyota windup limit: the PCM bled 3960 -> 0 N in 0.6 s while the gas request was already +0.6, and the car
+  # lurched at +1.7 m/s^2 for the first 0.1 s. The brake pressure cannot move the car while the request is well below
+  # zero, so the release keeps the fast rate down there and goes LINEAR AND SLOW only through the hand-over band where
+  # the clamp falls off and the creep torque takes over: RELEASE_FAST up to RELEASE_BAND_LO, RELEASE_SLOW from there
+  # up to RELEASE_BAND_HI, then the plan again. The first motion comes ~0.1 s later than before, the lurch is gone.
+  RELEASE_FAST = 4.0  # m/s^3 (the stock windup limit)
+  RELEASE_SLOW = 1.5  # m/s^3
+  RELEASE_BAND_LO = -0.6  # m/s^2
+  RELEASE_BAND_HI = 0.4  # m/s^2
+
+  # The tnpb2 additions below run on every car.
+  # B - re-roll at a standstill. Route 0000010e 17:38:48: the car stood behind the lead at the
+  # end request -0.30 (BRAKE 0xA6 640 N, less than the ~760-800 N the hybrid's creep torque needs); 0.9 s later, still
+  # inside the 1.0 s hold delay, all four wheel speeds read 0.8 km/h for 1.1 s (the pulse counter did not step: < 0.2 m)
+  # while the creep path raised the request only at 0.3 -> 1.0 m/s^2/s; it stood again once the force reached ~1300 N
+  # (-0.8). Now, once the car moves again after it has stood (creeping), the request steps down to REROLL_ACCEL at
+  # REROLL_JERK (0.05 s from -0.30), then continues on the creep / hold path. A car that stays still is not affected.
+  REROLL_ACCEL = -0.6  # m/s^2 (~1100 N or more)
+  REROLL_JERK = 6.0  # m/s^3
+  # C - lighter standstill hold on the flat. The launch from a standstill takes 2.1 s
+  # (median, routes fc..10e) from the lead moving to the wheels moving, against 1.7 s before 2026-10-04; most of the
+  # wait is the PCM bleeding the -2.0 hold (3240 N, BRAKE 0xA6) - 1.1 s from 3240 N to 0 on route 0000010d 23:56:58.
+  # On the flat the hold now stops at FLAT_HOLD_ACCEL (~2000 N, still 2.5x the creep torque and above the 1360 N the
+  # auto brake hold uses on the flat); on a slope (|pitch - FLAT_PITCH_DEG| >= SLOPE_PITCH_DEG, as in auto_brake_hold)
+  # or without a pitch, or while the car creeps, the stock stopAccel applies.
+  FLAT_HOLD_ACCEL = -1.2  # m/s^2
+  FLAT_PITCH_DEG = 1.0  # deg, what this car reports standing on the flat
+  SLOPE_PITCH_DEG = 2.0  # deg away from flat that counts as a slope
+  # H - creep-follow stop. Owner 2026-10-07: "at a standstill the lead moves a little, we
+  # follow and stop again - still not soft; release smoothly, roll forward, then stop smoothly with -0.24". Route
+  # 0000010d 23:56:59: after the release the plan turned to -0.4 within 0.2 s, but the command stayed positive for
+  # ~0.7 s (soft-onset shaper + a PID integral left from the launch), the end-of-stop line asked -0.48 at 2.5 km/h, and
+  # the brake bit 0 -> 1640 N in 0.4 s (aEgo +0.36 -> -0.88). Now, when the car has left a standstill less than
+  # CF_WINDOW ago and has not gone faster than CF_V_MAX, a stop asked by the plan (a_target <= CF_TRIGGER) is made with
+  # CF_END to the wheel stop: any positive request is dropped at once, the braking side is entered at CF_RATE (below the
+  # onset shaper's threshold, so it passes unshaped), and the end-of-stop line is not used. The plan still wins when it
+  # needs more: from CF_PLAN_SOFT the request blends to the plan, which it follows below CF_PLAN_FIRM (a lead that
+  # brakes, or the gap closing because -0.24 is too little). A plan that wants to go (a_target > 0) ends it.
+  CF_WINDOW = 10.0  # s
+  CF_V_MAX = 8.0 / 3.6  # m/s
+  CF_TRIGGER = -0.15  # m/s^2
+  CF_END = -0.24  # m/s^2
+  CF_PLAN_SOFT = -0.45  # m/s^2
+  CF_PLAN_FIRM = -0.70  # m/s^2
+  CF_RATE = 0.9  # m/s^3
+  # ... and once the wheels stop after such a creep-follow stop (owner 2026-10-07, route 00000112 23:16:57: "stopped
+  # with -0.24, then the hold came as a second brake"; there the request sat at -0.24 (~650 N, less than the creep
+  # torque) for the 1.0 s hold delay, then ramped to -1.2 within ~1 s - two separate pushes. Owner: "if it stops with
+  # -0.24, just add a little brake after the stop"): the brake is added straight away and smoothly, CF_SETTLE_ACCEL
+  # within ~0.5 s (CF_SETTLE_JERK), enough to hold the creep torque, then the rest of the hold very slowly
+  # (CF_HOLD_RATE) - one continuous motion, no hold delay. A re-roll still goes through B.
+  CF_SETTLE_ACCEL = -0.6  # m/s^2 (~1100 N)
+  CF_SETTLE_JERK = 0.72  # m/s^3: -0.24 -> -0.6 in 0.5 s
+  CF_HOLD_RATE = 0.3  # m/s^2/s: -0.6 -> -1.2 in 2 s
+
+  def __init__(self, stop_accel):
+    self.stop_accel = stop_accel
+    self.stop_confirm = StopConfirm()
+    self.cf_t: float | None = None  # time since leaving a standstill (creep-follow window)
+    self.cf_vmax = 0.0
+    self.cf_stopping = False
+    self.cf_stopped = False  # the wheels stopped at the end of a creep-follow stop
+    self.standstill_t = 0.0
+    self.stopping_t = 0.0
+    self.go_t = 0.0
+    self.engaged_t = 0.0
+    self.stopped_once = False
+    self.creep_t = 0.0
+    self.end_active = False
+    self.a_entry = self.END_HI
+    self.releasing = False
+
+  def _blend(self, v_ego):
+    if v_ego < self.END_V_HI:
+      return float(np.interp(max(v_ego, 0.0), self.END_CURVE_V, self.END_CURVE_A))
+    return float(np.interp(v_ego, [self.END_V_HI, self.BLEND_V], [self.END_HI, self.a_entry]))
+
+  def _end_request(self, a_target, prev_accel, v_ego):
+    target = min(a_target, self._blend(v_ego))  # the firmer of the plan and the line
+    step = self.END_RATE * DT_CTRL
+    return float(np.clip(target, prev_accel - step, prev_accel + step))
+
+  def _hold_floor(self, pitch, creeping):
+    if creeping or pitch is None or not math.isfinite(pitch):
+      return self.stop_accel
+    if abs(math.degrees(pitch) - self.FLAT_PITCH_DEG) >= self.SLOPE_PITCH_DEG:
+      return self.stop_accel
+    return max(self.stop_accel, self.FLAT_HOLD_ACCEL)
+
+  def _cf_request(self, a_target, prev_accel):
+    target = float(np.interp(a_target, [self.CF_PLAN_FIRM, self.CF_PLAN_SOFT], [self.CF_PLAN_FIRM, self.CF_END]))
+    target = min(target, a_target) if a_target < self.CF_PLAN_FIRM else target
+    if target < prev_accel:
+      return max(target, min(prev_accel, 0.0) - self.CF_RATE * DT_CTRL)
+    return min(target, prev_accel + self.END_RATE * DT_CTRL)
+
+  def _update_creep_follow(self, state, CS, a_target):
+    if state == LongCtrlState.off:
+      self.cf_t, self.cf_stopping, self.cf_stopped = None, False, False
+      return False
+    if CS.standstill:
+      if self.cf_stopping:
+        self.cf_stopped = True
+      self.cf_t = 0.0 if self.standstill_t > 0.5 else self.cf_t
+      self.cf_vmax, self.cf_stopping = 0.0, False
+      return False
+    self.cf_stopped = False
+    if self.cf_t is None:
+      return False
+    self.cf_t += DT_CTRL
+    self.cf_vmax = max(self.cf_vmax, CS.vEgo)
+    if self.cf_t > self.CF_WINDOW or self.cf_vmax >= self.CF_V_MAX or a_target > 0.0 and self.cf_stopping:
+      self.cf_t, self.cf_stopping = None, False
+      return False
+    if a_target <= self.CF_TRIGGER:
+      self.cf_stopping = True
+    return self.cf_stopping
+
+  def update(self, prev_state, state, CS, a_target, prev_accel, stock_accel, accel_limits, has_lead=False, pitch=None,
+             a_long=None):
+    if prev_state == LongCtrlState.stopping and state == LongCtrlState.pid and CS.standstill:
+      self.go_t += DT_CTRL
+      if self.go_t < self.STOPPING_EXIT_DEBOUNCE:
+        state = LongCtrlState.stopping
+    else:
+      self.go_t = 0.0
+
+    self.standstill_t = self.standstill_t + DT_CTRL if CS.standstill else 0.0
+    self.engaged_t = 0.0 if state == LongCtrlState.off else self.engaged_t + DT_CTRL
+    self.stopping_t = self.stopping_t + DT_CTRL if state == LongCtrlState.stopping else 0.0
+    if state != LongCtrlState.stopping:
+      self.stopped_once = False
+    elif CS.standstill:
+      self.stopped_once = True
+
+    creeping = self.stopped_once and not CS.standstill and CS.vEgo > self.CREEP_V_MIN
+    self.creep_t = self.creep_t + DT_CTRL if creeping else 0.0
+    cf_stop = self._update_creep_follow(state, CS, a_target)
+    stop_confirmed = self.stop_confirm.update(CS.standstill, CS.vEgo, a_long)
+
+    # end-of-stop window: rolling below BLEND_V with a plan that is braking to a stop
+    rolling = not CS.standstill and not self.stopped_once
+    if state == LongCtrlState.off or not rolling or CS.vEgo >= self.BLEND_V or a_target > 0.0:
+      self.end_active = False
+    elif not self.end_active and a_target <= self.END_PLAN_MIN:
+      self.end_active = True
+      self.a_entry = min(prev_accel, self.END_HI)  # the line starts where the request is now, at least END_HI
+
+    if state != LongCtrlState.stopping:
+      if cf_stop:
+        return state, float(np.clip(self._cf_request(a_target, prev_accel), accel_limits[0], accel_limits[1]))
+      if self.end_active:
+        return state, float(np.clip(self._end_request(a_target, prev_accel, CS.vEgo), accel_limits[0], accel_limits[1]))
+      # coming off the hold: linear, slow through the hand-over band (see RELEASE_*)
+      if prev_state == LongCtrlState.stopping and state == LongCtrlState.pid and prev_accel < self.RELEASE_BAND_HI:
+        self.releasing = True
+      if self.releasing:
+        if stock_accel <= prev_accel or prev_accel >= self.RELEASE_BAND_HI or state == LongCtrlState.off:
+          self.releasing = False
+          return state, stock_accel
+        rate = self.RELEASE_FAST if prev_accel < self.RELEASE_BAND_LO else self.RELEASE_SLOW
+        return state, float(min(stock_accel, prev_accel + rate * DT_CTRL))
+      return state, stock_accel
+
+    output_accel = prev_accel
+    hold_floor = self._hold_floor(pitch, creeping)
+    if output_accel > hold_floor:
+      output_accel = min(output_accel, 0.0)
+      if not CS.standstill and not self.stopped_once and a_target < self.STOPPING_FOLLOW_MIN and a_target > output_accel:
+        output_accel = min(a_target, output_accel + self.STOPPING_FOLLOW_RATE * DT_CTRL)
+      if cf_stop:
+        output_accel = self._cf_request(a_target, prev_accel)
+      elif self.end_active:
+        # still rolling inside the end window: the line (or the plan where it is firmer), never the eased-off curve
+        output_accel = self._end_request(a_target, prev_accel, CS.vEgo)
+
+      hold_delay = self.STANDSTILL_HOLD_DELAY_LEAD if has_lead else self.STANDSTILL_HOLD_DELAY_NO_LEAD
+      if CS.standstill and not has_lead and self.engaged_t < self.ENGAGE_STANDSTILL_GRACE:
+        rate = 0.0  # just engaged at a standstill: no brake pulse before the launch
+      elif self.cf_stopped and CS.standstill and output_accel > self.CF_SETTLE_ACCEL:
+        output_accel = max(self.CF_SETTLE_ACCEL, output_accel - self.CF_SETTLE_JERK * DT_CTRL)  # H: a little brake now
+        rate = 0.0
+      elif self.cf_stopped and CS.standstill:
+        rate = self.CF_HOLD_RATE  # H: then the rest of the hold, slowly
+      elif self.standstill_t >= hold_delay or stop_confirmed:
+        rate = self.STANDSTILL_HOLD_RATE
+      elif creeping and output_accel > self.REROLL_ACCEL:
+        output_accel = max(self.REROLL_ACCEL, output_accel - self.REROLL_JERK * DT_CTRL)  # B: moving again - hold now
+        rate = 0.0
+      elif creeping:
+        rate = min(self.STOPPING_DECEL_RATE + self.CREEP_RATE_GROWTH * self.creep_t, self.CREEP_RATE_MAX)
+      elif self.stopping_t >= self.STOPPING_FREEZE_MAX:
+        rate = self.STOPPING_DECEL_RATE
+      else:
+        rate = 0.0
+      if hold_floor != self.stop_accel:
+        output_accel = max(output_accel - rate * DT_CTRL, min(hold_floor, output_accel))  # C: stop at the flat hold
+      else:
+        output_accel -= rate * DT_CTRL
+
+    return state, float(np.clip(output_accel, accel_limits[0], accel_limits[1]))
@@ -0,0 +1,314 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+import math
+from types import SimpleNamespace
+
+from openpilot.common.realtime import DT_CTRL
+from openpilot.common.test import OpenpilotTestCase
+from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
+from openpilot.sunnypilot.selfdrive.controls.lib.stopping_controller import StoppingController
+
+LIMITS = (-3.5, 2.0)
+PID, STOPPING, OFF = LongCtrlState.pid, LongCtrlState.stopping, LongCtrlState.off
+END_LO, END_HI = StoppingController.END_LO, StoppingController.END_HI
+
+
+def cs(v_kph, standstill=False):
+  return SimpleNamespace(vEgo=v_kph / 3.6, standstill=standstill)
+
+
+def run(sc, state, car, a_target, prev, secs, stock=None):
+  """step the controller for secs with a fixed plan; returns the last output"""
+  out = prev
+  for _ in range(int(secs / DT_CTRL)):
+    _, out = sc.update(state, state, car, a_target, out, a_target if stock is None else stock, LIMITS, has_lead=True)
+  return out
+
+
+def blend(sc, v_kph):
+  return sc._blend(v_kph / 3.6)
+
+
+class TestEndOfStop(OpenpilotTestCase):
+  def test_the_line_runs_from_the_entry_request_through_5kph_to_the_rest_value(self):
+    sc = StoppingController(-2.0)
+    out = run(sc, PID, cs(9.9), -1.20, -1.20, 0.2)
+    assert sc.end_active and abs(sc.a_entry - (-1.20)) < 1e-6
+    # 7.5 km/h: halfway between the entry request and END_HI; the planner has eased to -0.30, the line is sent
+    out = run(sc, PID, cs(7.5), -0.30, out, 0.6)
+    assert abs(blend(sc, 7.5) - (-1.20 + (END_HI - -1.20) * 0.5)) < 1e-6
+    assert abs(out - blend(sc, 7.5)) < 1e-6
+    # 5 km/h: END_HI; below it a parabola: steep first, flat toward the rest value; the wheel stop: END_LO
+    out = run(sc, PID, cs(5.0), -0.20, out, 0.4)
+    assert abs(out - END_HI) < 1e-6
+    out = run(sc, PID, cs(2.5), -0.20, out, 0.4)
+    assert abs(out - (-0.48 + -0.60) / 2) < 1e-6
+    assert abs(blend(sc, 4.0) - (-0.65)) < 1e-6 and abs(blend(sc, 1.0) - (-0.30)) < 1e-6
+    assert abs(blend(sc, 0.5) - (-0.30)) < 1e-6   # 1-0 km/h unchanged
+    out = run(sc, PID, cs(0.0), -0.20, out, 0.4)
+    assert abs(out - END_LO) < 1e-6
+
+  def test_the_line_is_a_floor_a_firmer_plan_still_passes(self):
+    sc = StoppingController(-2.0)
+    out = run(sc, PID, cs(9.9), -1.20, -1.20, 0.2)
+    out = run(sc, PID, cs(5.0), -1.10, out, 0.5)
+    assert abs(out - (-1.10)) < 1e-6
+    out = run(sc, PID, cs(2.5), -1.8, out, 0.6)
+    assert abs(out - (-1.8)) < 1e-6
+
+  def test_a_light_plan_at_entry_starts_the_line_at_end_hi(self):
+    sc = StoppingController(-2.0)
+    light = END_HI + 0.1
+    out = run(sc, PID, cs(7.0), light, light, 0.5)
+    assert abs(sc.a_entry - END_HI) < 1e-6
+    assert abs(out - END_HI) < 1e-6
+    # the planner's own curve eases off below 5 km/h: the request follows the line, never the plan
+    out = run(sc, PID, cs(2.5), -0.10, out, 0.5)
+    assert abs(out - blend(sc, 2.5)) < 1e-6
+
+  def test_entry_is_rate_limited(self):
+    sc = StoppingController(-2.0)
+    light = END_HI + 0.2
+    _, out = sc.update(PID, PID, cs(7.0), light, light, light, LIMITS, has_lead=True)
+    assert out > END_HI
+    assert abs(out - (light - StoppingController.END_RATE * DT_CTRL)) < 1e-6
+
+  def test_an_emergency_plan_still_passes(self):
+    sc = StoppingController(-2.0)
+    out = run(sc, PID, cs(2.5), -1.8, -0.40, 0.8)
+    assert abs(out - (-1.8)) < 1e-6
+
+  def test_crawl_follow_is_untouched(self):
+    sc = StoppingController(-2.0)
+    out = run(sc, PID, cs(2.0), -0.10, -0.10, 0.5)
+    assert abs(out - (-0.10)) < 1e-6
+    assert not sc.end_active
+
+  def test_above_blend_v_nothing_changes(self):
+    sc = StoppingController(-2.0)
+    out = run(sc, PID, cs(12.0), -0.40, -0.40, 0.5)
+    assert abs(out - (-0.40)) < 1e-6
+    assert not sc.end_active
+
+  def test_lead_moving_off_releases_the_hold(self):
+    sc = StoppingController(-2.0)
+    run(sc, PID, cs(2.0), -0.60, -0.60, 0.5)
+    assert sc.end_active
+    _, out = sc.update(PID, PID, cs(1.5), 0.50, END_LO, 0.50, LIMITS, has_lead=True)
+    assert not sc.end_active and abs(out - 0.50) < 1e-6
+
+  def test_stopping_state_keeps_the_line_until_the_wheels_stop_then_holds(self):
+    sc = StoppingController(-1.0)
+    run(sc, PID, cs(2.0), END_HI, END_HI, 0.5)
+    # 1 km/h, stopping state, the planner's curve has eased to -0.10: Kumar's lighter-follow would ease the request
+    out = run(sc, STOPPING, cs(1.0), -0.10, blend(sc, 2.0), 0.4)
+    assert abs(out - blend(sc, 1.0)) < 1e-6
+    # wheels stop: the last value of the line is held until the hold delay, then ramps to stopAccel at the hold rate
+    out_at_stop = run(sc, STOPPING, cs(0.0, standstill=True), -0.10, out, 0.9)
+    assert abs(out_at_stop - out) < 1e-6 and END_HI < out_at_stop <= END_LO + 1e-6
+    out_held = run(sc, STOPPING, cs(0.0, standstill=True), -0.10, out_at_stop, 1.0)
+    assert out_held < out_at_stop - 0.3
+    assert out_held >= -1.0 - 1e-6
+
+
+class TestHoldRelease(OpenpilotTestCase):
+  """2026-10-04 14:43: leaving the hold is fast while the request is far below zero, then linear and slow through the
+  hand-over band, and the plan is never exceeded."""
+
+  def test_release_is_slow_through_the_handover_band(self):
+    sc = StoppingController(-2.0)
+    out = -2.0
+    t = 0.0
+    t_lo = t_hi = None
+    state = STOPPING  # controlsd feeds the returned state back as prev_state (the 0.2 s exit debounce lives there)
+    for _ in range(300):
+      state, out = sc.update(state, PID, cs(0.0, standstill=True), 0.8, out, 0.8, LIMITS, has_lead=True)
+      t += DT_CTRL
+      if t_lo is None and out >= StoppingController.RELEASE_BAND_LO:
+        t_lo = t
+      if t_hi is None and out >= StoppingController.RELEASE_BAND_HI:
+        t_hi = t
+      assert out <= 0.8 + 1e-9
+    assert t_lo is not None and t_lo < 0.6           # 0.2 s debounce, then -2.0 -> -0.6 at the fast rate (0.35 s)
+    assert t_hi is not None and 1.15 < t_hi < 1.3    # then -0.6 -> +0.4 at 1.5 m/s^3 (0.67 s)
+    assert abs(out - 0.8) < 1e-9 and not sc.releasing
+
+  def test_a_plan_that_drops_again_ends_the_release(self):
+    sc = StoppingController(-2.0)
+    state, out = STOPPING, -2.0
+    for _ in range(30):  # through the exit debounce
+      state, out = sc.update(state, PID, cs(0.0, standstill=True), 0.5, out, 0.5, LIMITS, has_lead=True)
+    assert sc.releasing and -2.0 < out < 0.5
+    _, out = sc.update(state, PID, cs(0.0, standstill=True), -2.0, out, -2.0, LIMITS, has_lead=True)
+    assert abs(out - (-2.0)) < 1e-9 and not sc.releasing
+
+
+class TestEngageAtStandstill(OpenpilotTestCase):
+  def _engage(self, has_lead, secs):
+    sc = StoppingController(-2.0)
+    car = cs(0.0, standstill=True)
+    for _ in range(300):  # standing in the auto hold, openpilot not engaged
+      sc.update(OFF, OFF, car, 0.0, 0.0, 0.0, LIMITS, has_lead=has_lead)
+    out = 0.0
+    for _ in range(int(secs / DT_CTRL)):
+      _, out = sc.update(STOPPING, STOPPING, car, 0.0, out, 0.0, LIMITS, has_lead=has_lead)
+    return out, sc
+
+  def test_no_brake_pulse_right_after_engaging_without_lead(self):
+    # route 0000010a 23:58:49: the hold ramp reached -0.21 in 0.2 s and the PCM braked 1600 N before the launch
+    out, sc = self._engage(False, 0.5)
+    assert out == 0.0
+
+  def test_the_hold_ramp_follows_after_the_grace(self):
+    out, sc = self._engage(False, StoppingController.ENGAGE_STANDSTILL_GRACE + 0.3)
+    assert out < -0.2
+
+  def test_with_a_lead_the_hold_starts_at_once(self):
+    out, _ = self._engage(True, 0.3)
+    assert out < -0.2
+
+
+def stand(sc, secs, out=StoppingController.END_LO, pitch=None, a_target=-0.1):
+  state = STOPPING
+  for _ in range(int(secs / DT_CTRL)):
+    state, out = sc.update(state, STOPPING, cs(0.0, standstill=True), a_target, out, a_target, LIMITS, has_lead=True, pitch=pitch)
+  return out
+
+
+FLAT = math.radians(1.0)
+SLOPE = math.radians(-4.0)
+
+
+class TestRerollHold(OpenpilotTestCase):
+  """route 0000010e 17:38:48: stood at -0.30, rolled at 0.8 km/h for 1.1 s inside the hold delay"""
+
+  def _reroll(self, sc):
+    out = stand(sc, 0.9)
+    state = STOPPING
+    outs = []
+    for _ in range(20):
+      state, out = sc.update(state, STOPPING, cs(0.8), -0.05, out, -0.05, LIMITS, has_lead=True)
+      outs.append(out)
+    return outs
+
+  def test_steps_to_the_reroll_request_within_0_1s(self):
+    outs = self._reroll(StoppingController(-2.0))
+    assert outs[4] <= StoppingController.REROLL_ACCEL + 1e-9   # 0.05 s
+    assert all(b - a >= -StoppingController.REROLL_JERK * DT_CTRL - 1e-9 for a, b in zip(outs, outs[1:], strict=False))
+
+
+class TestFlatHold(OpenpilotTestCase):
+  def test_flat_hold_stops_at_minus_1_2(self):
+    out = stand(StoppingController(-2.0), 4.0, pitch=FLAT)
+    assert abs(out - StoppingController.FLAT_HOLD_ACCEL) < 1e-9
+
+  def test_slope_or_no_pitch_keeps_stop_accel(self):
+    assert stand(StoppingController(-2.0), 4.0, pitch=SLOPE) <= -2.0 + 1e-6
+    assert stand(StoppingController(-2.0), 4.0, pitch=None) <= -2.0 + 1e-6
+
+  def test_a_creep_on_the_flat_hold_goes_deeper(self):
+    sc = StoppingController(-2.0)
+    out = stand(sc, 4.0, pitch=FLAT)
+    state = STOPPING
+    for _ in range(150):
+      state, out = sc.update(state, STOPPING, cs(0.8), -0.1, out, -0.1, LIMITS, has_lead=True, pitch=FLAT)
+    assert out < StoppingController.FLAT_HOLD_ACCEL - 0.5
+
+
+class TestCreepFollowStop(OpenpilotTestCase):
+  """route 0000010d 23:56:59: lead moved ~2 m, the car followed at 2.5 km/h and the brake bit at -0.88"""
+
+  def _creep(self, sc, plan, v_kph=2.0, secs=2.0, out=0.4):
+    stand(sc, 2.0)
+    outs = []
+    for _ in range(int(secs / DT_CTRL)):
+      _, out = sc.update(PID, PID, cs(v_kph), plan, out, plan, LIMITS, has_lead=True)
+      outs.append(out)
+    return outs
+
+  def test_stops_with_minus_0_24_not_the_end_line(self):
+    outs = self._creep(StoppingController(-2.0), -0.40)
+    assert outs[0] <= 0.0                       # a positive request is dropped at once
+    assert abs(outs[-1] - StoppingController.CF_END) < 1e-9
+    steps = [a - b for a, b in zip(outs, outs[1:], strict=False)]
+    assert max(steps) <= StoppingController.CF_RATE * DT_CTRL + 1e-9
+
+  def test_a_firm_plan_still_wins(self):
+    outs = self._creep(StoppingController(-2.0), -1.0)
+    assert abs(outs[-1] - (-1.0)) < 1e-9
+
+  def test_light_plan_keeps_minus_0_24_to_the_stop(self):
+    sc = StoppingController(-2.0)
+    outs = self._creep(sc, -0.40, secs=1.0)
+    _, out = sc.update(PID, PID, cs(1.0), -0.10, outs[-1], -0.10, LIMITS, has_lead=True)
+    assert abs(out - StoppingController.CF_END) < 1e-9
+
+  def test_not_after_a_real_launch(self):
+    sc = StoppingController(-2.0)
+    stand(sc, 2.0)
+    out = 0.5
+    for _ in range(300):
+      _, out = sc.update(PID, PID, cs(15.0), 0.3, out, 0.3, LIMITS, has_lead=True)
+    _, out = sc.update(PID, PID, cs(6.0), -0.40, out, -0.40, LIMITS, has_lead=True)
+    assert sc.cf_t is None
+
+  def test_lead_moving_off_ends_it(self):
+    sc = StoppingController(-2.0)
+    outs = self._creep(sc, -0.40, secs=0.5)
+    _, out = sc.update(PID, PID, cs(2.0), 0.5, outs[-1], 0.5, LIMITS, has_lead=True)
+    assert out == 0.5 and not sc.cf_stopping
+
+  def test_after_the_creep_stop_a_little_brake_at_once_then_the_hold_slowly(self):
+    """route 00000112 23:16:57: -0.24 for the 1.0 s hold delay, then -1.2 within ~1 s felt like a second brake"""
+    sc = StoppingController(-2.0)
+    outs = self._creep(sc, -0.40)
+    out, state, trace = outs[-1], STOPPING, []
+    for _ in range(300):
+      state, out = sc.update(state, STOPPING, cs(0.0, standstill=True), -0.05, out, -0.05, LIMITS, has_lead=True,
+                             pitch=FLAT)
+      trace.append(out)
+    assert trace[50] <= StoppingController.CF_SETTLE_ACCEL + 1e-9          # -0.6 within 0.5 s, no hold delay
+    assert trace[10] > StoppingController.CF_SETTLE_ACCEL                    # but gradually
+    assert abs(trace[150] - (StoppingController.CF_SETTLE_ACCEL - 1.0 * StoppingController.CF_HOLD_RATE)) < 0.02
+    assert abs(trace[-1] - StoppingController.FLAT_HOLD_ACCEL) < 1e-9       # the flat hold after ~2.5 s
+    steps = [a - b for a, b in zip(trace, trace[1:], strict=False)]
+    assert max(steps) <= StoppingController.CF_SETTLE_JERK * DT_CTRL + 1e-9
+
+  def test_an_ordinary_stop_keeps_the_hold_delay(self):
+    sc = StoppingController(-2.0)
+    out = stand(sc, 0.9, pitch=FLAT)
+    assert abs(out - END_LO) < 1e-9
+
+
+class TestConfirmedStopEarlyHold(OpenpilotTestCase):
+  """hold 0.2 s after a CONFIRMED stop (standstill flag + the body's deceleration collapsing), else the 1.0 s delay"""
+
+  def _stop(self, sc, collapse):
+    out, state = -0.40, PID
+    for k in range(100):  # rolling to the stop at -0.45 m/s^2 (body)
+      state, out = sc.update(state, STOPPING, cs(2.0 - 0.019 * k), -0.3, out, -0.3, LIMITS, has_lead=True, pitch=FLAT, a_long=-0.45)
+    trace = []
+    for k in range(150):  # standing: the body pitches back (deceleration collapses) after 0.1 s, or not
+      a = -0.05 if (collapse and k >= 10) else -0.45
+      state, out = sc.update(state, STOPPING, cs(0.0, True), -0.05, out, -0.05, LIMITS, has_lead=True, pitch=FLAT, a_long=a)
+      trace.append(out)
+    return trace
+
+  def test_confirmed_stop_holds_after_0_2s(self):
+    sc = StoppingController(-2.0)
+    tr = self._stop(sc, True)
+    assert tr[25] == tr[0]                 # 0.1 s to the pitch-back + 0.2 s: nothing yet at 0.25 s
+    assert tr[40] < tr[0] - 0.05           # holding by 0.4 s
+
+  def test_no_confirmation_keeps_the_1s_delay(self):
+    sc = StoppingController(-2.0)
+    tr = self._stop(sc, False)
+    assert tr[90] == tr[0] and tr[110] < tr[0]
+
+  def test_no_pose_keeps_the_1s_delay(self):
+    sc = StoppingController(-2.0)
+    assert not sc.stop_confirm.update(True, 0.0, None)
@@ -10,29 +10,29 @@ void live_update_32(double *in_x, double *in_P, double *in_z, double *in_R, doub
 void live_update_13(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void live_update_14(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void live_update_33(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
-void live_H(double *in_vec, double *out_8536236811475430067);
-void live_err_fun(double *nom_x, double *delta_x, double *out_4614868058485758139);
-void live_inv_err_fun(double *nom_x, double *true_x, double *out_8573554928028298681);
-void live_H_mod_fun(double *state, double *out_6128852447133223565);
-void live_f_fun(double *state, double dt, double *out_7195534889995166041);
-void live_F_fun(double *state, double dt, double *out_9012508306389018670);
-void live_h_4(double *state, double *unused, double *out_2657657745771735915);
-void live_H_4(double *state, double *unused, double *out_1457735078816209632);
-void live_h_9(double *state, double *unused, double *out_181525824636861445);
-void live_H_9(double *state, double *unused, double *out_8744954014080657102);
-void live_h_10(double *state, double *unused, double *out_2940864223365932608);
-void live_H_10(double *state, double *unused, double *out_6591157835936482265);
-void live_h_12(double *state, double *unused, double *out_830811219903448800);
-void live_H_12(double *state, double *unused, double *out_4923523298226523364);
-void live_h_35(double *state, double *unused, double *out_1177115113523305926);
-void live_H_35(double *state, double *unused, double *out_2177960265901509655);
-void live_h_32(double *state, double *unused, double *out_8970203849717307468);
-void live_H_32(double *state, double *unused, double *out_5493453269239321848);
-void live_h_13(double *state, double *unused, double *out_1337448462981849436);
-void live_H_13(double *state, double *unused, double *out_2400089242629978824);
-void live_h_14(double *state, double *unused, double *out_181525824636861445);
-void live_H_14(double *state, double *unused, double *out_8744954014080657102);
-void live_h_33(double *state, double *unused, double *out_3078698583527751553);
-void live_H_33(double *state, double *unused, double *out_972596738737347949);
+void live_H(double *in_vec, double *out_5926322025614992030);
+void live_err_fun(double *nom_x, double *delta_x, double *out_5043251176595415602);
+void live_inv_err_fun(double *nom_x, double *true_x, double *out_3360739843129809304);
+void live_H_mod_fun(double *state, double *out_8279386211598339514);
+void live_f_fun(double *state, double dt, double *out_6890687714462639744);
+void live_F_fun(double *state, double dt, double *out_7621965816561569981);
+void live_h_4(double *state, double *unused, double *out_3851734767229128263);
+void live_H_4(double *state, double *unused, double *out_5878769767090540838);
+void live_h_9(double *state, double *unused, double *out_2694197172040937469);
+void live_H_9(double *state, double *unused, double *out_1408449168173906632);
+void live_h_10(double *state, double *unused, double *out_7033125878671447195);
+void live_H_10(double *state, double *unused, double *out_65675330875969869);
+void live_h_12(double *state, double *unused, double *out_8799002316774637565);
+void live_H_12(double *state, double *unused, double *out_6186715929576277782);
+void live_h_35(double *state, double *unused, double *out_2868906334315872882);
+void live_H_35(double *state, double *unused, double *out_8932278961901291491);
+void live_h_32(double *state, double *unused, double *out_9199462926327450350);
+void live_H_32(double *state, double *unused, double *out_6730806947389842532);
+void live_h_13(double *state, double *unused, double *out_3826583407417889778);
+void live_H_13(double *state, double *unused, double *out_3672088104010170074);
+void live_h_14(double *state, double *unused, double *out_2694197172040937469);
+void live_H_14(double *state, double *unused, double *out_1408449168173906632);
+void live_h_33(double *state, double *unused, double *out_6802426037572342548);
+void live_H_33(double *state, double *unused, double *out_7684478583555780967);
 void live_predict(double *in_x, double *in_P, double *in_Q, double dt);
 }
@@ -0,0 +1,433 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+
+from collections import deque
+from collections.abc import Callable
+from dataclasses import asdict, dataclass
+import math
+import time
+from typing import Any
+
+import numpy as np
+
+from openpilot.cereal import log, messaging
+from opendbc.car.interfaces import ACCEL_MAX, ACCEL_MIN
+from openpilot.common.realtime import DT_CTRL, DT_MDL, Ratekeeper
+from openpilot.selfdrive.modeld.constants import ModelConstants
+from openpilot.selfdrive.controls.lib.longcontrol import LongControl, LongCtrlState
+from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
+from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
+from openpilot.selfdrive.test.longitudinal_maneuvers.plant import Plant, PlannerSM
+
+
+LeadObservation = dict[str, Any]
+LeadObservationFn = Callable[[float, str, LeadObservation], LeadObservation | None]
+ModelActionFn = Callable[[float, float, float], tuple[float, bool]]
+EgoObservationFn = Callable[[float, float, float], tuple[float, float]]
+ModelPlanFn = Callable[[float, float, float], list[float]]
+ModelMetaFn = Callable[[float], tuple[list[float], bool, float]]
+LeadFutureProbsFn = Callable[[float], tuple[float, float, float]]
+PositionYFn = Callable[[float], list[float]]
+ExperimentalModeFn = Callable[[float], bool]
+
+
+@dataclass(frozen=True)
+class ActuatorModel:
+  planner_delay: float
+  transport_delay: float
+  actuator_lag: float
+  command_rate_limit: float
+  stopping_acceleration: float
+  standstill_breakaway_acceleration: float
+  standstill_breakaway_time: float
+
+  def __post_init__(self):
+    nonnegative_fields = {
+      "planner_delay": self.planner_delay,
+      "transport_delay": self.transport_delay,
+      "actuator_lag": self.actuator_lag,
+      "standstill_breakaway_acceleration": self.standstill_breakaway_acceleration,
+      "standstill_breakaway_time": self.standstill_breakaway_time,
+    }
+    if any(not math.isfinite(value) or value < 0.0 for value in nonnegative_fields.values()):
+      raise ValueError(f"ActuatorModel fields must be finite and non-negative: {nonnegative_fields}")
+    if not math.isfinite(self.command_rate_limit) or self.command_rate_limit <= 0.0:
+      raise ValueError("command_rate_limit must be finite and positive")
+    if not math.isfinite(self.stopping_acceleration) or self.stopping_acceleration > 0.0:
+      raise ValueError("stopping_acceleration must be finite and non-positive")
+
+
+# Conservative Prius TSS2 actuator model.
+PRIUS_TSS2_ROUTE_MODEL = ActuatorModel(
+  planner_delay=0.05,
+  transport_delay=0.0,
+  actuator_lag=0.20,
+  command_rate_limit=4.0,
+  stopping_acceleration=-2.0,
+  standstill_breakaway_acceleration=1.0,
+  standstill_breakaway_time=0.05,
+)
+
+
+class PlantSP(Plant):
+  """Closed-loop plant with configurable observations and actuator response."""
+
+  def __init__(
+    self,
+    lead_relevancy=False,
+    speed=0.0,
+    distance_lead=2.0,
+    enabled=True,
+    only_lead2=False,
+    only_radar=False,
+    e2e=False,
+    personality=0,
+    force_decel=False,
+    lead_observation_fn: LeadObservationFn | None = None,
+    model_action_fn: ModelActionFn | None = None,
+    ego_observation_fn: EgoObservationFn | None = None,
+    model_plan_fn: ModelPlanFn | None = None,
+    model_meta_fn: ModelMetaFn | None = None,
+    lead_future_probs_fn: LeadFutureProbsFn | None = None,
+    position_y_fn: PositionYFn | None = None,
+    experimental_mode_fn: ExperimentalModeFn | None = None,
+    actuator_delay: float | None = None,
+    actuator_lag: float = 0.0,
+    actuator_model: ActuatorModel | None = None,
+    run_long_control: bool = False,
+  ):
+    if actuator_delay is not None and (not math.isfinite(actuator_delay) or actuator_delay < 0.0):
+      raise ValueError("actuator_delay must be finite and non-negative")
+    if not math.isfinite(actuator_lag) or actuator_lag < 0.0:
+      raise ValueError("actuator_lag must be finite and non-negative")
+
+    self.rate = 1.0 / DT_MDL
+
+    if not Plant.messaging_initialized:
+      Plant.radar = messaging.pub_sock('radarState')
+      Plant.controls_state = messaging.pub_sock('controlsState')
+      Plant.selfdrive_state = messaging.pub_sock('selfdriveState')
+      Plant.car_state = messaging.pub_sock('carState')
+      Plant.plan = messaging.sub_sock('longitudinalPlan')
+      Plant.messaging_initialized = True
+
+    self.v_lead_prev = 0.0
+
+    self.distance = 0.0
+    self.speed = speed
+    self.should_stop = False
+    self.acceleration = 0.0
+    self.a_target = 0.0
+    self.actuator_command = 0.0
+    self.applied_actuator_command = 0.0
+    self.breakaway_confirmed = False
+    self._breakaway_timer = 0.0
+
+    # lead car
+    self.lead_relevancy = lead_relevancy
+    self.distance_lead = distance_lead
+    self.enabled = enabled
+    self.only_lead2 = only_lead2
+    self.only_radar = only_radar
+    self.e2e = e2e
+    self.personality = personality
+    self.force_decel = force_decel
+    self.lead_observation_fn = lead_observation_fn
+    self.model_action_fn = model_action_fn
+    self.ego_observation_fn = ego_observation_fn
+    self.model_plan_fn = model_plan_fn
+    self.model_meta_fn = model_meta_fn
+    self.lead_future_probs_fn = lead_future_probs_fn
+    self.position_y_fn = position_y_fn
+    self.experimental_mode_fn = experimental_mode_fn
+    self.actuator_model = actuator_model
+    self.actuator_delay = actuator_model.planner_delay if actuator_model is not None else actuator_delay
+    self.transport_delay = actuator_model.transport_delay if actuator_model is not None else actuator_delay
+    self.actuator_lag = actuator_model.actuator_lag if actuator_model is not None else actuator_lag
+    self.publish_realized_a_ego = any((lead_observation_fn is not None, model_action_fn is not None, ego_observation_fn is not None,
+                                      actuator_delay is not None, actuator_lag > 0.0, actuator_model is not None, run_long_control))
+
+    self.rk = Ratekeeper(self.rate, print_delay_threshold=100.0)
+    self.ts = 1.0 / self.rate
+    time.sleep(0.1)
+    self.sm = messaging.SubMaster(['longitudinalPlan'])
+
+    from opendbc.car.honda.values import CAR
+    from opendbc.car.honda.interface import CarInterface
+
+    CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+    if self.actuator_delay is not None:
+      CP.longitudinalActuatorDelay = self.actuator_delay
+    CP_SP = CarInterface.get_non_essential_params_sp(CP, CAR.HONDA_CIVIC)
+    self.planner = LongitudinalPlanner(CP, CP_SP, init_v=self.speed)
+    self.long_control = LongControl(CP, CP_SP) if run_long_control else None
+
+    if self.actuator_model is not None and self.speed >= 0.01:
+      self.breakaway_confirmed = True
+    self.integration_dt = DT_CTRL if run_long_control else self.ts
+    delay_steps = 0 if self.transport_delay is None else round(self.transport_delay / self.integration_dt)
+    self._actuator_delay_queue = deque([self.acceleration] * delay_steps)
+
+  @staticmethod
+  def _lead_message(observation: LeadObservation):
+    lead = log.RadarState.LeadData.new_message()
+    for field, value in observation.items():
+      setattr(lead, field, value)
+    return lead
+
+  def _observe_lead(self, lead_name: str, truth: LeadObservation, present_by_default: bool) -> LeadObservation | None:
+    if self.lead_observation_fn is None:
+      return dict(truth) if present_by_default else None
+
+    observed = self.lead_observation_fn(self.current_time, lead_name, dict(truth))
+    if observed is None:
+      return None
+
+    complete_observation = dict(truth)
+    complete_observation.update(observed)
+    return complete_observation
+
+  def _update_actuator(self, command: float) -> tuple[float, float]:
+    if self._actuator_delay_queue:
+      self._actuator_delay_queue.append(command)
+      delayed_command = self._actuator_delay_queue.popleft()
+    else:
+      delayed_command = command
+
+    if self.actuator_model is not None:
+      max_command_delta = self.actuator_model.command_rate_limit * self.integration_dt
+      self.applied_actuator_command = float(np.clip(delayed_command,
+                                                   self.applied_actuator_command - max_command_delta,
+                                                   self.applied_actuator_command + max_command_delta))
+
+      if self.speed < 0.01:
+        if self.applied_actuator_command <= 0.0:
+          self.breakaway_confirmed = False
+          self._breakaway_timer = 0.0
+        elif not self.breakaway_confirmed:
+          breakaway_ready = self.applied_actuator_command + 1e-9 >= self.actuator_model.standstill_breakaway_acceleration
+          if breakaway_ready:
+            self._breakaway_timer += self.integration_dt
+          else:
+            self._breakaway_timer = 0.0
+
+          self.breakaway_confirmed = breakaway_ready and self._breakaway_timer + 1e-9 >= self.actuator_model.standstill_breakaway_time
+        if not self.breakaway_confirmed:
+          self.acceleration = 0.0
+          return delayed_command, self.acceleration
+      else:
+        self.breakaway_confirmed = True
+
+      response_command = self.applied_actuator_command
+    else:
+      self.applied_actuator_command = delayed_command
+      response_command = delayed_command
+
+    if self.actuator_lag > 0.0:
+      alpha = 1.0 - math.exp(-self.integration_dt / self.actuator_lag)
+      self.acceleration += alpha * (response_command - self.acceleration)
+    else:
+      self.acceleration = response_command
+    return delayed_command, self.acceleration
+
+  def _integrate_ego(self, dt: float, stop_at_standstill: bool = False) -> None:
+    self.speed += self.acceleration * dt
+    if self.speed <= 0.0 or stop_at_standstill and self.speed < 0.01 and self.actuator_command <= 0.0:
+      self.speed = self.acceleration = 0.0
+    self.distance += self.speed * dt
+
+  def step(self, v_lead=0.0, prob_lead=1.0, v_cruise=50.0, pitch=0.0, prob_throttle=1.0):
+    # ******** publish a fake model going straight and fake calibration ********
+    # note that this is worst case for MPC, since model will delay long mpc by one time step
+    radar = messaging.new_message('radarState')
+    control = messaging.new_message('controlsState')
+    ss = messaging.new_message('selfdriveState')
+    car_state = messaging.new_message('carState')
+    vehicle_parameters = messaging.new_message('vehicleParameters')
+    car_control = messaging.new_message('carControl')
+    model = messaging.new_message('modelV2')
+    car_state_sp = messaging.new_message('carStateSP')
+    live_map_data_sp = messaging.new_message('liveMapDataSP')
+    gps_data = messaging.new_message('gpsLocation')
+    a_lead = (v_lead - self.v_lead_prev) / self.ts
+    self.v_lead_prev = v_lead
+
+    if self.lead_relevancy:
+      d_rel = np.maximum(0.0, self.distance_lead - self.distance)
+      v_rel = v_lead - self.speed
+      if self.only_radar:
+        status = True
+      elif prob_lead > 0.5:
+        status = True
+      else:
+        status = False
+    else:
+      d_rel = 200.0
+      v_rel = 0.0
+      prob_lead = 0.0
+      status = False
+
+    truth_lead: LeadObservation = {
+      "dRel": float(d_rel),
+      "yRel": 0.0,
+      "vRel": float(v_rel),
+      "vLead": float(v_lead),
+      "vLeadK": float(v_lead),
+      "aLeadK": float(a_lead),
+      "present": bool(status),
+      # TODO use real radard logic for this
+      "aLeadTau": float(_LEAD_ACCEL_TAU),
+      "modelProb": float(prob_lead),
+      "radar": bool(self.only_radar),
+      "radarTrackId": -1,
+    }
+    lead_one_observation = self._observe_lead("leadOne", truth_lead, not self.only_lead2)
+    lead_two_observation = self._observe_lead("leadTwo", truth_lead, True)
+    if lead_one_observation is not None:
+      radar.radarState.leadOne = self._lead_message(lead_one_observation)
+    if lead_two_observation is not None:
+      radar.radarState.leadTwo = self._lead_message(lead_two_observation)
+
+    # Simulate model predicting slightly faster speed
+    # this is to ensure lead policy is effective when model
+    # does not predict slowdown in e2e mode
+    position = log.XYZTData.new_message()
+    position.x = [float(x) for x in (self.speed + 0.5) * np.array(ModelConstants.T_IDXS)]
+    if self.position_y_fn is None:
+      position.y = [0.0] * len(ModelConstants.T_IDXS)
+    else:
+      position.y = [float(y) for y in self.position_y_fn(self.current_time)]
+    model.modelV2.position = position
+    if self.model_action_fn is None:
+      model_acceleration, model_should_stop = self.acceleration + 0.5, False
+    else:
+      model_acceleration, model_should_stop = self.model_action_fn(self.current_time, self.speed, self.acceleration)
+    model.modelV2.action.desiredAcceleration = float(model_acceleration)
+    model.modelV2.action.shouldStop = bool(model_should_stop)
+    velocity = log.XYZTData.new_message()
+    if self.model_plan_fn is None:
+      velocity_plan = [float(x) for x in (self.speed + 0.5) * np.ones_like(ModelConstants.T_IDXS)]
+      velocity_plan[0] = float(self.speed)  # always start at current speed
+    else:
+      velocity_plan = [float(x) for x in self.model_plan_fn(self.current_time, self.speed, self.acceleration)]
+    velocity.x = velocity_plan
+    model.modelV2.velocity = velocity
+    acceleration = log.XYZTData.new_message()
+    acceleration.x = [float(x) for x in np.zeros_like(ModelConstants.T_IDXS)]
+    model.modelV2.acceleration = acceleration
+    model.modelV2.meta.disengagePredictions.gasPressProbs = [float(prob_throttle) for _ in range(6)]
+    if self.model_meta_fn is None:
+      brake3_probs, hard_brake_predicted, frame_drop_perc = [0.0] * 5, False, 0.0
+    else:
+      brake3_probs, hard_brake_predicted, frame_drop_perc = self.model_meta_fn(self.current_time)
+    model.modelV2.meta.disengagePredictions.brake3MetersPerSecondSquaredProbs = [float(p) for p in brake3_probs]
+    model.modelV2.meta.hardBrakePredicted = bool(hard_brake_predicted)
+    model.modelV2.frameDropPerc = float(frame_drop_perc)
+    if self.lead_future_probs_fn is not None:
+      model.modelV2.init('leadsV3', 3)
+      lead_future_probs = self.lead_future_probs_fn(self.current_time)
+      for i, (prob, prob_time) in enumerate(zip(lead_future_probs, (0.0, 2.0, 4.0), strict=True)):
+        model.modelV2.leadsV3[i].prob = float(prob)
+        model.modelV2.leadsV3[i].probTime = prob_time
+
+    control.controlsState.longControlState = self.long_control.long_control_state if self.long_control is not None else (
+      LongCtrlState.pid if self.enabled else LongCtrlState.off)
+    ss.selfdriveState.experimentalMode = self.e2e if self.experimental_mode_fn is None else bool(self.experimental_mode_fn(self.current_time))
+    ss.selfdriveState.personality = self.personality
+    control.controlsState.forceDecel = self.force_decel
+    true_v_ego = self.speed
+    true_a_ego = self.acceleration
+    published_v_ego = true_v_ego
+    published_a_ego = true_a_ego if self.publish_realized_a_ego else 0.0
+    if self.ego_observation_fn is not None:
+      published_v_ego, published_a_ego = self.ego_observation_fn(self.current_time, true_v_ego, true_a_ego)
+    car_state.carState.vEgo = float(published_v_ego)
+    car_state.carState.aEgo = float(published_a_ego)
+    car_state.carState.standstill = bool(self.speed < 0.01)
+    car_state.carState.vCruise = float(v_cruise * 3.6)
+    car_control.carControl.orientationNED = [0.0, float(pitch), 0.0]
+
+    # ******** get controlsState messages for plotting ***
+    sm = PlannerSM(self.rk.frame, {
+      'radarState': radar.radarState,
+      'carState': car_state.carState,
+      'carControl': car_control.carControl,
+      'controlsState': control.controlsState,
+      'selfdriveState': ss.selfdriveState,
+      'vehicleParameters': vehicle_parameters.vehicleParameters,
+      'modelV2': model.modelV2,
+      'carStateSP': car_state_sp.carStateSP,
+      'liveMapDataSP': live_map_data_sp.liveMapDataSP,
+      'gpsLocation': gps_data.gpsLocation,
+    })
+    self.planner.update(sm)
+    self.a_target = self.planner.output_a_target
+    if self.long_control is None:
+      self.actuator_command = self.a_target
+      if self.planner.output_should_stop:
+        stopping_acceleration = -0.5 if self.actuator_model is None else self.actuator_model.stopping_acceleration
+        self.actuator_command = min(stopping_acceleration, self.actuator_command)
+      self._update_actuator(self.actuator_command)
+      self._integrate_ego(self.ts)
+    else:
+      for _ in range(round(self.ts / DT_CTRL)):
+        car_state.carState.vEgo = self.speed
+        car_state.carState.aEgo = self.acceleration
+        car_state.carState.standstill = self.speed < 0.01
+        self.actuator_command = self.long_control.update(
+          self.enabled, car_state.carState, self.a_target, self.planner.output_should_stop, (ACCEL_MIN, ACCEL_MAX),
+        )
+        self._update_actuator(self.actuator_command)
+        self._integrate_ego(DT_CTRL, stop_at_standstill=True)
+    self.should_stop = self.planner.output_should_stop
+    fcw = self.planner.fcw
+    self.distance_lead = self.distance_lead + v_lead * self.ts
+
+    # *** radar model ***
+    if self.lead_relevancy:
+      d_rel = np.maximum(0.0, self.distance_lead - self.distance)
+      v_rel = v_lead - self.speed
+    else:
+      d_rel = 200.0
+      v_rel = 0.0
+
+    # print at 5hz
+    # if (self.rk.frame % (self.rate // 5)) == 0:
+    #   print("%2.2f sec   %6.2f m  %6.2f m/s  %6.2f m/s2   lead_rel: %6.2f m  %6.2f m/s"
+    #         % (self.current_time, self.distance, self.speed, self.acceleration, d_rel, v_rel))
+
+    # ******** update prevs ********
+    self.rk.monitor_time()
+
+    return {
+      "distance": self.distance,
+      "speed": self.speed,
+      "acceleration": self.acceleration,
+      "realized_acceleration": self.acceleration,
+      "a_target": self.a_target,
+      "actuator_command": self.actuator_command,
+      "published_a_ego": published_a_ego,
+      "published_v_ego": published_v_ego,
+      "should_stop": self.should_stop,
+      "long_control_state": (int(self.long_control.long_control_state) if self.long_control is not None
+                             else control.controlsState.longControlState.raw),
+      "distance_lead": self.distance_lead,
+      "fcw": fcw,
+      "mpc_source": self.planner.mpc.source,
+      "dec_mode": self.planner.dec.mode(),
+      "dec_want_blended": self.planner.dec.want_blended,
+      "dec_signals": asdict(self.planner.dec.signals),
+      "dec_lead_veto": self.planner.dec.lead_veto,
+      "controller_active": self.planner.accel_controller_active,
+      "model_action": {
+        "desiredAcceleration": float(model_acceleration),
+        "shouldStop": bool(model_should_stop),
+      },
+      "truth_lead": dict(truth_lead),
+      "lead_one_observation": None if lead_one_observation is None else dict(lead_one_observation),
+      "lead_two_observation": None if lead_two_observation is None else dict(lead_two_observation),
+    }
@@ -0,0 +1,186 @@
+import numpy as np
+
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+from openpilot.common.test import OpenpilotTestCase
+from openpilot.selfdrive.modeld.constants import ModelConstants
+from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import ENTER_FRAMES, MIN_BLENDED_FRAMES
+from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PlantSP
+
+T_IDXS = np.array(ModelConstants.T_IDXS)
+
+
+def decel_plan(a):
+  def fn(_current_time, speed, _acceleration):
+    return [float(max(0.0, speed + a * t)) for t in T_IDXS]
+  return fn
+
+
+def flat_plan():
+  def fn(_current_time, speed, _acceleration):
+    return [float(speed)] * len(T_IDXS)
+  return fn
+
+
+def alternating_plan(a):
+  def fn(current_time, speed, _acceleration):
+    frame_a = a if round(current_time / DT_MDL) % 2 == 0 else 0.0
+    return [float(max(0.0, speed + frame_a * t)) for t in T_IDXS]
+  return fn
+
+
+def persistent_lead_probs(_current_time):
+  return (1.0, 0.95, 0.9)
+
+
+def _run(plant, steps, v_lead=0.0, v_cruise=50.0):
+  solver_failures = 0
+  original_reset = plant.planner.mpc.reset
+
+  def counting_reset(*args, **kw):
+    nonlocal solver_failures
+    if plant.planner.mpc.solution_status != 0:
+      solver_failures += 1
+    return original_reset(*args, **kw)
+
+  plant.planner.mpc.reset = counting_reset
+  return [plant.step(v_lead=v_lead, v_cruise=v_cruise) for _ in range(steps)], solver_failures
+
+
+def mode_changes(results):
+  modes = [r["dec_mode"] for r in results]
+  return sum(a != b for a, b in zip(modes, modes[1:], strict=False))
+
+
+class TestDecManeuvers(OpenpilotTestCase):
+  def setUp(self):
+    super().setUp()
+    self.params = Params()
+    self.params.put_bool("DynamicExperimentalControl", True, block=True)
+
+  def test_s1_lead_clears_with_underlying_slowdown_blends_quickly(self):
+    clear_t = 1.0
+
+    def lead_obs(current_time, _lead_name, truth):
+      return None if current_time >= clear_t else dict(truth)
+
+    plant = PlantSP(lead_relevancy=True, speed=20.0, distance_lead=40.0, e2e=True, only_radar=True,
+                     lead_observation_fn=lead_obs, model_plan_fn=decel_plan(-2.5),
+                     lead_future_probs_fn=persistent_lead_probs)
+    clear_frame = round(clear_t / DT_MDL)
+    results, _ = _run(plant, steps=clear_frame + ENTER_FRAMES + 5, v_lead=20.0, v_cruise=20.0)
+
+    assert all(r["dec_mode"] == "acc" for r in results[:clear_frame])
+    assert all(r["dec_lead_veto"] for r in results[:clear_frame])
+    post_clear = [r["dec_mode"] for r in results[clear_frame:clear_frame + ENTER_FRAMES + 2]]
+    assert "blended" in post_clear
+
+  def test_s1b_lead_clears_with_no_underlying_slowdown_stays_acc(self):
+    clear_t = 1.0
+
+    def lead_obs(current_time, _lead_name, truth):
+      return None if current_time >= clear_t else dict(truth)
+
+    plant = PlantSP(lead_relevancy=True, speed=20.0, distance_lead=40.0, e2e=True, only_radar=True,
+                     lead_observation_fn=lead_obs, model_plan_fn=flat_plan(),
+                     lead_future_probs_fn=persistent_lead_probs)
+    clear_frame = round(clear_t / DT_MDL)
+    results, _ = _run(plant, steps=clear_frame + MIN_BLENDED_FRAMES, v_lead=20.0, v_cruise=20.0)
+
+    assert all(r["dec_mode"] == "acc" for r in results)
+
+  def test_s2_steady_highway_following_never_blends(self):
+    v = 80.0 / 3.6
+    plant = PlantSP(lead_relevancy=True, speed=v, distance_lead=40.0, e2e=True, only_radar=True,
+                     model_plan_fn=flat_plan(), lead_future_probs_fn=persistent_lead_probs)
+    results, failures = _run(plant, steps=100, v_lead=v, v_cruise=v)
+
+    assert failures <= 1
+    assert all(r["dec_mode"] == "acc" for r in results)
+
+  def test_s3_low_speed_cruise_no_lead_never_blends(self):
+    v = 15.0 / 3.6
+    plant = PlantSP(lead_relevancy=False, speed=v, e2e=True, model_plan_fn=flat_plan())
+    results, _ = _run(plant, steps=100, v_cruise=v)
+
+    assert all(r["dec_mode"] == "acc" for r in results)
+
+  def test_s4_highway_slowdown_without_lead_blends(self):
+    v0 = 110.0 / 3.6
+    a = (70.0 / 3.6 - v0) / 6.0
+    plant = PlantSP(lead_relevancy=False, speed=v0, e2e=True, model_plan_fn=decel_plan(a))
+    results, _ = _run(plant, steps=10, v_cruise=v0)
+
+    assert any(r["dec_mode"] == "blended" for r in results)
+
+  def test_s5_stop_then_depart_with_lead_present_stays_acc_throughout(self):
+    def departing_lead(current_time):
+      return 0.0 if current_time < 1.0 else min(15.0, 3.0 * (current_time - 1.0))
+
+    plant = PlantSP(lead_relevancy=True, speed=0.0, distance_lead=6.0, e2e=True)
+    results = []
+    solver_failures = 0
+    original_reset = plant.planner.mpc.reset
+
+    def counting_reset(*args, **kw):
+      nonlocal solver_failures
+      if plant.planner.mpc.solution_status != 0:
+        solver_failures += 1
+      return original_reset(*args, **kw)
+
+    plant.planner.mpc.reset = counting_reset
+    for _ in range(200):
+      results.append(plant.step(v_lead=departing_lead(plant.current_time), v_cruise=15.0))
+
+    assert solver_failures <= 1
+    assert all(r["dec_mode"] == "acc" for r in results)
+    assert all(r["dec_lead_veto"] for r in results)
+
+  def test_s6_creep_cycles_behind_lead_stay_acc(self):
+    def creep_cycle_lead(current_time):
+      return 1.5 + 1.5 * np.sin(current_time * 2.0)
+
+    plant = PlantSP(lead_relevancy=True, speed=1.0, distance_lead=8.0, e2e=True, only_radar=True,
+                     model_plan_fn=flat_plan(), lead_future_probs_fn=persistent_lead_probs)
+    results = [plant.step(v_lead=creep_cycle_lead(plant.current_time), v_cruise=5.0) for _ in range(200)]
+
+    assert all(r["dec_mode"] == "acc" for r in results)
+
+  def test_s7_oscillating_near_threshold_demand_does_not_flap(self):
+    plant = PlantSP(lead_relevancy=False, speed=20.0, e2e=True, model_plan_fn=alternating_plan(-2.5))
+    results, _ = _run(plant, steps=200, v_cruise=20.0)
+
+    assert mode_changes(results) <= 2
+
+  def test_s8_degraded_model_holds_acc_through_a_slowdown(self):
+    def degraded_meta(_current_time):
+      return [0.0] * 5, False, 60.0
+
+    plant = PlantSP(lead_relevancy=False, speed=20.0, e2e=True, model_plan_fn=decel_plan(-3.0), model_meta_fn=degraded_meta)
+    results, _ = _run(plant, steps=30, v_cruise=20.0)
+
+    assert all(r["dec_mode"] == "acc" for r in results)
+
+  def test_s9_curve_exclusion_prevents_false_blend_on_a_bend(self):
+    plant = PlantSP(lead_relevancy=False, speed=20.0, e2e=True, model_plan_fn=decel_plan(-1.0),
+                     position_y_fn=lambda _t: [6.0] * len(T_IDXS))
+    results, _ = _run(plant, steps=30, v_cruise=20.0)
+
+    assert all(r["dec_mode"] == "acc" for r in results)
+
+  def test_s11_curve_does_not_interrupt_an_active_hard_stop(self):
+    plant = PlantSP(lead_relevancy=False, speed=20.0, e2e=True, model_plan_fn=decel_plan(-2.5),
+                     position_y_fn=lambda _t: [6.0] * len(T_IDXS))
+    results, _ = _run(plant, steps=10, v_cruise=20.0)
+
+    assert all(r["dec_mode"] == "blended" for r in results[ENTER_FRAMES - 1:])
+
+  def test_s10_hard_brake_override_inert_while_lead_present(self):
+    def hard_brake_meta(_current_time):
+      return [0.0] * 5, True, 0.0
+
+    plant = PlantSP(lead_relevancy=True, speed=20.0, distance_lead=40.0, e2e=True, only_radar=True,
+                     model_plan_fn=flat_plan(), model_meta_fn=hard_brake_meta, lead_future_probs_fn=persistent_lead_probs)
+    results, _ = _run(plant, steps=10, v_lead=20.0, v_cruise=20.0)
+
+    assert all(r["dec_mode"] == "acc" for r in results)
@@ -0,0 +1,164 @@
+from collections.abc import Callable
+import math
+from typing import cast
+
+from openpilot.common.parameterized import parameterized
+from openpilot.common.realtime import DT_MDL
+from openpilot.common.test import OpenpilotTestCase
+from openpilot.selfdrive.test.longitudinal_maneuvers.plant import Plant
+from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PlantSP
+
+STOCK_STEP_KEYS = ("distance", "speed", "acceleration", "should_stop", "distance_lead", "fcw")
+
+
+def departing_lead(current_time: float) -> float:
+  return 0.0 if current_time < 1.0 else min(2.0, 2.0 * (current_time - 1.0))
+
+
+def stopped_lead(_current_time: float) -> float:
+  return 0.0
+
+
+PARITY_SCENARIOS = {
+  "approach_stopped_lead": {"lead_relevancy": True, "speed": 15.0, "distance_lead": 60.0, "v_cruise": 20.0, "v_lead": stopped_lead, "steps": 80},
+  "stop_then_depart": {"lead_relevancy": True, "speed": 0.0, "distance_lead": 6.0, "v_cruise": 8.0, "v_lead": departing_lead, "steps": 120},
+}
+
+
+def _drive(cls, *, v_cruise: float, v_lead: Callable[[float], float], steps: int, **kwargs):
+  plant = cls(**kwargs)
+  plant.v_lead_prev = v_lead(0.0)
+  solver_failures = 0
+  original_reset = plant.planner.mpc.reset
+
+  def counting_reset(*args, **kw):
+    nonlocal solver_failures
+    if plant.planner.mpc.solution_status != 0:
+      solver_failures += 1
+    return original_reset(*args, **kw)
+
+  plant.planner.mpc.reset = counting_reset
+  results = []
+  for _ in range(steps):
+    lead_speed = v_lead(plant.current_time)
+    result = plant.step(v_lead=lead_speed, v_cruise=v_cruise)
+    results.append((result, plant.planner.mpc.source, plant.planner.output_a_target))
+  return results, solver_failures
+
+
+class TestPlantSP(OpenpilotTestCase):
+  @parameterized.expand(PARITY_SCENARIOS, names=("scenario",), ids=lambda scenario: scenario)
+  def test_plant_sp_matches_stock_plant_on_shared_kwargs(self, scenario: str):
+    kwargs = dict(PARITY_SCENARIOS[scenario])
+    v_cruise = cast(float, kwargs.pop("v_cruise"))
+    v_lead = cast(Callable[[float], float], kwargs.pop("v_lead"))
+    steps = cast(int, kwargs.pop("steps"))
+
+    stock_results, stock_failures = _drive(Plant, v_cruise=v_cruise, v_lead=v_lead, steps=steps, **kwargs)
+    sp_results, sp_failures = _drive(PlantSP, v_cruise=v_cruise, v_lead=v_lead, steps=steps, **kwargs)
+
+    assert stock_failures == 0, f"stock Plant solver failed {stock_failures} times in {scenario!r}"
+    assert sp_failures == 0, f"PlantSP solver failed {sp_failures} times in {scenario!r}"
+
+    for frame, ((stock_result, stock_source, stock_a_target), (sp_result, sp_source, sp_a_target)) in enumerate(
+      zip(stock_results, sp_results, strict=True),
+    ):
+      for key in STOCK_STEP_KEYS:
+        if isinstance(stock_result[key], float):
+          self.assertAlmostEqual(sp_result[key], stock_result[key], msg=f"{scenario} frame {frame} key {key}")
+        else:
+          assert sp_result[key] == stock_result[key], f"{scenario} frame {frame} key {key}"
+      assert sp_source == stock_source, f"{scenario} frame {frame} mpc.source"
+      self.assertAlmostEqual(sp_a_target, stock_a_target, msg=f"{scenario} frame {frame} output_a_target")
+
+    if scenario == "stop_then_depart":
+      departure_frame = round(1.0 / DT_MDL)
+      for results in (stock_results, sp_results):
+        assert all(result["speed"] < 0.01 for result, _, _ in results[:departure_frame])
+        assert results[departure_frame - 1][0]["should_stop"]
+        assert any(not result["should_stop"] for result, _, _ in results[departure_frame:])
+        assert any(result["speed"] > 0.05 for result, _, _ in results[departure_frame:])
+      stock_release = next(frame for frame, (result, _, _) in enumerate(stock_results)
+                           if frame >= departure_frame and not result["should_stop"])
+      sp_release = next(frame for frame, (result, _, _) in enumerate(sp_results)
+                        if frame >= departure_frame and not result["should_stop"])
+      stock_motion = next(frame for frame, (result, _, _) in enumerate(stock_results)
+                          if frame >= departure_frame and result["speed"] > 0.05)
+      sp_motion = next(frame for frame, (result, _, _) in enumerate(sp_results)
+                       if frame >= departure_frame and result["speed"] > 0.05)
+      assert sp_release == stock_release
+      assert sp_motion == stock_motion
+
+  def test_full_lead_observation_is_independent_from_truth(self):
+    callback_inputs = []
+
+    def observe_lead(current_time, lead_name, truth):
+      callback_inputs.append((current_time, lead_name, truth))
+      if lead_name == "leadOne":
+        return {
+          "dRel": 12.5,
+          "vRel": -4.0,
+          "vLead": 6.0,
+          "vLeadK": 5.5,
+          "aLeadK": -1.25,
+          "aLeadTau": 0.7,
+          "present": True,
+          "modelProb": 0.9,
+          "radarTrackId": 42,
+        }
+      return None
+
+    plant = PlantSP(lead_relevancy=True, speed=10.0, distance_lead=50.0, lead_observation_fn=observe_lead)
+    result = plant.step(v_lead=8.0)
+
+    assert [entry[1] for entry in callback_inputs] == ["leadOne", "leadTwo"]
+    self.assertAlmostEqual(callback_inputs[0][2]["dRel"], 50.0)
+    self.assertAlmostEqual(result["truth_lead"]["dRel"], 50.0)
+    self.assertAlmostEqual(result["lead_one_observation"]["dRel"], 12.5)
+    assert result["lead_one_observation"]["radarTrackId"] == 42
+    assert result["lead_two_observation"] is None
+    self.assertAlmostEqual(result["distance_lead"], 50.0 + 8.0 * DT_MDL)
+
+  def test_model_action_realized_acceleration_and_source_logging(self):
+    def model_action(current_time, v_ego, a_ego):
+      return -1.25, True
+
+    plant = PlantSP(speed=10.0, e2e=True, force_decel=True, model_action_fn=model_action, actuator_lag=0.5)
+    first = plant.step()
+    second = plant.step()
+
+    assert first["model_action"] == {"desiredAcceleration": -1.25, "shouldStop": True}
+    self.assertAlmostEqual(first["published_a_ego"], 0.0)
+    self.assertAlmostEqual(second["published_a_ego"], first["realized_acceleration"])
+    assert first["acceleration"] == first["realized_acceleration"]
+    assert abs(first["realized_acceleration"]) < abs(first["actuator_command"])
+    assert first["mpc_source"] is not None
+    assert first["dec_mode"] in ("acc", "blended")
+    assert "controller_active" in first
+    assert first["lead_one_observation"] is not None
+    assert first["truth_lead"] == first["lead_one_observation"]
+
+  def test_default_model_action_matches_stock_plant(self):
+    result = PlantSP(speed=10.0).step()
+
+    self.assertAlmostEqual(result["model_action"]["desiredAcceleration"], 0.5)
+    assert not result["model_action"]["shouldStop"]
+
+  def test_configurable_transport_delay_and_first_order_lag(self):
+    plant = PlantSP(speed=10.0, actuator_delay=2 * DT_MDL, actuator_lag=0.2)
+
+    self.assertAlmostEqual(plant.planner.CP.longitudinalActuatorDelay, 2 * DT_MDL)
+    delayed_commands = [plant._update_actuator(-1.0) for _ in range(3)]
+    assert [command for command, _ in delayed_commands[:2]] == [0.0, 0.0]
+
+    expected_acceleration = -(1.0 - math.exp(-DT_MDL / 0.2))
+    assert delayed_commands[2][0] == -1.0
+    self.assertAlmostEqual(delayed_commands[2][1], expected_acceleration)
+
+  @parameterized.expand(
+    [(-0.1, 0.0), (float("nan"), 0.0), (float("inf"), 0.0), (None, -0.1), (None, float("nan")), (None, float("inf"))],
+    names=("delay", "lag"),
+  )
+  def test_invalid_actuator_dynamics(self, delay, lag):
+    with self.assertRaises(ValueError):
+      PlantSP(actuator_delay=delay, actuator_lag=lag)
@@ -652,6 +652,53 @@
                }
              ]
            },
+            {
+              "key": "AccelPersonalityEnabled",
+              "widget": "toggle",
+              "title": "Enable Accel Controller",
+              "description": "Lets you choose how sunnypilot starts, catches up, and settles at the cruise speed. Emergency braking and stopping are unchanged.",
+              "visibility": [
+                {
+                  "type": "capability",
+                  "field": "has_longitudinal_control",
+                  "equals": true
+                }
+              ],
+              "enablement": [
+                {
+                  "type": "capability",
+                  "field": "has_longitudinal_control",
+                  "equals": true
+                }
+              ]
+            },
+            {
+              "key": "AccelPersonality",
+              "widget": "multiple_button",
+              "title": "Acceleration Profile",
+              "description": "Eco is gentlest, Normal balances a prompt start with smooth catch-up, and Sport is more responsive.",
+              "options": [
+                {
+                  "value": 0,
+                  "label": "Eco"
+                },
+                {
+                  "value": 1,
+                  "label": "Normal"
+                },
+                {
+                  "value": 2,
+                  "label": "Sport"
+                }
+              ],
+              "enablement": [
+                {
+                  "type": "capability",
+                  "field": "has_longitudinal_control",
+                  "equals": true
+                }
+              ]
+            },
            {
              "key": "IntelligentCruiseButtonManagement",
              "widget": "toggle",
@@ -1605,6 +1652,18 @@
              "widget": "toggle",
              "title": "Use Metric System",
              "description": "Display speed in km/h instead of mph."
+            },
+            {
+              "key": "SunnypilotStoppingController",
+              "widget": "toggle",
+              "needs_onroad_cycle": true,
+              "title": "Stopping Controller",
+              "description": "Take over the end of a stop. The request follows the speed down to the wheel stop, then holds the car at a standstill.",
+              "enablement": [
+                {
+                  "type": "offroad_only"
+                }
+              ]
            }
          ]
        },
@@ -2302,6 +2361,61 @@
      "title": "Toyota / Lexus Settings",
      "description": "",
      "items": [
+        {
+          "key": "ToyotaAutoHold",
+          "widget": "toggle",
+          "needs_onroad_cycle": true,
+          "title": "Toyota: Auto Brake Hold FOR TSS2 HYBRID CARS",
+          "enablement": [
+            {
+              "type": "not_engaged"
+            }
+          ]
+        },
+        {
+          "key": "ToyotaBrakeOnset",
+          "widget": "toggle",
+          "needs_onroad_cycle": true,
+          "title": "Toyota: soft brake onset, overshoot limit and regen hand-over",
+          "enablement": [
+            {
+              "type": "not_engaged"
+            }
+          ]
+        },
+        {
+          "key": "ToyotaEnhancedBsm",
+          "widget": "toggle",
+          "needs_onroad_cycle": true,
+          "title": "Toyota: Prius TSS2 BSM and some tssp",
+          "enablement": [
+            {
+              "type": "not_engaged"
+            }
+          ]
+        },
+        {
+          "key": "ToyotaTSS2Long",
+          "widget": "toggle",
+          "needs_onroad_cycle": true,
+          "title": "Toyota: custom longitudinal for TSS2",
+          "enablement": [
+            {
+              "type": "not_engaged"
+            }
+          ]
+        },
+        {
+          "key": "ToyotaDriveMode",
+          "widget": "toggle",
+          "needs_onroad_cycle": true,
+          "title": "Enable drive mode btn link",
+          "enablement": [
+            {
+              "type": "not_engaged"
+            }
+          ]
+        },
        {
          "key": "ToyotaEnforceStockLongitudinal",
          "widget": "toggle",
--- a/Show More
+++ b/Show More