test

.github/workflows/build-all-tinygrad-models.yaml

@@ -8,13 +8,13 @@ on:
         required: true
         type: string
       target_hardware:
-        description: 'Hardware target to compile for (qcom or chestnut)'
+        description: 'Hardware target to compile for (qcom or usbgpu)'
         required: true
         type: choice
         default: 'qcom'
         options:
           - qcom
-          - chestnut
+          - usbgpu
       hf_repo:
         description: 'Hugging Face dataset repository'
         required: false
@@ -59,7 +59,7 @@ jobs:
         id: get-json
         run: |
           cd docs/docs
-          PREFIX="driving_models_${{ inputs.target_hardware == 'chestnut' && 'chestnut_' || '' }}v"
+          PREFIX="driving_models_${{ inputs.target_hardware == 'usbgpu' && 'usbgpu_' || '' }}v"
           latest=$(ls ${PREFIX}*.json | sed -E "s/${PREFIX}([0-9]+)\.json/\1/" | sort -n | tail -1)
           next=$((latest+1))
           json_file="${PREFIX}${next}.json"

.github/workflows/build-default-models.yaml

@@ -176,7 +176,7 @@ jobs:
   build_big_model:
     needs: resolve
     if: ${{ inputs.target == 'big' }}
-    runs-on: [self-hosted, chestnut]
+    runs-on: [self-hosted, usbgpu]
     env:
       BIG_ONNX: openpilot/selfdrive/modeld/models/big_driving_supercombo.onnx
       BIG_PKL: openpilot/selfdrive/modeld/models/big_driving_tinygrad.pkl

.github/workflows/build-single-tinygrad-model.yaml

@@ -30,7 +30,7 @@ on:
         type: boolean
         default: true
       target_hardware:
-        description: 'Hardware target to compile for (qcom or chestnut)'
+        description: 'Hardware target to compile for (qcom or usbgpu)'
         required: false
         type: string
         default: 'qcom'
@@ -101,7 +101,7 @@ on:
         default: 'qcom'
         options:
           - qcom
-          - chestnut
+          - usbgpu
       hf_repo:
         description: 'Hugging Face dataset repository'
         required: false
@@ -109,7 +109,7 @@ on:
         default: 'sunnypilot/sunnypilot_models_v1'
 env:
   RECOMPILED_DIR: recompiled${{ inputs.recompiled_dir }}
-  JSON_FILE: docs/docs/driving_models_${{ inputs.target_hardware == 'chestnut' && 'chestnut_v' || 'v' }}${{ inputs.json_version }}.json
+  JSON_FILE: docs/docs/driving_models_${{ inputs.target_hardware == 'usbgpu' && 'usbgpu_v' || 'v' }}${{ inputs.json_version }}.json
 
 jobs:
   build_model:

.github/workflows/sunnypilot-build-model.yaml

@@ -31,7 +31,7 @@ on:
         type: string
         default: ''
       target_hardware:
-        description: 'Hardware target to compile for (qcom or chestnut)'
+        description: 'Hardware target to compile for (qcom or usbgpu)'
         required: false
         type: string
         default: 'qcom'
@@ -57,7 +57,7 @@ on:
         type: choice
         options:
           - qcom
-          - chestnut
+          - usbgpu
         default: 'qcom'
 
 
@@ -102,7 +102,7 @@ jobs:
           cat $GITHUB_OUTPUT
       - run: |
           cd ${{ github.workspace }}/openpilot/openpilot
-          if [ "${{ inputs.target_hardware }}" != "chestnut" ]; then
+          if [ "${{ inputs.target_hardware }}" != "usbgpu" ]; then
             git lfs pull -X "**/selfdrive/modeld/models/big_*.onnx,**/selfdrive/modeld/models/dmonitoring_*.onnx"
             rm -f selfdrive/modeld/models/big_*.onnx selfdrive/modeld/models/dmonitoring_*.onnx
           else
@@ -121,7 +121,7 @@ jobs:
           if-no-files-found: error
 
   build_model:
-    runs-on: [self-hosted, chestnut]
+    runs-on: [self-hosted, usbgpu]
     needs: get_model
     env:
       MODEL_NAME: ${{ inputs.custom_name || inputs.upstream_branch }} (${{ needs.get_model.outputs.model_date }})
@@ -185,9 +185,9 @@ jobs:
           CAMERA_RES=$(python3 -c "from openpilot.common.transformations.camera import _ar_ox_fisheye as a, _os_fisheye as o; print(f'{a.width}x{a.height} {o.width}x{o.height}')")
 
           TG_FLAGS_QCOM="DEV=QCOM IMAGE=1 FLOAT16=1 NOLOCALS=1 JIT_BATCH_SIZE=0 OPENPILOT_HACKS=1"
-          if [ "${{ inputs.target_hardware }}" == "chestnut" ]; then
-            echo "CHESTNUT build"
-            export CHESTNUT=1
+          if [ "${{ inputs.target_hardware }}" == "usbgpu" ]; then
+            echo "USBGPU build"
+            export USBGPU=1
             TG_FLAGS="DEBUG=1 DEV=USB+AMD:LLVM WARP_DEV=QCOM FLOAT16=1 JIT_BATCH_SIZE=0 GMMU=0 TC_OPT=2"
             OUTPUT_PKL="${{ env.MODELS_DIR }}/big_driving_tinygrad.pkl"
           else

.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

@@ -204,11 +204,16 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
   aTarget @5 :Float32;
   events @6 :List(OnroadEventSP.Event);
   e2eAlerts @7 :E2eAlerts;
+  accelController @8 :AccelController;
 
   struct DynamicExperimentalControl {
     state @0 :DynamicExperimentalControlState;
     enabled @1 :Bool;
     active @2 :Bool;
+    decelIntent @3 :Float32;
+    curveDetected @4 :Bool;
+    wantBlended @5 :Bool;
+    leadVeto @6 :Bool;
 
     enum DynamicExperimentalControlState {
       acc @0;
@@ -306,6 +311,17 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
     greenLightAlert @0 :Bool;
     leadDepartAlert @1 :Bool;
   }
+
+  struct AccelController {
+    enabled @0 :Bool;
+    active @1 :Bool;
+    profile @2 :Profile;
+    enum Profile {
+      eco @0;
+      normal @1;
+      sport @2;
+    }
+  }
 }
 
 struct OnroadEventSP @0xda96579883444c35 {

openpilot/common/params_keys.h

@@ -130,8 +130,8 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"UpdaterLastFetchTime", {PERSISTENT, TIME}},
     {"UptimeOffroad", {PERSISTENT, FLOAT, "0.0"}},
     {"UptimeOnroad", {PERSISTENT, FLOAT, "0.0"}},
-    {"ChestnutActive", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION | CLEAR_ON_IGNITION_ON, BOOL}},
-    {"ChestnutLoading", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION | CLEAR_ON_IGNITION_ON, BOOL}},
+    {"UsbGpuActive", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION | CLEAR_ON_IGNITION_ON, BOOL}},
+    {"UsbGpuLoading", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION | CLEAR_ON_IGNITION_ON, BOOL}},
     {"Version", {PERSISTENT, STRING}},
 
     // --- sunnypilot params --- //
@@ -187,6 +187,12 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"StandstillTimer", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"TrueVEgoUI", {PERSISTENT | BACKUP, BOOL, "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"}},
     {"MadsMainCruiseAllowed", {PERSISTENT | BACKUP, BOOL, "1"}},
@@ -195,16 +201,15 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
 
     // Model Manager params
     {"ModelManager_ActiveBundle", {PERSISTENT, JSON}},
-    {"ModelManager_ActiveBundleUSBGPU", {PERSISTENT, JSON}}, //TODO-SP: kept for migration, remove on next sync?
-    {"ModelManager_ActiveBundleChestnut", {PERSISTENT, JSON}},
+    {"ModelManager_ActiveBundleUSBGPU", {PERSISTENT, JSON}},
     {"ModelManager_ActiveJson", {CLEAR_ON_MANAGER_START, JSON}},
     {"ModelManager_ClearCache", {CLEAR_ON_MANAGER_START, BOOL}},
     {"ModelManager_DownloadRef", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, STRING}},
     {"ModelManager_Favs", {PERSISTENT | BACKUP, STRING}},
     {"ModelManager_LastSyncTime", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, INT, "0"}},
-    {"ModelManager_LastSyncTime_Chestnut", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, INT, "0"}},
+    {"ModelManager_LastSyncTime_USBGPU", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, INT, "0"}},
     {"ModelManager_ModelsCache", {PERSISTENT | BACKUP, JSON}},
-    {"ModelManager_ModelsCache_Chestnut", {PERSISTENT | BACKUP, JSON}},
+    {"ModelManager_ModelsCache_USBGPU", {PERSISTENT | BACKUP, JSON}},
 
     // Neural Network Lateral Control
     {"NeuralNetworkLateralControl", {PERSISTENT | BACKUP, BOOL, "0"}},
@@ -236,6 +241,10 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"DynamicExperimentalControl", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"BlindSpot", {PERSISTENT | BACKUP, BOOL, "0"}},
 
+    // Accel Controller profiles (Eco / Normal / Sport)
+    {"AccelPersonalityEnabled", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"AccelPersonality", {PERSISTENT | BACKUP, INT, "1"}},
+
     // sunnypilot model params
     {"CameraOffset", {PERSISTENT | BACKUP, FLOAT, "0.0"}},
     {"LagdToggle", {PERSISTENT | BACKUP, BOOL, "1"}},

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/assets/icons_mici/chestnut_orange.png

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:845c40ff0d37612e8f2f482a36845744b5ae91ce2fcfc8117990d7d278b59820
-size 13079

openpilot/selfdrive/assets/icons_mici/egpu_crossed.png

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:8a8c5fece2a1c7587feb41cbe04c6aee08e768ecd9b5d00da6af9832a4ccc842
+size 2034

openpilot/selfdrive/assets/icons_mici/egpu_gray.png

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:7409c53d7c72681c24982fd83b56ce70f80797c9c0f936d9296a5c18557ac472
+size 7279

openpilot/selfdrive/assets/icons_mici/egpu_orange.png

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:58bd6155433f623b1f75d134bd8ca4745d9aa71f6767eb807cdbcf7deb3089a1
+size 10876

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/drive_helpers.py

@@ -14,8 +14,11 @@ MAX_LATERAL_JERK = 5.0  # m/s^3
 MAX_LATERAL_ACCEL_NO_ROLL = 3.0  # m/s^2
 
 
+STOPPING_SPEED = 0.25  # m/s, speed at which the car goes into the stopping state
+
+
 def should_stop(v_ego: float, a_target: float) -> bool:
-  return bool(v_ego < 0.3 and a_target < 0.1)
+  return bool(v_ego < STOPPING_SPEED and a_target < 0.1)
 
 def clamp(val, min_val, max_val):
   clamped_val = float(np.clip(val, min_val, max_val))

openpilot/selfdrive/controls/lib/longcontrol.py

@@ -7,6 +7,8 @@ from openpilot.selfdrive.modeld.constants import ModelConstants
 
 CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
 
+STOPPING_DECEL_RATE = 0.3  # m/s^2/s while trying to stop
+
 LongCtrlState = car.CarControl.Actuators.LongControlState
 
 
@@ -68,7 +70,7 @@ class LongControl:
       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 -= STOPPING_DECEL_RATE * DT_CTRL
       self.reset()
 
     else:  # LongCtrlState.pid

openpilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -35,9 +35,12 @@ def get_max_accel(v_ego):
 def get_coast_accel(pitch):
   return np.sin(pitch) * -5.65 - 0.3  # fitted from data using xx/projects/allow_throttle/compute_coast_accel.py
 
-def get_cruise_accel(e2e, v_cruise, v_ego, a_cruise_prev, angle_steers, CP, dt, accel_coast, allow_throttle):
-  max_accel = ACCEL_MAX if e2e else get_max_accel(v_ego)
-
+def get_cruise_accel(e2e, v_cruise, v_ego, a_cruise_prev, angle_steers, CP, dt, accel_coast, allow_throttle,
+                      max_accel_override=None):
+  if max_accel_override is not None:
+    max_accel = max_accel_override
+  else:
+    max_accel = ACCEL_MAX if e2e else get_max_accel(v_ego)
   if not e2e:
     a_total_max = np.interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V)
     a_y = v_ego ** 2 * angle_steers * CV.DEG_TO_RAD / (CP.steerRatio * CP.wheelbase)
@@ -84,7 +87,8 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     v_ego = sm['carState'].vEgo
     v_cruise_kph = min(sm['carState'].vCruise, V_CRUISE_MAX)
     v_cruise = v_cruise_kph * CV.KPH_TO_MS
-    if sm['controlsState'].forceDecel:
+    force_decel = sm['controlsState'].forceDecel
+    if force_decel:
       v_cruise = 0.0
 
     long_control_off = sm['controlsState'].longControlState == LongCtrlState.off
@@ -118,6 +122,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality)
     self.mpc.set_cur_state(self.v_desired_filter.x, self.output_a_target)
     self.mpc.update(sm['radarState'], personality=sm['selfdriveState'].personality)
+    self.update_dec(sm)
 
     self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)
     self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution)
@@ -140,9 +145,17 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
 
     is_e2e = self.is_e2e(sm)
 
-    self.a_cruise = get_cruise_accel(is_e2e, v_cruise, v_ego,
-                                     self.a_cruise, steer_angle_without_offset, self.CP, self.dt,
-                                     accel_coast, self.allow_throttle)
+    max_accel_override = self.get_max_accel_override(v_ego)
+    v_cruise = self.get_cruise_target_override(v_ego, v_cruise, force_decel)
+    a_cruise_prev = self.a_cruise
+    gated_cruise = get_cruise_accel(is_e2e, v_cruise, v_ego, a_cruise_prev, steer_angle_without_offset,
+                                    self.CP, self.dt, accel_coast, self.allow_throttle, max_accel_override)
+    ungated_cruise = get_cruise_accel(is_e2e, v_cruise, v_ego, a_cruise_prev, steer_angle_without_offset,
+                                      self.CP, self.dt, accel_coast, True, max_accel_override)
+    self.a_cruise = self.arbitrate_cruise_candidate(
+      sm, gated_cruise, ungated_cruise, output_a_target_mpc, self.mpc.source,
+      allow_throttle=self.allow_throttle, e2e=is_e2e, force_decel=force_decel,
+    )
     cruise_should_stop = should_stop(v_ego, self.a_cruise)
 
     candidates = [(output_a_target_mpc, self.mpc.source, output_should_stop_mpc),
@@ -153,6 +166,8 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     output_a_target, self.mpc.source, _ = min(candidates, key=lambda c: c[0])
     self.output_should_stop = any(should_stop for _, _, should_stop in candidates)
     self.output_a_target = np.clip(output_a_target, ACCEL_MIN, ACCEL_MAX)
+    self.accel_controller_active = bool(self.accel_controller.is_enabled() and not force_decel and
+                                        self.mpc.source == LongitudinalPlanSource.cruise)
 
     self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.output_a_target + a_prev) / 2.0
 
@@ -177,6 +192,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     longitudinalPlan.aTarget = float(self.output_a_target)
     longitudinalPlan.shouldStop = bool(self.output_should_stop)
     longitudinalPlan.allowBrake = True
+    # Raw model throttle intent used for path visualization; lead MPC can still request positive acceleration.
     longitudinalPlan.allowThrottle = bool(self.allow_throttle)
 
     pm.send('longitudinalPlan', plan_send)

openpilot/selfdrive/controls/tests/test_longcontrol.py

@@ -1,6 +1,7 @@
 from openpilot.common.test import OpenpilotTestCase
 from openpilot.cereal import custom
-from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState, long_control_state_trans
+from openpilot.selfdrive.controls.lib.drive_helpers import STOPPING_SPEED, should_stop
+from openpilot.selfdrive.controls.lib.longcontrol import STOPPING_DECEL_RATE, LongCtrlState, long_control_state_trans
 
 
 class TestLongControlStateTransition(OpenpilotTestCase):
@@ -42,3 +43,13 @@ class TestLongControlStateTransition(OpenpilotTestCase):
     next_state = long_control_state_trans(CP_SP, active, current_state,
                              should_stop=False, brake_pressed=False, cruise_standstill=False)
     assert next_state == LongCtrlState.pid
+
+class TestTerminalStop(OpenpilotTestCase):
+  def test_stopping_tune_is_gentler_than_upstream_default(self):
+    # Upstream #38394 hardcoded a 1.0 m/s^2/s ramp and a 0.3 m/s latch. comma's own one-stopping-tune uses
+    # 0.3 / 0.25, and every stop recorded on this car was driven with that pair. Both must stay on the less
+    # braking side, or a future edit re-deepens the terminal brake unnoticed - which already happened once.
+    assert 0.0 < STOPPING_DECEL_RATE <= 1.0
+    assert 0.0 < STOPPING_SPEED <= 0.3
+    assert should_stop(STOPPING_SPEED - 0.01, 0.0)
+    assert not should_stop(0.29, 0.0)  # the band upstream would latch in and we do not

openpilot/selfdrive/modeld/SConscript

@@ -7,7 +7,7 @@ from openpilot.common.file_chunker import chunk_file, get_chunk_targets, get_exi
 from openpilot.common.transformations.camera import _ar_ox_fisheye, _os_fisheye
 from openpilot.common.transformations.model import MEDMODEL_INPUT_SIZE, DM_INPUT_SIZE
 from openpilot.selfdrive.modeld.constants import ModelConstants
-from openpilot.selfdrive.modeld.helpers import TG_INPUT_DEVICES_PATH, chestnut_present, modeld_pkl_path
+from openpilot.selfdrive.modeld.helpers import TG_INPUT_DEVICES_PATH, usbgpu_present, modeld_pkl_path
 
 
 CAMERA_CONFIGS = [
@@ -36,18 +36,18 @@ else:
 tg_devices = { # which device to put jit inputs to at runtime
   'openpilot.selfdrive.modeld.modeld': {
     'default': {'WARP_DEV': tg_backend, 'QUEUE_DEV': tg_backend},
-    'chestnut': {'WARP_DEV': tg_backend, 'QUEUE_DEV': 'AMD'}
+    'usbgpu': {'WARP_DEV': tg_backend, 'QUEUE_DEV': 'AMD'}
   },
   'openpilot.selfdrive.modeld.dmonitoringmodeld': {
     'default': {'DEV': tg_backend}
   },
 }
 
-CHESTNUT = chestnut_present()
-if CHESTNUT:
-  chestnut_tg_flags = f'DEBUG=2 DEV=USB+AMD:LLVM WARP_DEV={tg_backend} FLOAT16=1 JIT_BATCH_SIZE=0 GMMU=0 TC_OPT=2'
+USBGPU = usbgpu_present()
+if USBGPU:
+  usbgpu_tg_flags = f'DEBUG=2 DEV=USB+AMD:LLVM WARP_DEV={tg_backend} FLOAT16=1 JIT_BATCH_SIZE=0 GMMU=0 TC_OPT=2'
   # the USB+AMD GPU takes an exclusive flock; serialize all targets that touch it
-  chestnut_lock = File("models/.chestnut.lock").abspath
+  usbgpu_lock = File("models/.usb_gpu.lock").abspath
 
 def write_tg_devices(target, source, env):
   with open(str(target[0]), "w") as f:
@@ -74,10 +74,10 @@ model_w, model_h = MEDMODEL_INPUT_SIZE
 frame_skip = ModelConstants.MODEL_RUN_FREQ // ModelConstants.MODEL_CONTEXT_FREQ
 
 if not os.getenv('SKIP_TINYGRAD_COMPILE'):
-  for chestnut in [False, True] if CHESTNUT else [False]:
-    target_pkl_path = File(modeld_pkl_path(chestnut)).abspath
-    # BIG_INTO_SMALL=1 builds the default target from the big model, e.g. to test it without a chestnut
-    file_prefix, cmd_flags = ('big_', chestnut_tg_flags) if chestnut else ('big_' if os.getenv('BIG_INTO_SMALL') else '', tg_flags)
+  for usbgpu in [False, True] if USBGPU else [False]:
+    target_pkl_path = File(modeld_pkl_path(usbgpu)).abspath
+    # BIG_INTO_SMALL=1 builds the default target from the big model, e.g. to test it without a USB GPU
+    file_prefix, cmd_flags = ('big_', usbgpu_tg_flags) if usbgpu else ('big_' if os.getenv('BIG_INTO_SMALL') else '', tg_flags)
     driving_onnx_deps = get_existing_chunks(File(f"models/{file_prefix}driving_supercombo.onnx").abspath)
     camera_res_args = ' '.join(f'{cw}x{ch}' for cw, ch in CAMERA_CONFIGS)
     # CPU 7 is isolated with isolcpus on AGNOS, so explicitly pin the compiler to it.
@@ -104,14 +104,14 @@ if not os.getenv('SKIP_TINYGRAD_COMPILE'):
       chunk_file(pkl, chunks)
     def do_chunk(target, source, env, pkl=target_pkl_path, chunks=chunk_targets):
       chunk_file(pkl, chunks)
-    actions = Action(do_compile, "  [CHESTNUT] $TARGET") if chestnut else [cmd, Action(do_chunk, "  [CHUNK] $TARGET")]
+    actions = Action(do_compile, "  [USBGPU] $TARGET") if usbgpu else [cmd, Action(do_chunk, "  [CHUNK] $TARGET")]
     node = lenv.Command(
       chunk_targets,
       tinygrad_files + compile_modeld_script + driving_onnx_deps + [Value(chunk_targets), chunker_file],
       actions,
     )
-    if chestnut:
-      lenv.SideEffect(chestnut_lock, node)
+    if usbgpu:
+      lenv.SideEffect(usbgpu_lock, node)
 
 # get model metadata
 fn = File(f"models/dmonitoring_model").abspath

openpilot/selfdrive/modeld/dmonitoringmodeld.py

@@ -29,7 +29,7 @@ class ModelState:
   output: np.ndarray
 
   def __init__(self, cam_w: int, cam_h: int):
-    self.DEV = get_tg_input_devices(PROCESS_NAME, chestnut=False)['DEV']
+    self.DEV = get_tg_input_devices(PROCESS_NAME, usbgpu=False)['DEV']
     with open(METADATA_PATH, 'rb') as f:
       model_metadata = pickle.load(f)
       self.input_shapes = model_metadata['input_shapes']

openpilot/selfdrive/modeld/helpers.py

@@ -13,12 +13,12 @@ MODELS_DIR = Path(__file__).resolve().parent / 'models'
 TG_INPUT_DEVICES_PATH = MODELS_DIR / 'tg_input_devices.json'
 
 
-def get_tg_input_devices(process_name: str, chestnut: bool):
+def get_tg_input_devices(process_name: str, usbgpu: bool):
   with open(TG_INPUT_DEVICES_PATH) as f:
-    return json.load(f)[process_name]['default' if not chestnut else 'chestnut']
+    return json.load(f)[process_name]['default' if not usbgpu else 'usbgpu']
 
-def modeld_pkl_path(chestnut: bool):
-  prefix = 'big_' if chestnut else ''
+def modeld_pkl_path(usbgpu: bool):
+  prefix = 'big_' if usbgpu else ''
   return MODELS_DIR / f'{prefix}driving_tinygrad.pkl'
 
 def dump_oob(obj, f):
@@ -45,7 +45,7 @@ def load_oob(f):
       yield pb
   return pickle.load(io.BytesIO(opcodes), buffers=buffers())
 
-def chestnut_present() -> bool:
+def usbgpu_present() -> bool:
   for d in USB_DEVICES_PATH.glob("*"):
     try:
       usb_id = (int((d / "idVendor").read_text(), 16), int((d / "idProduct").read_text(), 16))
@@ -56,5 +56,5 @@ def chestnut_present() -> bool:
       pass
   return False
 
-def chestnut_compiled() -> bool:
-  return Path(get_manifest_path(modeld_pkl_path(chestnut=True))).is_file()
+def usbgpu_compiled() -> bool:
+  return Path(get_manifest_path(modeld_pkl_path(usbgpu=True))).is_file()

openpilot/selfdrive/modeld/modeld.py

@@ -1,9 +1,7 @@
 #!/usr/bin/env python3
-from collections.abc import Callable
-import ctypes
 from functools import cached_property
 import os
-os.environ['GMMU'] = '0' # for chestnut fast loading, noop for qcom
+os.environ['GMMU'] = '0' # for usbgpu fast loading, noop for qcom
 from tinygrad.tensor import Tensor
 from tinygrad.device import Device
 import struct
@@ -32,7 +30,7 @@ from openpilot.selfdrive.modeld.compile_modeld import make_input_queues, WARP_IN
 from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_driving_model_data, fill_pose_msg, PublishState
 from openpilot.common.file_chunker import open_file_chunked
 from openpilot.selfdrive.modeld.constants import ModelConstants, Plan
-from openpilot.selfdrive.modeld.helpers import chestnut_present, chestnut_compiled, modeld_pkl_path, get_tg_input_devices, load_oob
+from openpilot.selfdrive.modeld.helpers import usbgpu_present, usbgpu_compiled, modeld_pkl_path, get_tg_input_devices, load_oob
 
 from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
 from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
@@ -96,10 +94,8 @@ class ChestnutState:
     if self.big and "AMD" in Device._opened_devices and self.sends % 100 == 1:
       try:
         smu = Device["AMD"].iface.dev_impl.smu
-        metrics_t = smu.smu_mod.SmuMetricsExternal_t
         smu._send_msg(smu.smu_mod.PPSMC_MSG_TransferTableSmu2Dram, smu.smu_mod.TABLE_SMU_METRICS, timeout=100)
-        metrics_buf = bytearray(smu.adev.vram.view(smu.driver_table_paddr, ctypes.sizeof(metrics_t))[:])
-        metrics = metrics_t.from_buffer(metrics_buf).SmuMetrics
+        metrics = smu.read_table(smu.smu_mod.SmuMetricsExternal_t, smu.smu_mod.TABLE_SMU_METRICS).SmuMetrics
         self.metrics = {'tempC': metrics.AvgTemperature[smu.smu_mod.TEMP_HOTSPOT],
                         'memoryTempC': metrics.AvgTemperature[smu.smu_mod.TEMP_MEM],
                         'powerDrawW': metrics.AverageSocketPower,
@@ -145,18 +141,18 @@ class FrameMeta:
 class ModelState(ModelStateBase):
   prev_desire: np.ndarray  # for tracking the rising edge of the pulse
 
-  def __init__(self, cam_w: int, cam_h: int, chestnut: bool):
+  def __init__(self, cam_w: int, cam_h: int, usbgpu: bool):
     ModelStateBase.__init__(self)
-    input_devices = get_tg_input_devices(PROCESS_NAME, chestnut)
+    input_devices = get_tg_input_devices(PROCESS_NAME, usbgpu)
     self.WARP_DEV, self.QUEUE_DEV = input_devices['WARP_DEV'], input_devices['QUEUE_DEV']
-    jits = load_oob(open_file_chunked(modeld_pkl_path(chestnut)))
+    jits = load_oob(open_file_chunked(modeld_pkl_path(usbgpu)))
     metadata = jits['metadata']
     self.input_shapes = metadata['input_shapes']
     self.vision_input_names = [k for k in self.input_shapes if 'img' in k]
     self.output_slices = metadata['output_slices']
 
     self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32)
-    self.chestnut = chestnut
+    self.usbgpu = usbgpu
 
     self.frame_skip = ModelConstants.MODEL_RUN_FREQ // ModelConstants.MODEL_CONTEXT_FREQ
     self.input_queues, self.npy = make_input_queues(self.input_shapes, self.frame_skip, device=self.QUEUE_DEV)
@@ -172,7 +168,7 @@ class ModelState(ModelStateBase):
     return parsed_model_outputs
 
   def run(self, bufs: dict[str, VisionBuf], transforms: dict[str, np.ndarray],
-          inputs: dict[str, np.ndarray], after_enqueue: Callable[[], None] | None = None) -> dict[str, np.ndarray]:
+          inputs: dict[str, np.ndarray]) -> dict[str, np.ndarray] | None:
     for key in bufs.keys():
       ptr = np.frombuffer(bufs[key].data, dtype=np.uint8).ctypes.data
       yuv_size = self.frame_buf_params[key][3]
@@ -196,11 +192,11 @@ class ModelState(ModelStateBase):
     outs, = self.run_policy(
       **{k: self.input_queues[k] for k in POLICY_INPUTS if k in self.input_queues}, warped=warped
     )
-    if after_enqueue is not None:
-      after_enqueue()
     model_output = outs.numpy()[0]
-    if self.chestnut and not np.all(np.isfinite(model_output)):
-      raise RuntimeError("model output not finite")
+    if self.usbgpu and not np.all(np.isfinite(model_output)):
+      # TODO remove with prev_feat
+      cloudlog.error("model output not finite, dropping frame")
+      return None
     outputs_dict = self.parser.parse_outputs(self.slice_outputs(model_output, self.output_slices))
     self.npy['prev_feat'][:] = model_output[self.output_slices['hidden_state']]
 
@@ -222,12 +218,12 @@ class ModelState(ModelStateBase):
 def main(demo=False):
   cloudlog.warning("modeld init")
 
-  CHESTNUT = chestnut_present() and chestnut_compiled()
-  if CHESTNUT:
+  USBGPU = usbgpu_present() and usbgpu_compiled()
+  if USBGPU:
     os.environ['HCQDEV_WAIT_TIMEOUT_MS'] = '3000'
   params = Params()
-  params.put_bool("ChestnutLoading", CHESTNUT)
-  params.remove("ChestnutActive")
+  params.put_bool("UsbGpuLoading", USBGPU)
+  params.remove("UsbGpuActive")
 
   config_realtime_process(7, 54)
 
