fast

.gitmodules

@@ -4,6 +4,7 @@
 [submodule "opendbc"]
   path = opendbc_repo
   url = https://github.com/sunnypilot/opendbc.git
+  branch = tn
 [submodule "msgq"]
   path = msgq_repo
   url = https://github.com/sunnypilot/msgq.git

openpilot/cereal/custom.capnp

@@ -203,6 +203,7 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
   aTarget @5 :Float32;
   events @6 :List(OnroadEventSP.Event);
   e2eAlerts @7 :E2eAlerts;
+  accelController @8 :AccelController;
 
   struct DynamicExperimentalControl {
     state @0 :DynamicExperimentalControlState;
@@ -305,6 +306,35 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
     greenLightAlert @0 :Bool;
     leadDepartAlert @1 :Bool;
   }
+
+  struct AccelController {
+    enabled @0 :Bool;
+    active @1 :Bool;
+    shadowOnlyDEPRECATED @2 :Bool;
+    profile @3 :Profile;
+    state @4 :State;
+
+    enum Profile {
+      eco @0;
+      normal @1;
+      sport @2;
+    }
+
+    enum State {
+      inactive @0;
+      free @1;
+      restrict @2;
+      hold @3;
+      release @4;
+      stopHold @5;
+    }
+  }
+
+  enum AccelerationPersonality {
+    eco @0;
+    normal @1;
+    sport @2;
+  }
 }
 
 struct OnroadEventSP @0xda96579883444c35 {

openpilot/common/params_keys.h

@@ -186,7 +186,12 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"ShowTurnSignals", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"StandstillTimer", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"TrueVEgoUI", {PERSISTENT | BACKUP, BOOL, "0"}},
-    {"CustomButtonAction", {PERSISTENT | BACKUP, INT, "0"}},
+
+    // toyota specific params
+    {"ToyotaAutoHold", {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"}},
@@ -228,10 +233,15 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"TeslaMadsScreenButton", {PERSISTENT | BACKUP, INT, "0"}},
     {"ToyotaEnforceStockLongitudinal", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"ToyotaStopAndGoHack", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"ToyotaVirtualCruiseSpeed", {PERSISTENT | BACKUP, BOOL, "0"}},
 
     {"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"}},

openpilot/common/tests/test_params.py

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

openpilot/selfdrive/car/card.py

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

openpilot/selfdrive/car/cruise.py

@@ -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:

openpilot/selfdrive/controls/lib/longcontrol.py

@@ -4,6 +4,7 @@ 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]
 
@@ -39,8 +40,9 @@ def long_control_state_trans(CP_SP, active, long_control_state,
 
   return long_control_state
 
-class LongControl:
+class LongControl(LongControlSP):
   def __init__(self, CP, CP_SP):
+    LongControlSP.__init__(self)
     self.CP = CP
     self.CP_SP = CP_SP
     self.long_control_state = LongCtrlState.off
@@ -60,16 +62,17 @@ class LongControl:
     self.long_control_state = long_control_state_trans(self.CP_SP, active, self.long_control_state,
                                                        should_stop, CS.brakePressed,
                                                        CS.cruiseState.standstill)
+    LongControlSP.update_state(self, self.long_control_state == LongCtrlState.stopping, active, CS)
     if self.long_control_state == LongCtrlState.off:
       self.reset()
       output_accel = 0.
 
     elif self.long_control_state == LongCtrlState.stopping:
-      output_accel = self.last_output_accel
+      output_accel = LongControlSP.stopping_accel(self, self.last_output_accel, CS)
       if output_accel > self.CP.stopAccel:
         output_accel = min(output_accel, 0.0)
         # TODO: can we just go straight to stopAccel?
-        output_accel -= 1.0 * DT_CTRL  # m/s^2/s while trying to stop
+        output_accel -= LongControlSP.stopping_decel_rate(self, CS, a_target, output_accel) * DT_CTRL
       self.reset()
 
     else:  # LongCtrlState.pid

openpilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -77,6 +77,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
 
   def update(self, sm):
     LongitudinalPlannerSP.update(self, sm)
+    self.previous_plan_accel = self.output_a_target
 
     if len(sm['carControl'].orientationNED) == 3:
       accel_coast = get_coast_accel(sm['carControl'].orientationNED[1])
@@ -99,7 +100,8 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
 
     throttle_probs = sm['modelV2'].meta.disengagePredictions.gasPressProbs
     throttle_prob = throttle_probs[1] if len(throttle_probs) > 1 else 1.0
-    self.allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD or v_ego <= MIN_ALLOW_THROTTLE_SPEED
+    stock_allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD or v_ego <= MIN_ALLOW_THROTTLE_SPEED
+    self.allow_throttle = stock_allow_throttle
 
     steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['vehicleParameters'].angleOffsetDeg
 
@@ -140,6 +142,10 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     output_should_stop_e2e = sm['modelV2'].action.shouldStop
 
     is_e2e = self.is_e2e(sm)
+    output_a_target_model = self.select_model_accel(
+      output_a_target_mpc, output_a_target_e2e, blended=is_e2e,
+      should_stop=output_should_stop_e2e or output_should_stop_mpc, fcw=self.fcw, reset=reset_state,
+    )
 
     self.a_cruise = get_cruise_accel(is_e2e, v_cruise, v_ego,
                                      self.a_cruise, steer_angle_without_offset, self.CP, self.dt,
@@ -149,8 +155,11 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     candidates = [(output_a_target_mpc, self.mpc.source, output_should_stop_mpc),
                   (self.a_cruise, LongitudinalPlanSource.cruise, cruise_should_stop)]
     if is_e2e:
-      candidates.append((output_a_target_e2e, LongitudinalPlanSource.e2e, output_should_stop_e2e))
+      candidates.append((output_a_target_model, LongitudinalPlanSource.e2e, output_should_stop_e2e))
+    elif self.model_accel_transition.active:
+      candidates[0] = (output_a_target_model, self.mpc.source, output_should_stop_mpc)
 
+    candidates = self.update_accel_controller(sm, candidates)
     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)

openpilot/selfdrive/test/longitudinal_maneuvers/plant.py

@@ -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:

openpilot/selfdrive/ui/layouts/main.py

@@ -3,7 +3,6 @@ from enum import IntEnum
 import openpilot.cereal.messaging as messaging
 from openpilot.system.ui.lib.application import gui_app
 from openpilot.system.ui.widgets import Widget
-from openpilot.selfdrive.ui.sunnypilot.custom_button import CustomButtonAction, handle_custom_button
 from openpilot.selfdrive.ui.layouts.sidebar import Sidebar, SIDEBAR_WIDTH
 from openpilot.selfdrive.ui.layouts.home import HomeLayout
 from openpilot.selfdrive.ui.layouts.settings.settings import SettingsLayout, PanelType
@@ -28,7 +27,6 @@ class MainLayout(Widget):
     super().__init__()
 
     self._pm = messaging.PubMaster(['bookmarkButton', 'userBookmark'])
-    self._custom_button_sock = messaging.sub_sock('carState')
 
     self._sidebar = Sidebar()
     self._current_mode = MainState.HOME
@@ -42,10 +40,6 @@ class MainLayout(Widget):
       MainState.SETTINGS: SettingsLayout(),
       MainState.ONROAD: AugmentedRoadView(),
     }
-    self._custom_button_callbacks = {
-      CustomButtonAction.BOOKMARK: self._on_bookmark_clicked,
-      CustomButtonAction.CYCLE_UI: self._cycle_ui,
-    }
 
     self._sidebar_rect = rl.Rectangle(0, 0, 0, 0)
     self._content_rect = rl.Rectangle(0, 0, 0, 0)
@@ -61,7 +55,6 @@ class MainLayout(Widget):
       gui_app.push_widget(self._onboarding_window)
 
   def _render(self, _):
-    handle_custom_button(messaging.drain_sock(self._custom_button_sock), ui_state.params, self._custom_button_callbacks)
     self._handle_onroad_transition()
     self._render_main_content()
 
@@ -121,18 +114,6 @@ class MainLayout(Widget):
   def _on_settings_clicked(self):
     self.open_settings(PanelType.DEVICE)
 
-  def _show_onroad(self):
-    self._set_current_layout(MainState.ONROAD)
-    self._sidebar.set_visible(False)
-
-  def _cycle_ui(self):
-    if self._current_mode == MainState.ONROAD and not self._sidebar.is_visible:
-      self._sidebar.set_visible(True)
-    elif self._current_mode == MainState.SETTINGS:
-      self._show_onroad()
-    else:
-      self._on_settings_clicked()
-
   def _on_bookmark_clicked(self):
     for service in ('bookmarkButton', 'userBookmark'):
       msg = messaging.new_message(service, valid=True)

openpilot/selfdrive/ui/layouts/settings/toggles.py

@@ -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(
+    "Use the Accel Controller for smooth, early lead following and stop-and-go. Stock emergency braking remains available as a safety backstop."
+  ),
+  "AccelPersonality": tr_noop(
+    "Select the vehicle acceleration response. Chauffeur braking and stopping behavior remain the same across profiles."
+  ),
   "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)

openpilot/selfdrive/ui/mici/layouts/main.py

@@ -4,7 +4,6 @@ from openpilot.selfdrive.ui.mici.layouts.home import MiciHomeLayout
 from openpilot.selfdrive.ui.mici.layouts.settings.settings import SettingsLayout
 from openpilot.selfdrive.ui.mici.layouts.offroad_alerts import MiciOffroadAlerts
 from openpilot.selfdrive.ui.mici.onroad.augmented_road_view import AugmentedRoadView
-from openpilot.selfdrive.ui.sunnypilot.custom_button import CustomButtonAction, handle_custom_button
 from openpilot.selfdrive.ui.ui_state import device, ui_state
 from openpilot.selfdrive.ui.mici.layouts.onboarding import OnboardingWindow
 from openpilot.selfdrive.ui.body.layouts.onroad import BodyLayout
@@ -15,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
 
@@ -24,7 +24,6 @@ class MiciMainLayout(Scroller):
     super().__init__(snap_items=True, spacing=0, pad=0, scroll_indicator=False, edge_shadows=False)
 
     self._pm = messaging.PubMaster(['bookmarkButton', 'userBookmark'])
-    self._custom_button_sock = messaging.sub_sock('carState')
 
     self._prev_onroad = False
     self._prev_standstill = False
@@ -37,10 +36,6 @@ class MiciMainLayout(Scroller):
     self._settings_layout = SettingsLayout()
     self._car_onroad_layout = AugmentedRoadView(bookmark_callback=self._on_bookmark_clicked)
     self._body_onroad_layout = BodyLayout()
-    self._custom_button_callbacks = {
-      CustomButtonAction.BOOKMARK: self._on_bookmark_clicked,
-      CustomButtonAction.CYCLE_UI: self._cycle_ui,
-    }
 
     # Initialize widget rects
     for widget in (self._home_layout, self._alerts_layout, self._settings_layout,
@@ -78,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),
@@ -101,8 +99,6 @@ class MiciMainLayout(Scroller):
     self._alerts_layout._update_state()
 
   def _render(self, _):
-    handle_custom_button(messaging.drain_sock(self._custom_button_sock), ui_state.params, self._custom_button_callbacks)
-
     if not self._setup:
       if self._alerts_layout.active_alerts() > 0:
         self._scroller.scroll_to(self._alerts_layout.rect.x)
@@ -130,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):
@@ -158,23 +156,6 @@ class MiciMainLayout(Scroller):
       msg = messaging.new_message(service, valid=True)
       self._pm.send(service, msg)
 
-  def _show_layout(self, layout: Widget):
-    if gui_app.widget_in_stack(self._onboarding_window):
-      return
-    gui_app.pop_widgets_to(self, lambda: self._scroll_to(layout))
-
-  def _layout_visible(self, layout: Widget) -> bool:
-    return abs(layout.rect.x - self._rect.x) < self._rect.width / 2
-
-  def _cycle_ui(self):
-    if gui_app.widget_in_stack(self._settings_layout):
-      self._show_layout(self._onroad_layout)
-    elif gui_app.get_active_widget() is self and self._layout_visible(self._home_layout):
-      if not gui_app.widget_in_stack(self._onboarding_window):
-        gui_app.push_widget(self._settings_layout)
-    else:
-      self._show_layout(self._home_layout)
-
   def _on_body_changed(self):
     self._car_onroad_layout.set_visible(not ui_state.is_body)
     self._body_onroad_layout.set_visible(bool(ui_state.is_body))

openpilot/selfdrive/ui/mici/layouts/settings/toggles.py

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

openpilot/selfdrive/ui/mici/onroad/model_renderer.py

@@ -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"""

openpilot/selfdrive/ui/mici/widgets/button.py

@@ -385,13 +385,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):

openpilot/selfdrive/ui/sunnypilot/custom_button.py

@@ -1,19 +0,0 @@
-from enum import IntEnum
-
-from opendbc.car.structs import car
-
-
-class CustomButtonAction(IntEnum):
-  NONE = 0
-  BOOKMARK = 1
-  CYCLE_UI = 3
-
-
-def handle_custom_button(messages, params, callbacks):
-  for msg in messages:
-    custom_pressed = any(be.type == car.CarState.ButtonEvent.Type.altButton2 and be.pressed
-                         for be in msg.carState.buttonEvents)
-    if custom_pressed:
-      action = CustomButtonAction(params.get('CustomButtonAction', return_default=True))
-      if callback := callbacks.get(action):
-        callback()

openpilot/selfdrive/ui/sunnypilot/layouts/settings/cruise.py

@@ -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:

openpilot/selfdrive/ui/sunnypilot/layouts/settings/vehicle/brands/toyota.py

@@ -11,10 +11,12 @@ from openpilot.system.ui.lib.multilang import tr, tr_noop
 from openpilot.system.ui.widgets import DialogResult
 from openpilot.system.ui.widgets.confirm_dialog import ConfirmDialog
 from openpilot.system.ui.sunnypilot.widgets.list_view import toggle_item_sp
+from opendbc.sunnypilot.car.toyota.values import ToyotaFlagsSP
 
 
 ONROAD_ONLY_DESCRIPTION = tr_noop("Start the vehicle to check vehicle compatibility.")
 SNG_HACK_UNAVAILABLE = tr_noop("sunnypilot Longitudinal Control must be available and enabled for your vehicle to use this feature.")
+VIRTUAL_CRUISE_UNAVAILABLE = tr_noop("Virtual Cruise Speed is available only on supported Toyota TSS2 configurations with sunnypilot Longitudinal Control.")
 
 DESCRIPTIONS = {
   'enforce_stock_longitudinal': tr_noop(
@@ -23,7 +25,14 @@ 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.'
-  )
+  ),
+  'virtual_cruise_speed': tr_noop(
+    'Use a sunnypilot-owned cruise target with the Toyota RES/SET buttons while sunnypilot longitudinal control is active. ' +
+    'This unlocks Custom ACC Speed Increments; set the short interval to 5 for next-5-unit tap behavior. ' +
+    'The Toyota cluster will continue to show the factory target and may differ from sunnypilot. ' +
+    'The direct button signals are route-validated on Corolla Cross and Prius TSS2, but held-button timing differs by platform. ' +
+    'This is an alpha feature; validate acceleration above the factory target in a controlled setting.'
+  ),
 }
 
 
@@ -47,8 +56,17 @@ class ToyotaSettings(BrandSettings):
       enabled=lambda: not ui_state.engaged,
     )
 
+    self.virtual_cruise_speed = toggle_item_sp(
+      lambda: tr("Virtual Cruise Speed (Alpha)"),
+      description=lambda: tr(DESCRIPTIONS["virtual_cruise_speed"]),
+      initial_state=ui_state.params.get_bool("ToyotaVirtualCruiseSpeed"),
+      callback=self._on_enable_virtual_cruise_speed,
+      enabled=lambda: not ui_state.engaged,
+    )
+
     self.items = [
       self.enforce_stock_longitudinal,
+      self.virtual_cruise_speed,
       self.stop_and_go_hack,
     ]
 
@@ -60,7 +78,9 @@ class ToyotaSettings(BrandSettings):
           if ui_state.params.get_bool("AlphaLongitudinalEnabled"):
             ui_state.params.put_bool("AlphaLongitudinalEnabled", False)
           ui_state.params.put_bool("ToyotaStopAndGoHack", False)
+          ui_state.params.put_bool("ToyotaVirtualCruiseSpeed", False)
           self.stop_and_go_hack.action_item.set_state(False)
+          self.virtual_cruise_speed.action_item.set_state(False)
           ui_state.params.put_bool("OnroadCycleRequested", True)
         else:
           self.enforce_stock_longitudinal.action_item.set_state(False)
@@ -94,10 +114,46 @@ class ToyotaSettings(BrandSettings):
       ui_state.params.put_bool("ToyotaStopAndGoHack", False)
       ui_state.params.put_bool("OnroadCycleRequested", True)
 
+  def _on_enable_virtual_cruise_speed(self, state: bool):
+    if state:
+      def confirm_callback(result: int):
+        enabled = result == DialogResult.CONFIRM
+        ui_state.params.put_bool("ToyotaVirtualCruiseSpeed", enabled)
+        self.virtual_cruise_speed.action_item.set_state(enabled)
+        if enabled:
+          ui_state.params.put_bool("OnroadCycleRequested", True)
+
+      content = (f"<h1>{self.virtual_cruise_speed.title}</h1><br>" +
+                 f"<p>{self.virtual_cruise_speed.description}</p>")
+      dlg = ConfirmDialog(content, tr("Enable"), rich=True, callback=confirm_callback)
+      gui_app.push_widget(dlg)
+    else:
+      ui_state.params.put_bool("ToyotaVirtualCruiseSpeed", False)
+      ui_state.params.put_bool("OnroadCycleRequested", True)
+
   def update_settings(self):
     if ui_state.CP is not None:
       longitudinal = ui_state.CP.openpilotLongitudinalControl
       enforce_stock = self.enforce_stock_longitudinal.action_item.get_state()
+      virtual_cruise_available = bool(ui_state.CP_SP is not None and
+                                      ui_state.CP_SP.flags & ToyotaFlagsSP.VIRTUAL_CRUISE_SPEED_AVAILABLE)
+
+      if longitudinal and virtual_cruise_available:
+        self.virtual_cruise_speed.action_item.set_enabled(not ui_state.engaged)
+        virtual_cruise_desc = tr(DESCRIPTIONS["virtual_cruise_speed"])
+        show_virtual_cruise_desc = False
+      else:
+        self.virtual_cruise_speed.action_item.set_enabled(False)
+        if self.virtual_cruise_speed.action_item.get_state():
+          self.virtual_cruise_speed.action_item.set_state(False)
+          ui_state.params.put_bool("ToyotaVirtualCruiseSpeed", False)
+        virtual_cruise_desc = "<b>" + tr(VIRTUAL_CRUISE_UNAVAILABLE) + "</b>\n\n" + tr(DESCRIPTIONS["virtual_cruise_speed"])
+        show_virtual_cruise_desc = True
+
+      if self.virtual_cruise_speed.description != virtual_cruise_desc:
+        self.virtual_cruise_speed.set_description(virtual_cruise_desc)
+        if show_virtual_cruise_desc:
+          self.virtual_cruise_speed.show_description(True)
 
       if longitudinal and not enforce_stock:
         self.stop_and_go_hack.action_item.set_enabled(not ui_state.engaged)
@@ -114,6 +170,12 @@ class ToyotaSettings(BrandSettings):
         if show_desc:
           self.stop_and_go_hack.show_description(True)
     else:
+      self.virtual_cruise_speed.action_item.set_enabled(False)
+      virtual_cruise_desc = "<b>" + tr(ONROAD_ONLY_DESCRIPTION) + "</b>\n\n" + tr(DESCRIPTIONS["virtual_cruise_speed"])
+      if self.virtual_cruise_speed.description != virtual_cruise_desc:
+        self.virtual_cruise_speed.set_description(virtual_cruise_desc)
+        self.virtual_cruise_speed.show_description(True)
+
       self.stop_and_go_hack.action_item.set_enabled(False)
       new_desc = "<b>" + tr(ONROAD_ONLY_DESCRIPTION) + "</b>\n\n" + tr(DESCRIPTIONS["stop_and_go_hack"])
       if self.stop_and_go_hack.description != new_desc:

openpilot/selfdrive/ui/sunnypilot/mici/layouts/main.py

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

openpilot/selfdrive/ui/sunnypilot/mici/layouts/onroad.py

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

openpilot/selfdrive/ui/sunnypilot/mici/layouts/onroad_info_panel.py

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

openpilot/selfdrive/ui/sunnypilot/mici/onroad/augmented_road_view.py

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

openpilot/selfdrive/ui/sunnypilot/mici/tests/test_scroller_sp.py

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

openpilot/selfdrive/ui/sunnypilot/mici/widgets/scroll_panel_sp.py

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

openpilot/selfdrive/ui/sunnypilot/mici/widgets/scroller_sp.py

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

openpilot/selfdrive/ui/tests/test_custom_button.py

@@ -1,21 +0,0 @@
-from types import SimpleNamespace
-from unittest.mock import Mock
-
-from opendbc.car.structs import car
-
-from openpilot.selfdrive.ui.sunnypilot.custom_button import CustomButtonAction, handle_custom_button
-
-
-def test_custom_button_actions():
-  params = Mock()
-  press = SimpleNamespace(carState=SimpleNamespace(buttonEvents=[SimpleNamespace(
-      type=car.CarState.ButtonEvent.Type.altButton2,
-      pressed=True,
-    )]))
-  messages = [press, SimpleNamespace(carState=SimpleNamespace(buttonEvents=[])), press]
-  callbacks = {action: Mock() for action in CustomButtonAction if action != CustomButtonAction.NONE}
-
-  for action, callback in callbacks.items():
-    params.get.return_value = action
-    handle_custom_button(messages, params, callbacks)
-    assert callback.call_count == 2

openpilot/selfdrive/ui/ui.py

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

openpilot/sunnypilot/selfdrive/car/intelligent_cruise_button_management/controller.py

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

openpilot/sunnypilot/selfdrive/car/interfaces.py

@@ -136,6 +136,10 @@ def initialize_params(params) -> list[dict[str, Any]]:
   keys.extend([
     "ToyotaEnforceStockLongitudinal",
     "ToyotaStopAndGoHack",
+    "ToyotaTSS2Long",
+    "ToyotaEnhancedBsm",
+    "ToyotaAutoHold",
+    "ToyotaVirtualCruiseSpeed",
   ])
 
   return [{k: params.get(k, return_default=True)} for k in keys]

openpilot/sunnypilot/selfdrive/car/tests/test_custom_cruise.py

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

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

@@ -0,0 +1,392 @@
+import math
+from dataclasses import dataclass
+
+import numpy as np
+
+from openpilot.cereal import custom, log
+from openpilot.common.realtime import DT_MDL
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import (
+  ACCEL_V, BRAKE_BUILD_JERK, BRAKE_ONSET_JERK, DECEL_V, DESIRED_STOP_DISTANCE, FOLLOW_HEADWAY, GAP_DEADBAND_METERS,
+  GAP_DEADBAND_SECONDS, LAUNCH_ACCEL, NEUTRAL_ACCEL, PACE_BUFFER_TIME, PACE_GAIN, PACE_MAX_CLOSING_SPEED, PACE_MAX_OPENING_SPEED,
+  PACE_STABILITY_MARGIN, RELEASE_JERK, ROUTINE_DECEL, SPEED_BP, SPEED_RESPONSE_TIME, STOP_MARGIN_BP, STOP_MARGIN_V, TERMINAL_MAX_DECEL,
+  TERMINAL_PREVIEW_TIME, TERMINAL_TIME_CONSTANT, URGENT_BRAKE_JERK, AccelProfile,
+)
+
+
+AccelControllerState = custom.LongitudinalPlanSP.AccelController.State
+
+
+@dataclass(frozen=True)
+class AccelDecision:
+  a_target: float | None = None
+  should_stop: bool = False
+  stock_safety_required: bool = False
+
+
+@dataclass(frozen=True)
+class LeadObservation:
+  index: int
+  distance: float
+  speed: float
+  accel: float
+  track_id: int
+  model_prob: float
+
+
+@dataclass(frozen=True)
+class ProjectedLead:
+  observation: LeadObservation
+  distance: float
+  speed: float
+  relative_speed: float
+  relative_speed_target: float
+  accel: float
+
+
+class AccelController:
+  def __init__(self, CP, dt: float = DT_MDL):
+    if not math.isfinite(dt) or dt <= 0.0:
+      raise ValueError("dt must be finite and positive")
+
+    self.dt = float(dt)
+    self.action_time = float(np.clip(float(CP.longitudinalActuatorDelay) + DT_MDL, DT_MDL, 1.0))
+
+    self._a_command: float | None = None
+    self._last_primary: LeadObservation | None = None
+    self._last_secondary: LeadObservation | None = None
+    self._primary_frames = 0
+    self._secondary_frames = 0
+    self._lead_clear_frames = 0
+    self._braking_latched = False
+    self._radar_faulted = False
+    self._radar_recovery_frames = 0
+
+    self.state = AccelControllerState.inactive
+    self.selected_lead = -1
+
+  @property
+  def is_active(self) -> bool:
+    return self.state != AccelControllerState.inactive
+
+  def reset(self) -> None:
+    self._a_command = None
+    self._last_primary = None
+    self._last_secondary = None
+    self._primary_frames = 0
+    self._secondary_frames = 0
+    self._lead_clear_frames = 0
+    self._braking_latched = False
+    self._radar_faulted = False
+    self._radar_recovery_frames = 0
+    self.state = AccelControllerState.inactive
+    self.selected_lead = -1
+
+  @staticmethod
+  def _read_lead(lead, index: int) -> LeadObservation | None:
+    try:
+      if not bool(lead.present):
+        return None
+      distance = float(lead.dRel)
+      speed = float(lead.vLeadK)
+      if not math.isfinite(distance) or distance < 0.0 or not math.isfinite(speed) or not -1.0 <= speed <= 100.0:
+        return None
+
+      accel = float(lead.aLeadK)
+      accel = float(np.clip(accel, -5.0, 3.0)) if math.isfinite(accel) else 0.0
+      track_id_value = float(lead.radarTrackId)
+      track_id = int(track_id_value) if math.isfinite(track_id_value) else -1
+      model_prob = float(lead.modelProb)
+      model_prob = float(np.clip(model_prob, 0.0, 1.0)) if math.isfinite(model_prob) else 0.0
+    except (AttributeError, TypeError, ValueError):
+      return None
+
+    return LeadObservation(index, distance, max(speed, 0.0), accel, max(track_id, -1), model_prob)
+
+  @staticmethod
+  def _claims_present(lead) -> bool:
+    try:
+      return bool(lead.present)
+    except (AttributeError, TypeError, ValueError):
+      return False
+
+  @staticmethod
+  def _project_lead(lead: LeadObservation, action_time: float) -> tuple[float, float]:
+    accel = min(lead.accel, 0.0)
+    travel_time = min(action_time, -lead.speed / accel) if accel < 0.0 else action_time
+    speed = max(lead.speed + accel * travel_time, 0.0)
+    travel = (lead.speed + speed) * travel_time * 0.5
+    return max(lead.distance + travel, 0.0), speed
+
+  @staticmethod
+  def _same_track(previous: LeadObservation | None, current: LeadObservation) -> bool:
+    if previous is None or previous.index != current.index:
+      return False
+    if previous.track_id >= 0 and current.track_id >= 0:
+      return previous.track_id == current.track_id
+    return abs(current.distance - previous.distance) <= 5.0
+
+  @staticmethod
+  def _same_physical_lead(first: ProjectedLead, second: ProjectedLead) -> bool:
+    first_track = first.observation.track_id
+    second_track = second.observation.track_id
+    if first_track >= 0 and second_track >= 0:
+      return first_track == second_track
+    return abs(first.distance - second.distance) <= 0.5 and abs(first.speed - second.speed) <= 0.5
+
+  def _update_lead_continuity(self, primary: LeadObservation | None, secondary: LeadObservation | None) -> None:
+    if primary is None:
+      self._primary_frames = 0
+      self._lead_clear_frames = self._lead_clear_frames + 1 if self._last_primary is not None else 0
+      if self._lead_clear_frames >= 3:
+        self._last_primary = None
+    elif self._same_track(self._last_primary, primary):
+      self._primary_frames += 1
+      self._lead_clear_frames = 0
+    else:
+      self._primary_frames = 1
+      self._lead_clear_frames = 0
+
+    if secondary is not None and self._same_track(self._last_secondary, secondary):
+      self._secondary_frames += 1
+    else:
+      self._secondary_frames = int(secondary is not None)
+
+  @staticmethod
+  def _pace_target(lead: LeadObservation, ego_distance: float, v_ego: float, headway: float, action_time: float) -> ProjectedLead:
+    lead_distance, lead_speed = AccelController._project_lead(lead, action_time)
+    projected_gap = max(lead_distance - ego_distance, 0.0)
+    desired_gap = DESIRED_STOP_DISTANCE + headway * v_ego
+    gap_error = projected_gap - desired_gap
+    gap_deadband = GAP_DEADBAND_METERS + GAP_DEADBAND_SECONDS * v_ego
+    effective_gap_error = math.copysign(max(abs(gap_error) - gap_deadband, 0.0), gap_error)
+    relative_speed = lead_speed - v_ego
+    relative_speed_target = -float(np.clip(effective_gap_error / PACE_BUFFER_TIME, -PACE_MAX_OPENING_SPEED, PACE_MAX_CLOSING_SPEED))
+    nominal_denominator = headway ** 2 / PACE_BUFFER_TIME + 2.0 * headway
+    pace_gain = max(PACE_GAIN, PACE_STABILITY_MARGIN / nominal_denominator)
+    accel = pace_gain * (relative_speed - relative_speed_target)
+    return ProjectedLead(lead, projected_gap, lead_speed, relative_speed, relative_speed_target, accel)
+
+  @staticmethod
+  def _terminal_target(projected: ProjectedLead, v_ego: float) -> float | None:
+    usable_distance = max(projected.distance - DESIRED_STOP_DISTANCE, 0.0)
+    if v_ego > 0.0 and usable_distance / v_ego > TERMINAL_PREVIEW_TIME:
+      return None
+    brake_tau = ROUTINE_DECEL * TERMINAL_TIME_CONSTANT
+    stop_speed = math.sqrt(brake_tau ** 2 + 2.0 * ROUTINE_DECEL * usable_distance) - brake_tau
+    if v_ego <= stop_speed:
+      return 0.0
+    curve_decel = ROUTINE_DECEL * v_ego / (v_ego + brake_tau) if v_ego > 0.0 else 0.0
+    braking_distance = max(usable_distance, 0.1)
+    required_decel = v_ego ** 2 / (2.0 * braking_distance) * float(np.interp(v_ego, STOP_MARGIN_BP, STOP_MARGIN_V))
+    return -min(max(curve_decel, required_decel), TERMINAL_MAX_DECEL)
+
+  @staticmethod
+  def _follow_target(projected: ProjectedLead, max_decel: float) -> float:
+    demand = max(-projected.accel, 0.0)
+    limit = min(ROUTINE_DECEL + 0.15 * max(demand - ROUTINE_DECEL, 0.0), max_decel)
+    return max(projected.accel, -limit)
+
+  @staticmethod
+  def _stopped(speed: float) -> bool:
+    return speed < 0.3
+
+  def _departure_confirmed(self, primary: LeadObservation | None, standstill: bool) -> bool:
+    if not standstill or self._radar_faulted:
+      return False
+    if primary is None:
+      return self._last_primary is None
+    if self._last_primary is None or not self._same_track(self._last_primary, primary):
+      return False
+    range_opening = primary.distance - self._last_primary.distance
+    return self._primary_frames >= 2 and not self._stopped(primary.speed) and range_opening >= 0.02
+
+  @staticmethod
+  def _lead_clear_for_launch(lead: ProjectedLead) -> bool:
+    distance_clear = lead.distance >= DESIRED_STOP_DISTANCE + 1.0 or not AccelController._stopped(lead.speed)
+    return lead.accel >= -NEUTRAL_ACCEL and distance_clear
+
+  def _fault_fallback(self) -> AccelDecision:
+    self.reset()
+    self._radar_faulted = True
+    return AccelDecision()
+
+  def _govern_accel(self, raw_target: float, max_accel: float, previous_plan_accel: float, *, launch: bool, terminal: bool) -> float:
+    if launch:
+      self._a_command = raw_target
+      return self._a_command
+
+    if self._a_command is None:
+      initial = previous_plan_accel if math.isfinite(previous_plan_accel) else 0.0
+      self._a_command = min(initial, max_accel)
+
+    previous = self._a_command
+    if raw_target < previous:
+      if raw_target < -ROUTINE_DECEL - 0.75:
+        jerk = URGENT_BRAKE_JERK
+      elif previous > -0.5:
+        jerk = BRAKE_ONSET_JERK
+      else:
+        jerk = BRAKE_BUILD_JERK
+      updated = max(raw_target, previous - jerk * self.dt)
+    else:
+      if terminal:
+        jerk = min(RELEASE_JERK, 0.6)
+      else:
+        jerk = RELEASE_JERK
+      updated = min(raw_target, previous + jerk * self.dt)
+
+    self._a_command = float(updated)
+    return self._a_command
+
+  def update(self, radar_state, *, v_ego: float, a_ego: float, v_cruise: float, follow_personality,
+             radar_valid: bool, stock_cruise_accel: float, stock_mpc_accel: float,
+             stock_should_stop: bool, fcw: bool, standstill: bool, stock_mpc_lead: int = -1,
+             previous_plan_accel: float = 0.0, profile: int = AccelProfile.normal) -> AccelDecision:
+    numeric_context = (v_ego, a_ego, v_cruise, stock_cruise_accel, stock_mpc_accel, self.action_time)
+    valid_context = (radar_valid and -0.1 <= v_ego <= 100.0 and 0.0 <= v_cruise <= 100.0
+                     and abs(a_ego) <= 10.0 and abs(stock_cruise_accel) <= 10.0
+                     and abs(stock_mpc_accel) <= 10.0 and all(math.isfinite(value) for value in numeric_context))
+    if not valid_context:
+      return self._fault_fallback()
+    if self._a_command is not None and math.isfinite(previous_plan_accel):
+      self._a_command = min(self._a_command, previous_plan_accel)
+
+    try:
+      raw_leads = (radar_state.leadOne, radar_state.leadTwo)
+      leads = [self._read_lead(raw_leads[0], 0), self._read_lead(raw_leads[1], 1)]
+    except AttributeError:
+      return self._fault_fallback()
+    if any(self._claims_present(raw_lead) and lead is None for raw_lead, lead in zip(raw_leads, leads, strict=True)):
+      return self._fault_fallback()
+    primary = leads[0]
+    secondary = leads[1]
+    if standstill and primary is None and secondary is not None:
+      return self._fault_fallback()
+    if self._radar_faulted:
+      if primary is None:
+        self._radar_recovery_frames += 1
+        self._radar_faulted = self._radar_recovery_frames < 3
+      else:
+        self._radar_faulted = False
+        self._radar_recovery_frames = 0
+    self._update_lead_continuity(primary, secondary)
+    primary_recently_missing = primary is None and self._last_primary is not None
+
+    current_v_ego = max(v_ego, 0.0)
+    projected_v_ego = max(current_v_ego + a_ego * self.action_time, 0.0)
+    ego_distance = (current_v_ego + projected_v_ego) * self.action_time * 0.5
+    accel_v = ACCEL_V.get(profile, ACCEL_V[AccelProfile.normal])
+    max_accel = float(np.interp(projected_v_ego, SPEED_BP, accel_v))
+    max_decel = float(np.interp(projected_v_ego, SPEED_BP, DECEL_V))
+    headway = FOLLOW_HEADWAY.get(follow_personality, FOLLOW_HEADWAY[log.LongitudinalPersonality.standard])
+    projected = [None if lead is None else self._pace_target(lead, ego_distance, projected_v_ego, headway, self.action_time) for lead in leads]
+    primary_projected, secondary_projected = projected
+    observed_leads = [lead for lead in projected if lead is not None]
+
+    free_accel = float(np.clip((v_cruise - projected_v_ego) / SPEED_RESPONSE_TIME, -ROUTINE_DECEL, max_accel))
+    raw_target = free_accel
+    selected_projected = None
+    secondary_trusted = secondary is not None and self._secondary_frames >= 3 and secondary.model_prob >= 0.5
+    trusted_leads = [lead for lead in (primary_projected, secondary_projected if secondary_trusted else None) if lead is not None]
+    terminal = False
+    for lead in trusted_leads:
+      if self._stopped(lead.speed):
+        lead_target = self._terminal_target(lead, projected_v_ego)
+        if lead_target is None:
+          continue
+        terminal = True
+      else:
+        lead_target = self._follow_target(lead, max_decel)
+      if lead_target < raw_target:
+        raw_target = lead_target
+        selected_projected = lead
+    if secondary_projected is not None and not secondary_trusted and secondary_projected.accel < -NEUTRAL_ACCEL:
+      raw_target = min(raw_target, 0.0)
+
+    all_leads_clear = all(self._lead_clear_for_launch(lead) for lead in observed_leads)
+    departure_confirmed = self._departure_confirmed(primary, bool(standstill)) and (primary is not None or secondary is None)
+    launch = bool(standstill and departure_confirmed and all_leads_clear and v_cruise > 0.3 and stock_cruise_accel > 0.0)
+    if launch:
+      self._braking_latched = False
+      raw_target = min(LAUNCH_ACCEL, v_cruise - projected_v_ego)
+      selected_projected = primary_projected
+    elif standstill:
+      raw_target = 0.0
+      self._a_command = 0.0
+
+    if primary_projected is not None:
+      pace_margin = primary_projected.relative_speed - primary_projected.relative_speed_target
+      if self._braking_latched and raw_target > 0.0 and pace_margin < 0.2 and not launch:
+        raw_target = 0.0
+      elif pace_margin >= 0.2:
+        self._braking_latched = False
+    elif not primary_recently_missing:
+      self._braking_latched = False
+
+    if primary_recently_missing:
+      raw_target = min(raw_target, 0.0)
+
+    if abs(raw_target) < NEUTRAL_ACCEL and not launch and not terminal:
+      raw_target = 0.0
+    raw_target = float(np.clip(raw_target, -(TERMINAL_MAX_DECEL if terminal else max_decel), max_accel))
+    if not standstill:
+      raw_target = min(raw_target, stock_cruise_accel)
+
+    a_target = self._govern_accel(raw_target, max_accel, previous_plan_accel, launch=launch, terminal=terminal)
+    if not standstill:
+      a_target = min(a_target, stock_cruise_accel)
+      self._a_command = a_target
+    if a_target <= -0.15 and primary_projected is not None:
+      self._braking_latched = True
+
+    should_stop = bool(standstill and not launch or
+      not launch and primary_projected is not None and self._stopped(primary_projected.speed)
+      and self._stopped(max(v_ego, 0.0))
+      and primary_projected.distance <= DESIRED_STOP_DISTANCE + 1.0
+    )
+    terminal_feasible = False
+    if terminal and selected_projected is not None and self._stopped(selected_projected.speed):
+      available_decel = min(max(-a_target, 0.0), max(-a_ego, 0.0))
+      usable_distance = max(selected_projected.distance - DESIRED_STOP_DISTANCE, 0.0)
+      stopping_distance = projected_v_ego ** 2 / (2.0 * max(available_decel, 0.1))
+      terminal_feasible = available_decel > NEUTRAL_ACCEL and usable_distance >= stopping_distance
+    stock_projected = projected[stock_mpc_lead] if 0 <= stock_mpc_lead < len(projected) else None
+    terminal_source_covered = (terminal_feasible and selected_projected is not None and stock_projected is not None
+                               and self._same_physical_lead(selected_projected, stock_projected))
+    urgent_ttc = False
+    danger_gap = False
+    current_desired_gap = DESIRED_STOP_DISTANCE + headway * current_v_ego
+    projected_desired_gap = DESIRED_STOP_DISTANCE + headway * projected_v_ego
+    for observed_lead in observed_leads:
+      current_closing_speed = max(current_v_ego - observed_lead.observation.speed, 0.0)
+      projected_closing_speed = max(-observed_lead.relative_speed, 0.0)
+      current_ttc = observed_lead.observation.distance / current_closing_speed if current_closing_speed > 1e-3 else math.inf
+      projected_ttc = observed_lead.distance / projected_closing_speed if projected_closing_speed > 1e-3 else math.inf
+      lead_covered = terminal_source_covered and selected_projected is not None and self._same_physical_lead(selected_projected, observed_lead)
+      urgent_ttc |= min(current_ttc, projected_ttc) < 4.0 and not lead_covered
+      danger_gap |= (observed_lead.observation.distance < 0.75 * current_desired_gap
+                     or observed_lead.distance < 0.75 * projected_desired_gap)
+    stock_more_urgent = not terminal_source_covered and stock_mpc_accel <= -1.2 and stock_mpc_accel < a_target - 0.3
+    stock_source_confirmed = (primary is None and stock_mpc_lead == -1) or (primary is not None and stock_mpc_lead == 0)
+    stale_stock_stop = bool(stock_should_stop and launch and stock_source_confirmed and not fcw and not danger_gap)
+    stock_safety_required = bool(
+      fcw or danger_gap or urgent_ttc or stock_more_urgent or stock_should_stop and not stale_stock_stop
+    )
+
+    self.selected_lead = -1 if selected_projected is None else selected_projected.observation.index
+    if should_stop:
+      self.state = AccelControllerState.stopHold
+    elif launch or a_target > (previous_plan_accel if math.isfinite(previous_plan_accel) else 0.0) + 1e-6:
+      self.state = AccelControllerState.release
+    elif a_target < -NEUTRAL_ACCEL:
+      self.state = AccelControllerState.restrict
+    elif primary_projected is not None:
+      self.state = AccelControllerState.hold
+    else:
+      self.state = AccelControllerState.free
+
+    decision = AccelDecision(a_target, should_stop, stock_safety_required)
+    if primary is not None:
+      self._last_primary = primary
+    self._last_secondary = secondary
+    return decision

openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/constants.py

@@ -0,0 +1,43 @@
+from openpilot.cereal import custom, log
+
+
+AccelProfile = custom.LongitudinalPlanSP.AccelController.Profile
+
+SPEED_BP = (0.0, 3.0, 10.0, 20.0, 30.0, 40.0)
+ACCEL_V = {
+  AccelProfile.eco: (2.00, 1.25, 0.90, 0.70, 0.55, 0.44),
+  AccelProfile.normal: (2.00, 1.40, 1.08, 0.84, 0.67, 0.52),
+  AccelProfile.sport: (2.00, 1.48, 1.20, 0.93, 0.73, 0.60),
+}
+DECEL_V = (1.0, 1.2, 2.3, 2.5, 2.5, 2.5)
+
+FOLLOW_HEADWAY = {
+  log.LongitudinalPersonality.relaxed: 1.8,
+  log.LongitudinalPersonality.standard: 1.6,
+  log.LongitudinalPersonality.aggressive: 1.4,
+}
+
+PACE_BUFFER_TIME = 10.0
+PACE_MAX_CLOSING_SPEED = 1.2
+PACE_MAX_OPENING_SPEED = 1.0
+PACE_GAIN = 0.65
+PACE_STABILITY_MARGIN = 2.2
+SPEED_RESPONSE_TIME = 3.0
+ROUTINE_DECEL = DECEL_V[0]
+
+BRAKE_ONSET_JERK = 1.0
+BRAKE_BUILD_JERK = 0.45
+URGENT_BRAKE_JERK = 2.2
+RELEASE_JERK = 0.8
+LAUNCH_ACCEL = 2.0
+
+DESIRED_STOP_DISTANCE = 6.0
+TERMINAL_TIME_CONSTANT = 3.0
+TERMINAL_PREVIEW_TIME = 12.0
+TERMINAL_MAX_DECEL = DECEL_V[-1]
+STOP_MARGIN_BP = (0.0, 5.0, 15.0)
+STOP_MARGIN_V = (1.0, 1.0, 1.6)
+
+GAP_DEADBAND_METERS = 2.0
+GAP_DEADBAND_SECONDS = 0.15
+NEUTRAL_ACCEL = 0.08

openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py

@@ -0,0 +1,529 @@
+import math
+from dataclasses import FrozenInstanceError
+from types import SimpleNamespace
+
+import numpy as np
+
+from openpilot.cereal import log
+from openpilot.common.realtime import DT_MDL
+from openpilot.common.test import OpenpilotTestCase
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController, AccelDecision
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import (
+  ACCEL_V, BRAKE_ONSET_JERK, DECEL_V, LAUNCH_ACCEL, RELEASE_JERK, SPEED_BP, AccelProfile,
+)
+
+
+def lead(*, present=False, distance=0.0, speed=0.0, accel=0.0, tau=1.5, track_id=-1, probability=1.0):
+  return SimpleNamespace(
+    present=present,
+    dRel=distance,
+    vRel=0.0,
+    vLead=speed,
+    vLeadK=speed,
+    aLeadK=accel,
+    aLeadTau=tau,
+    radarTrackId=track_id,
+    modelProb=probability,
+    radar=True,
+  )
+
+
+def radar(lead_one=None, lead_two=None):
+  return SimpleNamespace(leadOne=lead_one or lead(), leadTwo=lead_two or lead())
+
+
+def controller(*, delay=0.10, dt=DT_MDL):
+  CP = SimpleNamespace(longitudinalActuatorDelay=delay, openpilotLongitudinalControl=True)
+  return AccelController(CP, dt=dt)
+
+
+def update(instance, radar_state=None, **overrides):
+  arguments = {
+    "v_ego": 10.0,
+    "a_ego": 0.0,
+    "v_cruise": 25.0,
+    "follow_personality": log.LongitudinalPersonality.standard,
+    "radar_valid": True,
+    "stock_cruise_accel": 1.0,
+    "stock_mpc_accel": 1.0,
+    "stock_should_stop": False,
+    "stock_mpc_lead": -1,
+    "fcw": False,
+    "standstill": False,
+    "profile": AccelProfile.normal,
+  }
+  arguments.update(overrides)
+  return instance.update(radar() if radar_state is None else radar_state, **arguments)
+
+
+def settle(instance, radar_state=None, frames=80, **overrides):
+  decision = AccelDecision()
+  for _ in range(frames):
+    decision = update(instance, radar_state, **overrides)
+  return decision
+
+
+class TestAccelControllerContract(OpenpilotTestCase):
+  def test_decision_is_a_small_immutable_contract(self):
+    decision = AccelDecision(a_target=-0.4, should_stop=True, stock_safety_required=True)
+    self.assertEqual(
+      (decision.a_target, decision.should_stop, decision.stock_safety_required),
+      (-0.4, True, True),
+    )
+    with self.assertRaises(FrozenInstanceError):
+      decision.should_stop = False
+
+  def test_invalid_dt_is_rejected(self):
+    for dt in (0.0, -0.1, math.nan, math.inf):
+      with self.subTest(dt=dt), self.assertRaises(ValueError):
+        controller(dt=dt)
+
+  def test_inactive_context_resets_without_actuating(self):
+    cases = (
+      {"radar_valid": False},
+      {"v_ego": math.nan},
+      {"a_ego": math.inf},
+      {"a_ego": -100.0},
+      {"v_ego": 1e308},
+      {"v_cruise": -1.0},
+      {"stock_cruise_accel": math.nan},
+      {"stock_mpc_accel": math.nan},
+    )
+    for case in cases:
+      with self.subTest(case=case):
+        instance = controller()
+        self.assertEqual(update(instance, **case), AccelDecision())
+        self.assertFalse(instance.is_active)
+
+  def test_acceleration_limit_never_exceeds_stock(self):
+    decision = settle(controller(), v_ego=5.0, stock_cruise_accel=1.1, stock_mpc_accel=1.1)
+    self.assertIsNotNone(decision.a_target)
+    self.assertLessEqual(decision.a_target, min(1.1, np.interp(5.0, SPEED_BP, ACCEL_V[AccelProfile.normal])))
+
+  def test_selected_profile_changes_positive_acceleration_only(self):
+    outputs = {}
+    for profile in ACCEL_V:
+      instance = controller()
+      previous = 0.0
+      for _ in range(80):
+        decision = update(instance, v_ego=5.0, stock_cruise_accel=2.0, stock_mpc_accel=2.0, previous_plan_accel=previous, profile=profile)
+        previous = decision.a_target
+      outputs[profile] = decision.a_target
+    self.assertLess(outputs[AccelProfile.eco], outputs[AccelProfile.normal])
+    self.assertLess(outputs[AccelProfile.normal], outputs[AccelProfile.sport])
+
+    slower_lead = radar(lead(present=True, distance=55.0, speed=14.0, track_id=7))
+    braking = {
+      profile: settle(controller(), slower_lead, v_ego=20.0, v_cruise=30.0, stock_cruise_accel=0.7, stock_mpc_accel=0.7, profile=profile).a_target
+      for profile in ACCEL_V
+    }
+    self.assertEqual(len({round(value, 9) for value in braking.values()}), 1)
+
+  def test_acceleration_profiles_are_ordered_and_linearly_interpolated(self):
+    self.assertEqual(tuple(ACCEL_V), (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport))
+    self.assertEqual(set(ACCEL_V), {AccelProfile.eco, AccelProfile.normal, AccelProfile.sport})
+    for values in ACCEL_V.values():
+      self.assertEqual(len(values), len(SPEED_BP))
+      self.assertEqual(values[0], LAUNCH_ACCEL)
+      self.assertTrue(all(after <= before for before, after in zip(values[:-1], values[1:], strict=True)))
+      for speed, expected in zip(SPEED_BP, values, strict=True):
+        self.assertEqual(np.interp(speed, SPEED_BP, values), expected)
+      for index in range(len(SPEED_BP) - 1):
+        midpoint = (SPEED_BP[index] + SPEED_BP[index + 1]) / 2.0
+        expected = (values[index] + values[index + 1]) / 2.0
+        self.assertAlmostEqual(np.interp(midpoint, SPEED_BP, values), expected)
+    for speed in SPEED_BP[1:]:
+      self.assertLess(np.interp(speed, SPEED_BP, ACCEL_V[AccelProfile.eco]), np.interp(speed, SPEED_BP, ACCEL_V[AccelProfile.normal]))
+      self.assertLess(np.interp(speed, SPEED_BP, ACCEL_V[AccelProfile.normal]), np.interp(speed, SPEED_BP, ACCEL_V[AccelProfile.sport]))
+
+  def test_deceleration_curve_is_ascending_and_linearly_interpolated(self):
+    self.assertEqual(len(DECEL_V), len(SPEED_BP))
+    self.assertTrue(all(after >= before for before, after in zip(DECEL_V[:-1], DECEL_V[1:], strict=True)))
+    for speed, expected in zip(SPEED_BP, DECEL_V, strict=True):
+      self.assertEqual(np.interp(speed, SPEED_BP, DECEL_V), expected)
+    for index in range(len(SPEED_BP) - 1):
+      midpoint = (SPEED_BP[index] + SPEED_BP[index + 1]) / 2.0
+      expected = (DECEL_V[index] + DECEL_V[index + 1]) / 2.0
+      self.assertAlmostEqual(np.interp(midpoint, SPEED_BP, DECEL_V), expected)
+
+
+class TestElasticPace(OpenpilotTestCase):
+  def test_slower_lead_requests_early_decel_before_ttc_is_urgent(self):
+    instance = controller()
+    slower_lead = lead(present=True, distance=55.0, speed=14.0, track_id=7)
+    decision = settle(instance, radar(slower_lead), v_ego=20.0, v_cruise=30.0, stock_cruise_accel=0.7, stock_mpc_accel=0.7)
+
+    self.assertGreater(55.0 / (20.0 - 14.0), 5.0)
+    self.assertIsNotNone(decision.a_target)
+    self.assertLess(decision.a_target, 0.0)
+    self.assertTrue(instance.is_active)
+    self.assertFalse(decision.stock_safety_required)
+
+  def test_closing_gap_produces_one_monotonic_braking_decision(self):
+    instance = controller()
+    outputs = []
+    for distance in (65.0, 60.0, 55.0, 50.0, 45.0, 40.0, 35.0):
+      decision = settle(
+        instance,
+        radar(lead(present=True, distance=distance, speed=14.0, track_id=11)),
+        frames=4,
+        v_ego=20.0,
+        v_cruise=30.0,
+        stock_cruise_accel=0.7,
+        stock_mpc_accel=0.7,
+      )
+      outputs.append(decision.a_target)
+
+    self.assertTrue(all(value is not None and math.isfinite(value) for value in outputs))
+    self.assertTrue(all(after <= before + 1e-9 for before, after in zip(outputs[:-1], outputs[1:], strict=True)))
+
+  def test_actuator_delay_makes_the_same_closing_lead_no_less_restrictive(self):
+    radar_state = radar(lead(present=True, distance=45.0, speed=14.0, track_id=12))
+    short_delay = settle(controller(delay=0.05), radar_state, v_ego=20.0, v_cruise=30.0, stock_cruise_accel=0.7, stock_mpc_accel=0.7)
+    long_delay = settle(controller(delay=0.50), radar_state, v_ego=20.0, v_cruise=30.0, stock_cruise_accel=0.7, stock_mpc_accel=0.7)
+
+    self.assertLessEqual(long_delay.a_target, short_delay.a_target + 1e-9)
+
+  def test_routine_brake_onset_is_jerk_limited(self):
+    instance = controller()
+    settle(instance, frames=30, v_ego=20.0, v_cruise=30.0, stock_cruise_accel=0.7, stock_mpc_accel=0.7)
+    restrictive = radar(lead(present=True, distance=45.0, speed=12.0, track_id=13))
+    outputs = [
+      update(instance, restrictive, v_ego=20.0, v_cruise=30.0, stock_cruise_accel=0.7, stock_mpc_accel=0.7).a_target
+      for _ in range(20)
+    ]
+
+    self.assertTrue(all(value is not None for value in outputs))
+    drops = [before - after for before, after in zip(outputs[:-1], outputs[1:], strict=True)]
+    self.assertLessEqual(max(drops), BRAKE_ONSET_JERK * DT_MDL + 1e-9)
+    self.assertLess(outputs[-1], outputs[0])
+
+  def test_duplicate_radar_falls_back_to_stock(self):
+    instance = controller()
+    tracked = radar(lead(present=True, distance=50.0, speed=14.0, track_id=14))
+    settle(instance, tracked, frames=20, v_ego=20.0, v_cruise=30.0, stock_cruise_accel=0.7, stock_mpc_accel=0.7)
+    duplicate_with_different_data = radar(lead(present=True, distance=15.0, speed=2.0, track_id=14))
+
+    fallback = update(
+      instance,
+      duplicate_with_different_data,
+      v_ego=20.0,
+      v_cruise=30.0,
+      stock_cruise_accel=0.7,
+      stock_mpc_accel=0.7,
+      radar_valid=False,
+    )
+    self.assertEqual(fallback, AccelDecision())
+
+  def test_equal_speed_lead_does_not_chase_a_small_gap_error(self):
+    instance = controller()
+    decision = settle(instance, radar(lead(present=True, distance=24.0, speed=10.0, track_id=15)), v_ego=10.0, v_cruise=25.0)
+    self.assertAlmostEqual(decision.a_target, 0.0)
+
+  def test_fresh_lead_dropout_does_not_immediately_reaccelerate(self):
+    instance = controller()
+    tracked = radar(lead(present=True, distance=35.0, speed=6.0, track_id=16))
+    settle(instance, tracked, v_ego=12.0, v_cruise=25.0)
+    for _ in range(2):
+      self.assertLessEqual(update(instance, radar(), v_ego=12.0, v_cruise=25.0).a_target, 0.0)
+
+  def test_second_lead_requires_persistence_and_can_only_add_braking(self):
+    instance = controller()
+    primary = lead(present=True, distance=80.0, speed=20.0, track_id=21, probability=0.95)
+    second = lead(present=True, distance=55.0, speed=14.0, track_id=22, probability=0.95)
+    decisions = [
+      update(instance, radar(primary, second), v_ego=20.0, v_cruise=30.0, stock_cruise_accel=0.7, stock_mpc_accel=0.7)
+      for _ in range(3)
+    ]
+
+    self.assertGreaterEqual(decisions[1].a_target, decisions[0].a_target)
+    self.assertLess(decisions[2].a_target, decisions[1].a_target)
+    self.assertLess(decisions[2].a_target, 0.7)
+
+
+class TestStopAndLaunch(OpenpilotTestCase):
+  def test_distant_stationary_lead_does_not_start_a_long_brake_event(self):
+    decision = settle(controller(), radar(lead(present=True, distance=300.0, speed=0.0, track_id=29)), v_ego=20.0, v_cruise=25.0,
+                      stock_cruise_accel=0.8, stock_mpc_accel=0.8)
+    self.assertGreaterEqual(decision.a_target, 0.0)
+
+  def test_stopped_lead_converges_to_stop_hold_without_positive_rebound(self):
+    instance = controller()
+    outputs = []
+    should_stop = []
+    samples = ((2.0, 11.0), (1.2, 8.5), (0.6, 7.0), (0.25, 6.3), (0.05, 6.05), (0.0, 6.0))
+    for speed, distance in samples:
+      decision = settle(
+        instance,
+        radar(lead(present=True, distance=distance, speed=0.0, track_id=30)),
+        frames=10,
+        v_ego=speed,
+        v_cruise=8.0,
+        stock_cruise_accel=0.8,
+        stock_mpc_accel=-0.5,
+        stock_should_stop=speed < 0.3,
+        standstill=speed < 0.01,
+      )
+      outputs.append(decision.a_target)
+      should_stop.append(decision.should_stop)
+
+    self.assertTrue(all(value is not None and value <= 0.0 for value in outputs))
+    self.assertFalse(any(should_stop[:3]))
+    self.assertTrue(any(should_stop[3:]))
+    self.assertTrue(should_stop[-1])
+
+  def test_same_track_departure_has_no_controller_added_dwell(self):
+    instance = controller()
+    stopped = radar(lead(present=True, distance=6.0, speed=0.0, track_id=31))
+    held = settle(
+      instance,
+      stopped,
+      frames=8,
+      v_ego=0.0,
+      v_cruise=8.0,
+      stock_cruise_accel=0.8,
+      stock_mpc_accel=-0.5,
+      stock_should_stop=True,
+      stock_mpc_lead=0,
+      standstill=True,
+    )
+    self.assertTrue(held.should_stop)
+
+    departing = radar(lead(present=True, distance=6.1, speed=1.0, accel=1.0, track_id=31))
+    released = update(
+      instance,
+      departing,
+      v_ego=0.0,
+      v_cruise=8.0,
+      stock_cruise_accel=0.8,
+      stock_mpc_accel=0.8,
+      stock_should_stop=False,
+      standstill=True,
+    )
+    self.assertFalse(released.should_stop)
+    self.assertEqual(released.a_target, LAUNCH_ACCEL)
+
+    limited = controller()
+    settle(limited, stopped, frames=8, v_ego=0.0, v_cruise=0.5, stock_cruise_accel=0.1, stock_mpc_accel=-0.5,
+           stock_should_stop=True, stock_mpc_lead=0, standstill=True)
+    released = update(limited, departing, v_ego=0.0, v_cruise=0.5, stock_cruise_accel=0.1, stock_mpc_accel=0.1, standstill=True)
+    self.assertEqual(released.a_target, 0.5)
+
+  def test_radar_fault_requires_clear_road_confirmation_before_launch(self):
+    instance = controller()
+    stopped = radar(lead(present=True, distance=6.0, speed=0.0, track_id=31))
+    settle(instance, stopped, frames=8, v_ego=0.0, v_cruise=8.0, stock_cruise_accel=0.8, stock_mpc_accel=-0.5,
+           stock_should_stop=True, stock_mpc_lead=0, standstill=True)
+    self.assertEqual(update(instance, stopped, v_ego=0.0, v_cruise=8.0, radar_valid=False, standstill=True), AccelDecision())
+
+    clear_road = radar()
+    for _ in range(2):
+      held = update(instance, clear_road, v_ego=0.0, v_cruise=8.0, stock_cruise_accel=0.8, stock_mpc_accel=-0.5,
+                    stock_should_stop=True, stock_mpc_lead=-1, standstill=True)
+      self.assertTrue(held.should_stop)
+      self.assertLessEqual(held.a_target, 0.0)
+
+    released = update(instance, clear_road, v_ego=0.0, v_cruise=8.0, stock_cruise_accel=0.8, stock_mpc_accel=-0.5,
+                      stock_should_stop=True, stock_mpc_lead=-1, standstill=True)
+    self.assertFalse(released.should_stop)
+    self.assertGreater(released.a_target, 0.0)
+
+  def test_new_track_departure_requires_one_confirmation_frame(self):
+    instance = controller()
+    stopped = radar(lead(present=True, distance=6.0, speed=0.0, track_id=31))
+    settle(
+      instance,
+      stopped,
+      frames=8,
+      v_ego=0.0,
+      v_cruise=8.0,
+      stock_cruise_accel=0.8,
+      stock_mpc_accel=-0.5,
+      stock_should_stop=True,
+      stock_mpc_lead=0,
+      standstill=True,
+    )
+
+    changed_track = radar(lead(present=True, distance=6.1, speed=1.0, accel=1.0, track_id=99))
+    unconfirmed = update(
+      instance,
+      changed_track,
+      v_ego=0.0,
+      v_cruise=8.0,
+      stock_cruise_accel=0.8,
+      stock_mpc_accel=-0.5,
+      stock_should_stop=True,
+      stock_mpc_lead=0,
+      standstill=True,
+    )
+    self.assertTrue(unconfirmed.should_stop)
+    self.assertLessEqual(unconfirmed.a_target, 0.0)
+
+    confirmed_track = radar(lead(present=True, distance=6.2, speed=1.0, accel=1.0, track_id=99))
+    confirmed = update(
+      instance,
+      confirmed_track,
+      v_ego=0.0,
+      v_cruise=8.0,
+      stock_cruise_accel=0.8,
+      stock_mpc_accel=-0.5,
+      stock_should_stop=True,
+      standstill=True,
+    )
+    self.assertFalse(confirmed.should_stop)
+    self.assertGreater(confirmed.a_target, 0.0)
+
+  def test_secondary_lead_can_veto_but_not_authorize_launch(self):
+    def stopped_instance():
+      instance = controller()
+      stopped = radar(lead(present=True, distance=6.0, speed=0.0, track_id=60))
+      settle(instance, stopped, frames=8, v_ego=0.0, v_cruise=8.0, stock_cruise_accel=0.8, stock_mpc_accel=-0.5,
+             stock_should_stop=True, stock_mpc_lead=0, standstill=True)
+      return instance
+
+    departing = lead(present=True, distance=6.1, speed=1.0, accel=1.0, track_id=60)
+    blocked = update(stopped_instance(), radar(departing, lead(present=True, distance=4.0, speed=0.0, track_id=61)),
+                     v_ego=0.0, v_cruise=8.0, stock_cruise_accel=0.8, stock_mpc_accel=-0.5, stock_should_stop=True,
+                     stock_mpc_lead=0, standstill=True)
+    self.assertTrue(blocked.should_stop)
+    self.assertLessEqual(blocked.a_target, 0.0)
+
+    buffered = update(stopped_instance(), radar(departing, lead(present=True, distance=6.1, speed=0.0, track_id=61)),
+                      v_ego=0.0, v_cruise=8.0, stock_cruise_accel=0.8, stock_mpc_accel=-0.5, stock_should_stop=True,
+                      stock_mpc_lead=0, standstill=True)
+    self.assertTrue(buffered.should_stop)
+    self.assertLessEqual(buffered.a_target, 0.0)
+
+    released = update(stopped_instance(), radar(departing, lead(present=True, distance=20.0, speed=2.0, track_id=62)),
+                      v_ego=0.0, v_cruise=8.0, stock_cruise_accel=0.8, stock_mpc_accel=-0.5, stock_should_stop=True,
+                      stock_mpc_lead=0, standstill=True)
+    self.assertFalse(released.should_stop)
+    self.assertGreater(released.a_target, 0.0)
+
+  def test_secondary_only_at_standstill_falls_back_to_stock(self):
+    decision = update(controller(), radar(lead_two=lead(present=True, distance=20.0, speed=2.0, track_id=63)),
+                      v_ego=0.0, v_cruise=8.0, stock_cruise_accel=0.8, stock_mpc_accel=0.8, stock_mpc_lead=1, standstill=True)
+    self.assertEqual(decision, AccelDecision())
+
+
+class TestSafetyAndFallback(OpenpilotTestCase):
+  def test_nonurgent_stock_braking_does_not_own_normal_driving(self):
+    decision = settle(
+      controller(),
+      v_ego=15.0,
+      v_cruise=25.0,
+      stock_cruise_accel=1.0,
+      stock_mpc_accel=-0.6,
+      stock_should_stop=False,
+    )
+    self.assertFalse(decision.stock_safety_required)
+    self.assertGreater(decision.a_target, -0.6)
+
+  def test_stock_cruise_candidate_is_always_an_upper_bound(self):
+    decision = settle(controller(), v_ego=15.0, v_cruise=25.0, stock_cruise_accel=-0.3, stock_mpc_accel=0.8)
+    self.assertLessEqual(decision.a_target, -0.3)
+
+  def test_fcw_bypasses_comfort_ramp_and_preserves_stock_braking(self):
+    instance = controller()
+    settle(instance, v_ego=20.0, v_cruise=30.0, stock_cruise_accel=0.7, stock_mpc_accel=0.7)
+    decision = update(
+      instance,
+      radar(lead(present=True, distance=15.0, speed=5.0, track_id=40)),
+      v_ego=20.0,
+      v_cruise=30.0,
+      stock_cruise_accel=0.7,
+      stock_mpc_accel=-2.5,
+      fcw=True,
+    )
+    self.assertTrue(decision.stock_safety_required)
+    self.assertTrue(math.isfinite(decision.a_target))
+
+  def test_urgent_ttc_uses_stock_floor_without_fcw(self):
+    decision = update(
+      controller(),
+      radar(lead(present=True, distance=20.0, speed=10.0, track_id=41)),
+      v_ego=20.0,
+      v_cruise=30.0,
+      stock_cruise_accel=0.7,
+      stock_mpc_accel=-1.8,
+    )
+    self.assertTrue(decision.stock_safety_required)
+    self.assertTrue(math.isfinite(decision.a_target))
+
+  def test_current_ttc_stays_authoritative_during_measured_deceleration(self):
+    decision = update(
+      controller(delay=0.95),
+      radar(lead(present=True, distance=70.0, speed=0.0, track_id=42)),
+      v_ego=20.0,
+      a_ego=-10.0,
+      v_cruise=30.0,
+      stock_cruise_accel=0.7,
+      stock_mpc_accel=-1.0,
+    )
+    self.assertTrue(decision.stock_safety_required)
+
+  def test_feasible_terminal_braking_does_not_escalate_to_late_stock_braking(self):
+    instance = controller(delay=0.20)
+    stationary = radar(lead(present=True, distance=60.0, speed=0.0, track_id=43))
+    decision = settle(
+      instance,
+      stationary,
+      frames=60,
+      v_ego=15.0,
+      a_ego=-2.5,
+      v_cruise=30.0,
+      stock_cruise_accel=0.7,
+      stock_mpc_accel=-3.3,
+      stock_mpc_lead=0,
+    )
+    self.assertFalse(decision.stock_safety_required)
+    self.assertGreaterEqual(decision.a_target, -2.5)
+
+    unsafe = update(instance, radar(lead(present=True, distance=45.0, speed=0.0, track_id=43)), v_ego=15.0, a_ego=-2.5,
+                    v_cruise=30.0, stock_cruise_accel=0.7, stock_mpc_accel=-3.3, stock_mpc_lead=0)
+    self.assertTrue(unsafe.stock_safety_required)
+
+  def test_terminal_feasibility_only_covers_the_selected_stock_source(self):
+    instance = controller()
+    primary = lead(present=True, distance=180.0, speed=0.0, track_id=44)
+    settle(instance, radar(primary), frames=60, v_ego=20.0, a_ego=-2.0, v_cruise=30.0,
+           stock_cruise_accel=0.7, stock_mpc_accel=-2.0, stock_mpc_lead=0)
+
+    secondary = lead(present=True, distance=70.0, speed=0.0, track_id=45)
+    decision = update(instance, radar(primary, secondary), v_ego=20.0, a_ego=-2.0, v_cruise=30.0,
+                      stock_cruise_accel=0.7, stock_mpc_accel=-3.0, stock_mpc_lead=1)
+    self.assertTrue(decision.stock_safety_required)
+
+  def test_duplicate_radar_leads_share_terminal_feasibility(self):
+    instance = controller(delay=0.20)
+    primary = lead(present=True, distance=60.0, speed=0.0, track_id=-1)
+    settle(instance, radar(primary), frames=60, v_ego=15.0, a_ego=-2.5, v_cruise=30.0,
+           stock_cruise_accel=0.7, stock_mpc_accel=-3.3, stock_mpc_lead=0)
+
+    duplicate = lead(present=True, distance=60.0, speed=0.0, track_id=-1)
+    decision = update(instance, radar(primary, duplicate), v_ego=15.0, a_ego=-2.5, v_cruise=30.0,
+                      stock_cruise_accel=0.7, stock_mpc_accel=-3.3, stock_mpc_lead=1)
+    self.assertFalse(decision.stock_safety_required)
+
+  def test_stock_emergency_releases_through_the_jerk_governor(self):
+    instance = controller()
+    settle(instance, stock_cruise_accel=0.8, stock_mpc_accel=0.8)
+    update(instance, stock_cruise_accel=0.8, stock_mpc_accel=-2.5, fcw=True)
+    released = update(instance, stock_cruise_accel=0.8, stock_mpc_accel=0.8, previous_plan_accel=-2.5)
+    self.assertLessEqual(released.a_target - (-2.5), RELEASE_JERK * DT_MDL + 1e-9)
+
+  def test_unhealthy_radar_falls_back_to_stock(self):
+    decision = update(
+      controller(),
+      radar_valid=False,
+      stock_cruise_accel=0.7,
+      stock_mpc_accel=-0.4,
+      stock_should_stop=True,
+    )
+    self.assertEqual(decision, AccelDecision())
+
+  def test_malformed_lead_never_produces_nonfinite_output(self):
+    malformed = radar(lead(present=True, distance=math.nan, speed=math.inf, accel=math.nan, track_id=50))
+    decision = update(controller(), malformed, stock_cruise_accel=0.7, stock_mpc_accel=-0.3)
+
+    self.assertEqual(decision, AccelDecision())

openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller_interfaces.py

@@ -0,0 +1,361 @@
+from types import SimpleNamespace
+from typing import Any
+
+from openpilot.cereal import custom, log
+from openpilot.common.params import Params
+from openpilot.common.test import OpenpilotTestCase
+from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalPlanSource as MpcSource
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController, AccelControllerState
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import AccelProfile
+from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP, LongitudinalPlanSource
+
+
+def lead(*, present=False, distance=0.0, speed=0.0, accel=0.0, track_id=-1, probability=1.0):
+  return SimpleNamespace(
+    present=present,
+    dRel=distance,
+    vRel=0.0,
+    vLead=speed,
+    vLeadK=speed,
+    aLeadK=accel,
+    aLeadTau=1.5,
+    radarTrackId=track_id,
+    modelProb=probability,
+    radar=True,
+  )
+
+
+def radar(lead_one=None, lead_two=None):
+  return SimpleNamespace(leadOne=lead_one or lead(), leadTwo=lead_two or lead())
+
+
+class PlannerSM(dict):
+  def __init__(self, *, experimental=False, force_decel=False, radar_state=None, radar_time=100, standstill=False, frame=0):
+    super().__init__(
+      radarState=radar_state or radar(),
+      carState=SimpleNamespace(vEgo=10.0, aEgo=0.0, vCruise=72.0, standstill=standstill),
+      selfdriveState=SimpleNamespace(
+        enabled=True,
+        experimentalMode=experimental,
+        personality=log.LongitudinalPersonality.standard,
+      ),
+      controlsState=SimpleNamespace(forceDecel=force_decel, longControlState=LongCtrlState.pid),
+    )
+    self.valid = {"radarState": True}
+    self.alive = {"radarState": True}
+    self.logMonoTime = {"radarState": radar_time}
+    self.frame = frame
+
+  def all_checks(self, service_list=None):
+    return True
+
+
+def accel_controller():
+  CP = SimpleNamespace(longitudinalActuatorDelay=0.1, openpilotLongitudinalControl=True)
+  return AccelController(CP)
+
+
+def planner_for_hook(*, available=True, enabled=True, profile=AccelProfile.normal):
+  planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
+  dynamic_planner: Any = planner
+  dynamic_planner.accel_controller = accel_controller()
+  dynamic_planner.accel_controller_available = available
+  dynamic_planner.accel_controller_enabled = enabled
+  dynamic_planner.accel_controller_profile = profile
+  dynamic_planner.dec = SimpleNamespace(active=lambda: False)
+  dynamic_planner.model_accel_transition = SimpleNamespace(active=False)
+  dynamic_planner.output_v_target = 30.0
+  dynamic_planner.previous_plan_accel = 0.0
+  dynamic_planner.a_cruise = 0.0
+  dynamic_planner.fcw = False
+  dynamic_planner._radar_fresh_this_cycle = True
+  dynamic_planner._radar_healthy_this_cycle = True
+  return planner
+
+
+class TestPlannerOwnership(OpenpilotTestCase):
+  def test_active_controller_owns_routine_acc_instead_of_stock_mpc(self):
+    planner = planner_for_hook()
+    candidates = [(-0.6, MpcSource.lead0, False), (0.7, MpcSource.cruise, False)]
+
+    result = planner.update_accel_controller(PlannerSM(), candidates)
+
+    self.assertEqual(len(result), 1)
+    self.assertEqual(result[0][1:], (MpcSource.cruise, False))
+    self.assertGreater(result[0][0], -0.6)
+    self.assertLessEqual(result[0][0], 0.7)
+    self.assertTrue(planner.accel_controller.is_active)
+
+  def test_controller_candidate_uses_the_selected_primary_lead_source(self):
+    planner = planner_for_hook()
+    planner.output_v_target = 30.0
+    sm = PlannerSM(radar_state=radar(lead(present=True, distance=55.0, speed=14.0, track_id=10)))
+    sm["carState"].vEgo = 20.0
+    candidates = [(-0.6, MpcSource.lead0, False), (0.7, MpcSource.cruise, False)]
+
+    result = planner.update_accel_controller(sm, candidates)
+
+    self.assertEqual(len(result), 1)
+    self.assertEqual(result[0][1], MpcSource.lead0)
+    self.assertLess(result[0][0], 0.0)
+
+  def test_only_second_lead_is_published_as_lead_one_source(self):
+    planner = planner_for_hook()
+    sm = PlannerSM(radar_state=radar(lead_two=lead(present=True, distance=55.0, speed=14.0, track_id=20)))
+    sm["carState"].vEgo = 20.0
+    candidates = [(-0.6, MpcSource.lead0, False), (0.7, MpcSource.cruise, False)]
+
+    for _ in range(3):
+      result = planner.update_accel_controller(sm, candidates)
+
+    self.assertEqual(len(result), 1)
+    self.assertEqual(result[0][1], MpcSource.lead1)
+
+
+class TestPlannerSafetyArbitration(OpenpilotTestCase):
+  def test_fake_mpc_lead_does_not_block_free_road_launch(self):
+    planner = planner_for_hook()
+    sm = PlannerSM(standstill=True)
+    sm["carState"].vEgo = 0.0
+    candidates = [(0.0, MpcSource.lead1, True), (0.8, MpcSource.cruise, False)]
+
+    result = planner.update_accel_controller(sm, candidates)
+
+    self.assertEqual(len(result), 1)
+    self.assertGreater(result[0][0], 0.0)
+    self.assertFalse(result[0][2])
+
+  def test_fcw_keeps_stock_emergency_candidate_in_parallel(self):
+    planner = planner_for_hook()
+    planner.fcw = True
+    candidates = [(-2.5, MpcSource.lead0, False), (0.7, MpcSource.cruise, False)]
+
+    result = planner.update_accel_controller(PlannerSM(), candidates)
+
+    self.assertEqual(len(result), 2)
+    self.assertTrue(planner.accel_controller.is_active)
+    self.assertIn((-2.5, MpcSource.lead0, False), result)
+    self.assertEqual(min(result, key=lambda candidate: candidate[0]), (-2.5, MpcSource.lead0, False))
+
+  def test_urgent_ttc_keeps_stock_stop_and_braking_authoritative(self):
+    planner = planner_for_hook()
+    sm = PlannerSM(radar_state=radar(lead(present=True, distance=20.0, speed=10.0, track_id=30)))
+    sm["carState"].vEgo = 20.0
+    candidates = [(-1.8, MpcSource.lead0, True), (0.7, MpcSource.cruise, False)]
+
+    result = planner.update_accel_controller(sm, candidates)
+
+    self.assertEqual(len(result), 2)
+    self.assertIn((-1.8, MpcSource.lead0, True), result)
+    self.assertTrue(any(stop for _, _, stop in result))
+    self.assertEqual(min(result, key=lambda candidate: candidate[0]), (-1.8, MpcSource.lead0, True))
+
+  def test_confirmed_departure_suppresses_stale_stock_stop_in_same_frame(self):
+    planner = planner_for_hook()
+    stopped_sm = PlannerSM(
+      radar_state=radar(lead(present=True, distance=6.0, speed=0.0, track_id=31)),
+      standstill=True,
+    )
+    stopped_sm["carState"].vEgo = 0.0
+    stopped_candidates = [(-0.5, MpcSource.lead0, True), (0.8, MpcSource.cruise, False)]
+    for _ in range(2):
+      held = planner.update_accel_controller(stopped_sm, stopped_candidates)
+    self.assertTrue(any(stop for _, _, stop in held))
+
+    departure_sm = PlannerSM(
+      radar_state=radar(lead(present=True, distance=6.1, speed=1.0, accel=1.0, track_id=31)),
+      standstill=True,
+    )
+    departure_sm["carState"].vEgo = 0.0
+    released = planner.update_accel_controller(departure_sm, stopped_candidates)
+
+    self.assertEqual(len(released), 1)
+    self.assertGreater(released[0][0], 0.0)
+    self.assertFalse(released[0][2])
+
+
+class TestPlannerFallbacks(OpenpilotTestCase):
+  def test_disabled_e2e_force_decel_and_unhealthy_radar_preserve_exact_candidates(self):
+    candidates = (
+      (-0.4, MpcSource.lead0, True),
+      (0.6, MpcSource.cruise, False),
+      (0.2, MpcSource.e2e, False),
+    )
+    disabled = planner_for_hook(enabled=False)
+    blended = planner_for_hook()
+    forced = planner_for_hook()
+    unhealthy = planner_for_hook()
+    unhealthy._radar_healthy_this_cycle = False
+    cases = (
+      (disabled, PlannerSM()),
+      (blended, PlannerSM(experimental=True)),
+      (forced, PlannerSM(force_decel=True)),
+      (unhealthy, PlannerSM()),
+    )
+
+    for planner, sm in cases:
+      with self.subTest(planner=planner, sm=sm):
+        result = planner.update_accel_controller(sm, candidates)
+        self.assertIs(result, candidates)
+
+  def test_missing_cruise_or_mpc_candidate_is_exact_noop(self):
+    planner = planner_for_hook()
+    candidates_without_cruise = ((-0.4, MpcSource.lead0, True), (0.2, MpcSource.e2e, False))
+    candidates_without_mpc = ((0.6, MpcSource.cruise, False), (0.2, MpcSource.e2e, False))
+
+    self.assertIs(planner.update_accel_controller(PlannerSM(), candidates_without_cruise), candidates_without_cruise)
+    self.assertIs(planner.update_accel_controller(PlannerSM(), candidates_without_mpc), candidates_without_mpc)
+
+  def test_malformed_radar_is_exact_stock_fallback(self):
+    planner = planner_for_hook()
+    candidates = ((-0.4, MpcSource.lead0, True), (0.6, MpcSource.cruise, False))
+    malformed_sm = PlannerSM(radar_state=SimpleNamespace())
+
+    self.assertIs(planner.update_accel_controller(malformed_sm, candidates), candidates)
+
+  def test_disengaged_acc_cannot_publish_stale_positive_acceleration(self):
+    planner = planner_for_hook()
+    sm = PlannerSM()
+    sm["controlsState"].longControlState = LongCtrlState.off
+    candidates = [(-0.4, MpcSource.lead0, False), (0.6, MpcSource.cruise, False)]
+
+    result = planner.update_accel_controller(sm, candidates)
+
+    self.assertEqual(result, [candidates[0], (0.0, MpcSource.cruise, False)])
+    self.assertEqual(planner.a_cruise, 0.0)
+    self.assertEqual(planner.accel_controller.state, AccelControllerState.inactive)
+
+  def test_radar_freshness_requires_a_healthy_advanced_message(self):
+    planner = planner_for_hook()
+    planner._radar_log_mono_time = None
+    sm = PlannerSM(radar_time=100)
+    self.assertTrue(planner._update_radar_freshness(sm))
+    self.assertFalse(planner._update_radar_freshness(sm))
+    sm.logMonoTime["radarState"] = 101
+    self.assertTrue(planner._update_radar_freshness(sm))
+    sm.valid["radarState"] = False
+    sm.logMonoTime["radarState"] = 102
+    self.assertFalse(planner._update_radar_freshness(sm))
+    self.assertFalse(planner._radar_healthy_this_cycle)
+
+
+class TestParamsSchemaAndTelemetry(OpenpilotTestCase):
+  def test_planner_reads_and_sanitizes_controller_params(self):
+    planner = planner_for_hook(enabled=False)
+    planner.params = Params()
+    planner.params.put_bool("AccelPersonalityEnabled", False, block=True)
+
+    for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport):
+      planner.params.put("AccelPersonality", profile, block=True)
+      planner.read_accel_controller_params()
+      self.assertFalse(planner.accel_controller_enabled)
+      self.assertEqual(planner.accel_controller_profile, profile)
+
+    planner.params.put_bool("AccelPersonalityEnabled", True, block=True)
+    for value, expected in ((-99, AccelProfile.eco), (99, AccelProfile.sport)):
+      planner.params.put("AccelPersonality", value, block=True)
+      planner.read_accel_controller_params()
+      self.assertTrue(planner.accel_controller_enabled)
+      self.assertEqual(planner.accel_controller_profile, expected)
+      self.assertEqual(planner.params.get("AccelPersonality"), expected)
+
+  def test_planner_refreshes_controller_params_every_five_frames(self):
+    planner = planner_for_hook(enabled=False)
+    planner._radar_log_mono_time = None
+    planner.events_sp = SimpleNamespace(clear=lambda: None)
+    planner.dec.update = lambda sm: None
+    planner.e2e_alerts_helper = SimpleNamespace(update=lambda sm, events: None)
+    calls = []
+    sm = PlannerSM()
+    planner.read_accel_controller_params = lambda: calls.append(sm.frame)
+
+    for frame in range(10):
+      sm.frame = frame
+      sm.logMonoTime["radarState"] = frame
+      planner.update(sm)
+
+    self.assertEqual(calls, [0, 5])
+
+  def test_schema_contract_and_round_trip(self):
+    fields = custom.LongitudinalPlanSP.AccelController.schema.fields
+    self.assertEqual(
+      {name: field.proto.ordinal.explicit for name, field in fields.items()},
+      {"enabled": 0, "active": 1, "shadowOnlyDEPRECATED": 2, "profile": 3, "state": 4},
+    )
+    self.assertEqual(
+      custom.LongitudinalPlanSP.AccelController.Profile.schema.enumerants,
+      {"eco": 0, "normal": 1, "sport": 2},
+    )
+
+    message = custom.LongitudinalPlanSP.new_message()
+    message.accelController.enabled = True
+    message.accelController.active = True
+    message.accelController.profile = custom.LongitudinalPlanSP.AccelController.Profile.sport
+    message.accelController.state = AccelControllerState.stopHold
+    with custom.LongitudinalPlanSP.from_bytes(message.to_bytes()) as reader:
+      self.assertTrue(reader.accelController.enabled)
+      self.assertTrue(reader.accelController.active)
+      self.assertEqual(reader.accelController.profile, custom.LongitudinalPlanSP.AccelController.Profile.sport)
+      self.assertEqual(reader.accelController.state, AccelControllerState.stopHold)
+
+  def test_controller_telemetry_is_published(self):
+    planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
+    dynamic_planner: Any = planner
+    dynamic_planner.source = LongitudinalPlanSource.cruise
+    dynamic_planner.output_v_target = 20.0
+    dynamic_planner.output_a_target = -0.1
+    dynamic_planner.events_sp = SimpleNamespace(to_msg=list)
+    dynamic_planner.dec = SimpleNamespace(mode=lambda: "acc", enabled=lambda: False, active=lambda: False)
+    dynamic_planner.accel_controller = accel_controller()
+    dynamic_planner.accel_controller_available = True
+    dynamic_planner.accel_controller_enabled = True
+    dynamic_planner.accel_controller_profile = AccelProfile.sport
+    dynamic_planner.accel_controller.state = AccelControllerState.restrict
+    dynamic_planner.scc = SimpleNamespace(
+      vision=SimpleNamespace(
+        state=0,
+        output_v_target=20.0,
+        output_a_target=0.0,
+        current_lat_acc=0.0,
+        max_pred_lat_acc=0.0,
+        is_enabled=False,
+        is_active=False,
+      ),
+      map=SimpleNamespace(state=0, output_v_target=20.0, output_a_target=0.0, is_enabled=False, is_active=False),
+    )
+    dynamic_planner.resolver = SimpleNamespace(
+      speed_limit=0.0,
+      speed_limit_last=0.0,
+      speed_limit_final=0.0,
+      speed_limit_final_last=0.0,
+      speed_limit_valid=False,
+      speed_limit_last_valid=False,
+      speed_limit_offset=0.0,
+      distance=0.0,
+      source=custom.LongitudinalPlanSP.SpeedLimit.Source.none,
+    )
+    dynamic_planner.sla = SimpleNamespace(
+      state=custom.LongitudinalPlanSP.SpeedLimit.AssistState.disabled,
+      is_enabled=False,
+      is_active=False,
+      output_v_target=20.0,
+      output_a_target=0.0,
+    )
+    dynamic_planner.e2e_alerts_helper = SimpleNamespace(green_light_alert=False, lead_depart_alert=False)
+    sent = {}
+
+    dynamic_planner.publish_longitudinal_plan_sp(
+      PlannerSM(),
+      SimpleNamespace(send=lambda service, message: sent.update({service: message})),
+    )
+
+    telemetry = sent["longitudinalPlanSP"].longitudinalPlanSP.accelController
+    self.assertTrue(telemetry.enabled)
+    self.assertTrue(telemetry.active)
+    self.assertEqual(telemetry.profile, AccelProfile.sport)
+    self.assertEqual(telemetry.state, AccelControllerState.restrict)
+    self.assertEqual(
+      set(custom.LongitudinalPlanSP.AccelController.schema.fields),
+      {"enabled", "active", "shadowOnlyDEPRECATED", "profile", "state"},
+    )

openpilot/sunnypilot/selfdrive/controls/lib/dec/constants.py

@@ -1,4 +1,6 @@
 class WMACConstants:
+  MODEL_ACCEL_TRANSITION_RATE = 3.0
+
   # Lead detection parameters
   LEAD_WINDOW_SIZE = 6  # Stable detection window
   LEAD_PROB = 0.45  # Balanced threshold for lead detection

openpilot/sunnypilot/selfdrive/controls/lib/dec/dec.py

@@ -6,13 +6,15 @@ See the LICENSE.md file in the root directory for more details.
 """
 # Version = 2025-6-30
 