@@ -257,7 +253,7 @@ def main(demo=False):
   st = time.monotonic()
   cloudlog.warning("loading model")
   model = None
-  if CHESTNUT:
+  if USBGPU:
     big_model = None
     def load_big():
       nonlocal big_model
@@ -271,23 +267,23 @@ def main(demo=False):
     loader.start()
     loader.join(BIG_MODEL_TIMEOUT)
     model = big_model
-    params.put_bool("ChestnutActive", model is not None)
+    params.put_bool("UsbGpuActive", model is not None)
 
-  small_model = ModelState(vipc_client_main.width, vipc_client_main.height, False) if model is None or CHESTNUT else None
+  small_model = ModelState(vipc_client_main.width, vipc_client_main.height, False) if model is None or USBGPU else None
   if model is None:
     model = small_model
-  params.put_bool("ChestnutLoading", False)
+  params.put_bool("UsbGpuLoading", False)
   assert model is not None
   cloudlog.warning(f"models loaded in {time.monotonic() - st:.1f}s, modeld starting")
 
   # messaging
-  pub_socks = ["modelV2", "drivingModelData", "cameraOdometry", "modelDataV2SP"] + (["chestnutState"] if CHESTNUT else [])
+  pub_socks = ["modelV2", "drivingModelData", "cameraOdometry", "modelDataV2SP"] + (["chestnutState"] if USBGPU else [])
   pm = PubMaster(pub_socks)
   sm = SubMaster(["deviceState", "carState", "narrowRoadCameraState", "extrinsicsCalibration", "driverMonitoringState", "carControl", "lateralDelay"])
 
   publish_state = PublishState()
   params = Params()
-  chestnut_state = ChestnutState(pm, model.chestnut) if CHESTNUT else None
+  chestnut_state = ChestnutState(pm, model.usbgpu) if USBGPU else None
 
   # setup filter to track dropped frames
   frame_dropped_filter = FirstOrderFilter(0., 10., 1. / ModelConstants.MODEL_RUN_FREQ)
@@ -397,15 +393,13 @@ def main(demo=False):
 
     mt1 = time.perf_counter()
     try:
-      send_chestnut = (chestnut_state is not None and
-                       run_count % round(ModelConstants.MODEL_RUN_FREQ / SERVICE_LIST['chestnutState'].frequency) == 0)
-      model_output = model.run(bufs, transforms, inputs, chestnut_state.send if send_chestnut else None)
+      model_output = model.run(bufs, transforms, inputs)
     except Exception:
-      if not params.get_bool("ChestnutActive"):
+      if not params.get_bool("UsbGpuActive"):
         raise
       # fallback to small model
       cloudlog.exception("big model failed, fall back to small")
-      params.put_bool("ChestnutActive", False)
+      params.put_bool("UsbGpuActive", False)
       assert small_model is not None
       model = small_model
       if chestnut_state is not None:
@@ -425,7 +419,7 @@ def main(demo=False):
       fill_model_msg(modelv2_send, model_output, action,
                      publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id,
                      frame_drop_ratio, meta_main.timestamp_eof, model_execution_time, extrinsics_calibration_seen)
-      modelv2_send.modelV2.big = model.chestnut
+      modelv2_send.modelV2.big = model.usbgpu
 
       desire_state = modelv2_send.modelV2.meta.desireState
       l_lane_change_prob = desire_state[log.Desire.laneChangeLeft]
@@ -447,6 +441,10 @@ def main(demo=False):
       pm.send('modelDataV2SP', mdv2sp_send)
     last_vipc_frame_id = meta_main.frame_id
 
+    if chestnut_state is not None and run_count % round(ModelConstants.MODEL_RUN_FREQ / SERVICE_LIST['chestnutState'].frequency) == 0:
+      chestnut_state.send()
+
+
 if __name__ == "__main__":
   try:
     import argparse

openpilot/selfdrive/pandad/spi.cc

@@ -29,12 +29,6 @@ enum SpiError {
 
 const unsigned int SPI_ACK_TIMEOUT = 500; // milliseconds
 const std::string SPI_DEVICE = "/dev/spidev0.0";
-// TODO: fix SPI turnaround synchronization at the protocol level.
-static uint64_t spi_last_bus_activity_ns = 0;  // protected by hw_lock
-
-static void wait_for_spi_turnaround(uint64_t start_ns) {
-  while ((nanos_since_boot() - start_ns) < 400000) {}
-}
 
 class LockEx {
 public:
@@ -325,8 +319,6 @@ int PandaSpiHandle::spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx
   assert(tx_len < SPI_BUF_SIZE);
   assert(max_rx_len < SPI_BUF_SIZE);
 
-  wait_for_spi_turnaround(spi_last_bus_activity_ns);
-
   xfer_count++;
   header = {
     .sync = SPI_SYNC,
@@ -355,7 +347,6 @@ int PandaSpiHandle::spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx
   if (ret < 0) {
     goto fail;
   }
-  wait_for_spi_turnaround(nanos_since_boot());
 
   // Send data
   if (tx_data != NULL) {
@@ -398,7 +389,6 @@ int PandaSpiHandle::spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx
     memcpy(rx_data, rx_buf + 3, rx_data_len);
   }
 
-  spi_last_bus_activity_ns = nanos_since_boot();
   return rx_data_len;
 
 fail:
@@ -413,7 +403,6 @@ fail:
     }
   }
 
-  spi_last_bus_activity_ns = nanos_since_boot();
   if (ret >= 0) ret = -1;
   return ret;
 }

openpilot/selfdrive/selfdrived/selfdrived.py

@@ -195,17 +195,17 @@ class SelfdriveD(CruiseHelper):
       self.events.add(EventName.joystickDebug)
       self.startup_event = None
 
-    loading = self.params.get_bool("ChestnutLoading")
+    loading = self.params.get_bool("UsbGpuLoading")
     if self.big_model_loading and not loading:
       self.big_model_ready_t = time.monotonic()
     self.big_model_loading = loading
     if self.big_model_loading:
       self.events.add(EventName.bigModelLoading)
 
-    big_active = self.params.get("ChestnutActive")
-    chestnut_present = self.sm['deviceState'].chestnutPresent
+    big_active = self.params.get("UsbGpuActive")
+    usbgpu_present = self.sm['deviceState'].chestnutPresent
     model_unavailable = big_active is True and self.sm.seen['modelV2'] and not self.sm.alive['modelV2']
-    big_failed = big_active is False or model_unavailable or (self.big_model_active and not chestnut_present)
+    big_failed = big_active is False or model_unavailable or (self.big_model_active and not usbgpu_present)
     if big_failed and not self.big_model_failed:
       self.events.add(EventName.bigModelFailed)
     self.big_model_failed = big_failed

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/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(
+    "Lets you choose how sunnypilot starts, catches up, and settles at the cruise speed. Emergency braking and stopping are unchanged."
+  ),
+  "AccelPersonality": tr_noop(
+    "Eco is gentlest, Normal balances a prompt start with smooth catch-up, and Sport is more responsive."
+  ),
   "IsLdwEnabled": tr_noop(
     "Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line " +
     "without a turn signal activated while driving over 31 mph (50 km/h)."
@@ -106,6 +112,24 @@ class TogglesLayout(Widget):
       icon="speed_limit.png"
     )
 
+    self._accel_controller_enabled = toggle_item(
+      lambda: tr("Enable Accel Controller"),
+      lambda: tr(DESCRIPTIONS["AccelPersonalityEnabled"]),
+      self._params.get_bool("AccelPersonalityEnabled"),
+      callback=self._set_accel_controller_enabled,
+      icon="speed_limit.png",
+    )
+
+    self._accel_personality_setting = multiple_button_item(
+      lambda: tr("Acceleration Profile"),
+      lambda: tr(DESCRIPTIONS["AccelPersonality"]),
+      buttons=[lambda: tr("Eco"), lambda: tr("Normal"), lambda: tr("Sport")],
+      button_width=300,
+      callback=self._set_accel_personality,
+      selected_index=self._params.get("AccelPersonality", return_default=True),
+      icon="speed_limit.png"
+    )
+
     self._toggles = {}
     self._locked_toggles = set()
     for param, (title, desc, icon, needs_restart) in self._toggle_defs.items():
@@ -135,9 +159,11 @@ class TogglesLayout(Widget):
 
       self._toggles[param] = toggle
 
-      # insert longitudinal personality after NDOG toggle
+      # insert longitudinal personality and Accel Controller settings after NDOG toggle
       if param == "DisengageOnAccelerator":
         self._toggles["LongitudinalPersonality"] = self._long_personality_setting
+        self._toggles["AccelPersonalityEnabled"] = self._accel_controller_enabled
+        self._toggles["AccelPersonality"] = self._accel_personality_setting
 
     self._update_experimental_mode_icon()
     self._scroller = Scroller(list(self._toggles.values()), line_separator=True, spacing=0)
@@ -158,6 +184,7 @@ class TogglesLayout(Widget):
 
   def _update_toggles(self):
     ui_state.update_params()
+    accel_controller_enabled = self._params.get_bool("AccelPersonalityEnabled")
 
     e2e_description = tr(
       "sunnypilot defaults to driving in chill mode. Experimental mode enables alpha-level features that aren't ready for chill mode. " +
@@ -176,11 +203,15 @@ class TogglesLayout(Widget):
         self._toggles["ExperimentalMode"].action_item.set_enabled(True)
         self._toggles["ExperimentalMode"].set_description(e2e_description)
         self._long_personality_setting.action_item.set_enabled(True)
+        self._accel_controller_enabled.action_item.set_enabled(True)
+        self._accel_personality_setting.action_item.set_enabled(True)
       else:
         # no long for now
         self._toggles["ExperimentalMode"].action_item.set_enabled(False)
         self._toggles["ExperimentalMode"].action_item.set_state(False)
         self._long_personality_setting.action_item.set_enabled(False)
+        self._accel_controller_enabled.action_item.set_enabled(False)
+        self._accel_personality_setting.action_item.set_enabled(False)
         self._params.remove("ExperimentalMode")
 
         unavailable = tr("Experimental mode is currently unavailable on this car since the car's stock ACC is used for longitudinal control.")
@@ -203,6 +234,8 @@ class TogglesLayout(Widget):
     # refresh toggles from params to mirror external changes
     for param in self._toggle_defs:
       self._toggles[param].action_item.set_state(self._params.get_bool(param))
+    self._accel_controller_enabled.action_item.set_state(accel_controller_enabled)
+    self._accel_personality_setting.action_item.set_selected_button(self._params.get("AccelPersonality", return_default=True))
 
     # these toggles need restart, block while engaged
     for toggle_def in self._toggle_defs:
@@ -247,3 +280,9 @@ class TogglesLayout(Widget):
 
   def _set_longitudinal_personality(self, button_index: int):
     self._params.put("LongitudinalPersonality", button_index, block=True)
+
+  def _set_accel_personality(self, button_index: int):
+    self._params.put("AccelPersonality", button_index, block=True)
+
+  def _set_accel_controller_enabled(self, state: bool):
+    self._params.put_bool("AccelPersonalityEnabled", state, block=True)

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

@@ -9,7 +9,7 @@ from openpilot.system.ui.widgets.layouts import HBoxLayout
 from openpilot.system.ui.widgets.icon_widget import IconWidget
 from openpilot.system.ui.widgets.label import UnifiedLabel, gui_label
 from openpilot.system.ui.lib.application import gui_app, FontWeight, MousePos
-from openpilot.selfdrive.ui.ui_state import ui_state, ChestnutState
+from openpilot.selfdrive.ui.ui_state import ui_state
 from openpilot.common.version import RELEASE_BRANCHES
 
 HEAD_BUTTON_FONT_SIZE = 40
@@ -139,8 +139,8 @@ class MiciHomeLayout(Widget):
     self._version_text = self._get_version_text()
 
     self._experimental_icon = IconWidget("icons_mici/experimental_mode.png", (48, 48))
-    self._chestnut_icon = IconWidget("icons_mici/chestnut_green.png", (68, 40))
-    self._chestnut_failed_icon = IconWidget("icons_mici/chestnut_orange.png", (68, 40))
+    self._egpu_icon = IconWidget("icons_mici/egpu_green.png", (50, 37))
+    self._egpu_icon_gray = IconWidget("icons_mici/egpu_gray.png", (50, 37))
     self._mic_icon = IconWidget("icons_mici/microphone.png", (32, 46))
     self._body_icon = IconWidget("icons_mici/body.png", (54, 37))
 
@@ -150,8 +150,8 @@ class MiciHomeLayout(Widget):
       IconWidget("icons_mici/settings.png", (48, 48), opacity=0.9),
       NetworkIcon(),
       self._experimental_icon,
-      self._chestnut_icon,
-      self._chestnut_failed_icon,
+      self._egpu_icon,
+      self._egpu_icon_gray,
       self._body_icon,
       self._mic_icon,
     ], spacing=18)
@@ -249,10 +249,10 @@ class MiciHomeLayout(Widget):
     # ***** Center-aligned bottom section icons *****
     self._experimental_icon.set_visible(ui_state.experimental_mode)
     if gui_app.sunnypilot_ui():
-      self._set_chestnut_visibility()
+      self._set_egpu_visibility()
     else:
-      self._chestnut_icon.set_visible(ui_state.chestnut_state in (ChestnutState.READY, ChestnutState.LOADING, ChestnutState.ACTIVE))
-      self._chestnut_failed_icon.set_visible(ui_state.chestnut_state in (ChestnutState.UNCOMPILED, ChestnutState.FAILED))
+      self._egpu_icon.set_visible(ui_state.sm["deviceState"].chestnutPresent and ui_state.usbgpu_compiled)
+      self._egpu_icon_gray.set_visible(ui_state.sm["deviceState"].chestnutPresent and not ui_state.usbgpu_compiled)
     self._mic_icon.set_visible(ui_state.recording_audio)
     self._body_icon.set_visible(bool(ui_state.is_body))
 

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

@@ -14,6 +14,7 @@ from openpilot.system.ui.lib.application import gui_app
 if gui_app.sunnypilot_ui():
   from openpilot.selfdrive.ui.sunnypilot.mici.layouts.settings import SettingsLayoutSP as SettingsLayout
   from openpilot.selfdrive.ui.sunnypilot.mici.layouts.home import MiciHomeLayoutSP as MiciHomeLayout
+  from openpilot.selfdrive.ui.sunnypilot.mici.layouts.onroad import OnroadViewContainerSP as AugmentedRoadView
 
 ONROAD_DELAY = 2.5  # seconds
 
@@ -72,6 +73,9 @@ class MiciMainLayout(Scroller):
     # For scroll_to
     return self._body_onroad_layout if ui_state.is_body else self._car_onroad_layout
 
+  def _should_auto_scroll_to_onroad(self) -> bool:
+    return True
+
   def _setup_callbacks(self):
     self._home_layout.set_callbacks(
       on_settings=lambda: gui_app.push_widget(self._settings_layout),
@@ -122,13 +126,15 @@ class MiciMainLayout(Scroller):
 
     # FIXME: these two pops can interrupt user interacting in the settings
     if self._onroad_time_delay is not None and rl.get_time() - self._onroad_time_delay >= ONROAD_DELAY:
-      gui_app.pop_widgets_to(self, lambda: self._scroll_to(self._onroad_layout))
+      if not gui_app.sunnypilot_ui() or self._should_auto_scroll_to_onroad():
+        gui_app.pop_widgets_to(self, lambda: self._scroll_to(self._onroad_layout))
       self._onroad_time_delay = None
 
     # When car leaves standstill, pop nav stack and scroll to onroad
     CS = ui_state.sm["carState"]
     if not CS.standstill and self._prev_standstill:
-      gui_app.pop_widgets_to(self, lambda: self._scroll_to(self._onroad_layout))
+      if not gui_app.sunnypilot_ui() or self._should_auto_scroll_to_onroad():
+        gui_app.pop_widgets_to(self, lambda: self._scroll_to(self._onroad_layout))
     self._prev_standstill = CS.standstill
 
   def _on_interactive_timeout(self):

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/hud_renderer.py

@@ -3,7 +3,7 @@ import pyray as rl
 from dataclasses import dataclass
 from openpilot.common.constants import CV
 from openpilot.selfdrive.ui.mici.onroad.torque_bar import TorqueBar
-from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus, ChestnutState
+from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus
 from openpilot.system.ui.lib.application import gui_app, FontWeight
 from openpilot.system.ui.lib.multilang import tr
 from openpilot.system.ui.lib.text_measure import measure_text_cached
@@ -107,7 +107,8 @@ class HudRenderer(Widget):
     self.speed: float = 0.0
     self.v_ego_cluster_seen: bool = False
     self._engaged: bool = False
-    self._chestnut_fade_time: float = 0
+    self._small_model_engaged: bool = False
+    self._egpu_fade_time: float = 0
 
     self._can_draw_top_icons = True
     self._show_wheel_critical = False
@@ -123,15 +124,17 @@ class HudRenderer(Widget):
     self._txt_wheel: rl.Texture = gui_app.texture('icons_mici/wheel.png', 50, 50)
     self._txt_wheel_critical: rl.Texture = gui_app.texture('icons_mici/wheel_critical.png', 50, 50)
     self._txt_exclamation_point: rl.Texture = gui_app.texture('icons_mici/exclamation_point.png', 9, 44)
-    self._txt_chestnut: rl.Texture = gui_app.texture('icons_mici/chestnut.png', 60, 44)
-    self._txt_chestnut_green: rl.Texture = gui_app.texture('icons_mici/chestnut_green.png', 60, 44)
-    self._txt_chestnut_orange: rl.Texture = gui_app.texture('icons_mici/chestnut_orange.png', 75, 44)
-    self._chestnut_icon: rl.Texture | None = None
+    self._txt_egpu: rl.Texture = gui_app.texture('icons_mici/egpu.png', 60, 44)
+    self._txt_egpu_green: rl.Texture = gui_app.texture('icons_mici/egpu_green.png', 60, 44)
+    self._txt_egpu_orange: rl.Texture = gui_app.texture('icons_mici/egpu_orange.png', 60, 44)
+    self._txt_egpu_crossed: rl.Texture = gui_app.texture('icons_mici/egpu_crossed.png', 60, 52)
+    self._egpu_icon: rl.Texture | None = None
+
     self._wheel_alpha_filter = FirstOrderFilter(0, 0.05, 1 / gui_app.target_fps)
     self._wheel_y_filter = FirstOrderFilter(0, 0.1, 1 / gui_app.target_fps)
 
     self._set_speed_alpha_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps)
-    self._chestnut_alpha_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps)
+    self._egpu_alpha_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps)
 
   def set_wheel_critical_icon(self, critical: bool):
     """Set the wheel icon to critical or normal state."""
@@ -162,10 +165,13 @@ class HudRenderer(Widget):
       controls_state.deprecated.vCruise if v_cruise_cluster == 0.0 else v_cruise_cluster
     )
     engaged = sm['selfdriveState'].enabled
+    if (engaged and not self._engaged and not ui_state.usbgpu_loading and ui_state.usbgpu_active is not True and
+        ui_state.sm.recv_frame['modelV2'] > ui_state.started_frame):
+      self._small_model_engaged = True
+    if engaged != self._engaged:
+      self._egpu_fade_time = rl.get_time() if engaged else 0
     if (set_speed != self.set_speed and engaged) or (engaged and not self._engaged):
       self._set_speed_changed_time = rl.get_time()
-    if engaged != self._engaged:
-      self._chestnut_fade_time = rl.get_time() if engaged else 0
     self._engaged = engaged
     self.set_speed = set_speed
     self.is_cruise_set = 0 < self.set_speed < SET_SPEED_NA
@@ -185,7 +191,8 @@ class HudRenderer(Widget):
     if self.is_cruise_set:
       self._draw_set_speed(rect)
 
-    self._draw_model_source(rect)
+    if ui_state.usbgpu and ui_state.usbgpu_compiled:
+      self._draw_model_source(rect)
 
     self._draw_steering_wheel(rect)
 
@@ -193,24 +200,30 @@ class HudRenderer(Widget):
     if ui_state.sm.recv_frame['selfdriveState'] < ui_state.started_frame:
       return
 
-    loading = ui_state.chestnut_state == ChestnutState.LOADING
+    big_failed = (ui_state.usbgpu_active is False or not ui_state.sm['deviceState'].chestnutPresent or
+                  (ui_state.usbgpu_active is True and ui_state.sm.recv_frame['modelV2'] > ui_state.started_frame and
+                   not ui_state.sm.alive['modelV2']) or
+                  (ui_state.usbgpu_active is None and ui_state.sm.recv_frame['modelV2'] > ui_state.started_frame))
+    self._small_model_engaged &= big_failed
+    loading = ui_state.usbgpu_loading or (ui_state.usbgpu_active is None and not big_failed)
     if loading:
-      icon = self._txt_chestnut
-      opacity = 0.35 + 0.65 * (0.5 - 0.5 * math.cos(rl.get_time() * 6.0))
-    elif ui_state.chestnut_state in (ChestnutState.UNCOMPILED, ChestnutState.FAILED):
-      icon = self._txt_chestnut_orange
-      opacity = 1.0
-    elif ui_state.chestnut_state == ChestnutState.ACTIVE:
-      icon = self._txt_chestnut_green
+      pulse = 0.5 - 0.5 * math.cos(rl.get_time() * 6.0)
+      icon = self._txt_egpu
+      opacity = 0.35 + 0.65 * pulse
+    elif self._small_model_engaged:
+      icon = self._txt_egpu_crossed
+      opacity = 0.65
+    elif big_failed:
+      icon = self._txt_egpu_orange
       opacity = 1.0
     else:
-      return
+      icon = self._txt_egpu_green
+      opacity = 1.0
 
-    if icon is not self._chestnut_icon:
-      self._chestnut_fade_time = rl.get_time()
-      self._chestnut_icon = icon
-    visible = loading or rl.get_time() - self._chestnut_fade_time < SET_SPEED_PERSISTENCE
-    alpha = self._chestnut_alpha_filter.update(visible)
+    if icon is not self._egpu_icon:
+      self._egpu_fade_time = rl.get_time()
+      self._egpu_icon = icon
+    alpha = self._egpu_alpha_filter.update(loading or 0 < rl.get_time() - self._egpu_fade_time < SET_SPEED_PERSISTENCE)
     if alpha < 1e-2:
       return
 

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

@@ -383,13 +383,18 @@ class BigMultiParamToggle(BigMultiToggle):
     self._load_value()
 
   def _load_value(self):
-    self.set_value(self._options[self._params.get(self._param) or 0])
+    value = self._params.get(self._param, return_default=True)
+    index = value if isinstance(value, int) else 0
+    self.set_value(self._options[max(0, min(index, len(self._options) - 1))])
 
   def _handle_mouse_release(self, mouse_pos: MousePos):
     super()._handle_mouse_release(mouse_pos)
     new_idx = self._options.index(self.value)
     self._params.put(self._param, new_idx)
 
+  def refresh(self):
+    self._load_value()
+
 
 class BigParamControl(BigToggle):
   def __init__(self, text: str, param: str, toggle_callback: Callable | None = None):

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/models.py

@@ -62,14 +62,14 @@ class ModelsLayout(Widget):
     self.big_model_item = ListItemSP(
       title=tr("Big Model"),
       action_item=ScrollingButtonAction(tr("SELECT")),
-      callback=lambda: self._open_source_dialog("chestnut")
+      callback=lambda: self._open_source_dialog("usbgpu")
     )
 
     self.download_item = download_status_item(lambda: tr("Download") if self._downloading else tr("Model Status"))
 
     self.refresh_item = button_item(tr("Refresh Model List"), tr("REFRESH"), "",
                                     lambda: (ui_state.params.put("ModelManager_LastSyncTime", 0),
-                                             ui_state.params.put("ModelManager_LastSyncTime_Chestnut", 0),
+                                             ui_state.params.put("ModelManager_LastSyncTime_USBGPU", 0),
                                              gui_app.push_widget(alert_dialog(tr("Fetching Latest Models")))))
 
     self.clear_cache_item = ListItemSP(
@@ -177,14 +177,14 @@ class ModelsLayout(Widget):
     big_state = big_model_state()
     carry_source, carry_internal, _ = carrying_model()
     segments = []
-    for source, label in (("qcom", tr("small")), ("chestnut", tr("big"))):
+    for source, label in (("qcom", tr("small")), ("usbgpu", tr("big"))):
       if segments:
         segments.append(("|", rl.GRAY, None, None))
       bundle = get_selected_bundle(ui_state.params, source)
       name = bundle.internalName if bundle else default_model_name(source)
       color = ON_COLOR if (source == carry_source and name == carry_internal) else rl.LIGHTGRAY
       name = "● " + name
-      if source == "chestnut":
+      if source == "usbgpu":
         if big_state == 'failed':
           color = rl.RED
         elif big_state == 'loading':
@@ -208,10 +208,10 @@ class ModelsLayout(Widget):
     """The failover story for the Model Status row. One-way big -> small, and the
     fallback is runner-matched: a Default big can only fall back to the Default
     small (stock modeld), a custom big has no automatic fallback yet."""
-    if not ui_state.chestnut_present:
+    if not ui_state.usbgpu:
       return ""
-    big_bundle = get_selected_bundle(ui_state.params, "chestnut")
-    big_name = big_bundle.internalName if big_bundle else default_model_name("chestnut")
+    big_bundle = get_selected_bundle(ui_state.params, "usbgpu")
+    big_name = big_bundle.internalName if big_bundle else default_model_name("usbgpu")
     big_is_default = big_bundle is None
     fallback_name = default_model_name("qcom")
     state = big_model_state()
@@ -225,7 +225,7 @@ class ModelsLayout(Widget):
       return tr("Getting the big model ready.")
     if big_is_default:
       return tr("{} will drive. If it fails during a drive, {} takes over until the next drive.").format(big_name, fallback_name)
-    return tr("{} will drive when the chestnut is ready.").format(big_name)
+    return tr("{} will drive when the eGPU is ready.").format(big_name)
 
   @staticmethod
   def _download_row_state(progresses, name: str) -> dict:
@@ -261,7 +261,7 @@ class ModelsLayout(Widget):
       ui_state.params.put("ModelManager_DownloadRef", selected_bundle.ref)
 
   def _resolve_selected_bundle(self, ref):
-    source_bundles = {source: bundles_for_source(source) for source in ("qcom", "chestnut")}
+    source_bundles = {source: bundles_for_source(source) for source in ("qcom", "usbgpu")}
     resolved = resolve_bundle_by_ref(ref, source_bundles)
     return resolved[0] if resolved else None
 
@@ -329,7 +329,7 @@ class ModelsLayout(Widget):
     self._handle_bundle_download_progress()
 
     carry_source, _, carry_display = carrying_model()
-    for item, item_source in ((self.small_model_item, "qcom"), (self.big_model_item, "chestnut")):
+    for item, item_source in ((self.small_model_item, "qcom"), (self.big_model_item, "usbgpu")):
       bundle = get_selected_bundle(ui_state.params, item_source)
       name = bundle.displayName if bundle else default_model_name(item_source)
       color = ON_COLOR if (item_source == carry_source and name == carry_display) else style.ITEM_TEXT_VALUE_COLOR

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

@@ -23,7 +23,7 @@ DESCRIPTIONS = {
   'stop_and_go_hack': tr_noop(
     'sunnypilot will allow some Toyota/Lexus cars to auto resume during stop and go traffic. ' +
     'This feature is only applicable to certain models that are able to use longitudinal control. This is an alpha feature. Use at your own risk.'
-  )
+  ),
 }
 
 

openpilot/selfdrive/ui/sunnypilot/layouts/sidebar.py

@@ -21,8 +21,8 @@ METRIC_MARGIN = 30
 METRIC_START_Y = 300
 HOME_BTN = rl.Rectangle(60, 860, 180, 180)
 
-CHESTNUT_ICON_WIDTH = 180
-CHESTNUT_ICON_HEIGHT = 133
+EGPU_ICON_WIDTH = 180
+EGPU_ICON_HEIGHT = 133
 
 
 # Color scheme
@@ -59,11 +59,10 @@ class MetricData:
 class SidebarSP:
   def __init__(self):
     self._sunnylink_status = MetricData(tr_noop("SUNNYLINK"), tr_noop("OFFLINE"), Colors.WARNING)
-    self._chestnut_green_img = gui_app.texture("icons_mici/chestnut_green.png", CHESTNUT_ICON_WIDTH, CHESTNUT_ICON_HEIGHT)
-    self._chestnut_default_img = gui_app.texture("icons_mici/chestnut.png", CHESTNUT_ICON_WIDTH, CHESTNUT_ICON_HEIGHT)
-    self._chestnut_orange_img = gui_app.texture("icons_mici/chestnut_orange.png", CHESTNUT_ICON_WIDTH, CHESTNUT_ICON_HEIGHT)
-    # gray state uses the default art (chestnut_gray.png removed upstream)
-    self._chestnut_gray_img = gui_app.texture("icons_mici/chestnut.png", CHESTNUT_ICON_WIDTH, CHESTNUT_ICON_HEIGHT)
+    self._egpu_green_img = gui_app.texture("icons_mici/egpu_green.png", EGPU_ICON_WIDTH, EGPU_ICON_HEIGHT)
+    self._egpu_default_img = gui_app.texture("icons_mici/egpu.png", EGPU_ICON_WIDTH, EGPU_ICON_HEIGHT)
+    self._egpu_orange_img = gui_app.texture("icons_mici/egpu_orange.png", EGPU_ICON_WIDTH, EGPU_ICON_HEIGHT)
+    self._egpu_gray_img = gui_app.texture("icons_mici/egpu_gray.png", EGPU_ICON_WIDTH, EGPU_ICON_HEIGHT)
 
   def _update_sunnylink_status(self):
     if not ui_state.params.get_bool("SunnylinkEnabled"):
@@ -94,19 +93,19 @@ class SidebarSP:
     if not ui_state.sm["deviceState"].chestnutPresent:
       return default_img, default_pos, 1.0
 
-    big_model_selected = ui_state.chestnut_compiled or ui_state.model_runner_tinygrad
+    big_model_selected = ui_state.usbgpu_compiled or ui_state.model_runner_tinygrad
     big_model_failed = ui_state.started and ui_state.big_model_failed
-    loading = ui_state.chestnut_loading or (big_model_selected and ui_state.started and ui_state.chestnut_active is None)
+    loading = ui_state.usbgpu_loading or (big_model_selected and ui_state.started and ui_state.usbgpu_active is None)
 
     if loading:
-      icon = self._chestnut_default_img
+      icon = self._egpu_default_img
       opacity = 0.35 + 0.65 * (0.5 - 0.5 * math.cos(rl.get_time() * 6.0))
     elif big_model_selected and big_model_failed:
-      icon, opacity = self._chestnut_orange_img, 1.0
+      icon, opacity = self._egpu_orange_img, 1.0
     elif big_model_selected:
-      icon, opacity = self._chestnut_green_img, 1.0
+      icon, opacity = self._egpu_green_img, 1.0
     else:
-      icon, opacity = self._chestnut_gray_img, 1.0
+      icon, opacity = self._egpu_gray_img, 1.0
 
     x = HOME_BTN.x + (HOME_BTN.width - icon.width) / 2
     y = HOME_BTN.y + (HOME_BTN.height - icon.height) / 2

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

@@ -19,30 +19,35 @@ class MiciHomeLayoutSP(MiciHomeLayout):
   def __init__(self):
     super().__init__()
     self._openpilot_label = UnifiedLabel("sunnypilot", font_size=88, font_weight=FontWeight.AUDIOWIDE, max_width=480, wrap_text=False)
-    self._chestnut_icon_gray = IconWidget("icons_mici/chestnut.png", (68, 40))
-    self._chestnut_icon_gray.set_visible(False)
-    failed_idx = self._status_bar_layout.widgets.index(self._chestnut_failed_icon)
-    self._status_bar_layout.widgets.insert(failed_idx + 1, self._chestnut_icon_gray)
+    self._egpu_icon_default = IconWidget("icons_mici/egpu.png", (50, 37))
+    self._egpu_icon_default.set_visible(False)
+    self._egpu_icon_orange = IconWidget("icons_mici/egpu_orange.png", (50, 37))
+    self._egpu_icon_orange.set_visible(False)
+    gray_idx = self._status_bar_layout.widgets.index(self._egpu_icon_gray)
+    self._status_bar_layout.widgets.insert(gray_idx + 1, self._egpu_icon_default)
+    self._status_bar_layout.widgets.insert(gray_idx + 2, self._egpu_icon_orange)
 
-  def _set_chestnut_visibility(self):
+  def _set_egpu_visibility(self):
     chestnut = ui_state.sm["deviceState"].chestnutPresent
     if not chestnut:
-      self._chestnut_icon.set_visible(False)
-      self._chestnut_failed_icon.set_visible(False)
-      self._chestnut_icon_gray.set_visible(False)
+      self._egpu_icon.set_visible(False)
+      self._egpu_icon_default.set_visible(False)
+      self._egpu_icon_orange.set_visible(False)
+      self._egpu_icon_gray.set_visible(False)
       return
 
-    big_model_selected = ui_state.chestnut_compiled or ui_state.model_runner_tinygrad
+    big_model_selected = ui_state.usbgpu_compiled or ui_state.model_runner_tinygrad
     big_model_failed = ui_state.started and ui_state.big_model_failed
-    loading = ui_state.chestnut_loading or (big_model_selected and ui_state.started and ui_state.chestnut_active is None)
+    loading = ui_state.usbgpu_loading or (big_model_selected and ui_state.started and ui_state.usbgpu_active is None)
 
     if loading:
-      self._chestnut_icon_gray._opacity = 0.35 + 0.65 * (0.5 - 0.5 * math.cos(rl.get_time() * 6.0))
-      self._chestnut_icon_gray.set_visible(True)
-      self._chestnut_icon.set_visible(False)
-      self._chestnut_failed_icon.set_visible(False)
+      self._egpu_icon_default._opacity = 0.35 + 0.65 * (0.5 - 0.5 * math.cos(rl.get_time() * 6.0))
+      self._egpu_icon_default.set_visible(True)
+      self._egpu_icon.set_visible(False)
+      self._egpu_icon_orange.set_visible(False)
+      self._egpu_icon_gray.set_visible(False)
     else:
-      self._chestnut_icon_gray._opacity = 1.0
-      self._chestnut_icon_gray.set_visible(not big_model_selected)
-      self._chestnut_icon.set_visible(big_model_selected and not big_model_failed)
-      self._chestnut_failed_icon.set_visible(big_model_selected and big_model_failed)
+      self._egpu_icon_default.set_visible(False)
+      self._egpu_icon.set_visible(big_model_selected and not big_model_failed)
+      self._egpu_icon_orange.set_visible(big_model_selected and big_model_failed)
+      self._egpu_icon_gray.set_visible(not big_model_selected)

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/models.py

@@ -27,14 +27,14 @@ def _model_info() -> tuple[str, str, str]:
   state = big_model_state()
   _, _, carry_display = carrying_model()
   if carry_display is None:
-    big = get_selected_bundle(ui_state.params, "chestnut")
-    carry_display = big.displayName if big else default_model_name("chestnut")
+    big = get_selected_bundle(ui_state.params, "usbgpu")
+    carry_display = big.displayName if big else default_model_name("usbgpu")
   active_text = (carry_display or active_name).lower()
   if state == 'failed':
     return active_text, tr("big model"), tr("unavailable")
   if state == 'loading':
     return active_text, tr("big model"), tr("getting ready")
-  header = tr("small model") if source == "chestnut" else tr("big model")
+  header = tr("small model") if source == "usbgpu" else tr("big model")
   return active_text, header, other_name.lower()
 
 
@@ -113,7 +113,7 @@ class ModelsLayoutMici(NavScroller):
 
     hardware_btns = []
     active = active_source()
-    for source, label in (("qcom", tr("small models")), ("chestnut", tr("big models"))):
+    for source, label in (("qcom", tr("small models")), ("usbgpu", tr("big models"))):
       bundle = get_selected_bundle(ui_state.params, source)
       value = (bundle.internalName if bundle else default_model_name(source)).lower()
       if source == active:

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/onroad/hud_renderer.py

@@ -22,7 +22,7 @@ class HudRendererSP(HudRenderer):
 
   def _render(self, rect: rl.Rectangle) -> None:
     super()._render(rect)