+import math
+from typing import Literal
+
 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
 
 # d-e2e, from modeldata.h
 TRAJECTORY_SIZE = 33
@@ -130,6 +132,51 @@ class ModeTransitionManager:
     return self.current_mode
 
 
+class ModelAccelTransition:
+  """Smooths acceleration while DEC changes modes."""
+
+  def __init__(self, dt: float = DT_MDL):
+    self._max_step = WMACConstants.MODEL_ACCEL_TRANSITION_RATE * dt
+    self._accel = 0.0
+    self._active = False
+    self._blended = False
+
+  def reset(self) -> None:
+    self._active = False
+    self._blended = False
+
+  @property
+  def active(self) -> bool:
+    return self._active
+
+  def update(self, mpc_accel: float, model_accel: float, previous_accel: float, *, blended: bool,
+             urgent: bool = False, reset: bool = False) -> float:
+    selected_accel = min(mpc_accel, model_accel) if blended else mpc_accel
+    if reset or not all(math.isfinite(accel) for accel in (mpc_accel, model_accel, previous_accel)):
+      self.reset()
+      return selected_accel
+
+    if blended != self._blended:
+      self._accel = previous_accel
+      self._active = True
+      self._blended = blended
+
+    if urgent and selected_accel <= self._accel:
+      self._accel = selected_accel
+      self._active = True
+      return selected_accel
+    if not self._active:
+      return selected_accel
+
+    transition_target = model_accel if blended else mpc_accel
+    preview_accel = max(self._accel - self._max_step, min(self._accel + self._max_step, transition_target))
+    output_accel = min(mpc_accel, preview_accel)
+    self._accel = output_accel
+    if mpc_accel >= transition_target and math.isclose(preview_accel, transition_target, abs_tol=1e-9):
+      self._active = False
+    return output_accel
+
+
 class DynamicExperimentalController:
   def __init__(self, CP: structs.CarParams, mpc, params=None):
     self._CP = CP

openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/test_dec_planner_gate.py

@@ -33,11 +33,11 @@ class MockDec:
   def enabled(self) -> bool:
     return True
 
-
 class MockSubMaster(dict):
   def __init__(self, services: dict):
     super().__init__(services)
     self.valid = dict.fromkeys(services, True)
+    self.alive = dict.fromkeys(services, True)
     self.logMonoTime = dict.fromkeys(services, 0)
     self.updated = dict.fromkeys(services, True)
     self.recv_frame = dict.fromkeys(services, 1)

openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/test_dynamic_controller.py

@@ -1,5 +1,7 @@
+from openpilot.common.realtime import DT_MDL
 from openpilot.common.test import OpenpilotTestCase
-from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
+from openpilot.sunnypilot.selfdrive.controls.lib.dec.constants import WMACConstants
+from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController, ModelAccelTransition
 
 class MockLeadOne:
   def __init__(self, present=0.0):
@@ -89,3 +91,56 @@ class TestDynamicExperimentalController(OpenpilotTestCase):
       controller.update(default_sm)
 
     assert controller.mode() == "blended"
+
+
+class TestModelAccelTransition(OpenpilotTestCase):
+  def test_blended_entry_is_rate_bounded(self):
+    transition = ModelAccelTransition()
+    previous = 0.30
+    outputs = []
+    for _ in range(10):
+      previous = transition.update(0.50, -0.88, previous, blended=True)
+      outputs.append(previous)
+
+    max_step = WMACConstants.MODEL_ACCEL_TRANSITION_RATE * DT_MDL
+    self.assertAlmostEqual(outputs[0], 0.15)
+    assert min(b - a for a, b in zip([0.30, *outputs[:-1]], outputs, strict=True)) >= -max_step - 1e-9
+    self.assertAlmostEqual(outputs[-1], -0.88)
+
+  def test_harder_mpc_braking_is_immediate(self):
+    transition = ModelAccelTransition()
+    self.assertAlmostEqual(transition.update(-2.0, -0.88, 0.30, blended=True), -2.0)
+
+  def test_urgent_model_braking_is_immediate(self):
+    transition = ModelAccelTransition()
+    self.assertAlmostEqual(transition.update(0.0, -2.0, 0.30, blended=True, urgent=True), -2.0)
+    self.assertAlmostEqual(transition.update(0.0, 0.0, -2.0, blended=True), -1.85)
+
+  def test_harder_mpc_release_is_rate_bounded(self):
+    transition = ModelAccelTransition()
+    self.assertAlmostEqual(transition.update(-2.0, -0.5, 0.30, blended=True), -2.0)
+    self.assertAlmostEqual(transition.update(0.0, -0.5, -2.0, blended=True), -1.85)
+
+  def test_entry_and_exit_are_rate_bounded(self):
+    transition = ModelAccelTransition()
+    output = 0.30
+    for _ in range(20):
+      output = transition.update(0.0, -0.88, output, blended=True)
+      if output <= -0.88:
+        break
+    else:
+      self.fail("transition did not converge")
+
+    output = transition.update(0.0, -1.50, output, blended=True)
+    self.assertAlmostEqual(output, -1.50)
+    self.assertAlmostEqual(transition.update(0.0, -0.20, output, blended=False), -1.35)
+
+  def test_harder_mpc_braking_bypasses_exit_ramp(self):
+    transition = ModelAccelTransition()
+    self.assertAlmostEqual(transition.update(2.0, 0.76, 0.76, blended=True), 0.76)
+    self.assertAlmostEqual(transition.update(-2.0, 0.76, 0.76, blended=False), -2.0)
+
+  def test_urgent_release_remains_rate_bounded(self):
+    transition = ModelAccelTransition()
+    self.assertAlmostEqual(transition.update(-1.0, -1.0, -1.0, blended=True), -1.0)
+    self.assertAlmostEqual(transition.update(1.0, -1.0, -1.0, blended=False, urgent=True), -0.85)

openpilot/sunnypilot/selfdrive/controls/lib/longcontrol.py