-    if ui_state.chestnut_present and not ui_state.chestnut_compiled and ui_state.model_runner_tinygrad:
+    if ui_state.usbgpu and not ui_state.usbgpu_compiled and ui_state.model_runner_tinygrad:
       self._draw_model_source(rect)
     self.blind_spot_indicators.render(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/sunnypilot/model_info.py

@@ -11,8 +11,8 @@ from openpilot.sunnypilot.models.model_name import DEFAULT_BIG_MODEL, DEFAULT_MO
 
 
 def active_source() -> str:
-  return get_active_source(chestnut=ui_state.chestnut_present,
-                           chestnut_active=ui_state.chestnut_active, chestnut_loading=ui_state.chestnut_loading,
+  return get_active_source(usbgpu=ui_state.usbgpu,
+                           usbgpu_active=ui_state.usbgpu_active, usbgpu_loading=ui_state.usbgpu_loading,
                            offroad=ui_state.is_offroad())
 
 
@@ -23,7 +23,7 @@ def bundles_for_source(source: str):
 
 
 def default_model(source: str) -> str:
-  return DEFAULT_BIG_MODEL if source == 'chestnut' else DEFAULT_MODEL
+  return DEFAULT_BIG_MODEL if source == 'usbgpu' else DEFAULT_MODEL
 
 
 def default_model_name(source: str) -> str:
@@ -32,10 +32,10 @@ def default_model_name(source: str) -> str:
 
 def big_model_state() -> str | None:
   """'failed' | 'loading' | None, mirroring the sidebar's detection (#1969)."""
-  if ui_state.started and ui_state.chestnut_present and ui_state.big_model_failed:
+  if ui_state.started and ui_state.usbgpu and ui_state.big_model_failed:
     return 'failed'
-  big_selected = ui_state.chestnut_compiled or ui_state.model_runner_tinygrad
-  if ui_state.chestnut_loading or (big_selected and ui_state.started and ui_state.chestnut_active is None):
+  big_selected = ui_state.usbgpu_compiled or ui_state.model_runner_tinygrad
+  if ui_state.usbgpu_loading or (big_selected and ui_state.started and ui_state.usbgpu_active is None):
     return 'loading'
   return None
 
@@ -45,14 +45,14 @@ def carrying_model() -> tuple[str | None, str | None, str | None]:
   when a Default big cannot carry, stock modeld runs the Default small, never the
   small slot's pick; a custom big has no automatic fallback yet -> (None, None, None)."""
   source = active_source()
-  if source == "chestnut":
-    bundle = get_selected_bundle(ui_state.params, "chestnut")
+  if source == "usbgpu":
+    bundle = get_selected_bundle(ui_state.params, "usbgpu")
     if bundle:
-      return "chestnut", bundle.internalName, bundle.displayName
-    name = default_model_name("chestnut")
-    return "chestnut", name, name
-  if ui_state.chestnut_present:
-    if get_selected_bundle(ui_state.params, "chestnut") is None:
+      return "usbgpu", bundle.internalName, bundle.displayName
+    name = default_model_name("usbgpu")
+    return "usbgpu", name, name
+  if ui_state.usbgpu:
+    if get_selected_bundle(ui_state.params, "usbgpu") is None:
       name = default_model_name("qcom")
       return "qcom", name, name
     return None, None, None
@@ -66,7 +66,7 @@ def carrying_model() -> tuple[str | None, str | None, str | None]:
 def queued_name(current_ref) -> str | None:
   ref = ui_state.params.get("ModelManager_DownloadRef")
   if ref and ref != current_ref:
-    source_bundles = {source: bundles_for_source(source) for source in ("qcom", "chestnut")}
+    source_bundles = {source: bundles_for_source(source) for source in ("qcom", "usbgpu")}
     if resolved := resolve_bundle_by_ref(ref, source_bundles):
       return resolved[0].internalName
   return None
@@ -79,7 +79,7 @@ def model_info() -> tuple[str, str, str]:
   manager republishes a tick after a chestnut change, so the stale bundle
   would flash the wrong model."""
   source = active_source()
-  other = "qcom" if source == "chestnut" else "chestnut"
+  other = "qcom" if source == "usbgpu" else "usbgpu"
   active_bundle = get_selected_bundle(ui_state.params, source)
   other_bundle = get_selected_bundle(ui_state.params, other)
 

openpilot/selfdrive/ui/sunnypilot/ui_state.py

@@ -152,8 +152,8 @@ class UIStateSP:
       self.has_icbm = self.CP_SP.intelligentCruiseButtonManagementAvailable and self.params.get_bool("IntelligentCruiseButtonManagement")
 
     self._enforce_constraints()
-    source = get_active_source(chestnut=self.chestnut_present, chestnut_active=self.chestnut_active,
-                               chestnut_loading=self.chestnut_loading, offroad=self.is_offroad())
+    source = get_active_source(usbgpu=self.usbgpu, usbgpu_active=self.usbgpu_active,
+                               usbgpu_loading=self.usbgpu_loading, offroad=self.is_offroad())
     self.active_bundle = self.params.get(ACTIVE_BUNDLE_KEYS[source])
     self.model_runner_tinygrad = self.active_bundle is not None and self.active_bundle.get("runner") == "tinygrad"
     self.blindspot = self.params.get_bool("BlindSpot")

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/selfdrive/ui/ui_state.py

@@ -12,7 +12,7 @@ from openpilot.common.swaglog import cloudlog
 from openpilot.selfdrive.ui.lib.prime_state import PrimeState
 from openpilot.system.ui.lib.application import gui_app
 from openpilot.common.hardware import HARDWARE, PC
-from openpilot.selfdrive.modeld.helpers import chestnut_compiled
+from openpilot.selfdrive.modeld.helpers import usbgpu_compiled
 
 from openpilot.selfdrive.ui.sunnypilot.ui_state import UIStateSP, DeviceSP
 
@@ -28,15 +28,6 @@ class UIStatus(Enum):
   LONG_ONLY = "long_only"
 
 
-class ChestnutState(Enum):
-  DISCONNECTED = "disconnected"
-  UNCOMPILED = "uncompiled"
-  READY = "ready"
-  LOADING = "loading"
-  ACTIVE = "active"
-  FAILED = "failed"
-
-
 class UIState(UIStateSP):
   _instance: 'UIState | None' = None
 
@@ -91,11 +82,10 @@ class UIState(UIStateSP):
     self.always_on_dm: bool = self.params.get_bool("AlwaysOnDM")
     self.experimental_mode: bool = self.params.get_bool("ExperimentalMode")
     self.experimental_mode_confirmed: bool = self.params.get_bool("ExperimentalModeConfirmed")
-    self.chestnut_present: bool = False
-    self.chestnut_compiled: bool = chestnut_compiled()
-    self.chestnut_active: bool | None = None
-    self.chestnut_loading: bool = False
-    self.chestnut_state = ChestnutState.DISCONNECTED
+    self.usbgpu: bool = False
+    self.usbgpu_compiled: bool = usbgpu_compiled()
+    self.usbgpu_active: bool | None = self.params.get("UsbGpuActive")
+    self.usbgpu_loading: bool = self.params.get_bool("UsbGpuLoading")
     self.started: bool = False
     self.ignition: bool = False
     self.recording_audio: bool = False
@@ -125,11 +115,11 @@ class UIState(UIStateSP):
   @property
   def big_model_failed(self) -> bool:
     # Mirrors the onroad HUD's four-condition check so sidebar and home icons reflect the same failure states
-    return (self.chestnut_active is False or
+    return (self.usbgpu_active is False or
             not self.sm['deviceState'].chestnutPresent or
-            (self.chestnut_active is True and self.sm.recv_frame['modelV2'] > self.started_frame and
+            (self.usbgpu_active is True and self.sm.recv_frame['modelV2'] > self.started_frame and
              not self.sm.alive['modelV2']) or
-            (self.chestnut_active is None and self.sm.recv_frame['modelV2'] > self.started_frame))
+            (self.usbgpu_active is None and self.sm.recv_frame['modelV2'] > self.started_frame))
 
   @property
   def engaged(self) -> bool:
@@ -150,7 +140,6 @@ class UIState(UIStateSP):
     self.sm.update(0)
     self._update_state()
     self._update_status()
-    self._update_chestnut_state()
     device.update()
     UIStateSP.update(self)
 
@@ -214,35 +203,12 @@ class UIState(UIStateSP):
         self.status = UIStatus.DISENGAGED
         self.started_frame = self.sm.frame
         self.started_time = time.monotonic()
-        self.chestnut_present = self.sm["deviceState"].chestnutPresent
 
       for callback in self._offroad_transition_callbacks:
         callback()
 
       self._started_prev = self.started
 
-  def _update_chestnut_state(self) -> None:
-    detected = self.sm["deviceState"].chestnutPresent
-    if not self.started:
-      self.chestnut_present = detected
-      self.chestnut_state = (ChestnutState.READY if detected and self.chestnut_compiled else
-                             ChestnutState.UNCOMPILED if detected else ChestnutState.DISCONNECTED)
-      return
-
-    model_seen = self.sm.recv_frame["modelV2"] > self.started_frame
-    if not self.chestnut_present:
-      self.chestnut_state = ChestnutState.DISCONNECTED
-    elif not self.chestnut_compiled:
-      self.chestnut_state = ChestnutState.UNCOMPILED
-    elif self.chestnut_state == ChestnutState.FAILED or not detected or (model_seen and (not self.sm.alive["modelV2"] or not self.sm["modelV2"].big)):
-      self.chestnut_state = ChestnutState.FAILED
-    elif self.chestnut_loading or not model_seen:
-      self.chestnut_state = ChestnutState.LOADING
-    elif self.chestnut_active is False:
-      self.chestnut_state = ChestnutState.FAILED
-    else:
-      self.chestnut_state = ChestnutState.ACTIVE
-
   def update_params(self) -> None:
     # For slower operations
     # Update longitudinal control state
@@ -259,10 +225,12 @@ class UIState(UIStateSP):
     self.always_on_dm = self.params.get_bool("AlwaysOnDM")
     self.experimental_mode = self.params.get_bool("ExperimentalMode")
     self.experimental_mode_confirmed = self.params.get_bool("ExperimentalModeConfirmed")
-    if not self.chestnut_compiled:
-      self.chestnut_compiled = chestnut_compiled()
-    self.chestnut_active = self.params.get("ChestnutActive")
-    self.chestnut_loading = self.params.get_bool("ChestnutLoading")
+    # keep usbgpu UI active until offroad transition when gpu disappears
+    self.usbgpu = self.sm["deviceState"].chestnutPresent or (self.usbgpu and self.started)
+    if not self.usbgpu_compiled:
+      self.usbgpu_compiled = usbgpu_compiled()
+    self.usbgpu_active = self.params.get("UsbGpuActive")
+    self.usbgpu_loading = self.params.get_bool("UsbGpuLoading")
 
     UIStateSP.update_params(self)
 

openpilot/sunnypilot/modeld_v2/compile_modeld.py

@@ -298,7 +298,7 @@ def _load_policy_runners(args: argparse.Namespace) -> tuple[list, list]:
 
 
 if __name__ == "__main__":
-  if 'USB' in os.getenv('DEV', '') or os.getenv('CHESTNUT'):
+  if 'USB' in os.getenv('DEV', '') or os.getenv('USBGPU'):
     from openpilot.system.hardware.chestnut.flash import link_up
     for _ in range(10):
       if link_up():

openpilot/sunnypilot/modeld_v2/modeld.py

@@ -9,7 +9,7 @@ See the LICENSE.md file in the root directory for more details.
 import os
 os.environ['GMMU'] = '0'
 from openpilot.common.hardware import COMMA_HARDWARE
-from openpilot.selfdrive.modeld.helpers import chestnut_present, load_oob
+from openpilot.selfdrive.modeld.helpers import usbgpu_present, load_oob
 import time
 import numpy as np
 import openpilot.cereal.messaging as messaging
@@ -84,14 +84,14 @@ class ModelState(ModelStateBase):
   inputs: dict[str, np.ndarray]
   prev_desire: np.ndarray
 
-  def __init__(self, cam_w: int, cam_h: int, chestnut: bool = False):
+  def __init__(self, cam_w: int, cam_h: int, usbgpu: bool = False):
     ModelStateBase.__init__(self)
 
     env_pkl = os.environ.get('COMBINED_MODEL_PKL')
     if env_pkl and os.path.exists(env_pkl):
       model_bundle = None
     else:
-      model_bundle = get_active_bundle(chestnut=chestnut)
+      model_bundle = get_active_bundle(usbgpu=usbgpu)
     self.generation = model_bundle.generation if model_bundle is not None else None
     overrides = {override.key: override.value for override in model_bundle.overrides} if model_bundle else {}
 
@@ -99,7 +99,7 @@ class ModelState(ModelStateBase):
     self.LONG_SMOOTH_SECONDS = float(overrides.get('long', ".0"))
     self.MIN_LAT_CONTROL_SPEED = 0.3
     self.PLANPLUS_CONTROL: float = 1.0
-    self.chestnut = chestnut
+    self.usbgpu = usbgpu
 
     pkl_path = _find_driving_pkl(model_bundle)
     assert pkl_path is not None, "No driving pkl found — all models must be compiled with compile_modeld.py"
@@ -110,7 +110,7 @@ class ModelState(ModelStateBase):
     jits = load_oob(open_file_chunked(pkl_path))
 
     self.WARP_DEV = 'QCOM' if COMMA_HARDWARE else 'CPU'
-    self.DEV = 'AMD' if self.chestnut else self.WARP_DEV
+    self.DEV = 'AMD' if self.usbgpu else self.WARP_DEV
     self.QUEUE_DEV = self.DEV
     metadata = jits['metadata']
 
@@ -185,7 +185,7 @@ class ModelState(ModelStateBase):
     else:
       self.warp(**{k: self.input_queues[k] for k in WARP_INPUTS}, frame=frame_tensor, big_frame=big_frame_tensor)
 
-    if self.chestnut:
+    if self.usbgpu:
       self.warmup()
 
   def warmup(self) -> None:
@@ -287,7 +287,7 @@ class ModelState(ModelStateBase):
       buf[0, :-1] = buf[0, 1:]
       buf[0, -1, :] = outputs['desired_curvature'][0, :] if not self.mlsim else 0
 
-    if self.chestnut and not np.all(np.isfinite(outputs.get('plan', np.array([0.])))):
+    if self.usbgpu and not np.all(np.isfinite(outputs.get('plan', np.array([0.])))):
       cloudlog.error("model output not finite, dropping frame")
       return None
 
@@ -327,13 +327,13 @@ def main(demo=False):
   setproctitle(PROCESS_NAME)
   config_realtime_process(7, 54)
 
-  CHESTNUT = chestnut_present()
-  if CHESTNUT:
+  USBGPU = usbgpu_present()
+  if USBGPU:
     os.environ['HCQDEV_WAIT_TIMEOUT_MS'] = '3000'
 
   params = Params()
-  params.put_bool("ChestnutLoading", CHESTNUT)
-  params.remove("ChestnutActive")
+  params.put_bool("UsbGpuLoading", USBGPU)
+  params.remove("UsbGpuActive")
 
   # visionipc clients
   while True:
@@ -362,31 +362,31 @@ def main(demo=False):
   st = time.monotonic()
 
   model = None
-  if CHESTNUT:
+  if USBGPU:
     import threading
     def load():
       nonlocal model
-      model = ModelState(cam_w=vipc_client_main.width, cam_h=vipc_client_main.height, chestnut=True)
+      model = ModelState(cam_w=vipc_client_main.width, cam_h=vipc_client_main.height, usbgpu=True)
     t = threading.Thread(target=load, daemon=True)
     t.start()
     t.join(60)
     if model is None:
-      params.put_bool("ChestnutActive", False)
-      raise RuntimeError("chestnut model load failed or timed out (60s)")
-    params.put_bool("ChestnutActive", True)
+      params.put_bool("UsbGpuActive", False)
+      raise RuntimeError("eGPU model load failed or timed out (60s)")
+    params.put_bool("UsbGpuActive", True)
   else:
-    model = ModelState(cam_w=vipc_client_main.width, cam_h=vipc_client_main.height, chestnut=False)
+    model = ModelState(cam_w=vipc_client_main.width, cam_h=vipc_client_main.height, usbgpu=False)
 
-  params.put_bool("ChestnutLoading", False)
+  params.put_bool("UsbGpuLoading", False)
   cloudlog.warning(f"models loaded in {time.monotonic() - st:.1f}s, modeld starting")
 
   # messaging
-  pub_socks = ["modelV2", "drivingModelData", "cameraOdometry", "modelDataV2SP"] + (["chestnutState"] if CHESTNUT else [])
+  pub_socks = ["modelV2", "drivingModelData", "cameraOdometry", "modelDataV2SP"] + (["chestnutState"] if USBGPU else [])
   pm = PubMaster(pub_socks)
   sm = SubMaster(["deviceState", "carState", "narrowRoadCameraState", "extrinsicsCalibration", "driverMonitoringState", "carControl", "lateralDelay"])
 
   publish_state = PublishState()
-  chestnut_state = ChestnutState(pm, CHESTNUT) if CHESTNUT else None
+  chestnut_state = ChestnutState(pm, USBGPU) if USBGPU else None
 
   # setup filter to track dropped frames
   frame_dropped_filter = FirstOrderFilter(0., 10., 1. / model.constants.MODEL_FREQ)
@@ -524,7 +524,7 @@ def main(demo=False):
       fill_model_msg(drivingdata_send, modelv2_send, model_output, action,
                      publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id,
                      frame_drop_ratio, meta_main.timestamp_eof, model_execution_time, live_calib_seen, meta_constants)
-      modelv2_send.modelV2.big = model.chestnut
+      modelv2_send.modelV2.big = model.usbgpu
 
       desire_state = modelv2_send.modelV2.meta.desireState
       l_lane_change_prob = desire_state[log.Desire.laneChangeLeft]

openpilot/sunnypilot/modeld_v2/tests/helpers.py

@@ -190,8 +190,8 @@ def tmp_path():
 
 def patch_modeld(monkeypatch):
   def _patch(bundle):
-    monkeypatch.setattr(helpers, 'get_active_bundle', lambda params=None, *, chestnut=None: bundle)
-    monkeypatch.setattr(modeld_module, 'get_active_bundle', lambda params=None, *, chestnut=None: bundle)
+    monkeypatch.setattr(helpers, 'get_active_bundle', lambda params=None, *, usbgpu=None: bundle)
+    monkeypatch.setattr(modeld_module, 'get_active_bundle', lambda params=None, *, usbgpu=None: bundle)
 
   return _patch
 

openpilot/sunnypilot/modeld_v2/tests/test_combined_pkl_loader.py

@@ -59,8 +59,8 @@ class TestFindDrivingPkl(OpenpilotTestCase):
 class TestModelStateCombinedInit(OpenpilotTestCase):
   def test_asserts_when_no_pkl(self, monkeypatch):
     bundle = DummyBundle(models=[], is_20hz=True)
-    monkeypatch.setattr(helpers, 'get_active_bundle', lambda params=None, *, chestnut=None: bundle)
-    monkeypatch.setattr(modeld_module, 'get_active_bundle', lambda params=None, *, chestnut=None: bundle)
+    monkeypatch.setattr(helpers, 'get_active_bundle', lambda params=None, *, usbgpu=None: bundle)
+    monkeypatch.setattr(modeld_module, 'get_active_bundle', lambda params=None, *, usbgpu=None: bundle)
     with self.assertRaisesRegex(AssertionError, "No driving pkl found"):
       ModelState(cam_w=CAM_W, cam_h=CAM_H)
 

openpilot/sunnypilot/models/default_model.py

@@ -9,8 +9,8 @@ from openpilot.sunnypilot.models.model_name import DEFAULT_MODEL, DEFAULT_BIG_MO
 
 
 def get_default_model() -> str:
-  show_big_model = (ui_state.chestnut_present
-                    and (ui_state.chestnut_active or ui_state.chestnut_loading or ui_state.is_offroad()))
+  show_big_model = (ui_state.usbgpu
+                    and (ui_state.usbgpu_active or ui_state.usbgpu_loading or ui_state.is_offroad()))
 
   return DEFAULT_BIG_MODEL if show_big_model else DEFAULT_MODEL
 

openpilot/sunnypilot/models/fetcher.py

@@ -139,11 +139,11 @@ class ModelCache:
 class ModelFetcher:
   """Handles fetching and caching of model data from remote source"""
   MODEL_URL = "https://raw.githubusercontent.com/sunnypilot/sunnypilot-models/refs/heads/gh-pages/docs/driving_models_v21.json"
-  MODEL_URL_CHESTNUT = "https://raw.githubusercontent.com/sunnypilot/sunnypilot-models/refs/heads/gh-pages/docs/driving_models_chestnut_v22.json"
+  MODEL_URL_USBGPU = "https://raw.githubusercontent.com/sunnypilot/sunnypilot-models/refs/heads/gh-pages/docs/driving_models_usbgpu_v22.json"
 
   MODEL_SOURCES = {
     "qcom": (MODEL_URL, ""),
-    "chestnut": (MODEL_URL_CHESTNUT, "_Chestnut"),
+    "usbgpu": (MODEL_URL_USBGPU, "_USBGPU"),
   }
 
   def __init__(self, params: Params):
@@ -156,12 +156,12 @@ class ModelFetcher:
     self._refetched: set[str] = set()
     self.params.put("ModelManager_ActiveJson", {
       "qcom": self.MODEL_URL,
-      "chestnut": self.MODEL_URL_CHESTNUT,
+      "usbgpu": self.MODEL_URL_USBGPU,
     }, block=True)
 
   @staticmethod
   def active_source(chestnut_present: bool) -> str:
-    return "chestnut" if chestnut_present else "qcom"
+    return "usbgpu" if chestnut_present else "qcom"
 
   def _fetch_and_cache_models(self, source: str) -> list[custom.ModelManagerSP.ModelBundle] | None:
     """Fetches fresh model data from remote and updates cache.
@@ -200,7 +200,7 @@ class ModelFetcher:
   @staticmethod
   def _cache_matches_source(source: str, cached_data: dict) -> bool:
     bundles = cached_data.get("bundles", [])
-    if source == "chestnut":
+    if source == "usbgpu":
       return any(bundle.get("is_big") is True for bundle in bundles)
     return not any(bundle.get("is_big") is True for bundle in bundles)
 
@@ -261,10 +261,10 @@ def get_cached_bundles(params: Params, source: str) -> list[custom.ModelManagerS
 
 
 if __name__ == "__main__":
-  from openpilot.selfdrive.modeld.helpers import chestnut_present
+  from openpilot.selfdrive.modeld.helpers import usbgpu_present
   params = Params()
   model_fetcher = ModelFetcher(params)
-  bundles = model_fetcher.get_bundles_for_source(ModelFetcher.active_source(chestnut_present()))
+  bundles = model_fetcher.get_bundles_for_source(ModelFetcher.active_source(usbgpu_present()))
   for bundle in bundles:
     for model in bundle.models:
       model_overrides = {override.key: override.value for override in bundle.overrides}

openpilot/sunnypilot/models/helpers.py

@@ -16,7 +16,7 @@ from openpilot.common.params import Params
 from openpilot.common.swaglog import cloudlog
 from openpilot.sunnypilot.models.constants import Meta, MetaSimPose, MetaTombRaider
 from openpilot.common.hardware.hw import Paths
-from openpilot.selfdrive.modeld.helpers import chestnut_present
+from openpilot.selfdrive.modeld.helpers import usbgpu_present
 
 # SET ME TO THE EXACT JSON VERSION WE SET IN SUNNYPILOT_MODELS REPO
 REQUIRED_JSON_VERSION = 18
@@ -27,7 +27,7 @@ ModelManager = custom.ModelManagerSP
 
 ACTIVE_BUNDLE_KEYS = {
   "qcom": "ModelManager_ActiveBundle",
-  "chestnut": "ModelManager_ActiveBundleChestnut",
+  "usbgpu": "ModelManager_ActiveBundleUSBGPU",
 }
 _LAST_VALIDATED_RAW: dict[str, dict | None] = {}
 
@@ -126,20 +126,20 @@ def get_selected_bundle(params: Params | None = None, source: str = "qcom") -> "
   return _parse_active_bundle(params.get(ACTIVE_BUNDLE_KEYS[source]))
 
 
-def get_active_source(chestnut: bool | None = None, chestnut_active: bool | None = None,
-                      chestnut_loading: bool | None = None, offroad: bool | None = None) -> str:
-  if chestnut is None:
-    chestnut = chestnut_present()
-  state_valid = chestnut_active is not None or chestnut_loading is not None or offroad is not None
-  big_active = chestnut and (not state_valid or chestnut_active or chestnut_loading or offroad)
-  return "chestnut" if big_active else "qcom"
+def get_active_source(usbgpu: bool | None = None, usbgpu_active: bool | None = None,
+                      usbgpu_loading: bool | None = None, offroad: bool | None = None) -> str:
+  if usbgpu is None:
+    usbgpu = usbgpu_present()
+  state_valid = usbgpu_active is not None or usbgpu_loading is not None or offroad is not None
+  big_active = usbgpu and (not state_valid or usbgpu_active or usbgpu_loading or offroad)
+  return "usbgpu" if big_active else "qcom"
 
 
-def get_active_bundle(params: Params | None = None, *, chestnut: bool | None = None) -> "custom.ModelManagerSP.ModelBundle | None":
+def get_active_bundle(params: Params | None = None, *, usbgpu: bool | None = None) -> "custom.ModelManagerSP.ModelBundle | None":
   # no cross-slot fallback: an empty active slot means the hardware default, which
   # only stock modeld can run - modeld_v2 requires a real bundle
   params = params or Params()
-  return get_selected_bundle(params, get_active_source(chestnut=chestnut))
+  return get_selected_bundle(params, get_active_source(usbgpu=usbgpu))
 
 
 def resolve_bundle_by_ref(

openpilot/sunnypilot/models/manager.py

@@ -40,7 +40,7 @@ class ModelManagerSP:
     self.available_models: list[custom.ModelManagerSP.ModelBundle] = []
     self.source_models: dict[str, list[custom.ModelManagerSP.ModelBundle]] = {}
     self.selected_bundle: custom.ModelManagerSP.ModelBundle = None
-    self.active_bundle: custom.ModelManagerSP.ModelBundle = get_active_bundle(self.params, chestnut=self.chestnut_present)
+    self.active_bundle: custom.ModelManagerSP.ModelBundle = get_active_bundle(self.params, usbgpu=self.chestnut_present)
     self._chunk_size = 128 * 1000  # 128 KB chunks
     self._download_start_times: dict[str, float] = {}  # Track start time per model
     self._download_ref: bytes | str | None = None
@@ -280,7 +280,7 @@ class ModelManagerSP:
         raise DownloadCancelled("Download cancelled")
       self.selected_bundle.status = custom.ModelManagerSP.DownloadStatus.downloaded
       self.params.put(ACTIVE_BUNDLE_KEYS[source], model_bundle.to_dict(), block=True)
-      self.active_bundle = get_active_bundle(self.params, chestnut=self.chestnut_present)
+      self.active_bundle = get_active_bundle(self.params, usbgpu=self.chestnut_present)
 
     except Exception:
       if self.selected_bundle is not None:
@@ -326,7 +326,7 @@ class ModelManagerSP:
         self.source_models = {source: self.model_fetcher.get_bundles_for_source(source) for source in ModelFetcher.MODEL_SOURCES}
         self.available_models = self.source_models[ModelFetcher.active_source(self.chestnut_present)]
         validate_active_bundles(self.params, self.source_models)
-        self.active_bundle = get_active_bundle(self.params, chestnut=self.chestnut_present)
+        self.active_bundle = get_active_bundle(self.params, usbgpu=self.chestnut_present)
 
         self._process_download_requests()
 

openpilot/sunnypilot/models/tests/test_manager_download.py

@@ -392,7 +392,7 @@ class TestManagerDownload(ManagerDownloadTestBase):
       asyncio.run(self.manager._download_bundle(self._bundle, self.dest, "qcom"))
 
       assert "ModelManager_ActiveBundle" in store, "qcom download must write the qcom slot"
-      assert "ModelManager_ActiveBundleChestnut" not in store, "qcom download must not touch the chestnut slot"
+      assert "ModelManager_ActiveBundleUSBGPU" not in store, "qcom download must not touch the usbgpu slot"
       assert self.manager.selected_bundle.status == custom.ModelManagerSP.DownloadStatus.downloaded
       assert self.manager.active_bundle is not None and self.manager.active_bundle.ref == "test-ref"
       assert self.manager.active_bundle.status == custom.ModelManagerSP.DownloadStatus.downloaded
@@ -401,18 +401,18 @@ class TestManagerDownload(ManagerDownloadTestBase):
       assert missing == [], f"chunks missing from the cache: {missing}"
     self.run_with_server(body)
 
-  def test_download_writes_chestnut_slot(self):
-    """A download resolved to the chestnut source writes the chestnut active bundle slot only."""
+  def test_download_writes_usbgpu_slot(self):
+    """A download resolved to the usbgpu source writes the usbgpu active bundle slot only."""
     def body():
       self.make_artifact(chunked=True)
       self._bundle.ref = "big-ref"
       self._bundle.minimumSelectorVersion = 18
       params, store = self._make_params_with_store()
       self.manager.params = params
-      asyncio.run(self.manager._download_bundle(self._bundle, self.dest, "chestnut"))
+      asyncio.run(self.manager._download_bundle(self._bundle, self.dest, "usbgpu"))
 
-      assert "ModelManager_ActiveBundleChestnut" in store, "chestnut download must write the chestnut slot"
-      assert "ModelManager_ActiveBundle" not in store, "chestnut download must not touch the qcom slot"
+      assert "ModelManager_ActiveBundleUSBGPU" in store, "usbgpu download must write the usbgpu slot"
+      assert "ModelManager_ActiveBundle" not in store, "usbgpu download must not touch the qcom slot"
       assert self.manager.selected_bundle.status == custom.ModelManagerSP.DownloadStatus.downloaded
     self.run_with_server(body)
 
@@ -447,20 +447,20 @@ class TestResolveBundleByRef(OpenpilotTestCase):
 
   def test_qcom_ref_resolves_to_qcom_slot(self):
     small = self._bundle("small")
-    assert resolve_bundle_by_ref("small", {"qcom": [small], "chestnut": []}) == (small, "qcom")
+    assert resolve_bundle_by_ref("small", {"qcom": [small], "usbgpu": []}) == (small, "qcom")
 
-  def test_chestnut_ref_resolves_to_chestnut_slot(self):
+  def test_usbgpu_ref_resolves_to_usbgpu_slot(self):
     big = self._bundle("big")
-    assert resolve_bundle_by_ref("big", {"qcom": [], "chestnut": [big]}) == (big, "chestnut")
+    assert resolve_bundle_by_ref("big", {"qcom": [], "usbgpu": [big]}) == (big, "usbgpu")
 
   def test_unknown_ref_returns_none(self):
-    source_bundles = {"qcom": [self._bundle("small")], "chestnut": []}
+    source_bundles = {"qcom": [self._bundle("small")], "usbgpu": []}
     assert resolve_bundle_by_ref("nope", source_bundles) is None
 
 
 def manifest_bundle(short_name: str, ref: str, index: int = 0, is_big: bool = False) -> dict:
   """Minimal manifest bundle dict, version-compatible (no chunks to avoid disk side effects).
-  Big (chestnut) bundles carry `is_big: true` in the manifest JSON."""
+  Big (usbgpu) bundles carry `is_big: true` in the manifest JSON."""
   return {
     "index": index,
     "short_name": short_name,
@@ -489,15 +489,15 @@ class TestModelFetcherSources(OpenpilotTestCase):
   """Both manifests are always maintained: get_bundles_for_source exposes either
   source by name, and active_source picks which one matches the attached hardware."""
 
-  def _make_params(self, qcom_manifest, chestnut_manifest):
+  def _make_params(self, qcom_manifest, usbgpu_manifest):
     params = mock.MagicMock()
 
     def get(key):
       if key == "ModelManager_ModelsCache":
         return qcom_manifest
-      if key == "ModelManager_ModelsCache_Chestnut":
-        return chestnut_manifest
-      if key in ("ModelManager_LastSyncTime", "ModelManager_LastSyncTime_Chestnut"):
+      if key == "ModelManager_ModelsCache_USBGPU":
+        return usbgpu_manifest
+      if key in ("ModelManager_LastSyncTime", "ModelManager_LastSyncTime_USBGPU"):
         return fresh_sync_time()
       return None
 
@@ -506,14 +506,14 @@ class TestModelFetcherSources(OpenpilotTestCase):
 
   def test_active_source_follows_chestnut_presence(self):
     assert ModelFetcher.active_source(False) == "qcom"
-    assert ModelFetcher.active_source(True) == "chestnut"
+    assert ModelFetcher.active_source(True) == "usbgpu"
 
   def test_get_bundles_for_source_returns_each_source(self):
     params = self._make_params({"bundles": [manifest_bundle("small", "aaa")]},
                                {"bundles": [manifest_bundle("big", "bbb", is_big=True)]})
     fetcher = ModelFetcher(params)
     assert [bundle.ref for bundle in fetcher.get_bundles_for_source("qcom")] == ["aaa"]
-    assert [bundle.ref for bundle in fetcher.get_bundles_for_source("chestnut")] == ["bbb"]
+    assert [bundle.ref for bundle in fetcher.get_bundles_for_source("usbgpu")] == ["bbb"]
 
   def test_get_bundles_for_source_unknown(self):
     assert ModelFetcher(mock.MagicMock()).get_bundles_for_source("bogus") == []
@@ -522,16 +522,16 @@ class TestModelFetcherSources(OpenpilotTestCase):
     params = self._make_params({"bundles": [manifest_bundle("small", "aaa")]},
                                {"bundles": [manifest_bundle("big", "bbb", is_big=True)]})
     qcom_bundles = get_cached_bundles(params, "qcom")
-    chestnut_bundles = get_cached_bundles(params, "chestnut")
+    usbgpu_bundles = get_cached_bundles(params, "usbgpu")
     assert [b.ref for b in qcom_bundles] == ["aaa"]
-    assert [b.ref for b in chestnut_bundles] == ["bbb"]
+    assert [b.ref for b in usbgpu_bundles] == ["bbb"]
     assert qcom_bundles[0].displayName == "SMALL"
 
   def test_get_cached_bundles_empty_when_missing(self):
     params = mock.MagicMock()
     params.get.return_value = None
     assert get_cached_bundles(params, "qcom") == []
-    assert get_cached_bundles(params, "chestnut") == []
+    assert get_cached_bundles(params, "usbgpu") == []
 
   def test_get_cached_bundles_unknown_source(self):
     assert get_cached_bundles(mock.MagicMock(), "bogus") == []
@@ -543,27 +543,27 @@ class TestModelFetcherSources(OpenpilotTestCase):
     assert active_json_calls, "expected ModelManager_ActiveJson to be written"
     assert active_json_calls[-1].args[1] == {
       "qcom": ModelFetcher.MODEL_URL,
-      "chestnut": ModelFetcher.MODEL_URL_CHESTNUT,
+      "usbgpu": ModelFetcher.MODEL_URL_USBGPU,
     }
 
 
 
 class TestSourceCacheIntegrity(OpenpilotTestCase):
   """Each source's cached manifest must contain only that source's models; the
-  `is_big` flag in the JSON marks the big (chestnut) models. A mismatched cache is
+  `is_big` flag in the JSON marks the big (usbgpu) models. A mismatched cache is
   legacy data from before the per-source split (the active manifest was cached
   under the unsuffixed key regardless of hardware) and is refetched. This
   replaces the old one-time bundle migration."""
 
-  def _make_params(self, qcom_manifest, chestnut_manifest):
+  def _make_params(self, qcom_manifest, usbgpu_manifest):
     params = mock.MagicMock()
 
     def get(key):
       if key == "ModelManager_ModelsCache":
         return qcom_manifest
-      if key == "ModelManager_ModelsCache_Chestnut":
-        return chestnut_manifest
-      if key in ("ModelManager_LastSyncTime", "ModelManager_LastSyncTime_Chestnut"):
+      if key == "ModelManager_ModelsCache_USBGPU":
+        return usbgpu_manifest
+      if key in ("ModelManager_LastSyncTime", "ModelManager_LastSyncTime_USBGPU"):
         return fresh_sync_time()
       return None
 
@@ -584,13 +584,13 @@ class TestSourceCacheIntegrity(OpenpilotTestCase):
       bundles = fetcher.get_bundles_for_source("qcom")
     assert [bundle.ref for bundle in bundles] == ["aaa"]
 
-  def test_chestnut_cache_without_big_models_is_refetched(self):
+  def test_usbgpu_cache_without_big_models_is_refetched(self):
     params = self._make_params({"bundles": [manifest_bundle("small", "aaa")]},
                                {"bundles": [manifest_bundle("big2", "ccc")]})
     fetcher = ModelFetcher(params)
     fetched = self._fetched(manifest_bundle("big", "bbb", is_big=True))
     with mock.patch.object(fetcher, "_fetch_and_cache_models", return_value=fetched):
-      bundles = fetcher.get_bundles_for_source("chestnut")
+      bundles = fetcher.get_bundles_for_source("usbgpu")
     assert [bundle.ref for bundle in bundles] == ["bbb"]
 
   def test_matching_caches_are_used_without_fetch(self):
@@ -599,7 +599,7 @@ class TestSourceCacheIntegrity(OpenpilotTestCase):
     fetcher = ModelFetcher(params)
     with mock.patch.object(fetcher, "_fetch_and_cache_models", side_effect=AssertionError("cache should be used")):
       assert [bundle.ref for bundle in fetcher.get_bundles_for_source("qcom")] == ["aaa"]
-      assert [bundle.ref for bundle in fetcher.get_bundles_for_source("chestnut")] == ["bbb"]
+      assert [bundle.ref for bundle in fetcher.get_bundles_for_source("usbgpu")] == ["bbb"]
 
   def test_stale_version_cache_is_refetched(self):
     """A source-matching cache whose bundles are all filtered by the selector version
@@ -660,28 +660,28 @@ class TestActiveBundleValidation(OpenpilotTestCase):
       bundle.runner = runner
     return bundle.to_dict()
 
-  def _params(self, qcom=None, chestnut=None):
+  def _params(self, qcom=None, usbgpu=None):
     params = mock.MagicMock()
 
     def get(key, *args, **kwargs):
-      return {"ModelManager_ActiveBundle": qcom, "ModelManager_ActiveBundleChestnut": chestnut}.get(key)
+      return {"ModelManager_ActiveBundle": qcom, "ModelManager_ActiveBundleUSBGPU": usbgpu}.get(key)
 
     params.get.side_effect = get
     return params
 
   def test_empty_catalog_does_not_reset_slot(self):
     params = self._params(qcom=self._raw_bundle("small"))
-    with mock.patch("openpilot.sunnypilot.models.helpers.chestnut_present", return_value=False):
-      validate_active_bundles(params, {"qcom": [], "chestnut": []})
+    with mock.patch("openpilot.sunnypilot.models.helpers.usbgpu_present", return_value=False):
+      validate_active_bundles(params, {"qcom": [], "usbgpu": []})
     params.remove.assert_not_called()
 
   def test_reset_recomputes_runner_from_surviving_slot(self):
     tinygrad = int(custom.ModelManagerSP.Runner.tinygrad)
     big_raw = self._raw_bundle("big", runner=tinygrad)
-    params = self._params(qcom=self._raw_bundle("gone"), chestnut=big_raw)
+    params = self._params(qcom=self._raw_bundle("gone"), usbgpu=big_raw)
     catalog = {"qcom": [custom.ModelManagerSP.ModelBundle(**self._raw_bundle("other"))],
-               "chestnut": [custom.ModelManagerSP.ModelBundle(**big_raw)]}
-    with mock.patch("openpilot.sunnypilot.models.helpers.chestnut_present", return_value=True):
+               "usbgpu": [custom.ModelManagerSP.ModelBundle(**big_raw)]}
+    with mock.patch("openpilot.sunnypilot.models.helpers.usbgpu_present", return_value=True):
       validate_active_bundles(params, catalog)
     params.remove.assert_called_once_with("ModelManager_ActiveBundle")
     runner_puts = [call for call in params.put.call_args_list if call.args[0] == "ModelRunnerTypeCache"]
@@ -689,7 +689,7 @@ class TestActiveBundleValidation(OpenpilotTestCase):
 
 
 class TestActiveBundleSelection(OpenpilotTestCase):
-  """The effective active bundle is the active source's slot: chestnut when a GPU is
+  """The effective active bundle is the active source's slot: usbgpu when a GPU is
   present, qcom otherwise. An empty active slot means the hardware default (stock
   runner), never the other slot's pick - modeld_v2 requires a real bundle."""
 
@@ -700,37 +700,37 @@ class TestActiveBundleSelection(OpenpilotTestCase):
     bundle.minimumSelectorVersion = 18
     return bundle.to_dict()
 
-  def _params(self, qcom=None, chestnut=None):
+  def _params(self, qcom=None, usbgpu=None):
     params = mock.MagicMock()
 
     def get(key, *args, **kwargs):
       if key == "ModelManager_ActiveBundle":
         return qcom
-      if key == "ModelManager_ActiveBundleChestnut":
-        return chestnut
+      if key == "ModelManager_ActiveBundleUSBGPU":
+        return usbgpu
       return None
 
     params.get.side_effect = get
     return params
 
   def test_selected_bundle_is_per_slot(self):
-    params = self._params(qcom=self._raw_bundle("small"), chestnut=self._raw_bundle("big"))
+    params = self._params(qcom=self._raw_bundle("small"), usbgpu=self._raw_bundle("big"))
     assert get_selected_bundle(params, "qcom").ref == "small"
-    assert get_selected_bundle(params, "chestnut").ref == "big"
+    assert get_selected_bundle(params, "usbgpu").ref == "big"
 
   def test_no_gpu_uses_qcom_slot(self):
-    params = self._params(qcom=self._raw_bundle("small"), chestnut=self._raw_bundle("big"))
-    with mock.patch("openpilot.sunnypilot.models.helpers.chestnut_present", return_value=False):
+    params = self._params(qcom=self._raw_bundle("small"), usbgpu=self._raw_bundle("big"))
+    with mock.patch("openpilot.sunnypilot.models.helpers.usbgpu_present", return_value=False):
       assert get_active_bundle(params).ref == "small"
 
-  def test_gpu_uses_chestnut_slot(self):
-    params = self._params(qcom=self._raw_bundle("small"), chestnut=self._raw_bundle("big"))
-    with mock.patch("openpilot.sunnypilot.models.helpers.chestnut_present", return_value=True):
+  def test_gpu_uses_usbgpu_slot(self):
+    params = self._params(qcom=self._raw_bundle("small"), usbgpu=self._raw_bundle("big"))
+    with mock.patch("openpilot.sunnypilot.models.helpers.usbgpu_present", return_value=True):
       assert get_active_bundle(params).ref == "big"
 
   def test_gpu_without_big_selection_is_hardware_default(self):
-    params = self._params(qcom=self._raw_bundle("small"), chestnut=None)
-    with mock.patch("openpilot.sunnypilot.models.helpers.chestnut_present", return_value=True):
+    params = self._params(qcom=self._raw_bundle("small"), usbgpu=None)
+    with mock.patch("openpilot.sunnypilot.models.helpers.usbgpu_present", return_value=True):
       assert get_active_bundle(params) is None
 
 
@@ -748,36 +748,36 @@ class TestEffectiveSource(OpenpilotTestCase):
     return bundle.to_dict()
 
   def test_runtime_no_gpu(self):
-    with mock.patch("openpilot.sunnypilot.models.helpers.chestnut_present", return_value=False):
+    with mock.patch("openpilot.sunnypilot.models.helpers.usbgpu_present", return_value=False):
       assert get_active_source() == "qcom"
 
   def test_runtime_gpu_present(self):
-    with mock.patch("openpilot.sunnypilot.models.helpers.chestnut_present", return_value=True):
-      assert get_active_source() == "chestnut"
+    with mock.patch("openpilot.sunnypilot.models.helpers.usbgpu_present", return_value=True):
+      assert get_active_source() == "usbgpu"
 
   def test_display_offroad_gpu_present_shows_big(self):
-    assert get_active_source(chestnut=True, chestnut_active=False, chestnut_loading=False, offroad=True) == "chestnut"
+    assert get_active_source(usbgpu=True, usbgpu_active=False, usbgpu_loading=False, offroad=True) == "usbgpu"
 
   def test_display_onroad_gpu_loading_shows_big(self):
-    assert get_active_source(chestnut=True, chestnut_active=False, chestnut_loading=True, offroad=False) == "chestnut"
+    assert get_active_source(usbgpu=True, usbgpu_active=False, usbgpu_loading=True, offroad=False) == "usbgpu"
 
   def test_display_onroad_gpu_active_shows_big(self):
-    assert get_active_source(chestnut=True, chestnut_active=True, chestnut_loading=False, offroad=False) == "chestnut"
+    assert get_active_source(usbgpu=True, usbgpu_active=True, usbgpu_loading=False, offroad=False) == "usbgpu"
 
   def test_display_onroad_gpu_idle_shows_small(self):
-    assert get_active_source(chestnut=True, chestnut_active=False, chestnut_loading=False, offroad=False) == "qcom"
+    assert get_active_source(usbgpu=True, usbgpu_active=False, usbgpu_loading=False, offroad=False) == "qcom"
 
   def test_display_active_none_is_idle(self):
-    assert get_active_source(chestnut=True, chestnut_active=None, chestnut_loading=False, offroad=False) == "qcom"
+    assert get_active_source(usbgpu=True, usbgpu_active=None, usbgpu_loading=False, offroad=False) == "qcom"
 
   def test_active_bundle_follows_source(self):
     params = mock.MagicMock()
     params.get.side_effect = lambda key: {"ModelManager_ActiveBundle": self._raw_bundle("small"),
-                                          "ModelManager_ActiveBundleChestnut": self._raw_bundle("big")}.get(key)
-    with mock.patch("openpilot.sunnypilot.models.helpers.chestnut_present", return_value=False):
+                                          "ModelManager_ActiveBundleUSBGPU": self._raw_bundle("big")}.get(key)
+    with mock.patch("openpilot.sunnypilot.models.helpers.usbgpu_present", return_value=False):
       assert get_active_bundle(params).ref == "small"
-    assert get_selected_bundle(params, get_active_source(chestnut=True, chestnut_active=False,
-                                                         chestnut_loading=False, offroad=True)).ref == "big"
+    assert get_selected_bundle(params, get_active_source(usbgpu=True, usbgpu_active=False,
+                                                         usbgpu_loading=False, offroad=True)).ref == "big"
 
 
 @unittest.skipUnless(os.environ.get('RUN_INTEGRATION_TESTS'), 'requires external network')

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,9 @@ def initialize_params(params) -> list[dict[str, Any]]:
   keys.extend([
     "ToyotaEnforceStockLongitudinal",
     "ToyotaStopAndGoHack",
+    "ToyotaTSS2Long",
+    "ToyotaEnhancedBsm",
+    "ToyotaAutoHold",
   ])
 
   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,65 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+
+import numpy as np
+
+from openpilot.cereal import custom
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+from openpilot.sunnypilot import get_sanitize_int_param
+
+AccelProfile = custom.LongitudinalPlanSP.AccelController.Profile
+
+MAX_ACCEL_BREAKPOINTS = [0., 3., 5., 10., 20., 25., 40.]  # m/s
+MAX_ACCEL_PROFILES = {
+  AccelProfile.eco:    [1.60, 1.48, 1.22, 0.86, 0.66, 0.52, 0.40],
+  AccelProfile.normal: [1.90, 1.70, 1.42, 0.99, 0.80, 0.66, 0.52],
+  AccelProfile.sport:  [2.00, 2.00, 1.86, 1.30, 1.02, 0.86, 0.72],
+}
+CRUISE_DECEL_RESPONSE_TIME = {  # seconds
+  AccelProfile.eco: 4.0,
+  AccelProfile.normal: 3.5,
+  AccelProfile.sport: 3.0,
+}
+CRUISE_DECEL_ACCEL = {  # m/s^2; comfort-first cruise deceleration target
+  AccelProfile.eco: -0.35,
+  AccelProfile.normal: -0.50,
+  AccelProfile.sport: -0.65,
+}
+
+
+class AccelController:
+  def __init__(self):
+    self.params = Params()
+    self.frame = 0
+    self._profile = get_sanitize_int_param("AccelPersonality", AccelProfile.eco, AccelProfile.sport, self.params)
+    self._enabled = self.params.get_bool("AccelPersonalityEnabled")
+
+  def update(self) -> None:
+    self.frame += 1
+    if self.frame % int(1.0 / DT_MDL) == 0:
+      self._profile = get_sanitize_int_param("AccelPersonality", AccelProfile.eco, AccelProfile.sport, self.params)
+      self._enabled = self.params.get_bool("AccelPersonalityEnabled")
+
+  @property
+  def profile(self) -> int:
+    return self._profile
+
+  def is_enabled(self) -> bool:
+    return self._enabled
+
+  def get_max_accel(self, v_ego: float) -> float:
+    return float(np.interp(max(0.0, v_ego), MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[self._profile]))
+
+  def get_cruise_target(self, v_ego: float, v_target: float) -> float:
+    if not np.isfinite(v_target) or v_target <= 0.0 or v_target >= v_ego:
+      return v_target
+
+    response_time = CRUISE_DECEL_RESPONSE_TIME[self._profile]
+    target_delta = v_target - v_ego
+    target_delta = max(target_delta / response_time, CRUISE_DECEL_ACCEL[self._profile] * response_time)
+    return float(v_ego + target_delta)

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

@@ -0,0 +1,344 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+
+import numpy as np
+
+from opendbc.car.interfaces import ACCEL_MAX
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+from openpilot.common.test import OpenpilotTestCase
+from openpilot.selfdrive.controls.lib.longitudinal_planner import (
+  A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS, A_CRUISE_MIN, J_CRUISE_VALS, get_cruise_accel,
+)
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import (
+  AccelController, AccelProfile, CRUISE_DECEL_ACCEL, CRUISE_DECEL_RESPONSE_TIME, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES,
+)
+
+
+class TestAccelController(OpenpilotTestCase):
+  def setUp(self):
+    self.params = Params()
+    self.params.put_bool("AccelPersonalityEnabled", True, block=True)
+    self.params.put("AccelPersonality", AccelProfile.normal, block=True)
+
+  def set_profile(self, profile: int) -> AccelController:
+    self.params.put("AccelPersonality", profile, block=True)
+    return AccelController()
+
+  def test_table_breakpoints(self):
+    for profile, values in MAX_ACCEL_PROFILES.items():
+      controller = self.set_profile(profile)
+      for speed, expected in zip(MAX_ACCEL_BREAKPOINTS, values, strict=True):
+        assert controller.get_max_accel(speed) == expected
+
+  def test_profile_ordering_and_bounds(self):
+    controllers = {
+      AccelProfile.eco: self.set_profile(AccelProfile.eco),
+      AccelProfile.normal: self.set_profile(AccelProfile.normal),
+      AccelProfile.sport: self.set_profile(AccelProfile.sport),
+    }
+    previous = {profile: float("inf") for profile in controllers}
+
+    for speed in np.linspace(0.0, 55.0, 551):
+      values = {profile: controller.get_max_accel(speed) for profile, controller in controllers.items()}
+      assert 0.0 <= values[AccelProfile.eco] <= values[AccelProfile.normal] <= values[AccelProfile.sport] <= 2.0
+      for profile, value in values.items():
+        assert value <= previous[profile]
+        previous[profile] = value
+
+  def test_profiles_stay_within_openpilot_accel_max(self):
+    for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport):
+      controller = self.set_profile(profile)
+      for speed in np.linspace(0.0, 55.0, 551):
+        assert controller.get_max_accel(speed) <= ACCEL_MAX
+
+  def test_profiles_have_material_separation(self):
+    controllers = [self.set_profile(profile) for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport)]
+    for speed in MAX_ACCEL_BREAKPOINTS:
+      eco, normal, sport = (controller.get_max_accel(speed) for controller in controllers)
+      assert normal - eco >= 0.1 - 1e-12
+      assert sport - normal >= 0.1 - 1e-12
+    for speed in MAX_ACCEL_BREAKPOINTS[1:-1]:
+      assert controllers[2].get_max_accel(speed) - controllers[0].get_max_accel(speed) >= 0.3 - 1e-12
+
+  def test_profiles_keep_usable_road_speed_acceleration(self):
+    # A previous revision had eco at 0.20 m/s^2 at 40 m/s. 1% of road grade costs 0.098 m/s^2 of gravity, so
+    # that profile cannot hold speed on anything steeper than ~2% and can never recover once it bleeds off.
+    # This is a LOWER bound on purpose: the tapered upper bounds it replaces let highway accel go to zero.
+    controllers = {profile: self.set_profile(profile) for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport)}
+    for speed in np.linspace(8.0, 40.0, 321):
+      stock = float(np.interp(speed, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS))
+      values = {profile: controller.get_max_accel(speed) for profile, controller in controllers.items()}
+      # 0.35 m/s^2 holds a 3% grade; the fractions keep merges and passes usable.
+      assert values[AccelProfile.eco] >= max(0.35, 0.60 * stock), speed
+      assert values[AccelProfile.normal] >= 0.80 * stock, speed
+      assert values[AccelProfile.sport] >= stock, speed
+
+  def test_eco_never_exceeds_stock(self):
+    controller = self.set_profile(AccelProfile.eco)
+    for speed in np.linspace(0.0, 55.0, 551):
+      assert controller.get_max_accel(speed) <= float(np.interp(speed, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS)) + 1e-12, speed
+
+  def test_comfort_profile_caps_taper_after_launch(self):
+    for profile in (AccelProfile.eco, AccelProfile.normal):
+      values = MAX_ACCEL_PROFILES[profile]
+      assert values[3] <= 0.55 * values[0]
+
+  def test_sport_uses_openpilot_accel_max_at_launch(self):
+    controller = self.set_profile(AccelProfile.sport)
+    assert controller.get_max_accel(0.0) == ACCEL_MAX
+    assert all(controller.get_max_accel(speed) <= ACCEL_MAX for speed in np.linspace(0.0, 55.0, 551))
+
+  def test_ceiling_is_continuous_in_speed(self):
+    # The ceiling is the only thing the controller sets, so a step in it is a step in the commanded
+    # acceleration. dt=10 makes the stock jerk limiter a no-op so nothing can hide a discontinuity.
+    for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport):
+      controller = self.set_profile(profile)
+      speeds = np.linspace(0.0, 45.0, 451)
+      spacing = float(speeds[1] - speeds[0])
+      commands = np.asarray([
+        get_cruise_accel(False, 60.0, speed, 0.0, 0.0, _fake_cp(), 10.0, 0.0, True, controller.get_max_accel(speed))
+        for speed in speeds
+      ])
+      assert np.all(np.isfinite(commands)), profile
+      assert np.all(np.abs(np.diff(commands)) <= spacing * 1.05 + 1e-9), profile
+
+  def test_negative_speed_uses_standstill_value(self):
+    controller = self.set_profile(AccelProfile.sport)
+    assert controller.get_max_accel(-1.0) == MAX_ACCEL_PROFILES[AccelProfile.sport][0]
+
+  def test_cruise_decel_response(self):
+    v_ego = 25.0
+    v_target = 22.5
+    targets = {}
+    for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport):
+      controller = self.set_profile(profile)
+      targets[profile] = controller.get_cruise_target(v_ego, v_target)
+      assert np.isclose(targets[profile] - v_ego, (v_target - v_ego) / CRUISE_DECEL_RESPONSE_TIME[profile])
+
+    assert v_ego > targets[AccelProfile.eco] > targets[AccelProfile.normal] > targets[AccelProfile.sport] > v_target
+    assert all(CRUISE_DECEL_RESPONSE_TIME[profile] >= 3.0 for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport))
+    assert all(CRUISE_DECEL_ACCEL[profile] < 0.0 for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport))
+
+  def test_cruise_target_bypasses_non_decel_requests(self):
+    controller = self.set_profile(AccelProfile.eco)
+    assert controller.get_cruise_target(20.0, 25.0) == 25.0
+    assert controller.get_cruise_target(20.0, 20.0) == 20.0
+    assert controller.get_cruise_target(20.0, 0.0) == 0.0
+    assert np.isnan(controller.get_cruise_target(20.0, float("nan")))
+
+  def test_profile_change_refreshes_ceiling(self):
+    controller = self.set_profile(AccelProfile.normal)
+    self.params.put("AccelPersonality", AccelProfile.sport, block=True)
+    controller.frame = int(1.0 / DT_MDL) - 1
+    controller.update()
+    index = MAX_ACCEL_BREAKPOINTS.index(10.0)
+    assert controller.get_max_accel(10.0) == MAX_ACCEL_PROFILES[AccelProfile.sport][index]
+
+  def test_params_refresh_once_per_second(self):
+    controller = self.set_profile(AccelProfile.normal)
+    self.params.put("AccelPersonality", AccelProfile.sport, block=True)
+    controller.update()
+    assert controller.profile == AccelProfile.normal
+    controller.frame = int(1.0 / DT_MDL) - 1
+    controller.update()
+    assert controller.profile == AccelProfile.sport
+    assert controller.frame == 0
+
+  def test_enabled_param_refresh(self):
+    controller = self.set_profile(AccelProfile.normal)
+    self.params.put_bool("AccelPersonalityEnabled", False, block=True)
+    controller.frame = int(1.0 / DT_MDL) - 1
+    controller.update()
+    assert not controller.is_enabled()
+    assert controller.frame == 0
+
+
+class TestPlannerIntegration(OpenpilotTestCase):
+  def setUp(self):
+    self.params = Params()
+    self.params.put_bool("AccelPersonalityEnabled", False, block=True)
+
+  def test_none_override_matches_stock(self):
+    for e2e in (False, True):
+      for allow_throttle in (False, True):
+        args = (e2e, 30.0, 12.0, 0.2, 4.0, _fake_cp(), DT_MDL, -0.3, allow_throttle)
+        assert get_cruise_accel(*args) == get_cruise_accel(*args, max_accel_override=None)
+
+  def test_profiles_do_not_change_far_braking(self):
+    # The ceiling is an upper bound only, so it can never participate in a deceleration. Braking authority
+    # stays with stock's clip to A_CRUISE_MIN for every profile.
+    args = (False, 0.0, 20.0, 0.0, 0.0, _fake_cp(), 10.0, -0.3, True)
+    stock = get_cruise_accel(*args)
+    assert stock == A_CRUISE_MIN
+    for profile_values in MAX_ACCEL_PROFILES.values():
+      assert get_cruise_accel(*args, max_accel_override=profile_values[0]) == stock
+
+  def test_stock_jerk_limit_still_owns_smoothing(self):
+    speed = 8.0
+    sport_limit = np.interp(speed, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[AccelProfile.sport])
+    target = get_cruise_accel(False, 30.0, speed, 0.0, 0.0, _fake_cp(), DT_MDL, 0.0, True, sport_limit)
+    jerk_limit = np.interp(speed, A_CRUISE_MAX_BP, J_CRUISE_VALS) * DT_MDL
+    assert np.isclose(target, jerk_limit)
+
+  def test_disabled_leaves_stock_limit_active(self):
+    planner = _bare_planner()
+    assert planner.get_max_accel_override(5.0) is None
+    assert planner.accel_controller_active is False
+
+  def test_enabled_profile_applies_to_cruise_candidate(self):
+    self.params.put_bool("AccelPersonalityEnabled", True, block=True)
+    planner = _bare_planner()
+    expected = np.interp(5.0, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[AccelProfile.normal])
+    assert planner.get_max_accel_override(5.0) == expected
+
+  def test_enabled_acc_uses_python_native_telemetry_types(self):
+    self.params.put_bool("AccelPersonalityEnabled", True, block=True)
+    self.params.put("AccelPersonality", AccelProfile.sport, block=True)
+    planner = _bare_planner()
+    expected = np.interp(5.0, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[AccelProfile.sport])
+    assert planner.get_max_accel_override(5.0) == expected
+    assert type(planner.accel_controller_active) is bool
+    assert type(planner.accel_controller.is_enabled()) is bool
+    assert type(planner.accel_controller.profile) is int
+
+  def test_normal_profile_uses_tuned_limit(self):
+    self.params.put_bool("AccelPersonalityEnabled", True, block=True)
+    self.params.put("AccelPersonality", AccelProfile.normal, block=True)
+    planner = _bare_planner()
+    expected = np.interp(5.0, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[AccelProfile.normal])
+    assert planner.get_max_accel_override(5.0) == expected
+
+  def test_ceiling_applies_to_every_target_source(self):
+    from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanSource
+
+    self.params.put_bool("AccelPersonalityEnabled", True, block=True)
+    planner = _bare_planner()
+    speed = 29.0
+    expected = planner.accel_controller.get_max_accel(speed)
+
+    for source in (LongitudinalPlanSource.cruise, LongitudinalPlanSource.sccVision,
+                   LongitudinalPlanSource.sccMap, LongitudinalPlanSource.speedLimitAssist):
+      planner.source = source
+      assert np.isclose(planner.get_max_accel_override(speed), expected), source
+
+  def test_ceiling_remains_active_without_throttle_intent(self):
+    self.params.put_bool("AccelPersonalityEnabled", True, block=True)
+    self.params.put("AccelPersonality", AccelProfile.eco, block=True)
+    planner = _bare_planner()
+    planner.allow_throttle = False
+    assert planner.get_max_accel_override(12.0) == planner.accel_controller.get_max_accel(12.0)
+
+  def test_profile_switch_uses_stock_jerk_limit(self):
+    self.params.put_bool("AccelPersonalityEnabled", True, block=True)
+    self.params.put("AccelPersonality", AccelProfile.sport, block=True)
+    planner = _bare_planner()
+    v_ego = 12.0
+    planner.a_cruise = planner.accel_controller.get_max_accel(v_ego)
+
+    self.params.put("AccelPersonality", AccelProfile.eco, block=True)
+    planner.accel_controller.frame = int(1.0 / DT_MDL) - 1
+    planner.accel_controller.update()
+    ceiling = planner.get_max_accel_override(v_ego)
+    previous = planner.a_cruise
+    accel = get_cruise_accel(False, 30.0, v_ego, previous, 0.0, _fake_cp(), DT_MDL, 0.0, True, ceiling)
+    jerk_step = float(np.interp(v_ego, A_CRUISE_MAX_BP, J_CRUISE_VALS)) * DT_MDL
+
+    assert previous > ceiling
+    assert np.isclose(previous - accel, jerk_step)
+    assert planner.a_cruise == previous
+
+  def test_cruise_target_shaping_is_source_and_force_decel_gated(self):
+    from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanSource
+
+    self.params.put_bool("AccelPersonalityEnabled", True, block=True)
+    planner = _bare_planner()
+    shaped = planner.get_cruise_target_override(25.0, 22.5, force_decel=False)
+    assert 22.5 < shaped < 25.0
+
+    for source in (LongitudinalPlanSource.sccVision, LongitudinalPlanSource.sccMap, LongitudinalPlanSource.speedLimitAssist):
+      planner.source = source
+      assert planner.get_cruise_target_override(25.0, 22.5, force_decel=False) == 22.5
+
+    planner.source = LongitudinalPlanSource.cruise
+    assert planner.get_cruise_target_override(25.0, 0.0, force_decel=True) == 0.0
+
+  def test_e2e_candidate_is_held_through_a_brake_but_not_otherwise(self):
+    # Route 000005dd: e2e -> lead1 stepped +2.25 m/s^2 in one frame (45 m/s^3) and back the next, while the
+    # model held desiredAcceleration at -1.63 and never moved more than 0.024. Dropping a candidate the model
+    # still owns is what produced the brake/gas/brake flip.
+    from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import E2E_BRAKE_HOLD_ACCEL, MpcPlanSource
+
+    planner = _bare_planner()
+
+    class _Mpc:
+      source = MpcPlanSource.cruise
+
+    class _Dec:
+      def __init__(self):
+        self._active = True
+        self._mode = "acc"
+
+      def active(self):
+        return self._active
+
+      def mode(self):
+        return self._mode
+
+    planner.mpc = _Mpc()
+    planner.dec = _Dec()
+
+    def sm(experimental: bool, model_accel: float):
+      return {
+        'selfdriveState': type("S", (), {"experimentalMode": experimental})(),
+        'modelV2': type("M", (), {"action": type("A", (), {"desiredAcceleration": model_accel})()})(),
+      }
+
+    braking = E2E_BRAKE_HOLD_ACCEL - 1.0
+
+    # Not experimental: never e2e, whatever the model wants.
+    assert planner.is_e2e(sm(False, braking)) is False
+
+    # DEC in acc, and the model was NOT the selected source: acc stands. This is the case that must stay
+    # untouched, or a phantom model brake could be pulled into the arbitration that never won it.
+    planner.mpc.source = MpcPlanSource.lead0
+    assert planner.is_e2e(sm(True, braking)) is False
+
+    # DEC in acc, model WAS selected and is still braking: hold it rather than release the brake.
+    planner.mpc.source = MpcPlanSource.e2e
+    assert planner.is_e2e(sm(True, braking)) is True
+
+    # Still selected but no longer braking: release, DEC's decision stands.
+    assert planner.is_e2e(sm(True, 0.0)) is False
+    assert planner.is_e2e(sm(True, E2E_BRAKE_HOLD_ACCEL + 0.01)) is False
+
+    # DEC blended, or DEC inactive, is unconditionally e2e as before.
+    planner.dec._mode = "blended"
+    assert planner.is_e2e(sm(True, 1.0)) is True
+    planner.dec._mode = "acc"
+    planner.dec._active = False
+    assert planner.is_e2e(sm(True, 1.0)) is True
+
+
+def _fake_cp():
+  class CP:
+    steerRatio = 15.0
+    wheelbase = 2.7
+
+  return CP()
+
+
+def _bare_planner():
+  from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP, LongitudinalPlanSource
+
+  planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
+  planner.accel_controller = AccelController()
+  planner.accel_controller_active = False
+  planner.allow_throttle = True
+  planner.a_cruise = 0.0
+  planner.source = LongitudinalPlanSource.cruise
+  return planner

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

@@ -0,0 +1,329 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+
+from collections.abc import Callable
+
+import numpy as np
+
+from openpilot.common.constants import CV
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+from openpilot.common.test import OpenpilotTestCase
+from openpilot.selfdrive.controls.lib.drive_helpers import should_stop
+from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MAX_BP, A_CRUISE_MIN, J_CRUISE_VALS, get_cruise_accel
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalPlanSource, T_IDXS as T_IDXS_MPC
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import (
+  AccelController, AccelProfile, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES,
+)
+from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PRIUS_TSS2_ROUTE_MODEL, PlantSP
+
+
+class CarParams:
+  steerRatio = 15.0
+  wheelbase = 2.7
+
+
+def _set_mpc_acceleration(plant: PlantSP, acceleration: float = 2.0) -> None:
+  def update(_radar_state, **_kwargs):
+    mpc = plant.planner.mpc
+    mpc.source = LongitudinalPlanSource.lead0
+    mpc.v_solution[:] = mpc.x0[1] + acceleration * T_IDXS_MPC
+    mpc.a_solution.fill(acceleration)
+    mpc.j_solution.fill(0.0)
+
+  plant.planner.mpc.update = update
+
+
+def run_profile(profile: int, *, enabled: bool = True, speed: float = 0.0, v_cruise: float = 30.0,
+                v_cruise_fn: Callable[[int], float] | None = None, e2e: bool = False, steps: int = 120,
+                speed_noise: float = 0.0, seed: int = 0):
+  params = Params()
+  params.put_bool("AccelPersonalityEnabled", enabled, block=True)
+  params.put("AccelPersonality", profile, block=True)
+  controller = AccelController()
+  rng = np.random.default_rng(seed)
+
+  accel = 0.0
+  rows = []
+  for frame in range(steps):
+    target_speed = v_cruise if v_cruise_fn is None else v_cruise_fn(frame)
+    measured = speed + (float(rng.normal(0.0, speed_noise)) if speed_noise else 0.0)
+    max_accel_override = controller.get_max_accel(measured) if controller.is_enabled() else None
+    if controller.is_enabled():
+      target_speed = controller.get_cruise_target(measured, target_speed)
+    accel = get_cruise_accel(e2e, target_speed, measured, accel, 0.0, CarParams(), DT_MDL, 2.0, True, max_accel_override)
+    speed = max(0.0, speed + accel * DT_MDL)
+    rows.append((speed, accel, should_stop(speed, accel)))
+  return rows
+
+
+def run_vehicle_profile(profile: int, duration: float = 80.0, enabled: bool = True, speed: float = 0.0,
+                        v_cruise_fn: Callable[[float], float] | None = None):
+  params = Params()
+  params.put_bool("AccelPersonalityEnabled", enabled, block=True)
+  params.put("AccelPersonality", profile, block=True)
+
+  plant = PlantSP(speed=speed, actuator_model=PRIUS_TSS2_ROUTE_MODEL, run_long_control=True)
+  _set_mpc_acceleration(plant)
+  rows = []
+  while plant.current_time < duration:
+    v_cruise = 25.0 if v_cruise_fn is None else v_cruise_fn(plant.current_time)
+    result = plant.step(v_cruise=v_cruise)
+    rows.append((plant.current_time, result["speed"], result["a_target"], result["actuator_command"], result["acceleration"]))
+  return np.asarray(rows)
+
+
+class TestAccelControllerClosedLoop(OpenpilotTestCase):
+  def test_profiles_are_immediate_smooth_and_clearly_distinct(self):
+    traces = {profile: run_vehicle_profile(profile) for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport)}
+    stock = run_vehicle_profile(AccelProfile.normal, enabled=False)
+
+    def crossing(trace, speed):
+      return float(trace[np.flatnonzero(trace[:, 1] >= speed)[0], 0])
+
+    time_to_20 = {profile: crossing(trace, 20.0 * CV.MPH_TO_MS) for profile, trace in traces.items()}
+    time_to_50 = {profile: crossing(trace, 50.0 * CV.MPH_TO_MS) for profile, trace in traces.items()}
+    first_motion = {profile: int(np.flatnonzero(trace[:, 1] > 0.01)[0]) for profile, trace in traces.items()}
+
+    self.assertEqual(len(set(first_motion.values())), 1)
+    self.assertTrue(all(trace[0, 2] > 0.0 and trace[1, 3] > 0.0 for trace in traces.values()))
+    self.assertLess(time_to_20[AccelProfile.eco], 8.0)
+    self.assertLess(time_to_50[AccelProfile.eco], 27.0)
+    self.assertGreaterEqual(time_to_20[AccelProfile.eco] - time_to_20[AccelProfile.normal], 0.5)
+    self.assertGreaterEqual(time_to_20[AccelProfile.normal] - time_to_20[AccelProfile.sport], 0.5)
+    self.assertGreaterEqual(time_to_50[AccelProfile.eco] - time_to_50[AccelProfile.normal], 2.0)
+    self.assertGreaterEqual(time_to_50[AccelProfile.normal] - time_to_50[AccelProfile.sport], 3.0)
+
+    # Asserted against stock rather than against the actuator's rate limit. The peak command jerk in this run
+    # is stock's stop-release ramp at launch (LongCtrlState.stopping -> pid), which on its own already exceeds
+    # PRIUS_TSS2_ROUTE_MODEL.command_rate_limit: measured 4.392 for stock and for all three profiles alike.
+    # An absolute bound here would only be testing that stock ramp, and would pass or fail on stock changes
+    # that have nothing to do with the profiles. What this test can honestly own is that the profiles add no
+    # command jerk of their own.
+    stock_peak_jerk = float(np.max(np.abs(np.diff(stock[:, 3])) / DT_MDL))
+    for profile, trace in traces.items():
+      command_jerk = np.abs(np.diff(trace[:, 3])) / DT_MDL
+      self.assertLessEqual(float(np.max(command_jerk)), stock_peak_jerk + 1e-9, profile)
+
+      settled = np.flatnonzero(trace[:, 1] >= 25.0 - 0.15)
+      self.assertGreater(len(settled), 0)
+      settled_trace = trace[settled[0]:]
+      self.assertGreaterEqual(float(np.min(settled_trace[:, 3])), -0.05)
+      self.assertLessEqual(float(np.max(trace[:, 1])), 25.0 + 1e-9)
+
+  def test_blended_positive_model_request_uses_profile_cruise_cap(self):
+    params = Params()
+    params.put_bool("DynamicExperimentalControl", False, block=True)
+    params.put_bool("AccelPersonalityEnabled", True, block=True)
+    params.put("AccelPersonality", AccelProfile.eco, block=True)
+
+    def request_acceleration(_current_time: float, _speed: float, _acceleration: float) -> tuple[float, bool]:
+      return 2.0, False
+
+    plant = PlantSP(speed=15.0, e2e=True, model_action_fn=request_acceleration)
+    _set_mpc_acceleration(plant)
+    results = [plant.step(v_cruise=35.0) for _ in range(20)]
+    settled = results[-1]
+    eco_limit = float(np.interp(settled["published_v_ego"], MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[AccelProfile.eco]))
+
+    self.assertTrue(settled["controller_active"])
+    self.assertEqual(settled["mpc_source"], LongitudinalPlanSource.cruise)
+    self.assertAlmostEqual(settled["a_target"], eco_limit, delta=0.01)
+    self.assertLess(settled["a_target"], settled["model_action"]["desiredAcceleration"])
+
+  def test_lower_cruise_target_does_not_soften_model_braking(self):
+    params = Params()
+    params.put_bool("DynamicExperimentalControl", False, block=True)
+    params.put("AccelPersonality", AccelProfile.eco, block=True)
+
+    def request_braking(_current_time: float, _speed: float, _acceleration: float) -> tuple[float, bool]:
+      return -0.8, False
+
+    traces = {}
+    for enabled in (False, True):
+      params.put_bool("AccelPersonalityEnabled", enabled, block=True)
+      plant = PlantSP(speed=20.0, e2e=True, model_action_fn=request_braking)
+      _set_mpc_acceleration(plant)
+      traces[enabled] = [plant.step(v_cruise=19.5) for _ in range(10)]
+
+    self.assertTrue(all(row["mpc_source"] == LongitudinalPlanSource.e2e for row in traces[True]))
+    self.assertTrue(all(not row["controller_active"] for trace in traces.values() for row in trace))
+    for key in ("a_target", "should_stop", "mpc_source"):
+      self.assertEqual([row[key] for row in traces[True]], [row[key] for row in traces[False]])
+
+  def test_profile_does_not_change_model_stop_request(self):
+    params = Params()
+    params.put_bool("DynamicExperimentalControl", False, block=True)
+    params.put("AccelPersonality", AccelProfile.eco, block=True)
+
+    def request_stop(_current_time: float, _speed: float, _acceleration: float) -> tuple[float, bool]:
+      return -0.8, True
+
+    traces = {}
+    for enabled in (False, True):
+      params.put_bool("AccelPersonalityEnabled", enabled, block=True)
+      plant = PlantSP(speed=1.0, e2e=True, model_action_fn=request_stop)
+      _set_mpc_acceleration(plant)
+      traces[enabled] = [plant.step(v_cruise=30.0) for _ in range(10)]
+
+    for key in ("a_target", "should_stop", "mpc_source"):
+      self.assertEqual([row[key] for row in traces[True]], [row[key] for row in traces[False]])
+
+  def test_lower_cruise_target_does_not_soften_lead_braking(self):
+    params = Params()
+    params.put_bool("DynamicExperimentalControl", False, block=True)
+    params.put("AccelPersonality", AccelProfile.eco, block=True)
+
+    traces = {}
+    for enabled in (False, True):
+      params.put_bool("AccelPersonalityEnabled", enabled, block=True)
+      plant = PlantSP(speed=20.0)
+      _set_mpc_acceleration(plant, -0.8)
+      traces[enabled] = [plant.step(v_cruise=19.5) for _ in range(10)]
+
+    self.assertTrue(all(row["mpc_source"] == LongitudinalPlanSource.lead0 for row in traces[True]))
+    for key in ("a_target", "should_stop", "mpc_source"):
+      self.assertEqual([row[key] for row in traces[True]], [row[key] for row in traces[False]])
+
+  def test_normal_launch_is_faster_than_eco(self):
+    eco = run_profile(AccelProfile.eco, speed=4.0, steps=120)
+    normal = run_profile(AccelProfile.normal, speed=4.0, steps=120)
+    self.assertGreater(normal[-1][0], eco[-1][0])
+
+  def test_zero_speed_stop_request_is_unchanged(self):
+    # Zero-speed stop requests bypass the small cruise-setpoint pre-shape.
+    for e2e in (False, True):
+      stock = run_profile(AccelProfile.normal, enabled=False, speed=20.0, v_cruise=0.0, e2e=e2e, steps=100)
+      for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport):
+        self.assertEqual(run_profile(profile, speed=20.0, v_cruise=0.0, e2e=e2e, steps=100), stock)
+
+  def test_large_cruise_decel_retains_stock_authority(self):
+    for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport):
+      trace = np.asarray(run_profile(profile, speed=25.0, v_cruise=20.0, steps=220))
+      self.assertAlmostEqual(float(np.min(trace[:, 1])), A_CRUISE_MIN, places=12)
+      self.assertGreaterEqual(float(np.min(trace[:, 0])), 20.0 - 1e-9)
+
+  def test_small_cruise_decel_is_profiled_and_smooth(self):
+    target = 25.0 - 5.0 * CV.MPH_TO_MS
+    stock = np.asarray(run_profile(AccelProfile.normal, enabled=False, speed=25.0, v_cruise=target, steps=300))
+    traces = {
+      profile: np.asarray(run_profile(profile, speed=25.0, v_cruise=target, steps=300))
+      for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport)
+    }
+    peak_decel = {profile: float(np.min(trace[:, 1])) for profile, trace in traces.items()}
+
+    self.assertGreater(peak_decel[AccelProfile.eco], peak_decel[AccelProfile.normal])
+    self.assertGreater(peak_decel[AccelProfile.normal], peak_decel[AccelProfile.sport])
+    self.assertGreater(peak_decel[AccelProfile.sport], float(np.min(stock[:, 1])))
+
+    for trace in traces.values():
+      peak_frame = int(np.argmin(trace[:, 1]))
+      self.assertTrue(np.all(np.diff(trace[peak_frame:, 1]) >= -1e-12))
+      self.assertGreaterEqual(float(np.min(trace[:, 0])), target - 1e-9)
+
+      speeds = np.concatenate(([25.0], trace[:-1, 0]))
+      jerk_limit = np.interp(speeds, A_CRUISE_MAX_BP, J_CRUISE_VALS)
+      jerk = np.abs(np.diff(np.concatenate(([0.0], trace[:, 1])))) / DT_MDL
+      self.assertTrue(np.all(jerk <= jerk_limit + 1e-9))
+
+  def test_small_cruise_decel_stays_smooth_through_actuator(self):
+    target = 25.0 - 5.0 * CV.MPH_TO_MS
+
+    def cruise_target(current_time: float) -> float:
+      return 25.0 if current_time < 2.0 else target
+
+    stock = run_vehicle_profile(AccelProfile.normal, duration=12.0, enabled=False, speed=25.0, v_cruise_fn=cruise_target)
+    traces = [
+      run_vehicle_profile(profile, duration=12.0, speed=25.0, v_cruise_fn=cruise_target)
+      for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport)
+    ]
+    step_frame = int(np.flatnonzero(stock[:, 0] >= 2.0)[0])
+    stock_decel = stock[step_frame:]
+
+    for trace in traces:
+      decel = trace[step_frame:]
+      self.assertGreater(float(np.min(decel[:, 2])), float(np.min(stock_decel[:, 2])))
+      self.assertGreater(float(np.min(decel[:, 4])), float(np.min(stock_decel[:, 4])))
+      self.assertGreaterEqual(float(np.min(decel[:, 1])), target - 1e-9)
+
+      for column in (2, 3, 4):
+        jerk = np.max(np.abs(np.diff(trace[step_frame - 1:, column]))) / DT_MDL
+        stock_jerk = np.max(np.abs(np.diff(stock[step_frame - 1:, column]))) / DT_MDL
+        self.assertLessEqual(float(jerk), float(stock_jerk) + 1e-9)
+
+  def test_blended_launch_respects_profiles(self):
+    traces = {
+      profile: run_profile(profile, v_cruise=8.0, e2e=True, steps=180)
+      for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport)
+    }
+    time_to_five = {
+      profile: next(frame for frame, row in enumerate(rows) if row[0] >= 5.0) * DT_MDL
+      for profile, rows in traces.items()
+    }
+
+    self.assertLess(time_to_five[AccelProfile.sport], time_to_five[AccelProfile.normal])
+    self.assertLess(time_to_five[AccelProfile.normal], time_to_five[AccelProfile.eco])
+
+  def test_launch_ordering_without_departure_delay(self):
+    stock = run_profile(AccelProfile.normal, enabled=False, v_cruise=8.0, steps=160)
+    traces = {
+      profile: run_profile(profile, v_cruise=8.0, steps=160)
+      for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport)
+    }
+    first_motion = {
+      profile: next(frame for frame, row in enumerate(rows) if row[0] > 0.01)
+      for profile, rows in traces.items()
+    }
+    time_to_five = {
+      profile: next(frame for frame, row in enumerate(rows) if row[0] >= 5.0) * DT_MDL
+      for profile, rows in traces.items()
+    }
+    stock_first_motion = next(frame for frame, row in enumerate(stock) if row[0] > 0.01)
+
+    # No launch dead time: motion starts on the same frame as stock, because the ceiling only ever bounds the
+    # command from above and stock's own law owns the first frame.
+    self.assertEqual(len(set(first_motion.values())), 1)
+    self.assertTrue(all(frame == stock_first_motion for frame in first_motion.values()))
+    self.assertGreaterEqual(time_to_five[AccelProfile.eco] - time_to_five[AccelProfile.normal], 0.1)
+    self.assertGreaterEqual(time_to_five[AccelProfile.normal] - time_to_five[AccelProfile.sport], 0.1)
+
+  def test_road_speed_catchup_stays_useful(self):
+    traces = {
+      profile: run_profile(profile, speed=20.0, v_cruise=30.0, steps=100)
+      for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport)
+    }
+    gains = {profile: rows[-1][0] - 20.0 for profile, rows in traces.items()}
+    self.assertGreater(gains[AccelProfile.normal] - gains[AccelProfile.eco], 0.05)
+    self.assertGreater(gains[AccelProfile.sport] - gains[AccelProfile.normal], 0.1)
+
+  def test_full_catchup_trace_respects_stock_jerk(self):
+    for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport):
+      rows = run_profile(profile, v_cruise=30.0, steps=300)
+      previous_speed = 0.0
+      previous_accel = 0.0
+      for speed, accel, _should_stop in rows:
+        jerk_step = float(np.interp(previous_speed, A_CRUISE_MAX_BP, J_CRUISE_VALS)) * DT_MDL
+        self.assertLessEqual(abs(accel - previous_accel), jerk_step + 1e-12)
+        previous_speed = speed
+        previous_accel = accel
+
+  def test_stop_release_frame_is_profile_independent(self):
+    def target_speed(frame: int) -> float:
+      return 0.0 if frame < 20 else 8.0
+
+    traces = {
+      profile: run_profile(profile, v_cruise_fn=target_speed, steps=80)
+      for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport)
+    }
+    release_frames = {
+      profile: next(frame for frame, row in enumerate(rows) if frame >= 20 and not row[2])
+      for profile, rows in traces.items()
+    }
+    stock = run_profile(AccelProfile.normal, enabled=False, v_cruise_fn=target_speed, steps=80)
+    stock_release_frame = next(frame for frame, row in enumerate(stock) if frame >= 20 and not row[2])
+    self.assertEqual(len(set(release_frames.values())), 1)
+    self.assertTrue(all(frame == stock_release_frame for frame in release_frames.values()))

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