@@ -0,0 +1,101 @@
+"""
+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 typing import cast
+
+from opendbc.car import DT_CTRL
+
+STOPPING_DISTANCE = 0.75
+STOPPING_TIME = 2.5
+STOPPING_ACCEL_TOLERANCE = 0.1
+STOPPING_SPEED_TOLERANCE = 0.05
+STOPPING_SETTLE_FRAMES = 30
+STOPPING_HOLD_ACCEL = -1.2
+STOPPING_HOLD_MARGIN = 0.6
+STOPPING_HOLD_SPEED_TOLERANCE = 0.01
+
+
+class LongControlSP:
+  def __init__(self):
+    self._stopping_settle_frames: int | None = None
+    self._stopping_hold_accel: float | None = None
+
+  def _hold_supported(self) -> bool:
+    return self.CP.openpilotLongitudinalControl and not self.CP.notCar and self.CP.stopAccel < 0.0
+
+  def update_state(self, stopping: bool, active: bool, CS) -> None:
+    if not active:
+      self._stopping_settle_frames = None
+      self._stopping_hold_accel = None
+      return
+
+    invalid_speed = not all(math.isfinite(speed) for speed in (CS.vEgo, CS.vEgoRaw))
+    moving = max(abs(CS.vEgo), abs(CS.vEgoRaw)) > STOPPING_SPEED_TOLERANCE
+    if invalid_speed or (not stopping and moving):
+      self._stopping_hold_accel = None
+    elif (self._hold_supported() and math.isfinite(self.last_output_accel)
+          and self.last_output_accel <= self.CP.stopAccel):
+      previous_hold = self._stopping_hold_accel if self._stopping_hold_accel is not None else self.last_output_accel
+      self._stopping_hold_accel = min(self.last_output_accel, previous_hold)
+    if not stopping:
+      self._stopping_settle_frames = None
+      if self._stopping_hold_accel is not None and math.isfinite(self.last_output_accel):
+        self._stopping_hold_accel = min(self.last_output_accel, self._stopping_hold_accel)
+
+  def stopping_accel(self, output_accel: float, CS) -> float:
+    if self._stopping_hold_accel is not None and math.isfinite(CS.vEgo) and abs(CS.vEgo) <= STOPPING_SPEED_TOLERANCE:
+      return min(output_accel, self._stopping_hold_accel)
+    return output_accel
+
+  def stopping_decel_rate(self, CS, a_target: float, output_accel: float) -> float:
+    if not all(math.isfinite(value) for value in (output_accel, a_target, CS.vEgo, CS.vEgoRaw, CS.aEgo)):
+      return 1.0
+    hold_supported = self._hold_supported()
+    preserving_hold = self._stopping_hold_accel is not None
+    can_hold = output_accel <= 0.0 and a_target >= output_accel
+    terminal_speed = (0.0 <= CS.vEgo <= STOPPING_SPEED_TOLERANCE
+                      or CS.standstill and abs(CS.vEgo) <= STOPPING_SPEED_TOLERANCE)
+    positive_stop_entry = self.last_output_accel > 0.0 and output_accel == 0.0
+    if output_accel > 0.0 or positive_stop_entry or CS.vEgo < 0.0 and not terminal_speed:
+      return 1.0
+    if terminal_speed and self._stopping_settle_frames is None:
+      if not preserving_hold and (not can_hold or output_accel > -STOPPING_ACCEL_TOLERANCE or CS.aEgo >= -STOPPING_ACCEL_TOLERANCE):
+        return 1.0
+      self._stopping_settle_frames = 0
+
+    time_decel = 0.0 if self._stopping_settle_frames is not None else CS.vEgo / STOPPING_TIME
+    required_decel = max(time_decel, CS.vEgo ** 2 / (2.0 * STOPPING_DISTANCE), 1e-3)
+    adequacy = min(max(-CS.aEgo / required_decel, 0.0), 1.0)
+    planner_need = min(max((output_accel - a_target) / max(required_decel, STOPPING_ACCEL_TOLERANCE), 0.0), 1.0)
+    if not terminal_speed and self._stopping_settle_frames is None and can_hold and adequacy >= 1.0:
+      self._stopping_settle_frames = 0
+      if hold_supported:
+        self._stopping_hold_accel = output_accel
+
+    motion_need = 1.0 - adequacy ** 2
+    terminal_need = 0.0
+    if terminal_speed or self._stopping_settle_frames not in (None, 0):
+      settle_frames = cast(int, self._stopping_settle_frames)
+      self._stopping_settle_frames = min(settle_frames + 1, STOPPING_SETTLE_FRAMES)
+      terminal_need = (self._stopping_settle_frames / STOPPING_SETTLE_FRAMES) ** 2
+
+    if preserving_hold and self._stopping_hold_accel is not None:
+      self._stopping_hold_accel = min(output_accel, self._stopping_hold_accel)
+      if terminal_speed:
+        minimum_hold = min(STOPPING_HOLD_ACCEL, self.CP.stopAccel + STOPPING_HOLD_MARGIN)
+        hold_target = max(self.CP.stopAccel, min(minimum_hold, self._stopping_hold_accel))
+        if CS.aEgo > STOPPING_ACCEL_TOLERANCE or abs(CS.vEgoRaw) > STOPPING_HOLD_SPEED_TOLERANCE:
+          return 1.0
+        hold_rate = max(planner_need, terminal_need)
+        if CS.vEgoRaw == 0.0 and abs(CS.vEgo) <= STOPPING_HOLD_SPEED_TOLERANCE:
+          if output_accel <= hold_target:
+            return planner_need
+          hold_rate = max(planner_need, min(hold_rate, (output_accel - hold_target) / DT_CTRL))
+        return hold_rate
+
+    return max(motion_need, planner_need, terminal_need)

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

@@ -8,8 +8,14 @@ See the LICENSE.md file in the root directory for more details.
 from openpilot.cereal import messaging, custom
 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.dec.dec import DynamicExperimentalController
+from openpilot.common.params import Params
+from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET
+from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalPlanSource as MpcSource
+from openpilot.sunnypilot import get_sanitize_int_param
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import AccelProfile
+from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController, ModelAccelTransition
 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
 from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import SpeedLimitAssist
@@ -23,18 +29,32 @@ LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource
 
 class LongitudinalPlannerSP:
   def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP, mpc):
+    self.accel_controller = AccelController(CP, dt=mpc.dt)
+    self.accel_controller_available = bool(CP.openpilotLongitudinalControl)
+    self.accel_controller_enabled = False
+    self.accel_controller_profile = AccelProfile.normal
+    self.params = Params()
+    self.read_accel_controller_params()
     self.events_sp = EventsSP()
-    self.resolver = SpeedLimitResolver()
     self.dec = DynamicExperimentalController(CP, mpc)
+    self.model_accel_transition = ModelAccelTransition(mpc.dt)
     self.scc = SmartCruiseControl()
     self.resolver = SpeedLimitResolver()
     self.sla = SpeedLimitAssist(CP, CP_SP)
     self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None
     self.source = LongitudinalPlanSource.cruise
     self.e2e_alerts_helper = E2EAlertsHelper()
+    self._radar_log_mono_time = None
+    self._radar_fresh_this_cycle = True
+    self._radar_healthy_this_cycle = True
 
     self.output_v_target = 0.
     self.output_a_target = 0.
+    self.previous_plan_accel = 0.
+
+  def read_accel_controller_params(self) -> None:
+    self.accel_controller_enabled = self.params.get_bool("AccelPersonalityEnabled")
+    self.accel_controller_profile = get_sanitize_int_param("AccelPersonality", AccelProfile.eco, AccelProfile.sport, self.params)
 
   def is_e2e(self, sm: messaging.SubMaster) -> bool:
     experimental_mode = sm['selfdriveState'].experimentalMode
@@ -43,6 +63,73 @@ class LongitudinalPlannerSP:
 
     return experimental_mode and self.dec.mode() == "blended"
 
+  def select_model_accel(self, mpc_accel: float, model_accel: float, *, blended: bool,
+                         should_stop: bool, fcw: bool, reset: bool) -> float:
+    return self.model_accel_transition.update(
+      mpc_accel, model_accel, self.previous_plan_accel, blended=blended, urgent=should_stop or fcw, reset=reset or not self.dec.active(),
+    )
+
+  def update_accel_controller(self, sm: messaging.SubMaster, candidates):
+    if not candidates:
+      return candidates
+    cruise_index = next((i for i, candidate in enumerate(candidates[1:], start=1) if candidate[1] == MpcSource.cruise), -1)
+    if cruise_index < 0:
+      return candidates
+
+    CS = sm['carState']
+    long_control_off = sm['controlsState'].longControlState == LongCtrlState.off
+    reset_state = ((long_control_off if self.accel_controller_available else not sm['selfdriveState'].enabled)
+                   or CS.vCruise == V_CRUISE_UNSET)
+    cruise_accel = candidates[cruise_index][0]
+    mpc_accel = candidates[0][0]
+    mpc_source = candidates[0][1]
+    radar_state = sm['radarState']
+    lead_one_present = bool(getattr(getattr(radar_state, 'leadOne', None), 'present', False))
+    lead_two_present = bool(getattr(getattr(radar_state, 'leadTwo', None), 'present', False))
+    stock_mpc_lead = 0 if mpc_source == MpcSource.lead0 and lead_one_present else 1 if mpc_source == MpcSource.lead1 and lead_two_present else -1
+    stock_should_stop = any(candidate[2] for candidate in candidates)
+    transitioning_from_e2e = self.model_accel_transition.active
+    acc_selected = not self.is_e2e(sm) and not transitioning_from_e2e
+    active = not reset_state and acc_selected and not sm['controlsState'].forceDecel
+    radar_valid = self._radar_fresh_this_cycle and self._radar_healthy_this_cycle
+    if not self.accel_controller_available or not self.accel_controller_enabled or not active:
+      self.accel_controller.reset()
+      if self.accel_controller_available and self.accel_controller_enabled and acc_selected and reset_state:
+        self.a_cruise = 0.0
+        if cruise_accel > 0.0:
+          candidates = list(candidates)
+          _, source, stop = candidates[cruise_index]
+          candidates[cruise_index] = (0.0, source, stop)
+      return candidates
+
+    decision = self.accel_controller.update(
+      radar_state, v_ego=CS.vEgo, a_ego=CS.aEgo, v_cruise=self.output_v_target, follow_personality=sm['selfdriveState'].personality,
+      radar_valid=radar_valid, stock_cruise_accel=cruise_accel, stock_mpc_accel=mpc_accel, stock_should_stop=stock_should_stop,
+      fcw=self.fcw, standstill=CS.standstill, stock_mpc_lead=stock_mpc_lead, previous_plan_accel=self.previous_plan_accel,
+      profile=self.accel_controller_profile,
+    )
+    if decision.a_target is None:
+      return candidates
+
+    controller_source = (MpcSource.lead1 if self.accel_controller.selected_lead == 1 else
+                         MpcSource.lead0 if self.accel_controller.selected_lead == 0 else MpcSource.cruise)
+    controller_candidate = (decision.a_target, controller_source, decision.should_stop)
+    if not decision.stock_safety_required:
+      return [controller_candidate]
+
+    stock_candidate = min(candidates, key=lambda candidate: candidate[0])
+    stock_safety_candidate = (stock_candidate[0], stock_candidate[1], stock_should_stop)
+    return [controller_candidate, stock_safety_candidate]
+
+  def _update_radar_freshness(self, sm: messaging.SubMaster) -> bool:
+    radar_log_mono_time = sm.logMonoTime['radarState']
+    radar_healthy = sm.valid['radarState'] and sm.alive['radarState']
+    self._radar_healthy_this_cycle = radar_healthy
+    radar_advanced = self._radar_log_mono_time is None or radar_log_mono_time > self._radar_log_mono_time
+    if radar_advanced:
+      self._radar_log_mono_time = radar_log_mono_time
+    return radar_healthy and radar_advanced
+
   def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]:
     CS = sm['carState']
     v_cruise_cluster_kph = min(CS.vCruiseCluster, V_CRUISE_MAX)
@@ -74,6 +161,9 @@ class LongitudinalPlannerSP:
     return self.output_v_target, self.output_a_target
 
   def update(self, sm: messaging.SubMaster) -> None:
+    self._radar_fresh_this_cycle = self._update_radar_freshness(sm)
+    if sm.frame % 5 == 0:
+      self.read_accel_controller_params()
     self.events_sp.clear()
     self.dec.update(sm)
     self.e2e_alerts_helper.update(sm, self.events_sp)
@@ -95,6 +185,12 @@ class LongitudinalPlannerSP:
     dec.enabled = self.dec.enabled()
     dec.active = self.dec.active()
 
+    accel_controller = longitudinalPlanSP.accelController
+    accel_controller.enabled = self.accel_controller_available and self.accel_controller_enabled
+    accel_controller.active = self.accel_controller.is_active
+    accel_controller.profile = self.accel_controller_profile
+    accel_controller.state = self.accel_controller.state
+
     # Smart Cruise Control
     smartCruiseControl = longitudinalPlanSP.smartCruiseControl
     # Vision Control

openpilot/sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_vision_controller.py

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

openpilot/sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_vision_controller_closed_loop.py

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

openpilot/sunnypilot/selfdrive/controls/lib/smart_cruise_control/vision_controller.py

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

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

@@ -0,0 +1,396 @@
+from contextlib import contextmanager
+import inspect
+from typing import Any
+from unittest import mock
+
+import numpy as np
+
+from opendbc.car.interfaces import ACCEL_MAX, ACCEL_MIN
+from openpilot.common.test import OpenpilotTestCase
+from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import N, LongitudinalMpc, LongitudinalPlanSource
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import ACCEL_V, LAUNCH_ACCEL, NEUTRAL_ACCEL, AccelProfile
+from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PRIUS_TSS2_ROUTE_MODEL, PlantSP as Plant
+
+
+def configure(plant, *, enabled=True, profile=AccelProfile.normal):
+  planner: Any = plant.planner
+  planner.accel_controller_enabled = enabled
+  planner.accel_controller_profile = profile
+  planner.read_accel_controller_params = lambda: None
+  dec: Any = plant.planner.dec
+  dec._enabled = False
+  dec._read_params = lambda: None
+
+
+def record_candidates(plant):
+  snapshots = []
+  planner: Any = plant.planner
+  original = planner.update_accel_controller
+
+  def wrapper(sm, candidates):
+    before = tuple(candidates)
+    after = original(sm, candidates)
+    snapshots.append((before, tuple(after)))
+    return after
+
+  planner.update_accel_controller = wrapper
+  return snapshots
+
+
+@contextmanager
+def scripted_stock_candidates(plant, *, mpc_accel, cruise_accel, source=LongitudinalPlanSource.cruise):
+  values = {"mpc": float(mpc_accel), "cruise": float(cruise_accel)}
+
+  def update_mpc(_radar_state, personality):
+    plant.planner.mpc.source = source
+    plant.planner.mpc.crash_cnt = 0
+
+  with (
+    mock.patch.object(plant.planner.mpc, "update", side_effect=update_mpc),
+    mock.patch(
+      "openpilot.selfdrive.controls.lib.longitudinal_planner.get_accel_from_plan",
+      side_effect=lambda *_args, **_kwargs: values["mpc"],
+    ),
+    mock.patch(
+      "openpilot.selfdrive.controls.lib.longitudinal_planner.get_cruise_accel",
+      side_effect=lambda *_args, **_kwargs: values["cruise"],
+    ),
+  ):
+    yield values
+
+
+class TestAccelControllerPlannerIntegration(OpenpilotTestCase):
+  def test_one_stock_mpc_solve_with_unmodified_bounds_and_api(self):
+    plant = Plant(enabled=True, lead_relevancy=True, speed=20.0, distance_lead=70.0)
+    configure(plant)
+    mpc: Any = plant.planner.mpc
+    original_run = mpc.run
+    run_calls = []
+    params_at_solve = []
+
+    def count_run():
+      run_calls.append(None)
+      params_at_solve.append(mpc.params.copy())
+      return original_run()
+
+    mpc.run = count_run
+    result = plant.step(v_lead=14.0, v_cruise=25.0)
+
+    self.assertEqual(len(run_calls), 1)
+    self.assertTrue(np.isfinite(result["a_target"]))
+    self.assertEqual(LongitudinalMpc.__bases__, (object,))
+    self.assertEqual(tuple(inspect.signature(LongitudinalMpc.update).parameters), ("self", "radarstate", "personality"))
+    self.assertFalse(hasattr(mpc, "set_jerk_cost_multiplier"))
+    self.assertFalse(hasattr(mpc, "cruise_accel_max"))
+    self.assertEqual(params_at_solve[0].shape, (N + 1, 6))
+    np.testing.assert_array_equal(params_at_solve[0][:, 0], ACCEL_MIN)
+    np.testing.assert_array_equal(params_at_solve[0][:, 1], ACCEL_MAX)
+
+  def test_stock_lead_mpc_braking_remains_authoritative(self):
+    plant = Plant(enabled=True, lead_relevancy=True, speed=20.0, distance_lead=30.0)
+    configure(plant)
+    snapshots = record_candidates(plant)
+
+    stock_mpc_won = False
+    for _ in range(20):
+      result = plant.step(v_lead=5.0, v_cruise=30.0)
+      stock, augmented = snapshots[-1]
+      mpc_candidate = stock[0]
+      selected = min(augmented, key=lambda candidate: candidate[0])
+      self.assertAlmostEqual(result["a_target"], selected[0])
+      if mpc_candidate[0] < 0.0 and selected == mpc_candidate:
+        stock_mpc_won = True
+        assert mpc_candidate[1] in (LongitudinalPlanSource.lead0, LongitudinalPlanSource.lead1)
+        break
+
+    self.assertTrue(stock_mpc_won, "the hook must never mask stock lead braking")
+
+  def test_stock_should_stop_survives_controller_candidates(self):
+    plant = Plant(enabled=True, lead_relevancy=True, speed=0.2, distance_lead=3.0)
+    configure(plant)
+    snapshots = record_candidates(plant)
+
+    result = plant.step(v_lead=0.0, v_cruise=8.0)
+    stock, augmented = snapshots[-1]
+    self.assertTrue(any(candidate[2] for candidate in stock))
+    self.assertTrue(any(candidate[2] for candidate in augmented))
+    self.assertTrue(result["should_stop"])
+    self.assertAlmostEqual(result["a_target"], min(augmented, key=lambda candidate: candidate[0])[0])
+
+  def test_dec_blended_entry_limits_the_first_model_brake_step(self):
+    class DecStub:
+      mode_name = "acc"
+
+      def update(self, _sm):
+        pass
+
+      def active(self):
+        return True
+
+      def mode(self):
+        return self.mode_name
+
+    plant = Plant(
+      enabled=True, e2e=True, speed=22.0,
+      model_action_fn=lambda _current_time, _v_ego, _a_ego: (-2.0, False),
+      actuator_delay=0.15, actuator_lag=0.20,
+    )
+    configure(plant)
+    dec = DecStub()
+    planner: Any = plant.planner
+    planner.dec = dec
+
+    outputs = []
+    for frame in range(20):
+      if frame == 10:
+        dec.mode_name = "blended"
+      result = plant.step(v_lead=0.0, v_cruise=22.0)
+      outputs.append(result["a_target"])
+
+    self.assertGreaterEqual(outputs[10] - outputs[9], -0.15 - 1e-9)
+    self.assertFalse(result["fcw"])
+
+  def test_model_endpoint_urgency_does_not_bypass_entry_ramp(self):
+    plant = Plant(enabled=True, e2e=True, speed=21.0)
+    planner: Any = plant.planner
+    planner.dec._active = True
+    planner.dec._has_slow_down = True
+    planner.dec._urgency = 1.0
+    planner.previous_plan_accel = -0.136
+
+    first = planner.select_model_accel(0.0, -1.140, blended=True, should_stop=False, fcw=False, reset=False)
+    second = planner.select_model_accel(0.0, -1.140, blended=True, should_stop=False, fcw=False, reset=False)
+
+    self.assertAlmostEqual(first, -0.286)
+    self.assertAlmostEqual(second, -0.436)
+
+  def test_dec_exit_limits_positive_acceleration_step(self):
+    plant = Plant(enabled=True, e2e=True, speed=6.0)
+    planner: Any = plant.planner
+    planner.dec._active = True
+    planner.previous_plan_accel = 0.76
+
+    self.assertAlmostEqual(planner.select_model_accel(1.94, 0.76, blended=True, should_stop=False, fcw=False, reset=False), 0.76)
+    first = planner.select_model_accel(1.94, 0.76, blended=False, should_stop=False, fcw=False, reset=False)
+    second = planner.select_model_accel(1.94, 0.76, blended=False, should_stop=False, fcw=False, reset=False)
+
+    self.assertAlmostEqual(first, 0.91)
+    self.assertAlmostEqual(second, 1.06)
+
+  def test_dec_exit_transition_reaches_the_final_candidate_list(self):
+    class DecStub:
+      mode_name = "blended"
+
+      def update(self, _sm):
+        pass
+
+      def active(self):
+        return True
+
+      def mode(self):
+        return self.mode_name
+
+    plant = Plant(
+      enabled=True, e2e=True, speed=22.0,
+      model_action_fn=lambda _current_time, _v_ego, _a_ego: (0.20, False),
+    )
+    configure(plant, enabled=False)
+    dec = DecStub()
+    planner: Any = plant.planner
+    planner.dec = dec
+
+    def update_mpc(_radar_state, personality):
+      plant.planner.mpc.source = LongitudinalPlanSource.cruise
+      plant.planner.mpc.crash_cnt = 0
+
+    with (
+      mock.patch.object(plant.planner.mpc, "update", side_effect=update_mpc),
+      mock.patch("openpilot.selfdrive.controls.lib.longitudinal_planner.get_accel_from_plan", return_value=1.94),
+      mock.patch("openpilot.selfdrive.controls.lib.longitudinal_planner.get_cruise_accel", return_value=1.94),
+    ):
+      outputs = [plant.step(v_lead=0.0, v_cruise=30.0)["a_target"] for _ in range(4)]
+      self.assertAlmostEqual(outputs[-1], 0.20)
+
+      dec.mode_name = "acc"
+      first = plant.step(v_lead=0.0, v_cruise=30.0)["a_target"]
+      second = plant.step(v_lead=0.0, v_cruise=30.0)["a_target"]
+
+    self.assertLessEqual(first - outputs[-1], 0.15 + 1e-9)
+    self.assertLessEqual(second - first, 0.15 + 1e-9)
+    self.assertEqual(plant.planner.mpc.source, LongitudinalPlanSource.cruise)
+
+  def test_disengaged_cruise_state_cannot_leak_into_first_accel(self):
+    plant = Plant(enabled=False, speed=10.0)
+    configure(plant)
+
+    with (
+      mock.patch.object(plant.planner.mpc, "update", return_value=None),
+      mock.patch("openpilot.selfdrive.controls.lib.longitudinal_planner.get_accel_from_plan", return_value=1.30),
+    ):
+      for _ in range(12):
+        self.assertAlmostEqual(plant.step(v_lead=0.0, v_cruise=30.0)["a_target"], 0.0)
+        self.assertAlmostEqual(plant.planner.a_cruise, 0.0)
+
+      plant.enabled = True
+      first = plant.step(v_lead=0.0, v_cruise=30.0)["a_target"]
+
+    self.assertGreater(first, 0.0)
+    self.assertLessEqual(first, 0.10)
+
+
+class TestAccelControllerClosedLoopAcceptance(OpenpilotTestCase):
+  def test_every_profile_launches_promptly_after_confirmed_departure(self):
+    for profile in ACCEL_V:
+      plant = Plant(enabled=True, lead_relevancy=True, speed=0.0, distance_lead=6.0, actuator_model=PRIUS_TSS2_ROUTE_MODEL, run_long_control=True)
+      configure(plant, profile=profile)
+      with scripted_stock_candidates(plant, mpc_accel=-0.5, cruise_accel=1.6, source=LongitudinalPlanSource.lead0) as stock:
+        for _ in range(12):
+          held = plant.step(v_lead=0.0, v_cruise=8.0)
+        self.assertTrue(held["should_stop"])
+        self.assertLess(held["actuator_command"], 0.0)
+
+        stock["mpc"] = 1.6
+        unconfirmed = plant.step(v_lead=1.0, v_cruise=8.0)
+        confirmed = plant.step(v_lead=1.0, v_cruise=8.0)
+
+        self.assertTrue(unconfirmed["should_stop"])
+        self.assertFalse(confirmed["should_stop"])
+        self.assertEqual(confirmed["a_target"], LAUNCH_ACCEL)
+        self.assertGreater(confirmed["actuator_command"], 0.0)
+        self.assertEqual(confirmed["long_control_state"], LongCtrlState.pid)
+
+        moving = confirmed
+        for _ in range(12):
+          moving = plant.step(v_lead=1.0, v_cruise=8.0)
+          if moving["speed"] >= 0.01:
+            break
+        self.assertGreaterEqual(moving["speed"], 0.01, profile)
+
+  def test_slower_lead_causes_routine_decel_while_ttc_is_still_long(self):
+    initial_gap = 55.0
+    initial_ego_speed = 20.0
+    lead_speed = 14.0
+    plant = Plant(enabled=True, lead_relevancy=True, speed=initial_ego_speed, distance_lead=initial_gap)
+    configure(plant)
+
+    with scripted_stock_candidates(plant, mpc_accel=0.7, cruise_accel=0.7):
+      result = plant.step(v_lead=lead_speed, v_cruise=30.0)
+
+    self.assertGreater(initial_gap / (initial_ego_speed - lead_speed), 8.0)
+    self.assertTrue(result["controller_active"])
+    self.assertLess(result["a_target"], 0.0)
+
+  def test_monotonically_slowing_lead_does_not_cause_brake_gas_brake(self):
+    plant = Plant(
+      enabled=True,
+      lead_relevancy=True,
+      speed=18.0,
+      distance_lead=65.0,
+      actuator_delay=0.15,
+      actuator_lag=0.20,
+    )
+    configure(plant)
+    targets = []
+
+    with scripted_stock_candidates(plant, mpc_accel=0.8, cruise_accel=0.8):
+      for frame in range(80):
+        lead_speed = max(8.0, 20.0 - 0.15 * frame)
+        targets.append(plant.step(v_lead=lead_speed, v_cruise=30.0)["a_target"])
+
+    phases = []
+    for target in targets:
+      phase = 1 if target > NEUTRAL_ACCEL else -1 if target < -NEUTRAL_ACCEL else 0
+      if phase and (not phases or phase != phases[-1]):
+        phases.append(phase)
+
+    self.assertIn(-1, phases)
+    first_brake = phases.index(-1)
+    self.assertNotIn(1, phases[first_brake + 1:])
+
+  def test_terminal_stop_is_bounded_and_has_no_positive_rebound(self):
+    plant = Plant(
+      enabled=True,
+      lead_relevancy=True,
+      speed=5.0,
+      distance_lead=24.0,
+      actuator_model=PRIUS_TSS2_ROUTE_MODEL,
+      run_long_control=True,
+    )
+    configure(plant)
+    samples = []
+
+    with scripted_stock_candidates(plant, mpc_accel=0.8, cruise_accel=0.8):
+      for _ in range(400):
+        result = plant.step(v_lead=0.0, v_cruise=8.0)
+        samples.append(result)
+        if result["speed"] == 0.0:
+          break
+
+    self.assertEqual(samples[-1]["speed"], 0.0)
+    self.assertTrue(samples[-1]["should_stop"])
+    self.assertGreaterEqual(plant.distance_lead - plant.distance, 5.8)
+    self.assertLessEqual(plant.distance_lead - plant.distance, 7.1)
+
+    moving_samples = samples[:-1]
+    terminal_samples = [sample for sample in moving_samples if sample["speed"] <= 0.3]
+    self.assertTrue(terminal_samples)
+    self.assertLessEqual(max(sample["a_target"] for sample in terminal_samples), 1e-9)
+    self.assertLessEqual(max(sample["realized_acceleration"] for sample in terminal_samples), 0.02)
+    self.assertGreaterEqual(min(sample["realized_acceleration"] for sample in moving_samples), -1.2)
+
+    realized_jerk = [
+      abs(current["realized_acceleration"] - previous["realized_acceleration"]) / plant.ts
+      for previous, current in zip(moving_samples[:-1], moving_samples[1:], strict=True)
+    ]
+    self.assertLessEqual(max(realized_jerk), 1.2)
+
+  def test_high_speed_stop_uses_planned_decel_before_stock_emergency(self):
+    plant = Plant(enabled=True, lead_relevancy=True, speed=20.0, distance_lead=120.0, actuator_model=PRIUS_TSS2_ROUTE_MODEL, run_long_control=True)
+    configure(plant)
+    samples = []
+
+    with scripted_stock_candidates(plant, mpc_accel=0.8, cruise_accel=0.8):
+      for _ in range(800):
+        result = plant.step(v_lead=0.0, v_cruise=25.0)
+        samples.append(result)
+        if result["speed"] == 0.0 and result["should_stop"]:
+          break
+
+    self.assertEqual(samples[-1]["speed"], 0.0)
+    self.assertTrue(samples[-1]["should_stop"])
+    self.assertGreaterEqual(plant.distance_lead - plant.distance, 5.8)
+    self.assertGreaterEqual(min(sample["a_target"] for sample in samples), -2.5)
+
+  def test_terminal_decel_ceiling_does_not_fade_before_highway_stop(self):
+    plant = Plant(enabled=True, lead_relevancy=True, speed=25.0, distance_lead=150.0, actuator_model=PRIUS_TSS2_ROUTE_MODEL, run_long_control=True)
+    configure(plant)
+    samples = []
+
+    with scripted_stock_candidates(plant, mpc_accel=0.8, cruise_accel=0.8):
+      for _ in range(1000):
+        result = plant.step(v_lead=0.0, v_cruise=30.0)
+        samples.append(result)
+        if result["speed"] == 0.0 and result["should_stop"]:
+          break
+
+    self.assertEqual(samples[-1]["speed"], 0.0)
+    self.assertTrue(samples[-1]["should_stop"])
+    self.assertGreaterEqual(plant.distance_lead - plant.distance, 5.8)
+    self.assertGreaterEqual(min(sample["a_target"] for sample in samples), -2.5)
+
+  def test_feasible_highway_stop_does_not_fall_back_to_late_stock_braking(self):
+    plant = Plant(enabled=True, lead_relevancy=True, speed=25.0, distance_lead=150.0, actuator_model=PRIUS_TSS2_ROUTE_MODEL, run_long_control=True)
+    configure(plant)
+    samples = []
+
+    for _ in range(500):
+      result = plant.step(v_lead=0.0, v_cruise=30.0)
+      samples.append(result)
+      if result["speed"] == 0.0 and result["should_stop"]:
+        break
+
+    self.assertEqual(samples[-1]["speed"], 0.0)
+    self.assertTrue(samples[-1]["should_stop"])
+    self.assertGreaterEqual(plant.distance_lead - plant.distance, 5.8)
+    self.assertGreaterEqual(min(sample["a_target"] for sample in samples), -2.55)

openpilot/sunnypilot/selfdrive/controls/lib/tests/test_longcontrol_sp.py

@@ -0,0 +1,642 @@
+import numpy as np
+from unittest import mock
+
+from opendbc.car import DT_CTRL, gen_empty_fingerprint, structs
+from openpilot.common.parameterized import parameterized
+from openpilot.common.test import OpenpilotTestCase
+from opendbc.car.body.values import CAR as BODY
+from opendbc.car.car_helpers import interfaces
+from opendbc.car.ford.values import CAR as FORD
+from opendbc.car.gm.values import CAR as GM
+from opendbc.car.honda.values import CAR as HONDA
+from opendbc.car.hyundai.values import CAR as HYUNDAI
+from opendbc.car.rivian.values import CAR as RIVIAN
+from opendbc.car.subaru.values import CAR as SUBARU
+from opendbc.car.tesla.values import CAR as TESLA
+from opendbc.car.toyota.values import CAR as TOYOTA
+from opendbc.car.volkswagen.values import CAR as VOLKSWAGEN
+from openpilot.selfdrive.controls.lib.drive_helpers import should_stop
+from openpilot.selfdrive.controls.lib.longcontrol import LongControl, LongCtrlState
+from openpilot.sunnypilot.selfdrive.controls.lib.longcontrol import (
+  STOPPING_HOLD_ACCEL, STOPPING_HOLD_MARGIN, STOPPING_SETTLE_FRAMES, STOPPING_SPEED_TOLERANCE,
+)
+from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PRIUS_TSS2_ROUTE_MODEL, PlantSP
+
+
+PRESERVED_HOLD_VEHICLES = (
+  FORD.FORD_ESCAPE_MK4,
+  GM.CHEVROLET_VOLT,
+  GM.CHEVROLET_BOLT_EUV,
+  HONDA.HONDA_CIVIC_2022,
+  HYUNDAI.HYUNDAI_SONATA,
+  SUBARU.SUBARU_ASCENT,
+  TESLA.TESLA_MODEL_3,
+  TOYOTA.TOYOTA_RAV4_TSS2,
+  VOLKSWAGEN.VOLKSWAGEN_ARTEON_MK1,
+)
+STOP_ACCEL_VEHICLES = (*PRESERVED_HOLD_VEHICLES, RIVIAN.RIVIAN_R1)
+SETTLE_VEHICLES = (TOYOTA.TOYOTA_RAV4_TSS2, HONDA.HONDA_CIVIC_2022, VOLKSWAGEN.VOLKSWAGEN_ARTEON_MK1)
+UNSUPPORTED_HOLD_VEHICLES = (
+  (BODY.COMMA_BODY, True),
+  (SUBARU.SUBARU_OUTBACK, True),
+  (HYUNDAI.HYUNDAI_SONATA, False),
+  (RIVIAN.RIVIAN_R1, True),
+)
+ROUTE_STOP_ONSETS = (
+  (0.280, -0.290, -0.220, -0.220),
+  (0.290, -0.497, -0.270, -0.302),
+  (0.464, -0.223, -0.264, -0.292),
+  (0.467, -0.582, -0.316, -0.359),
+  (0.530, -0.311, -0.309, -0.333),
+  (0.581, -0.467, -0.312, -0.352),
+  (0.398, -0.557, -0.311, -0.348),
+  (0.517, -0.290, -0.301, -0.327),
+  (0.312, -0.420, -0.271, -0.304),
+  (0.474, -0.509, -0.303, -0.347),
+  (0.241, -0.554, -0.573, -0.617),
+  (0.292, -0.154, -0.302, -0.326),
+)
+GRADE_HOLD_CASES = (
+  (-0.49, -1.40),
+  (0.00, -1.40),
+  (0.49, -1.40),
+  (0.75, -1.40),
+  (0.98, -1.65),
+  (1.25, -2.00),
+  (1.47, -2.00),
+)
+
+
+def get_car_params(candidate, experimental_long=True):
+  fingerprint = gen_empty_fingerprint()
+  interface = interfaces[candidate]
+  CP = interface.get_params(candidate, fingerprint, [], experimental_long, False, False)
+  return CP, interface.get_params_sp(CP, candidate, fingerprint, [], experimental_long, False, False)
+
+
+def make_car_state(v_ego=0.2, a_ego=0.0, standstill=False, v_ego_raw=None) -> structs.CarState:
+  raw_speed = v_ego if v_ego_raw is None else v_ego_raw
+  state = structs.CarState(vEgo=float(v_ego), vEgoRaw=float(raw_speed), aEgo=float(a_ego), standstill=standstill)
+  state.cruiseState.standstill = standstill
+  return state
+
+
+def make_control(candidate, initial_accel=-0.33, experimental_long=True):
+  CP, CP_SP = get_car_params(candidate, experimental_long)
+  control = LongControl(CP, CP_SP)
+  control.long_control_state = LongCtrlState.pid
+  control.last_output_accel = initial_accel
+  return CP, control
+
+
+def stock_stopping_output(output_accel, stop_accel):
+  return min(output_accel, 0.0) - DT_CTRL if output_accel > stop_accel else output_accel
+
+
+def expected_hold_accel(CP, initial_accel=-0.33):
+  minimum_hold = min(STOPPING_HOLD_ACCEL, CP.stopAccel + STOPPING_HOLD_MARGIN)
+  return min(initial_accel, max(CP.stopAccel, minimum_hold))
+
+
+def settle_preserved_hold(control):
+  control.update(True, make_car_state(0.28, -0.29), -0.22, True, (-3.5, 2.0))
+  CS = make_car_state(0.0, 0.0, standstill=True)
+  for _ in range(round(4.0 / DT_CTRL)):
+    control.update(True, CS, -0.1, True, (-3.5, 2.0))
+
+
+class TestLongControlSP(OpenpilotTestCase):
+  def test_stop_threshold_matches_the_shared_helper(self):
+    assert should_stop(0.29, 0.0)
+    assert not should_stop(0.3, 0.0)
+    assert not should_stop(0.29, 0.1)
+
+  def test_hold_scope_matches_every_car_interface(self):
+    for candidate in interfaces:
+      for experimental_long in (False, True):
+        with self.subTest(candidate=candidate, experimental_long=experimental_long):
+          CP, control = make_control(candidate, experimental_long=experimental_long)
+          output = control.update(True, make_car_state(0.28, -0.29), -0.22, True, (-3.5, 2.0))
+          supported = CP.openpilotLongitudinalControl and not CP.notCar and CP.stopAccel < 0.0
+
+          self.assertAlmostEqual(output, -0.33)
+          assert (control._stopping_hold_accel is not None) == supported
+
+  @parameterized.expand(UNSUPPORTED_HOLD_VEHICLES, names=("candidate", "experimental_long"))
+  def test_unsupported_hold_semantics_keep_the_cache_disabled(self, candidate, experimental_long):
+    _, control = make_control(candidate, experimental_long=experimental_long)
+    control.update(True, make_car_state(0.28, -0.29), -0.22, True, (-3.5, 2.0))
+
+    assert control._stopping_hold_accel is None
+
+  @parameterized.expand(ROUTE_STOP_ONSETS, names=("v_ego", "a_ego", "a_target", "initial_accel"))
+  def test_logged_stop_onsets_hold_the_existing_brake(self, v_ego, a_ego, a_target, initial_accel):
+    _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2, initial_accel)
+    output = control.update(True, make_car_state(v_ego, a_ego), a_target, True, (-3.5, 2.0))
+    assert control.long_control_state == LongCtrlState.stopping
+    self.assertAlmostEqual(output, initial_accel)
+
+  @parameterized.expand(ROUTE_STOP_ONSETS, names=("v_ego", "a_ego", "a_target", "initial_accel"))
+  def test_logged_stop_onsets_preserve_a_settled_hold(self, v_ego, a_ego, a_target, initial_accel):
+    CP, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2, initial_accel)
+    control.update(True, make_car_state(v_ego, a_ego), a_target, True, (-3.5, 2.0))
+    CS = make_car_state(0.0, 0.0, standstill=True)
+    outputs = [control.update(True, CS, a_target, True, (-3.5, 2.0)) for _ in range(round(10.0 / DT_CTRL))]
+
+    hold_floor = expected_hold_accel(CP, initial_accel)
+    self.assertAlmostEqual(outputs[-1], hold_floor)
+    np.testing.assert_allclose(outputs[-100:], outputs[-1], rtol=0.0, atol=1e-12)
+    assert all(current <= previous for previous, current in zip(outputs[:-1], outputs[1:], strict=True))
+
+  @parameterized.expand(PRESERVED_HOLD_VEHICLES, names=("candidate",))
+  def test_preserved_hold_does_not_change_the_moving_approach(self, candidate):
+    CP, control = make_control(candidate)
+    control.update(True, make_car_state(0.28, -0.29), -0.22, True, (-3.5, 2.0))
+    moving = [control.update(True, make_car_state(0.25, -0.25), -0.22, True, (-3.5, 2.0)) for _ in range(20)]
+    CS = make_car_state(0.0, 0.0, standstill=True)
+    terminal = [control.update(True, CS, -0.1, True, (-3.5, 2.0)) for _ in range(round(4.0 / DT_CTRL))]
+
+    np.testing.assert_allclose(moving, -0.33, rtol=0.0, atol=1e-12)
+    self.assertAlmostEqual(terminal[-1], expected_hold_accel(CP))
+
+  def test_glide_hold_survives_a_soft_deceleration_sample(self):
+    _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2, -0.166)
+    samples = ((0.388, -0.201, -0.164), (0.330, -0.120, -0.140), (0.283, -0.0675, -0.120))
+    outputs = [control.update(True, make_car_state(v_ego, a_ego), a_target, True, (-3.5, 2.0)) for v_ego, a_ego, a_target in samples]
+
+    np.testing.assert_allclose(outputs, [-0.166] * len(samples), rtol=1e-6, atol=1e-12)
+
+  def test_glide_response_reaches_the_stock_rate_when_deceleration_stops(self):
+    _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2, -0.166)
+    control.update(True, make_car_state(0.388, -0.201), -0.164, True, (-3.5, 2.0))
+    output = control.update(True, make_car_state(0.330, -0.01), -0.140, True, (-3.5, 2.0))
+
+    assert -0.176 < output < -0.175
+
+  def test_glide_response_increases_with_stopping_distance_error(self):
+    _, nominal = make_control(TOYOTA.TOYOTA_RAV4_TSS2, -0.166)
+    _, distance_error = make_control(TOYOTA.TOYOTA_RAV4_TSS2, -0.166)
+    for control in (nominal, distance_error):
+      control.update(True, make_car_state(0.388, -0.201), -0.164, True, (-3.5, 2.0))
+    nominal_output = nominal.update(True, make_car_state(0.330, -0.050), -0.140, True, (-3.5, 2.0))
+    distance_error_output = distance_error.update(True, make_car_state(0.400, -0.050), -0.140, True, (-3.5, 2.0))
+
+    assert -0.176 < distance_error_output < nominal_output
+
+  @parameterized.expand(((1.0, 0.0), (0.75, 0.4375), (0.5, 0.75), (0.0, 1.0)), names=("decel_fraction", "expected_rate"))
+  def test_stopping_rate_scales_with_realized_deceleration(self, decel_fraction, expected_rate):
+    _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
+    output = control.update(True, make_car_state(0.3, -0.12 * decel_fraction), 0.0, True, (-3.5, 2.0))
+
+    self.assertAlmostEqual((-0.33 - output) / DT_CTRL, expected_rate, delta=1e-6)
+
+  def test_stopping_rate_scales_with_planner_demand(self):
+    _, gentle = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
+    _, urgent = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
+
+    gentle_output = gentle.update(True, make_car_state(0.3, -0.12), -0.34, True, (-3.5, 2.0))
+    urgent_output = urgent.update(True, make_car_state(0.3, -0.12), -1.0, True, (-3.5, 2.0))
+
+    assert -0.331 < gentle_output < -0.33
+    self.assertAlmostEqual(urgent_output, -0.34)
+
+  def test_glide_hold_yields_to_stronger_planner_braking(self):
+    CP, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2, -0.166)
+    control.update(True, make_car_state(0.388, -0.201), -0.164, True, (-3.5, 2.0))
+    output = control.update(True, make_car_state(0.330, -0.120), -1.0, True, (-3.5, 2.0))
+
+    self.assertAlmostEqual(output, stock_stopping_output(-0.166, CP.stopAccel))
+
+  @parameterized.expand(STOP_ACCEL_VEHICLES, names=("candidate",))
+  def test_urgent_braking_matches_the_stock_ramp(self, candidate):
+    CP, control = make_control(candidate)
+    CS = make_car_state(0.8, -0.1)
+    output = control.last_output_accel
+
+    for _ in range(round(1.0 / DT_CTRL)):
+      output = control.update(True, CS, -3.0, True, (-3.5, 2.0))
+
+    expected = -0.33
+    for _ in range(round(1.0 / DT_CTRL)):
+      expected = stock_stopping_output(expected, CP.stopAccel)
+    self.assertAlmostEqual(output, expected)
+
+  @parameterized.expand(STOP_ACCEL_VEHICLES, names=("candidate",))
+  def test_stronger_planner_brake_matches_the_stock_ramp(self, candidate):
+    CP, control = make_control(candidate)
+    outputs = [control.update(True, make_car_state(0.3, -0.3), -1.0, True, (-3.5, 2.0)) for _ in range(10)]
+    expected = []
+    output = -0.33
+    for _ in range(10):
+      output = stock_stopping_output(output, CP.stopAccel)
+      expected.append(output)
+    np.testing.assert_allclose(outputs, expected, rtol=1e-6, atol=1e-12)
+
+  @parameterized.expand(STOP_ACCEL_VEHICLES, names=("candidate",))
+  def test_insufficient_deceleration_uses_most_of_the_stock_ramp(self, candidate):
+    CP, control = make_control(candidate)
+    output = control.update(True, make_car_state(0.6, -0.1), -0.1, True, (-3.5, 2.0))
+    if -0.33 > CP.stopAccel:
+      assert -0.34 < output < -0.338
+    else:
+      self.assertAlmostEqual(output, -0.33)
+
+  def test_deceleration_noise_cannot_release_the_brake(self):
+    _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
+    outputs = [control.update(True, make_car_state(0.3, -0.3 if frame % 2 else 0.0), -0.1, True, (-3.5, 2.0)) for frame in range(40)]
+    assert all(current <= previous for previous, current in zip(outputs[:-1], outputs[1:], strict=True))
+
+  def test_planner_noise_cannot_release_the_brake(self):
+    _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
+    outputs = [control.update(True, make_car_state(0.3, -0.3), -1.0 if frame % 2 else -0.1, True, (-3.5, 2.0)) for frame in range(40)]
+    assert all(current <= previous for previous, current in zip(outputs[:-1], outputs[1:], strict=True))
+
+  @parameterized.expand(
+    (
+      (float("nan"), -0.3, -0.1),
+      (0.3, float("nan"), -0.1),
+      (0.3, -0.3, float("nan")),
+      (float("inf"), -0.3, -0.1),
+      (0.3, -float("inf"), -0.1),
+      (0.3, -0.3, float("inf")),
+    ),
+    names=("v_ego", "a_ego", "a_target"),
+  )
+  def test_invalid_state_uses_the_stock_ramp(self, v_ego, a_ego, a_target):
+    CP, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
+    output = control.update(True, make_car_state(v_ego, a_ego), a_target, True, (-3.5, 2.0))
+    self.assertAlmostEqual(output, stock_stopping_output(-0.33, CP.stopAccel))
+
+  @parameterized.expand(
+    (
+      (0.24, 0.0, -0.49, 0.15, 0.0),
+      (0.53, -0.31, -0.49, 0.35, 0.1),
+      (0.24, 0.0, 0.0, 0.15, 0.0),
+      (0.464, -0.223, 0.0, 0.25, 0.05),
+      (0.53, -0.31, 0.0, 0.35, 0.1),
+      (0.24, 0.0, 0.49, 0.15, 0.0),
+      (0.53, -0.31, 0.49, 0.25, 0.05),
+      (0.6, -0.3, 0.49, 0.35, 0.1),
+      (0.6, -0.3, 0.49, 0.5, 0.1),
+    ),
+    names=("speed", "initial_accel", "grade_accel", "actuator_lag", "actuator_delay"),
+  )
+  def test_smooth_stop_distance_is_bounded(self, speed, initial_accel, grade_accel, actuator_lag, actuator_delay):
+    _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2, initial_accel)
+    applied_accel = initial_accel
+    delay = [initial_accel] * round(actuator_delay / DT_CTRL)
+    distance = 0.0
+    outputs = []
+
+    for _ in range(round(4.0 / DT_CTRL)):
+      command = control.update(True, make_car_state(speed, applied_accel), -0.1, True, (-3.5, 2.0))
+      outputs.append(command)
+      delayed_command = command
+      if delay:
+        delay.append(command)
+        delayed_command = delay.pop(0)
+      applied_accel += DT_CTRL / actuator_lag * (delayed_command + grade_accel - applied_accel)
+      speed = max(0.0, speed + applied_accel * DT_CTRL)
+      distance += speed * DT_CTRL
+      if speed == 0.0:
+        break
+
+    assert speed == 0.0
+    assert distance < 1.0
+    assert all(current <= previous for previous, current in zip(outputs[:-1], outputs[1:], strict=True))
+
+  @parameterized.expand(STOP_ACCEL_VEHICLES, names=("candidate",))
+  def test_standstill_uses_the_stock_ramp(self, candidate):
+    CP, control = make_control(candidate)
+    control.long_control_state = LongCtrlState.off
+    CS = make_car_state(0.0, 0.0, standstill=True)
+    outputs = [control.update(True, CS, 0.0, False, (-3.5, 2.0)) for _ in range(round(2.0 / DT_CTRL))]
+    expected = -0.33
+    for _ in range(round(2.0 / DT_CTRL)):
+      expected = stock_stopping_output(expected, CP.stopAccel)
+    self.assertAlmostEqual(outputs[0], stock_stopping_output(-0.33, CP.stopAccel))
+    self.assertAlmostEqual(outputs[-1], expected)
+
+  @parameterized.expand(PRESERVED_HOLD_VEHICLES, names=("candidate",))
+  def test_preserved_hold_yields_to_stronger_planner_braking(self, candidate):
+    CP, control = make_control(candidate)
+    settle_preserved_hold(control)
+    previous = control.last_output_accel
+    output = control.update(True, make_car_state(0.0, 0.0, standstill=True), CP.stopAccel, True, (-3.5, 2.0))
+
+    self.assertAlmostEqual(output, stock_stopping_output(previous, CP.stopAccel))
+
+  def test_false_departure_restores_a_stronger_preserved_hold(self):
+    CP, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
+    settle_preserved_hold(control)
+    stronger_hold = control.update(True, make_car_state(0.0, 0.0, standstill=True), CP.stopAccel, True, (-3.5, 2.0))
+    departure_state = make_car_state(0.0, 0.0, standstill=True)
+    departure_state.cruiseState.standstill = False
+    control.update(True, departure_state, 0.6, False, (-3.5, 2.0))
+    restored = control.update(True, make_car_state(0.0, 0.0, standstill=True), -0.1, True, (-3.5, 2.0))
+
+    self.assertAlmostEqual(restored, stronger_hold)
+
+  @parameterized.expand(SETTLE_VEHICLES, names=("candidate",))
+  def test_false_departure_restores_a_command_at_the_stop_limit(self, candidate):
+    CP, control = make_control(candidate)
+    strong_hold = max(CP.stopAccel - 0.2, -3.5)
+    control.last_output_accel = strong_hold
+    held = control.update(True, make_car_state(0.0, 0.0, standstill=True), -0.1, True, (-3.5, 2.0))
+    departure_state = make_car_state(0.0, 0.0, standstill=True)
+    departure_state.cruiseState.standstill = False
+    control.update(True, departure_state, 0.6, False, (-3.5, 2.0))
+    restored = control.update(True, make_car_state(0.0, 0.0, standstill=True), -0.1, True, (-3.5, 2.0))
+
+    self.assertAlmostEqual(restored, held)
+
+  @parameterized.expand(SETTLE_VEHICLES, names=("candidate",))
+  def test_false_departure_after_reaching_the_stop_limit_restores_braking(self, candidate):
+    CP, control = make_control(candidate)
+    CS = make_car_state(0.0, 0.0, standstill=True)
+    while control.last_output_accel > CP.stopAccel:
+      reached = control.update(True, CS, -0.1, True, (-3.5, 2.0))
+
+    departure_state = make_car_state(0.0, 0.0, standstill=True)
+    departure_state.cruiseState.standstill = False
+    control.update(True, departure_state, 0.6, False, (-3.5, 2.0))
+    restored = control.update(True, CS, -0.1, True, (-3.5, 2.0))
+
+    self.assertAlmostEqual(restored, reached)
+
+  @parameterized.expand(PRESERVED_HOLD_VEHICLES, names=("candidate",))
+  def test_inadequate_preserved_hold_uses_the_stock_ramp(self, candidate):
+    for v_ego, a_ego, standstill in ((0.0, 0.2, True), (-0.1, 0.0, False)):
+      with self.subTest(v_ego=v_ego, a_ego=a_ego, standstill=standstill):
+        CP, control = make_control(candidate)
+        settle_preserved_hold(control)
+        output = control.last_output_accel
+        expected = output
+        for _ in range(round(4.0 / DT_CTRL)):
+          output = control.update(True, make_car_state(v_ego, a_ego, standstill), -0.1, True, (-3.5, 2.0))
+          expected = max(stock_stopping_output(expected, CP.stopAccel), -3.5)
+
+        self.assertAlmostEqual(output, expected)
+
+  @parameterized.expand(PRESERVED_HOLD_VEHICLES, names=("candidate",))
+  def test_false_departure_restores_the_preserved_hold(self, candidate):
+    _, control = make_control(candidate)
+    settle_preserved_hold(control)
+    hold_accel = control.last_output_accel
+    departure_state = make_car_state(0.0, 0.0, standstill=True)
+    departure_state.cruiseState.standstill = False
+    departure = control.update(True, departure_state, 0.6, False, (-3.5, 2.0))
+    restored = control.update(True, make_car_state(0.0, 0.0, standstill=True), -0.1, True, (-3.5, 2.0))
+
+    assert departure > 0.0
+    self.assertAlmostEqual(restored, hold_accel)
+
+  @parameterized.expand(
+    ((True, 0.0, 0.0, True), (False, 0.06, 0.06, False), (False, 0.0, 0.06, False)),
+    names=("inactive", "v_ego", "v_ego_raw", "standstill"),
+  )
+  def test_preserved_hold_clears_after_inactive_or_real_motion(self, inactive, v_ego, v_ego_raw, standstill):
+    CP, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
+    settle_preserved_hold(control)
+    control.update(not inactive, make_car_state(v_ego, 0.0, standstill=standstill, v_ego_raw=v_ego_raw), 0.6, False, (-3.5, 2.0))
+    output = control.update(True, make_car_state(0.0, 0.0, standstill=True), -0.1, True, (-3.5, 2.0))
+
+    assert control._stopping_hold_accel is None
+    self.assertAlmostEqual(output, stock_stopping_output(0.0, CP.stopAccel))
+
+  @parameterized.expand(((float("nan"), 0.0), (0.0, float("nan"))), names=("v_ego", "v_ego_raw"))
+  def test_invalid_speed_clears_the_preserved_hold(self, v_ego, v_ego_raw):
+    _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
+    settle_preserved_hold(control)
+    previous = control.last_output_accel
+    output = control.update(True, make_car_state(v_ego, 0.0, standstill=True, v_ego_raw=v_ego_raw), -0.1, True, (-3.5, 2.0))
+
+    assert control._stopping_hold_accel is None
+    self.assertAlmostEqual(output, previous - DT_CTRL)
+
+  @parameterized.expand(((0.005, True), (-0.005, True), (0.02, True), (-0.02, False)), names=("v_ego_raw", "standstill"))
+  def test_raw_wheel_motion_keeps_building_brake(self, v_ego_raw, standstill):
+    _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
+    settle_preserved_hold(control)
+    previous = control.last_output_accel
+    CS = make_car_state(0.0, 0.0, standstill=standstill, v_ego_raw=v_ego_raw)
+    output = control.update(True, CS, -0.1, True, (-3.5, 2.0))
+
+    self.assertAlmostEqual(output, previous - DT_CTRL)
+
+  def test_preserved_hold_removes_launch_brake_backlog(self):
+    CP, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
+    control.update(True, make_car_state(0.28, -0.29), -0.22, True, (-3.5, 2.0))
+    CS = make_car_state(0.0, 0.0, standstill=True)
+    for _ in range(round(3.0 / DT_CTRL)):
+      control.update(True, CS, -0.1, True, (-3.5, 2.0))
+    preserved_hold = control.last_output_accel
+
+    CS.cruiseState.standstill = False
+    requested_accels = [control.update(True, CS, min(0.15 + frame * DT_CTRL, 1.2), False, (-3.5, 2.0)) for frame in range(round(1.0 / DT_CTRL))]
+
+    def release_time(initial_accel):
+      applied_accel = initial_accel
+      for frame, requested_accel in enumerate(requested_accels):
+        accel_step = PRIUS_TSS2_ROUTE_MODEL.command_rate_limit * DT_CTRL
+        applied_accel += np.clip(requested_accel - applied_accel, -accel_step, accel_step)
+        if applied_accel >= 0.0:
+          return (frame + 1) * DT_CTRL
+      raise AssertionError("brake command did not release")
+
+    stock_release = release_time(CP.stopAccel)
+    preserved_release = release_time(preserved_hold)
+    self.assertAlmostEqual(preserved_hold, expected_hold_accel(CP))
+    assert stock_release >= 0.45
+    assert preserved_release <= 0.37
+    assert stock_release - preserved_release >= 0.14
+
+  @parameterized.expand(GRADE_HOLD_CASES, names=("grade_accel", "expected_hold"))
+  def test_preserved_hold_adapts_to_grade_without_creep(self, grade_accel, expected_hold):
+    _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2, -0.3)
+    speed = 0.6
+    actuator_accel = -0.3
+    physical_accel = actuator_accel + grade_accel
+    stopped_frames = 0
+    max_post_stop_speed = 0.0
+
+    for _ in range(round(16.0 / DT_CTRL)):
+      standstill = bool(speed <= 1e-6)
+      measured_accel = max(physical_accel, 0.0) if standstill else physical_accel
+      output = control.update(True, make_car_state(speed, measured_accel, standstill), -0.1, True, (-3.5, 2.0))
+      actuator_accel += DT_CTRL / 0.25 * (output - actuator_accel)
+      physical_accel = actuator_accel + grade_accel
+      speed = max(0.0, speed + physical_accel * DT_CTRL) if speed > 0.0 or physical_accel > 0.0 else 0.0
+
+      if stopped_frames:
+        max_post_stop_speed = max(max_post_stop_speed, speed)
+        stopped_frames += 1
+      elif speed == 0.0:
+        stopped_frames = 1
+      if stopped_frames >= round(8.0 / DT_CTRL):
+        break
+
+    assert stopped_frames >= round(8.0 / DT_CTRL)
+    assert max_post_stop_speed == 0.0
+    self.assertAlmostEqual(output, expected_hold, delta=0.03)
+
+  @parameterized.expand(SETTLE_VEHICLES, names=("candidate",))
+  def test_final_stop_builds_brake_smoothly_while_vehicle_settles(self, candidate):
+    _, control = make_control(candidate)
+    control.update(True, make_car_state(0.28, -0.29), -0.22, True, (-3.5, 2.0))
+    outputs = [control.update(True, make_car_state(0.0006, a_ego, standstill=True), -0.032, True, (-3.5, 2.0)) for a_ego in (-1.098, -0.950, -0.609, -0.286)]
+    changes = -np.diff([-0.33, *outputs])
+    assert np.all(changes > 0.0)
+    assert np.all(np.diff(changes) > 0.0)
+    assert changes[-1] < 0.001
+
+  @parameterized.expand((-0.09, 0.0, 0.1), names=("a_ego",))
+  def test_settled_vehicle_uses_the_stock_hold_ramp(self, a_ego):
+    CP, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
+    output = control.update(True, make_car_state(0.0, a_ego, standstill=True), -0.1, True, (-3.5, 2.0))
+    self.assertAlmostEqual(output, stock_stopping_output(-0.33, CP.stopAccel))
+
+  @parameterized.expand(SETTLE_VEHICLES, names=("candidate",))
+  def test_direct_terminal_entry_builds_brake_smoothly(self, candidate):
+    _, control = make_control(candidate)
+    CS = make_car_state(0.0006, -0.3, standstill=True)
+    outputs = [control.update(True, CS, -0.1, True, (-3.5, 2.0)) for _ in range(4)]
+
+    rates = -np.diff([-0.33, *outputs]) / DT_CTRL
+    np.testing.assert_allclose(rates, [(frame / STOPPING_SETTLE_FRAMES) ** 2 for frame in range(1, 5)], rtol=1e-6, atol=1e-12)
+
+  def test_direct_terminal_entry_keeps_urgent_stock_braking(self):
+    CP, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
+    output = control.update(True, make_car_state(0.0006, -0.3, standstill=True), -1.0, True, (-3.5, 2.0))
+
+    self.assertAlmostEqual(output, stock_stopping_output(-0.33, CP.stopAccel))
+
+  @parameterized.expand((0.0, -0.05), names=("initial_accel",))
+  def test_direct_terminal_entry_first_builds_meaningful_brake(self, initial_accel):
+    CP, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2, initial_accel)
+    output = control.update(True, make_car_state(0.0006, -0.3, standstill=True), 0.0, True, (-3.5, 2.0))
+
+    self.assertAlmostEqual(output, stock_stopping_output(initial_accel, CP.stopAccel))
+
+  @parameterized.expand(SETTLE_VEHICLES, names=("candidate",))
+  def test_final_settling_ramp_is_bounded(self, candidate):
+    _, control = make_control(candidate)
+    control.update(True, make_car_state(0.28, -0.29), -0.22, True, (-3.5, 2.0))
+    CS = make_car_state(0.0, -0.3, standstill=True)
+    outputs = [control.update(True, CS, -0.1, True, (-3.5, 2.0)) for _ in range(STOPPING_SETTLE_FRAMES + 1)]
+
+    rates = -np.diff([-0.33, *outputs]) / DT_CTRL
+    expected = [(frame / STOPPING_SETTLE_FRAMES) ** 2 for frame in range(1, STOPPING_SETTLE_FRAMES + 1)] + [1.0]
+    np.testing.assert_allclose(rates, expected, rtol=1e-6, atol=1e-12)
+
+  @parameterized.expand(((0.6, -0.1, False), (0.0, 0.0, True)), names=("v_ego", "a_ego", "standstill"))
+  def test_stopping_never_releases_a_stronger_command(self, v_ego, a_ego, standstill):
+    _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2, -3.0)
+    output = control.update(True, make_car_state(v_ego, a_ego, standstill), 0.0, True, (-3.5, 2.0))
+    self.assertAlmostEqual(output, -3.0)
+
+  def test_reported_standstill_while_moving_can_hold_the_brake(self):
+    _, control = make_control(GM.CHEVROLET_BOLT_EUV)
+    control.long_control_state = LongCtrlState.off
+    output = control.update(True, make_car_state(0.3, -0.3, standstill=True), -0.1, False, (-3.5, 2.0))
+    self.assertAlmostEqual(output, -0.33)
+
+  @parameterized.expand((-0.1, 0.09), names=("a_target",))
+  def test_stopping_removes_positive_acceleration_immediately(self, a_target):
+    _, control = make_control(HYUNDAI.HYUNDAI_SONATA, 0.2)
+    output = control.update(True, make_car_state(0.2, -0.2), a_target, True, (-3.5, 2.0))
+    self.assertAlmostEqual(output, -DT_CTRL)
+
+  def test_rollback_uses_the_stock_ramp(self):
+    CP, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
+    output = control.update(True, make_car_state(-0.1, 0.1), -0.1, True, (-3.5, 2.0))
+    self.assertAlmostEqual(output, stock_stopping_output(-0.33, CP.stopAccel))
+
+  def test_rollback_after_settling_arms_uses_the_stock_ramp(self):
+    _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
+    control.update(True, make_car_state(0.28, -0.29), -0.22, True, (-3.5, 2.0))
+    control.update(True, make_car_state(0.01, -0.3), -0.1, True, (-3.5, 2.0))
+    previous = control.last_output_accel
+    output = control.update(True, make_car_state(-0.04, -0.3), -0.1, True, (-3.5, 2.0))
+
+    self.assertAlmostEqual(output, previous - DT_CTRL)
+
+  def test_small_velocity_noise_does_not_trigger_the_stock_rate(self):
+    _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
+    control.update(True, make_car_state(0.28, -0.29), -0.22, True, (-3.5, 2.0))
+    output = control.update(True, make_car_state(-0.04, -0.3, standstill=True, v_ego_raw=0.0), -0.1, True, (-3.5, 2.0))
+    assert -0.331 < output < -0.33
+
+  def test_terminal_speed_chatter_cannot_extend_settling_ramp(self):
+    _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
+    control.update(True, make_car_state(0.28, -0.29), -0.22, True, (-3.5, 2.0))
+    outputs = [
+      control.update(True, make_car_state(0.049 if frame % 2 == 0 else 0.051, -0.3, v_ego_raw=0.0), -0.1, True, (-3.5, 2.0))
+      for frame in range(STOPPING_SETTLE_FRAMES + 2)
+    ]
+
+    rates = -np.diff([-0.33, *outputs]) / DT_CTRL
+    np.testing.assert_allclose(
+      rates[:STOPPING_SETTLE_FRAMES], [(frame / STOPPING_SETTLE_FRAMES) ** 2 for frame in range(1, STOPPING_SETTLE_FRAMES + 1)], rtol=1e-6, atol=1e-12
+    )
+    np.testing.assert_allclose(rates[-2:], [1.0, 1.0], rtol=1e-6, atol=1e-12)
+
+  def test_terminal_speed_plateau_cannot_extend_settling_ramp(self):
+    _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
+    control.update(True, make_car_state(0.28, -0.29), -0.22, True, (-3.5, 2.0))
+    CS = make_car_state(0.03, -0.3, v_ego_raw=0.0)
+    outputs = [control.update(True, CS, -0.1, True, (-3.5, 2.0)) for _ in range(STOPPING_SETTLE_FRAMES + 1)]
+
+    rates = -np.diff([-0.33, *outputs]) / DT_CTRL
+    np.testing.assert_allclose(rates[-2:], [1.0, 1.0], rtol=1e-6, atol=1e-12)
+
+  def test_interrupted_stop_cannot_reuse_settling_hold(self):
+    CP, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
+    control.update(True, make_car_state(0.28, -0.29), -0.22, True, (-3.5, 2.0))
+    control.update(False, make_car_state(0.0, 0.0, standstill=True), 0.0, False, (-3.5, 2.0))
+    output = control.update(True, make_car_state(0.0, -0.3, standstill=True), -0.1, True, (-3.5, 2.0))
+
+    self.assertAlmostEqual(output, stock_stopping_output(0.0, CP.stopAccel))
+
+  def test_departure_uses_the_stock_pid_path(self):
+    _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
+    control.long_control_state = LongCtrlState.stopping
+    output = control.update(True, make_car_state(0.0), 0.6, False, (-3.5, 2.0))
+    assert control.long_control_state == LongCtrlState.pid
+    assert output > 0.0
+
+  def test_planner_mpc_and_longcontrol_complete_a_smooth_stop(self):
+    plant = PlantSP(
+      lead_relevancy=True,
+      speed=0.6,
+      distance_lead=3.6,
+      run_long_control=True,
+      actuator_model=PRIUS_TSS2_ROUTE_MODEL,
+    )
+    plant.planner.accel_controller_enabled = True
+    plant.planner.dec._enabled = False
+    commands = []
+    speeds = []
+    states = []
+    solver_statuses = []
+
+    with (
+      mock.patch.object(plant.planner, "read_accel_controller_params", return_value=None),
+      mock.patch.object(plant.planner.dec, "_read_params", return_value=None),
+    ):
+      while plant.current_time < 5.0:
+        result = plant.step(v_lead=0.0, v_cruise=8.0)
+        commands.append(result["actuator_command"])
+        speeds.append(result["speed"])
+        states.append(result["long_control_state"])
+        solver_statuses.append(plant.planner.mpc.solution_status)
+
+    stopping = states.index(LongCtrlState.stopping)
+    moving_stop_commands = [
+      command for command, state, speed in zip(commands, states, speeds, strict=True) if state == LongCtrlState.stopping and speed > STOPPING_SPEED_TOLERANCE
+    ]
+    assert all(current <= previous + 1e-9 for previous, current in zip(commands[stopping:-1], commands[stopping + 1 :], strict=True))
+    assert len(moving_stop_commands) > 1 and max(moving_stop_commands) - min(moving_stop_commands) < 1e-9
+    assert plant.speed == 0.0 and plant.distance < 1.0
+    assert plant.distance_lead - plant.distance > 3.0
+    assert all(status == 0 for status in solver_statuses)

openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/plant.py

@@ -0,0 +1,395 @@
+"""
+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 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]]
+
+
+@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,
+    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.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)]
+    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()
+    velocity.x = [float(x) for x in (self.speed + 0.5) * np.ones_like(ModelConstants.T_IDXS)]
+    velocity.x[0] = float(self.speed)  # always start at current speed
+    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)]
+
+    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
+    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()
+
+    accel_controller = self.planner.accel_controller
+    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(),
+      "controller_active": accel_controller.is_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),
+    }

openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/tests/test_plant_sp.py

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

openpilot/sunnypilot/sunnylink/capabilities.py

@@ -11,6 +11,7 @@ from opendbc.car.structs import car
 from opendbc.car.hyundai.values import CAR as HYUNDAI_CAR, UNSUPPORTED_LONGITUDINAL_CAR
 from opendbc.car.subaru.values import CAR as SUBARU_CAR, SubaruFlags
 from opendbc.sunnypilot.car.tesla.values import TeslaFlagsSP
+from opendbc.sunnypilot.car.toyota.values import ToyotaFlagsSP, VIRTUAL_CRUISE_SPEED_CAR
 from openpilot.common.params import Params
 from openpilot.common.swaglog import cloudlog
 from openpilot.common.hardware import HARDWARE
@@ -19,6 +20,7 @@ from openpilot.common.hardware import HARDWARE
 # Wire-protocol version for the capabilities payload. Bump on breaking changes
 # only; additive fields are backward-compatible and do not require a bump.
 PROTOCOL_VERSION = 1
+TOYOTA_VIRTUAL_CRUISE_SPEED_PLATFORMS = {str(platform) for platform in VIRTUAL_CRUISE_SPEED_CAR}
 
 # All capability fields that rules may reference.
 # Non-boolean fields must have defaults in CAPABILITY_DEFAULTS.