@@ -1,17 +0,0 @@
-class WMACConstants:
-  # Lead detection parameters
-  LEAD_WINDOW_SIZE = 6  # Stable detection window
-  LEAD_PROB = 0.45  # Balanced threshold for lead detection
-
-  # Slow down detection parameters
-  SLOW_DOWN_WINDOW_SIZE = 5  # Responsive but stable
-  SLOW_DOWN_PROB = 0.3  # Balanced threshold for slow down scenarios
-
-  # Optimized slow down distance curve - smooth and progressive
-  SLOW_DOWN_BP = [0., 10., 20., 30., 40., 50., 55., 60.]
-  SLOW_DOWN_DIST = [32., 46., 64., 86., 108., 130., 145., 165.]
-
-  # Slowness detection parameters
-  SLOWNESS_WINDOW_SIZE = 10  # Stable slowness detection
-  SLOWNESS_PROB = 0.55  # Clear threshold for slowness
-  SLOWNESS_CRUISE_OFFSET = 1.025  # Conservative cruise speed offset

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

@@ -4,192 +4,120 @@ Copyright (c) 2021-, rav4kumar, sunnypilot, and a number of other contributors.
 This file is part of sunnypilot and is licensed under the MIT License.
 See the LICENSE.md file in the root directory for more details.
 """
-# Version = 2025-6-30
+from dataclasses import dataclass
+from typing import Literal
+
+import numpy as np
 
 from openpilot.cereal import messaging
 from opendbc.car import structs
-from numpy import interp
 from openpilot.common.params import Params
-from openpilot.common.realtime import DT_MDL
-from openpilot.sunnypilot.selfdrive.controls.lib.dec.constants import WMACConstants
-from typing import Literal
+from openpilot.selfdrive.modeld.constants import ModelConstants
 
-# d-e2e, from modeldata.h
-TRAJECTORY_SIZE = 33
-SET_MODE_TIMEOUT = 15
-
-# Define the valid mode types
 ModeType = Literal['acc', 'blended']
 
+_DECEL_LOOKAHEAD_MIN_T = 1.0
+_DECEL_LOOKAHEAD_MAX_T = 6.0
+_T_IDXS = np.array(ModelConstants.T_IDXS)
+_DECEL_IDX = np.where((_T_IDXS >= _DECEL_LOOKAHEAD_MIN_T) & (_T_IDXS <= _DECEL_LOOKAHEAD_MAX_T))[0]
+_DECEL_INV_T = 1.0 / _T_IDXS[_DECEL_IDX]
 
-class SmoothKalmanFilter:
-  """Enhanced Kalman filter with smoothing for stable decision making."""
+DECEL_INTENT_A_HINT = 0.35
+DECEL_INTENT_A_FULL = 1.30
+DECEL_INTENT_TRIGGER = 0.5
+DECEL_INTENT_CURVE_OVERRIDE = 0.9
 
-  def __init__(self, initial_value=0, measurement_noise=0.1, process_noise=0.01,
-               alpha=1.0, smoothing_factor=0.85):
-    self.x = initial_value
-    self.P = 1.0
-    self.R = measurement_noise
-    self.Q = process_noise
-    self.alpha = alpha
-    self.smoothing_factor = smoothing_factor
-    self.initialized = False
-    self.history = []
-    self.max_history = 10
-    self.confidence = 0.0
+CURVE_Y_MAX = 5.0
 
-  def add_data(self, measurement):
-    if len(self.history) >= self.max_history:
-      self.history.pop(0)
-    self.history.append(measurement)
+LEAD_FUTURE_PROB_VANISH = 0.35
+LEAD_VETO_CONFIRM_FRAMES = 4
 
-    if not self.initialized:
-      self.x = measurement
-      self.initialized = True
-      self.confidence = 0.1
-      return
+MODEL_DROP_TRUST_FULL = 5.0
+MODEL_DROP_TRUST_NONE = 30.0
+MODEL_TRUST_MIN = 0.5
 
-    self.P = self.alpha * self.P + self.Q
+CREEP_SPEED_ENTER = 2.0
+CREEP_SPEED_EXIT = 3.0
 
-    K = self.P / (self.P + self.R)
-    effective_K = K * (1.0 - self.smoothing_factor) + self.smoothing_factor * 0.1
+ENTER_FRAMES = 3
+EXIT_FRAMES = 16
+MIN_BLENDED_FRAMES = 20
 
-    innovation = measurement - self.x
-    self.x = self.x + effective_K * innovation
-    self.P = (1 - effective_K) * self.P
-
-    if abs(innovation) < 0.1:
-      self.confidence = min(1.0, self.confidence + 0.05)
-    else:
-      self.confidence = max(0.1, self.confidence - 0.02)
-
-  def get_value(self):
-    return self.x if self.initialized else None
-
-  def get_confidence(self):
-    return self.confidence
-
-  def reset_data(self):
-    self.initialized = False
-    self.history = []
-    self.confidence = 0.0
+PARAM_READ_FRAMES = 5
 
 
-class ModeTransitionManager:
-  """Manages smooth transitions between driving modes with hysteresis."""
+@dataclass
+class DecSignals:
+  decel_intent: float = 0.0
+  curve_detected: bool = False
+  model_trust: float = 1.0
+  creeping: bool = False
 
+
+def should_blend(s: DecSignals) -> bool:
+  degraded = s.model_trust < MODEL_TRUST_MIN
+  curve_gate = s.decel_intent >= DECEL_INTENT_CURVE_OVERRIDE or not s.curve_detected
+  slowdown_detected = not degraded and s.decel_intent >= DECEL_INTENT_TRIGGER and curve_gate
+  return slowdown_detected or s.creeping
+
+
+class ModeHysteresis:
   def __init__(self):
-    self.current_mode: ModeType = 'acc'
-    self.mode_confidence = {'acc': 1.0, 'blended': 0.0}
-    self.transition_timeout = 0
-    self.min_mode_duration = 10
-    self.mode_duration = 0
-    self.emergency_override = False
+    self.mode: ModeType = 'acc'
+    self.above = 0
+    self.below = 0
+    self.blended_frames = 0
 
-  def request_mode(self, mode: ModeType, confidence: float = 1.0, emergency: bool = False):
-    # Emergency override for critical situations (stops, collisions)
-    if emergency:
-      self.emergency_override = True
-      self.current_mode = mode
-      self.transition_timeout = SET_MODE_TIMEOUT
-      self.mode_duration = 0
-      return
+  def update(self, want_blended: bool, override: bool, veto: bool) -> ModeType:
+    self.above = self.above + 1 if want_blended else 0
+    self.below = 0 if want_blended else self.below + 1
 
-    self.mode_confidence[mode] = min(1.0, self.mode_confidence[mode] + 0.1 * confidence)
-    for m in self.mode_confidence:
-      if m != mode:
-        self.mode_confidence[m] = max(0.0, self.mode_confidence[m] - 0.05)
+    if override:
+      self.mode, self.blended_frames = 'blended', 0
+    elif veto:
+      self.mode = 'acc'
+    elif self.mode == 'acc':
+      if self.above >= ENTER_FRAMES:
+        self.mode, self.blended_frames = 'blended', 0
+    else:
+      self.blended_frames += 1
+      if self.blended_frames >= MIN_BLENDED_FRAMES and self.below >= EXIT_FRAMES:
+        self.mode = 'acc'
+    return self.mode
 
-    # Require minimum duration in current mode (unless emergency)
-    if self.mode_duration < self.min_mode_duration and not self.emergency_override:
-      return
-
-    # Hysteresis: higher threshold for mode changes
-    confidence_threshold = 0.6 if mode != self.current_mode else 0.3  # Lower threshold for faster response
-
-    if self.mode_confidence[mode] > confidence_threshold:
-      if mode != self.current_mode and self.transition_timeout == 0:
-        self.transition_timeout = SET_MODE_TIMEOUT
-        self.current_mode = mode
-        self.mode_duration = 0
-
-  def update(self):
-    if self.transition_timeout > 0:
-      self.transition_timeout -= 1
-    self.mode_duration += 1
-
-    # Reset emergency override after some time
-    if self.emergency_override and self.mode_duration > 20:
-      self.emergency_override = False
-
-    # Gradual confidence decay
-    for mode in self.mode_confidence:
-      self.mode_confidence[mode] *= 0.98
-
-  def get_mode(self) -> ModeType:
-    return self.current_mode
+  def reset(self) -> None:
+    self.mode = 'acc'
+    self.above = 0
+    self.below = 0
+    self.blended_frames = 0
 
 
 class DynamicExperimentalController:
   def __init__(self, CP: structs.CarParams, mpc, params=None):
-    self._CP = CP
     self._mpc = mpc
     self._params = params or Params()
     self._enabled: bool = self._params.get_bool("DynamicExperimentalControl")
     self._active: bool = False
     self._frame: int = 0
-    self._urgency = 0.0
 
-    self._mode_manager = ModeTransitionManager()
+    self._hysteresis = ModeHysteresis()
+    self._creeping = False
+    self._lead_veto_frames = 0
 
-    # Smooth filters for stable decision making with faster response for critical scenarios
-    self._lead_filter = SmoothKalmanFilter(
-      measurement_noise=0.15,
-      process_noise=0.05,
-      alpha=1.02,
-      smoothing_factor=0.8
-    )
+    self.signals = DecSignals()
+    self.want_blended = False
+    self.lead_veto = False
 
-    self._slow_down_filter = SmoothKalmanFilter(
-      measurement_noise=0.1,
-      process_noise=0.1,
-      alpha=1.05,
-      smoothing_factor=0.7
-    )
-
-    self._slowness_filter = SmoothKalmanFilter(
-      measurement_noise=0.1,
-      process_noise=0.06,
-      alpha=1.015,
-      smoothing_factor=0.92
-    )
-
-    self._mpc_fcw_filter = SmoothKalmanFilter(
-      measurement_noise=0.2,
-      process_noise=0.1,
-      alpha=1.1,
-      smoothing_factor=0.5
-    )
-    self._has_lead_filtered = False
-    self._has_slow_down = False
-    self._has_slowness = False
-    self._has_mpc_fcw = False
-    self._v_ego_kph = 0.0
-    self._v_cruise_kph = 0.0
-    self._has_standstill = False
-    self._mpc_fcw_crash_cnt = 0
-    self._standstill_count = 0
-    # debug
-    self._endpoint_x = float('inf')
-    self._expected_distance = 0.0
-    self._trajectory_valid = False
+  def _update_creeping(self, v_ego: float) -> bool:
+    self._creeping = v_ego < CREEP_SPEED_EXIT if self._creeping else v_ego <= CREEP_SPEED_ENTER
+    return self._creeping
 
   def _read_params(self) -> None:
-    if self._frame % int(1. / DT_MDL) == 0:
+    if self._frame % PARAM_READ_FRAMES == 0:
       self._enabled = self._params.get_bool("DynamicExperimentalControl")
 
   def mode(self) -> str:
-    return self._mode_manager.get_mode()
+    return self._hysteresis.mode
 
   def enabled(self) -> bool:
     return self._enabled
@@ -197,192 +125,77 @@ class DynamicExperimentalController:
   def active(self) -> bool:
     return self._active
 
-  def set_mpc_fcw_crash_cnt(self) -> None:
-    """Set MPC FCW crash count"""
-    self._mpc_fcw_crash_cnt = self._mpc.crash_cnt
+  @staticmethod
+  def _decel_intent(md) -> float:
+    v = np.asarray(md.velocity.x)
+    if len(v) != len(_T_IDXS):
+      return 0.0
+    a_req = float(np.min((v[_DECEL_IDX] - v[0]) * _DECEL_INV_T))
+    return float(np.interp(-a_req, [DECEL_INTENT_A_HINT, DECEL_INTENT_A_FULL], [0.0, 1.0]))
 
-  def _update_calculations(self, sm: messaging.SubMaster) -> None:
-    car_state = sm['carState']
-    lead_one = sm['radarState'].leadOne
-    md = sm['modelV2']
+  @staticmethod
+  def _curve_detected(md) -> bool:
+    y = md.position.y
+    if len(y) < 1:
+      return False
+    return abs(y[-1]) >= CURVE_Y_MAX
 
-    self._v_ego_kph = car_state.vEgo * 3.6
-    self._v_cruise_kph = car_state.vCruise
-    self._has_standstill = car_state.standstill
+  @staticmethod
+  def _model_trust(md) -> float:
+    if len(md.velocity.x) != len(_T_IDXS):
+      return 0.0
+    return float(np.interp(md.frameDropPerc, [MODEL_DROP_TRUST_FULL, MODEL_DROP_TRUST_NONE], [1.0, 0.0]))
 
-    # standstill detection
-    if self._has_standstill:
-      self._standstill_count = min(20, self._standstill_count + 1)
-    else:
-      self._standstill_count = max(0, self._standstill_count - 1)
+  @staticmethod
+  def _lead_veto(radar_state, md) -> bool:
+    lead_one, lead_two = radar_state.leadOne, radar_state.leadTwo
+    lead_now = lead_one.present or lead_two.present
+    probs = md.leadsV3
+    future = min(probs[1].prob, probs[2].prob) if len(probs) >= 3 else 1.0
+    return bool(lead_now and future > LEAD_FUTURE_PROB_VANISH)
 
-    # Lead detection
-    self._lead_filter.add_data(float(lead_one.present))
-    lead_value = self._lead_filter.get_value() or 0.0
-    self._has_lead_filtered = lead_value > WMACConstants.LEAD_PROB
+  def _update_lead_veto(self, raw_veto: bool, lead_present: bool, urgent_override: bool) -> bool:
+    if raw_veto:
+      self._lead_veto_frames = min(self._lead_veto_frames + 1, LEAD_VETO_CONFIRM_FRAMES)
+      return self.lead_veto or self._lead_veto_frames >= LEAD_VETO_CONFIRM_FRAMES
 
-    # MPC FCW detection
-    fcw_filtered_value = self._mpc_fcw_filter.get_value() or 0.0
-    self._mpc_fcw_filter.add_data(float(self._mpc_fcw_crash_cnt > 0))
-    self._has_mpc_fcw = fcw_filtered_value > 0.5
+    if not lead_present or urgent_override or not self.lead_veto:
+      self._lead_veto_frames = 0
+      return False
 
-    # Slow down detection
-    self._calculate_slow_down(md)
-
-    # Slowness detection
-    if not (self._standstill_count > 5) and not self._has_slow_down:
-      current_slowness = float(self._v_ego_kph <= (self._v_cruise_kph * WMACConstants.SLOWNESS_CRUISE_OFFSET))
-      self._slowness_filter.add_data(current_slowness)
-      slowness_value = self._slowness_filter.get_value() or 0.0
-
-      # Hysteresis for slowness
-      threshold = WMACConstants.SLOWNESS_PROB * (0.8 if self._has_slowness else 1.1)
-      self._has_slowness = slowness_value > threshold
-
-  def _calculate_slow_down(self, md):
-    """Calculate urgency based on trajectory endpoint vs expected distance."""
-
-    # Reset to safe defaults
-    urgency = 0.0
-    self._endpoint_x = float('inf')
-    self._trajectory_valid = False
-
-    #Require exact trajectory size
-    position_valid = len(md.position.x) == TRAJECTORY_SIZE
-    orientation_valid = len(md.orientation.x) == TRAJECTORY_SIZE
-
-    if not (position_valid and orientation_valid):
-      # Invalid trajectory - this itself might indicate a stop scenario
-      # Apply moderate urgency for incomplete trajectories at speed
-      if self._v_ego_kph > 20.0:
-        urgency = 0.3
-
-      self._slow_down_filter.add_data(urgency)
-      urgency_filtered = self._slow_down_filter.get_value() or 0.0
-      self._has_slow_down = urgency_filtered > WMACConstants.SLOW_DOWN_PROB
-      self._urgency = urgency_filtered
-      return
-
-    # We have a valid full trajectory
-    self._trajectory_valid = True
-
-    # Use the exact endpoint (33rd point, index 32)
-    endpoint_x = md.position.x[TRAJECTORY_SIZE - 1]
-    self._endpoint_x = endpoint_x
-
-    # Get expected distance based on current speed using tuned constants
-    expected_distance = interp(self._v_ego_kph,
-                               WMACConstants.SLOW_DOWN_BP,
-                               WMACConstants.SLOW_DOWN_DIST)
-    self._expected_distance = expected_distance
-
-    # Calculate urgency based on trajectory shortage
-    if endpoint_x < expected_distance:
-      shortage = expected_distance - endpoint_x
-      shortage_ratio = shortage / expected_distance
-
-      # Base urgency on shortage ratio
-      urgency = min(1.0, shortage_ratio * 2.0)
-
-      # Increase urgency for very short trajectories (imminent stops)
-      critical_distance = expected_distance * 0.3
-      if endpoint_x < critical_distance:
-        urgency = min(1.0, urgency * 2.0)
-
-      # Speed-based urgency adjustment
-      if self._v_ego_kph > 25.0:
-        speed_factor = 1.0 + (self._v_ego_kph - 25.0) / 80.0
-        urgency = min(1.0, urgency * speed_factor)
-
-    # Apply filtering but with less smoothing for stops
-    self._slow_down_filter.add_data(urgency)
-    urgency_filtered = self._slow_down_filter.get_value() or 0.0
-
-    # Update state with lower threshold for better stop detection
-    self._has_slow_down = urgency_filtered > (WMACConstants.SLOW_DOWN_PROB * 0.8)
-    self._urgency = urgency_filtered
-
-  def _radarless_mode(self) -> None:
-    """Radarless mode decision logic with emergency handling."""
-
-    # EMERGENCY: MPC FCW - immediate blended mode
-    if self._has_mpc_fcw:
-      self._mode_manager.request_mode('blended', confidence=1.0, emergency=True)
-      return
-
-    # Standstill: use blended
-    if self._standstill_count > 3:
-      self._mode_manager.request_mode('blended', confidence=0.9)
-      return
-
-    # Slow down scenarios: emergency for high urgency, normal for lower urgency
-    if self._has_slow_down:
-      if self._urgency > 0.7:
-        # Emergency: immediate blended mode for high urgency stops
-        self._mode_manager.request_mode('blended', confidence=1.0, emergency=True)
-      else:
-        # Normal: blended with urgency-based confidence
-        confidence = min(1.0, self._urgency * 1.5)
-        self._mode_manager.request_mode('blended', confidence=confidence)
-      return
-
-    # Driving slow: use ACC (but not if actively slowing down)
-    if self._has_slowness and not self._has_slow_down:
-      self._mode_manager.request_mode('acc', confidence=0.8)
-      return
-
-    # Default: ACC
-    self._mode_manager.request_mode('acc', confidence=0.7)
-
-  def _radar_mode(self) -> None:
-    """Radar mode with emergency handling."""
-
-    # EMERGENCY: MPC FCW - immediate blended mode
-    if self._has_mpc_fcw:
-      self._mode_manager.request_mode('blended', confidence=1.0, emergency=True)
-      return
-
-    # If lead detected and not in standstill: always use ACC
-    if self._has_lead_filtered and not (self._standstill_count > 3):
-      self._mode_manager.request_mode('acc', confidence=1.0)
-      return
-
-    # Slow down scenarios: emergency for high urgency, normal for lower urgency
-    if self._has_slow_down:
-      if self._urgency > 0.7:
-        # Emergency: immediate blended mode for high urgency stops
-        self._mode_manager.request_mode('blended', confidence=1.0, emergency=True)
-      else:
-        # Normal: blended with urgency-based confidence
-        confidence = min(1.0, self._urgency * 1.3)
-        self._mode_manager.request_mode('blended', confidence=confidence)
-      return
-
-    # Standstill: use blended
-    if self._standstill_count > 3:
-      self._mode_manager.request_mode('blended', confidence=0.9)
-      return
-
-    # Driving slow: use ACC (but not if actively slowing down)
-    if self._has_slowness and not self._has_slow_down:
-      self._mode_manager.request_mode('acc', confidence=0.8)
-      return
-
-    # Default: ACC
-    self._mode_manager.request_mode('acc', confidence=0.7)
+    self._lead_veto_frames = max(self._lead_veto_frames - 1, 0)
+    return self._lead_veto_frames > 0
 
   def update(self, sm: messaging.SubMaster) -> None:
     self._read_params()
 
-    self.set_mpc_fcw_crash_cnt()
+    car_state = sm['carState']
+    md = sm['modelV2']
+    radar_state = sm['radarState']
 
-    self._update_calculations(sm)
+    is_creeping = self._update_creeping(car_state.vEgo)
+    lead_present = radar_state.leadOne.present or radar_state.leadTwo.present
+    self.signals = DecSignals(
+      decel_intent=self._decel_intent(md),
+      curve_detected=self._curve_detected(md),
+      model_trust=self._model_trust(md),
+      creeping=is_creeping and not lead_present,
+    )
+    self.want_blended = should_blend(self.signals)
 
-    if self._CP.radarUnavailable:
-      self._radarless_mode()
+    crash_override = self._mpc.crash_cnt >= 1
+    hard_brake_override = bool(md.meta.hardBrakePredicted)
+    strong_stop = self.signals.model_trust >= MODEL_TRUST_MIN and self.signals.decel_intent >= DECEL_INTENT_CURVE_OVERRIDE
+    raw_lead_veto = self._lead_veto(radar_state, md)
+    urgent_release = (crash_override or hard_brake_override or strong_stop) and not raw_lead_veto
+    self.lead_veto = self._update_lead_veto(raw_lead_veto, lead_present, urgent_release)
+
+    override = (crash_override or hard_brake_override) and not self.lead_veto
+
+    if self._enabled:
+      self._hysteresis.update(self.want_blended, override, self.lead_veto)
     else:
-      self._radar_mode()
+      self._hysteresis.reset()
 
-    self._mode_manager.update()
     self._active = sm['selfdriveState'].experimentalMode and self._enabled
     self._frame += 1

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

@@ -1,91 +1,465 @@
+import numpy as np
+
+from openpilot.cereal import messaging
+from opendbc.car import structs
 from openpilot.common.test import OpenpilotTestCase
-from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
+from openpilot.selfdrive.modeld.constants import ModelConstants
+from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import (
+  DecSignals,
+  DynamicExperimentalController,
+  ModeHysteresis,
+  should_blend,
+  ENTER_FRAMES,
+  LEAD_VETO_CONFIRM_FRAMES,
+  MIN_BLENDED_FRAMES,
+)
 
-class MockLeadOne:
-  def __init__(self, present=0.0):
-    self.present = present
+T_IDXS = np.array(ModelConstants.T_IDXS)
 
-class MockRadarState:
-  def __init__(self, present=0.0):
-    self.leadOne = MockLeadOne(present=present)
-
-class MockCarState:
-  def __init__(self, vEgo=0.0, vCruise=0.0, standstill=False):
-    self.vEgo = vEgo
-    self.vCruise = vCruise
-    self.standstill = standstill
-
-class MockModelData:
-  def __init__(self, valid=True):
-    size = 33 if valid else 10  # incomplete if invalid
-    self.position = type("Pos", (), {"x": [0.0] * size})()
-    self.orientation = type("Ori", (), {"x": [0.0] * size})()
-
-class MockSelfDriveState:
-  def __init__(self, experimentalMode=False):
-    self.experimentalMode = experimentalMode
 
 class MockParams:
+  def __init__(self, enabled=True):
+    self._enabled = enabled
+
   def get_bool(self, name):
-    return True
+    return self._enabled
 
-def default_sm():
-  sm = {
-    'carState': MockCarState(vEgo=10.0, vCruise=20.0),
-    'radarState': MockRadarState(present=1.0),
-    'modelV2': MockModelData(valid=True),
-    'selfdriveState': MockSelfDriveState(experimentalMode=True),
+
+class MockMpc:
+  def __init__(self, crash_cnt=0):
+    self.crash_cnt = crash_cnt
+
+
+def flat_velocity(v):
+  return [float(v)] * len(T_IDXS)
+
+
+def decel_velocity(v0, a):
+  return [float(max(0.0, v0 + a * t)) for t in T_IDXS]
+
+
+def make_car_state(v_ego=10.0, v_cruise=20.0):
+  msg = messaging.new_message('carState')
+  msg.carState.vEgo = v_ego
+  msg.carState.vCruise = v_cruise
+  return msg.carState.as_reader()
+
+
+def make_selfdrive_state(experimental_mode=True):
+  msg = messaging.new_message('selfdriveState')
+  msg.selfdriveState.experimentalMode = experimental_mode
+  return msg.selfdriveState.as_reader()
+
+
+def make_radar_state(lead_present=False, lead_radar=False, lead_two_present=False):
+  msg = messaging.new_message('radarState')
+  msg.radarState.leadOne.present = lead_present
+  msg.radarState.leadOne.radar = lead_radar
+  msg.radarState.leadTwo.present = lead_two_present
+  return msg.radarState.as_reader()
+
+
+def make_model_v2(velocity=None, position_y=None, hard_brake=False, lead_probs=None, frame_drop_perc=0.0):
+  msg = messaging.new_message('modelV2')
+  msg.modelV2.velocity.x = velocity if velocity is not None else flat_velocity(0.0)
+  msg.modelV2.position.y = position_y if position_y is not None else [0.0] * len(T_IDXS)
+  msg.modelV2.frameDropPerc = frame_drop_perc
+  msg.modelV2.meta.hardBrakePredicted = hard_brake
+  if lead_probs is not None:
+    msg.modelV2.init('leadsV3', 3)
+    for i, (prob, prob_time) in enumerate(zip(lead_probs, (0.0, 2.0, 4.0), strict=True)):
+      msg.modelV2.leadsV3[i].prob = prob
+      msg.modelV2.leadsV3[i].probTime = prob_time
+  return msg.modelV2.as_reader()
+
+
+def make_sm(v_ego=10.0, v_cruise=20.0, velocity=None, position_y=None, hard_brake=False,
+            lead_present=False, lead_radar=False, lead_two_present=False, lead_probs=None,
+            frame_drop_perc=0.0, experimental_mode=True):
+  return {
+    'carState': make_car_state(v_ego, v_cruise),
+    'radarState': make_radar_state(lead_present, lead_radar, lead_two_present),
+    'modelV2': make_model_v2(velocity, position_y, hard_brake, lead_probs, frame_drop_perc),
+    'selfdriveState': make_selfdrive_state(experimental_mode),
   }
-  return sm
 
-def mock_cp():
-  class CP:
-    radarUnavailable = False
-  return CP()
 
-def mock_mpc():
-  class MPC:
-    crash_cnt = 0
-  return MPC()
+def make_controller(cp=None, mpc=None, enabled=True):
+  return DynamicExperimentalController(cp or structs.CarParams(), mpc or MockMpc(), params=MockParams(enabled))
 
-# Fake Kalman Filter that always returns a given value
-class FakeKalman:
-  def __init__(self, value=1.0):
-    self.value = value
-  def add_data(self, v): pass
-  def get_value(self): return self.value
-  def get_confidence(self): return 1.0
-  def reset_data(self): pass
 
 class TestDynamicExperimentalController(OpenpilotTestCase):
-  def test_initial_mode_is_acc(self, mock_cp, mock_mpc):
-    controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  def test_initial_mode_is_acc(self):
+    controller = make_controller()
     assert controller.mode() == "acc"
 
-  def test_standstill_triggers_blended(self, mock_cp, mock_mpc, default_sm):
-    controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-    default_sm['carState'].standstill = True
+  def test_flat_plan_never_blends_at_any_speed(self):
+    for v_ego in (2.5, 5.6, 8.3, 13.9, 22.2, 30.6):
+      controller = make_controller()
+      sm = make_sm(v_ego=v_ego, velocity=flat_velocity(v_ego))
+      for _ in range(100):
+        controller.update(sm)
+      assert controller.mode() == "acc", f"false blend on a flat plan at v_ego={v_ego}"
+
+  def test_highway_slowdown_without_lead_blends(self):
+    v0 = 110 / 3.6
+    a = (70 / 3.6 - v0) / 6.0
+    controller = make_controller()
+    sm = make_sm(v_ego=v0, velocity=decel_velocity(v0, a))
     for _ in range(10):
-      controller.update(default_sm)
+      controller.update(sm)
     assert controller.mode() == "blended"
 
-  def test_emergency_blended_on_fcw(self, mock_cp, mock_mpc, default_sm):
-    controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-    mock_mpc.crash_cnt = 1  # simulate FCW
-    for _ in range(2):
-      controller.update(default_sm)
+  def test_curve_exclusion_prevents_false_blend(self):
+    controller = make_controller()
+    sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -1.0), position_y=[6.0] * len(T_IDXS))
+    for _ in range(30):
+      controller.update(sm)
+    assert controller.mode() == "acc"
+
+  def test_curve_does_not_override_saturated_decel_intent(self):
+    controller = make_controller()
+    sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0), position_y=[6.0] * len(T_IDXS))
+    for _ in range(10):
+      controller.update(sm)
     assert controller.mode() == "blended"
 
-  def test_radarless_slowdown_triggers_blended(self, mock_cp, mock_mpc, default_sm):
-    mock_cp.radarUnavailable = True
-    controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-
-    # Force conditions to simulate slowdown
-    controller._slow_down_filter = FakeKalman(value=1.0)  # ty: ignore[invalid-assignment]
-    controller._v_ego_kph = 35.0
-    default_sm['modelV2'] = MockModelData(valid=False)  # Incomplete trajectory
-
-    for _ in range(3):
-      controller.update(default_sm)
+  def test_persistent_lead_forces_acc_even_with_strong_model_signal(self):
+    for lead_radar in (True, False):
+      controller = make_controller()
+      sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0),
+                   lead_present=True, lead_radar=lead_radar, lead_probs=[1.0, 1.0, 1.0])
+      for _ in range(LEAD_VETO_CONFIRM_FRAMES - 1):
+        controller.update(sm)
+        assert not controller.lead_veto
+      for _ in range(60):
+        controller.update(sm)
+        assert controller.lead_veto
+        assert controller.mode() == "acc"
 
+  def test_single_frame_lead_veto_pulse_does_not_leave_blended(self):
+    controller = make_controller()
+    no_lead_sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0))
+    lead_sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0),
+                      lead_present=True, lead_probs=[1.0, 1.0, 1.0])
+    for _ in range(ENTER_FRAMES):
+      controller.update(no_lead_sm)
     assert controller.mode() == "blended"
+
+    controller.update(lead_sm)
+    assert controller.mode() == "blended"
+    assert not controller.lead_veto
+
+    controller.update(no_lead_sm)
+    assert controller.mode() == "blended"
+    assert not controller.lead_veto
+
+  def test_three_frame_lead_veto_pulse_does_not_leave_blended(self):
+    controller = make_controller()
+    no_lead_sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0))
+    lead_sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0),
+                      lead_present=True, lead_probs=[1.0, 1.0, 1.0])
+    for _ in range(ENTER_FRAMES):
+      controller.update(no_lead_sm)
+    assert controller.mode() == "blended"
+
+    for _ in range(LEAD_VETO_CONFIRM_FRAMES - 1):
+      controller.update(lead_sm)
+      assert controller.mode() == "blended"
+      assert not controller.lead_veto
+
+    controller.update(no_lead_sm)
+    assert controller.mode() == "blended"
+    assert not controller.lead_veto
+
+  def test_persistent_lead_veto_forces_acc_after_confirmation(self):
+    controller = make_controller()
+    no_lead_sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0))
+    lead_sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0),
+                      lead_present=True, lead_probs=[1.0, 1.0, 1.0])
+    for _ in range(ENTER_FRAMES):
+      controller.update(no_lead_sm)
+    assert controller.mode() == "blended"
+
+    for _ in range(LEAD_VETO_CONFIRM_FRAMES - 1):
+      controller.update(lead_sm)
+      assert controller.mode() == "blended"
+      assert not controller.lead_veto
+
+    controller.update(lead_sm)
+    assert controller.mode() == "acc"
+    assert controller.lead_veto
+
+    controller.update(no_lead_sm)
+    assert controller.mode() == "blended"
+    assert not controller.lead_veto
+
+  def test_veto_releases_without_rebuild_lag(self):
+    controller = make_controller()
+    lead_sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0),
+                       lead_present=True, lead_probs=[1.0, 1.0, 1.0])
+    for _ in range(30):
+      controller.update(lead_sm)
+    assert controller.mode() == "acc"
+    assert controller.lead_veto
+
+    no_lead_sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0), lead_present=False)
+    for _ in range(ENTER_FRAMES + 2):
+      controller.update(no_lead_sm)
+      if controller.mode() == "blended":
+        break
+    assert controller.mode() == "blended"
+
+  def test_lead_gone_with_no_underlying_slowdown_stays_acc(self):
+    controller = make_controller()
+    lead_sm = make_sm(v_ego=20.0, velocity=flat_velocity(20.0), lead_present=True, lead_probs=[1.0, 1.0, 1.0])
+    for _ in range(30):
+      controller.update(lead_sm)
+    assert controller.mode() == "acc"
+
+    no_lead_sm = make_sm(v_ego=20.0, velocity=flat_velocity(20.0), lead_present=False)
+    for _ in range(20):
+      controller.update(no_lead_sm)
+    assert controller.mode() == "acc"
+
+  def test_creep_does_not_release_lead_veto(self):
+    controller = make_controller()
+    sm = make_sm(v_ego=1.0, velocity=flat_velocity(1.0), lead_present=True, lead_probs=[1.0, 1.0, 1.0])
+    for _ in range(10):
+      controller.update(sm)
+    assert controller.mode() == "acc"
+    assert controller.lead_veto
+
+  def test_lead_prevents_creep_only_blending_when_model_probability_drops(self):
+    controller = make_controller()
+    confirmed_sm = make_sm(v_ego=1.0, velocity=flat_velocity(1.0), lead_present=True, lead_probs=[1.0, 1.0, 1.0])
+    for _ in range(LEAD_VETO_CONFIRM_FRAMES):
+      controller.update(confirmed_sm)
+    assert controller.lead_veto
+
+    low_probability_sm = make_sm(v_ego=1.0, velocity=flat_velocity(1.0), lead_present=True, lead_probs=[1.0, 0.1, 0.1])
+    for _ in range(LEAD_VETO_CONFIRM_FRAMES - 1):
+      controller.update(low_probability_sm)
+      assert controller.lead_veto
+      assert controller.mode() == "acc"
+
+    for _ in range(20):
+      controller.update(low_probability_sm)
+      assert not controller.lead_veto
+      assert not controller.signals.creeping
+      assert controller.mode() == "acc"
+
+    no_lead_sm = make_sm(v_ego=1.0, velocity=flat_velocity(1.0), lead_present=False, lead_probs=[1.0, 0.1, 0.1])
+    for _ in range(ENTER_FRAMES):
+      controller.update(no_lead_sm)
+    assert controller.mode() == "blended"
+
+  def test_confirmed_lead_veto_ignores_short_future_probability_dropout(self):
+    controller = make_controller()
+    confirmed_sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -1.0),
+                           lead_present=True, lead_probs=[1.0, 1.0, 1.0])
+    for _ in range(LEAD_VETO_CONFIRM_FRAMES):
+      controller.update(confirmed_sm)
+    assert controller.lead_veto
+
+    dropout_sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -1.0),
+                         lead_present=True, lead_probs=[1.0, 0.1, 0.1])
+    for _ in range(LEAD_VETO_CONFIRM_FRAMES - 1):
+      controller.update(dropout_sm)
+      assert controller.lead_veto
+      assert controller.mode() == "acc"
+
+    controller.update(confirmed_sm)
+    assert controller.lead_veto
+    assert controller.mode() == "acc"
+
+  def test_urgent_override_bypasses_confirmed_veto_release(self):
+    controller = make_controller()
+    confirmed_sm = make_sm(v_ego=20.0, velocity=flat_velocity(20.0),
+                           lead_present=True, lead_probs=[1.0, 1.0, 1.0])
+    for _ in range(LEAD_VETO_CONFIRM_FRAMES):
+      controller.update(confirmed_sm)
+    assert controller.lead_veto
+
+    hard_brake_sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0), hard_brake=True,
+                            lead_present=True, lead_probs=[1.0, 0.1, 0.1])
+    controller.update(hard_brake_sm)
+    assert not controller.lead_veto
+    assert controller.mode() == "blended"
+
+  def test_trusted_strong_stop_bypasses_confirmed_veto_release(self):
+    controller = make_controller()
+    confirmed_sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0),
+                           lead_present=True, lead_probs=[1.0, 1.0, 1.0])
+    for _ in range(LEAD_VETO_CONFIRM_FRAMES):
+      controller.update(confirmed_sm)
+    assert controller.lead_veto
+    assert controller.mode() == "acc"
+
+    departing_lead_sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0),
+                                lead_present=True, lead_probs=[1.0, 0.1, 0.1])
+    controller.update(departing_lead_sm)
+    assert not controller.lead_veto
+    assert controller.mode() == "blended"
+
+  def test_degraded_strong_stop_does_not_bypass_confirmed_veto_release(self):
+    controller = make_controller()
+    confirmed_sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0),
+                           lead_present=True, lead_probs=[1.0, 1.0, 1.0])
+    for _ in range(LEAD_VETO_CONFIRM_FRAMES):
+      controller.update(confirmed_sm)
+    assert controller.lead_veto
+
+    degraded_sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0), lead_present=True,
+                          lead_probs=[1.0, 0.1, 0.1], frame_drop_perc=60.0)
+    for _ in range(LEAD_VETO_CONFIRM_FRAMES - 1):
+      controller.update(degraded_sm)
+      assert controller.lead_veto
+      assert controller.mode() == "acc"
+
+  def test_model_slowdown_still_blends_while_creeping_with_a_lead(self):
+    controller = make_controller()
+    sm = make_sm(v_ego=1.0, velocity=decel_velocity(1.0, -1.0), lead_present=True, lead_probs=[1.0, 0.1, 0.1])
+    for _ in range(ENTER_FRAMES):
+      controller.update(sm)
+    assert not controller.lead_veto
+    assert controller.mode() == "blended"
+
+  def test_hard_brake_still_blends_while_creeping_with_a_lead(self):
+    controller = make_controller()
+    sm = make_sm(v_ego=1.0, velocity=flat_velocity(1.0), hard_brake=True,
+                 lead_present=True, lead_probs=[1.0, 0.1, 0.1])
+    controller.update(sm)
+    assert not controller.lead_veto
+    assert controller.mode() == "blended"
+
+  def test_creep_hysteresis_band_without_lead(self):
+    controller = make_controller()
+    controller.update(make_sm(v_ego=1.5, velocity=flat_velocity(1.5)))
+    assert controller.signals.creeping
+
+    controller.update(make_sm(v_ego=2.5, velocity=flat_velocity(2.5)))
+    assert controller.signals.creeping, "a small excursion above CREEP_SPEED_ENTER should not exit creeping"
+
+    controller.update(make_sm(v_ego=5.0, velocity=flat_velocity(5.0)))
+    assert not controller.signals.creeping, "should exit creeping once genuinely above CREEP_SPEED_EXIT"
+
+  def test_crash_cnt_override_inert_while_lead_present(self):
+    mpc = MockMpc(crash_cnt=0)
+    controller = make_controller(mpc=mpc)
+    sm = make_sm(v_ego=20.0, velocity=flat_velocity(20.0), lead_present=True, lead_probs=[1.0, 1.0, 1.0])
+    for _ in range(30):
+      controller.update(sm)
+    assert controller.mode() == "acc"
+
+    mpc.crash_cnt = 1
+    controller.update(sm)
+    assert controller.mode() == "acc"
+
+  def test_crash_cnt_blends_within_one_frame_without_lead(self):
+    mpc = MockMpc(crash_cnt=1)
+    controller = make_controller(mpc=mpc)
+    sm = make_sm(v_ego=20.0, velocity=flat_velocity(20.0), lead_present=False)
+    controller.update(sm)
+    assert controller.mode() == "blended"
+
+  def test_hard_brake_predicted_blends_within_one_frame_without_lead(self):
+    controller = make_controller()
+    sm = make_sm(v_ego=20.0, velocity=flat_velocity(20.0), hard_brake=True, lead_present=False)
+    controller.update(sm)
+    assert controller.mode() == "blended"
+
+  def test_confirmed_lead_veto_suppresses_hard_brake_override(self):
+    controller = make_controller()
+    sm = make_sm(v_ego=20.0, velocity=flat_velocity(20.0), hard_brake=True,
+                 lead_present=True, lead_probs=[1.0, 1.0, 1.0])
+    for _ in range(LEAD_VETO_CONFIRM_FRAMES - 1):
+      controller.update(sm)
+      assert controller.mode() == "blended"
+      assert not controller.lead_veto
+    controller.update(sm)
+    assert controller.mode() == "acc"
+    assert controller.lead_veto
+
+  def test_degraded_model_does_not_blend(self):
+    controller = make_controller()
+    sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -3.0), frame_drop_perc=60.0)
+    for _ in range(30):
+      controller.update(sm)
+    assert controller.mode() == "acc"
+
+  def test_short_plan_arrays_do_not_blend(self):
+    controller = make_controller()
+    sm = make_sm(v_ego=20.0, velocity=[20.0] * 5)
+    for _ in range(30):
+      controller.update(sm)
+    assert controller.mode() == "acc"
+
+  def test_disabled_param_holds_acc(self):
+    controller = make_controller(enabled=False)
+    sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -3.0))
+    for _ in range(30):
+      controller.update(sm)
+    assert controller.mode() == "acc"
+
+
+class TestModeHysteresis(OpenpilotTestCase):
+  def test_entry_requires_enter_frames(self):
+    h = ModeHysteresis()
+    for _ in range(ENTER_FRAMES - 1):
+      assert h.update(want_blended=True, override=False, veto=False) == "acc"
+    assert h.update(want_blended=True, override=False, veto=False) == "blended"
+
+  def test_override_beats_veto(self):
+    h = ModeHysteresis()
+    assert h.update(want_blended=False, override=True, veto=True) == "blended"
+
+  def test_veto_forces_acc_even_when_reason_active(self):
+    h = ModeHysteresis()
+    for _ in range(ENTER_FRAMES + 5):
+      assert h.update(want_blended=True, override=False, veto=True) == "acc"
+
+  def test_counter_accumulates_under_veto_then_releases_instantly(self):
+    h = ModeHysteresis()
+    for _ in range(ENTER_FRAMES + 5):
+      h.update(want_blended=True, override=False, veto=True)
+    assert h.mode == "acc"
+    assert h.update(want_blended=True, override=False, veto=False) == "blended"
+
+  def test_exit_requires_min_dwell_and_sustained_absence(self):
+    h = ModeHysteresis()
+    for _ in range(ENTER_FRAMES):
+      h.update(want_blended=True, override=False, veto=False)
+    assert h.mode == "blended"
+    for _ in range(MIN_BLENDED_FRAMES - 1):
+      assert h.update(want_blended=False, override=False, veto=False) == "blended"
+    assert h.update(want_blended=False, override=False, veto=False) == "acc"
+
+  def test_no_flapping_on_alternating_reason(self):
+    h = ModeHysteresis()
+    changes = 0
+    prev = h.mode
+    for i in range(200):
+      mode = h.update(want_blended=i % 2 == 0, override=False, veto=False)
+      changes += mode != prev
+      prev = mode
+    assert changes == 0
+
+
+class TestShouldBlend(OpenpilotTestCase):
+  def test_slowdown_detected_triggers(self):
+    assert should_blend(DecSignals(decel_intent=1.0))
+    assert not should_blend(DecSignals(decel_intent=0.0))
+
+  def test_curve_exclusion_suppresses_slowdown(self):
+    assert not should_blend(DecSignals(decel_intent=0.7, curve_detected=True))
+
+  def test_curve_exclusion_does_not_override_saturated_decel_intent(self):
+    assert should_blend(DecSignals(decel_intent=1.0, curve_detected=True))
+
+  def test_degraded_model_suppresses_model_based_reasons(self):
+    s = DecSignals(decel_intent=1.0, model_trust=0.0)
+    assert not should_blend(s)
+
+  def test_creep_bypasses_everything(self):
+    assert should_blend(DecSignals(model_trust=0.0, creeping=True))

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

@@ -5,10 +5,13 @@ This file is part of sunnypilot and is licensed under the MIT License.
 See the LICENSE.md file in the root directory for more details.
 """
 
-from openpilot.cereal import messaging, custom
+import math
+
+from openpilot.cereal import messaging, custom, log
 from opendbc.car import structs
 from openpilot.common.constants import CV
 from openpilot.selfdrive.car.cruise import V_CRUISE_MAX
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController
 from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
 from openpilot.sunnypilot.selfdrive.controls.lib.e2e_alerts_helper import E2EAlertsHelper
 from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.smart_cruise_control import SmartCruiseControl
@@ -19,12 +22,16 @@ from openpilot.sunnypilot.models.helpers import get_active_bundle
 
 DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState
 LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource
+MpcPlanSource = log.LongitudinalPlan.LongitudinalPlanSource
+
+E2E_BRAKE_HOLD_ACCEL = -0.2  # m/s^2
 
 
 class LongitudinalPlannerSP:
   def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP, mpc):
+    self.accel_controller = AccelController()
+    self.accel_controller_active = False
     self.events_sp = EventsSP()
-    self.resolver = SpeedLimitResolver()
     self.dec = DynamicExperimentalController(CP, mpc)
     self.scc = SmartCruiseControl()
     self.resolver = SpeedLimitResolver()
@@ -38,10 +45,46 @@ class LongitudinalPlannerSP:
 
   def is_e2e(self, sm: messaging.SubMaster) -> bool:
     experimental_mode = sm['selfdriveState'].experimentalMode
-    if not self.dec.active():
-      return experimental_mode
+    if not experimental_mode:
+      return False
 
-    return experimental_mode and self.dec.mode() == "blended"
+    if not self.dec.active() or self.dec.mode() == "blended":
+      return True
+
+    # hold a brake the model already owns rather than release it mid-brake; min() means this can only ever
+    # add deceleration, and never one that was not already the selected source
+    if self.mpc.source == MpcPlanSource.e2e and sm['modelV2'].action.desiredAcceleration < E2E_BRAKE_HOLD_ACCEL:
+      return True
+
+    return False
+
+  def get_max_accel_override(self, v_ego: float) -> float | None:
+    if not self.accel_controller.is_enabled():
+      return None
+
+    return self.accel_controller.get_max_accel(v_ego)
+
+  def get_cruise_target_override(self, v_ego: float, v_target: float, force_decel: bool) -> float:
+    if not self.accel_controller.is_enabled() or force_decel or self.source != LongitudinalPlanSource.cruise:
+      return v_target
+
+    return self.accel_controller.get_cruise_target(v_ego, v_target)
+
+  def _has_valid_selected_lead(self, sm: messaging.SubMaster, source: MpcPlanSource) -> bool:
+    radar_valid = sm.valid.get('radarState', False) and getattr(sm, 'alive', {}).get('radarState', False)
+    return radar_valid and ((source == MpcPlanSource.lead0 and sm['radarState'].leadOne.present) or
+                            (source == MpcPlanSource.lead1 and sm['radarState'].leadTwo.present))
+
+  def arbitrate_cruise_candidate(self, sm: messaging.SubMaster, gated: float, ungated: float,
+                                 mpc_accel: float, mpc_source: MpcPlanSource, *, allow_throttle: bool,
+                                 e2e: bool, force_decel: bool) -> float:
+    finite = all(math.isfinite(value) for value in (gated, ungated, mpc_accel))
+    coast_gate_changed_source = gated < mpc_accel <= ungated
+    if (finite and not allow_throttle and not e2e and not force_decel
+        and self._has_valid_selected_lead(sm, mpc_source) and coast_gate_changed_source):
+      return ungated
+
+    return gated
 
   def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]:
     CS = sm['carState']
@@ -74,10 +117,13 @@ class LongitudinalPlannerSP:
     return self.output_v_target, self.output_a_target
 
   def update(self, sm: messaging.SubMaster) -> None:
+    self.accel_controller.update()
     self.events_sp.clear()
-    self.dec.update(sm)
     self.e2e_alerts_helper.update(sm, self.events_sp)
 
+  def update_dec(self, sm: messaging.SubMaster) -> None:
+    self.dec.update(sm)
+
   def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
     plan_sp_send = messaging.new_message('longitudinalPlanSP')
 
@@ -94,6 +140,15 @@ class LongitudinalPlannerSP:
     dec.state = DecState.blended if self.dec.mode() == 'blended' else DecState.acc
     dec.enabled = self.dec.enabled()
     dec.active = self.dec.active()
+    dec.decelIntent = float(self.dec.signals.decel_intent)
+    dec.curveDetected = bool(self.dec.signals.curve_detected)
+    dec.wantBlended = bool(self.dec.want_blended)
+    dec.leadVeto = bool(self.dec.lead_veto)
+
+    accel_controller = longitudinalPlanSP.accelController
+    accel_controller.enabled = bool(self.accel_controller.is_enabled())
+    accel_controller.active = bool(self.accel_controller_active)
+    accel_controller.profile = int(self.accel_controller.profile)
 
     # Smart Cruise Control
     smartCruiseControl = longitudinalPlanSP.smartCruiseControl

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/test/longitudinal_maneuvers/plant.py