@@ -42,6 +44,7 @@ CAPABILITY_FIELDS = (
   "device_type",
   "subaru_has_sng",
   "hyundai_alpha_long_available",
+  "toyota_virtual_cruise_speed_available",
 )
 
 CAPABILITY_LABELS: dict[str, str] = {
@@ -64,6 +67,7 @@ CAPABILITY_LABELS: dict[str, str] = {
   "device_type": "Device type",
   "subaru_has_sng": "Subaru Stop-and-Go available",
   "hyundai_alpha_long_available": "Hyundai Alpha Longitudinal available",
+  "toyota_virtual_cruise_speed_available": "Toyota Virtual Cruise Speed available",
 }
 
 # Explicit defaults for non-boolean capability fields
@@ -110,6 +114,12 @@ def _resolve_brand_capabilities(caps: dict, bundle_platform: str, CP) -> None:
       caps["subaru_has_sng"] = not bool(CP.flags & (SubaruFlags.GLOBAL_GEN2 | SubaruFlags.HYBRID))
       caps["has_stop_and_go"] = caps["subaru_has_sng"]
 
+  elif brand == "toyota":
+    if bundle_platform:
+      caps["toyota_virtual_cruise_speed_available"] = bundle_platform in TOYOTA_VIRTUAL_CRUISE_SPEED_PLATFORMS
+    elif CP is not None:
+      caps["toyota_virtual_cruise_speed_available"] = str(CP.carFingerprint) in TOYOTA_VIRTUAL_CRUISE_SPEED_PLATFORMS
+
 
 def generate_capabilities(params: Params | None = None) -> dict:
   """Generate a SettingsCapabilities dict from CarParams + boolean params.
@@ -174,6 +184,8 @@ def generate_capabilities(params: Params | None = None) -> dict:
       caps["icbm_available"] = bool(CP_SP.intelligentCruiseButtonManagementAvailable)
       caps["has_icbm"] = bool(CP_SP.intelligentCruiseButtonManagementAvailable) and params.get_bool("IntelligentCruiseButtonManagement")
       caps["tesla_has_vehicle_bus"] = bool(CP_SP.flags & TeslaFlagsSP.HAS_VEHICLE_BUS)
+      if caps["brand"] == "toyota":
+        caps["toyota_virtual_cruise_speed_available"] = bool(CP_SP.flags & ToyotaFlagsSP.VIRTUAL_CRUISE_SPEED_AVAILABLE)
     except Exception:
       cloudlog.exception("capabilities: failed to deserialize CarParamsSPPersistent")
 

openpilot/sunnypilot/sunnylink/settings_ui.json

@@ -652,6 +652,53 @@
                 }
               ]
             },
+            {
+              "key": "AccelPersonalityEnabled",
+              "widget": "toggle",
+              "title": "Enable Accel Controller",
+              "description": "Use the Accel Controller for smooth, early lead following and stop-and-go. Stock emergency braking remains available as a safety backstop.",
+              "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": "Select the vehicle acceleration response. Chauffeur braking and stopping behavior remain the same across profiles.",
+              "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",
@@ -712,6 +759,21 @@
                   "type": "capability",
                   "field": "has_icbm",
                   "equals": true
+                },
+                {
+                  "type": "all",
+                  "conditions": [
+                    {
+                      "type": "capability",
+                      "field": "toyota_virtual_cruise_speed_available",
+                      "equals": true
+                    },
+                    {
+                      "type": "param",
+                      "key": "ToyotaVirtualCruiseSpeed",
+                      "equals": true
+                    }
+                  ]
                 }
               ]
             }
@@ -750,6 +812,21 @@
                       "type": "capability",
                       "field": "has_icbm",
                       "equals": true
+                    },
+                    {
+                      "type": "all",
+                      "conditions": [
+                        {
+                          "type": "capability",
+                          "field": "toyota_virtual_cruise_speed_available",
+                          "equals": true
+                        },
+                        {
+                          "type": "param",
+                          "key": "ToyotaVirtualCruiseSpeed",
+                          "equals": true
+                        }
+                      ]
                     }
                   ]
                 }
@@ -2094,6 +2171,22 @@
                   "equals": true
                 }
               ]
+            },
+            {
+              "key": "PlanplusControl",
+              "widget": "option",
+              "title": "Plan Plus Controls",
+              "description": "Adjust planplus model recentering strength. The higher this number the more aggressively the model will recover to lane center; too high and it will ping-pong.",
+              "min": 0.0,
+              "max": 2.0,
+              "step": 0.1,
+              "enablement": [
+                {
+                  "type": "param",
+                  "key": "ShowAdvancedControls",
+                  "equals": true
+                }
+              ]
             }
           ]
         },
@@ -2172,26 +2265,6 @@
       "title": "Hyundai / Kia / Genesis Settings",
       "description": "",
       "items": [
-        {
-          "key": "CustomButtonAction",
-          "widget": "multiple_button",
-          "title": "Steering Custom Button",
-          "description": "Choose the openpilot action for the steering wheel custom/star button. OEM functionality is unchanged.",
-          "options": [
-            {
-              "value": 0,
-              "label": "None"
-            },
-            {
-              "value": 1,
-              "label": "Bookmark"
-            },
-            {
-              "value": 3,
-              "label": "Cycle UI"
-            }
-          ]
-        },
         {
           "key": "HyundaiLongitudinalTuning",
           "widget": "multiple_button",
@@ -2322,6 +2395,50 @@
       "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": "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",
@@ -2331,6 +2448,40 @@
           "enablement": [
             {
               "type": "not_engaged"
+            },
+            {
+              "type": "param",
+              "key": "ToyotaVirtualCruiseSpeed",
+              "equals": false
+            }
+          ]
+        },
+        {
+          "key": "ToyotaVirtualCruiseSpeed",
+          "widget": "toggle",
+          "needs_onroad_cycle": true,
+          "title": "Toyota: Virtual Cruise Speed (Alpha)",
+          "description": "Uses a sunnypilot-owned cruise target with the Toyota RES/SET buttons and unlocks Custom ACC Speed Intervals. Set the short interval to 5 for next-5-unit tap behavior. The Toyota cluster continues to show the factory target and may differ from sunnypilot. The direct button signals are route-validated on Corolla Cross and Prius TSS2, but held-button timing differs by platform. Validate acceleration above the factory target in a controlled setting.",
+          "visibility": [
+            {
+              "type": "capability",
+              "field": "toyota_virtual_cruise_speed_available",
+              "equals": true
+            }
+          ],
+          "enablement": [
+            {
+              "type": "not_engaged"
+            },
+            {
+              "type": "capability",
+              "field": "has_longitudinal_control",
+              "equals": true
+            },
+            {
+              "type": "param",
+              "key": "ToyotaEnforceStockLongitudinal",
+              "equals": false
             }
           ]
         },

openpilot/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml

@@ -43,6 +43,29 @@ sections:
       label: Relaxed
     enablement:
     - $ref: '#/macros/longitudinal'
+  - key: AccelPersonalityEnabled
+    widget: toggle
+    title: Enable Accel Controller
+    description: Use the Accel Controller for smooth, early lead following and stop-and-go. Stock emergency braking
+      remains available as a safety backstop.
+    visibility:
+    - $ref: '#/macros/longitudinal'
+    enablement:
+    - $ref: '#/macros/longitudinal'
+  - key: AccelPersonality
+    widget: multiple_button
+    title: Acceleration Profile
+    description: Select the vehicle acceleration response. Chauffeur braking and stopping behavior remain the same across
+      profiles.
+    options:
+    - value: 0
+      label: Eco
+    - value: 1
+      label: Normal
+    - value: 2
+      label: Sport
+    enablement:
+    - $ref: '#/macros/longitudinal'
   - key: IntelligentCruiseButtonManagement
     widget: toggle
     title: Intelligent Cruise Button Management (ICBM) (Alpha)
@@ -77,6 +100,14 @@ sections:
     - type: capability
       field: has_icbm
       equals: true
+    - type: all
+      conditions:
+      - type: capability
+        field: toyota_virtual_cruise_speed_available
+        equals: true
+      - type: param
+        key: ToyotaVirtualCruiseSpeed
+        equals: true
   items:
   - key: CustomAccIncrementsEnabled
     widget: toggle
@@ -98,6 +129,14 @@ sections:
       - type: capability
         field: has_icbm
         equals: true
+      - type: all
+        conditions:
+        - type: capability
+          field: toyota_virtual_cruise_speed_available
+          equals: true
+        - type: param
+          key: ToyotaVirtualCruiseSpeed
+          equals: true
   sub_panels:
   - id: custom_acc_intervals
     label: Custom ACC Speed Intervals Settings

openpilot/sunnypilot/sunnylink/settings_ui_src/pages/models.yaml

@@ -51,6 +51,16 @@ sections:
         key: LagdToggle
         equals: true
     - $ref: '#/macros/advanced_only'
+  - key: PlanplusControl
+    widget: option
+    title: Plan Plus Controls
+    description: Adjust planplus model recentering strength. The higher this number the more aggressively the model will recover
+      to lane center; too high and it will ping-pong.
+    min: 0.0
+    max: 2.0
+    step: 0.1
+    enablement:
+    - $ref: '#/macros/advanced_only'
 - id: lateral_control
   title: Lateral Control
   description: Neural network lateral control for supported models

openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml

@@ -10,17 +10,6 @@ sections:
   title: Hyundai / Kia / Genesis Settings
   description: ''
   items:
-  - key: CustomButtonAction
-    widget: multiple_button
-    title: Steering Custom Button
-    description: Choose the openpilot action for the steering wheel custom/star button. OEM functionality is unchanged.
-    options:
-    - value: 0
-      label: None
-    - value: 1
-      label: Bookmark
-    - value: 3
-      label: Cycle UI
   - key: HyundaiLongitudinalTuning
     widget: multiple_button
     title: Custom Longitudinal Tuning
@@ -93,6 +82,30 @@ sections:
   title: Toyota / Lexus Settings
   description: ''
   items:
+  - key: ToyotaAutoHold
+    widget: toggle
+    needs_onroad_cycle: true
+    title: 'Toyota: Auto Brake Hold FOR TSS2 HYBRID CARS'
+    enablement:
+    - $ref: '#/macros/not_engaged'
+  - key: ToyotaEnhancedBsm
+    widget: toggle
+    needs_onroad_cycle: true
+    title: 'Toyota: Prius TSS2 BSM and some tssp'
+    enablement:
+    - $ref: '#/macros/not_engaged'
+  - key: ToyotaTSS2Long
+    widget: toggle
+    needs_onroad_cycle: true
+    title: 'Toyota: custom longitudinal for TSS2'
+    enablement:
+    - $ref: '#/macros/not_engaged'
+  - key: ToyotaDriveMode
+    widget: toggle
+    needs_onroad_cycle: true
+    title: Enable drive mode btn link
+    enablement:
+    - $ref: '#/macros/not_engaged'
   - key: ToyotaEnforceStockLongitudinal
     widget: toggle
     needs_onroad_cycle: true
@@ -100,6 +113,28 @@ sections:
     description: sunnypilot will not take over control of gas and brakes. Factory Toyota longitudinal control will be used.
     enablement:
     - $ref: '#/macros/not_engaged'
+    - type: param
+      key: ToyotaVirtualCruiseSpeed
+      equals: false
+  - key: ToyotaVirtualCruiseSpeed
+    widget: toggle
+    needs_onroad_cycle: true
+    title: 'Toyota: Virtual Cruise Speed (Alpha)'
+    description: Uses a sunnypilot-owned cruise target with the Toyota RES/SET buttons and unlocks Custom ACC Speed
+      Intervals. Set the short interval to 5 for next-5-unit tap behavior. The Toyota cluster continues to show the
+      factory target and may differ from sunnypilot. The direct button signals are route-validated on Corolla Cross
+      and Prius TSS2, but held-button timing differs by platform. Validate acceleration above the factory target in a
+      controlled setting.
+    visibility:
+    - type: capability
+      field: toyota_virtual_cruise_speed_available
+      equals: true
+    enablement:
+    - $ref: '#/macros/not_engaged'
+    - $ref: '#/macros/longitudinal'
+    - type: param
+      key: ToyotaEnforceStockLongitudinal
+      equals: false
   - key: ToyotaStopAndGoHack
     widget: toggle
     needs_onroad_cycle: true

openpilot/sunnypilot/sunnylink/tests/test_capabilities.py

@@ -10,9 +10,18 @@ change and must be intentional. KNOWN_PROTOCOL_VERSIONS pins the set we
 explicitly support — when the constant is bumped, this list must be edited in
 the same commit so the bump shows up in code review.
 """
+
 from __future__ import annotations
 
+from typing import cast
 
+from openpilot.cereal import custom
+from openpilot.common.params import Params
+from openpilot.common.parameterized import parameterized
+from openpilot.common.test import OpenpilotTestCase
+from opendbc.car.structs import car
+from opendbc.car.toyota.values import CAR as TOYOTA_CAR
+from opendbc.sunnypilot.car.toyota.values import ToyotaFlagsSP
 from openpilot.sunnypilot.sunnylink.capabilities import (
   CAPABILITY_DEFAULTS,
   CAPABILITY_FIELDS,
@@ -20,13 +29,41 @@ from openpilot.sunnypilot.sunnylink.capabilities import (
   PROTOCOL_VERSION,
   generate_capabilities,
 )
-from openpilot.common.test import OpenpilotTestCase
 
 
 KNOWN_PROTOCOL_VERSIONS = (1,)
 LATEST_KNOWN = max(KNOWN_PROTOCOL_VERSIONS)
 
 
+class FakeParams:
+  def __init__(self, values=None):
+    self.values = values or {}
+
+  def get(self, key, *args, **kwargs):
+    return self.values.get(key)
+
+  def get_bool(self, key):
+    return bool(self.values.get(key, False))
+
+
+def build_persistent_toyota_params(platform, *, sp_flags=0) -> Params:
+  CP = car.CarParams.new_message()
+  CP.brand = "toyota"
+  CP.carFingerprint = str(platform)
+  CP.pcmCruise = True
+  CP.openpilotLongitudinalControl = True
+
+  CP_SP = custom.CarParamsSP.new_message()
+  CP_SP.flags = int(sp_flags)
+
+  return cast(Params, FakeParams(
+    {
+      "CarParamsPersistent": CP.to_bytes(),
+      "CarParamsSPPersistent": CP_SP.to_bytes(),
+    }
+  ))
+
+
 def caps():
   return generate_capabilities()
 
@@ -52,14 +89,12 @@ class TestProtocolVersion(OpenpilotTestCase):
   def test_protocol_version_is_known(self):
     """Sentinel against accidental bumps. Edit KNOWN_PROTOCOL_VERSIONS if intentional."""
     assert PROTOCOL_VERSION in KNOWN_PROTOCOL_VERSIONS, (
-      f"PROTOCOL_VERSION={PROTOCOL_VERSION} is not in KNOWN_PROTOCOL_VERSIONS={KNOWN_PROTOCOL_VERSIONS}. " +
-      "If this bump is intentional, add it to KNOWN_PROTOCOL_VERSIONS."
+      f"PROTOCOL_VERSION={PROTOCOL_VERSION} is not in KNOWN_PROTOCOL_VERSIONS={KNOWN_PROTOCOL_VERSIONS}. "
+      + "If this bump is intentional, add it to KNOWN_PROTOCOL_VERSIONS."
     )
 
   def test_protocol_version_matches_latest_known(self):
-    assert PROTOCOL_VERSION == LATEST_KNOWN, (
-      "Test invariant: PROTOCOL_VERSION must equal max(KNOWN_PROTOCOL_VERSIONS)."
-    )
+    assert PROTOCOL_VERSION == LATEST_KNOWN, "Test invariant: PROTOCOL_VERSION must equal max(KNOWN_PROTOCOL_VERSIONS)."
 
 
 class TestOpaquePerBrandFlags(OpenpilotTestCase):
@@ -76,6 +111,66 @@ class TestOpaquePerBrandFlags(OpenpilotTestCase):
     assert caps["hyundai_alpha_long_available"] is False
 
 
+class TestToyotaVirtualCruiseSpeedCapability(OpenpilotTestCase):
+  def test_field_present_and_labeled(self):
+    assert "toyota_virtual_cruise_speed_available" in CAPABILITY_FIELDS
+    assert "toyota_virtual_cruise_speed_available" in CAPABILITY_LABELS
+
+  def test_default_false(self):
+    caps = generate_capabilities(cast(Params, FakeParams()))
+    assert caps["toyota_virtual_cruise_speed_available"] is False
+
+  @parameterized.expand(
+    (
+      (TOYOTA_CAR.TOYOTA_COROLLA_TSS2, True),
+      (TOYOTA_CAR.TOYOTA_PRIUS_TSS2, True),
+      (TOYOTA_CAR.TOYOTA_RAV4_TSS2, False),
+    )
+  )
+  def test_bundle_platform_gating(self, platform, expected):
+    params = FakeParams(
+      {
+        "CarPlatformBundle": {
+          "brand": "toyota",
+          "platform": str(platform),
+        },
+      }
+    )
+    caps = generate_capabilities(cast(Params, params))
+    assert caps["toyota_virtual_cruise_speed_available"] is expected
+
+  @parameterized.expand(
+    (
+      (TOYOTA_CAR.TOYOTA_COROLLA_TSS2, ToyotaFlagsSP.VIRTUAL_CRUISE_SPEED_AVAILABLE, True),
+      (TOYOTA_CAR.TOYOTA_COROLLA_TSS2, 0, True),
+      (TOYOTA_CAR.TOYOTA_PRIUS_TSS2, ToyotaFlagsSP.VIRTUAL_CRUISE_SPEED_AVAILABLE, True),
+      (TOYOTA_CAR.TOYOTA_PRIUS_TSS2, 0, True),
+      (TOYOTA_CAR.TOYOTA_RAV4_TSS2, ToyotaFlagsSP.VIRTUAL_CRUISE_SPEED_AVAILABLE, False),
+    )
+  )
+  def test_persistent_car_params_platform_gating(self, platform, sp_flags, expected):
+    caps = generate_capabilities(build_persistent_toyota_params(platform, sp_flags=sp_flags))
+    assert caps["toyota_virtual_cruise_speed_available"] is expected
+
+  @parameterized.expand(
+    (
+      (TOYOTA_CAR.TOYOTA_COROLLA_TSS2, TOYOTA_CAR.TOYOTA_RAV4_TSS2, True),
+      (TOYOTA_CAR.TOYOTA_PRIUS_TSS2, TOYOTA_CAR.TOYOTA_RAV4_TSS2, True),
+      (TOYOTA_CAR.TOYOTA_RAV4_TSS2, TOYOTA_CAR.TOYOTA_COROLLA_TSS2, False),
+      (TOYOTA_CAR.TOYOTA_RAV4_TSS2, TOYOTA_CAR.TOYOTA_PRIUS_TSS2, False),
+    )
+  )
+  def test_bundle_platform_takes_precedence_over_stale_persistent_params(self, bundle_platform, persistent_platform, expected):
+    params = build_persistent_toyota_params(persistent_platform, sp_flags=ToyotaFlagsSP.VIRTUAL_CRUISE_SPEED_AVAILABLE)
+    params.values["CarPlatformBundle"] = {
+      "brand": "toyota",
+      "platform": str(bundle_platform),
+    }
+
+    caps = generate_capabilities(params)
+    assert caps["toyota_virtual_cruise_speed_available"] is expected
+
+
 class TestCapabilitiesShape(OpenpilotTestCase):
   def test_all_fields_present(self, caps):
     for field in CAPABILITY_FIELDS:

openpilot/sunnypilot/sunnylink/tests/test_settings_changes.py

@@ -9,6 +9,7 @@ isolates one of the gating bugs that the design-overhaul branch fixes so a
 future regression is loud and obvious. These tests are intentionally narrow
 and additive — they do not replace the broader test_settings_schema.py.
 """
+
 from __future__ import annotations
 
 import json
@@ -24,14 +25,13 @@ from openpilot.sunnypilot.sunnylink.tools.generate_settings_schema import (
   _load_torque_versions,
   generate_schema,
 )
+from openpilot.sunnypilot.sunnylink.tools.validate_settings_ui import validate as validate_settings_ui
 from openpilot.common.test import OpenpilotTestCase
 
 
-SCHEMA_VALIDATOR_PATH = os.path.join(os.path.dirname(DEFINITION_PATH), "settings_ui.schema.json")
-
-
 def _walk_items(schema: dict[str, Any]):
   """Yield every item dict from the schema."""
+
   def _yield(item: dict[str, Any]):
     yield item
     for sub in item.get("sub_items", []):
@@ -106,6 +106,26 @@ def _references_capability_field(rules: list[dict[str, Any]] | None, field: str)
   return found
 
 
+def _has_toyota_virtual_cruise_gate(rules: list[dict[str, Any]] | None) -> bool:
+  def _walk(rule: dict[str, Any]) -> bool:
+    if rule.get("type") == "all":
+      conditions = rule.get("conditions", [])
+      has_capability = any(
+        c.get("type") == "capability" and c.get("field") == "toyota_virtual_cruise_speed_available" and c.get("equals") is True for c in conditions
+      )
+      has_param = any(c.get("type") == "param" and c.get("key") == "ToyotaVirtualCruiseSpeed" and c.get("equals") is True for c in conditions)
+      if has_capability and has_param:
+        return True
+
+    if rule.get("type") == "not" and "condition" in rule:
+      return _walk(rule["condition"])
+    if rule.get("type") in ("any", "all"):
+      return any(_walk(c) for c in rule.get("conditions", []))
+    return False
+
+  return any(_walk(rule) for rule in rules or [])
+
+
 def schema():
   return generate_schema()
 
@@ -149,22 +169,13 @@ class TestTestManeuversSection(OpenpilotTestCase):
     assert "is_sp_release" in vis_refs
     enablement = section.get("enablement") or []
     enable_refs = json.dumps(enablement)
-    assert "ShowAdvancedControls" in enable_refs, \
-      "test_maneuvers must gate ShowAdvancedControls via enablement"
+    assert "ShowAdvancedControls" in enable_refs, "test_maneuvers must gate ShowAdvancedControls via enablement"
 
 
 class TestValidator(OpenpilotTestCase):
   def test_validator_accepts_real_json(self):
-    """settings_ui.json validates against settings_ui.schema.json."""
-    try:
-      import jsonschema
-    except ImportError:
-      self.skipTest("jsonschema not installed")
-    with open(DEFINITION_PATH) as f:
-      data = json.load(f)
-    with open(SCHEMA_VALIDATOR_PATH) as f:
-      validator = json.load(f)
-    jsonschema.validate(instance=data, schema=validator)
+    """settings_ui.json passes the repository's production schema validator."""
+    self.assertTrue(validate_settings_ui(DEFINITION_PATH))
 
 
 class TestTorqueOptionGeneration(OpenpilotTestCase):
@@ -177,16 +188,17 @@ class TestTorqueOptionGeneration(OpenpilotTestCase):
     assert item.get("options") == expected
 
   def test_torque_versions_path_resolves(self):
-    assert os.path.exists(TORQUE_VERSIONS_PATH), (
-      f"latcontrol_torque_versions.json not found at {TORQUE_VERSIONS_PATH}"
-    )
+    assert os.path.exists(TORQUE_VERSIONS_PATH), f"latcontrol_torque_versions.json not found at {TORQUE_VERSIONS_PATH}"
 
 
 class TestReleaseBranchGates(OpenpilotTestCase):
-  @parameterized.expand([
-    "EnableGithubRunner",
-    "QuickBootToggle",
-  ], names=["key"])
+  @parameterized.expand(
+    [
+      "EnableGithubRunner",
+      "QuickBootToggle",
+    ],
+    names=["key"],
+  )
   def test_sp_dev_items_gate_on_is_sp_release(self, schema, key):
     """sunnypilot dev items must hide on sunnypilot release branches (is_sp_release gate)."""
     item = _find_item(schema, key)
@@ -208,11 +220,14 @@ class TestSpuriousOffroadGatesDropped(OpenpilotTestCase):
 
 
 class TestNotEngagedReplacement(OpenpilotTestCase):
-  @parameterized.expand([
-    "AlphaLongitudinalEnabled",
-    "ToyotaEnforceStockLongitudinal",
-    "ToyotaStopAndGoHack",
-  ], names=["key"])
+  @parameterized.expand(
+    [
+      "AlphaLongitudinalEnabled",
+      "ToyotaEnforceStockLongitudinal",
+      "ToyotaStopAndGoHack",
+    ],
+    names=["key"],
+  )
   def test_offroad_only_replaced_with_not_engaged(self, schema, key):
     """These items should use not_engaged, not offroad_only."""
     item = _find_item(schema, key)
@@ -220,3 +235,25 @@ class TestNotEngagedReplacement(OpenpilotTestCase):
     rule_types = _flatten_rule_types(item.get("enablement"))
     assert "offroad_only" not in rule_types, f"{key} still uses offroad_only"
     assert "not_engaged" in rule_types, f"{key} missing not_engaged"
+
+
+class TestToyotaVirtualCruiseSpeed(OpenpilotTestCase):
+  def test_vehicle_toggle_contract(self, schema):
+    toyota = schema["vehicle_settings"]["toyota"]
+    item = next((item for item in toyota["items"] if item.get("key") == "ToyotaVirtualCruiseSpeed"), None)
+
+    assert item is not None
+    assert item["widget"] == "toggle"
+    assert item.get("needs_onroad_cycle") is True
+    assert _references_capability_field(item.get("visibility"), "toyota_virtual_cruise_speed_available")
+    assert _references_capability_field(item.get("enablement"), "has_longitudinal_control")
+    assert "not_engaged" in _flatten_rule_types(item.get("enablement"))
+
+  def test_custom_acc_section_links_virtual_cruise_opt_in(self, schema):
+    section = _find_section(schema, "cruise", "custom_acc_increments")
+    assert section is not None
+    assert _has_toyota_virtual_cruise_gate(section.get("enablement"))
+
+    item = _find_item(schema, "CustomAccIncrementsEnabled")
+    assert item is not None
+    assert _has_toyota_virtual_cruise_gate(item.get("enablement"))

openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py

@@ -276,16 +276,40 @@ class TestKnownPanels(OpenpilotTestCase):
     enhanced_enable_keys = {r.get("key") for r in enhanced.get("enablement", []) if r.get("type") == "param"}
     assert "NeuralNetworkLateralControl" in enhanced_enable_keys
 
+  def test_accel_controller_profile_mapping_and_enablement(self, schema):
+    cruise = next(p for p in schema["panels"] if p["id"] == "cruise")
+    items = {item["key"]: item for item in _iter_panel_items(cruise)}
+
+    assert items["AccelPersonalityEnabled"]["widget"] == "toggle"
+    assert items["AccelPersonality"]["options"] == [
+      {"value": 0, "label": "Eco"},
+      {"value": 1, "label": "Normal"},
+      {"value": 2, "label": "Sport"},
+    ]
+    assert {
+      "type": "capability",
+      "field": "has_longitudinal_control",
+      "equals": True,
+    } in items["AccelPersonalityEnabled"]["enablement"]
+    assert {
+      "type": "capability",
+      "field": "has_longitudinal_control",
+      "equals": True,
+    } in items["AccelPersonality"]["enablement"]
+    profile_enable_keys = {rule.get("key") for rule in items["AccelPersonality"]["enablement"] if rule.get("type") == "param"}
+    assert "AccelPersonalityEnabled" not in profile_enable_keys
+
 
 class TestKnownVehicleSettings(OpenpilotTestCase):
   def test_hyundai_has_longitudinal_tuning(self, schema):
     keys = {i["key"] for i in _brand_items(schema["vehicle_settings"].get("hyundai"))}
     assert "HyundaiLongitudinalTuning" in keys
 
-  def test_toyota_has_enforce_stock_and_stop_go(self, schema):
+  def test_toyota_has_enforce_stock_stop_go_and_virtual_cruise(self, schema):
     keys = {i["key"] for i in _brand_items(schema["vehicle_settings"].get("toyota"))}
     assert "ToyotaEnforceStockLongitudinal" in keys
     assert "ToyotaStopAndGoHack" in keys
+    assert "ToyotaVirtualCruiseSpeed" in keys
 
   def test_tesla_has_coop_steering(self, schema):
     keys = {i["key"] for i in _brand_items(schema["vehicle_settings"].get("tesla"))}

openpilot/system/ui/lib/scroll_panel2.py

@@ -45,8 +45,9 @@ class ScrollState(Enum):
 
 
 class GuiScrollPanel2:
-  def __init__(self, horizontal: bool = True) -> None:
+  def __init__(self, horizontal: bool = True, handle_out_of_bounds: bool = True) -> None:
     self._horizontal = horizontal
+    self._handle_out_of_bounds = handle_out_of_bounds
     self._state = ScrollState.STEADY
     self._offset: rl.Vector2 = rl.Vector2(0, 0)
     self._initial_click_event: MouseEvent | None = None
@@ -85,6 +86,20 @@ class GuiScrollPanel2:
     """Returns (max_offset, min_offset) for the given bounds and content size."""
     return 0.0, min(0.0, bounds_size - content_size)
 
+  def _clamp_offset(self, bounds_size: float, content_size: float) -> None:
+    if self._handle_out_of_bounds:
+      return
+
+    max_offset, min_offset = self._get_offset_bounds(bounds_size, content_size)
+    offset = self.get_offset()
+    clamped_offset = max(min_offset, min(max_offset, offset))
+    if clamped_offset == offset:
+      return
+
+    self.set_offset(clamped_offset)
+    if (clamped_offset == max_offset and self._velocity > 0) or (clamped_offset == min_offset and self._velocity < 0):
+      self._velocity = 0.0
+
   def _update_state(self, bounds_size: float, content_size: float, snap_target: float | None) -> None:
     """Runs per render frame, independent of mouse events. Updates auto-scrolling state and velocity."""
     max_offset, min_offset = self._get_offset_bounds(bounds_size, content_size)
@@ -138,6 +153,8 @@ class GuiScrollPanel2:
         factor = 1.0 - math.exp(-SNAP_RATE * dt)
         self.set_offset(self.get_offset() + dist * factor)
 
+    self._clamp_offset(bounds_size, content_size)
+
   def _handle_mouse_event(self, mouse_event: MouseEvent, bounds: rl.Rectangle, bounds_size: float,
                           content_size: float) -> None:
     max_offset, min_offset = self._get_offset_bounds(bounds_size, content_size)

openpilot/system/ui/widgets/scroller.py

@@ -75,7 +75,6 @@ class _Scroller(Widget):
     self._items: list[Widget] = []
     self._horizontal = horizontal
     self._snap_items = snap_items
-    assert not self._snap_items or self._horizontal, "Snapping is only supported for horizontal scrolling"
     self._spacing = spacing
     self._pad = pad
 
@@ -191,12 +190,20 @@ class _Scroller(Widget):
     snap_target: float | None = None
     if self._snap_items and visible_items and self._scrolling_to[0] is None:
       # TODO: this doesn't handle two small buttons at the edges well
-      center_pos = self._rect.x + self._rect.width / 2
-      closest_delta_pos = min((((item.rect.x + item.rect.width / 2) - center_pos) for item in visible_items), key=abs)
+      center_pos = (self._rect.x + self._rect.width / 2) if self._horizontal else (self._rect.y + self._rect.height / 2)
+      closest_delta_pos = min(
+        (self._item_center_pos(item) - center_pos for item in visible_items),
+        key=abs,
+      )
       snap_target = self.scroll_panel.get_offset() - closest_delta_pos
 
     return self.scroll_panel.update(self._rect, content_size, snap_target=snap_target)
 
+  def _item_center_pos(self, item: Widget) -> float:
+    if self._horizontal:
+      return item.rect.x + item.rect.width / 2
+    return item.rect.y + item.rect.height / 2
+
   @property
   def moving_items(self) -> bool:
     return len(self._move_animations) > 0 or len(self._move_lift) > 0