@@ -0,0 +1,433 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+
+from collections import deque
+from collections.abc import Callable
+from dataclasses import asdict, dataclass
+import math
+import time
+from typing import Any
+
+import numpy as np
+
+from openpilot.cereal import log, messaging
+from opendbc.car.interfaces import ACCEL_MAX, ACCEL_MIN
+from openpilot.common.realtime import DT_CTRL, DT_MDL, Ratekeeper
+from openpilot.selfdrive.modeld.constants import ModelConstants
+from openpilot.selfdrive.controls.lib.longcontrol import LongControl, LongCtrlState
+from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
+from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
+from openpilot.selfdrive.test.longitudinal_maneuvers.plant import Plant, PlannerSM
+
+
+LeadObservation = dict[str, Any]
+LeadObservationFn = Callable[[float, str, LeadObservation], LeadObservation | None]
+ModelActionFn = Callable[[float, float, float], tuple[float, bool]]
+EgoObservationFn = Callable[[float, float, float], tuple[float, float]]
+ModelPlanFn = Callable[[float, float, float], list[float]]
+ModelMetaFn = Callable[[float], tuple[list[float], bool, float]]
+LeadFutureProbsFn = Callable[[float], tuple[float, float, float]]
+PositionYFn = Callable[[float], list[float]]
+ExperimentalModeFn = Callable[[float], bool]
+
+
+@dataclass(frozen=True)
+class ActuatorModel:
+  planner_delay: float
+  transport_delay: float
+  actuator_lag: float
+  command_rate_limit: float
+  stopping_acceleration: float
+  standstill_breakaway_acceleration: float
+  standstill_breakaway_time: float
+
+  def __post_init__(self):
+    nonnegative_fields = {
+      "planner_delay": self.planner_delay,
+      "transport_delay": self.transport_delay,
+      "actuator_lag": self.actuator_lag,
+      "standstill_breakaway_acceleration": self.standstill_breakaway_acceleration,
+      "standstill_breakaway_time": self.standstill_breakaway_time,
+    }
+    if any(not math.isfinite(value) or value < 0.0 for value in nonnegative_fields.values()):
+      raise ValueError(f"ActuatorModel fields must be finite and non-negative: {nonnegative_fields}")
+    if not math.isfinite(self.command_rate_limit) or self.command_rate_limit <= 0.0:
+      raise ValueError("command_rate_limit must be finite and positive")
+    if not math.isfinite(self.stopping_acceleration) or self.stopping_acceleration > 0.0:
+      raise ValueError("stopping_acceleration must be finite and non-positive")
+
+
+# Conservative Prius TSS2 actuator model.
+PRIUS_TSS2_ROUTE_MODEL = ActuatorModel(
+  planner_delay=0.05,
+  transport_delay=0.0,
+  actuator_lag=0.20,
+  command_rate_limit=4.0,
+  stopping_acceleration=-2.0,
+  standstill_breakaway_acceleration=1.0,
+  standstill_breakaway_time=0.05,
+)
+
+
+class PlantSP(Plant):
+  """Closed-loop plant with configurable observations and actuator response."""
+
+  def __init__(
+    self,
+    lead_relevancy=False,
+    speed=0.0,
+    distance_lead=2.0,
+    enabled=True,
+    only_lead2=False,
+    only_radar=False,
+    e2e=False,
+    personality=0,
+    force_decel=False,
+    lead_observation_fn: LeadObservationFn | None = None,
+    model_action_fn: ModelActionFn | None = None,
+    ego_observation_fn: EgoObservationFn | None = None,
+    model_plan_fn: ModelPlanFn | None = None,
+    model_meta_fn: ModelMetaFn | None = None,
+    lead_future_probs_fn: LeadFutureProbsFn | None = None,
+    position_y_fn: PositionYFn | None = None,
+    experimental_mode_fn: ExperimentalModeFn | None = None,
+    actuator_delay: float | None = None,
+    actuator_lag: float = 0.0,
+    actuator_model: ActuatorModel | None = None,
+    run_long_control: bool = False,
+  ):
+    if actuator_delay is not None and (not math.isfinite(actuator_delay) or actuator_delay < 0.0):
+      raise ValueError("actuator_delay must be finite and non-negative")
+    if not math.isfinite(actuator_lag) or actuator_lag < 0.0:
+      raise ValueError("actuator_lag must be finite and non-negative")
+
+    self.rate = 1.0 / DT_MDL
+
+    if not Plant.messaging_initialized:
+      Plant.radar = messaging.pub_sock('radarState')
+      Plant.controls_state = messaging.pub_sock('controlsState')
+      Plant.selfdrive_state = messaging.pub_sock('selfdriveState')
+      Plant.car_state = messaging.pub_sock('carState')
+      Plant.plan = messaging.sub_sock('longitudinalPlan')
+      Plant.messaging_initialized = True
+
+    self.v_lead_prev = 0.0
+
+    self.distance = 0.0
+    self.speed = speed
+    self.should_stop = False
+    self.acceleration = 0.0
+    self.a_target = 0.0
+    self.actuator_command = 0.0
+    self.applied_actuator_command = 0.0
+    self.breakaway_confirmed = False
+    self._breakaway_timer = 0.0
+
+    # lead car
+    self.lead_relevancy = lead_relevancy
+    self.distance_lead = distance_lead
+    self.enabled = enabled
+    self.only_lead2 = only_lead2
+    self.only_radar = only_radar
+    self.e2e = e2e
+    self.personality = personality
+    self.force_decel = force_decel
+    self.lead_observation_fn = lead_observation_fn
+    self.model_action_fn = model_action_fn
+    self.ego_observation_fn = ego_observation_fn
+    self.model_plan_fn = model_plan_fn
+    self.model_meta_fn = model_meta_fn
+    self.lead_future_probs_fn = lead_future_probs_fn
+    self.position_y_fn = position_y_fn
+    self.experimental_mode_fn = experimental_mode_fn
+    self.actuator_model = actuator_model
+    self.actuator_delay = actuator_model.planner_delay if actuator_model is not None else actuator_delay
+    self.transport_delay = actuator_model.transport_delay if actuator_model is not None else actuator_delay
+    self.actuator_lag = actuator_model.actuator_lag if actuator_model is not None else actuator_lag
+    self.publish_realized_a_ego = any((lead_observation_fn is not None, model_action_fn is not None, ego_observation_fn is not None,
+                                      actuator_delay is not None, actuator_lag > 0.0, actuator_model is not None, run_long_control))
+
+    self.rk = Ratekeeper(self.rate, print_delay_threshold=100.0)
+    self.ts = 1.0 / self.rate
+    time.sleep(0.1)
+    self.sm = messaging.SubMaster(['longitudinalPlan'])
+
+    from opendbc.car.honda.values import CAR
+    from opendbc.car.honda.interface import CarInterface
+
+    CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+    if self.actuator_delay is not None:
+      CP.longitudinalActuatorDelay = self.actuator_delay
+    CP_SP = CarInterface.get_non_essential_params_sp(CP, CAR.HONDA_CIVIC)
+    self.planner = LongitudinalPlanner(CP, CP_SP, init_v=self.speed)
+    self.long_control = LongControl(CP, CP_SP) if run_long_control else None
+
+    if self.actuator_model is not None and self.speed >= 0.01:
+      self.breakaway_confirmed = True
+    self.integration_dt = DT_CTRL if run_long_control else self.ts
+    delay_steps = 0 if self.transport_delay is None else round(self.transport_delay / self.integration_dt)
+    self._actuator_delay_queue = deque([self.acceleration] * delay_steps)
+
+  @staticmethod
+  def _lead_message(observation: LeadObservation):
+    lead = log.RadarState.LeadData.new_message()
+    for field, value in observation.items():
+      setattr(lead, field, value)
+    return lead
+
+  def _observe_lead(self, lead_name: str, truth: LeadObservation, present_by_default: bool) -> LeadObservation | None:
+    if self.lead_observation_fn is None:
+      return dict(truth) if present_by_default else None
+
+    observed = self.lead_observation_fn(self.current_time, lead_name, dict(truth))
+    if observed is None:
+      return None
+
+    complete_observation = dict(truth)
+    complete_observation.update(observed)
+    return complete_observation
+
+  def _update_actuator(self, command: float) -> tuple[float, float]:
+    if self._actuator_delay_queue:
+      self._actuator_delay_queue.append(command)
+      delayed_command = self._actuator_delay_queue.popleft()
+    else:
+      delayed_command = command
+
+    if self.actuator_model is not None:
+      max_command_delta = self.actuator_model.command_rate_limit * self.integration_dt
+      self.applied_actuator_command = float(np.clip(delayed_command,
+                                                   self.applied_actuator_command - max_command_delta,
+                                                   self.applied_actuator_command + max_command_delta))
+
+      if self.speed < 0.01:
+        if self.applied_actuator_command <= 0.0:
+          self.breakaway_confirmed = False
+          self._breakaway_timer = 0.0
+        elif not self.breakaway_confirmed:
+          breakaway_ready = self.applied_actuator_command + 1e-9 >= self.actuator_model.standstill_breakaway_acceleration
+          if breakaway_ready:
+            self._breakaway_timer += self.integration_dt
+          else:
+            self._breakaway_timer = 0.0
+
+          self.breakaway_confirmed = breakaway_ready and self._breakaway_timer + 1e-9 >= self.actuator_model.standstill_breakaway_time
+        if not self.breakaway_confirmed:
+          self.acceleration = 0.0
+          return delayed_command, self.acceleration
+      else:
+        self.breakaway_confirmed = True
+
+      response_command = self.applied_actuator_command
+    else:
+      self.applied_actuator_command = delayed_command
+      response_command = delayed_command
+
+    if self.actuator_lag > 0.0:
+      alpha = 1.0 - math.exp(-self.integration_dt / self.actuator_lag)
+      self.acceleration += alpha * (response_command - self.acceleration)
+    else:
+      self.acceleration = response_command
+    return delayed_command, self.acceleration
+
+  def _integrate_ego(self, dt: float, stop_at_standstill: bool = False) -> None:
+    self.speed += self.acceleration * dt
+    if self.speed <= 0.0 or stop_at_standstill and self.speed < 0.01 and self.actuator_command <= 0.0:
+      self.speed = self.acceleration = 0.0
+    self.distance += self.speed * dt
+
+  def step(self, v_lead=0.0, prob_lead=1.0, v_cruise=50.0, pitch=0.0, prob_throttle=1.0):
+    # ******** publish a fake model going straight and fake calibration ********
+    # note that this is worst case for MPC, since model will delay long mpc by one time step
+    radar = messaging.new_message('radarState')
+    control = messaging.new_message('controlsState')
+    ss = messaging.new_message('selfdriveState')
+    car_state = messaging.new_message('carState')
+    vehicle_parameters = messaging.new_message('vehicleParameters')
+    car_control = messaging.new_message('carControl')
+    model = messaging.new_message('modelV2')
+    car_state_sp = messaging.new_message('carStateSP')
+    live_map_data_sp = messaging.new_message('liveMapDataSP')
+    gps_data = messaging.new_message('gpsLocation')
+    a_lead = (v_lead - self.v_lead_prev) / self.ts
+    self.v_lead_prev = v_lead
+
+    if self.lead_relevancy:
+      d_rel = np.maximum(0.0, self.distance_lead - self.distance)
+      v_rel = v_lead - self.speed
+      if self.only_radar:
+        status = True
+      elif prob_lead > 0.5:
+        status = True
+      else:
+        status = False
+    else:
+      d_rel = 200.0
+      v_rel = 0.0
+      prob_lead = 0.0
+      status = False
+
+    truth_lead: LeadObservation = {
+      "dRel": float(d_rel),
+      "yRel": 0.0,
+      "vRel": float(v_rel),
+      "vLead": float(v_lead),
+      "vLeadK": float(v_lead),
+      "aLeadK": float(a_lead),
+      "present": bool(status),
+      # TODO use real radard logic for this
+      "aLeadTau": float(_LEAD_ACCEL_TAU),
+      "modelProb": float(prob_lead),
+      "radar": bool(self.only_radar),
+      "radarTrackId": -1,
+    }
+    lead_one_observation = self._observe_lead("leadOne", truth_lead, not self.only_lead2)
+    lead_two_observation = self._observe_lead("leadTwo", truth_lead, True)
+    if lead_one_observation is not None:
+      radar.radarState.leadOne = self._lead_message(lead_one_observation)
+    if lead_two_observation is not None:
+      radar.radarState.leadTwo = self._lead_message(lead_two_observation)
+
+    # Simulate model predicting slightly faster speed
+    # this is to ensure lead policy is effective when model
+    # does not predict slowdown in e2e mode
+    position = log.XYZTData.new_message()
+    position.x = [float(x) for x in (self.speed + 0.5) * np.array(ModelConstants.T_IDXS)]
+    if self.position_y_fn is None:
+      position.y = [0.0] * len(ModelConstants.T_IDXS)
+    else:
+      position.y = [float(y) for y in self.position_y_fn(self.current_time)]
+    model.modelV2.position = position
+    if self.model_action_fn is None:
+      model_acceleration, model_should_stop = self.acceleration + 0.5, False
+    else:
+      model_acceleration, model_should_stop = self.model_action_fn(self.current_time, self.speed, self.acceleration)
+    model.modelV2.action.desiredAcceleration = float(model_acceleration)
+    model.modelV2.action.shouldStop = bool(model_should_stop)
+    velocity = log.XYZTData.new_message()
+    if self.model_plan_fn is None:
+      velocity_plan = [float(x) for x in (self.speed + 0.5) * np.ones_like(ModelConstants.T_IDXS)]
+      velocity_plan[0] = float(self.speed)  # always start at current speed
+    else:
+      velocity_plan = [float(x) for x in self.model_plan_fn(self.current_time, self.speed, self.acceleration)]
+    velocity.x = velocity_plan
+    model.modelV2.velocity = velocity
+    acceleration = log.XYZTData.new_message()
+    acceleration.x = [float(x) for x in np.zeros_like(ModelConstants.T_IDXS)]
+    model.modelV2.acceleration = acceleration
+    model.modelV2.meta.disengagePredictions.gasPressProbs = [float(prob_throttle) for _ in range(6)]
+    if self.model_meta_fn is None:
+      brake3_probs, hard_brake_predicted, frame_drop_perc = [0.0] * 5, False, 0.0
+    else:
+      brake3_probs, hard_brake_predicted, frame_drop_perc = self.model_meta_fn(self.current_time)
+    model.modelV2.meta.disengagePredictions.brake3MetersPerSecondSquaredProbs = [float(p) for p in brake3_probs]
+    model.modelV2.meta.hardBrakePredicted = bool(hard_brake_predicted)
+    model.modelV2.frameDropPerc = float(frame_drop_perc)
+    if self.lead_future_probs_fn is not None:
+      model.modelV2.init('leadsV3', 3)
+      lead_future_probs = self.lead_future_probs_fn(self.current_time)
+      for i, (prob, prob_time) in enumerate(zip(lead_future_probs, (0.0, 2.0, 4.0), strict=True)):
+        model.modelV2.leadsV3[i].prob = float(prob)
+        model.modelV2.leadsV3[i].probTime = prob_time
+
+    control.controlsState.longControlState = self.long_control.long_control_state if self.long_control is not None else (
+      LongCtrlState.pid if self.enabled else LongCtrlState.off)
+    ss.selfdriveState.experimentalMode = self.e2e if self.experimental_mode_fn is None else bool(self.experimental_mode_fn(self.current_time))
+    ss.selfdriveState.personality = self.personality
+    control.controlsState.forceDecel = self.force_decel
+    true_v_ego = self.speed
+    true_a_ego = self.acceleration
+    published_v_ego = true_v_ego
+    published_a_ego = true_a_ego if self.publish_realized_a_ego else 0.0
+    if self.ego_observation_fn is not None:
+      published_v_ego, published_a_ego = self.ego_observation_fn(self.current_time, true_v_ego, true_a_ego)
+    car_state.carState.vEgo = float(published_v_ego)
+    car_state.carState.aEgo = float(published_a_ego)
+    car_state.carState.standstill = bool(self.speed < 0.01)
+    car_state.carState.vCruise = float(v_cruise * 3.6)
+    car_control.carControl.orientationNED = [0.0, float(pitch), 0.0]
+
+    # ******** get controlsState messages for plotting ***
+    sm = PlannerSM(self.rk.frame, {
+      'radarState': radar.radarState,
+      'carState': car_state.carState,
+      'carControl': car_control.carControl,
+      'controlsState': control.controlsState,
+      'selfdriveState': ss.selfdriveState,
+      'vehicleParameters': vehicle_parameters.vehicleParameters,
+      'modelV2': model.modelV2,
+      'carStateSP': car_state_sp.carStateSP,
+      'liveMapDataSP': live_map_data_sp.liveMapDataSP,
+      'gpsLocation': gps_data.gpsLocation,
+    })
+    self.planner.update(sm)
+    self.a_target = self.planner.output_a_target
+    if self.long_control is None:
+      self.actuator_command = self.a_target
+      if self.planner.output_should_stop:
+        stopping_acceleration = -0.5 if self.actuator_model is None else self.actuator_model.stopping_acceleration
+        self.actuator_command = min(stopping_acceleration, self.actuator_command)
+      self._update_actuator(self.actuator_command)
+      self._integrate_ego(self.ts)
+    else:
+      for _ in range(round(self.ts / DT_CTRL)):
+        car_state.carState.vEgo = self.speed
+        car_state.carState.aEgo = self.acceleration
+        car_state.carState.standstill = self.speed < 0.01
+        self.actuator_command = self.long_control.update(
+          self.enabled, car_state.carState, self.a_target, self.planner.output_should_stop, (ACCEL_MIN, ACCEL_MAX),
+        )
+        self._update_actuator(self.actuator_command)
+        self._integrate_ego(DT_CTRL, stop_at_standstill=True)
+    self.should_stop = self.planner.output_should_stop
+    fcw = self.planner.fcw
+    self.distance_lead = self.distance_lead + v_lead * self.ts
+
+    # *** radar model ***
+    if self.lead_relevancy:
+      d_rel = np.maximum(0.0, self.distance_lead - self.distance)
+      v_rel = v_lead - self.speed
+    else:
+      d_rel = 200.0
+      v_rel = 0.0
+
+    # print at 5hz
+    # if (self.rk.frame % (self.rate // 5)) == 0:
+    #   print("%2.2f sec   %6.2f m  %6.2f m/s  %6.2f m/s2   lead_rel: %6.2f m  %6.2f m/s"
+    #         % (self.current_time, self.distance, self.speed, self.acceleration, d_rel, v_rel))
+
+    # ******** update prevs ********
+    self.rk.monitor_time()
+
+    return {
+      "distance": self.distance,
+      "speed": self.speed,
+      "acceleration": self.acceleration,
+      "realized_acceleration": self.acceleration,
+      "a_target": self.a_target,
+      "actuator_command": self.actuator_command,
+      "published_a_ego": published_a_ego,
+      "published_v_ego": published_v_ego,
+      "should_stop": self.should_stop,
+      "long_control_state": (int(self.long_control.long_control_state) if self.long_control is not None
+                             else control.controlsState.longControlState.raw),
+      "distance_lead": self.distance_lead,
+      "fcw": fcw,
+      "mpc_source": self.planner.mpc.source,
+      "dec_mode": self.planner.dec.mode(),
+      "dec_want_blended": self.planner.dec.want_blended,
+      "dec_signals": asdict(self.planner.dec.signals),
+      "dec_lead_veto": self.planner.dec.lead_veto,
+      "controller_active": self.planner.accel_controller_active,
+      "model_action": {
+        "desiredAcceleration": float(model_acceleration),
+        "shouldStop": bool(model_should_stop),
+      },
+      "truth_lead": dict(truth_lead),
+      "lead_one_observation": None if lead_one_observation is None else dict(lead_one_observation),
+      "lead_two_observation": None if lead_two_observation is None else dict(lead_two_observation),
+    }

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

@@ -0,0 +1,186 @@
+import numpy as np
+
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+from openpilot.common.test import OpenpilotTestCase
+from openpilot.selfdrive.modeld.constants import ModelConstants
+from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import ENTER_FRAMES, MIN_BLENDED_FRAMES
+from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PlantSP
+
+T_IDXS = np.array(ModelConstants.T_IDXS)
+
+
+def decel_plan(a):
+  def fn(_current_time, speed, _acceleration):
+    return [float(max(0.0, speed + a * t)) for t in T_IDXS]
+  return fn
+
+
+def flat_plan():
+  def fn(_current_time, speed, _acceleration):
+    return [float(speed)] * len(T_IDXS)
+  return fn
+
+
+def alternating_plan(a):
+  def fn(current_time, speed, _acceleration):
+    frame_a = a if round(current_time / DT_MDL) % 2 == 0 else 0.0
+    return [float(max(0.0, speed + frame_a * t)) for t in T_IDXS]
+  return fn
+
+
+def persistent_lead_probs(_current_time):
+  return (1.0, 0.95, 0.9)
+
+
+def _run(plant, steps, v_lead=0.0, v_cruise=50.0):
+  solver_failures = 0
+  original_reset = plant.planner.mpc.reset
+
+  def counting_reset(*args, **kw):
+    nonlocal solver_failures
+    if plant.planner.mpc.solution_status != 0:
+      solver_failures += 1
+    return original_reset(*args, **kw)
+
+  plant.planner.mpc.reset = counting_reset
+  return [plant.step(v_lead=v_lead, v_cruise=v_cruise) for _ in range(steps)], solver_failures
+
+
+def mode_changes(results):
+  modes = [r["dec_mode"] for r in results]
+  return sum(a != b for a, b in zip(modes, modes[1:], strict=False))
+
+
+class TestDecManeuvers(OpenpilotTestCase):
+  def setUp(self):
+    super().setUp()
+    self.params = Params()
+    self.params.put_bool("DynamicExperimentalControl", True, block=True)
+
+  def test_s1_lead_clears_with_underlying_slowdown_blends_quickly(self):
+    clear_t = 1.0
+
+    def lead_obs(current_time, _lead_name, truth):
+      return None if current_time >= clear_t else dict(truth)
+
+    plant = PlantSP(lead_relevancy=True, speed=20.0, distance_lead=40.0, e2e=True, only_radar=True,
+                     lead_observation_fn=lead_obs, model_plan_fn=decel_plan(-2.5),
+                     lead_future_probs_fn=persistent_lead_probs)
+    clear_frame = round(clear_t / DT_MDL)
+    results, _ = _run(plant, steps=clear_frame + ENTER_FRAMES + 5, v_lead=20.0, v_cruise=20.0)
+
+    assert all(r["dec_mode"] == "acc" for r in results[:clear_frame])
+    assert all(r["dec_lead_veto"] for r in results[:clear_frame])
+    post_clear = [r["dec_mode"] for r in results[clear_frame:clear_frame + ENTER_FRAMES + 2]]
+    assert "blended" in post_clear
+
+  def test_s1b_lead_clears_with_no_underlying_slowdown_stays_acc(self):
+    clear_t = 1.0
+
+    def lead_obs(current_time, _lead_name, truth):
+      return None if current_time >= clear_t else dict(truth)
+
+    plant = PlantSP(lead_relevancy=True, speed=20.0, distance_lead=40.0, e2e=True, only_radar=True,
+                     lead_observation_fn=lead_obs, model_plan_fn=flat_plan(),
+                     lead_future_probs_fn=persistent_lead_probs)
+    clear_frame = round(clear_t / DT_MDL)
+    results, _ = _run(plant, steps=clear_frame + MIN_BLENDED_FRAMES, v_lead=20.0, v_cruise=20.0)
+
+    assert all(r["dec_mode"] == "acc" for r in results)
+
+  def test_s2_steady_highway_following_never_blends(self):
+    v = 80.0 / 3.6
+    plant = PlantSP(lead_relevancy=True, speed=v, distance_lead=40.0, e2e=True, only_radar=True,
+                     model_plan_fn=flat_plan(), lead_future_probs_fn=persistent_lead_probs)
+    results, failures = _run(plant, steps=100, v_lead=v, v_cruise=v)
+
+    assert failures <= 1
+    assert all(r["dec_mode"] == "acc" for r in results)
+
+  def test_s3_low_speed_cruise_no_lead_never_blends(self):
+    v = 15.0 / 3.6
+    plant = PlantSP(lead_relevancy=False, speed=v, e2e=True, model_plan_fn=flat_plan())
+    results, _ = _run(plant, steps=100, v_cruise=v)
+
+    assert all(r["dec_mode"] == "acc" for r in results)
+
+  def test_s4_highway_slowdown_without_lead_blends(self):
+    v0 = 110.0 / 3.6
+    a = (70.0 / 3.6 - v0) / 6.0
+    plant = PlantSP(lead_relevancy=False, speed=v0, e2e=True, model_plan_fn=decel_plan(a))
+    results, _ = _run(plant, steps=10, v_cruise=v0)
+
+    assert any(r["dec_mode"] == "blended" for r in results)
+
+  def test_s5_stop_then_depart_with_lead_present_stays_acc_throughout(self):
+    def departing_lead(current_time):
+      return 0.0 if current_time < 1.0 else min(15.0, 3.0 * (current_time - 1.0))
+
+    plant = PlantSP(lead_relevancy=True, speed=0.0, distance_lead=6.0, e2e=True)
+    results = []
+    solver_failures = 0
+    original_reset = plant.planner.mpc.reset
+
+    def counting_reset(*args, **kw):
+      nonlocal solver_failures
+      if plant.planner.mpc.solution_status != 0:
+        solver_failures += 1
+      return original_reset(*args, **kw)
+
+    plant.planner.mpc.reset = counting_reset
+    for _ in range(200):
+      results.append(plant.step(v_lead=departing_lead(plant.current_time), v_cruise=15.0))
+
+    assert solver_failures <= 1
+    assert all(r["dec_mode"] == "acc" for r in results)
+    assert all(r["dec_lead_veto"] for r in results)
+
+  def test_s6_creep_cycles_behind_lead_stay_acc(self):
+    def creep_cycle_lead(current_time):
+      return 1.5 + 1.5 * np.sin(current_time * 2.0)
+
+    plant = PlantSP(lead_relevancy=True, speed=1.0, distance_lead=8.0, e2e=True, only_radar=True,
+                     model_plan_fn=flat_plan(), lead_future_probs_fn=persistent_lead_probs)
+    results = [plant.step(v_lead=creep_cycle_lead(plant.current_time), v_cruise=5.0) for _ in range(200)]
+
+    assert all(r["dec_mode"] == "acc" for r in results)
+
+  def test_s7_oscillating_near_threshold_demand_does_not_flap(self):
+    plant = PlantSP(lead_relevancy=False, speed=20.0, e2e=True, model_plan_fn=alternating_plan(-2.5))
+    results, _ = _run(plant, steps=200, v_cruise=20.0)
+
+    assert mode_changes(results) <= 2
+
+  def test_s8_degraded_model_holds_acc_through_a_slowdown(self):
+    def degraded_meta(_current_time):
+      return [0.0] * 5, False, 60.0
+
+    plant = PlantSP(lead_relevancy=False, speed=20.0, e2e=True, model_plan_fn=decel_plan(-3.0), model_meta_fn=degraded_meta)
+    results, _ = _run(plant, steps=30, v_cruise=20.0)
+
+    assert all(r["dec_mode"] == "acc" for r in results)
+
+  def test_s9_curve_exclusion_prevents_false_blend_on_a_bend(self):
+    plant = PlantSP(lead_relevancy=False, speed=20.0, e2e=True, model_plan_fn=decel_plan(-1.0),
+                     position_y_fn=lambda _t: [6.0] * len(T_IDXS))
+    results, _ = _run(plant, steps=30, v_cruise=20.0)
+
+    assert all(r["dec_mode"] == "acc" for r in results)
+
+  def test_s11_curve_does_not_interrupt_an_active_hard_stop(self):
+    plant = PlantSP(lead_relevancy=False, speed=20.0, e2e=True, model_plan_fn=decel_plan(-2.5),
+                     position_y_fn=lambda _t: [6.0] * len(T_IDXS))
+    results, _ = _run(plant, steps=10, v_cruise=20.0)
+
+    assert all(r["dec_mode"] == "blended" for r in results[ENTER_FRAMES - 1:])
+
+  def test_s10_hard_brake_override_inert_while_lead_present(self):
+    def hard_brake_meta(_current_time):
+      return [0.0] * 5, True, 0.0
+
+    plant = PlantSP(lead_relevancy=True, speed=20.0, distance_lead=40.0, e2e=True, only_radar=True,
+                     model_plan_fn=flat_plan(), model_meta_fn=hard_brake_meta, lead_future_probs_fn=persistent_lead_probs)
+    results, _ = _run(plant, steps=10, v_lead=20.0, v_cruise=20.0)
+
+    assert all(r["dec_mode"] == "acc" for r in results)

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/athena/sunnylinkd.py

@@ -183,7 +183,7 @@ def getParamsMetadata() -> str:
     schema["capability_labels"] = CAPABILITY_LABELS
     schema["default_model"] = DEFAULT_MODEL
     schema["default_big_model"] = DEFAULT_BIG_MODEL
-    schema["chestnut_active"] = params.get_bool("ChestnutActive")
+    schema["usbgpu_active"] = params.get_bool("UsbGpuActive")
     raw = json.dumps(schema, separators=(",", ":")).encode("utf-8")
     return base64.b64encode(gzip.compress(raw)).decode("utf-8")
   except Exception:

openpilot/sunnypilot/sunnylink/settings_ui.json

@@ -652,6 +652,53 @@
                 }
               ]
             },
+            {
+              "key": "AccelPersonalityEnabled",
+              "widget": "toggle",
+              "title": "Enable Accel Controller",
+              "description": "Lets you choose how sunnypilot starts, catches up, and settles at the cruise speed. Emergency braking and stopping are unchanged.",
+              "visibility": [
+                {
+                  "type": "capability",
+                  "field": "has_longitudinal_control",
+                  "equals": true
+                }
+              ],
+              "enablement": [
+                {
+                  "type": "capability",
+                  "field": "has_longitudinal_control",
+                  "equals": true
+                }
+              ]
+            },
+            {
+              "key": "AccelPersonality",
+              "widget": "multiple_button",
+              "title": "Acceleration Profile",
+              "description": "Eco is gentlest, Normal balances a prompt start with smooth catch-up, and Sport is more responsive.",
+              "options": [
+                {
+                  "value": 0,
+                  "label": "Eco"
+                },
+                {
+                  "value": 1,
+                  "label": "Normal"
+                },
+                {
+                  "value": 2,
+                  "label": "Sport"
+                }
+              ],
+              "enablement": [
+                {
+                  "type": "capability",
+                  "field": "has_longitudinal_control",
+                  "equals": true
+                }
+              ]
+            },
             {
               "key": "IntelligentCruiseButtonManagement",
               "widget": "toggle",
@@ -2302,6 +2349,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",

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

@@ -43,6 +43,28 @@ sections:
       label: Relaxed
     enablement:
     - $ref: '#/macros/longitudinal'
+  - key: AccelPersonalityEnabled
+    widget: toggle
+    title: Enable Accel Controller
+    description: Lets you choose how sunnypilot starts, catches up, and settles at the cruise speed. Emergency braking and stopping are
+      unchanged.
+    visibility:
+    - $ref: '#/macros/longitudinal'
+    enablement:
+    - $ref: '#/macros/longitudinal'
+  - key: AccelPersonality
+    widget: multiple_button
+    title: Acceleration Profile
+    description: Eco is gentlest, Normal balances a prompt start with smooth catch-up, and Sport is more responsive.
+    options:
+    - value: 0
+      label: Eco
+    - value: 1
+      label: Normal
+    - value: 2
+      label: Sport
+    enablement:
+    - $ref: '#/macros/longitudinal'
   - key: IntelligentCruiseButtonManagement
     widget: toggle
     title: Intelligent Cruise Button Management (ICBM) (Alpha)

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

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

openpilot/sunnypilot/sunnylink/statsd.py

@@ -65,7 +65,7 @@ def sp_stats(end_event):
     'MadsSteeringMode',
     'MadsUnifiedEngagementMode',
     'ModelManager_ActiveBundle',
-    'ModelManager_ActiveBundleChestnut',
+    'ModelManager_ActiveBundleUSBGPU',
     'ModelManager_Favs',
     'EnableSunnylinkUploader',
     'SunnylinkEnabled',

openpilot/sunnypilot/sunnylink/tests/test_capabilities.py

@@ -10,9 +10,10 @@ 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 openpilot.common.test import OpenpilotTestCase
 from openpilot.sunnypilot.sunnylink.capabilities import (
   CAPABILITY_DEFAULTS,
   CAPABILITY_FIELDS,
@@ -20,13 +21,23 @@ 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 caps():
   return generate_capabilities()
 
@@ -52,14 +63,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):

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", []):
@@ -149,22 +149,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 +168,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 +200,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 +215,5 @@ 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"
+
+

openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py

@@ -276,13 +276,36 @@ 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(self, schema):
     keys = {i["key"] for i in _brand_items(schema["vehicle_settings"].get("toyota"))}
     assert "ToyotaEnforceStockLongitudinal" in keys
     assert "ToyotaStopAndGoHack" in keys

openpilot/sunnypilot/system/params_migration.py

@@ -89,16 +89,12 @@ def _migrate_model_bundle_slots(_params):
   # ActiveBundle. Seed both slots; validation drops whichever does not match
   # its own manifest.
   try:
-    if _params.get("ModelManager_ActiveBundleChestnut") is not None:
-      return
-    if (chestnut_bundle := _params.get("ModelManager_ActiveBundleUSBGPU")) is not None:
-      _params.put("ModelManager_ActiveBundleChestnut", chestnut_bundle, block=True)
-      cloudlog.info("params_migration: seeded ModelManager_ActiveBundleChestnut from ModelManager_ActiveBundleUSBGPU")
+    if _params.get("ModelManager_ActiveBundleUSBGPU") is not None:
       return
     if (bundle := _params.get("ModelManager_ActiveBundle")) is None:
       return
-    _params.put("ModelManager_ActiveBundleChestnut", bundle, block=True)
-    cloudlog.info("params_migration: seeded ModelManager_ActiveBundleChestnut from ModelManager_ActiveBundle")
+    _params.put("ModelManager_ActiveBundleUSBGPU", bundle, block=True)
+    cloudlog.info("params_migration: seeded ModelManager_ActiveBundleUSBGPU from ModelManager_ActiveBundle")
   except Exception as e:
     cloudlog.exception(f"Error migrating model bundle slots: {e}")
 
@@ -140,5 +136,5 @@ def run_migration(_params):
   # seed TeslaMadsScreenButton for existing Tesla installs
   _migrate_tesla_mads_screen_button(_params)
 
-  # seed the chestnut model slot from the pre-split single slot
+  # seed the usbgpu model slot from the pre-split single slot
   _migrate_model_bundle_slots(_params)

openpilot/sunnypilot/system/tests/test_params_migration.py

@@ -15,22 +15,22 @@ class TestModelBundleSlotMigration(OpenpilotTestCase):
   The migration seeds both slots; per-source validation later drops whichever does not
   match its own manifest."""
 
-  def test_seeds_chestnut_slot_from_active_bundle(self):
+  def test_seeds_usbgpu_slot_from_active_bundle(self):
     params = Params()
     bundle = {"ref": "big", "minimumSelectorVersion": 18}
     params.put("ModelManager_ActiveBundle", bundle, block=True)
     _migrate_model_bundle_slots(params)
-    assert params.get("ModelManager_ActiveBundleChestnut") == bundle
+    assert params.get("ModelManager_ActiveBundleUSBGPU") == bundle
     assert params.get("ModelManager_ActiveBundle") == bundle
 
-  def test_noop_when_chestnut_slot_already_set(self):
+  def test_noop_when_usbgpu_slot_already_set(self):
     params = Params()
     params.put("ModelManager_ActiveBundle", {"ref": "small"}, block=True)
-    params.put("ModelManager_ActiveBundleChestnut", {"ref": "big"}, block=True)
+    params.put("ModelManager_ActiveBundleUSBGPU", {"ref": "big"}, block=True)
     _migrate_model_bundle_slots(params)
-    assert params.get("ModelManager_ActiveBundleChestnut") == {"ref": "big"}
+    assert params.get("ModelManager_ActiveBundleUSBGPU") == {"ref": "big"}
 
   def test_noop_when_no_selection(self):
     params = Params()
     _migrate_model_bundle_slots(params)
-    assert params.get("ModelManager_ActiveBundleChestnut") is None
+    assert params.get("ModelManager_ActiveBundleUSBGPU") is None

openpilot/system/hardware/hardwared.py

@@ -16,7 +16,7 @@ from openpilot.common.utils import strip_deprecated_keys
 from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.common.params import Params
 from openpilot.common.realtime import DT_HW
-from openpilot.selfdrive.modeld.helpers import MODELS_DIR, chestnut_compiled
+from openpilot.selfdrive.modeld.helpers import MODELS_DIR, usbgpu_compiled
 from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert
 from openpilot.common.hardware import HARDWARE, COMMA_HARDWARE
 from openpilot.common.basedir import BASEDIR
@@ -239,7 +239,7 @@ def hardware_thread(end_event, hw_queue) -> None:
 
   fan_controller = FanController(int(1./DT_HW))
   chestnut = Chestnut()
-  big_model_available = (MODELS_DIR / 'big_driving_supercombo.onnx').is_file() or chestnut_compiled()
+  big_model_available = (MODELS_DIR / 'big_driving_supercombo.onnx').is_file() or usbgpu_compiled()
 
   while not end_event.is_set():
     sm.update(PANDA_STATES_TIMEOUT)

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