long: prevent lead pulsing acceleration

# Conflicts:
#	openpilot/sunnypilot/modeld_v2/compile_modeld.py
#	openpilot/sunnypilot/modeld_v2/modeld.py
#	openpilot/sunnypilot/modeld_v2/tests/test_combined_pkl_loader.py
#	openpilot/sunnypilot/modeld_v2/tests/test_warp.py
#	openpilot/sunnypilot/modeld_v2/warp.py
#	openpilot/sunnypilot/models/manager.py
#	openpilot/sunnypilot/models/runners/helpers.py
#	openpilot/sunnypilot/models/runners/model_runner.py
#	openpilot/sunnypilot/models/runners/tinygrad/model_types.py
#	openpilot/sunnypilot/models/runners/tinygrad/tinygrad_runner.py
#	sunnypilot/models/helpers.py

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

@@ -34,14 +34,6 @@ on:
         required: false
         default: true
         type: boolean
-      target_hardware:
-        description: 'Hardware target to compile for'
-        required: false
-        type: choice
-        default: 'qcom'
-        options:
-          - qcom
-          - usbgpu
   workflow_dispatch:
     inputs:
       upstream_branch:
@@ -89,17 +81,9 @@ on:
         description: 'Minimum selector version'
         required: false
         type: string
-      target_hardware:
-        description: 'Hardware target to compile for'
-        required: false
-        type: choice
-        default: 'qcom'
-        options:
-          - qcom
-          - usbgpu
 env:
   RECOMPILED_DIR: recompiled${{ inputs.recompiled_dir }}
-  JSON_FILE: docs/docs/driving_models_${{ inputs.target_hardware == 'usbgpu' && 'usbgpu_v' || 'v' }}${{ inputs.json_version }}.json
+  JSON_FILE: docs/docs/driving_models_v${{ inputs.json_version }}.json
 
 jobs:
   build_model:
@@ -109,7 +93,6 @@ jobs:
       custom_name: ${{ inputs.custom_name || inputs.upstream_branch }}
       is_20hz: ${{ inputs.is_20hz }}
       artifact_suffix: ${{ inputs.artifact_suffix }}
-      target_hardware: ${{ inputs.target_hardware }}
     secrets: inherit
 
   publish_model:

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

@@ -191,7 +191,7 @@ jobs:
           if [ "${{ inputs.target_hardware }}" == "usbgpu" ]; then
             echo "USBGPU build"
             export USBGPU=1
-            TG_FLAGS="DEBUG=2 DEV=USB+AMD:LLVM WARP_DEV=QCOM FLOAT16=1 JIT_BATCH_SIZE=0 GMMU=0"
+            TG_FLAGS="DEV=AMD USBGPU=1 IMAGE=1 FLOAT16=1 NOLOCALS=1 JIT_BATCH_SIZE=0 OPENPILOT_HACKS=1"
             OUTPUT_PKL="${{ env.MODELS_DIR }}/big_driving_tinygrad.pkl"
           else
             echo "QCOM build"

.gitmodules

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

openpilot/cereal/custom.capnp

@@ -203,6 +203,7 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
   aTarget @5 :Float32;
   events @6 :List(OnroadEventSP.Event);
   e2eAlerts @7 :E2eAlerts;
+  accelController @8 :AccelController;
 
   struct DynamicExperimentalControl {
     state @0 :DynamicExperimentalControlState;
@@ -305,6 +306,35 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
     greenLightAlert @0 :Bool;
     leadDepartAlert @1 :Bool;
   }
+
+  struct AccelController {
+    enabled @0 :Bool;
+    active @1 :Bool;
+    shadowOnlyDEPRECATED @2 :Bool;
+    profile @3 :Profile;
+    state @4 :State;
+
+    enum Profile {
+      eco @0;
+      normal @1;
+      sport @2;
+    }
+
+    enum State {
+      inactive @0;
+      free @1;
+      restrict @2;
+      hold @3;
+      release @4;
+      stopHold @5;
+    }
+  }
+
+  enum AccelerationPersonality {
+    eco @0;
+    normal @1;
+    sport @2;
+  }
 }
 
 struct OnroadEventSP @0xda96579883444c35 {
@@ -351,6 +381,7 @@ struct OnroadEventSP @0xda96579883444c35 {
     speedLimitChanged @21;
     speedLimitPending @22;
     e2eChime @23;
+    laneChangeRoadEdge @24;
   }
 }
 
@@ -457,6 +488,8 @@ struct LiveMapDataSP @0xf416ec09499d9d19 {
 
 struct ModelDataV2SP @0xa1680744031fdb2d {
   laneTurnDirection @0 :TurnDirection;
+  leftLaneChangeEdgeBlock @1 :Bool;
+  rightLaneChangeEdgeBlock @2 :Bool;
 
   enum TurnDirection {
     none @0;

openpilot/common/params_keys.h

@@ -179,12 +179,20 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"QuickBootToggle", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"QuietMode", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"RainbowMode", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"RoadEdgeLaneChangeEnabled", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"RocketFuel", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"ShowAdvancedControls", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"ShowTurnSignals", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"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"}},
+    {"ToyotaPriusTss2Pid", {PERSISTENT | BACKUP, BOOL, "0"}},
+
     // MADS params
     {"Mads", {PERSISTENT | BACKUP, BOOL, "1"}},
     {"MadsMainCruiseAllowed", {PERSISTENT | BACKUP, BOOL, "1"}},
@@ -197,9 +205,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"ModelManager_DownloadIndex", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, INT}},
     {"ModelManager_Favs", {PERSISTENT | BACKUP, STRING}},
     {"ModelManager_LastSyncTime", {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_USBGPU", {PERSISTENT | BACKUP, JSON}},
 
     // Neural Network Lateral Control
     {"NeuralNetworkLateralControl", {PERSISTENT | BACKUP, BOOL, "0"}},
@@ -231,6 +237,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

@@ -112,12 +112,16 @@ class TestParams:
   def test_params_default_value(self):
     self.params.remove("LanguageSetting")
     self.params.remove("LongitudinalPersonality")
+    self.params.remove("AccelPersonalityEnabled")
+    self.params.remove("AccelPersonality")
     self.params.remove("LiveParametersV2")
 
     assert self.params.get("LanguageSetting") is None
     assert self.params.get("LanguageSetting", return_default=False) is None
     assert isinstance(self.params.get("LanguageSetting", return_default=True), str)
     assert isinstance(self.params.get("LongitudinalPersonality", return_default=True), int)
+    assert self.params.get("AccelPersonalityEnabled", return_default=True) is False
+    assert self.params.get("AccelPersonality", return_default=True) == 1
     assert self.params.get("LiveParametersV2") is None
     assert self.params.get("LiveParametersV2", return_default=True) is None
 

openpilot/selfdrive/car/card.py

@@ -11,7 +11,7 @@ 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.safety import ALTERNATIVE_EXPERIENCE
 from opendbc.car import DT_CTRL, structs
 from opendbc.car.can_definitions import CanData, CanRecvCallable, CanSendCallable
 from opendbc.car.carlog import carlog
@@ -122,7 +122,13 @@ class Car:
       self.CI, self.CP, self.CP_SP = CI, CI.CP, CI.CP_SP
       self.RI = RI
 
+    # set alternative experiences from parameters
+    sp_toyota_auto_brake_hold = self.params.get_bool("ToyotaAutoHold")
     self.CP.alternativeExperience = 0
+    if sp_toyota_auto_brake_hold:
+      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/controls/lib/desire_helper.py

@@ -33,7 +33,7 @@ class DesireHelper:
   def get_lane_change_direction(CS):
     return LaneChangeDirection.left if CS.leftBlinker else LaneChangeDirection.right
 
-  def update(self, carstate, lateral_active, lane_change_prob):
+  def update(self, carstate, lateral_active, lane_change_prob, left_edge_detected=False, right_edge_detected=False):
     self.alc.update_params()
     self.lane_turn_controller.update_params()
     v_ego = carstate.vEgo
@@ -64,8 +64,8 @@ class DesireHelper:
                          ((carstate.steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or
                           (carstate.steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right))
 
-        blindspot_detected = ((carstate.leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or
-                              (carstate.rightBlindspot and self.lane_change_direction == LaneChangeDirection.right))
+        blindspot_detected = (((carstate.leftBlindspot or left_edge_detected) and self.lane_change_direction == LaneChangeDirection.left) or
+                              ((carstate.rightBlindspot or right_edge_detected) and self.lane_change_direction == LaneChangeDirection.right))
 
         self.alc.update_lane_change(blindspot_detected, carstate.brakePressed)
 

openpilot/selfdrive/controls/lib/longcontrol.py

@@ -4,6 +4,7 @@ from openpilot.common.realtime import DT_CTRL
 from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
 from openpilot.common.pid import PIDController
 from openpilot.selfdrive.modeld.constants import ModelConstants
+from openpilot.sunnypilot.selfdrive.controls.lib.longcontrol import LongControlSP
 
 CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
 
@@ -39,7 +40,7 @@ def long_control_state_trans(CP_SP, active, long_control_state,
 
   return long_control_state
 
-class LongControl:
+class LongControl(LongControlSP):
   def __init__(self, CP, CP_SP):
     self.CP = CP
     self.CP_SP = CP_SP
@@ -66,7 +67,7 @@ class LongControl:
 
     elif self.long_control_state == LongCtrlState.stopping:
       output_accel = self.last_output_accel
-      if output_accel > self.CP.stopAccel:
+      if output_accel > self.CP.stopAccel and not LongControlSP.should_hold_stopping(self, CS, a_target):
         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

openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -9,6 +9,7 @@ from openpilot.common.swaglog import cloudlog
 # WARNING: imports outside of constants will not trigger a rebuild
 from openpilot.selfdrive.modeld.constants import index_function
 from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
+from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpcSP
 
 if __name__ == '__main__':  # generating code
   from acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
@@ -213,8 +214,9 @@ def gen_long_ocp():
   return ocp
 
 
-class LongitudinalMpc:
+class LongitudinalMpc(LongitudinalMpcSP):
   def __init__(self, dt=DT_MDL):
+    LongitudinalMpcSP.__init__(self)
     self.dt = dt
     self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
     self.reset()
@@ -266,7 +268,8 @@ class LongitudinalMpc:
   def set_weights(self, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard):
     jerk_factor = get_jerk_factor(personality)
     a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0
-    cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * a_change_cost, jerk_factor * J_EGO_COST]
+    cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * a_change_cost,
+                    LongitudinalMpcSP.scale_jerk_cost(self, jerk_factor * J_EGO_COST)]
     constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST]
     self.set_cost_weights(cost_weights, constraint_cost_weights)
 
@@ -326,7 +329,7 @@ class LongitudinalMpc:
     # when the leads are no factor.
     v_lower = v_ego + (T_IDXS * CRUISE_MIN_ACCEL * 1.05)
     # TODO does this make sense when max_a is negative?
-    v_upper = v_ego + (T_IDXS * CRUISE_MAX_ACCEL * 1.05)
+    v_upper = v_ego + (T_IDXS * self.cruise_accel_max(CRUISE_MAX_ACCEL) * 1.05)
     v_cruise_clipped = np.clip(v_cruise * np.ones(N+1), v_lower, v_upper)
     cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, t_follow)
 
@@ -340,6 +343,7 @@ class LongitudinalMpc:
 
     self.params[:,0] = ACCEL_MIN
     self.params[:,1] = ACCEL_MAX
+    LongitudinalMpcSP.apply_accel_limits(self)
     self.params[:,2] = np.min(x_obstacles, axis=1)
     self.params[:,3] = np.copy(self.a_prev)
     self.params[:,4] = t_follow
@@ -359,6 +363,7 @@ class LongitudinalMpc:
     self.solver.constraints_set(0, "ubx", self.x0)
 
     self.solution_status = self.solver.solve()
+    LongitudinalMpcSP.save_solution_status(self)
     self.solve_time = float(self.solver.get_stats('time_tot')[0])
 
     for i in range(N+1):

openpilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -51,7 +51,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
   def __init__(self, CP, CP_SP, init_v=0.0, init_a=0.0, dt=DT_MDL):
     self.CP = CP
     self.mpc = LongitudinalMpc(dt=dt)
-    LongitudinalPlannerSP.__init__(self, self.CP, CP_SP, self.mpc)
+    LongitudinalPlannerSP.__init__(self, self.CP, CP_SP, self.mpc, dt=dt)
     self.fcw = False
     self.dt = dt
     self.allow_throttle = True
@@ -110,13 +110,13 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
       clipped_accel_coast = max(accel_coast, accel_clip[0])
       clipped_accel_coast_interp = np.interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [accel_clip[1], clipped_accel_coast])
       accel_clip[1] = min(accel_clip[1], clipped_accel_coast_interp)
-
     # Get new v_cruise and a_desired from Smart Cruise Control and Speed Limit Assist
     v_cruise, self.a_desired = LongitudinalPlannerSP.update_targets(self, sm, self.v_desired_filter.x, self.a_desired, v_cruise)
-
     if force_slow_decel:
       v_cruise = 0.0
 
+    is_e2e, v_cruise = LongitudinalPlannerSP.update_accel_controller(self, sm, v_cruise, prev_accel_constraint, accel_clip[1], reset_state)
+
     self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality)
     self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
     self.mpc.update(sm['radarState'], v_cruise, personality=sm['selfdriveState'].personality)
@@ -135,13 +135,13 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     self.a_desired = float(np.interp(self.dt, CONTROL_N_T_IDX, self.a_desired_trajectory))
     self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.a_desired + a_prev) / 2.0
 
-    action_t =  self.CP.longitudinalActuatorDelay + DT_MDL
+    action_t = self.CP.longitudinalActuatorDelay + DT_MDL
     output_a_target_mpc, output_should_stop_mpc = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX,
                                                                         action_t=action_t)
     output_a_target_e2e = sm['modelV2'].action.desiredAcceleration
     output_should_stop_e2e = sm['modelV2'].action.shouldStop
 
-    if self.is_e2e(sm):
+    if is_e2e:
       output_a_target = min(output_a_target_e2e, output_a_target_mpc)
       self.output_should_stop = output_should_stop_e2e or output_should_stop_mpc
       if output_a_target < output_a_target_mpc:
@@ -149,6 +149,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     else:
       output_a_target = output_a_target_mpc
       self.output_should_stop = output_should_stop_mpc
+    self.output_should_stop = LongitudinalPlannerSP.update_should_stop(self, self.output_should_stop)
 
     for idx in range(2):
       accel_clip[idx] = np.clip(accel_clip[idx], self.prev_accel_clip[idx] - 0.05, self.prev_accel_clip[idx] + 0.05)

openpilot/selfdrive/modeld/compile_modeld.py

@@ -3,6 +3,7 @@ import argparse
 import atexit
 import math
 import os
+import pickle
 import tempfile
 import time
 import shutil
@@ -12,6 +13,7 @@ from collections import namedtuple
 import numpy as np
 
 from openpilot.selfdrive.modeld.helpers import dump_oob, load_oob
+from openpilot.selfdrive.modeld.usbgpu_link import wait_usbgpu_link
 
 def _patch_tinygrad_fetch_fw():
   import hashlib
@@ -29,6 +31,22 @@ def _patch_tinygrad_fetch_fw():
   helpers.fetch_fw = fetch_fw
 _patch_tinygrad_fetch_fw()
 
+def _patch_tinygrad_buffer_reduce():
+  from tinygrad.device import Buffer
+  def __reduce_ex__(self, protocol):
+    buf = None
+    if self._base is not None:
+      return self.__class__, (self.device, self.size, self.dtype, None, None, None, 0, self.base, self.offset, self.is_allocated())
+    if self.device == "NPY":
+      return self.__class__, (self.device, self.size, self.dtype, self._buf, self.options, None, self.uop_refcount)
+    if self.is_allocated():
+      buf = bytearray(self.nbytes)
+      self.copyout(memoryview(buf))
+      if protocol >= 5:
+        buf = pickle.PickleBuffer(buf)
+    return self.__class__, (self.device, self.size, self.dtype, None, self.options, buf, self.uop_refcount)
+  Buffer.__reduce_ex__ = __reduce_ex__
+_patch_tinygrad_buffer_reduce()
 
 from tinygrad.tensor import Tensor
 from tinygrad.helpers import Context
@@ -294,6 +312,9 @@ if __name__ == "__main__":
   p.add_argument('--frame-skip', type=int, required=True)
   args = p.parse_args()
 
+  if 'USB+AMD' in os.environ.get('DEV', ''):
+    wait_usbgpu_link()
+
   model_path = read_file_chunked_to_disk(args.onnx)
   model_w, model_h = args.model_size
 

openpilot/selfdrive/modeld/helpers.py

@@ -6,11 +6,10 @@ import struct
 import tempfile
 from pathlib import Path
 
-from openpilot.common.file_chunker import get_manifest_path
-from openpilot.common.hardware.usb import CHESTNUT_VENDOR_ID, CHESTNUT_PRODUCT_ID, USB_DEVICES_PATH
-
 MODELS_DIR = Path(__file__).resolve().parent / 'models'
 TG_INPUT_DEVICES_PATH = MODELS_DIR / 'tg_input_devices.json'
+USBGPU_VID = 0xADD1
+USBGPU_PID = 0x0001
 
 
 def get_tg_input_devices(process_name: str, usbgpu: bool):
@@ -39,21 +38,22 @@ def dump_oob(obj, f):
 def load_oob(f):
   opcodes = f.read(struct.unpack('<q', f.read(8))[0])
   def buffers():
+    prev = None
     while (h := f.read(8)):
-      pb = pickle.PickleBuffer(bytearray(struct.unpack('<q', h)[0]))
-      f.readinto(pb)
-      yield pb
+      if prev is not None:
+        prev.release()
+      buf = bytearray(struct.unpack('<q', h)[0])
+      f.readinto(buf)
+      prev = pickle.PickleBuffer(buf)
+      yield prev
   return pickle.load(io.BytesIO(opcodes), buffers=buffers())
 
 def usbgpu_present() -> bool:
-  for d in USB_DEVICES_PATH.glob("*"):
+  for d in Path("/sys/bus/usb/devices").glob("*"):
     try:
-      usb_id = (int((d / "idVendor").read_text(), 16), int((d / "idProduct").read_text(), 16))
-      if usb_id == (CHESTNUT_VENDOR_ID, CHESTNUT_PRODUCT_ID):
+      if int((d / "idVendor").read_text(), 16) == USBGPU_VID and \
+          int((d / "idProduct").read_text(), 16) == USBGPU_PID:
         return True
     except Exception:
       pass
   return False
-
-def usbgpu_compiled() -> bool:
-  return Path(get_manifest_path(modeld_pkl_path(usbgpu=True))).is_file()

openpilot/selfdrive/modeld/modeld.py

@@ -2,7 +2,6 @@
 import os
 os.environ['GMMU'] = '0' # for usbgpu fast loading, noop for qcom
 from tinygrad.tensor import Tensor
-import threading
 import time
 import numpy as np
 import openpilot.cereal.messaging as messaging
@@ -19,13 +18,17 @@ from openpilot.common.transformations.camera import DEVICE_CAMERAS
 from openpilot.system.camerad.cameras.nv12_info import get_nv12_info
 from openpilot.common.transformations.model import get_warp_matrix
 from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
-from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, should_stop, smooth_value, get_curvature_from_plan
+from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, smooth_value, get_curvature_from_plan
 from openpilot.selfdrive.modeld.parse_model_outputs import Parser
 from openpilot.selfdrive.modeld.compile_modeld import make_input_queues, WARP_INPUTS, POLICY_INPUTS
 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.common.file_chunker import open_file_chunked, get_manifest_path
 from openpilot.selfdrive.modeld.constants import ModelConstants, Plan
-from openpilot.selfdrive.modeld.helpers import usbgpu_present, usbgpu_compiled, modeld_pkl_path, get_tg_input_devices, load_oob
+from openpilot.selfdrive.modeld.helpers import usbgpu_present, modeld_pkl_path, get_tg_input_devices, load_oob
+from openpilot.selfdrive.modeld.usbgpu_link import wait_usbgpu_link
+
+from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
+from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
 
 PROCESS_NAME = "openpilot.selfdrive.modeld.modeld"
 SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
@@ -33,17 +36,16 @@ SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
 LAT_SMOOTH_SECONDS = 0.0
 LONG_SMOOTH_SECONDS = 0.3
 MIN_LAT_CONTROL_SPEED = 0.3
-BIG_MODEL_TIMEOUT = 60
 
 
 def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action,
                           lat_action_t: float, long_action_t: float, v_ego: float) -> log.ModelDataV2.Action:
   if 'action' not in model_output:
     plan = model_output['plan'][0]
-    desired_accel = get_accel_from_plan(plan[:,Plan.VELOCITY][:,0],
-                                        plan[:,Plan.ACCELERATION][:,0],
-                                        ModelConstants.T_IDXS,
-                                        action_t=long_action_t)
+    desired_accel, should_stop = get_accel_from_plan(plan[:,Plan.VELOCITY][:,0],
+                                                     plan[:,Plan.ACCELERATION][:,0],
+                                                     ModelConstants.T_IDXS,
+                                                     action_t=long_action_t)
     desired_curvature = get_curvature_from_plan(plan[:,Plan.T_FROM_CURRENT_EULER][:,2],
                                                 plan[:,Plan.ORIENTATION_RATE][:,2],
                                                 ModelConstants.T_IDXS,
@@ -52,7 +54,7 @@ def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.
   else:
     desired_accel = model_output['action'][0,1]
     desired_curvature = model_output['action'][0,0] / (max(1.0, v_ego))**2
-  stop = should_stop(v_ego, desired_accel)
+    should_stop = (v_ego < 0.3 and desired_accel < 0.1)
   desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, LONG_SMOOTH_SECONDS)
   if v_ego > MIN_LAT_CONTROL_SPEED:
     desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, LAT_SMOOTH_SECONDS)
@@ -61,7 +63,7 @@ def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.
 
   return log.ModelDataV2.Action(desiredCurvature=float(desired_curvature),
                                 desiredAcceleration=float(desired_accel),
-                                shouldStop=bool(stop))
+                                shouldStop=bool(should_stop))
 
 
 class FrameMeta:
@@ -74,10 +76,12 @@ class FrameMeta:
       self.frame_id, self.timestamp_sof, self.timestamp_eof = vipc.frame_id, vipc.timestamp_sof, vipc.timestamp_eof
 
 
-class ModelState:
+class ModelState(ModelStateBase):
   prev_desire: np.ndarray  # for tracking the rising edge of the pulse
 
   def __init__(self, cam_w: int, cam_h: int, usbgpu: bool):
+    ModelStateBase.__init__(self)
+    self.LAT_SMOOTH_SECONDS = LAT_SMOOTH_SECONDS
     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(usbgpu)))
@@ -134,24 +138,16 @@ class ModelState:
       outputs_dict['raw_pred'] = model_output.copy()
     return outputs_dict
 
-  def warmup(self) -> None:
-    dummy_frames = {k: np.zeros(self.frame_buf_params[k][3], dtype=np.uint8) for k in self.vision_input_names}
-    eye = np.eye(3, dtype=np.float32)
-    dims = {'desire_pulse': ModelConstants.DESIRE_LEN, 'traffic_convention': 2, 'action_t': 2}
-    self.run(dummy_frames, dict.fromkeys(self.vision_input_names, eye), {k: np.zeros(v, dtype=np.float32) for k, v in dims.items()})
-    self.input_queues, self.npy = make_input_queues(self.input_shapes, self.frame_skip, device=self.QUEUE_DEV)
-    self.prev_desire[:] = 0
-    self.full_frames.clear()
-    self._blob_cache.clear()
-
 
 def main(demo=False):
   cloudlog.warning("modeld init")
 
-  USBGPU = usbgpu_present() and usbgpu_compiled()
+  _present = usbgpu_present()
+  _compiled = os.path.isfile(get_manifest_path(modeld_pkl_path(usbgpu=True)))
+  USBGPU = _present and _compiled
   params = Params()
-  params.put_bool("UsbGpuLoading", USBGPU)
-  params.remove("UsbGpuActive")
+  params.put_bool("UsbGpuPresent", _present)
+  params.put_bool("UsbGpuCompiled", _compiled)
 
   config_realtime_process(7, 54)
 
@@ -178,33 +174,15 @@ def main(demo=False):
   if use_extra_client:
     cloudlog.warning(f"connected extra cam with buffer size: {vipc_client_extra.buffer_len} ({vipc_client_extra.width} x {vipc_client_extra.height})")
 
+  if USBGPU:
+    wait_usbgpu_link()
   st = time.monotonic()
   cloudlog.warning("loading model")
-  model = None
-  if USBGPU:
-    big_model = None
-    def load_big():
-      nonlocal big_model
-      try:
-        m = ModelState(vipc_client_main.width, vipc_client_main.height, True)
-        m.warmup()
-        big_model = m
-      except Exception:
-        cloudlog.exception("big model load failed")
-    loader = threading.Thread(target=load_big, daemon=True)
-    loader.start()
-    loader.join(BIG_MODEL_TIMEOUT)
-    model = big_model
-    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 USBGPU else None
-  if model is None:
-    model = small_model
-  params.put_bool("UsbGpuLoading", False)
+  model = ModelState(vipc_client_main.width, vipc_client_main.height, USBGPU)
   cloudlog.warning(f"models loaded in {time.monotonic() - st:.1f}s, modeld starting")
 
   # messaging
-  pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry"])
+  pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry", "modelDataV2SP"])
   sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl", "liveDelay"])
 
   publish_state = PublishState()
@@ -274,7 +252,9 @@ def main(demo=False):
     is_rhd = sm["driverMonitoringState"].isRHD
     frame_id = sm["roadCameraState"].frameId
     v_ego = max(sm["carState"].vEgo, 0.)
-    lat_delay = sm["liveDelay"].lateralDelay + LAT_SMOOTH_SECONDS
+    if sm.frame % 60 == 0:
+      model.lat_delay = get_lat_delay(params, sm["liveDelay"].lateralDelay)
+    lat_delay = model.lat_delay + LAT_SMOOTH_SECONDS
     if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']:
       device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32)
       dc = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['roadCameraState'].sensor))]
@@ -313,17 +293,7 @@ def main(demo=False):
     }
 
     mt1 = time.perf_counter()
-    try:
-      model_output = model.run(bufs, transforms, inputs)
-    except Exception:
-      if not params.get_bool("UsbGpuActive"):
-        raise
-      # fallback to small model
-      cloudlog.exception("big model failed, fall back to small")
-      params.put_bool("UsbGpuActive", False)
-      model = small_model
-      run_count = 0
-      model_output = None
+    model_output = model.run(bufs, transforms, inputs)
     mt2 = time.perf_counter()
     model_execution_time = mt2 - mt1
 
@@ -331,6 +301,7 @@ def main(demo=False):
       modelv2_send = messaging.new_message('modelV2')
       drivingdata_send = messaging.new_message('drivingModelData')
       posenet_send = messaging.new_message('cameraOdometry')
+      mdv2sp_send = messaging.new_message('modelDataV2SP')
 
       action = get_action_from_model(model_output, prev_action, lat_action_t, long_action_t, v_ego)
       prev_action = action
@@ -345,12 +316,14 @@ def main(demo=False):
       DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
       modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
       modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
+      mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_direction
 
       fill_driving_model_data(drivingdata_send, modelv2_send)
       fill_pose_msg(posenet_send, model_output, meta_main.frame_id, vipc_dropped_frames, meta_main.timestamp_eof, live_calib_seen)
       pm.send('modelV2', modelv2_send)
       pm.send('drivingModelData', drivingdata_send)
       pm.send('cameraOdometry', posenet_send)
+      pm.send('modelDataV2SP', mdv2sp_send)
     last_vipc_frame_id = meta_main.frame_id
 
 

openpilot/selfdrive/selfdrived/selfdrived.py

@@ -327,9 +327,16 @@ class SelfdriveD(CruiseHelper):
     # Handle lane change
     if self.sm['modelV2'].meta.laneChangeState == LaneChangeState.preLaneChange:
       direction = self.sm['modelV2'].meta.laneChangeDirection
+      mdv2sp = self.sm['modelDataV2SP']
+
       if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \
-         (CS.rightBlindspot and direction == LaneChangeDirection.right):
+            (CS.rightBlindspot and direction == LaneChangeDirection.right):
         self.events.add(EventName.laneChangeBlocked)
+
+      elif (mdv2sp.leftLaneChangeEdgeBlock and direction == LaneChangeDirection.left) or \
+           (mdv2sp.rightLaneChangeEdgeBlock and direction == LaneChangeDirection.right):
+        self.events_sp.add(custom.OnroadEventSP.EventName.laneChangeRoadEdge)
+
       else:
         if direction == LaneChangeDirection.left:
           self.events.add(EventName.preLaneChangeLeft)

openpilot/selfdrive/test/longitudinal_maneuvers/plant.py

@@ -11,6 +11,14 @@ 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.logMonoTime = {"radarState": radar_frame}
+    self.valid = {"radarState": True}
+    self.alive = {"radarState": True}
+
+
 class Plant:
   messaging_initialized = False
 
@@ -132,7 +140,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 +149,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(
+    "Begin slowing early and smoothly behind lead vehicles. Stock longitudinal control retains braking and stopping authority."
+  ),
+  "AccelPersonality": tr_noop(
+    "Eco slows earliest and recovers gently, Normal balances comfort and response, and Sport reacts and recovers more quickly."
+  ),
   "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_personality_enabled = toggle_item(
+      lambda: tr("Enable Accel Controller"),
+      lambda: tr(DESCRIPTIONS["AccelPersonalityEnabled"]),
+      self._params.get_bool("AccelPersonalityEnabled"),
+      callback=self._set_accel_personality_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_personality_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_personality_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_personality_enabled.action_item.set_enabled(True)
+        self._accel_personality_setting.action_item.set_enabled(accel_personality_enabled)
       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_personality_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,10 @@ 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_personality_enabled.action_item.set_state(accel_personality_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 +282,10 @@ 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_personality_enabled(self, state: bool):
+    self._params.put_bool("AccelPersonalityEnabled", state, block=True)
+    self._accel_personality_setting.action_item.set_enabled(state and ui_state.has_longitudinal_control)

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

@@ -13,6 +13,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.onroad import OnroadViewContainerSP as AugmentedRoadView
 
 ONROAD_DELAY = 2.5  # seconds
 
@@ -118,13 +119,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

@@ -14,6 +14,8 @@ class TogglesLayoutMici(NavScroller):
     super().__init__()
 
     self._personality_toggle = BigMultiParamToggle("driving personality", "LongitudinalPersonality", ["aggressive", "standard", "relaxed"])
+    self._accel_personality_enabled = BigParamControl("enable accel controller", "AccelPersonalityEnabled")
+    self._accel_personality_toggle = BigMultiParamToggle("acceleration profile", "AccelPersonality", ["eco", "normal", "sport"])
     self._experimental_btn = BigParamControl("experimental mode", "ExperimentalMode")
     is_metric_toggle = BigParamControl("use metric units", "IsMetric")
     ldw_toggle = BigParamControl("lane departure warnings", "IsLdwEnabled")
@@ -24,6 +26,8 @@ class TogglesLayoutMici(NavScroller):
 
     self._scroller.add_widgets([
       self._personality_toggle,
+      self._accel_personality_enabled,
+      self._accel_personality_toggle,
       self._experimental_btn,
       is_metric_toggle,
       ldw_toggle,
@@ -36,6 +40,7 @@ class TogglesLayoutMici(NavScroller):
     # Toggle lists
     self._refresh_toggles = (
       ("ExperimentalMode", self._experimental_btn),
+      ("AccelPersonalityEnabled", self._accel_personality_enabled),
       ("IsMetric", is_metric_toggle),
       ("IsLdwEnabled", ldw_toggle),
       ("AlwaysOnDM", always_on_dm_toggle),
@@ -45,6 +50,9 @@ class TogglesLayoutMici(NavScroller):
     )
 
     enable_openpilot.set_enabled(lambda: not ui_state.engaged)
+    self._accel_personality_toggle.set_enabled(
+      lambda: ui_state.has_longitudinal_control and ui_state.params.get_bool("AccelPersonalityEnabled")
+    )
     record_front.set_enabled(False if ui_state.params.get_bool("RecordFrontLock") else (lambda: not ui_state.engaged))
     record_mic.set_enabled(lambda: not ui_state.engaged)
 
@@ -75,13 +83,18 @@ class TogglesLayoutMici(NavScroller):
       if ui_state.has_longitudinal_control:
         self._experimental_btn.set_visible(True)
         self._personality_toggle.set_visible(True)
+        self._accel_personality_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_personality_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()

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/steering_sub_layouts/lane_change_settings.py

@@ -51,11 +51,17 @@ class LaneChangeSettingsLayout(Widget):
       description=lambda: tr("Toggle to enable a delay timer for seamless lane changes when blind spot monitoring " +
                              "(BSM) detects a obstructing vehicle, ensuring safe maneuvering."),
     )
+    self._road_edge_block = toggle_item_sp(
+      param="RoadEdgeLaneChangeEnabled",
+      title=lambda: tr("Block Lane Change: Road Edge Detection"),
+      description=lambda: tr("Blocks the lane change if the model sees a road edge on your signaled side."),
+    )
 
     items = [
       self._lane_change_timer,
       LineSeparatorSP(40),
       self._bsm_delay,
+      self._road_edge_block,
     ]
 
     return items

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

@@ -0,0 +1,13 @@
+"""
+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
+
+
+class MiciMainLayoutSP(MiciMainLayout):
+  def _should_auto_scroll_to_onroad(self) -> bool:
+    return not self._onroad_layout.is_on_info_panel()

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

@@ -0,0 +1,63 @@
+"""
+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 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, callback) -> None:
+    self.road_view.set_click_callback(callback)
+    self.onroad_info_panel.set_click_callback(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,324 @@
+"""
+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
+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
+from openpilot.system.ui.lib.multilang import tr
+from openpilot.system.ui.lib.text_measure import measure_text_cached
+from openpilot.system.ui.lib.application import MousePos
+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
+
+
+@dataclass(frozen=True)
+class OnroadInfoPanelColors:
+  white: rl.Color = rl.WHITE
+  black: rl.Color = rl.BLACK
+  red: rl.Color = rl.Color(255, 0, 0, 255)
+  green: rl.Color = rl.Color(0, 255, 0, 255)
+  grey: rl.Color = rl.Color(190, 195, 190, 255)
+  light_grey: rl.Color = rl.Color(200, 200, 200, 255)
+  dark_grey: rl.Color = rl.Color(100, 100, 100, 255)
+  bg_dark: rl.Color = rl.Color(0, 0, 0, 255)
+  card_bg: rl.Color = rl.Color(50, 50, 50, 200)
+  badge_bg: rl.Color = 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)
+    margin = 20
+    mid_y = rect.y + rect.height / 2
+
+    left_x = rect.x + 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
+
+    speed_val = str(round(display_speed))
+    if self.speed_limit_valid and display_speed > self.speed_limit:
+      speed_color = COLORS.red
+    else:
+      speed_color = COLORS.white
+
+    rl.draw_text_ex(self._font_semi_bold, unit, rl.Vector2(left_x, mid_y - 95), 38, 0, COLORS.grey)
+    rl.draw_text_ex(self._font_bold, speed_val, rl.Vector2(left_x, mid_y - 60), 110, 0, speed_color)
+
+    sign_width = 135
+    sign_height = 135 if ui_state.is_metric else 175
+
+    has_next = self.next_speed_limit > 0 and self.next_speed_limit != self.speed_limit
+    target_slide = 1.0 if has_next else 0.0
+    slide_speed = 3.0 * rl.get_frame_time()
+    if self._sign_slide < target_slide:
+      self._sign_slide = min(self._sign_slide + slide_speed, target_slide)
+    elif self._sign_slide > target_slide:
+      self._sign_slide = max(self._sign_slide - slide_speed, target_slide)
+
+    next_w = int(sign_width * 0.7)
+    next_h = int(sign_height * 0.7)
+    next_peek = int(next_w * 0.85) + 5
+    centered_x = rect.x + rect.width - sign_width - margin
+    shifted_x = rect.x + rect.width - sign_width - margin - next_peek
+    sign_x = centered_x + (shifted_x - centered_x) * self._sign_slide
+    sign_y = rect.y + (rect.height - sign_height) / 2
+
+    road_y = mid_y + 55
+    road_width = sign_x - left_x - margin
+    self._draw_road_name(left_x, road_y, road_width)
+
+    if has_next and self._sign_slide > 0.01:
+      next_val = str(round(self.next_speed_limit))
+      dist_str = self._format_distance(self.next_speed_limit_distance)
+      next_x = sign_x + sign_width - int(next_w * 0.15)
+      next_y = sign_y + (sign_height - next_h) / 2
+
+      next_speed_color = COLORS.black
+      if ui_state.is_metric:
+        self._draw_vienna_sign(next_x, next_y, next_w, next_h, next_val, next_speed_color, is_upcoming=True)
+      else:
+        self._draw_mutcd_sign(next_x, next_y, next_w, next_h, next_val, next_speed_color, is_upcoming=True)
+
+      dist_size = measure_text_cached(self._font_medium, dist_str, 24)
+      rl.draw_text_ex(self._font_medium, dist_str, rl.Vector2(next_x + next_w / 2 - dist_size.x / 2, next_y + next_h + 4), 24, 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_val = str(abs(round(self.speed_limit_offset)))
+      badge_sz = 42
+      badge_x = sign_x + sign_width - badge_sz * 0.85
+      badge_y = sign_y - badge_sz * 0.25
+
+      if ui_state.is_metric:
+        badge_r = badge_sz / 2
+        badge_cx = badge_x + badge_r
+        badge_cy = badge_y + badge_r
+        rl.draw_circle(int(badge_cx), int(badge_cy), badge_r + 2, COLORS.dark_grey)
+        rl.draw_circle(int(badge_cx), int(badge_cy), badge_r, COLORS.badge_bg)
+        self._draw_text_centered(self._font_bold, offset_val, 24, rl.Vector2(badge_cx, badge_cy), COLORS.white)
+      else:
+        mutcd_badge_x = sign_x + sign_width - badge_sz * 0.65
+        mutcd_badge_y = sign_y - badge_sz * 0.50
+        badge_rect = rl.Rectangle(mutcd_badge_x, mutcd_badge_y, badge_sz, badge_sz)
+        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(self._font_bold, offset_val, 24, rl.Vector2(mutcd_badge_x + badge_sz / 2, mutcd_badge_y + badge_sz / 2), COLORS.white)
+
+    # SCC
+    speed_size = measure_text_cached(self._font_bold, speed_val, 110)
+    scc_x = left_x + speed_size.x + 30
+    scc_y = mid_y - 50
+    self._draw_scc_icons(scc_x, scc_y)
+
+    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) -> None:
+    sm = ui_state.sm
+    if not sm.valid["longitudinalPlanSP"]:
+      return
+    scc = sm["longitudinalPlanSP"].smartCruiseControl
+
+    box_w, box_h = 100, 36
+    gap = 6
+    drawn = 0
+
+    for label, active in [("SCC-V", scc.vision.active), ("SCC-M", scc.map.active)]:
+      if not active:
+        continue
+      bx = x
+      by = y + drawn * (box_h + gap)
+      rl.draw_rectangle_rounded(rl.Rectangle(bx, by, box_w, box_h), 0.3, 10, COLORS.green)
+      self._draw_text_centered(self._font_bold, label, 20, rl.Vector2(bx + box_w / 2, by + box_h / 2), COLORS.black)
+      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:
+    road_display = self.road_name if self.road_name else "--"
+    font_size = 30
+    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)
+    text_size = measure_text_cached(self._font_bold, speed_str, int(font_size))
+    text_pos = rl.Vector2(center.x - text_size.x / 2, center.y - text_size.y / 2)
+    rl.draw_text_ex(self._font_bold, speed_str, text_pos, font_size, 0, speed_color)
+
+  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(self._font_bold, tr("AHEAD"), label_size, rl.Vector2(mid_x, y + height * 0.27), COLORS.black)
+    else:
+      self._draw_text_centered(self._font_bold, tr("SPEED"), label_size, rl.Vector2(mid_x, y + height * 0.20), COLORS.black)
+      self._draw_text_centered(self._font_bold, tr("LIMIT"), label_size, rl.Vector2(mid_x, y + height * 0.40), COLORS.black)
+
+    speed_font_size = int(width * 0.52) if len(speed_str) >= 3 else int(width * 0.62)
+    self._draw_text_centered(self._font_bold, speed_str, speed_font_size, rl.Vector2(mid_x, y + height * 0.72), speed_color)
+
+  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 _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,30 @@
+"""
+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.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, rect: rl.Rectangle) -> None:
+    super()._render(rect)
+    if self._show_confidence_ball:
+      self._real_confidence_ball.render(self.rect)

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

@@ -0,0 +1,34 @@
+"""
+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):
+  """Reject orthogonal-dominant drags so nested scrollers (outer horizontal +
+  inner vertical) don't both engage on a slightly diagonal swipe.
+
+  Implemented as a post-super state rollback rather than reimplementing the
+  PRESSED state machine — keeps stock behaviour authoritative."""
+
+  def _handle_mouse_event(self, mouse_event: MouseEvent, bounds: rl.Rectangle, bounds_size: float,
+                          content_size: float) -> None:
+    pre_state = self._state
+    super()._handle_mouse_event(mouse_event, bounds, bounds_size, content_size)
+
+    if self._state == ScrollState.MANUAL_SCROLL and pre_state == ScrollState.PRESSED and \
+       self._initial_click_event is not None:
+      diff_x = abs(mouse_event.pos.x - self._initial_click_event.pos.x)
+      diff_y = abs(mouse_event.pos.y - self._initial_click_event.pos.y)
+      along = diff_x if self._horizontal else diff_y
+      anti = diff_y if self._horizontal else diff_x
+      if anti > along:
+        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/ui.py

@@ -10,6 +10,9 @@ from openpilot.selfdrive.ui.layouts.main import MainLayout
 from openpilot.selfdrive.ui.mici.layouts.main import MiciMainLayout
 from openpilot.selfdrive.ui.ui_state import ui_state
 
+if gui_app.sunnypilot_ui():
+  from openpilot.selfdrive.ui.sunnypilot.mici.layouts.main import MiciMainLayoutSP as MiciMainLayout
+
 BIG_UI = gui_app.big_ui()
 
 

openpilot/sunnypilot/modeld_v2/SConscript

@@ -1,12 +1,9 @@
 import os
 import glob
-import sys
-import subprocess
 
 from openpilot.common.transformations.camera import _ar_ox_fisheye, _os_fisheye
 from openpilot.common.transformations.model import MEDMODEL_INPUT_SIZE
 from openpilot.common.hardware import HARDWARE, PC
-from openpilot.selfdrive.modeld.helpers import usbgpu_present
 
 Import('env', 'arch', 'release')
 lenv = env.Clone()
@@ -25,21 +22,14 @@ def get_camera_configs():
 
 CAMERA_CONFIGS = get_camera_configs()
 
-def probe_devices():
-  return set(subprocess.run(
-    [sys.executable, '-c', 'from tinygrad import Device\nprint("\\n".join(Device.get_available_devices()))'],
-    capture_output=True, text=True, check=True).stdout.strip().splitlines())
+tg_flags = {
+  'larch64': 'DEV=QCOM FLOAT16=1 NOLOCALS=1 JIT_BATCH_SIZE=0',
+  'Darwin': f'DEV=CPU HOME={os.path.expanduser("~")}',
+}.get(arch, 'DEV=CPU:LLVM')
 
-available = probe_devices()
-if 'CUDA' in available:
-  tg_backend = 'CUDA'
-  tg_flags = f'DEV={tg_backend}'
-elif 'QCOM' in available:
-  tg_backend = 'QCOM'
-  tg_flags = f'DEV={tg_backend} IMAGE=1 FLOAT16=1 NOLOCALS=1 JIT_BATCH_SIZE=0 OPENPILOT_HACKS=1'
-else:
-  tg_backend = 'CPU'
-  tg_flags = f'DEV=CPU HOME={os.path.expanduser("~")}' if arch == 'Darwin' else 'DEV=CPU:LLVM'
+image_flag = {
+  'larch64': 'IMAGE=2',
+}.get(arch, 'IMAGE=0')
 
 model_w, model_h = MEDMODEL_INPUT_SIZE
 from openpilot.selfdrive.modeld.constants import ModelConstants
@@ -51,30 +41,17 @@ compile_modeld_script = File("compile_modeld.py").abspath
 upstream_compile_script = File(Dir("#openpilot/selfdrive/modeld").File("compile_modeld.py").abspath)
 script_deps = [File("compile_modeld.py"), upstream_compile_script]
 
-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'
-  usbgpu_lock = File("models/.usb_gpu.lock").abspath
-
 def compile_combined(model_type, onnx_args, output_name):
-  for usbgpu in ([False, True] if USBGPU else [False]):
-    prefix = 'big_' if usbgpu else ('big_' if os.getenv('BIG_INTO_SMALL') else '')
-    final_output_name = prefix + output_name
-    output_pkl = File(f"models/{final_output_name}").abspath
-
-    active_tg_flags = usbgpu_tg_flags if usbgpu else tg_flags
-
-    cmd = (f'{pythonpath_string} {active_tg_flags} python3 {compile_modeld_script} '
-           f'--model-type {model_type} '
-           f'--model-size {model_w}x{model_h} '
-           f'--camera-resolutions {camera_res_args} '
-           f'{onnx_args} '
-           f'--frame-skip {frame_skip} '
-           f'--output {output_pkl}')
-    onnx_files = [f for f in onnx_args.split() if f.endswith('.onnx')]
-    node = lenv.Command(output_pkl, tinygrad_files + script_deps + [File(f) for f in onnx_files if os.path.isfile(f)], cmd)
-    if usbgpu:
-      lenv.SideEffect(usbgpu_lock, node)
+  output_pkl = File(f"models/{output_name}").abspath
+  cmd = (f'{pythonpath_string} {tg_flags} {image_flag} python3 {compile_modeld_script} '
+         f'--model-type {model_type} '
+         f'--model-size {model_w}x{model_h} '
+         f'--camera-resolutions {camera_res_args} '
+         f'{onnx_args} '
+         f'--frame-skip {frame_skip} '
+         f'--output {output_pkl}')
+  onnx_files = [f for f in onnx_args.split() if f.endswith('.onnx')]
+  return lenv.Command(output_pkl, tinygrad_files + script_deps + [File(f) for f in onnx_files if os.path.isfile(f)], cmd)
 
 # Vision + Policy (stock default model)
 vision_onnx = File("models/driving_vision.onnx").abspath

openpilot/sunnypilot/modeld_v2/compile_modeld.py

@@ -8,10 +8,10 @@ See the LICENSE.md file in the root directory for more details.
 
 import argparse
 import os
-import tempfile
+import pickle
+import time
 from collections import defaultdict
 from functools import partial
-from openpilot.selfdrive.modeld.helpers import dump_oob, load_oob
 import numpy as np
 os.environ['GMMU'] = '0'
 
@@ -76,7 +76,7 @@ def get_policy_npy_shapes(input_shapes: dict, is_supercombo: bool = False) -> tu
 
 
 def generate_queues_and_npy(input_shapes: dict, frame_skip: int, device: str = Device.DEFAULT,
-                            is_supercombo: bool = False) -> tuple[dict, dict]:
+                            is_supercombo: bool = False, use_packed: bool = True) -> tuple[dict, dict]:
   road_key, _ = _detect_vision_keys(input_shapes)
   if not road_key:
     raise ValueError("Vision road key missing from input shapes.")
@@ -92,44 +92,69 @@ def generate_queues_and_npy(input_shapes: dict, frame_skip: int, device: str = D
   desire_shape = input_shapes[desire_key]
   features_buffer = input_shapes.get('features_buffer')
 
-  npy_arrays = {
-    'tfm': np.zeros((3, 3), dtype=np.float32),
-    'big_tfm': np.zeros((3, 3), dtype=np.float32)
-  }
+  if use_packed:  # remove packed detection block after all models are recompiled
+    npy_arrays = {
+      'tfm': np.zeros((3, 3), dtype=np.float32),
+      'big_tfm': np.zeros((3, 3), dtype=np.float32)
+    }
 
-  shapes, sizes = get_policy_npy_shapes(input_shapes, is_supercombo=is_supercombo)
-  packed_npy_inputs = np.zeros(sum(sizes), dtype=np.float32)
+    shapes, sizes = get_policy_npy_shapes(input_shapes, is_supercombo=is_supercombo)
+    packed_npy_inputs = np.zeros(sum(sizes), dtype=np.float32)
 
-  split_indices = np.cumsum(sizes[:-1]) if len(sizes) > 1 else []
-  split_views = np.split(packed_npy_inputs, split_indices) if len(sizes) > 0 else []
-  for (k, s), v in zip(shapes.items(), split_views, strict=True):
-    npy_arrays[k] = v.reshape(s)
+    split_indices = np.cumsum(sizes[:-1]) if len(sizes) > 1 else []
+    split_views = np.split(packed_npy_inputs, split_indices) if len(sizes) > 0 else []
+    for (k, s), v in zip(shapes.items(), split_views, strict=True):
+      npy_arrays[k] = v.reshape(s)
 
-  queues = {
-    'img_q': Tensor(np.zeros(img_buf_shape, dtype=np.uint8), device=device).contiguous().realize(),
-    'big_img_q': Tensor(np.zeros(img_buf_shape, dtype=np.uint8), device=device).contiguous().realize(),
-    'desire_q': Tensor(np.zeros((frame_skip * desire_shape[1], desire_shape[0], desire_shape[2]),
-                  dtype=np.float32), device=device).contiguous().realize(),
-    'packed_npy_inputs': Tensor(packed_npy_inputs, device='NPY').realize(),
-  }
+    queues = {
+      'img_q': Tensor(np.zeros(img_buf_shape, dtype=np.uint8), device=device).contiguous().realize(),
+      'big_img_q': Tensor(np.zeros(img_buf_shape, dtype=np.uint8), device=device).contiguous().realize(),
+      'desire_q': Tensor(np.zeros((frame_skip * desire_shape[1], desire_shape[0], desire_shape[2]),
+                    dtype=np.float32), device=device).contiguous().realize(),
+      'packed_npy_inputs': Tensor(packed_npy_inputs, device='NPY').realize(),
+    }
 
-  if features_buffer:
-    queues['feat_q'] = Tensor(np.zeros((frame_skip * (features_buffer[1] - 1) + 1, features_buffer[0], features_buffer[2]),
-                       dtype=np.float32), device=device).contiguous().realize()
+    if features_buffer:
+      queues['feat_q'] = Tensor(np.zeros((frame_skip * (features_buffer[1] - 1) + 1, features_buffer[0], features_buffer[2]),
+                         dtype=np.float32), device=device).contiguous().realize()
 
-  queues.update({key: Tensor(value, device='NPY').realize() for key, value in npy_arrays.items() if key in ('tfm', 'big_tfm')})
+    queues.update({key: Tensor(value, device='NPY').realize() for key, value in npy_arrays.items() if key in ('tfm', 'big_tfm')})
+  else:
+    # TODO-SP: Remove legacy queuing fallback else block after all models are recompiled
+    npy_arrays = {
+      'desire': np.zeros(desire_shape[2], dtype=np.float32),
+      'tfm': np.zeros((3, 3), dtype=np.float32),
+      'big_tfm': np.zeros((3, 3), dtype=np.float32)
+    }
+
+    for key, shape in input_shapes.items():
+      if key not in npy_arrays and 'img' not in key and key not in ('features_buffer', desire_key):
+        npy_arrays[key] = np.zeros(shape, dtype=np.float32)
+
+    queues = {
+      'img_q': Tensor(np.zeros(img_buf_shape, dtype=np.uint8), device=device).contiguous().realize(),
+      'big_img_q': Tensor(np.zeros(img_buf_shape, dtype=np.uint8), device=device).contiguous().realize(),
+      'desire_q': Tensor(np.zeros((frame_skip * desire_shape[1], desire_shape[0], desire_shape[2]),
+                    dtype=np.float32), device=device).contiguous().realize()
+    }
+
+    if features_buffer:
+      queues['feat_q'] = Tensor(np.zeros((frame_skip * (features_buffer[1] - 1) + 1, features_buffer[0], features_buffer[2]),
+                         dtype=np.float32), device=device).contiguous().realize()
+
+    queues.update({key: Tensor(value, device='NPY').realize() for key, value in npy_arrays.items()})
 
   return queues, npy_arrays
 
 
 def make_split_input_queues(vision_input_shapes: dict, policy_input_shapes: dict,
-                            frame_skip: int, device: str = Device.DEFAULT) -> tuple[dict, dict]:
-  return generate_queues_and_npy({**vision_input_shapes, **policy_input_shapes}, frame_skip, device, is_supercombo=False)
+                            frame_skip: int, device: str = Device.DEFAULT, use_packed: bool = True) -> tuple[dict, dict]:
+  return generate_queues_and_npy({**vision_input_shapes, **policy_input_shapes}, frame_skip, device, is_supercombo=False, use_packed=use_packed)
 
 
 def make_supercombo_input_queues(input_shapes: dict, frame_skip: int,
-                                 device: str = Device.DEFAULT) -> tuple[dict, dict]:
-  return generate_queues_and_npy(input_shapes, frame_skip, device, is_supercombo=True)
+                                 device: str = Device.DEFAULT, use_packed: bool = True) -> tuple[dict, dict]:
+  return generate_queues_and_npy(input_shapes, frame_skip, device, is_supercombo=True, use_packed=use_packed)
 
 
 def create_jit_runner(vision_runner, policy_runners: list, nv12: NV12Frame, model_size: tuple[int, int],
@@ -208,34 +233,29 @@ def compile_and_warmup(nv12: NV12Frame, model_size: tuple[int, int], prepare_onl
     raise ValueError("Could not find vision, model, or policy metadata.")
 
   features_slice = feat_meta['output_slices']['hidden_state']
-  WARP_DEV = os.getenv('WARP_DEV', Device.DEFAULT)
+  WARP_DEV = 'CPU' if "USBGPU" in os.environ else Device.DEFAULT
 
   is_supercombo = vision_runner is None
   run_func = create_jit_runner(vision_runner, policy_runners, nv12, model_size, features_slice, frame_skip, all_shapes, prepare_only)
   run_jit = TinyJit(run_func, prune=True)
-  def run_once(seed):
-    queues, npy = generate_queues_and_npy(all_shapes, frame_skip, Device.DEFAULT, is_supercombo=is_supercombo)
-    rng = np.random.default_rng(seed)
-    frame = Tensor.randint(nv12.size, low=0, high=256, dtype=dtypes.uint8, device=WARP_DEV).realize()
-    big_frame = Tensor.randint(nv12.size, low=0, high=256, dtype=dtypes.uint8, device=WARP_DEV).realize()
-    for value in npy.values():
-      value[:] = rng.standard_normal(value.shape).astype(value.dtype)
-    Device.default.synchronize()
-    outs = run_jit(**queues, frame=frame, big_frame=big_frame)
-    Device.default.synchronize()
-    return [np.copy(value.numpy()) for value in (outs if isinstance(outs, tuple) else [outs])] if outs is not None else []
+  queues, npy_arrays = generate_queues_and_npy(all_shapes, frame_skip, Device.DEFAULT, is_supercombo=is_supercombo)
 
   for i in range(3):
-    run_once(42 + i)
+    rng = np.random.default_rng(42 + i)
+    frame = Tensor.randint(nv12.size, low=0, high=256, dtype=dtypes.uint8, device=WARP_DEV).realize()
+    big_frame = Tensor.randint(nv12.size, low=0, high=256, dtype=dtypes.uint8, device=WARP_DEV).realize()
+    for arr in npy_arrays.values():
+      arr[:] = rng.standard_normal(arr.shape).astype(arr.dtype)
 
-  if not prepare_only:
-    baseline = run_once(42)
-    with tempfile.TemporaryFile(dir=".") as f:
-      dump_oob(run_jit, f)
-      f.seek(0)
-      run_jit = load_oob(f)
-    assert all(np.array_equal(baseline, deserialized) for baseline, deserialized in zip(baseline, run_once(42), strict=True)), "OOB pickling regression"
-  return run_jit
+    Device.default.synchronize()
+    start_time = time.perf_counter()
+    run_jit(**queues, frame=frame, big_frame=big_frame)
+    mid_time = time.perf_counter()
+    Device.default.synchronize()
+    print(f"  [{i + 1}/3] enqueue {(mid_time - start_time) * 1e3:6.2f} ms -- total {(time.perf_counter() - start_time) * 1e3:6.2f} ms")
+
+  # TODO-SP: switch to dump_oob/load_oob on next full recompile of all models
+  return pickle.loads(pickle.dumps(run_jit)) if not prepare_only else run_jit
 
 
 def _parse_size(size_str: str) -> tuple[int, int]:
@@ -332,7 +352,8 @@ if __name__ == "__main__":
                         vision_runner, policy_runners, output_data['metadata']))
 
   with open(args.output, "wb") as file:
-    dump_oob(output_data, file)
+    # TODO-SP: switch to dump_oob from openpilot/selfdrive/helpers on next full recompile of all models
+    pickle.dump(output_data, file)
 
   pkl_size = os.path.getsize(args.output)
   print(f"Saved combined JIT to {args.output} ({pkl_size / 1e6:.2f} MB)")

openpilot/sunnypilot/modeld_v2/modeld.py

@@ -10,14 +10,11 @@ import os
 os.environ['GMMU'] = '0'
 from openpilot.common.hardware import TICI
 os.environ['DEV'] = 'QCOM' if TICI else 'CPU'
-
-from openpilot.selfdrive.modeld.helpers import usbgpu_present, load_oob
-from openpilot.selfdrive.modeld.usbgpu_link import wait_usbgpu_link
-
-USBGPU = usbgpu_present()
+USBGPU = "USBGPU" in os.environ
 if USBGPU:
   os.environ['DEV'] = 'AMD'
   os.environ['AMD_IFACE'] = 'USB'
+import pickle
 import time
 import numpy as np
 import openpilot.cereal.messaging as messaging
@@ -52,6 +49,7 @@ from openpilot.sunnypilot.modeld_v2.compile_modeld import derive_frame_skip, mak
 from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
 from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
 from openpilot.sunnypilot.models.helpers import get_active_bundle
+from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController
 
 PROCESS_NAME = "openpilot.selfdrive.modeld.modeld_tinygrad"
 
@@ -111,10 +109,11 @@ class ModelState(ModelStateBase):
 
   def _init_combined(self, pkl_path, cam_w, cam_h, bundle):
     cloudlog.warning(f"loading combined pkl: {pkl_path}")
-    jits = load_oob(open_file_chunked(pkl_path))
+    # TODO-SP: switch to load_oob from openpilot/selfdrive/helpers on next full recompile of all models
+    jits = pickle.load(open_file_chunked(pkl_path))
 
     self.DEV = Device.DEFAULT
-    self.WARP_DEV = ('QCOM' if TICI else 'CPU') if USBGPU else self.DEV
+    self.WARP_DEV = 'CPU' if USBGPU else self.DEV
     self.QUEUE_DEV = self.DEV
 
     metadata = jits['metadata']
@@ -122,6 +121,13 @@ class ModelState(ModelStateBase):
     self._run_policy = jits[(cam_w, cam_h)]['run_policy']
     self._warp_enqueue = jits[(cam_w, cam_h)]['warp_enqueue']
 
+    # TODO-SP: Remove legacy use_packed detection block after all models are recompiled
+    captured = getattr(self._run_policy, 'captured', None)
+    if captured is not None:
+      use_packed = 'packed_npy_inputs' in getattr(captured, 'expected_names', [])
+    else:
+      use_packed = True
+
     if 'model' in metadata:
       model_metadata = metadata['model']
       self.vision_output_slices = model_metadata['output_slices']
@@ -132,7 +138,7 @@ class ModelState(ModelStateBase):
       from openpilot.sunnypilot.modeld_v2.compile_modeld import make_supercombo_input_queues
       frame_skip = derive_frame_skip({}, model_metadata['input_shapes'])
       self.input_queues, self.numpy_inputs = make_supercombo_input_queues(model_metadata['input_shapes'],
-                                                                          frame_skip, device=self.QUEUE_DEV)
+                                                                          frame_skip, device=self.QUEUE_DEV, use_packed=use_packed)
     else:
       vision_metadata = metadata['vision']
       policy_keys = [k for k in metadata if k != 'vision']
@@ -151,7 +157,7 @@ class ModelState(ModelStateBase):
       self._vision_input_names = [k for k in vision_input_shapes if 'img' in k]
       frame_skip = derive_frame_skip(vision_input_shapes, policy_input_shapes)
       self.input_queues, self.numpy_inputs = make_split_input_queues(vision_input_shapes, policy_input_shapes,
-                                                                     frame_skip, device=self.QUEUE_DEV)
+                                                                     frame_skip, device=self.QUEUE_DEV, use_packed=use_packed)
 
     self._desire_key = next(key for key in self.numpy_inputs if key.startswith('desire'))
     self._road_key = next(key for key in self._vision_input_names if 'big' not in key)
@@ -184,26 +190,6 @@ class ModelState(ModelStateBase):
       frame=Tensor(np.zeros(yuv_size, dtype=np.uint8), device=self.WARP_DEV).contiguous().realize(),
       big_frame=Tensor(np.zeros(yuv_size, dtype=np.uint8), device=self.WARP_DEV).contiguous().realize())
 
-    if USBGPU:
-      self.warmup()
-
-  def warmup(self) -> None:
-    dummy_frames = {k: np.zeros(self.frame_buf_params[k][3], dtype=np.uint8) for k in self._vision_input_names}
-    transforms = {k: np.eye(3, dtype=np.float32) for k in [self._road_key, self._wide_key] if k}
-
-    dummy_inputs = {}
-    for k, v in self.numpy_inputs.items():
-      if k not in ['tfm', 'big_tfm', 'prev_feat']:
-        dummy_inputs[k] = np.zeros(v.shape, dtype=v.dtype)
-
-    self.run(dummy_frames, transforms, dummy_inputs, prepare_only=False)
-
-    for v in self.numpy_inputs.values():
-      v[:] = 0
-    self.prev_desire[:] = 0
-    self.full_frames.clear()
-    self._blob_cache.clear()
-
 
   @property
   def mlsim(self) -> bool:
@@ -280,6 +266,11 @@ class ModelState(ModelStateBase):
       buf[0, :-1] = buf[0, 1:]
       buf[0, -1, :] = outputs['desired_curvature'][0, :] if not self.mlsim else 0
 
+    # TODO-SP: This is a hack to prevent GPU corruption by calculating in CPU space, it can be removed on next recompile
+    if 'prev_feat' not in self.numpy_inputs and 'feat_q' in self.input_queues:
+      feat_val = self.input_queues['feat_q'].numpy()
+      self.input_queues['feat_q'].assign(feat_val).realize()
+
     return outputs
 
   def get_action_from_model(self, model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action,
@@ -316,9 +307,6 @@ def main(demo=False):
   setproctitle(PROCESS_NAME)
   config_realtime_process(7, 54)
 
-  if USBGPU:
-    wait_usbgpu_link()
-
   # visionipc clients
   while True:
     available_streams = VisionIpcClient.available_streams("camerad", block=False)
@@ -353,9 +341,6 @@ def main(demo=False):
   publish_state = PublishState()
   params = Params()
 
-  params.put_bool("UsbGpuPresent", USBGPU)
-  params.put_bool("UsbGpuCompiled", USBGPU)
-
   # setup filter to track dropped frames
   frame_dropped_filter = FirstOrderFilter(0., 10., 1. / model.constants.MODEL_FREQ)
   frame_id = 0
@@ -382,6 +367,7 @@ def main(demo=False):
   prev_action = log.ModelDataV2.Action()
 
   DH = DesireHelper()
+  RELC = RoadEdgeLaneChangeController(DH)
   meta_constants = load_meta_constants()
 
   while True:
@@ -495,7 +481,10 @@ def main(demo=False):
       l_lane_change_prob = desire_state[log.Desire.laneChangeLeft]
       r_lane_change_prob = desire_state[log.Desire.laneChangeRight]
       lane_change_prob = l_lane_change_prob + r_lane_change_prob
-      DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
+      RELC.update(modelv2_send.modelV2.roadEdgeStds, modelv2_send.modelV2.laneLineProbs, v_ego)
+      mdv2sp_send.modelDataV2SP.leftLaneChangeEdgeBlock = RELC.left_edge_detected
+      mdv2sp_send.modelDataV2SP.rightLaneChangeEdgeBlock = RELC.right_edge_detected
+      DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob, RELC.left_edge_detected, RELC.right_edge_detected)
       modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
       modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
       mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_direction

openpilot/sunnypilot/models/fetcher.py

@@ -13,7 +13,6 @@ from openpilot.common.params import Params
 from openpilot.common.swaglog import cloudlog
 from openpilot.common.hardware.hw import Paths
 from openpilot.sunnypilot.models.helpers import is_bundle_version_compatible
-from openpilot.selfdrive.modeld.helpers import usbgpu_present
 
 from openpilot.cereal import custom
 
@@ -104,11 +103,11 @@ class ModelParser:
 class ModelCache:
   """Handles caching of model data to avoid frequent remote fetches"""
 
-  def __init__(self, params: Params, cache_timeout: int = int(3600 * 1e9), suffix: str = ""):
+  def __init__(self, params: Params, cache_timeout: int = int(3600 * 1e9)):
     self.params = params
     self.cache_timeout = cache_timeout
-    self._LAST_SYNC_KEY = f"ModelManager_LastSyncTime{suffix}"
-    self._CACHE_KEY = f"ModelManager_ModelsCache{suffix}"
+    self._LAST_SYNC_KEY = "ModelManager_LastSyncTime"
+    self._CACHE_KEY = "ModelManager_ModelsCache"
 
   def _is_expired(self) -> bool:
     """Checks if the cache has expired"""
@@ -140,28 +139,24 @@ 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_v18.json"
 
   def __init__(self, params: Params):
     self.params = params
+    self.model_cache = ModelCache(params)
     self.model_parser = ModelParser()
-    if usbgpu_present():
-      self.model_cache = ModelCache(params, suffix="_USBGPU")
-      self.model_url = "https://raw.githubusercontent.com/sunnypilot/sunnypilot-models/refs/heads/gh-pages/docs/driving_models_usbgpu_v18.json"
-    else:
-      self.model_cache = ModelCache(params)
-      self.model_url = "https://raw.githubusercontent.com/sunnypilot/sunnypilot-models/refs/heads/gh-pages/docs/driving_models_v18.json"
 
   def _fetch_and_cache_models(self) -> list[custom.ModelManagerSP.ModelBundle] | None:
     """Fetches fresh model data from remote and updates cache.
     Returns None on transport errors. Raises on 404 and other fatal HTTP errors.
     """
     try:
-      response = requests.get(self.model_url, timeout=10)
+      response = requests.get(self.MODEL_URL, timeout=10)
 
       # Explicitly handle 404 differently
       if response.status_code == 404:
-        cloudlog.error(f"Models URL returned 404 Not Found: {self.model_url}")
-        raise HTTPError(f"404 Not Found: {self.model_url}", response=response)
+        cloudlog.error(f"Models URL returned 404 Not Found: {self.MODEL_URL}")
+        raise HTTPError(f"404 Not Found: {self.MODEL_URL}", response=response)
 
       # Raise for any other 4xx/5xx
       response.raise_for_status()

openpilot/sunnypilot/models/tests/test_tinygrad_ref.py

@@ -1,12 +1,11 @@
 import requests
 
-from openpilot.common.params import Params
 from openpilot.sunnypilot.models.tinygrad_ref import get_tinygrad_ref
 from openpilot.sunnypilot.models.fetcher import ModelFetcher
 
+
 def fetch_tinygrad_ref():
-  fetcher = ModelFetcher(Params())
-  response = requests.get(fetcher.model_url, timeout=10)
+  response = requests.get(ModelFetcher.MODEL_URL, timeout=10)
   response.raise_for_status()
   json_data = response.json()
   return json_data.get("tinygrad_ref")

openpilot/sunnypilot/selfdrive/car/interfaces.py

@@ -8,6 +8,7 @@ from typing import Any
 
 from opendbc.car import structs
 from opendbc.car.interfaces import CarInterfaceBase
+from opendbc.car.toyota.values import CAR as TOYOTA_CAR
 from openpilot.common.params import Params
 from openpilot.common.swaglog import cloudlog
 from openpilot.sunnypilot.selfdrive.controls.lib.nnlc.helpers import get_nn_model_path
@@ -69,6 +70,32 @@ def _initialize_torque_lateral_control(CI: CarInterfaceBase, CP: structs.CarPara
     CI.configure_torque_tune(CP.carFingerprint, CP.lateralTuning)
 
 
+_PRIUS_TSS2_PID_KP_BP = [1.0, 1.5, 2.0, 3.0, 5.0, 7.5, 10.0, 15.0, 30.0]
+_PRIUS_TSS2_PID_KI_BP = [1.0, 1.5, 2.0, 3.0, 5.0, 7.5, 10.0, 15.0, 30.0]
+_PRIUS_TSS2_PID_KP_V = [0.1304, 0.1409, 0.1357, 0.1409, 0.15, 0.1614, 0.1826, 0.2348, 0.4696]
+_PRIUS_TSS2_PID_KI_V = [0.00016, 0.00035, 0.00063, 0.00141, 0.00391, 0.0088, 0.01565, 0.03522, 0.14087]
+_PRIUS_TSS2_PID_KF = 4e-05
+
+
+def _enforce_prius_tss2_pid_lateral_control(CP: structs.CarParams, params: Params = None) -> bool:
+  if params is None:
+    params = Params()
+
+  if CP.carFingerprint != TOYOTA_CAR.TOYOTA_PRIUS_TSS2:
+    return False
+
+  return params.get_bool("ToyotaPriusTss2Pid")
+
+
+def _initialize_prius_tss2_pid_lateral_control(CP: structs.CarParams) -> None:
+  CP.lateralTuning.init('pid')
+  CP.lateralTuning.pid.kpBP = _PRIUS_TSS2_PID_KP_BP
+  CP.lateralTuning.pid.kpV = _PRIUS_TSS2_PID_KP_V
+  CP.lateralTuning.pid.kiBP = _PRIUS_TSS2_PID_KI_BP
+  CP.lateralTuning.pid.kiV = _PRIUS_TSS2_PID_KI_V
+  CP.lateralTuning.pid.kf = _PRIUS_TSS2_PID_KF
+
+
 def _cleanup_unsupported_params(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params: Params = None) -> None:
   if params is None:
     params = Params()
@@ -95,8 +122,15 @@ def _cleanup_unsupported_params(CP: structs.CarParams, CP_SP: structs.CarParamsS
 def setup_interfaces(CI: CarInterfaceBase, params: Params = None) -> None:
   enforce_torque = _enforce_torque_lateral_control(CI.CP, params)
   nnlc_enabled = _initialize_neural_network_lateral_control(CI.CP, CI.CP_SP, params)
+  prius_tss2_pid_enabled = _enforce_prius_tss2_pid_lateral_control(CI.CP, params)
+  if prius_tss2_pid_enabled:
+    # Prius TSS2 PID toggle takes priority over NNLC/EnforceTorqueControl for this car.
+    enforce_torque = False
+    nnlc_enabled = False
   _initialize_intelligent_cruise_button_management(CI.CP, CI.CP_SP, params)
   _initialize_torque_lateral_control(CI, CI.CP, enforce_torque, nnlc_enabled)
+  if prius_tss2_pid_enabled:
+    _initialize_prius_tss2_pid_lateral_control(CI.CP)
   _cleanup_unsupported_params(CI.CP, CI.CP_SP)
 
   try:
@@ -130,6 +164,9 @@ def initialize_params(params) -> list[dict[str, Any]]:
   keys.extend([
     "ToyotaEnforceStockLongitudinal",
     "ToyotaStopAndGoHack",
+    "ToyotaEnhancedBsm",
+    "ToyotaAutoHold",
+    "ToyotaPriusTss2Pid",
   ])
 
   return [{k: params.get(k, return_default=True)} for k in keys]

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

@@ -0,0 +1,122 @@
+import numpy as np
+import pytest
+from opendbc.car import structs
+from openpilot.sunnypilot.selfdrive.car import interfaces as si
+
+
+class FakeParams:
+  def __init__(self, values=None):
+    self.values = values or {}
+
+  def get_bool(self, key):
+    return bool(self.values.get(key, False))
+
+  def get(self, key, return_default=False):
+    return self.values.get(key)
+
+  def remove(self, key):
+    self.values.pop(key, None)
+
+
+class FakeCI:
+  def __init__(self, CP, CP_SP):
+    self.CP = CP
+    self.CP_SP = CP_SP
+    self.configure_torque_tune_calls = 0
+
+  def configure_torque_tune(self, fingerprint, tune):
+    self.configure_torque_tune_calls += 1
+    tune.init('torque')
+
+
+def make_prius_tss2_cp():
+  CP = structs.CarParams(carFingerprint='TOYOTA_PRIUS_TSS2', steerControlType=structs.CarParams.SteerControlType.torque)
+  CP.lateralTuning.init('torque')
+  return CP
+
+
+class TestPriusTss2PidGate:
+  def test_disabled_for_other_toyota_platforms(self):
+    CP = structs.CarParams(carFingerprint='TOYOTA_RAV4_TSS2')
+    assert si._enforce_prius_tss2_pid_lateral_control(CP, FakeParams({'ToyotaPriusTss2Pid': True})) is False
+
+  def test_disabled_when_param_off(self):
+    CP = make_prius_tss2_cp()
+    assert si._enforce_prius_tss2_pid_lateral_control(CP, FakeParams({'ToyotaPriusTss2Pid': False})) is False
+
+  def test_enabled_for_prius_tss2_with_param_on(self):
+    CP = make_prius_tss2_cp()
+    assert si._enforce_prius_tss2_pid_lateral_control(CP, FakeParams({'ToyotaPriusTss2Pid': True})) is True
+
+
+class TestPriusTss2PidApply:
+  def test_flips_union_and_sets_gains(self):
+    CP = make_prius_tss2_cp()
+    assert CP.lateralTuning.which() == 'torque'
+
+    si._initialize_prius_tss2_pid_lateral_control(CP)
+
+    assert CP.lateralTuning.which() == 'pid'
+    assert list(CP.lateralTuning.pid.kpV) == pytest.approx(si._PRIUS_TSS2_PID_KP_V)
+    assert list(CP.lateralTuning.pid.kiV) == pytest.approx(si._PRIUS_TSS2_PID_KI_V)
+    assert CP.lateralTuning.pid.kf == pytest.approx(si._PRIUS_TSS2_PID_KF)
+    # PIDController interp needs non-empty breakpoints matching V lists.
+    assert len(CP.lateralTuning.pid.kpBP) == len(CP.lateralTuning.pid.kpV)
+    assert len(CP.lateralTuning.pid.kiBP) == len(CP.lateralTuning.pid.kiV)
+
+  def test_kp_rises_toward_highway_not_boosted_at_low_speed(self):
+    """Real on-road data (route 550a71ee4c7a7fbe/00000549--01e8f2ab51) showed boosting kp below
+    5 m/s increased saturation and hunting rather than helping - the shape must rise toward highway
+    speed, matching every other real multi-breakpoint PID car's tune (GM Volt, Cadillac Escalade
+    ESV, Honda Civic 2022) and the LatControlTorqueV0-derived KP_INTERP shape, not the reverse."""
+    CP = make_prius_tss2_cp()
+    si._initialize_prius_tss2_pid_lateral_control(CP)
+
+    kp_parking_lot = np.interp(2.0, CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV)
+    kp_cruise = np.interp(5.0, CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV)
+    kp_highway = np.interp(30.0, CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV)
+    assert kp_parking_lot < kp_cruise < kp_highway
+
+
+class TestSetupInterfacesPrecedence:
+  def test_pid_toggle_wins_over_nnlc_and_enforce_torque(self):
+    """The Prius TSS2 PID toggle must be the last thing to touch lateralTuning: if the user also
+    has NNLC and/or EnforceTorqueControl on, the union must still end up 'pid' and
+    configure_torque_tune must never run, or the car would silently keep driving on torque."""
+    CP = make_prius_tss2_cp()
+    CP_SP = structs.CarParamsSP()
+    CI = FakeCI(CP, CP_SP)
+    params = FakeParams({
+      'EnforceTorqueControl': True,
+      'NeuralNetworkLateralControl': True,
+      'ToyotaPriusTss2Pid': True,
+    })
+
+    si.setup_interfaces(CI, params)
+
+    assert CP.lateralTuning.which() == 'pid'
+    assert CI.configure_torque_tune_calls == 0
+
+  def test_other_toyota_platform_unaffected_by_toggle(self):
+    """The same param being on must not leak into a different car's tuning."""
+    CP = structs.CarParams(carFingerprint='TOYOTA_RAV4_TSS2', steerControlType=structs.CarParams.SteerControlType.torque)
+    CP.lateralTuning.init('torque')
+    CP_SP = structs.CarParamsSP()
+    CI = FakeCI(CP, CP_SP)
+    params = FakeParams({'ToyotaPriusTss2Pid': True})
+
+    si.setup_interfaces(CI, params)
+
+    assert CP.lateralTuning.which() == 'torque'
+    assert CI.configure_torque_tune_calls == 0
+
+  def test_toggle_off_leaves_torque_control_path_intact(self):
+    CP = make_prius_tss2_cp()
+    CP_SP = structs.CarParamsSP()
+    CI = FakeCI(CP, CP_SP)
+    params = FakeParams({'EnforceTorqueControl': True, 'ToyotaPriusTss2Pid': False})
+
+    si.setup_interfaces(CI, params)
+
+    assert CP.lateralTuning.which() == 'torque'
+    assert CI.configure_torque_tune_calls == 1

openpilot/sunnypilot/selfdrive/controls/controlsd_ext.py

@@ -10,12 +10,14 @@ import openpilot.cereal.messaging as messaging
 from openpilot.cereal import log, custom
 
 from opendbc.car import structs
+from opendbc.car.toyota.values import CAR as TOYOTA_CAR
 from openpilot.common.params import Params
 from openpilot.common.swaglog import cloudlog
 from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
 from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
 from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
 from openpilot.sunnypilot.selfdrive.controls.lib.blinker_pause_lateral import BlinkerPauseLateral
+from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_pid_ext import LatControlPidSmooth
 from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_torque_v0 import LatControlTorque as LatControlTorqueV0
 
 
@@ -35,12 +37,15 @@ class ControlsExt(ModelStateBase):
     self.pm_services_ext = ['carControlSP']
 
   def initialize_lateral_control(self, lac, CI, dt):
+    if self.CP.lateralTuning.which() != 'torque':
+      if self.CP.carFingerprint == TOYOTA_CAR.TOYOTA_PRIUS_TSS2 and self.CP.lateralTuning.which() == 'pid':
+        return LatControlPidSmooth(self.CP, self.CP_SP, CI, dt)
+      return lac
+
     enforce_torque_control = self.params.get_bool("EnforceTorqueControl")
     torque_versions = self.params.get("TorqueControlTune")
     if not enforce_torque_control:
-      if self.CP.lateralTuning.which() == 'torque':
-        return LatControlTorqueV0(self.CP, self.CP_SP, CI, dt)  # FIXME-SP: revert when upstream fixes tuning issues with v1
-      return lac
+      return LatControlTorqueV0(self.CP, self.CP_SP, CI, dt)  # FIXME-SP: revert when upstream fixes tuning issues with v1
 
     if torque_versions == 0.0:  # v0
       return LatControlTorqueV0(self.CP, self.CP_SP, CI, dt)

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

@@ -0,0 +1,418 @@
+import math
+
+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.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalPlanSource
+from openpilot.sunnypilot import get_sanitize_int_param
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import (
+  CAP_FILTER_FRAMES, COMFORT_DECEL, DEPARTURE_MOTION_NOISE_FLOOR, LAUNCH_END_SPEED, LAUNCH_TARGET_HEADROOM, LAUNCH_TARGET_SLEW,
+  LEAD_BRAKING_ACCEL_THRESHOLD, LEAD_DROPOUT_COAST_TIME, LEAD_LOSS_HOLD_TIME, LEAD_MATCH_ACCEL_SLEW, LEAD_MATCH_GAP_GAIN, LEAD_MATCH_SPEED_HEADROOM,
+  LEAD_SWITCH_MAX_HOLD_TIME,
+  MATCHED_SPEED_DECEL_RATE, MPC_DECEL_JERK_COST_MULTIPLIER, MPC_DECEL_JERK_MAX_REQUIRED_DECEL, MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE,
+  MPC_DECEL_JERK_MAX_TARGET_REDUCTION, MPC_DECEL_TREND_FRAMES, SPEED_RELIEF_DEADBAND, SPEED_RESTRICT_DEADBAND, TARGET_SPEED_ARM_MARGIN,
+  TARGET_RELEASE_SLEW, TARGET_SPEED_RESERVE, PLANNER_BRAKING_ACCEL_THRESHOLD, RADAR_STALE_TIMEOUT, STOP_HOLD_CREEP_DISTANCE, STOP_HOLD_EGO_SPEED,
+  STOP_HOLD_EXIT_FRAMES, STOP_HOLD_EXIT_SPEED, STOP_HOLD_MAX_LEAD_DISTANCE, VEGO_NOISE_TOLERANCE, PARAM_READ_INTERVAL, AccelProfile,
+  profile_accel_max, sanitize_profile,
+)
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.helpers import build_accel_ceiling, is_valid_context
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.lead import LeadPlan, calculate_lead_plan, has_radar_lead, is_lead_source
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.state import AccelControllerState, TargetState
+
+
+class AccelController:
+  def __init__(self, CP, dt: float = DT_MDL):
+    if not math.isfinite(dt) or dt <= 0.0:
+      raise ValueError("dt must be finite and positive")
+
+    self.dt = dt
+    self.delay = float(CP.longitudinalActuatorDelay) + DT_MDL
+    self.lead_loss_hold_frames = max(CAP_FILTER_FRAMES, math.ceil(LEAD_LOSS_HOLD_TIME / dt))
+    self.lead_dropout_coast_frames = max(self.lead_loss_hold_frames, math.ceil(LEAD_DROPOUT_COAST_TIME / dt))
+    self.lead_switch_max_hold_frames = max(self.lead_loss_hold_frames, math.ceil(LEAD_SWITCH_MAX_HOLD_TIME / dt))
+    self.radar_stale_frames = max(1, math.ceil(RADAR_STALE_TIMEOUT / dt))
+    self.params = Params()
+    self.available = bool(CP.openpilotLongitudinalControl)
+    self.enabled = False
+    self.profile = AccelProfile.normal
+    self._param_read_frames = max(1, int(round(PARAM_READ_INTERVAL / dt)))
+    self._param_frame = 0
+    self._jerk_smoothing_blocked = False
+    self._required_decel_samples: list[float] = []
+    self._required_decel_lead = -1
+    self._required_decel_lead_track_id = -1
+    self._lead_trend_warmup = False
+    self._cruise_accel_limited = False
+    self.target_state = TargetState()
+    self._held_lead_plan: LeadPlan | None = None
+    self.is_active = self.launching = self.departure_launching = False
+    self.output_v_target = 0.0
+    self.mpc_accel_max: tuple[float, ...] | None = None
+    self.cruise_accel_max: float | None = None
+    self.state = AccelControllerState.inactive
+    self.selected_lead = -1
+    self.selected_lead_track_id = -1
+    self.required_decel = 0.0
+
+  @property
+  def is_enabled(self) -> bool:
+    return self.available and self.enabled
+
+  def update_params(self) -> None:
+    if self._param_frame % self._param_read_frames == 0:
+      self.enabled = self.params.get_bool("AccelPersonalityEnabled")
+      self.profile = get_sanitize_int_param("AccelPersonality", AccelProfile.eco, AccelProfile.sport, self.params)
+    self._param_frame += 1
+
+  def _update_target(self, lead_plan: LeadPlan, base_speed: float, v_ego: float, profile: int, profile_max_accel: float,
+                     previous_should_stop: bool, previous_mpc_source, planner_speed: float, planner_accel: float) -> float:
+    state = self.target_state
+    lead_filter_ready = state.update_samples(lead_plan, self.dt)
+    state.active_frames += 1
+    has_lead = lead_plan.selected_lead >= 0
+    filtered_cap = state.filtered_cap
+    slot_changed = has_lead and state.selected_lead >= 0 and lead_plan.selected_lead != state.selected_lead
+    track_changed = (has_lead and state.selected_lead >= 0 and lead_plan.selected_lead == state.selected_lead
+      and lead_plan.selected_lead_track_id != state.selected_lead_track_id
+      and (state.selected_lead_track_id >= 0 or lead_plan.selected_lead_track_id >= 0))
+    false_relief = has_lead and math.isfinite(filtered_cap) and lead_plan.cap >= filtered_cap + SPEED_RELIEF_DEADBAND
+    guarded_restriction = state.state in (AccelControllerState.restrict, AccelControllerState.hold, AccelControllerState.release)
+    switched_to_relief = ((slot_changed or track_changed) and false_relief
+                          and (guarded_restriction or planner_accel <= PLANNER_BRAKING_ACCEL_THRESHOLD))
+    confirmed_relief = (not has_lead or (state.target_speed is not None and lead_plan.closing_speed <= 0.0
+                        and lead_plan.cap >= state.target_speed + SPEED_RELIEF_DEADBAND))
+    state.update_lead_switch_guard(switched_to_relief, confirmed_relief, slot_changed or track_changed or false_relief,
+                                   self.lead_loss_hold_frames, self.lead_switch_max_hold_frames)
+    if has_lead:
+      state.selected_lead = lead_plan.selected_lead
+      state.selected_lead_track_id = lead_plan.selected_lead_track_id
+    elif state.lead_loss_frames >= self.lead_loss_hold_frames:
+      state.reset_lead_switch_guard()
+      state.selected_lead = state.selected_lead_track_id = -1
+    departure_separation = (lead_plan.departure_lead_separations[lead_plan.departure_lead_index]
+                            if lead_plan.departure_lead_index >= 0 else math.inf)
+    stopped_lead_hold = (has_lead and lead_plan.has_nearly_stopped_lead
+      and (lead_plan.departure_cap < 0.50 or (state.lead_braking and departure_separation <= STOP_HOLD_MAX_LEAD_DISTANCE)))
+    invalid_lead = lead_plan.lead_status and not has_lead
+    prior_lead_context = is_lead_source(previous_mpc_source) or math.isfinite(filtered_cap) or state.lead_braking
+    previous_stop = previous_should_stop and prior_lead_context and (not has_lead or lead_plan.departure_lead_speed < STOP_HOLD_EXIT_SPEED)
+    stop_evidence = stopped_lead_hold or lead_plan.cap < 0.50 or filtered_cap < 0.50 or (previous_stop and not state.launching) or invalid_lead
+    departure_motion_confirmed = (state.launching and state.departure_launch and has_lead
+      and (state.departure.progress(lead_plan, DEPARTURE_MOTION_NOISE_FLOOR) or state.departure.recent_motion()))
+    if state.active_frames >= self.lead_loss_hold_frames and math.isfinite(filtered_cap) and has_lead and planner_accel <= PLANNER_BRAKING_ACCEL_THRESHOLD:
+      state.lead_braking = True
+    elif not has_lead and state.lead_loss_frames >= self.lead_loss_hold_frames:
+      state.lead_braking = False
+
+    if state.target_speed is None:
+      e2e_handoff = previous_mpc_source == LongitudinalPlanSource.e2e
+      seed_from_ego = has_lead and planner_accel > PLANNER_BRAKING_ACCEL_THRESHOLD and not e2e_handoff
+      state.target_speed = min(base_speed, v_ego) if seed_from_ego else base_speed
+      if seed_from_ego and v_ego >= LAUNCH_END_SPEED and lead_plan.closing_speed > 0.0:
+        state.arm_release_slew()
+      state.e2e_braking_handoff = e2e_handoff and planner_accel < 0.0
+      state.state = AccelControllerState.free
+      if v_ego < STOP_HOLD_EGO_SPEED and not stop_evidence:
+        state.target_speed = min(base_speed, v_ego + LAUNCH_TARGET_HEADROOM)
+        state.state = AccelControllerState.release
+        state.launching = True
+        state.departure_launch = False
+    elif state.e2e_braking_handoff and planner_accel >= 0.0:
+      state.e2e_braking_handoff = False
+
+    state.target_speed = min(state.target_speed, base_speed)
+    if v_ego < STOP_HOLD_EGO_SPEED and stop_evidence and not departure_motion_confirmed and state.state != AccelControllerState.stopHold:
+      state.enter_stop_hold(lead_plan)
+      return state.target_speed
+
+    if state.state == AccelControllerState.stopHold:
+      state.departure.backfill_references()
+      fast_departure = (has_lead and min(lead_plan.selected_lead_speed, lead_plan.departure_lead_speed) > STOP_HOLD_EXIT_SPEED
+        and lead_plan.departure_cap > STOP_HOLD_EXIT_SPEED)
+      raw_departure = fast_departure or not lead_plan.lead_status and state.lead_loss_frames >= self.lead_loss_hold_frames
+      departed = state.departure.progress(lead_plan, STOP_HOLD_CREEP_DISTANCE) or raw_departure
+      if fast_departure and state.departure_frames == 0:
+        state.departure.keep_latest_motion_sample()
+      state.departure_frames = state.departure_frames + 1 if departed else 0
+      state.target_speed = 0.0
+      fast_departure_confirmed = fast_departure and state.departure.recent_motion()
+      if state.departure_frames < STOP_HOLD_EXIT_FRAMES or fast_departure and not fast_departure_confirmed:
+        return state.target_speed
+      state.target_speed = base_speed
+      state.state = AccelControllerState.release
+      state.departure_frames = 0
+      state.launching = True
+      state.departure_launch = has_lead
+      return state.target_speed
+
+    if state.launching:
+      renewed_stop = (has_lead and not departure_motion_confirmed
+        and (lead_plan.cap < STOP_HOLD_EXIT_SPEED
+             or (lead_plan.has_nearly_stopped_lead and lead_plan.departure_cap < STOP_HOLD_EXIT_SPEED)))
+      guarded_departure_loss = state.departure_launch and not lead_plan.lead_status and state.lead_loss_frames < self.lead_loss_hold_frames
+      if invalid_lead:
+        state.launching = state.departure_launch = False
+        if v_ego < STOP_HOLD_EGO_SPEED:
+          state.enter_stop_hold(lead_plan)
+          return state.target_speed
+        state.state = AccelControllerState.hold
+        return state.target_speed
+      if guarded_departure_loss:
+        state.state = AccelControllerState.hold
+        return state.target_speed
+      if state.departure_launch and not has_lead:
+        state.departure_launch = False
+      if renewed_stop:
+        state.launching = state.departure_launch = False
+        if v_ego < STOP_HOLD_EGO_SPEED:
+          state.enter_stop_hold(lead_plan)
+          return state.target_speed
+      if state.launching:
+        if state.departure_launch:
+          state.target_speed = base_speed
+        else:
+          launch_target = min(base_speed, v_ego + LAUNCH_TARGET_HEADROOM)
+          state.target_speed = min(base_speed, max(state.target_speed, launch_target) + LAUNCH_TARGET_SLEW * self.dt)
+        if v_ego >= LAUNCH_END_SPEED:
+          state.launching = state.departure_launch = False
+
+    comfort_decel = COMFORT_DECEL[profile]
+    if (has_lead and not state.launching and state.state == AccelControllerState.restrict
+      and lead_plan.closing_speed <= 0.0 and v_ego >= state.filtered_lead_speed - VEGO_NOISE_TOLERANCE):
+      state.matched_lead = True
+    elif not has_lead and state.lead_loss_frames >= self.lead_loss_hold_frames:
+      state.matched_lead = False
+
+    lost_lead_source = is_lead_source(previous_mpc_source) and not has_lead and planner_speed < state.target_speed
+    if not has_lead and (state.matched_lead or lost_lead_source):
+      if lost_lead_source:
+        state.lead_dropout = True
+        state.target_speed = planner_speed
+        state.arm_release_slew()
+      state.state = AccelControllerState.hold
+      return state.target_speed
+
+    if state.matched_lead:
+      if math.isfinite(state.filtered_lead_speed):
+        recovery_speed = min(base_speed, state.filtered_lead_speed + min(LEAD_MATCH_SPEED_HEADROOM, LEAD_MATCH_GAP_GAIN * lead_plan.usable_gap))
+        desired_accel_limit = min(profile_max_accel, max(recovery_speed - v_ego, 0.0))
+      else:
+        desired_accel_limit = 0.0
+      if state.filtered_lead_accel < LEAD_BRAKING_ACCEL_THRESHOLD:
+        desired_accel_limit = profile_max_accel
+      if state.matched_accel_limit is None:
+        state.matched_accel_limit = profile_max_accel
+      if state.lead_switch_guard_frames > 0:
+        desired_accel_limit = min(desired_accel_limit, state.matched_accel_limit)
+      state.matched_accel_limit = min(profile_max_accel, float(np.clip(
+        desired_accel_limit, state.matched_accel_limit - LEAD_MATCH_ACCEL_SLEW * self.dt,
+        state.matched_accel_limit + LEAD_MATCH_ACCEL_SLEW * self.dt,
+      )))
+      matched_ceiling = min(base_speed, filtered_cap)
+      if matched_ceiling <= state.target_speed - SPEED_RESTRICT_DEADBAND:
+        state.target_speed = max(matched_ceiling, state.target_speed - MATCHED_SPEED_DECEL_RATE * self.dt)
+        state.arm_release_slew()
+        state.state = AccelControllerState.restrict
+      elif (state.lead_switch_guard_frames == 0 and matched_ceiling >= state.target_speed + SPEED_RELIEF_DEADBAND
+            and (state.lead_switch_elapsed_frames < self.lead_switch_max_hold_frames or planner_accel > PLANNER_BRAKING_ACCEL_THRESHOLD)):
+        state.target_speed = min(matched_ceiling, state.target_speed + profile_max_accel * self.dt)
+        state.state = AccelControllerState.free if state.target_speed >= base_speed - SPEED_RESTRICT_DEADBAND else AccelControllerState.release
+      else:
+        state.state = AccelControllerState.free if state.target_speed >= base_speed - SPEED_RESTRICT_DEADBAND else AccelControllerState.hold
+      if state.state == AccelControllerState.free:
+        state.reset_release_slew(state.target_speed)
+      else:
+        state.update_release_slew(matched_ceiling, math.isfinite(matched_ceiling) and state.target_speed == matched_ceiling)
+      return state.target_speed
+    state.matched_accel_limit = None
+
+    ceiling = min(base_speed, filtered_cap)
+    synced_to_planner = lead_filter_ready and not state.launching and planner_speed < state.target_speed
+    if synced_to_planner:
+      state.target_speed = max(planner_speed, state.target_speed - comfort_decel * self.dt)
+
+    if ceiling <= state.target_speed - SPEED_RESTRICT_DEADBAND or (state.state == AccelControllerState.restrict and ceiling < state.target_speed):
+      if not synced_to_planner:
+        state.target_speed = max(ceiling, state.target_speed - comfort_decel * self.dt)
+      state.arm_release_slew()
+      state.state = AccelControllerState.restrict
+      return state.target_speed
+
+    filter_warmup = has_lead and not math.isfinite(filtered_cap)
+    guarded_lead_loss = not has_lead and state.lead_loss_frames < (self.lead_dropout_coast_frames if state.lead_dropout else self.lead_loss_hold_frames)
+    if (filter_warmup or guarded_lead_loss) and state.target_speed < base_speed - SPEED_RESTRICT_DEADBAND:
+      state.state = AccelControllerState.hold
+      return state.target_speed
+
+    confirmed_clear_road = not math.isfinite(filtered_cap) and not guarded_lead_loss
+    relief = (not has_lead or lead_plan.closing_speed <= 0.0) and planner_accel > PLANNER_BRAKING_ACCEL_THRESHOLD
+    continuing_release = state.release_slew_armed and ceiling > state.target_speed
+    if relief and (continuing_release or ceiling >= state.target_speed + SPEED_RELIEF_DEADBAND
+                   or (confirmed_clear_road and ceiling > state.target_speed)):
+      if state.lead_switch_guard_frames == 0:
+        timed_out = state.lead_switch_elapsed_frames >= self.lead_switch_max_hold_frames
+        if not state.release_slew_armed and (timed_out or (state.release_settle_speed is not None
+                                                          and ceiling - state.target_speed > TARGET_RELEASE_SLEW * self.dt)):
+          state.arm_release_slew(force=True)
+        release_rate = comfort_decel if timed_out else TARGET_RELEASE_SLEW
+        state.target_speed = min(ceiling, state.target_speed + release_rate * self.dt) if state.release_slew_armed else ceiling
+      if state.release_slew_armed and state.target_speed < ceiling:
+        state.state = AccelControllerState.release
+      else:
+        state.state = AccelControllerState.free if state.target_speed >= base_speed - SPEED_RESTRICT_DEADBAND else AccelControllerState.hold
+    else:
+      state.state = AccelControllerState.free if state.target_speed >= base_speed - SPEED_RESTRICT_DEADBAND else AccelControllerState.hold
+    if state.target_speed >= base_speed:
+      state.lead_dropout = False
+      state.reset_release_slew(state.target_speed)
+    else:
+      state.update_release_slew(ceiling, math.isfinite(ceiling) and state.target_speed == ceiling)
+    return state.target_speed
+
+  def _update_freshness(self, radar_fresh: bool) -> None:
+    self.target_state.stale_frames = 0 if radar_fresh else self.target_state.stale_frames + 1
+    if self.target_state.stale_frames >= self.radar_stale_frames:
+      self.target_state = TargetState()
+
+  def reset(self) -> None:
+    self.target_state = TargetState()
+    self._held_lead_plan = None
+    self._jerk_smoothing_blocked = False
+    self._required_decel_samples.clear()
+    self._required_decel_lead = self._required_decel_lead_track_id = -1
+    self._lead_trend_warmup = False
+    self._cruise_accel_limited = False
+    self.is_active = self.launching = self.departure_launching = False
+    self.output_v_target = 0.0
+    self.mpc_accel_max = None
+    self.cruise_accel_max = None
+    self.state = AccelControllerState.inactive
+    self.selected_lead = -1
+    self.selected_lead_track_id = -1
+    self.required_decel = 0.0
+
+  def update(self, radar_state, *, base_speed: float, v_ego: float, a_ego: float, follow_personality, acc_selected: bool,
+             engaged: bool, cruise_initialized: bool, stock_accel_max: float, previous_should_stop: bool, radar_fresh: bool = True,
+             previous_mpc_source=None, planner_speed: float | None = None, planner_accel: float = 0.0) -> None:
+    self.profile = sanitize_profile(self.profile)
+    sanitized_v_ego = max(v_ego, 0.0) if math.isfinite(v_ego) and v_ego >= -VEGO_NOISE_TOLERANCE else v_ego
+    profile_max_accel = profile_accel_max(self.profile, sanitized_v_ego)
+    stock_accel_max = float(stock_accel_max)
+    positive_accel_max = (max(0.0, min(profile_max_accel, stock_accel_max, ACCEL_MAX))
+                          if math.isfinite(profile_max_accel) and math.isfinite(stock_accel_max) else math.nan)
+    planner_speed = sanitized_v_ego if planner_speed is None else planner_speed
+    valid_context = is_valid_context(base_speed, sanitized_v_ego, a_ego, planner_speed, planner_accel, stock_accel_max, self.delay,
+                                     engaged, cruise_initialized)
+    enabled_context = valid_context and self.is_enabled and bool(acc_selected)
+    if enabled_context and radar_fresh:
+      lead_plan = calculate_lead_plan(radar_state, sanitized_v_ego, a_ego, self.delay, self.profile, follow_personality)
+      self._held_lead_plan = lead_plan
+    elif enabled_context and self._held_lead_plan is not None:
+      lead_plan = self._held_lead_plan
+    else:
+      lead_plan = LeadPlan(lead_status=has_radar_lead(radar_state))
+      self._held_lead_plan = None
+
+    if enabled_context:
+      self._update_freshness(radar_fresh)
+    active = enabled_context and (radar_fresh or self.target_state.target_speed is not None)
+    if active and radar_fresh:
+      target_speed = self._update_target(
+        lead_plan, base_speed, sanitized_v_ego, self.profile, profile_max_accel, previous_should_stop,
+        previous_mpc_source, planner_speed, planner_accel,
+      )
+    elif active:
+      target_speed = self.target_state.target_speed
+    else:
+      self.target_state = TargetState()
+      target_speed = base_speed
+
+    if not radar_fresh and not active:
+      self._held_lead_plan = None
+      lead_plan = LeadPlan(lead_status=has_radar_lead(radar_state))
+
+    state = self.target_state
+    stop_hold_active = active and state.state == AccelControllerState.stopHold
+    matched_limit_active = active and state.matched_lead and state.matched_accel_limit is not None and not state.e2e_braking_handoff
+    lead_accel_request = active and lead_plan.selected_lead >= 0 and lead_plan.closing_speed <= 0.0 and planner_accel >= 0.0
+    profile_limit_active = active and not stop_hold_active and (state.launching or not lead_plan.lead_status or lead_accel_request)
+    if matched_limit_active:
+      effective_accel_max = min(positive_accel_max, state.matched_accel_limit)
+    elif profile_limit_active:
+      effective_accel_max = positive_accel_max
+    else:
+      effective_accel_max = math.inf
+    mpc_accel_max = build_accel_ceiling(effective_accel_max, planner_accel) if matched_limit_active or profile_limit_active else None
+    guarded_lead_loss = not lead_plan.lead_status and state.selected_lead >= 0 and state.lead_loss_frames < self.lead_loss_hold_frames
+    lead_context = lead_plan.lead_status or math.isfinite(state.filtered_cap) or guarded_lead_loss
+    reserve_eligible = active and lead_context and not stop_hold_active and not state.launching and not state.e2e_braking_handoff
+    reserve_can_arm = reserve_eligible and state.lead_switch_guard_frames == 0
+    if not lead_context:
+      state.speed_reserve_armed = False
+    elif (reserve_can_arm and not state.speed_reserve_armed and math.isfinite(state.filtered_cap)
+          and state.filtered_cap <= target_speed + TARGET_SPEED_ARM_MARGIN):
+      state.speed_reserve_armed = True
+
+    output_target = 0.0 if stop_hold_active else target_speed
+    if reserve_eligible and state.speed_reserve_armed:
+      output_target = max(0.0, output_target - TARGET_SPEED_RESERVE)
+
+    self.is_active = active
+    self.launching = active and state.launching
+    self.departure_launching = self.launching and state.departure_launch
+    self.output_v_target = output_target
+    self.mpc_accel_max = mpc_accel_max
+    start_cruise_accel_limit = (active and state.state == AccelControllerState.free and lead_plan.lead_status
+                                and lead_plan.closing_speed > 0.0 and planner_accel >= 0.0
+                                and previous_mpc_source == LongitudinalPlanSource.cruise)
+    keep_cruise_accel_limit = (self._cruise_accel_limited and active and lead_context and state.state == AccelControllerState.free
+                               and not state.e2e_braking_handoff)
+    self._cruise_accel_limited = start_cruise_accel_limit or keep_cruise_accel_limit
+    self.cruise_accel_max = positive_accel_max if self._cruise_accel_limited else None
+    self.state = state.state
+    self.selected_lead = lead_plan.selected_lead
+    self.selected_lead_track_id = lead_plan.selected_lead_track_id
+    self.required_decel = lead_plan.required_decel
+
+  def get_jerk_cost_multiplier(self, actuating: bool, prev_accel_constraint: bool, target_reduction: float, previous_mpc_failed: bool) -> float:
+    lead_restriction = (actuating and prev_accel_constraint and self.state == AccelControllerState.restrict and self.selected_lead >= 0
+      and not self.launching and target_reduction > 1e-6)
+    same_lead = self.selected_lead == self._required_decel_lead and self.selected_lead_track_id == self._required_decel_lead_track_id
+    lead_changed = lead_restriction and self._required_decel_lead >= 0 and not same_lead
+    if lead_changed:
+      self._lead_trend_warmup = True
+    elif not lead_restriction:
+      self._lead_trend_warmup = False
+    if not lead_restriction or not same_lead or not math.isfinite(self.required_decel):
+      self._required_decel_samples.clear()
+    if lead_restriction and math.isfinite(self.required_decel):
+      self._required_decel_samples.append(self.required_decel)
+      if len(self._required_decel_samples) > MPC_DECEL_TREND_FRAMES:
+        self._required_decel_samples.pop(0)
+    self._required_decel_lead = self.selected_lead if lead_restriction else -1
+    self._required_decel_lead_track_id = self.selected_lead_track_id if lead_restriction else -1
+
+    history = self._required_decel_samples
+    history_ready = len(history) == MPC_DECEL_TREND_FRAMES
+    tightening_lead = (history_ready
+      and (history[-1] - history[0]) / (self.dt * (len(history) - 1)) > MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE
+      and sum(after > before for before, after in zip(history[:-1], history[1:], strict=True)) >= 2)
+    modest_decel = (lead_restriction and target_reduction < MPC_DECEL_JERK_MAX_TARGET_REDUCTION
+      and 0.0 < self.required_decel < MPC_DECEL_JERK_MAX_REQUIRED_DECEL)
+    smoothing_eligible = modest_decel and (not self._lead_trend_warmup or history_ready) and not tightening_lead
+    if history_ready:
+      self._lead_trend_warmup = False
+    if previous_mpc_failed or (lead_restriction and not self._jerk_smoothing_blocked and (not modest_decel or tightening_lead)):
+      self._jerk_smoothing_blocked = True
+    elif not lead_restriction:
+      self._jerk_smoothing_blocked = False
+    return MPC_DECEL_JERK_COST_MULTIPLIER if smoothing_eligible and not self._jerk_smoothing_blocked else 1.0
+
+  def update_should_stop(self, should_stop: bool) -> bool:
+    if not self.is_active:
+      return should_stop
+    if self.departure_launching:
+      return False
+    return should_stop or self.state == AccelControllerState.stopHold

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

@@ -0,0 +1,76 @@
+import math
+
+import numpy as np
+
+from openpilot.cereal import custom
+
+
+AccelProfile = custom.LongitudinalPlanSP.AccelController.Profile
+ACCEL_PROFILES = tuple(AccelProfile.schema.enumerants.values())
+
+COMFORT_DECEL = {
+  AccelProfile.eco: 0.25,
+  AccelProfile.normal: 0.30,
+  AccelProfile.sport: 0.35,
+}
+
+ACCEL_PROFILE_MAX_BP = [0.0, 3.0, 10.0, 25.0, 40.0]
+ACCEL_PROFILE_MAX_V = {
+  AccelProfile.eco: [1.65, 1.30, 0.72, 0.32, 0.16],
+  AccelProfile.normal: [1.80, 1.50, 0.97, 0.48, 0.30],
+  AccelProfile.sport: [2.00, 1.90, 1.15, 0.68, 0.42],
+}
+
+CAP_FILTER_FRAMES = 5
+LEAD_LOSS_HOLD_TIME = 0.50
+LEAD_DROPOUT_COAST_TIME = 1.50
+LEAD_SWITCH_MAX_HOLD_TIME = 6.0
+SPEED_RESTRICT_DEADBAND = 0.15
+SPEED_RELIEF_DEADBAND = 0.35
+TARGET_RELEASE_SLEW = 8.75
+TARGET_SPEED_ARM_MARGIN = 1.0
+TARGET_SPEED_RESERVE = 0.10
+LAUNCH_TARGET_HEADROOM = 3.0
+LAUNCH_TARGET_SLEW = 8.75
+LAUNCH_END_SPEED = 3.0
+ACCEL_LIMIT_HORIZON_JERK = 1.0
+LEAD_MATCH_GAP_GAIN = 0.04
+LEAD_MATCH_SPEED_HEADROOM = 1.25
+LEAD_MATCH_ACCEL_SLEW = 0.25
+MATCHED_SPEED_DECEL_RATE = 0.50
+PLANNER_BRAKING_ACCEL_THRESHOLD = -0.11
+LEAD_BRAKING_ACCEL_THRESHOLD = -0.11
+MPC_DECEL_JERK_COST_MULTIPLIER = 1.05
+MPC_DECEL_JERK_MAX_REQUIRED_DECEL = 0.80
+MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE = 0.35
+MPC_DECEL_JERK_MAX_TARGET_REDUCTION = 9.0
+MPC_DECEL_TREND_FRAMES = 4
+
+STOP_HOLD_EGO_SPEED = 0.30
+STOPPED_LEAD_SPEED = 0.30
+STOP_HOLD_EXIT_SPEED = 0.80
+STOP_HOLD_EXIT_FRAMES = 4
+STOP_HOLD_CREEP_SPEED = 0.15
+STOP_HOLD_CREEP_DISTANCE = 0.30
+DEPARTURE_MOTION_NOISE_FLOOR = 0.03
+DEPARTURE_MOTION_STEP_MIN = 0.005
+STOP_HOLD_MAX_LEAD_DISTANCE = 30.0
+STOP_GAP_RESERVE = 0.75
+STOP_GAP_RESERVE_LEAD_SPEED = 2.0
+STOP_GAP_RESERVE_DECEL_BP = (0.30, 0.80)
+
+RADAR_STALE_TIMEOUT = 0.50
+MAX_LEAD_ACCEL_TAU = 10.0
+MIN_LEAD_SPEED = -1.0
+VEGO_NOISE_TOLERANCE = 0.10
+PARAM_READ_INTERVAL = 0.25
+
+
+def sanitize_profile(profile: int) -> int:
+  return profile if profile in ACCEL_PROFILES else AccelProfile.normal
+
+
+def profile_accel_max(profile: int, v_ego: float) -> float:
+  if not math.isfinite(v_ego):
+    return math.nan
+  return float(np.interp(max(v_ego, 0.0), ACCEL_PROFILE_MAX_BP, ACCEL_PROFILE_MAX_V[sanitize_profile(profile)]))

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

@@ -0,0 +1,22 @@
+import math
+
+import numpy as np
+
+from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import ACCEL_LIMIT_HORIZON_JERK, VEGO_NOISE_TOLERANCE
+
+
+def is_valid_context(base_speed: float, v_ego: float, a_ego: float, planner_speed: float, planner_accel: float, stock_accel_max: float,
+                     delay: float, engaged: bool, cruise_initialized: bool) -> bool:
+  values = (base_speed, v_ego, a_ego, planner_speed, planner_accel, stock_accel_max, delay)
+  return (engaged and cruise_initialized and base_speed >= 0.0 and v_ego >= -VEGO_NOISE_TOLERANCE
+          and planner_speed >= 0.0 and stock_accel_max >= 0.0 and delay >= 0.0 and all(math.isfinite(value) for value in values))
+
+
+def build_accel_ceiling(limit: float, planner_accel: float) -> tuple[float, ...] | None:
+  if limit >= ACCEL_MAX - 1e-9:
+    return None
+  a0 = float(np.clip(planner_accel, ACCEL_MIN, ACCEL_MAX))
+  ceiling = np.clip(np.maximum(limit, a0 - ACCEL_LIMIT_HORIZON_JERK * T_IDXS), 0.0, ACCEL_MAX)
+  return tuple(float(value) for value in ceiling)

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

@@ -0,0 +1,147 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+
+import math
+from typing import NamedTuple
+
+import numpy as np
+
+from openpilot.cereal import log
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import (
+  LongitudinalMpc, LongitudinalPlanSource, STOP_DISTANCE, T_IDXS, get_T_FOLLOW, get_stopped_equivalence_factor,
+)
+from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import (
+  COMFORT_DECEL, MAX_LEAD_ACCEL_TAU, MIN_LEAD_SPEED, STOP_GAP_RESERVE, STOP_GAP_RESERVE_DECEL_BP,
+  STOP_GAP_RESERVE_LEAD_SPEED, STOPPED_LEAD_SPEED, sanitize_profile,
+)
+
+
+class LeadPlan(NamedTuple):
+  cap: float = math.inf
+  selected_lead: int = -1
+  selected_lead_track_id: int = -1
+  selected_lead_speed: float = math.inf
+  selected_lead_accel: float = 0.0
+  departure_lead_index: int = -1
+  departure_lead_speed: float = math.inf
+  departure_cap: float = math.inf
+  departure_lead_speeds: tuple[float, float] = (math.inf, math.inf)
+  departure_lead_distances: tuple[float, float] = (-math.inf, -math.inf)
+  departure_lead_track_ids: tuple[int, int] = (-1, -1)
+  departure_lead_separations: tuple[float, float] = (-math.inf, -math.inf)
+  usable_gap: float = math.inf
+  closing_speed: float = 0.0
+  required_decel: float = 0.0
+  has_nearly_stopped_lead: bool = False
+  lead_status: bool = False
+
+
+def is_lead_source(source) -> bool:
+  return source in (LongitudinalPlanSource.lead0, LongitudinalPlanSource.lead1)
+
+
+def has_radar_lead(radar_state) -> bool:
+  return bool(radar_state.leadOne.present or radar_state.leadTwo.present)
+
+
+def _project_ego(v_ego: float, a_ego: float, delay: float) -> tuple[float, float]:
+  if a_ego < 0.0:
+    stop_time = -v_ego / a_ego if v_ego > 0.0 else 0.0
+    if stop_time <= delay:
+      distance = -v_ego**2 / (2.0 * a_ego) if v_ego > 0.0 else 0.0
+      return distance, 0.0
+  return max(v_ego * delay + 0.5 * a_ego * delay**2, 0.0), max(v_ego + a_ego * delay, 0.0)
+
+
+def _lead_values(lead) -> tuple[float, float, float, float] | None:
+  if not lead.present:
+    return None
+  d_rel, v_lead = float(lead.dRel), float(lead.vLeadK)
+  if not math.isfinite(d_rel) or d_rel < 0.0 or not math.isfinite(v_lead) or v_lead < MIN_LEAD_SPEED:
+    return None
+
+  a_lead = float(lead.aLeadK)
+  if not math.isfinite(a_lead):
+    a_lead = 0.0
+  a_lead_tau = float(lead.aLeadTau)
+  if not math.isfinite(a_lead_tau) or not 0.0 < a_lead_tau <= MAX_LEAD_ACCEL_TAU:
+    a_lead_tau = _LEAD_ACCEL_TAU
+  return d_rel, max(v_lead, 0.0), float(np.clip(a_lead, -10.0, 5.0)), a_lead_tau
+
+
+def calculate_lead_plan(radar_state, v_ego: float, a_ego: float, delay: float, profile: int,
+                        follow_personality=log.LongitudinalPersonality.standard) -> LeadPlan:
+  if not all(math.isfinite(value) for value in (v_ego, a_ego, delay)) or v_ego < 0.0 or delay < 0.0:
+    return LeadPlan()
+
+  leads = (radar_state.leadOne, radar_state.leadTwo)
+  lead_status = any(lead.present for lead in leads)
+  t_follow = get_T_FOLLOW(follow_personality)
+  if not math.isfinite(t_follow) or t_follow < 0.0:
+    return LeadPlan(lead_status=lead_status)
+
+  profile = sanitize_profile(profile)
+  x_ego, v_ego_delay = _project_ego(v_ego, a_ego, delay)
+  comfort_decel = COMFORT_DECEL[profile]
+  candidates: list[LeadPlan] = []
+  departure_candidates: list[tuple[float, int]] = []
+  departure_speeds = [math.inf, math.inf]
+  departure_distances = [-math.inf, -math.inf]
+  departure_track_ids = [-1, -1]
+  departure_separations = [-math.inf, -math.inf]
+  departure_caps = [math.inf, math.inf]
+
+  for lead_index, lead in enumerate(leads):
+    values = _lead_values(lead)
+    if values is None:
+      continue
+
+    d_rel, v_lead, a_lead, a_lead_tau = values
+    lead_xv = LongitudinalMpc.extrapolate_lead(d_rel, v_lead, a_lead, a_lead_tau)
+    x_lead = float(np.interp(delay, T_IDXS, lead_xv[:, 0]))
+    v_lead_delay = float(np.interp(delay, T_IDXS, lead_xv[:, 1]))
+    safety_gap = max(x_lead - x_ego - STOP_DISTANCE - t_follow * v_lead_delay, 0.0)
+    closing_speed = max(v_ego_delay - v_lead_delay, 0.0)
+    required_decel = 0.0 if closing_speed == 0.0 else math.inf if safety_gap == 0.0 else closing_speed**2 / (2.0 * safety_gap)
+    reserve = float(np.interp(v_lead_delay, (0.0, STOP_GAP_RESERVE_LEAD_SPEED), (STOP_GAP_RESERVE, 0.0)))
+    reserve_scale = float(np.interp(required_decel, STOP_GAP_RESERVE_DECEL_BP, (1.0, 0.0)))
+    usable_gap = max(safety_gap - reserve * reserve_scale, 0.0)
+    cap = v_lead_delay + math.sqrt(2.0 * comfort_decel * usable_gap)
+    departure_cap = v_lead_delay + math.sqrt(2.0 * comfort_decel * safety_gap)
+    separation = x_lead - x_ego
+    departure_distance = x_lead + float(get_stopped_equivalence_factor(v_lead_delay))
+
+    finite_values = (x_lead, v_lead_delay, safety_gap, usable_gap, closing_speed, cap, departure_cap, departure_distance)
+    if (not all(math.isfinite(value) and value >= 0.0 for value in finite_values) or math.isnan(required_decel)
+        or required_decel < 0.0 or not math.isfinite(separation)):
+      continue
+
+    track_id = max(int(lead.radarTrackId), -1) if math.isfinite(lead.radarTrackId) else -1
+    candidates.append(LeadPlan(
+      cap=cap, selected_lead=lead_index, selected_lead_track_id=track_id, selected_lead_speed=v_lead_delay, selected_lead_accel=a_lead,
+      usable_gap=usable_gap, closing_speed=closing_speed, required_decel=required_decel, lead_status=lead_status,
+    ))
+    departure_candidates.append((departure_distance, lead_index))
+    departure_speeds[lead_index] = v_lead_delay
+    departure_distances[lead_index] = d_rel
+    departure_track_ids[lead_index] = track_id
+    departure_separations[lead_index] = separation
+    departure_caps[lead_index] = departure_cap
+
+  if not candidates:
+    return LeadPlan(lead_status=lead_status)
+
+  selected = min(candidates, key=lambda candidate: candidate.cap)
+  departure_lead_index = min(departure_candidates, key=lambda candidate: candidate[0])[1]
+  departure_lead_speed = departure_speeds[departure_lead_index]
+  return selected._replace(
+    departure_lead_index=departure_lead_index, departure_lead_speed=departure_lead_speed,
+    departure_cap=departure_caps[departure_lead_index], departure_lead_speeds=tuple(departure_speeds),
+    departure_lead_distances=tuple(departure_distances), departure_lead_track_ids=tuple(departure_track_ids),
+    departure_lead_separations=tuple(departure_separations), has_nearly_stopped_lead=departure_lead_speed < STOPPED_LEAD_SPEED,
+  )

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

@@ -0,0 +1,190 @@
+import math
+from statistics import median
+
+import numpy as np
+
+from openpilot.cereal import custom
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import (
+  CAP_FILTER_FRAMES, DEPARTURE_MOTION_NOISE_FLOOR, DEPARTURE_MOTION_STEP_MIN, SPEED_RELIEF_DEADBAND, STOP_HOLD_CREEP_DISTANCE,
+  STOP_HOLD_CREEP_SPEED, STOP_HOLD_EXIT_FRAMES,
+)
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.lead import LeadPlan
+
+
+AccelControllerState = custom.LongitudinalPlanSP.AccelController.State
+
+
+class DepartureTracker:
+  def __init__(self) -> None:
+    self.samples: list[list[float]] = [[], []]
+    self.motion_samples: list[float] = []
+    self.references: list[float | None] = [None, None]
+    self.track_ids = [-1, -1]
+
+  def separation(self, lead_index: int) -> float:
+    samples = self.samples[lead_index]
+    return float(median(samples)) if samples else -math.inf
+
+  def update(self, lead_plan: LeadPlan, dt: float) -> None:
+    for lead_index, distance in enumerate(lead_plan.departure_lead_distances):
+      if not math.isfinite(distance):
+        continue
+      samples = self.samples[lead_index]
+      track_id = lead_plan.departure_lead_track_ids[lead_index]
+      identity_changed = bool(samples) and track_id != self.track_ids[lead_index] and (track_id >= 0 or self.track_ids[lead_index] >= 0)
+      max_distance_step = max(STOP_HOLD_CREEP_DISTANCE / 2.0, 3.0 * lead_plan.departure_lead_speeds[lead_index] * dt)
+      geometry_jump = bool(samples) and abs(distance - samples[-1]) > max_distance_step
+      if identity_changed or geometry_jump:
+        samples.clear()
+        self.references[lead_index] = distance
+      samples.append(distance)
+      if len(samples) > CAP_FILTER_FRAMES:
+        samples.pop(0)
+      self.track_ids[lead_index] = track_id
+    lead_index = lead_plan.departure_lead_index
+    if lead_index >= 0:
+      distance = lead_plan.departure_lead_distances[lead_index]
+      samples = self.motion_samples
+      max_distance_step = max(STOP_HOLD_CREEP_DISTANCE / 2.0, 3.0 * lead_plan.departure_lead_speed * dt)
+      if samples and abs(distance - samples[-1]) > max_distance_step:
+        samples.clear()
+      samples.append(distance)
+      if len(samples) > CAP_FILTER_FRAMES:
+        samples.pop(0)
+
+  def seed(self, lead_plan: LeadPlan) -> None:
+    self.samples = [[], []]
+    self.motion_samples = []
+    self.references = [None, None]
+    self.track_ids = list(lead_plan.departure_lead_track_ids)
+    for lead_index, distance in enumerate(lead_plan.departure_lead_distances):
+      if math.isfinite(distance):
+        self.samples[lead_index].append(distance)
+        self.references[lead_index] = distance
+    if lead_plan.departure_lead_index >= 0:
+      self.motion_samples.append(lead_plan.departure_lead_distances[lead_plan.departure_lead_index])
+
+  def progress(self, lead_plan: LeadPlan, minimum_distance: float) -> bool:
+    lead_index = lead_plan.departure_lead_index
+    if lead_index < 0 or lead_plan.departure_lead_speed <= STOP_HOLD_CREEP_SPEED:
+      return False
+    reference = self.references[lead_index]
+    distance = self.separation(lead_index)
+    return reference is not None and distance - reference >= minimum_distance
+
+  def recent_motion(self) -> bool:
+    samples = self.motion_samples[-STOP_HOLD_EXIT_FRAMES:]
+    if len(samples) < STOP_HOLD_EXIT_FRAMES:
+      return False
+    deltas = np.diff(samples)
+    return bool(samples[-1] - samples[0] >= DEPARTURE_MOTION_NOISE_FLOOR and np.count_nonzero(deltas > DEPARTURE_MOTION_STEP_MIN) >= 2)
+
+  def backfill_references(self) -> None:
+    for lead_index in range(len(self.references)):
+      separation = self.separation(lead_index)
+      if math.isfinite(separation) and self.references[lead_index] is None:
+        self.references[lead_index] = separation
+
+  def keep_latest_motion_sample(self) -> None:
+    if self.motion_samples:
+      self.motion_samples = self.motion_samples[-1:]
+
+
+class TargetState:
+  def __init__(self) -> None:
+    self.cap_samples = [math.inf] * CAP_FILTER_FRAMES
+    self.lead_speed_samples = [math.inf] * CAP_FILTER_FRAMES
+    self.lead_accel_samples = [0.0] * CAP_FILTER_FRAMES
+    self.departure = DepartureTracker()
+    self.target_speed: float | None = None
+    self.state = AccelControllerState.inactive
+    self.departure_frames = self.active_frames = self.lead_loss_frames = self.release_settle_frames = 0
+    self.lead_switch_guard_frames = self.lead_switch_elapsed_frames = self.lead_switch_stable_frames = self.stale_frames = 0
+    self.selected_lead = self.selected_lead_track_id = -1
+    self.launching = self.departure_launch = self.matched_lead = self.lead_dropout = self.release_slew_armed = False
+    self.lead_braking = self.e2e_braking_handoff = self.speed_reserve_armed = False
+    self.matched_accel_limit: float | None = None
+    self.release_settle_speed: float | None = None
+
+  def reset_lead_switch_guard(self) -> None:
+    self.lead_switch_guard_frames = self.lead_switch_elapsed_frames = self.lead_switch_stable_frames = 0
+
+  def arm_release_slew(self, force: bool = False) -> None:
+    if self.release_slew_armed:
+      self.release_settle_frames = 0
+      return
+    if not force and self.release_settle_speed is not None and self.target_speed is not None:
+      if self.release_settle_speed - self.target_speed < SPEED_RELIEF_DEADBAND:
+        return
+    self.release_slew_armed = True
+    self.release_settle_frames = 0
+    self.release_settle_speed = None
+
+  def reset_release_slew(self, settled_speed: float | None = None) -> None:
+    self.release_slew_armed = False
+    self.release_settle_frames = 0
+    self.release_settle_speed = settled_speed
+
+  def update_release_slew(self, ceiling: float, settled: bool) -> None:
+    if not self.release_slew_armed:
+      return
+    if not settled:
+      self.release_settle_frames = 0
+    elif self.release_settle_speed is None or ceiling > self.release_settle_speed:
+      self.release_settle_frames = 1
+      self.release_settle_speed = ceiling
+    else:
+      self.release_settle_frames += 1
+    if self.release_settle_frames >= CAP_FILTER_FRAMES:
+      self.release_slew_armed = False
+      self.release_settle_frames = 0
+
+  def update_lead_switch_guard(self, arm: bool, confirmed: bool, unstable: bool, hold_frames: int, max_frames: int) -> None:
+    if self.lead_switch_elapsed_frames > 0:
+      self.lead_switch_stable_frames = 0 if unstable else self.lead_switch_stable_frames + 1
+      if self.lead_switch_guard_frames == 0 and self.lead_switch_stable_frames >= hold_frames:
+        self.reset_lead_switch_guard()
+    if arm and self.lead_switch_elapsed_frames == 0:
+      self.lead_switch_guard_frames, self.lead_switch_elapsed_frames = hold_frames, 1
+    elif self.lead_switch_guard_frames > 0:
+      self.lead_switch_elapsed_frames += 1
+      if self.lead_switch_elapsed_frames >= max_frames:
+        self.lead_switch_guard_frames = 0
+      else:
+        self.lead_switch_guard_frames = self.lead_switch_guard_frames - 1 if confirmed else hold_frames
+
+  @property
+  def filtered_cap(self) -> float:
+    return sorted(self.cap_samples)[CAP_FILTER_FRAMES // 2]
+
+  @property
+  def filtered_lead_speed(self) -> float:
+    return sorted(self.lead_speed_samples)[CAP_FILTER_FRAMES // 2]
+
+  @property
+  def filtered_lead_accel(self) -> float:
+    return sorted(self.lead_accel_samples)[CAP_FILTER_FRAMES // 2]
+
+  def update_samples(self, lead_plan: LeadPlan, dt: float) -> bool:
+    had_filtered_lead = math.isfinite(self.filtered_cap)
+    has_lead = lead_plan.selected_lead >= 0
+    self.cap_samples.append(lead_plan.cap if has_lead else math.inf)
+    self.lead_speed_samples.append(lead_plan.selected_lead_speed if has_lead else math.inf)
+    self.lead_accel_samples.append(lead_plan.selected_lead_accel if has_lead else 0.0)
+    self.cap_samples.pop(0)
+    self.lead_speed_samples.pop(0)
+    self.lead_accel_samples.pop(0)
+    self.lead_loss_frames = 0 if has_lead else self.lead_loss_frames + 1
+    self.departure.update(lead_plan, dt)
+    return not had_filtered_lead and math.isfinite(self.filtered_cap)
+
+  def enter_stop_hold(self, lead_plan: LeadPlan) -> None:
+    self.departure.seed(lead_plan)
+    self.target_speed = 0.0
+    self.state = AccelControllerState.stopHold
+    self.departure_frames = 0
+    self.launching = self.departure_launch = False
+    self.reset_release_slew()
+    self.matched_lead = self.speed_reserve_armed = False
+    self.matched_accel_limit = None
+    self.reset_lead_switch_guard()

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

@@ -0,0 +1,532 @@
+import inspect
+import math
+from types import SimpleNamespace
+
+import numpy as np
+import pytest
+
+from openpilot.cereal import custom, log, messaging
+from opendbc.car.interfaces import ACCEL_MAX, ACCEL_MIN
+from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import N, LongitudinalMpc
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalPlanSource as MpcLongitudinalPlanSource
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController, AccelControllerState
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import (
+  MPC_DECEL_JERK_COST_MULTIPLIER, MPC_DECEL_JERK_MAX_REQUIRED_DECEL, MPC_DECEL_JERK_MAX_TARGET_REDUCTION, AccelProfile,
+)
+from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpcSP
+from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP, LongitudinalPlanSource
+
+
+def radar_state():
+  return messaging.new_message("radarState").radarState
+
+
+class PlannerSM(dict):
+  def __init__(self, radar_log_mono_time: int):
+    super().__init__(
+      radarState=radar_state(),
+      carState=SimpleNamespace(vEgo=10.0, aEgo=0.0, vCruise=20.0),
+      selfdriveState=SimpleNamespace(personality=0),
+      controlsState=SimpleNamespace(forceDecel=False),
+    )
+    self.valid = {"radarState": True}
+    self.alive = {"radarState": True}
+    self.logMonoTime = {"radarState": radar_log_mono_time}
+
+
+class ControllerStub:
+  def __init__(self, *, target_speed=15.0, active=True, mpc_accel_max=None, cruise_accel_max=None,
+               state=AccelControllerState.free, selected_lead=-1,
+               selected_lead_track_id=-1, launching=False, departure_launching=False, required_decel=0.0):
+    self.available = self.enabled = True
+    self.profile = AccelProfile.normal
+    self.output_v_target = target_speed
+    self.is_active = active
+    self.mpc_accel_max = mpc_accel_max
+    self.cruise_accel_max = cruise_accel_max
+    self.state = state
+    self.selected_lead = selected_lead
+    self.selected_lead_track_id = selected_lead_track_id
+    self.launching = launching
+    self.departure_launching = departure_launching
+    self.required_decel = required_decel
+    self.dt = DT_MDL
+    self._jerk_smoothing_blocked = False
+    self._required_decel_samples = []
+    self._required_decel_lead = -1
+    self._required_decel_lead_track_id = -1
+    self._lead_trend_warmup = False
+    self.update_kwargs = None
+    self.reset_calls = 0
+
+  def update(self, _radar_state, **kwargs):
+    self.update_kwargs = kwargs
+
+  @property
+  def is_enabled(self):
+    return self.available and self.enabled
+
+  def update_params(self):
+    pass
+
+  def reset(self):
+    self.reset_calls += 1
+
+  def get_jerk_cost_multiplier(self, *args):
+    return AccelController.get_jerk_cost_multiplier(self, *args)
+
+  def update_should_stop(self, should_stop):
+    return AccelController.update_should_stop(self, should_stop)
+
+
+def planner_for_mpc_test(*, target_speed=15.0, active=True, is_e2e=False, mpc_accel_max=None,
+                         cruise_accel_max=None,
+                         state=AccelControllerState.free, selected_lead=-1, launching=False,
+                         departure_launching=False, required_decel=0.0,
+                         mpc_source=MpcLongitudinalPlanSource.lead0):
+  planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
+  is_e2e_calls = []
+  planner.is_e2e = lambda _sm: is_e2e_calls.append(True) or is_e2e
+  planner.output_v_target = 20.0
+  planner.output_should_stop = False
+  planner.allow_throttle = True
+  planner.a_desired = 0.0
+  planner.v_desired_filter = SimpleNamespace(x=10.0)
+  planner._radar_fresh_this_cycle = True
+  planner.mpc = SimpleNamespace(source=mpc_source, last_solution_status=0)
+  planner.accel_controller = ControllerStub(
+    target_speed=target_speed, active=active, state=state, selected_lead=selected_lead, launching=launching,
+    departure_launching=departure_launching, required_decel=required_decel, mpc_accel_max=mpc_accel_max,
+    cruise_accel_max=cruise_accel_max,
+  )
+  return planner, is_e2e_calls
+
+
+def prepare_controller_mpc(planner, *, mpc_v_cruise=20.0, force_decel=False):
+  configs = []
+  sm = {
+    "radarState": radar_state(),
+    "controlsState": SimpleNamespace(forceDecel=force_decel),
+    "carState": SimpleNamespace(vCruise=20.0, vEgo=10.0, aEgo=0.0),
+    "selfdriveState": SimpleNamespace(personality=0),
+  }
+  planner.mpc.set_accel_controller_params = lambda *args: configs.append(args)
+  is_e2e, target = planner.update_accel_controller(sm, mpc_v_cruise, True, ACCEL_MAX, False)
+  assert len(configs) == 1
+  return is_e2e, target, configs[0]
+
+
+def test_accel_controller_schema_contract():
+  expected = {"eco": 0, "normal": 1, "sport": 2}
+  state = {"inactive": 0, "free": 1, "restrict": 2, "hold": 3, "release": 4, "stopHold": 5}
+  accel_controller = custom.LongitudinalPlanSP.schema.fields["accelController"]
+  fields = custom.LongitudinalPlanSP.AccelController.schema.fields
+
+  assert accel_controller.proto.ordinal.explicit == 8
+  assert {name: field.proto.ordinal.explicit for name, field in fields.items()} == {
+    "enabled": 0, "active": 1, "shadowOnlyDEPRECATED": 2, "profile": 3, "state": 4,
+  }
+  assert fields["shadowOnlyDEPRECATED"].proto.slot.type.which() == "bool"
+  assert custom.LongitudinalPlanSP.AccelerationPersonality.schema.enumerants == expected
+  assert custom.LongitudinalPlanSP.AccelController.Profile.schema.enumerants == expected
+  assert custom.LongitudinalPlanSP.AccelController.State.schema.enumerants == state
+
+
+def test_accel_controller_schema_round_trip_and_toyota_compatibility():
+  message = custom.LongitudinalPlanSP.new_message()
+  message.accelController.enabled = True
+  message.accelController.active = True
+  message.accelController.profile = custom.LongitudinalPlanSP.AccelController.Profile.sport
+  message.accelController.state = custom.LongitudinalPlanSP.AccelController.State.release
+
+  with custom.LongitudinalPlanSP.from_bytes(message.to_bytes()) as reader:
+    assert reader.accelController.enabled and reader.accelController.active
+    assert reader.accelController.profile == custom.LongitudinalPlanSP.AccelController.Profile.sport
+    assert reader.accelController.state == custom.LongitudinalPlanSP.AccelController.State.release
+
+  from opendbc.car.toyota.carstate import AccelPersonality, CarState
+
+  assert AccelPersonality.schema.enumerants == {"eco": 0, "normal": 1, "sport": 2}
+  assert CarState.__module__ == "opendbc.car.toyota.carstate"
+
+
+def test_mpc_inherits_accel_controller_extension_without_changing_stock_signature_or_bounds():
+  assert LongitudinalMpc.__bases__ == (LongitudinalMpcSP,)
+  assert tuple(inspect.signature(LongitudinalMpc.update).parameters) == ("self", "radarstate", "v_cruise", "personality")
+  mpc = LongitudinalMpc()
+  radar = radar_state()
+  mpc.run = lambda: None
+
+  mpc.set_cur_state(10.0, 0.8)
+  mpc.update(radar, 30.0)
+  np.testing.assert_array_equal(mpc.params[:, 0], ACCEL_MIN)
+  np.testing.assert_array_equal(mpc.params[:, 1], ACCEL_MAX)
+  assert mpc.cruise_accel_max(1.6) == 1.6
+
+  mpc.set_accel_controller_params(None, 1.0, 0.4)
+  assert mpc.cruise_accel_max(1.6) == 0.4
+
+  requested_ceiling = tuple(np.full(N + 1, 0.4))
+  mpc.set_accel_controller_params(requested_ceiling, 1.0)
+  mpc.update(radar, 30.0)
+  np.testing.assert_array_equal(mpc.params[:, 0], ACCEL_MIN)
+  assert mpc.params[0, 1] == pytest.approx(0.8)
+  np.testing.assert_array_equal(mpc.params[1:, 1], requested_ceiling[1:])
+
+  for malformed_ceiling in ("bad", [0.4] * N, np.full(N + 1, math.nan), [10**10000] * (N + 1)):
+    mpc.set_accel_controller_params(malformed_ceiling, 1.0)
+    mpc.update(radar, 30.0)
+    np.testing.assert_array_equal(mpc.params[:, 0], ACCEL_MIN)
+    np.testing.assert_array_equal(mpc.params[:, 1], ACCEL_MAX)
+
+  mpc.set_accel_controller_params(None, 1.0)
+  mpc.update(radar, 30.0)
+  np.testing.assert_array_equal(mpc.params[:, 1], ACCEL_MAX)
+
+
+def test_mpc_jerk_cost_multiplier_is_backward_compatible_and_does_not_change_other_costs():
+  mpc = LongitudinalMpc.__new__(LongitudinalMpc)
+  LongitudinalMpcSP.__init__(mpc)
+  captured = []
+  mpc.set_cost_weights = lambda costs, constraints: captured.append((np.asarray(costs), np.asarray(constraints)))
+
+  mpc.set_weights(True, personality=log.LongitudinalPersonality.standard)
+  default_costs, default_constraints = captured[-1]
+  mpc.set_accel_controller_params(None, 1.0)
+  mpc.set_weights(True, personality=log.LongitudinalPersonality.standard)
+  explicit_costs, explicit_constraints = captured[-1]
+  mpc.set_accel_controller_params(None, 1.2)
+  mpc.set_weights(True, personality=log.LongitudinalPersonality.standard)
+  smoothed_costs, smoothed_constraints = captured[-1]
+
+  np.testing.assert_array_equal(explicit_costs, default_costs)
+  np.testing.assert_array_equal(explicit_constraints, default_constraints)
+  np.testing.assert_array_equal(smoothed_costs[:-1], default_costs[:-1])
+  assert smoothed_costs[-1] == pytest.approx(default_costs[-1] * 1.2)
+  np.testing.assert_array_equal(smoothed_constraints, default_constraints)
+
+  mpc.set_weights(False, personality=log.LongitudinalPersonality.standard)
+  assert captured[-1][0][-2] == 0.0
+  assert captured[-1][0][-1] == pytest.approx(default_costs[-1] * 1.2)
+
+
+def test_accel_controller_hook_only_configures_mpc():
+  radar = radar_state()
+  planner, _ = planner_for_mpc_test(active=False)
+  calls = []
+  planner.mpc = SimpleNamespace(
+    source=MpcLongitudinalPlanSource.cruise,
+    last_solution_status=0,
+    set_accel_controller_params=lambda accel_max, multiplier, cruise_accel_max: calls.append(
+      ("configure", accel_max, multiplier, cruise_accel_max)),
+    set_weights=lambda constraint, personality: calls.append(("weights", constraint, personality)),
+    set_cur_state=lambda speed, accel: calls.append(("state", speed, accel)),
+    update=lambda radar_arg, target, *, personality: calls.append(("update", radar_arg, target, personality)),
+  )
+  sm = {
+    "radarState": radar,
+    "controlsState": SimpleNamespace(forceDecel=False),
+    "carState": SimpleNamespace(vCruise=20.0, vEgo=10.0, aEgo=0.0),
+    "selfdriveState": SimpleNamespace(personality=2),
+  }
+  is_e2e, target = planner.update_accel_controller(sm, 17.5, True, ACCEL_MAX, False)
+
+  assert not is_e2e and target == 17.5
+  assert calls == [("configure", None, 1.0, None)]
+
+
+def test_active_acc_uses_target_and_ceiling_in_exactly_one_solve():
+  ceiling = tuple(np.linspace(0.8, 0.4, N + 1))
+  planner, mode_calls = planner_for_mpc_test(mpc_accel_max=ceiling)
+  is_e2e, target, config = prepare_controller_mpc(planner)
+
+  assert not is_e2e
+  assert len(mode_calls) == 1
+  assert target == 15.0
+  assert config == (ceiling, 1.0, None)
+
+
+def test_cruise_accel_ceiling_is_forwarded_to_mpc():
+  planner, _ = planner_for_mpc_test(cruise_accel_max=0.3)
+  _, _, config = prepare_controller_mpc(planner)
+  assert config == (None, 1.0, 0.3)
+
+
+def test_valid_lead_stop_hold_preplans_from_raw_target_without_an_accel_ceiling():
+  planner, _ = planner_for_mpc_test(
+    target_speed=0.0, mpc_accel_max=None, state=AccelControllerState.stopHold, selected_lead=0,
+  )
+  _, target, config = prepare_controller_mpc(planner)
+
+  assert target == 20.0
+  assert config == (None, 1.0, None)
+
+
+def test_missing_lead_stop_hold_keeps_zero_mpc_target_without_an_accel_ceiling():
+  planner, _ = planner_for_mpc_test(
+    target_speed=0.0, mpc_accel_max=None, state=AccelControllerState.stopHold, selected_lead=-1,
+  )
+  _, target, config = prepare_controller_mpc(planner)
+
+  assert target == 0.0
+  assert config == (None, 1.0, None)
+
+
+@pytest.mark.parametrize(
+  ("active", "departure_launching", "expected"),
+  [
+    (True, True, False),
+    (True, False, True),
+    (False, True, True),
+  ],
+)
+def test_only_confirmed_live_acc_departure_clears_should_stop(active, departure_launching, expected):
+  planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
+  planner.accel_controller = ControllerStub(active=active, departure_launching=departure_launching, state=AccelControllerState.stopHold)
+  assert planner.update_should_stop(True) is expected
+  assert planner.update_should_stop(False) is (active and not departure_launching)
+
+
+@pytest.mark.parametrize(("active", "is_e2e"), [(False, False), (True, True)])
+def test_disabled_or_e2e_is_an_exact_mpc_bypass(active, is_e2e):
+  ceiling = tuple(np.linspace(0.8, 0.4, N + 1))
+  planner, mode_calls = planner_for_mpc_test(active=active, is_e2e=is_e2e, mpc_accel_max=ceiling)
+  returned_e2e, target, config = prepare_controller_mpc(planner)
+
+  assert returned_e2e is is_e2e
+  assert len(mode_calls) == 1
+  assert target == 20.0
+  assert config == (None, 1.0, None)
+
+
+def test_force_decel_target_remains_authoritative_and_disables_ceiling():
+  ceiling = tuple(np.linspace(0.8, 0.4, N + 1))
+  planner, mode_calls = planner_for_mpc_test(mpc_accel_max=ceiling)
+  _, target, config = prepare_controller_mpc(planner, mpc_v_cruise=0.0, force_decel=True)
+
+  assert len(mode_calls) == 1
+  assert target == 0.0
+  assert config == (None, 1.0, None)
+
+
+def test_previous_mpc_failure_gets_one_stock_recovery_cycle_without_resetting_controller_state():
+  ceiling = tuple(np.linspace(0.8, 0.4, N + 1))
+  planner, mode_calls = planner_for_mpc_test(mpc_accel_max=ceiling)
+  controller = planner.accel_controller
+  planner.mpc.last_solution_status = 4
+
+  _, failed_target, failed_config = prepare_controller_mpc(planner)
+  assert controller.reset_calls == 0
+  assert controller.update_kwargs["acc_selected"]
+  assert len(mode_calls) == 1
+  assert failed_target == 20.0
+  assert failed_config == (None, 1.0, None)
+
+  planner.mpc.last_solution_status = 0
+  _, recovered_target, recovered_config = prepare_controller_mpc(planner)
+  assert controller.reset_calls == 0
+  assert len(mode_calls) == 2
+  assert recovered_target == 15.0
+  assert recovered_config == (ceiling, 1.0, None)
+
+
+@pytest.mark.parametrize(
+  "mpc_source",
+  (MpcLongitudinalPlanSource.cruise, MpcLongitudinalPlanSource.lead0, MpcLongitudinalPlanSource.lead1),
+)
+def test_routine_governor_restriction_forwards_the_jerk_cost_multiplier(mpc_source):
+  planner, _ = planner_for_mpc_test(
+    state=AccelControllerState.restrict, selected_lead=0, required_decel=0.30,
+    mpc_source=mpc_source,
+  )
+  _, target, config = prepare_controller_mpc(planner)
+
+  assert target == 15.0
+  assert config == (None, MPC_DECEL_JERK_COST_MULTIPLIER, None)
+
+
+def test_ineligible_required_decel_blocks_smoothing_only_until_the_restriction_episode_ends():
+  planner, _ = planner_for_mpc_test(
+    state=AccelControllerState.restrict, selected_lead=0, required_decel=0.30,
+  )
+  _, _, initial_config = prepare_controller_mpc(planner)
+  controller = planner.accel_controller
+  assert initial_config[1] == MPC_DECEL_JERK_COST_MULTIPLIER
+
+  controller.required_decel = MPC_DECEL_JERK_MAX_REQUIRED_DECEL
+  _, _, ineligible_config = prepare_controller_mpc(planner)
+  assert ineligible_config[1] == 1.0
+
+  controller.required_decel = 0.30
+  _, _, flicker_config = prepare_controller_mpc(planner)
+  assert flicker_config[1] == 1.0
+
+  controller.state = AccelControllerState.free
+  controller.output_v_target = 20.0
+  prepare_controller_mpc(planner)
+  controller.state = AccelControllerState.restrict
+  controller.output_v_target = 15.0
+  _, _, rearmed_config = prepare_controller_mpc(planner)
+  assert rearmed_config[1] == MPC_DECEL_JERK_COST_MULTIPLIER
+
+
+def test_consistently_tightening_lead_releases_smoothing_until_the_restriction_ends():
+  planner, _ = planner_for_mpc_test(
+    state=AccelControllerState.restrict, selected_lead=0, required_decel=0.18,
+  )
+  _, _, config = prepare_controller_mpc(planner)
+  controller = planner.accel_controller
+  multipliers = [config[1]]
+  for required_decel in (0.20, 0.23, 0.25):
+    controller.required_decel = required_decel
+    _, _, config = prepare_controller_mpc(planner)
+    multipliers.append(config[1])
+
+  assert multipliers == [MPC_DECEL_JERK_COST_MULTIPLIER] * 3 + [1.0]
+
+  controller.required_decel = 0.20
+  _, _, config = prepare_controller_mpc(planner)
+  assert config[1] == 1.0
+
+  controller.state = AccelControllerState.free
+  controller.output_v_target = 20.0
+  prepare_controller_mpc(planner)
+  controller.state = AccelControllerState.restrict
+  controller.output_v_target = 15.0
+  controller.required_decel = 0.18
+  _, _, config = prepare_controller_mpc(planner)
+  assert config[1] == MPC_DECEL_JERK_COST_MULTIPLIER
+
+
+def test_one_frame_required_decel_noise_does_not_disable_routine_smoothing():
+  planner, _ = planner_for_mpc_test(
+    state=AccelControllerState.restrict, selected_lead=0, required_decel=0.18,
+  )
+  _, _, config = prepare_controller_mpc(planner)
+  controller = planner.accel_controller
+  multipliers = [config[1]]
+  for required_decel in (0.24, 0.19, 0.22):
+    controller.required_decel = required_decel
+    _, _, config = prepare_controller_mpc(planner)
+    multipliers.append(config[1])
+
+  assert multipliers == [MPC_DECEL_JERK_COST_MULTIPLIER] * 4
+
+
+@pytest.mark.parametrize(
+  ("state", "selected_lead", "launching", "required_decel", "target_speed", "mpc_source"),
+  [
+    (AccelControllerState.free, 0, False, 0.30, 15.0, MpcLongitudinalPlanSource.cruise),
+    (AccelControllerState.hold, 0, False, 0.30, 15.0, MpcLongitudinalPlanSource.cruise),
+    (AccelControllerState.stopHold, 0, False, 0.30, 15.0, MpcLongitudinalPlanSource.cruise),
+    (AccelControllerState.restrict, -1, False, 0.30, 15.0, MpcLongitudinalPlanSource.cruise),
+    (AccelControllerState.restrict, 0, True, 0.30, 15.0, MpcLongitudinalPlanSource.cruise),
+    (AccelControllerState.restrict, 0, False, MPC_DECEL_JERK_MAX_REQUIRED_DECEL, 15.0, MpcLongitudinalPlanSource.cruise),
+    (AccelControllerState.restrict, 0, False, math.inf, 15.0, MpcLongitudinalPlanSource.cruise),
+    (AccelControllerState.restrict, 0, False, math.nan, 15.0, MpcLongitudinalPlanSource.cruise),
+    (AccelControllerState.restrict, 0, False, 0.0, 15.0, MpcLongitudinalPlanSource.cruise),
+    (AccelControllerState.restrict, 0, False, -0.01, 15.0, MpcLongitudinalPlanSource.cruise),
+    (AccelControllerState.restrict, 0, False, 0.30, 20.0 - MPC_DECEL_JERK_MAX_TARGET_REDUCTION, MpcLongitudinalPlanSource.cruise),
+    (AccelControllerState.restrict, 0, False, 0.30, 20.0, MpcLongitudinalPlanSource.cruise),
+    (AccelControllerState.restrict, 0, False, 0.30, 25.0, MpcLongitudinalPlanSource.cruise),
+  ],
+)
+def test_non_routine_or_stock_lead_states_keep_stock_jerk_cost(
+  state, selected_lead, launching, required_decel, target_speed, mpc_source,
+):
+  planner, _ = planner_for_mpc_test(
+    state=state, selected_lead=selected_lead, launching=launching,
+    required_decel=required_decel, target_speed=target_speed, mpc_source=mpc_source,
+  )
+  _, _, config = prepare_controller_mpc(planner)
+
+  assert config[1] == 1.0
+
+
+def test_controller_receives_previous_mpc_state_and_cached_radar_freshness():
+  planner, _ = planner_for_mpc_test(mpc_source=log.LongitudinalPlan.LongitudinalPlanSource.lead0)
+  planner._radar_fresh_this_cycle = True
+  planner.a_desired = -0.4
+  planner.v_desired_filter = SimpleNamespace(x=9.5)
+  prepare_controller_mpc(planner)
+  received = planner.accel_controller.update_kwargs
+
+  assert received["previous_mpc_source"] == log.LongitudinalPlan.LongitudinalPlanSource.lead0
+  assert received["planner_speed"] == 9.5
+  assert received["planner_accel"] == -0.4
+  assert received["radar_fresh"] is True
+
+
+def test_controller_is_disabled_when_openpilot_longitudinal_control_is_unavailable():
+  controller = AccelController(SimpleNamespace(longitudinalActuatorDelay=0.1, openpilotLongitudinalControl=False))
+  controller.enabled = True
+  assert not controller.is_enabled
+
+
+def test_radar_freshness_is_computed_once_and_shared_with_dec_and_controller():
+  planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
+  planner._radar_log_mono_time = None
+  planner._radar_fresh_this_cycle = True
+  planner.events_sp = SimpleNamespace(clear=lambda: None)
+  dec_freshness = []
+  planner.dec = SimpleNamespace(update=lambda _sm, *, radar_fresh, planner_accel: dec_freshness.append(radar_fresh))
+  planner.e2e_alerts_helper = SimpleNamespace(update=lambda *_args: None)
+  planner.output_a_target = 0.0
+  planner.output_v_target = 20.0
+  planner.output_should_stop = False
+  planner.allow_throttle = True
+  planner.a_desired = 0.0
+  planner.v_desired_filter = SimpleNamespace(x=10.0)
+  planner.mpc = SimpleNamespace(
+    source=log.LongitudinalPlan.LongitudinalPlanSource.cruise, last_solution_status=0,
+    set_accel_controller_params=lambda *_args: None,
+  )
+  planner.is_e2e = lambda _sm: False
+  planner.accel_controller = ControllerStub(target_speed=20.0, active=False)
+
+  sm = PlannerSM(100)
+  for expected in (True, False):
+    planner.update(sm)
+    planner.update_accel_controller(sm, 20.0, True, ACCEL_MAX, False)
+    assert dec_freshness[-1] is expected and planner.accel_controller.update_kwargs["radar_fresh"] is expected
+
+  sm.logMonoTime["radarState"] = 101
+  planner.update(sm)
+  planner.update_accel_controller(sm, 20.0, True, ACCEL_MAX, False)
+  assert dec_freshness[-1] is True and planner.accel_controller.update_kwargs["radar_fresh"] is True
+
+
+def test_accel_controller_status_publishes_minimal_fields():
+  planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
+  planner.source = LongitudinalPlanSource.cruise
+  planner.output_v_target = 20.0
+  planner.output_a_target = 0.0
+  planner.events_sp = SimpleNamespace(to_msg=list)
+  planner.dec = SimpleNamespace(mode=lambda: "acc", enabled=lambda: False, active=lambda: False)
+  planner.accel_controller = ControllerStub(active=False, state=AccelControllerState.restrict)
+  planner.scc = SimpleNamespace(
+    vision=SimpleNamespace(state=0, output_v_target=20.0, output_a_target=0.0, current_lat_acc=0.0, max_pred_lat_acc=0.0, is_enabled=False, is_active=False),
+    map=SimpleNamespace(state=0, output_v_target=20.0, output_a_target=0.0, is_enabled=False, is_active=False),
+  )
+  planner.resolver = SimpleNamespace(
+    speed_limit=0.0, speed_limit_last=0.0, speed_limit_final=0.0, speed_limit_final_last=0.0,
+    speed_limit_valid=False, speed_limit_last_valid=False, speed_limit_offset=0.0, distance=0.0,
+    source=custom.LongitudinalPlanSP.SpeedLimit.Source.none,
+  )
+  planner.sla = SimpleNamespace(
+    state=custom.LongitudinalPlanSP.SpeedLimit.AssistState.disabled, is_enabled=False, is_active=False,
+    output_v_target=20.0, output_a_target=0.0,
+  )
+  planner.e2e_alerts_helper = SimpleNamespace(green_light_alert=False, lead_depart_alert=False)
+  sent = {}
+  planner.publish_longitudinal_plan_sp(
+    SimpleNamespace(all_checks=lambda service_list: True),
+    SimpleNamespace(send=lambda service, message: sent.update({service: message})),
+  )
+
+  telemetry = sent["longitudinalPlanSP"].longitudinalPlanSP.accelController
+  assert telemetry.enabled and not telemetry.active
+  assert telemetry.profile == int(AccelProfile.normal)
+  assert telemetry.state == int(AccelControllerState.restrict)
+  assert set(custom.LongitudinalPlanSP.AccelController.schema.fields) == {"enabled", "active", "shadowOnlyDEPRECATED", "profile", "state"}

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

@@ -1,17 +1,48 @@
+from openpilot.common.realtime import DT_MDL
+
+
 class WMACConstants:
-  # Lead detection parameters
-  LEAD_WINDOW_SIZE = 6  # Stable detection window
-  LEAD_PROB = 0.45  # Balanced threshold for lead detection
+  TRAJECTORY_SIZE = 33
+  PARAM_READ_FRAMES = max(1, int(round(1.0 / DT_MDL)))
 
-  # Slow down detection parameters
-  SLOW_DOWN_WINDOW_SIZE = 5  # Responsive but stable
-  SLOW_DOWN_PROB = 0.3  # Balanced threshold for slow down scenarios
+  EMERGENCY_HOLD_FRAMES = max(1, int(round(0.75 / DT_MDL)))
+  MIN_MODE_DURATION = {'acc': max(1, int(round(0.6 / DT_MDL))), 'blended': max(1, int(round(0.5 / DT_MDL)))}
+  ENTER_BLENDED_FRAMES = max(1, int(round(0.4 / DT_MDL)))
+  EXIT_BLENDED_FRAMES = max(1, int(round(0.35 / DT_MDL)))
+  STANDSTILL_FRAMES = max(1, int(round(0.2 / DT_MDL)))
 
-  # Optimized slow down distance curve - smooth and progressive
+  LEAD_PROB = 0.45
+  LEAD_EXIT_PROB = 0.25
+  LEAD_RISE_RATE = 1.0
+  LEAD_FALL_RATE = 0.35
+  RADAR_LEAD_CONTINUITY_FRAMES = max(1, int(round(1.0 / DT_MDL)))
+  RADAR_LEAD_DROPOUT_FRAMES = max(1, int(round(0.2 / DT_MDL)))
+  RADAR_STALE_FRAMES = max(1, int(round(0.5 / DT_MDL)))
+
+  SLOW_DOWN_PROB = 0.5
+  SLOW_DOWN_EXIT_PROB = 0.4
+  SLOW_DOWN_RISE_RATE = 0.65
+  SLOW_DOWN_FALL_RATE = 0.15
   SLOW_DOWN_BP = [0., 10., 20., 30., 40., 50., 55., 60.]
   SLOW_DOWN_DIST = [32., 46., 64., 86., 108., 130., 145., 165.]
+  URGENT_SLOW_DOWN_PROB = 0.85
 
-  # 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
+  MODEL_DECEL_START = -0.5
+  MODEL_DECEL_RANGE = 2.0
+  MODEL_DECEL_TREND_FRAMES = 4
+  MODEL_DECEL_TREND_ACCEL = -0.075
+  MODEL_DECEL_TREND_RATE = 0.35
+  MODEL_DECEL_TREND_MAX_MPC_ACCEL = 0.075
+  MODEL_DECEL_TREND_MAX_COMMAND_STEP = 0.15
+  MODEL_DECEL_TREND_RELEASE_ACCEL = -0.02
+  ENDPOINT_URGENCY_GAIN = 1.3
+  CRITICAL_ENDPOINT_FACTOR = 0.3
+  CRITICAL_URGENCY_GAIN = 1.5
+  SPEED_URGENCY_MIN = 25.0
+  SPEED_URGENCY_RANGE = 80.0
+
+  SLOWNESS_PROB = 0.55
+  SLOWNESS_EXIT_PROB = 0.45
+  SLOWNESS_RISE_RATE = 0.35
+  SLOWNESS_FALL_RATE = 0.5
+  SLOWNESS_CRUISE_OFFSET = 1.025

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

@@ -6,129 +6,119 @@ See the LICENSE.md file in the root directory for more details.
 """
 # Version = 2025-6-30
 
+from collections import deque
+import math
+from typing import Literal
+
 from openpilot.cereal import messaging
-from opendbc.car import structs
 from numpy import interp
+from opendbc.car import structs
 from openpilot.common.params import Params
 from openpilot.common.realtime import DT_MDL
 from openpilot.sunnypilot.selfdrive.controls.lib.dec.constants import WMACConstants
-from typing import Literal
 
-# d-e2e, from modeldata.h
-TRAJECTORY_SIZE = 33
-SET_MODE_TIMEOUT = 15
-
-# Define the valid mode types
 ModeType = Literal['acc', 'blended']
 
 
-class SmoothKalmanFilter:
-  """Enhanced Kalman filter with smoothing for stable decision making."""
+def clip01(value: float) -> float:
+  return max(0.0, min(1.0, float(value)))
 
-  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
 
-  def add_data(self, measurement):
-    if len(self.history) >= self.max_history:
-      self.history.pop(0)
-    self.history.append(measurement)
+class SmoothedSignal:
+  def __init__(self, rise_rate: float, fall_rate: float, initial_value: float = 0.0):
+    self.rise_rate = clip01(rise_rate)
+    self.fall_rate = clip01(fall_rate)
+    self.value = clip01(initial_value)
 
-    if not self.initialized:
-      self.x = measurement
-      self.initialized = True
-      self.confidence = 0.1
-      return
+  def update(self, measurement: float) -> float:
+    measurement = clip01(measurement)
+    rate = self.rise_rate if measurement > self.value else self.fall_rate
+    self.value += (measurement - self.value) * rate
+    return self.value
 
-    self.P = self.alpha * self.P + self.Q
+  def reset(self, value: float = 0.0) -> None:
+    self.value = clip01(value)
 
-    K = self.P / (self.P + self.R)
-    effective_K = K * (1.0 - self.smoothing_factor) + self.smoothing_factor * 0.1
 
-    innovation = measurement - self.x
-    self.x = self.x + effective_K * innovation
-    self.P = (1 - effective_K) * self.P
+class HysteresisSignal:
+  def __init__(self, enter_threshold: float, exit_threshold: float, rise_rate: float, fall_rate: float):
+    self.enter_threshold = clip01(enter_threshold)
+    self.exit_threshold = clip01(exit_threshold)
+    self.filter = SmoothedSignal(rise_rate, fall_rate)
+    self.active = False
 
-    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 update(self, measurement: float) -> bool:
+    value = self.filter.update(measurement)
+    threshold = self.exit_threshold if self.active else self.enter_threshold
+    self.active = value > threshold
+    return self.active
 
-  def get_value(self):
-    return self.x if self.initialized else None
+  def reset(self) -> None:
+    self.filter.reset()
+    self.active = False
 
-  def get_confidence(self):
-    return self.confidence
-
-  def reset_data(self):
-    self.initialized = False
-    self.history = []
-    self.confidence = 0.0
+  @property
+  def value(self) -> float:
+    return self.filter.value
 
 
 class ModeTransitionManager:
-  """Manages smooth transitions between driving modes with hysteresis."""
-
   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._pending_mode: ModeType = 'acc'
+    self._pending_count = 0
+    self._blended_hold_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
+  def request_mode(self, mode: ModeType, immediate: bool = False, hold_frames: int = 0, cancel_hold: bool = False) -> None:
+    if immediate:
+      self._blended_hold_frames = max(self._blended_hold_frames, hold_frames) if mode == 'blended' else 0
+      self._pending_mode = mode
+      self._pending_count = 0
+      self._switch_mode(mode)
       return
 
-    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 cancel_hold and mode == 'acc':
+      self._blended_hold_frames = 0
 
-    # Require minimum duration in current mode (unless emergency)
-    if self.mode_duration < self.min_mode_duration and not self.emergency_override:
+    if self._blended_hold_frames > 0:
+      mode = 'blended'
+
+    if mode == self.current_mode:
+      self._pending_mode = mode
+      self._pending_count = 0
       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 mode != self._pending_mode:
+      self._pending_mode = mode
+      self._pending_count = 1
+    else:
+      self._pending_count += 1
 
-    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
+    if self.mode_duration < WMACConstants.MIN_MODE_DURATION[self.current_mode]:
+      return
 
-  def update(self):
-    if self.transition_timeout > 0:
-      self.transition_timeout -= 1
+    required_count = WMACConstants.ENTER_BLENDED_FRAMES if mode == 'blended' else WMACConstants.EXIT_BLENDED_FRAMES
+    if self._pending_count >= required_count:
+      self._switch_mode(mode)
+
+  def update(self) -> None:
+    if self._blended_hold_frames > 0:
+      self._blended_hold_frames -= 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 _switch_mode(self, mode: ModeType) -> None:
+    if mode == self.current_mode:
+      return
+
+    self.current_mode = mode
+    self.mode_duration = 0
+    self._pending_mode = mode
+    self._pending_count = 0
+
 
 class DynamicExperimentalController:
   def __init__(self, CP: structs.CarParams, mpc, params=None):
@@ -142,35 +132,32 @@ class DynamicExperimentalController:
 
     self._mode_manager = ModeTransitionManager()
 
-    # 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._lead_tracker = HysteresisSignal(
+      enter_threshold=WMACConstants.LEAD_PROB,
+      exit_threshold=WMACConstants.LEAD_EXIT_PROB,
+      rise_rate=WMACConstants.LEAD_RISE_RATE,
+      fall_rate=WMACConstants.LEAD_FALL_RATE,
+    )
+    self._slow_down_tracker = HysteresisSignal(
+      enter_threshold=WMACConstants.SLOW_DOWN_PROB,
+      exit_threshold=WMACConstants.SLOW_DOWN_EXIT_PROB,
+      rise_rate=WMACConstants.SLOW_DOWN_RISE_RATE,
+      fall_rate=WMACConstants.SLOW_DOWN_FALL_RATE,
+    )
+    self._slowness_tracker = HysteresisSignal(
+      enter_threshold=WMACConstants.SLOWNESS_PROB,
+      exit_threshold=WMACConstants.SLOWNESS_EXIT_PROB,
+      rise_rate=WMACConstants.SLOWNESS_RISE_RATE,
+      fall_rate=WMACConstants.SLOWNESS_FALL_RATE,
     )
 
-    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_any_lead = False
+    self._has_current_radar_acc_lead = False
+    self._has_radar_acc_lead = False
+    self._radar_acc_lead_frames = 0
+    self._radar_fresh = True
+    self._radar_stale_frames = 0
     self._has_slow_down = False
     self._has_slowness = False
     self._has_mpc_fcw = False
@@ -179,13 +166,18 @@ class DynamicExperimentalController:
     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
+    self._raw_urgency = 0.0
+    self._model_accel_samples = deque(maxlen=WMACConstants.MODEL_DECEL_TREND_FRAMES)
+    self._model_decel_trending = False
+    self._model_decel_latched = False
+    self._planner_accel = math.nan
 
   def _read_params(self) -> None:
-    if self._frame % int(1. / DT_MDL) == 0:
+    if self._frame % WMACConstants.PARAM_READ_FRAMES == 0:
       self._enabled = self._params.get_bool("DynamicExperimentalControl")
 
   def mode(self) -> str:
@@ -198,191 +190,202 @@ class DynamicExperimentalController:
     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
 
-  def _update_calculations(self, sm: messaging.SubMaster) -> None:
+  def _update_calculations(self, sm: messaging.SubMaster, radar_fresh: bool) -> None:
     car_state = sm['carState']
-    lead_one = sm['radarState'].leadOne
+    radar_state = sm['radarState']
+    lead_one = radar_state.leadOne
+    lead_two = radar_state.leadTwo
     md = sm['modelV2']
 
     self._v_ego_kph = car_state.vEgo * 3.6
     self._v_cruise_kph = car_state.vCruise
     self._has_standstill = car_state.standstill
 
-    # standstill detection
     if self._has_standstill:
-      self._standstill_count = min(20, self._standstill_count + 1)
+      self._standstill_count = min(WMACConstants.STANDSTILL_FRAMES * 3, self._standstill_count + 1)
     else:
       self._standstill_count = max(0, self._standstill_count - 1)
 
-    # 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
-
-    # 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
-
-    # Slow down detection
+    self._radar_fresh = bool(radar_fresh)
+    if self._radar_fresh:
+      self._radar_stale_frames = 0
+      self._has_lead_filtered = self._lead_tracker.update(float(lead_one.present))
+      self._has_any_lead = bool(lead_one.present or lead_two.present)
+      self._has_current_radar_acc_lead = bool(max(self._radar_acc_lead_score(lead_one), self._radar_acc_lead_score(lead_two)))
+      self._update_radar_acc_lead()
+    else:
+      self._radar_stale_frames += 1
+      self._has_current_radar_acc_lead = False
+      if self._radar_stale_frames < WMACConstants.RADAR_STALE_FRAMES:
+        self._update_radar_acc_lead()
+      else:
+        self._lead_tracker.reset()
+        self._has_lead_filtered = False
+        self._has_any_lead = False
+        self._has_radar_acc_lead = False
+        self._radar_acc_lead_frames = 0
+    self._has_mpc_fcw = self._mpc_fcw_crash_cnt > 0
     self._calculate_slow_down(md)
 
-    # Slowness detection
-    if not (self._standstill_count > 5) and not self._has_slow_down:
+    if self._standstill_count > WMACConstants.STANDSTILL_FRAMES or self._has_slow_down:
+      self._slowness_tracker.reset()
+      self._has_slowness = False
+    else:
       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
+      self._has_slowness = self._slowness_tracker.update(current_slowness)
 
-      # 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
+  def _calculate_slow_down(self, md) -> None:
     self._endpoint_x = float('inf')
+    self._expected_distance = 0.0
     self._trajectory_valid = False
 
-    #Require exact trajectory size
-    position_valid = len(md.position.x) == TRAJECTORY_SIZE
-    orientation_valid = len(md.orientation.x) == TRAJECTORY_SIZE
+    self._update_model_decel_trend(md)
+    urgency = self._model_action_urgency(md)
+    position_valid = len(md.position.x) == WMACConstants.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
+    if position_valid:
+      self._trajectory_valid = True
+      self._endpoint_x = md.position.x[WMACConstants.TRAJECTORY_SIZE - 1]
+      self._expected_distance = interp(self._v_ego_kph, WMACConstants.SLOW_DOWN_BP, WMACConstants.SLOW_DOWN_DIST)
+      urgency = max(urgency, self._endpoint_urgency(self._endpoint_x, self._expected_distance))
 
-      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
+    self._raw_urgency = clip01(urgency)
+    self._has_slow_down = self._slow_down_tracker.update(self._raw_urgency)
+    self._urgency = self._slow_down_tracker.value
+
+  def _update_model_decel_trend(self, md) -> None:
+    try:
+      desired_accel = float(md.action.desiredAcceleration)
+    except (AttributeError, OverflowError, TypeError, ValueError):
+      desired_accel = math.nan
+    if not math.isfinite(desired_accel):
+      self._reset_model_decel_trend()
+    else:
+      self._model_accel_samples.append(desired_accel)
+    history = tuple(self._model_accel_samples)
+    self._model_decel_trending = (len(history) == self._model_accel_samples.maxlen
+                                  and history[-1] <= WMACConstants.MODEL_DECEL_TREND_ACCEL
+                                  and (history[0] - history[-1]) / (DT_MDL * (len(history) - 1)) > WMACConstants.MODEL_DECEL_TREND_RATE
+                                  and all(after <= before for before, after in zip(history[:-1], history[1:], strict=True))
+                                  and sum(after < before for before, after in zip(history[:-1], history[1:], strict=True)) >= 2)
+    if len(history) == self._model_accel_samples.maxlen and all(
+      accel >= WMACConstants.MODEL_DECEL_TREND_RELEASE_ACCEL for accel in history
+    ):
+      self._model_decel_latched = False
+
+  def _reset_model_decel_trend(self) -> None:
+    self._model_accel_samples.clear()
+    self._model_decel_trending = False
+    self._model_decel_latched = False
+
+  def _radar_acc_lead_score(self, lead_one) -> float:
+    radar_track_id = int(getattr(lead_one, 'radarTrackId', -1))
+    return float(lead_one.present and (bool(getattr(lead_one, 'radar', False)) or radar_track_id >= 0))
+
+  def _update_radar_acc_lead(self) -> None:
+    if self._has_current_radar_acc_lead:
+      self._radar_acc_lead_frames = WMACConstants.RADAR_LEAD_CONTINUITY_FRAMES
+      self._has_radar_acc_lead = True
       return
 
-    # We have a valid full trajectory
-    self._trajectory_valid = True
+    if not self._has_any_lead:
+      self._radar_acc_lead_frames = min(self._radar_acc_lead_frames, WMACConstants.RADAR_LEAD_DROPOUT_FRAMES)
 
-    # Use the exact endpoint (33rd point, index 32)
-    endpoint_x = md.position.x[TRAJECTORY_SIZE - 1]
-    self._endpoint_x = endpoint_x
+    self._has_radar_acc_lead = self._radar_acc_lead_frames > 0
+    self._radar_acc_lead_frames = max(0, self._radar_acc_lead_frames - 1)
 
-    # 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
+  def _model_action_urgency(self, md) -> float:
+    action = getattr(md, 'action', None)
+    if action is None:
+      return 0.0
 
-    # Calculate urgency based on trajectory shortage
-    if endpoint_x < expected_distance:
-      shortage = expected_distance - endpoint_x
-      shortage_ratio = shortage / expected_distance
+    urgency = 1.0 if getattr(action, 'shouldStop', False) else 0.0
+    desired_accel = getattr(action, 'desiredAcceleration', 0.0)
+    if desired_accel < WMACConstants.MODEL_DECEL_START:
+      urgency = max(urgency, min(1.0, (WMACConstants.MODEL_DECEL_START - desired_accel) / WMACConstants.MODEL_DECEL_RANGE))
+    return urgency
 
-      # Base urgency on shortage ratio
-      urgency = min(1.0, shortage_ratio * 2.0)
+  def _endpoint_urgency(self, endpoint_x: float, expected_distance: float) -> float:
+    if endpoint_x >= expected_distance:
+      return 0.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)
+    shortage_ratio = (expected_distance - endpoint_x) / expected_distance
+    urgency = min(1.0, shortage_ratio * WMACConstants.ENDPOINT_URGENCY_GAIN)
 
-      # 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)
+    if endpoint_x < expected_distance * WMACConstants.CRITICAL_ENDPOINT_FACTOR:
+      urgency = min(1.0, urgency * WMACConstants.CRITICAL_URGENCY_GAIN)
 
-    # 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
+    if self._v_ego_kph > WMACConstants.SPEED_URGENCY_MIN:
+      speed_factor = 1.0 + (self._v_ego_kph - WMACConstants.SPEED_URGENCY_MIN) / WMACConstants.SPEED_URGENCY_RANGE
+      urgency = min(1.0, urgency * speed_factor)
 
-    # 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
+    return urgency
 
-  def _radarless_mode(self) -> None:
-    """Radarless mode decision logic with emergency handling."""
+  def _model_decel_handoff_ready(self) -> bool:
+    try:
+      mpc_accel = float(self._mpc.a_solution[1])
+      return (math.isfinite(mpc_accel) and mpc_accel <= WMACConstants.MODEL_DECEL_TREND_MAX_MPC_ACCEL
+              and math.isfinite(self._planner_accel) and self._planner_accel <= WMACConstants.MODEL_DECEL_TREND_MAX_MPC_ACCEL
+              and self._planner_accel - self._model_accel_samples[-1] <= WMACConstants.MODEL_DECEL_TREND_MAX_COMMAND_STEP)
+    except (AttributeError, IndexError, OverflowError, TypeError, ValueError):
+      return False
+
+  def _desired_mode(self) -> tuple[ModeType, bool]:
+    standstill = self._standstill_count > WMACConstants.STANDSTILL_FRAMES
+    urgent_slow_down = self._has_slow_down and self._raw_urgency > WMACConstants.URGENT_SLOW_DOWN_PROB
+
+    if not self._CP.radarUnavailable and self._has_current_radar_acc_lead:
+      self._reset_model_decel_trend()
+      return 'acc', True
+
+    radar_stale = not self._radar_fresh if self._has_mpc_fcw else self._radar_stale_frames > 1
+    if (radar_stale or not self._has_any_lead) and (self._has_mpc_fcw or urgent_slow_down):
+      self._radar_acc_lead_frames = 0
+      self._has_radar_acc_lead = False
+      return 'blended', True
+
+    if not self._CP.radarUnavailable and self._has_radar_acc_lead:
+      self._reset_model_decel_trend()
+      return 'acc', True
+
+    entering_model_slowdown = self._model_decel_trending and self._model_decel_handoff_ready() and not self._model_decel_latched
+    self._model_decel_latched |= entering_model_slowdown
+    if self._model_decel_latched:
+      return 'blended', entering_model_slowdown
 
-    # 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)
-
-  def update(self, sm: messaging.SubMaster) -> None:
-    self._read_params()
-
-    self.set_mpc_fcw_crash_cnt()
-
-    self._update_calculations(sm)
+      return 'blended', True
 
     if self._CP.radarUnavailable:
-      self._radarless_mode()
-    else:
-      self._radar_mode()
+      if standstill or self._has_slow_down:
+        return 'blended', urgent_slow_down
+      return 'acc', False
 
-    self._mode_manager.update()
+    if standstill or self._has_slow_down:
+      return 'blended', urgent_slow_down
+
+    return 'acc', False
+
+  def update(self, sm: messaging.SubMaster, *, radar_fresh: bool = True, planner_accel: float | None = None) -> None:
+    self._read_params()
+    self.set_mpc_fcw_crash_cnt()
+    try:
+      self._planner_accel = float(planner_accel)
+    except (OverflowError, TypeError, ValueError):
+      self._planner_accel = math.nan
+    self._update_calculations(sm, radar_fresh)
     self._active = sm['selfdriveState'].experimentalMode and self._enabled
+    if not self._active:
+      model_decel_latched = self._model_decel_latched
+      self._reset_model_decel_trend()
+      if model_decel_latched:
+        self._mode_manager.request_mode('acc', immediate=True)
+
+    mode, immediate = self._desired_mode()
+    self._mode_manager.request_mode(mode, immediate=immediate, hold_frames=WMACConstants.EMERGENCY_HOLD_FRAMES,
+                                    cancel_hold=not self._CP.radarUnavailable and self._has_radar_acc_lead)
+    self._mode_manager.update()
+
     self._frame += 1

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

@@ -1,94 +0,0 @@
-import pytest
-
-from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
-
-class MockLeadOne:
-  def __init__(self, status=0.0):
-    self.status = status
-
-class MockRadarState:
-  def __init__(self, status=0.0):
-    self.leadOne = MockLeadOne(status=status)
-
-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 get_bool(self, name):
-    return True
-
-@pytest.fixture
-def default_sm():
-  sm = {
-    'carState': MockCarState(vEgo=10.0, vCruise=20.0),
-    'radarState': MockRadarState(status=1.0),
-    'modelV2': MockModelData(valid=True),
-    'selfdriveState': MockSelfDriveState(experimentalMode=True),
-  }
-  return sm
-
-@pytest.fixture
-def mock_cp():
-  class CP:
-    radarUnavailable = False
-  return CP()
-
-@pytest.fixture
-def mock_mpc():
-  class MPC:
-    crash_cnt = 0
-  return MPC()
-
-# 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
-
-def test_initial_mode_is_acc(mock_cp, mock_mpc):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  assert controller.mode() == "acc"
-
-def test_standstill_triggers_blended(mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['carState'].standstill = True
-  for _ in range(10):
-    controller.update(default_sm)
-  assert controller.mode() == "blended"
-
-def test_emergency_blended_on_fcw(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)
-  assert controller.mode() == "blended"
-
-def test_radarless_slowdown_triggers_blended(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)
-
-  assert controller.mode() == "blended"

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

@@ -0,0 +1,633 @@
+import pytest
+
+from openpilot.sunnypilot.selfdrive.controls.lib.dec.constants import WMACConstants
+from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController, HysteresisSignal
+
+
+class MockLeadOne:
+  def __init__(self, status=0.0, dRel=30.0, vRel=0.0, radar=False, radarTrackId=-1):
+    self.present = status
+    self.dRel = dRel
+    self.vRel = vRel
+    self.radar = radar
+    self.radarTrackId = radarTrackId
+
+
+class MockRadarState:
+  def __init__(self, status=0.0, dRel=30.0, vRel=0.0, radar=False, radarTrackId=-1, leadTwo=None):
+    self.leadOne = MockLeadOne(status=status, dRel=dRel, vRel=vRel, radar=radar, radarTrackId=radarTrackId)
+    self.leadTwo = leadTwo if leadTwo is not None else MockLeadOne()
+
+
+class MockCarState:
+  def __init__(self, vEgo=0.0, vCruise=0.0, standstill=False):
+    self.vEgo = vEgo
+    self.vCruise = vCruise
+    self.standstill = standstill
+
+
+class MockAction:
+  def __init__(self, desiredAcceleration=0.0, shouldStop=False):
+    self.desiredAcceleration = desiredAcceleration
+    self.shouldStop = shouldStop
+
+
+class MockModelData:
+  def __init__(self, valid=True, endpoint_x=200.0, orientation_valid=None, desired_acceleration=0.0, should_stop=False):
+    position_size = 33 if valid else 10
+    orientation_size = position_size if orientation_valid is None else (33 if orientation_valid else 10)
+    position_x = [0.0] * position_size
+    if position_x:
+      position_x[-1] = endpoint_x
+    self.position = type("Pos", (), {"x": position_x})()
+    self.orientation = type("Ori", (), {"x": [0.0] * orientation_size})()
+    self.acceleration = type("Accel", (), {"x": [0.0] * position_size})()
+    self.action = MockAction(desired_acceleration, should_stop)
+
+
+class MockSelfDriveState:
+  def __init__(self, experimentalMode=False):
+    self.experimentalMode = experimentalMode
+
+
+class MockParams:
+  def get_bool(self, name):
+    return True
+
+
+@pytest.fixture
+def default_sm():
+  sm = {
+    'carState': MockCarState(vEgo=10.0, vCruise=20.0),
+    'radarState': MockRadarState(status=1.0, radar=True, radarTrackId=7),
+    'modelV2': MockModelData(valid=True),
+    'selfdriveState': MockSelfDriveState(experimentalMode=True),
+  }
+  return sm
+
+
+@pytest.fixture
+def mock_cp():
+  class CP:
+    radarUnavailable = False
+  return CP()
+
+
+@pytest.fixture
+def mock_mpc():
+  class MPC:
+    crash_cnt = 0
+    a_solution = [0.0, 0.0]
+  return MPC()
+
+
+def test_initial_mode_is_acc(mock_cp, mock_mpc):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  assert controller.mode() == "acc"
+
+
+def test_standstill_triggers_blended(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=0.0)
+  default_sm['carState'].standstill = True
+  for _ in range(20):
+    controller.update(default_sm)
+  assert controller.mode() == "blended"
+
+
+def test_emergency_blended_on_fcw(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=0.0)
+  mock_mpc.crash_cnt = 1
+  controller.update(default_sm)
+  assert controller.mode() == "blended"
+
+
+def test_radarless_slowdown_triggers_blended(mock_cp, mock_mpc, default_sm):
+  mock_cp.radarUnavailable = True
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=0.0)
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
+
+  controller.update(default_sm)
+
+  assert controller.mode() == "blended"
+
+
+def test_valid_position_with_missing_orientation_can_trigger_slowdown(mock_cp, mock_mpc, default_sm):
+  mock_cp.radarUnavailable = True
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=0.0)
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0, orientation_valid=False)
+
+  controller.update(default_sm)
+
+  assert controller._trajectory_valid
+  assert controller.mode() == "blended"
+
+
+def test_incomplete_position_does_not_trigger_slowdown(mock_cp, mock_mpc, default_sm):
+  mock_cp.radarUnavailable = True
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=0.0)
+  default_sm['modelV2'] = MockModelData(valid=False, endpoint_x=0.0)
+
+  for _ in range(3):
+    controller.update(default_sm)
+
+  assert not controller._trajectory_valid
+  assert not controller._has_slow_down
+  assert controller.mode() == "acc"
+
+
+def test_slowdown_hysteresis_prevents_threshold_chatter():
+  signal = HysteresisSignal(enter_threshold=0.5, exit_threshold=0.4, rise_rate=1.0, fall_rate=1.0)
+
+  assert signal.update(0.55)
+  assert signal.update(0.45)
+  assert not signal.update(0.35)
+
+
+def test_model_should_stop_triggers_blended_without_valid_trajectory(mock_cp, mock_mpc, default_sm):
+  mock_cp.radarUnavailable = True
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=0.0)
+  default_sm['modelV2'] = MockModelData(valid=False, should_stop=True)
+
+  controller.update(default_sm)
+
+  assert not controller._trajectory_valid
+  assert controller.mode() == "blended"
+
+
+def test_confirmed_model_decel_trend_enters_blended_before_a_large_command(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=0.0)
+
+  for desired_acceleration in (-0.02, -0.05, -0.08):
+    default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
+    controller.update(default_sm, planner_accel=0.0)
+    assert controller.mode() == "acc"
+
+  default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=-0.12)
+  controller.update(default_sm, planner_accel=0.0)
+
+  assert controller._model_decel_trending
+  assert not controller._has_slow_down
+  assert controller.mode() == "blended"
+
+
+def test_confirmed_model_decel_handoff_stays_latched_through_a_plateau(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=0.0)
+
+  for desired_acceleration in (-0.02, -0.05, -0.08, -0.12):
+    default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
+    controller.update(default_sm, planner_accel=0.0)
+
+  for _ in range(WMACConstants.EMERGENCY_HOLD_FRAMES + WMACConstants.EXIT_BLENDED_FRAMES + 1):
+    default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=-0.12)
+    controller.update(default_sm, planner_accel=0.0)
+
+  assert not controller._model_decel_trending
+  assert controller._model_decel_latched
+  assert controller.mode() == "blended"
+
+  for _ in range(WMACConstants.MODEL_DECEL_TREND_FRAMES + WMACConstants.EXIT_BLENDED_FRAMES):
+    default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=0.0)
+    controller.update(default_sm, planner_accel=0.0)
+
+  assert not controller._model_decel_latched
+  assert controller.mode() == "acc"
+
+
+def test_model_decel_trend_never_overrides_a_radar_lead(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+
+  for desired_acceleration in (-0.02, -0.05, -0.08, -0.12):
+    default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
+    controller.update(default_sm)
+
+  assert not controller._model_accel_samples
+  assert not controller._model_decel_latched
+  assert controller._has_radar_acc_lead
+  assert controller.mode() == "acc"
+
+
+def test_radar_acquisition_clears_a_latched_model_decel_handoff(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=0.0)
+  for desired_acceleration in (-0.02, -0.05, -0.08, -0.12):
+    default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
+    controller.update(default_sm, planner_accel=0.0)
+  assert controller._model_decel_latched
+
+  default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
+  controller.update(default_sm, planner_accel=0.0)
+
+  assert not controller._model_accel_samples
+  assert not controller._model_decel_latched
+  assert controller.mode() == "acc"
+
+
+def test_model_decel_trend_does_not_accumulate_while_dec_is_inactive(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=0.0)
+  default_sm['selfdriveState'].experimentalMode = False
+  for desired_acceleration in (-0.02, -0.05, -0.08, -0.12):
+    default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
+    controller.update(default_sm, planner_accel=0.0)
+
+  assert not controller._model_accel_samples
+  assert not controller._model_decel_latched
+
+  default_sm['selfdriveState'].experimentalMode = True
+  controller.update(default_sm, planner_accel=0.0)
+  assert not controller._model_decel_trending
+  assert controller.mode() == "acc"
+
+
+def test_disabling_dec_clears_a_latched_model_decel_mode(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=0.0)
+  for desired_acceleration in (-0.02, -0.05, -0.08, -0.12):
+    default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
+    controller.update(default_sm, planner_accel=0.0)
+  assert controller._model_decel_latched
+  assert controller.mode() == "blended"
+
+  default_sm['selfdriveState'].experimentalMode = False
+  default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=0.0)
+  controller.update(default_sm, planner_accel=0.0)
+
+  assert not controller._model_decel_latched
+  assert controller.mode() == "acc"
+
+
+def test_model_decel_trend_waits_while_mpc_is_accelerating(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=0.0)
+  mock_mpc.a_solution[1] = 0.5
+
+  for desired_acceleration in (-0.02, -0.05, -0.08, -0.12):
+    default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
+    controller.update(default_sm, planner_accel=0.0)
+
+  assert controller._model_decel_trending
+  assert controller.mode() == "acc"
+
+
+def test_steep_model_decel_trend_defers_to_the_existing_urgent_path(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=0.0)
+
+  for desired_acceleration in (0.0, -0.2, -0.4, -0.6):
+    default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
+    controller.update(default_sm, planner_accel=0.05)
+
+  assert controller._model_decel_trending
+  assert controller.mode() == "acc"
+
+
+def test_model_decel_trend_waits_while_the_planner_is_accelerating(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=0.0)
+
+  for desired_acceleration in (-0.02, -0.05, -0.08, -0.12):
+    default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
+    controller.update(default_sm, planner_accel=0.2)
+
+  assert controller._model_decel_trending
+  assert controller.mode() == "acc"
+
+
+def test_alternating_model_accel_noise_does_not_trigger_an_early_handoff(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=0.0)
+
+  for desired_acceleration in (0.0, -0.2, 0.0, -0.2):
+    default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
+    controller.update(default_sm)
+
+  assert not controller._model_decel_trending
+  assert controller.mode() == "acc"
+
+
+def test_radar_lead_keeps_acc_over_model_slowdown(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
+
+  for _ in range(3):
+    controller.update(default_sm)
+
+  assert controller._has_slow_down
+  assert controller._has_radar_acc_lead
+  assert controller.mode() == "acc"
+
+
+def test_far_radar_lead_always_uses_acc(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=1.0, dRel=120.0, vRel=0.0, radar=True)
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
+
+  controller.update(default_sm)
+
+  assert controller._has_lead_filtered
+  assert controller._has_radar_acc_lead
+  assert controller.mode() == "acc"
+
+
+def test_radar_acquisition_immediately_returns_blended_to_acc(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=0.0)
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
+  controller.update(default_sm)
+  assert controller.mode() == "blended"
+
+  default_sm['radarState'] = MockRadarState(status=1.0, dRel=120.0, radar=True, radarTrackId=7)
+  controller.update(default_sm)
+
+  assert controller._has_radar_acc_lead
+  assert controller.mode() == "acc"
+
+  default_sm['radarState'] = MockRadarState(status=0.0)
+  default_sm['modelV2'] = MockModelData(valid=True)
+  for _ in range(20):
+    controller.update(default_sm)
+  assert controller.mode() == "acc"
+
+
+def test_close_vision_only_lead_can_use_blended(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=1.0, dRel=30.0, vRel=-5.0)
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
+  controller.update(default_sm)
+
+  assert not controller._has_radar_acc_lead
+  assert controller.mode() == "blended"
+
+
+def test_second_radar_lead_forces_acc(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  lead_two = MockLeadOne(status=1.0, dRel=120.0, radar=True, radarTrackId=8)
+  default_sm['radarState'] = MockRadarState(status=1.0, dRel=30.0, vRel=-5.0, leadTwo=lead_two)
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
+  controller.update(default_sm)
+
+  assert controller._has_radar_acc_lead
+  assert controller.mode() == "acc"
+
+
+def test_second_vision_only_lead_does_not_force_acc(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  lead_two = MockLeadOne(status=1.0, dRel=20.0, vRel=-10.0)
+  default_sm['radarState'] = MockRadarState(status=0.0, leadTwo=lead_two)
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
+  controller.update(default_sm)
+
+  assert not controller._has_radar_acc_lead
+  assert controller.mode() == "blended"
+
+
+def test_inactive_lead_with_radar_marker_does_not_force_acc(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=0.0, radar=True, radarTrackId=7)
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
+  controller.update(default_sm)
+
+  assert not controller._has_radar_acc_lead
+  assert controller.mode() == "blended"
+
+
+def test_radarless_car_ignores_marked_radar_track(mock_cp, mock_mpc, default_sm):
+  mock_cp.radarUnavailable = True
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
+  controller.update(default_sm)
+
+  assert controller._has_radar_acc_lead
+  assert controller.mode() == "blended"
+
+
+def test_closing_far_radar_lead_returns_to_acc(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=1.0, dRel=120.0, vRel=-25.0, radarTrackId=7)
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
+
+  for _ in range(20):
+    controller.update(default_sm)
+
+  assert controller._has_radar_acc_lead
+  assert controller.mode() == "acc"
+
+
+def test_radar_lead_keeps_acc_over_fcw_and_standstill(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
+  default_sm['carState'].standstill = True
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0, should_stop=True)
+  mock_mpc.crash_cnt = 1
+
+  for _ in range(10):
+    controller.update(default_sm)
+
+  assert controller._has_lead_filtered
+  assert controller._has_mpc_fcw
+  assert controller.mode() == "acc"
+
+
+def test_lead_flicker_hold_prevents_one_frame_mode_flip(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=50.0)
+  for _ in range(2):
+    controller.update(default_sm)
+  assert controller._has_slow_down
+
+  default_sm['radarState'] = MockRadarState(status=0.0)
+  controller.update(default_sm)
+
+  assert controller._has_lead_filtered
+  assert controller.mode() == "acc"
+
+
+def test_radar_lead_continuity_with_vision_fallback_expires_into_confirmed_transition(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=50.0)
+  for _ in range(2):
+    controller.update(default_sm)
+  assert controller._has_slow_down
+
+  default_sm['radarState'] = MockRadarState(status=1.0)
+  for _ in range(WMACConstants.RADAR_LEAD_CONTINUITY_FRAMES):
+    controller.update(default_sm)
+    assert controller._has_radar_acc_lead
+    assert controller.mode() == "acc"
+
+  controller.update(default_sm)
+  assert not controller._has_radar_acc_lead
+  assert controller.mode() == "acc"
+
+  for _ in range(WMACConstants.ENTER_BLENDED_FRAMES - 1):
+    controller.update(default_sm)
+  assert controller.mode() == "blended"
+
+
+def test_radar_lead_short_dropout_guard_expires_without_any_lead(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
+  controller.update(default_sm)
+
+  default_sm['radarState'] = MockRadarState(status=0.0)
+  for _ in range(WMACConstants.RADAR_LEAD_DROPOUT_FRAMES):
+    controller.update(default_sm)
+    assert controller._has_radar_acc_lead
+
+  controller.update(default_sm)
+  assert not controller._has_radar_acc_lead
+
+
+def test_one_stale_radar_frame_does_not_drop_acc_authority(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  controller.update(default_sm)
+
+  controller.update(default_sm, radar_fresh=False)
+
+  assert not controller._has_current_radar_acc_lead
+  assert controller._has_radar_acc_lead
+  assert controller._radar_acc_lead_frames == WMACConstants.RADAR_LEAD_CONTINUITY_FRAMES - 1
+  assert controller._radar_stale_frames == 1
+  assert controller.mode() == "acc"
+
+
+def test_one_stale_radar_frame_does_not_override_retained_lead_for_model_urgency(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  controller.update(default_sm)
+  default_sm['modelV2'] = MockModelData(valid=False, should_stop=True)
+
+  controller.update(default_sm, radar_fresh=False)
+  assert controller.mode() == "acc"
+
+  controller.update(default_sm, radar_fresh=False)
+  assert controller.mode() == "blended"
+
+
+def test_one_stale_radar_frame_does_not_delay_fcw(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  controller.update(default_sm)
+  mock_mpc.crash_cnt = 1
+
+  controller.update(default_sm, radar_fresh=False)
+
+  assert controller.mode() == "blended"
+
+
+def test_frozen_radar_marker_cannot_rearm_acc_authority(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  controller.update(default_sm)
+
+  for _ in range(WMACConstants.RADAR_STALE_FRAMES - 1):
+    controller.update(default_sm, radar_fresh=False)
+    assert controller._has_radar_acc_lead
+
+  controller.update(default_sm, radar_fresh=False)
+
+  assert not controller._has_current_radar_acc_lead
+  assert not controller._has_radar_acc_lead
+  assert not controller._has_any_lead
+  assert not controller._has_lead_filtered
+
+
+def test_fresh_radar_reacquisition_after_stale_timeout_is_immediate(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  controller.update(default_sm)
+  for _ in range(WMACConstants.RADAR_STALE_FRAMES):
+    controller.update(default_sm, radar_fresh=False)
+
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
+  controller.update(default_sm, radar_fresh=False)
+  assert controller.mode() == "blended"
+
+  lead_two = MockLeadOne(status=1.0, radar=True, radarTrackId=8)
+  default_sm['radarState'] = MockRadarState(status=0.0, leadTwo=lead_two)
+  controller.update(default_sm, radar_fresh=True)
+
+  assert controller._radar_stale_frames == 0
+  assert controller._has_current_radar_acc_lead
+  assert controller.mode() == "acc"
+
+
+@pytest.mark.parametrize("urgent_source", ["fcw", "should_stop"])
+def test_no_lead_urgent_slowdown_bypasses_radar_dropout_guard(mock_cp, mock_mpc, default_sm, urgent_source):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
+  controller.update(default_sm)
+
+  default_sm['radarState'] = MockRadarState(status=0.0)
+  if urgent_source == "fcw":
+    mock_mpc.crash_cnt = 1
+  else:
+    default_sm['modelV2'] = MockModelData(valid=False, should_stop=True)
+  controller.update(default_sm)
+
+  assert not controller._has_radar_acc_lead
+  assert controller.mode() == "blended"
+
+  mock_mpc.crash_cnt = 0
+  default_sm['modelV2'] = MockModelData(valid=True)
+  controller.update(default_sm)
+  assert controller.mode() == "blended"
+
+
+def test_lead_two_radar_authority_continues_with_vision_lead_one(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  lead_two = MockLeadOne(status=1.0, radar=True, radarTrackId=8)
+  default_sm['radarState'] = MockRadarState(status=0.0, leadTwo=lead_two)
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
+  controller.update(default_sm)
+  assert controller._has_current_radar_acc_lead
+  assert controller.mode() == "acc"
+
+  default_sm['radarState'] = MockRadarState(status=1.0)
+  for _ in range(WMACConstants.RADAR_LEAD_CONTINUITY_FRAMES):
+    controller.update(default_sm)
+    assert controller._has_radar_acc_lead
+    assert controller.mode() == "acc"
+
+
+def test_alternating_radar_slots_keep_acc_authority(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
+
+  for frame in range(WMACConstants.RADAR_LEAD_CONTINUITY_FRAMES * 2):
+    if frame % 2 == 0:
+      default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7, leadTwo=MockLeadOne(status=1.0))
+    else:
+      default_sm['radarState'] = MockRadarState(status=1.0, leadTwo=MockLeadOne(status=1.0, radar=True, radarTrackId=8))
+    controller.update(default_sm)
+
+    assert controller._has_current_radar_acc_lead
+    assert controller.mode() == "acc"
+
+
+def test_radar_reacquisition_immediately_restores_acc_after_continuity_expiry(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
+  controller.update(default_sm)
+
+  default_sm['radarState'] = MockRadarState(status=1.0)
+  for _ in range(WMACConstants.RADAR_LEAD_CONTINUITY_FRAMES + 1):
+    controller.update(default_sm)
+  assert not controller._has_radar_acc_lead
+  assert controller.mode() == "blended"
+
+  lead_two = MockLeadOne(status=1.0, radar=True, radarTrackId=8)
+  default_sm['radarState'] = MockRadarState(status=1.0, leadTwo=lead_two)
+  controller.update(default_sm)
+
+  assert controller._has_current_radar_acc_lead
+  assert controller.mode() == "acc"

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

@@ -0,0 +1,32 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+import math
+
+from openpilot.common.pid import PIDController
+from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
+
+DECAY_TAU = 2.0  # seconds; starting guess, not validated against a real car
+
+
+class DecayingIntegratorPIDController(PIDController):
+  def __init__(self, *args, decay_tau=DECAY_TAU, **kwargs):
+    super().__init__(*args, **kwargs)
+    self.decay_tau = decay_tau
+
+  def update(self, error, error_rate=0.0, speed=0.0, feedforward=0., freeze_integrator=False):
+    if freeze_integrator:
+      self.i *= math.exp(-self.i_dt / self.decay_tau)
+    return super().update(error, error_rate=error_rate, speed=speed, feedforward=feedforward, freeze_integrator=freeze_integrator)
+
+
+class LatControlPidSmooth(LatControlPID):
+  def __init__(self, CP, CP_SP, CI, dt, decay_tau=DECAY_TAU):
+    super().__init__(CP, CP_SP, CI, dt)
+    self.pid = DecayingIntegratorPIDController(
+      (CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
+      (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
+      pos_limit=self.steer_max, neg_limit=-self.steer_max, decay_tau=decay_tau)

openpilot/sunnypilot/selfdrive/controls/lib/longcontrol.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.
+"""
+
+STOPPING_DISTANCE = 0.75
+STOPPED_SPEED = 0.02
+STOPPING_TIME = 2.5
+
+
+class LongControlSP:
+  def should_hold_stopping(self, CS, a_target: float) -> bool:
+    return (self.last_output_accel <= 0.0 and a_target >= self.last_output_accel and CS.vEgo > STOPPED_SPEED and CS.aEgo < 0.0
+            and CS.vEgo <= -CS.aEgo * STOPPING_TIME and CS.vEgo ** 2 <= -2.0 * CS.aEgo * STOPPING_DISTANCE)

openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -0,0 +1,46 @@
+"""
+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_MIN, ACCEL_MAX
+
+
+class LongitudinalMpcSP:
+  def __init__(self) -> None:
+    self._accel_max_trajectory: tuple[float, ...] | None = None
+    self._cruise_accel_max: float | None = None
+    self._jerk_cost_multiplier = 1.0
+    self.last_solution_status = 0
+
+  def set_accel_controller_params(self, accel_max: tuple[float, ...] | None, jerk_cost_multiplier: float,
+                                  cruise_accel_max: float | None = None) -> None:
+    self._accel_max_trajectory = accel_max
+    self._cruise_accel_max = cruise_accel_max
+    self._jerk_cost_multiplier = jerk_cost_multiplier
+
+  def cruise_accel_max(self, stock_accel_max: float) -> float:
+    if self._cruise_accel_max is None or not np.isfinite(self._cruise_accel_max):
+      return stock_accel_max
+    return min(max(self._cruise_accel_max, 0.0), stock_accel_max)
+
+  def scale_jerk_cost(self, jerk_cost: float) -> float:
+    return jerk_cost * self._jerk_cost_multiplier
+
+  def apply_accel_limits(self) -> None:
+    if self._accel_max_trajectory is None:
+      return
+
+    accel_max = np.asarray(self._accel_max_trajectory)
+    if accel_max.shape != self.params[:, 1].shape or accel_max.dtype.kind not in "iuf" or not np.all(np.isfinite(accel_max)):
+      return
+
+    self.params[:, 1] = np.clip(accel_max, 0.0, ACCEL_MAX)
+    self.params[0, 1] = max(self.params[0, 1], float(np.clip(self.x0[2], ACCEL_MIN, ACCEL_MAX)))
+
+  def save_solution_status(self) -> None:
+    self.last_solution_status = self.solution_status

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

@@ -8,7 +8,9 @@ See the LICENSE.md file in the root directory for more details.
 from openpilot.cereal import messaging, custom
 from opendbc.car import structs
 from openpilot.common.constants import CV
-from openpilot.selfdrive.car.cruise import V_CRUISE_MAX
+from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController, AccelControllerState
 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
@@ -22,9 +24,10 @@ LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource
 
 
 class LongitudinalPlannerSP:
-  def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP, mpc):
+  def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP, mpc, dt: float = DT_MDL):
+    self.mpc = mpc
+    self.accel_controller = AccelController(CP, dt=dt)
     self.events_sp = EventsSP()
-    self.resolver = SpeedLimitResolver()
     self.dec = DynamicExperimentalController(CP, mpc)
     self.scc = SmartCruiseControl()
     self.resolver = SpeedLimitResolver()
@@ -32,6 +35,8 @@ class LongitudinalPlannerSP:
     self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None
     self.source = LongitudinalPlanSource.cruise
     self.e2e_alerts_helper = E2EAlertsHelper()
+    self._radar_log_mono_time = None
+    self._radar_fresh_this_cycle = True
 
     self.output_v_target = 0.
     self.output_a_target = 0.
@@ -43,6 +48,35 @@ class LongitudinalPlannerSP:
 
     return experimental_mode and self.dec.mode() == "blended"
 
+  def update_accel_controller(self, sm: messaging.SubMaster, v_cruise: float, prev_accel_constraint: bool,
+                              stock_accel_max: float, reset_state: bool) -> tuple[bool, float]:
+    is_e2e = self.is_e2e(sm)
+    force_decel = sm['controlsState'].forceDecel
+    previous_mpc_failed = self.mpc.last_solution_status != 0
+
+    self.accel_controller.update(
+      sm['radarState'], base_speed=self.output_v_target, v_ego=sm['carState'].vEgo, a_ego=sm['carState'].aEgo,
+      follow_personality=sm['selfdriveState'].personality, acc_selected=not is_e2e,
+      engaged=not reset_state and not force_decel, cruise_initialized=sm['carState'].vCruise != V_CRUISE_UNSET,
+      stock_accel_max=stock_accel_max if self.allow_throttle else 0.0, previous_should_stop=self.output_should_stop,
+      radar_fresh=self._radar_fresh_this_cycle, previous_mpc_source=self.mpc.source, planner_speed=self.v_desired_filter.x,
+      planner_accel=self.a_desired,
+    )
+    controller = self.accel_controller
+    actuating = controller.is_active and not is_e2e and not force_decel and not previous_mpc_failed
+    valid_lead_stop_hold = actuating and controller.state == AccelControllerState.stopHold and controller.selected_lead >= 0
+    controller_v_cruise = v_cruise if valid_lead_stop_hold else min(v_cruise, controller.output_v_target) if actuating else v_cruise
+    accel_max = controller.mpc_accel_max if actuating else None
+    cruise_accel_max = controller.cruise_accel_max if actuating else None
+    jerk_cost_multiplier = controller.get_jerk_cost_multiplier(
+      actuating, prev_accel_constraint, v_cruise - controller_v_cruise, previous_mpc_failed,
+    )
+    self.mpc.set_accel_controller_params(accel_max, jerk_cost_multiplier, cruise_accel_max)
+    return is_e2e, controller_v_cruise
+
+  def update_should_stop(self, should_stop: bool) -> bool:
+    return self.accel_controller.update_should_stop(should_stop)
+
   def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]:
     CS = sm['carState']
     v_cruise_cluster_kph = min(CS.vCruiseCluster, V_CRUISE_MAX)
@@ -73,9 +107,19 @@ class LongitudinalPlannerSP:
     self.output_v_target, self.output_a_target = targets[self.source]
     return self.output_v_target, self.output_a_target
 
+  def _update_radar_freshness(self, sm: messaging.SubMaster) -> bool:
+    radar_log_mono_time = sm.logMonoTime['radarState']
+    radar_healthy = sm.valid['radarState'] and sm.alive['radarState']
+    radar_advanced = self._radar_log_mono_time is None or radar_log_mono_time > self._radar_log_mono_time
+    if radar_advanced:
+      self._radar_log_mono_time = radar_log_mono_time
+    return radar_healthy and radar_advanced
+
   def update(self, sm: messaging.SubMaster) -> None:
+    self._radar_fresh_this_cycle = self._update_radar_freshness(sm)
+    self.accel_controller.update_params()
     self.events_sp.clear()
-    self.dec.update(sm)
+    self.dec.update(sm, radar_fresh=self._radar_fresh_this_cycle, planner_accel=self.output_a_target)
     self.e2e_alerts_helper.update(sm, self.events_sp)
 
   def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
@@ -95,6 +139,12 @@ class LongitudinalPlannerSP:
     dec.enabled = self.dec.enabled()
     dec.active = self.dec.active()
 
+    accelController = longitudinalPlanSP.accelController
+    accelController.enabled = self.accel_controller.is_enabled
+    accelController.active = self.accel_controller.is_active
+    accelController.profile = self.accel_controller.profile
+    accelController.state = self.accel_controller.state
+
     # Smart Cruise Control
     smartCruiseControl = longitudinalPlanSP.smartCruiseControl
     # Vision Control

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

@@ -0,0 +1,84 @@
+"""
+Copyright (c) 2021-, rav4kumar, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+import numpy as np
+
+from openpilot.common.constants import CV
+from openpilot.common.realtime import DT_MDL
+from openpilot.common.params import Params
+
+NEARSIDE_PROB = 0.2
+EDGE_PROB = 0.35
+EDGE_REACTION_TIME = 1.0
+EDGE_CLEAR_TIME = 0.3
+MIN_SPEED = 20 * CV.MPH_TO_MS
+
+
+class RoadEdgeLaneChangeController:
+  def __init__(self, desire_helper):
+    self.DH = desire_helper
+    self.params = Params()
+    self.enabled = self.params.get_bool("RoadEdgeLaneChangeEnabled")
+    self.param_read_counter = 0
+    self.left_edge_detected = False
+    self.right_edge_detected = False
+    self.left_edge_timer = 0.0
+    self.right_edge_timer = 0.0
+    self.left_clear_timer = 0.0
+    self.right_clear_timer = 0.0
+
+  def read_params(self) -> None:
+    self.enabled = self.params.get_bool("RoadEdgeLaneChangeEnabled")
+
+  def update_params(self) -> None:
+    if self.param_read_counter % 50 == 0:
+      self.read_params()
+    self.param_read_counter += 1
+
+  def reset(self) -> None:
+    self.left_edge_detected = False
+    self.right_edge_detected = False
+    self.left_edge_timer = 0.0
+    self.right_edge_timer = 0.0
+    self.left_clear_timer = 0.0
+    self.right_clear_timer = 0.0
+
+  def update(self, road_edge_stds, lane_line_probs, v_ego: float) -> None:
+    self.update_params()
+
+    if not self.enabled or v_ego < MIN_SPEED:
+      self.reset()
+      return
+
+    left_edge_prob = np.clip(1.0 - road_edge_stds[0], 0.0, 1.0)
+    right_edge_prob = np.clip(1.0 - road_edge_stds[1], 0.0, 1.0)
+    left_lane_prob = lane_line_probs[0]
+    right_lane_prob = lane_line_probs[3]
+
+    left_cond = left_edge_prob > EDGE_PROB and left_lane_prob < NEARSIDE_PROB and right_lane_prob >= left_lane_prob
+    right_cond = right_edge_prob > EDGE_PROB and right_lane_prob < NEARSIDE_PROB and left_lane_prob >= right_lane_prob
+
+    if left_cond:
+      self.left_edge_timer = min(self.left_edge_timer + DT_MDL, EDGE_REACTION_TIME + EDGE_CLEAR_TIME)
+      self.left_clear_timer = 0.0
+      if self.left_edge_timer > EDGE_REACTION_TIME:
+        self.left_edge_detected = True
+    else:
+      self.left_clear_timer += DT_MDL
+      if self.left_clear_timer > EDGE_CLEAR_TIME:
+        self.left_edge_timer = 0.0
+        self.left_edge_detected = False
+
+    if right_cond:
+      self.right_edge_timer = min(self.right_edge_timer + DT_MDL, EDGE_REACTION_TIME + EDGE_CLEAR_TIME)
+      self.right_clear_timer = 0.0
+      if self.right_edge_timer > EDGE_REACTION_TIME:
+        self.right_edge_detected = True
+    else:
+      self.right_clear_timer += DT_MDL
+      if self.right_clear_timer > EDGE_CLEAR_TIME:
+        self.right_edge_timer = 0.0
+        self.right_edge_detected = False

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

@@ -4,6 +4,7 @@ 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 +16,12 @@ 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_RATE, SmartCruiseControlVision,
+)
 
 VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.VisionState
 
@@ -120,6 +125,21 @@ class TestSmartCruiseControlVision:
   def reset_params(self):
     self.params.put_bool("SmartCruiseControlVision", True, block=True)
 
+  def set_lat_accels(self, current: float, predicted: float, v_ego: float = 20., model_speed: float = 20.) -> 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., a_ego: float = 0.,
+                        v_ego: float = 20., model_speed: float = 20.) -> 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
@@ -145,6 +165,253 @@ class TestSmartCruiseControlVision:
       self.scc_v.update(self.sm, True, False, 0., 0., 0.)
     assert self.scc_v.state == VisionState.enabled
 
+  def test_unconfirmed_leaving_and_reentry_only_shape_speed(self):
+    self.enter_curve()
+    targets = [self.scc_v.output_v_target]
+
+    self.update_lat_accels(2., 2.2, a_ego=-0.8)
+    assert self.scc_v.state == VisionState.turning
+    assert self.scc_v.output_a_target == -0.8
+    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., 3., a_ego=-1.2)
+    assert self.scc_v.state == VisionState.entering
+    assert self.scc_v.output_a_target == -1.2
+    targets.append(self.scc_v.output_v_target)
+
+    entering, turning, leaving, reentering = targets
+    assert turning == pytest.approx(entering)
+    assert 0. < leaving - turning <= _BELOW_EGO_TARGET_RELEASE_RATE * DT_MDL + 1e-9
+    assert reentering < leaving
+
+  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., 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
+
+  @pytest.mark.parametrize("planner_accel", (-2., -0.5, 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., 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., planner_accel, 30.)
+      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., 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. <= 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.
+    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. <= 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
+    assert active_v_targets[-1] == pytest.approx(release_cruise)
+    assert np.all((np.diff(active_v_targets) >= 0.) &
+                  (np.diff(active_v_targets) <= _BELOW_EGO_TARGET_RELEASE_RATE * DT_MDL + 1e-9))
+
+  def test_target_release_slows_after_reaching_ego_speed(self):
+    self.enter_curve()
+
+    for _ in range(100):
+      previous_v_target = self.scc_v.output_v_target
+      self.update_lat_accels(0.8, 0.8)
+      if previous_v_target >= self.scc_v.v_ego:
+        rise = self.scc_v.output_v_target - previous_v_target
+        assert 0. < rise <= _TARGET_RELEASE_RATE * DT_MDL + 1e-9
+        break
+    else:
+      pytest.fail("curve target did not release to ego speed")
+
+  def test_curve_target_is_independent_of_ego_speed(self):
+    model_speed = 24.
+    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., 28.):
+      controller = SmartCruiseControlVision()
+      self.set_lat_accels(0.5, predicted_lat_accel, v_ego, model_speed)
+      controller.update(self.sm, True, False, v_ego, 0., 30.)
+      controller.update(self.sm, True, False, v_ego, 0., 30.)
+      assert controller.state == VisionState.entering
+      targets.append(controller.v_target)
+
+    assert targets[0] == pytest.approx(expected_v_target)
+    assert targets[1] == pytest.approx(expected_v_target)
+
+  def test_curve_target_respects_minimum_speed_floor(self):
+    model_speed = 10.
+    predicted_yaw_rate = 2.
+    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., 30.)
+    self.scc_v.update(self.sm, True, False, 20., 0., 30.)
+
+    assert self.scc_v.state == VisionState.entering
+    assert self.scc_v.v_target < MIN_V
+    assert self.scc_v.output_v_target == pytest.approx(MIN_V)
+
+  @pytest.mark.parametrize(
+    ("velocities", "yaw_rates"),
+    [([], []), ([np.nan] * len(ModelConstants.T_IDXS), [np.nan] * len(ModelConstants.T_IDXS)), ([20.] * 5, [0.1] * 3)],
+    ids=("empty", "nonfinite", "mismatched"),
+  )
+  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., 30.)
+    self.scc_v.update(self.sm, True, False, 20., 0., 30.)
+
+    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,
+    ))
+
+  @pytest.mark.parametrize("launch_speed", (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., launch_speed)
+    self.scc_v.update(self.sm, True, False, launch_speed, 0., 30.)
+    self.scc_v.update(self.sm, True, False, launch_speed, 0., 30.)
+
+    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., speed)
+    self.scc_v.update(self.sm, True, False, speed, 0., 30.)
+    self.scc_v.update(self.sm, True, False, speed, 0., 30.)
+
+    assert self.scc_v.state == VisionState.entering
+    assert self.scc_v.is_active
+
+  def test_sequential_curve_tightens_immediately_and_releases_bounded(self):
+    self.enter_curve(3.)
+    for _ in range(20):
+      self.update_lat_accels(0.5, 3.)
+    restrictive_v_target = self.scc_v.output_v_target
+
+    self.update_lat_accels(0.5, 1.4, a_ego=0.4)
+    first_relief_v_target = self.scc_v.output_v_target
+    assert self.scc_v.state == VisionState.entering
+    assert 0. < first_relief_v_target - restrictive_v_target <= _BELOW_EGO_TARGET_RELEASE_RATE * DT_MDL + 1e-9
+    assert self.scc_v.output_a_target == 0.4
+
+    self.update_lat_accels(0.5, 1.4)
+    assert 0. <= self.scc_v.output_v_target - first_relief_v_target <= _BELOW_EGO_TARGET_RELEASE_RATE * DT_MDL + 1e-9
+
+    self.update_lat_accels(0.5, 3., a_ego=-0.6)
+    assert self.scc_v.state == VisionState.entering
+    assert self.scc_v.output_v_target == pytest.approx(restrictive_v_target)
+    assert self.scc_v.output_a_target == -0.6
+
+    for _ in range(4):
+      self.update_lat_accels(0.5, 1.4)
+      assert 0. < self.scc_v.output_v_target - restrictive_v_target <= _BELOW_EGO_TARGET_RELEASE_RATE * DT_MDL + 1e-9
+      self.update_lat_accels(0.5, 3.)
+      assert self.scc_v.output_v_target == pytest.approx(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.
+
+    planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
+    planner.scc = SimpleNamespace(
+      vision=self.scc_v,
+      map=SimpleNamespace(output_v_target=V_CRUISE_UNSET, output_a_target=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., speed_limit_final_last=0., distance=0.,
+      update=lambda _v_ego, _sm: None,
+    )
+    planner.sla = SimpleNamespace(
+      output_v_target=V_CRUISE_UNSET, output_a_target=0., update=lambda *_args: None,
+    )
+    planner.events_sp = SimpleNamespace()
+
+    self.set_lat_accels(0.5, 2.2)
+    planner.update_targets(self.sm, 20., -0.8, 30.)
+    planner.update_targets(self.sm, 20., -0.8, 30.)
+    assert planner.source == LongitudinalPlanSource.sccVision
+    assert planner.output_a_target == -0.8
+
+    for planner_accel in (-2., 0.5, -0.2):
+      planner.update_targets(self.sm, 20., planner_accel, 30.)
+      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. / (_TARGET_RELEASE_RATE * DT_MDL)) + 10):
+      planner.update_targets(self.sm, 20., 0.4, 30.)
+      assert planner.output_a_target == 0.4
+      if planner.source == LongitudinalPlanSource.cruise:
+        break
+    else:
+      pytest.fail("SCC Vision did not release to cruise")
+
+    planner.update_targets(self.sm, 20., 0.4, 30.)
+    assert self.scc_v.state == VisionState.enabled
+    assert planner.source == LongitudinalPlanSource.cruise
+
   @pytest.mark.parametrize(
     "case, should_enter",
     [

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

@@ -0,0 +1,82 @@
+"""
+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
+
+import numpy as np
+
+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
+from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PlantSP as Plant
+
+
+def _run_constant_curve(*, scc_enabled: bool, cruise: float, duration: float = 70.) -> dict[str, np.ndarray]:
+  gc.collect()
+  curvature = 0.005
+  plant = Plant(lead_relevancy=False, speed=30., actuator_delay=0.15, actuator_lag=0.20)
+  planner = plant.planner
+  planner.accel_controller.enabled = False
+  planner.accel_controller.update_params = lambda: None
+  planner.dec._enabled = False
+  planner.dec._read_params = lambda: None
+  planner.scc.map.enabled = False
+  planner.scc.map.update_params = lambda: None
+  planner.scc.vision.enabled = scc_enabled
+  planner.scc.vision._update_params = lambda: None
+
+  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)
+
+    planner.scc.vision._update_calculations = inject_constant_curvature
+
+  original_update = planner.update
+
+  def enable_longitudinal(sm):
+    sm['carControl'].enabled = True
+    sm['carControl'].longActive = True
+    original_update(sm)
+
+  planner.update = enable_longitudinal
+  rows = []
+  while plant.current_time < duration:
+    output = plant.step(v_cruise=cruise)
+    rows.append((
+      plant.current_time, output['speed'], planner.mpc.last_solution_status, 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], 'solver_status': data[:, 2], 'should_stop': data[:, 3],
+    'active': data[:, 4], 'scc_source': data[:, 5], 'target': data[:, 6],
+  }
+
+
+def test_constant_curve_recovers_like_stock_speed_cap():
+  target = (_A_LAT_REG_MAX / 0.005) ** 0.5
+  scc = _run_constant_curve(scc_enabled=True, cruise=30.)
+  stock = _run_constant_curve(scc_enabled=False, cruise=target)
+  scc_final = scc['speed'][scc['time'] >= 60.]
+  stock_final = stock['speed'][stock['time'] >= 60.]
+
+  assert not scc['solver_status'].any()
+  assert not stock['solver_status'].any()
+  assert not scc['should_stop'].any()
+  assert np.all(scc['active'][scc['time'] >= 60.])
+  assert np.all(scc['scc_source'][scc['time'] >= 60.])
+  assert np.allclose(scc['target'][scc['time'] >= 60.], target)
+  assert scc_final.min() >= target - 1.
+  assert abs(scc_final.mean() - stock_final.mean()) < 0.5
+  assert abs(scc_final.min() - stock_final.min()) < 1.
+  assert abs(scc_final.max() - stock_final.max()) < 1.

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

@@ -29,19 +29,11 @@ _FINISH_LAT_ACC_TH = 1.1  # Lat Acc threshold to trigger the end of the turn cyc
 
 _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_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,13 +57,26 @@ class SmartCruiseControlVision:
     self.state = VisionState.disabled
     self.current_lat_acc = 0.
     self.max_pred_lat_acc = 0.
+    self.relief_frames = 0
+
+  def _v_demand(self) -> float:
+    return max(MIN_V, min(self.v_target, self.v_cruise_setpoint))
+
+  def _released_v_target(self) -> float:
+    demand = self._v_demand()
+    if demand < self.output_v_target:
+      return demand
+    release_rate = _BELOW_EGO_TARGET_RELEASE_RATE if self.output_v_target < min(self.v_ego, demand) 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
+      if self.output_v_target == V_CRUISE_UNSET:
+        return self._v_demand()
+      return self._released_v_target()
 
     return V_CRUISE_UNSET
 
@@ -82,25 +87,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 +119,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 +135,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 +167,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 +184,7 @@ class SmartCruiseControlVision:
     self._update_calculations(sm)
 
     self.is_enabled, self.is_active = self._update_state_machine()
-    self.a_target = self._update_solution()
+    self.a_target = self.a_ego
 
     self.output_v_target = self.get_v_target_from_control()
     self.output_a_target = self.get_a_target_from_control()

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

@@ -1,10 +1,11 @@
 import pytest
 from openpilot.cereal import log, custom
 from openpilot.common.params import Params
-
 from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
 from openpilot.sunnypilot.selfdrive.controls.lib.lane_turn_desire import LaneTurnController, LANE_CHANGE_SPEED_MIN
 from openpilot.sunnypilot.selfdrive.controls.lib.auto_lane_change import AutoLaneChangeMode
+from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController
+
 
 TurnDirection = custom.ModelDataV2SP.TurnDirection
 
@@ -107,7 +108,10 @@ def set_lane_turn_params():
 ])
 def test_desire_helper_integration(carstate, lateral_active, lane_change_prob, expected_desire, set_lane_turn_params):
   dh = DesireHelper()
+  relc = RoadEdgeLaneChangeController(dh)
+  relc.enabled = True
   dh.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGE
   for _ in range(10):
-    dh.update(carstate, lateral_active, lane_change_prob)
+    dh.update(carstate, lateral_active, lane_change_prob,
+              left_edge_detected=relc.left_edge_detected, right_edge_detected=relc.right_edge_detected)
   assert dh.desire == expected_desire  # The first four tests were unit tests to test the controller, where this tests the integration in desire helpers

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

@@ -0,0 +1,73 @@
+import math
+
+import pytest
+
+from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_pid_ext import DecayingIntegratorPIDController
+
+RATE = 100
+DT = 1.0 / RATE
+
+
+def build_pid(decay_tau=2.0):
+  return DecayingIntegratorPIDController(0.05, 0.05, pos_limit=1.0, neg_limit=-1.0, rate=RATE, decay_tau=decay_tau)
+
+
+class TestDecayingIntegratorPIDController:
+  def test_accumulates_normally_when_not_frozen(self):
+    """Unfrozen behavior must be identical to stock PIDController - only freeze behavior changes."""
+    pid = build_pid()
+    for _ in range(50):
+      pid.update(error=1.0, speed=1.0, feedforward=0.0, freeze_integrator=False)
+    assert pid.i > 0
+
+  def test_decays_toward_zero_while_frozen(self):
+    pid = build_pid(decay_tau=2.0)
+    for _ in range(200):  # 2s build-up, well below saturation so anti-windup doesn't clip i
+      pid.update(error=1.0, speed=1.0, feedforward=0.0, freeze_integrator=False)
+    i_before = pid.i
+    assert i_before > 0
+
+    i_trace = []
+    for _ in range(600):  # 6s frozen = 3 time constants
+      pid.update(error=1.0, speed=1.0, feedforward=0.0, freeze_integrator=True)
+      i_trace.append(pid.i)
+
+    # monotonic decay toward zero, never grows, never flips sign
+    assert all(0 <= i_trace[k + 1] <= i_trace[k] for k in range(len(i_trace) - 1))
+    assert i_trace[-1] < 0.05 * i_before, "should be mostly decayed after 3 time constants"
+
+  def test_matches_exponential_decay_time_constant(self):
+    """Sanity-checks the decay is a real exp(-t/tau), not just 'decreasing'."""
+    pid = build_pid(decay_tau=2.0)
+    pid.i = 1.0
+
+    for _ in range(200):  # exactly one time constant (2s @ 100Hz)
+      pid.update(error=0.0, speed=1.0, feedforward=0.0, freeze_integrator=True)
+
+    assert pid.i == pytest.approx(math.exp(-1.0), rel=1e-3)
+
+  def test_no_discontinuity_at_freeze_transition(self):
+    """The whole point: control output must not jump the instant freeze conditions engage."""
+    pid = build_pid(decay_tau=2.0)
+    for _ in range(200):
+      pid.update(error=1.0, speed=1.0, feedforward=0.0, freeze_integrator=False)
+    control_before = pid.control
+
+    control_after = pid.update(error=1.0, speed=1.0, feedforward=0.0, freeze_integrator=True)
+
+    assert abs(control_after - control_before) < 0.01, "output jumped at the freeze transition"
+
+  def test_stale_integral_does_not_kick_back_in_on_unfreeze(self):
+    """The bug this exists to fix: after a long freeze, unfreezing must not suddenly reapply a
+    large stale integral untouched for however long the freeze lasted."""
+    pid = build_pid(decay_tau=2.0)
+    for _ in range(200):
+      pid.update(error=1.0, speed=1.0, feedforward=0.0, freeze_integrator=False)
+    i_peak = pid.i
+
+    for _ in range(1000):  # 10s frozen, ~5 time constants
+      pid.update(error=0.0, speed=1.0, feedforward=0.0, freeze_integrator=True)
+
+    # unfreeze: the resumed integral must be near zero, not the stale peak
+    pid.update(error=0.0, speed=1.0, feedforward=0.0, freeze_integrator=False)
+    assert abs(pid.i) < 0.01 * i_peak

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

@@ -0,0 +1,215 @@
+import pytest
+
+from opendbc.car import DT_CTRL, gen_empty_fingerprint, structs
+from opendbc.car.car_helpers import interfaces
+from opendbc.car.gm.values import CAR as GM
+from opendbc.car.honda.values import CAR as HONDA
+from opendbc.car.hyundai.values import CAR as HYUNDAI
+from opendbc.car.rivian.values import CAR as RIVIAN
+from opendbc.car.toyota.values import CAR as TOYOTA
+from opendbc.car.volkswagen.values import CAR as VOLKSWAGEN
+from openpilot.selfdrive.controls.lib.drive_helpers import should_stop
+from openpilot.selfdrive.controls.lib.longcontrol import LongControl, LongCtrlState
+from openpilot.sunnypilot.selfdrive.controls.lib.longcontrol import STOPPED_SPEED
+from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PRIUS_TSS2_ROUTE_MODEL, PlantSP
+
+
+STOP_ACCEL_VEHICLES = (TOYOTA.TOYOTA_RAV4_TSS2, HONDA.HONDA_CIVIC_2022, VOLKSWAGEN.VOLKSWAGEN_ARTEON_MK1, RIVIAN.RIVIAN_R1)
+ROUTE_STOP_ONSETS = (
+  (0.290, -0.497, -0.270, -0.302), (0.464, -0.223, -0.264, -0.292), (0.467, -0.582, -0.316, -0.359),
+  (0.530, -0.311, -0.309, -0.333), (0.581, -0.467, -0.312, -0.352), (0.398, -0.557, -0.311, -0.348),
+  (0.517, -0.290, -0.301, -0.327), (0.312, -0.420, -0.271, -0.304), (0.474, -0.509, -0.303, -0.347),
+  (0.241, -0.554, -0.573, -0.617), (0.292, -0.154, -0.302, -0.326),
+)
+
+
+def get_car_params(candidate):
+  fingerprint = gen_empty_fingerprint()
+  interface = interfaces[candidate]
+  CP = interface.get_params(candidate, fingerprint, [], True, False, False)
+  return CP, interface.get_params_sp(CP, candidate, fingerprint, [], True, False, False)
+
+
+def make_car_state(v_ego=0.2, a_ego=0.0, standstill=False) -> structs.CarState:
+  state = structs.CarState(vEgo=float(v_ego), aEgo=float(a_ego), standstill=standstill)
+  state.cruiseState.standstill = standstill
+  return state
+
+
+def make_control(candidate, initial_accel=-0.33):
+  CP, CP_SP = get_car_params(candidate)
+  control = LongControl(CP, CP_SP)
+  control.long_control_state = LongCtrlState.pid
+  control.last_output_accel = initial_accel
+  return CP, control
+
+
+def stock_stopping_output(output_accel, stop_accel):
+  return min(output_accel, 0.0) - DT_CTRL if output_accel > stop_accel else output_accel
+
+
+def test_stop_threshold_remains_unchanged():
+  assert should_stop(0.24, 0.0)
+  assert not should_stop(0.26, 0.0)
+  assert not should_stop(0.24, 0.1)
+
+
+@pytest.mark.parametrize(("v_ego", "a_ego", "a_target", "initial_accel"), ROUTE_STOP_ONSETS)
+def test_logged_stop_onsets_hold_the_existing_brake(v_ego, a_ego, a_target, initial_accel):
+  _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2, initial_accel)
+  output = control.update(True, make_car_state(v_ego, a_ego), a_target, True, (-3.5, 2.0))
+  assert control.long_control_state == LongCtrlState.stopping
+  assert output == pytest.approx(initial_accel)
+
+
+@pytest.mark.parametrize("candidate", STOP_ACCEL_VEHICLES)
+def test_urgent_braking_matches_the_stock_ramp(candidate):
+  CP, control = make_control(candidate)
+  CS = make_car_state(0.8, -0.1)
+  output = control.last_output_accel
+
+  for _ in range(round(1.0 / DT_CTRL)):
+    output = control.update(True, CS, -3.0, True, (-3.5, 2.0))
+
+  expected = -0.33
+  for _ in range(round(1.0 / DT_CTRL)):
+    expected = stock_stopping_output(expected, CP.stopAccel)
+  assert output == pytest.approx(expected)
+
+
+@pytest.mark.parametrize("candidate", STOP_ACCEL_VEHICLES)
+def test_stronger_planner_brake_matches_the_stock_ramp(candidate):
+  CP, control = make_control(candidate)
+  outputs = [control.update(True, make_car_state(0.3, -0.3), -1.0, True, (-3.5, 2.0)) for _ in range(10)]
+  expected = []
+  output = -0.33
+  for _ in range(10):
+    output = stock_stopping_output(output, CP.stopAccel)
+    expected.append(output)
+  assert outputs == pytest.approx(expected)
+
+
+@pytest.mark.parametrize("candidate", STOP_ACCEL_VEHICLES)
+def test_insufficient_deceleration_uses_the_stock_ramp_immediately(candidate):
+  CP, control = make_control(candidate)
+  output = control.update(True, make_car_state(0.6, -0.1), -0.1, True, (-3.5, 2.0))
+  assert output == pytest.approx(stock_stopping_output(-0.33, CP.stopAccel))
+
+
+def test_deceleration_noise_cannot_release_the_brake():
+  _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
+  outputs = [control.update(True, make_car_state(0.3, -0.3 if frame % 2 else 0.0), -0.1, True, (-3.5, 2.0)) for frame in range(40)]
+  assert all(current <= previous for previous, current in zip(outputs[:-1], outputs[1:], strict=True))
+
+
+def test_planner_noise_cannot_release_the_brake():
+  _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
+  outputs = [control.update(True, make_car_state(0.3, -0.3), -1.0 if frame % 2 else -0.1, True, (-3.5, 2.0)) for frame in range(40)]
+  assert all(current <= previous for previous, current in zip(outputs[:-1], outputs[1:], strict=True))
+
+
+@pytest.mark.parametrize(("v_ego", "a_ego", "a_target"), ((float("nan"), -0.3, -0.1), (0.3, float("nan"), -0.1), (0.3, -0.3, float("nan"))))
+def test_invalid_state_uses_the_stock_ramp(v_ego, a_ego, a_target):
+  CP, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
+  output = control.update(True, make_car_state(v_ego, a_ego), a_target, True, (-3.5, 2.0))
+  assert output == pytest.approx(stock_stopping_output(-0.33, CP.stopAccel))
+
+
+@pytest.mark.parametrize(("speed", "initial_accel", "grade_accel", "actuator_lag"), (
+  (0.24, 0.0, 0.0, 0.15), (0.464, -0.223, 0.0, 0.25), (0.53, -0.31, 0.0, 0.35),
+  (0.24, 0.0, 0.49, 0.15), (0.53, -0.31, 0.49, 0.25), (0.6, -0.3, 0.49, 0.35), (0.6, -0.3, 0.49, 0.5),
+))
+def test_smooth_stop_distance_is_bounded(speed, initial_accel, grade_accel, actuator_lag):
+  _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2, initial_accel)
+  applied_accel = initial_accel
+  distance = 0.0
+
+  for _ in range(round(4.0 / DT_CTRL)):
+    command = control.update(True, make_car_state(speed, applied_accel), -0.1, True, (-3.5, 2.0))
+    applied_accel += DT_CTRL / actuator_lag * (command + grade_accel - applied_accel)
+    speed = max(0.0, speed + applied_accel * DT_CTRL)
+    distance += speed * DT_CTRL
+    if speed == 0.0:
+      break
+
+  assert speed == 0.0
+  assert distance < 1.0
+
+
+@pytest.mark.parametrize("candidate", STOP_ACCEL_VEHICLES)
+def test_standstill_uses_the_stock_ramp(candidate):
+  CP, control = make_control(candidate)
+  control.long_control_state = LongCtrlState.off
+  CS = make_car_state(0.0, 0.0, standstill=True)
+  outputs = [control.update(True, CS, 0.0, False, (-3.5, 2.0)) for _ in range(round(2.0 / DT_CTRL))]
+  expected = -0.33
+  for _ in range(round(2.0 / DT_CTRL)):
+    expected = stock_stopping_output(expected, CP.stopAccel)
+  assert outputs[0] == pytest.approx(stock_stopping_output(-0.33, CP.stopAccel))
+  assert outputs[-1] == pytest.approx(expected)
+
+
+@pytest.mark.parametrize(("v_ego", "a_ego", "standstill"), ((0.6, -0.1, False), (0.0, 0.0, True)))
+def test_stopping_never_releases_a_stronger_command(v_ego, a_ego, standstill):
+  _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2, -3.0)
+  output = control.update(True, make_car_state(v_ego, a_ego, standstill), 0.0, True, (-3.5, 2.0))
+  assert output == pytest.approx(-3.0)
+
+
+def test_reported_standstill_while_moving_can_hold_the_brake():
+  _, control = make_control(GM.CHEVROLET_BOLT_EUV)
+  control.long_control_state = LongCtrlState.off
+  output = control.update(True, make_car_state(0.3, -0.3, standstill=True), -0.1, False, (-3.5, 2.0))
+  assert output == pytest.approx(-0.33)
+
+
+def test_stopping_removes_positive_acceleration_immediately():
+  _, control = make_control(HYUNDAI.HYUNDAI_SONATA, 0.2)
+  output = control.update(True, make_car_state(0.2, -0.2), -0.1, True, (-3.5, 2.0))
+  assert output == pytest.approx(-DT_CTRL)
+
+
+def test_rollback_uses_the_stock_ramp():
+  CP, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
+  output = control.update(True, make_car_state(-0.1, 0.1), -0.1, True, (-3.5, 2.0))
+  assert output == pytest.approx(stock_stopping_output(-0.33, CP.stopAccel))
+
+
+def test_departure_uses_the_stock_pid_path():
+  _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
+  control.long_control_state = LongCtrlState.stopping
+  output = control.update(True, make_car_state(0.0), 0.6, False, (-3.5, 2.0))
+  assert control.long_control_state == LongCtrlState.pid
+  assert output > 0.0
+
+
+def test_planner_mpc_and_longcontrol_complete_a_smooth_stop():
+  plant = PlantSP(
+    lead_relevancy=True, speed=0.6, distance_lead=3.6, run_long_control=True,
+    actuator_model=PRIUS_TSS2_ROUTE_MODEL,
+  )
+  plant.planner.accel_controller.enabled = True
+  plant.planner.accel_controller.profile = 1
+  plant.planner.accel_controller.update_params = lambda: None
+  plant.planner.dec._enabled = False
+  plant.planner.dec._read_params = lambda: None
+  commands = []
+  speeds = []
+  states = []
+  solver_statuses = []
+
+  while plant.current_time < 5.0:
+    result = plant.step(v_lead=0.0, v_cruise=8.0)
+    commands.append(result["actuator_command"])
+    speeds.append(result["speed"])
+    states.append(result["long_control_state"])
+    solver_statuses.append(plant.planner.mpc.last_solution_status)
+
+  stopping = states.index(LongCtrlState.stopping)
+  moving_stop_commands = [command for command, state, speed in zip(commands, states, speeds, strict=True)
+                          if state == LongCtrlState.stopping and speed > STOPPED_SPEED]
+  assert all(current <= previous + 1e-9 for previous, current in zip(commands[stopping:-1], commands[stopping + 1:], strict=True))
+  assert len(moving_stop_commands) > 1 and max(moving_stop_commands) - min(moving_stop_commands) < 1e-9
+  assert plant.speed == 0.0 and plant.distance < 1.0
+  assert plant.distance_lead - plant.distance > 3.0
+  assert all(status == 0 for status in solver_statuses)

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

@@ -0,0 +1,159 @@
+"""
+Closed-loop smoke test for the Prius TSS2 PID lateral-control toggle's starting gains
+(openpilot/sunnypilot/selfdrive/car/interfaces.py::_PRIUS_TSS2_PID_*).
+
+IMPORTANT LIMITATION: there is no real Prius TSS2 EPS actuator model anywhere in this repo (unlike
+the longitudinal plant model used by test_accel_controller_closed_loop.py, which was fit to logged
+routes). The actuator here is a generic, uncalibrated 2nd-order lag (see `SurrogateEpsActuator`) —
+it stands in for "some steering rack with plausible bandwidth," not this specific car's real EPS.
+
+This test can only prove the starting gains are stable and roughly critically damped against that
+generic surrogate. It CANNOT prove they are correctly tuned for a real Prius TSS2 — that requires
+on-road A/B via tools/lateral_maneuvers (see its README) before trusting this tune on its own.
+
+SCOPE: cruise-speed (20-30mph) only. The surrogate's steady-state gain is K = 1/(kf*v_ego**2) (see
+`SurrogateEpsActuator`), which blows up as v_ego -> 0 and produces meaningless multi-hundred-degree
+oscillation at parking-lot speed — an artifact of the surrogate, not of the kp/ki tune. This mirrors
+a real constraint: angle*v_ego**2 feedforward (and this kf calibration) is explicitly a higher-speed
+approximation (see the "25+mph" comment in latcontrol_torque_v0.py) — there's no valid basis here to
+simulate the low-speed "sharp turn" boost in _PRIUS_TSS2_PID_KP_BP/_V at all. That boost is only
+covered by the static shape check in test_prius_tss2_pid.py
+(test_kp_is_boosted_below_integrator_freeze_speed) — it has NOT been closed-loop or on-road
+verified. Validate it in a parking lot before trusting it anywhere faster.
+"""
+import math
+
+import numpy as np
+import pytest
+
+from opendbc.car import DT_CTRL
+from opendbc.car.car_helpers import interfaces as car_interfaces
+from opendbc.car.vehicle_model import VehicleModel
+from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
+from openpilot.sunnypilot.selfdrive.car.interfaces import _initialize_prius_tss2_pid_lateral_control
+
+MPH_TO_MS = 0.44704
+DURATION_S = 6.0
+STEADY_WINDOW_S = 1.0
+TARGET_LAT_ACCEL = 1.5  # m/s^2, roughly a lateral_maneuvers "step" size
+
+
+class FakeCarState:
+  def __init__(self, v_ego):
+    self.vEgo = v_ego
+    self.steeringAngleDeg = 0.0
+    self.steeringRateDeg = 0.0
+    self.steeringPressed = False
+
+
+class FakeLiveParams:
+  roll = 0.0
+  angleOffsetDeg = 0.0
+
+
+class SurrogateEpsActuator:
+  """Generic critically-damped 2nd-order torque->angle lag. NOT fit to any real car.
+
+  The steady-state gain (deg per unit torque) is derived from the tune's own `kf`, i.e.
+  K = 1 / (kf * v_ego**2) — the same steady-state relationship LatControlPID's feedforward term
+  assumes (ff = kf * angle_deg * v_ego**2 ~= torque needed to hold that angle). A fixed, unrelated
+  gain guess saturated the actuator well below the test's target angle at 20-30mph — this ties the
+  surrogate to the one steady-state assumption already baked into the tune, so the test only
+  exercises kp/ki dynamic response and stability, not an arbitrary extra unknown.
+  """
+
+  def __init__(self, deg_per_unit_torque, natural_freq_hz=3.0, zeta=1.0):
+    self.wn = 2 * math.pi * natural_freq_hz
+    self.zeta = zeta
+    self.k = deg_per_unit_torque
+    self.angle = 0.0
+    self.rate = 0.0
+
+  def step(self, torque, dt):
+    accel = self.wn ** 2 * (self.k * torque - self.angle) - 2 * self.zeta * self.wn * self.rate
+    self.rate += accel * dt
+    self.angle += self.rate * dt
+    return self.angle, self.rate
+
+
+def run_closed_loop(CP, v_ego, target_lat_accel, duration_s=DURATION_S):
+  VM = VehicleModel(CP)
+  lac = LatControlPID(CP, structs_car_params_sp(), FakeCI(), DT_CTRL)
+  deg_per_unit_torque = 1.0 / (CP.lateralTuning.pid.kf * v_ego ** 2)
+  actuator = SurrogateEpsActuator(deg_per_unit_torque)
+  CS = FakeCarState(v_ego)
+  params = FakeLiveParams()
+
+  desired_curvature = -target_lat_accel / v_ego ** 2
+  desired_angle_deg = math.degrees(VM.get_steer_from_curvature(-desired_curvature, v_ego, 0.0))
+
+  n_steps = int(duration_s / DT_CTRL)
+  angle_trace = np.zeros(n_steps)
+  torque_trace = np.zeros(n_steps)
+
+  for i in range(n_steps):
+    output_torque, _, _ = lac.update(True, CS, VM, params, False, desired_curvature, None, False, 0.0)
+    output_torque = float(output_torque)
+    angle, rate = actuator.step(output_torque, DT_CTRL)
+    CS.steeringAngleDeg = float(angle)
+    CS.steeringRateDeg = float(rate)
+    angle_trace[i] = angle
+    torque_trace[i] = output_torque
+
+  return angle_trace, torque_trace, desired_angle_deg
+
+
+def structs_car_params_sp():
+  from opendbc.car import structs
+  return structs.CarParamsSP()
+
+
+class FakeCI:
+  @staticmethod
+  def get_steer_feedforward_function():
+    return lambda desired_angle, v_ego: desired_angle * (v_ego ** 2)
+
+
+def make_prius_tss2_cp():
+  CarInterface = car_interfaces['TOYOTA_PRIUS_TSS2']
+  CP = CarInterface.get_params('TOYOTA_PRIUS_TSS2', {0: {}, 1: {}, 2: {}}, [], alpha_long=False, is_release=False, docs=False)
+  _initialize_prius_tss2_pid_lateral_control(CP)
+  assert CP.lateralTuning.which() == 'pid'
+  return CP
+
+
+@pytest.mark.parametrize('v_mph', [20.0, 30.0])
+def test_starting_gains_settle_without_diverging(v_mph):
+  CP = make_prius_tss2_cp()
+  v_ego = v_mph * MPH_TO_MS
+
+  angle_trace, torque_trace, desired_angle_deg = run_closed_loop(CP, v_ego, TARGET_LAT_ACCEL)
+
+  assert np.all(np.isfinite(angle_trace)), "diverged/NaN — unsafe to ever test on-road"
+  assert np.all(np.abs(torque_trace) <= 1.0 + 1e-6), "output_torque exceeded steer_max=1.0 saturation bound"
+
+  steady_n = int(STEADY_WINDOW_S / DT_CTRL)
+  steady_angle = angle_trace[-steady_n:]
+  settle_error_deg = abs(np.mean(steady_angle) - desired_angle_deg)
+  oscillation_deg = np.ptp(steady_angle)
+
+  assert settle_error_deg < 1.0, f"steady-state tracking error too large: {settle_error_deg:.3f} deg (target {desired_angle_deg:.2f} deg)"
+  assert oscillation_deg < 0.5, f"sustained oscillation in tail window: {oscillation_deg:.3f} deg peak-to-peak (limit-cycle candidate)"
+
+
+def test_gains_are_not_a_no_op_sanity_check():
+  """Confirms this harness actually has teeth: gains far more aggressive than the shipped starting
+  point produce a limit cycle against the same surrogate actuator, so the tolerances above aren't
+  trivially satisfied by any input."""
+  CP = make_prius_tss2_cp()
+  CP.lateralTuning.pid.kpBP = [0.0]
+  CP.lateralTuning.pid.kpV = [1.5]   # 10x the cruise-speed kp
+  CP.lateralTuning.pid.kiBP = [0.0]
+  CP.lateralTuning.pid.kiV = [0.5]   # 10x the shipped ki
+  v_ego = 20.0 * MPH_TO_MS
+
+  angle_trace, _, desired_angle_deg = run_closed_loop(CP, v_ego, TARGET_LAT_ACCEL)
+
+  steady_n = int(STEADY_WINDOW_S / DT_CTRL)
+  oscillation_deg = np.ptp(angle_trace[-steady_n:])
+  assert oscillation_deg > 0.5, "expected an aggressive 10x-gain tune to visibly ring against this actuator; harness may not be sensitive"

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

@@ -0,0 +1,99 @@
+"""
+Copyright (c) 2021-, rav4kumar, 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 pytest
+
+from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
+from openpilot.sunnypilot.selfdrive.controls.lib.relc import (
+  RoadEdgeLaneChangeController, EDGE_REACTION_TIME, EDGE_CLEAR_TIME, MIN_SPEED,
+)
+
+V_HIGH = MIN_SPEED + 2.0
+V_LOW = MIN_SPEED - 1.0
+
+
+@pytest.fixture
+def relc(mocker):
+  mock_params = mocker.patch("openpilot.sunnypilot.selfdrive.controls.lib.relc.Params")
+  mock_params.return_value.get_bool.return_value = True
+  controller = RoadEdgeLaneChangeController(DesireHelper())
+  controller.enabled = True
+  return controller
+
+
+def drive(controller, road_edge_stds, lane_line_probs, seconds, v_ego=V_HIGH):
+  for _ in range(int(seconds / DT_MDL) + 1):
+    controller.update(road_edge_stds, lane_line_probs, v_ego)
+
+
+@pytest.mark.parametrize("road_edge_stds,lane_line_probs,attr", [
+  ([0.0, 0.9], [0.0, 0.8, 0.8, 0.8], "left_edge_detected"),
+  ([0.9, 0.0], [0.8, 0.8, 0.8, 0.0], "right_edge_detected"),
+])
+def test_edge_detection(relc, road_edge_stds, lane_line_probs, attr):
+  drive(relc, road_edge_stds, lane_line_probs, EDGE_REACTION_TIME + 0.1)
+  assert getattr(relc, attr)
+
+
+def test_edge_detection_requires_time(relc):
+  drive(relc, [0.0, 0.9], [0.0, 0.8, 0.8, 0.8], EDGE_REACTION_TIME - 0.05)
+  assert not relc.left_edge_detected
+
+
+def test_both_edges_detected(relc):
+  drive(relc, [0.0, 0.0], [0.0, 0.8, 0.8, 0.0], EDGE_REACTION_TIME + 0.1)
+  assert relc.left_edge_detected
+  assert relc.right_edge_detected
+
+
+def test_noise_doesnt_clear(relc):
+  edge = ([0.0, 0.9], [0.0, 0.8, 0.8, 0.8])
+  clear = ([0.9, 0.9], [0.8, 0.8, 0.8, 0.8])
+
+  drive(relc, *edge, EDGE_REACTION_TIME + 0.1)
+  assert relc.left_edge_detected
+
+  relc.update(*clear, V_HIGH)
+  relc.update(*edge, V_HIGH)
+  assert relc.left_edge_detected
+
+
+def test_clears_after_window(relc):
+  edge = ([0.0, 0.9], [0.0, 0.8, 0.8, 0.8])
+  clear = ([0.9, 0.9], [0.8, 0.8, 0.8, 0.8])
+
+  drive(relc, *edge, EDGE_REACTION_TIME + 0.1)
+  assert relc.left_edge_detected
+
+  drive(relc, *clear, EDGE_CLEAR_TIME + 0.05)
+  assert not relc.left_edge_detected
+  assert relc.left_edge_timer == 0.0
+
+
+def test_low_speed_skips(relc):
+  drive(relc, [0.0, 0.9], [0.0, 0.8, 0.8, 0.8], EDGE_REACTION_TIME + 0.1, v_ego=V_LOW)
+  assert not relc.left_edge_detected
+  assert relc.left_edge_timer == 0.0
+
+
+def test_speed_drop_resets(relc):
+  drive(relc, [0.0, 0.9], [0.0, 0.8, 0.8, 0.8], EDGE_REACTION_TIME + 0.1)
+  assert relc.left_edge_detected
+
+  relc.update([0.0, 0.9], [0.0, 0.8, 0.8, 0.8], V_LOW)
+  assert not relc.left_edge_detected
+
+
+def test_param_off_resets(relc):
+  drive(relc, [0.0, 0.9], [0.0, 0.8, 0.8, 0.8], EDGE_REACTION_TIME + 0.1)
+  assert relc.left_edge_detected
+
+  relc.params.get_bool.return_value = False
+  relc.read_params()
+  relc.update([0.0, 0.9], [0.0, 0.8, 0.8, 0.8], V_HIGH)
+  assert not relc.left_edge_detected
+  assert not relc.right_edge_detected

openpilot/sunnypilot/selfdrive/controls/tests/test_controlsd_ext.py

@@ -0,0 +1,109 @@
+from opendbc.car import structs
+
+from openpilot.sunnypilot.selfdrive.controls import controlsd_ext
+from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_pid_ext import LatControlPidSmooth
+
+
+class FakeParams:
+  def __init__(self, values=None):
+    self.values = values or {}
+
+  def get_bool(self, key):
+    return bool(self.values.get(key, False))
+
+  def get(self, key, return_default=False):
+    return self.values.get(key)
+
+
+class FakeCI:
+  def get_steer_feedforward_function(self):
+    return lambda desired_angle, v_ego: desired_angle * (v_ego ** 2)
+
+
+def make_ext(CP, params_values=None):
+  # Bypass __init__: it blocks on CarParamsSP over messaging, which isn't available in a unit test.
+  ext = controlsd_ext.ControlsExt.__new__(controlsd_ext.ControlsExt)
+  ext.CP = CP
+  ext.CP_SP = structs.CarParamsSP()
+  ext.params = FakeParams(params_values)
+  return ext
+
+
+def make_prius_tss2_pid_cp():
+  CP = structs.CarParams(carFingerprint='TOYOTA_PRIUS_TSS2')
+  CP.lateralTuning.init('pid')
+  CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV = [0.0, 5.0], [0.30, 0.15]
+  CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV = [0.0], [0.05]
+  CP.lateralTuning.pid.kf = 4e-05
+  return CP
+
+
+class TestInitializeLateralControlPidSmoothDispatch:
+  """The Prius TSS2 PID toggle's decaying-integrator variant must be scoped to exactly the one
+  (fingerprint, union) combination it applies to - never touch any other PID car's controller."""
+
+  def test_prius_tss2_pid_gets_smooth_variant(self):
+    ext = make_ext(make_prius_tss2_pid_cp())
+    lac = object()
+
+    result = ext.initialize_lateral_control(lac, FakeCI(), 0.01)
+
+    assert isinstance(result, LatControlPidSmooth)
+
+  def test_other_native_pid_car_is_untouched(self):
+    """A hypothetical other brand's native PID car must NOT get swapped to our variant just
+    because the union happens to be 'pid' - only our exact fingerprint qualifies."""
+    CP = structs.CarParams(carFingerprint='SOME_OTHER_PID_CAR')
+    CP.lateralTuning.init('pid')
+    ext = make_ext(CP)
+    lac = object()
+
+    result = ext.initialize_lateral_control(lac, FakeCI(), 0.01)
+
+    assert result is lac
+
+
+class TestInitializeLateralControlPidGuard:
+  """Regression test for the crash this toggle would otherwise cause: torque-only LatControl
+  variants read CP.lateralTuning.torque directly, which raises on a capnp union that's actually
+  'pid' (e.g. the Prius TSS2 PID toggle). initialize_lateral_control must never attempt that."""
+
+  def test_pid_union_returns_lac_unchanged_even_with_enforce_torque_on(self):
+    CP = structs.CarParams()
+    CP.lateralTuning.init('pid')
+    ext = make_ext(CP, {'EnforceTorqueControl': True, 'TorqueControlTune': 0.0})
+
+    lac = object()
+    result = ext.initialize_lateral_control(lac, CI=None, dt=0.01)
+
+    assert result is lac
+
+  def test_pid_union_returns_lac_unchanged_with_enforce_torque_off(self):
+    CP = structs.CarParams()
+    CP.lateralTuning.init('pid')
+    ext = make_ext(CP, {'EnforceTorqueControl': False})
+
+    lac = object()
+    result = ext.initialize_lateral_control(lac, CI=None, dt=0.01)
+
+    assert result is lac
+
+  def test_torque_union_still_dispatches_to_torque_v0(self, monkeypatch):
+    calls = []
+
+    class StubTorqueV0:
+      def __init__(self, CP, CP_SP, CI, dt):
+        calls.append((CP, CP_SP, CI, dt))
+
+    monkeypatch.setattr(controlsd_ext, 'LatControlTorqueV0', StubTorqueV0)
+
+    CP = structs.CarParams()
+    CP.lateralTuning.init('torque')
+    ext = make_ext(CP, {'EnforceTorqueControl': False})
+    ext.CP_SP = None
+
+    lac = object()
+    result = ext.initialize_lateral_control(lac, CI=None, dt=0.01)
+
+    assert isinstance(result, StubTorqueV0)
+    assert len(calls) == 1

openpilot/sunnypilot/selfdrive/selfdrived/events.py

@@ -244,4 +244,12 @@ EVENTS_SP: dict[int, dict[str, Alert | AlertCallbackType]] = {
       AlertStatus.normal, AlertSize.none,
       Priority.MID, VisualAlert.none, AudibleAlert.prompt, 3.),
   },
+
+  EventNameSP.laneChangeRoadEdge: {
+    ET.WARNING: Alert(
+      "Lane Change Unavailable: Road Edge",
+      "",
+      AlertStatus.userPrompt, AlertSize.small,
+      Priority.LOW, VisualAlert.none, AudibleAlert.prompt, 0.1),
+  },
 }

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

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

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

@@ -0,0 +1,161 @@
+from collections.abc import Callable
+import math
+from typing import cast
+
+import pytest
+
+from openpilot.common.realtime import DT_MDL
+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
+
+
+@pytest.mark.parametrize("scenario", PARITY_SCENARIOS, ids=list(PARITY_SCENARIOS))
+def test_plant_sp_matches_stock_plant_on_shared_kwargs(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):
+        assert sp_result[key] == pytest.approx(stock_result[key]), 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"
+    assert sp_a_target == pytest.approx(stock_a_target), 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():
+  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"]
+  assert callback_inputs[0][2]["dRel"] == pytest.approx(50.0)
+  assert result["truth_lead"]["dRel"] == pytest.approx(50.0)
+  assert result["lead_one_observation"]["dRel"] == pytest.approx(12.5)
+  assert result["lead_one_observation"]["radarTrackId"] == 42
+  assert result["lead_two_observation"] is None
+  assert result["distance_lead"] == pytest.approx(50.0 + 8.0 * DT_MDL)
+
+
+def test_model_action_realized_acceleration_and_source_logging():
+  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}
+  assert first["published_a_ego"] == pytest.approx(0.0)
+  assert second["published_a_ego"] == pytest.approx(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_target" in first
+  assert "base_target" in first
+  assert "raw_energy_cap" in first
+  assert "live_filtered_cap" in first
+  assert "shadow_filtered_cap" not in first
+  assert first["lead_one_observation"] is not None
+  assert first["truth_lead"] == first["lead_one_observation"]
+
+
+def test_configurable_transport_delay_and_first_order_lag():
+  plant = PlantSP(speed=10.0, actuator_delay=2 * DT_MDL, actuator_lag=0.2)
+
+  assert plant.planner.CP.longitudinalActuatorDelay == pytest.approx(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
+  assert delayed_commands[2][1] == pytest.approx(expected_acceleration)
+
+
+@pytest.mark.parametrize(
+  ("delay", "lag"),
+  [(-0.1, 0.0), (float("nan"), 0.0), (float("inf"), 0.0), (None, -0.1), (None, float("nan")), (None, float("inf"))],
+)
+def test_invalid_actuator_dynamics(delay, lag):
+  with pytest.raises(ValueError):
+    PlantSP(actuator_delay=delay, actuator_lag=lag)

openpilot/sunnypilot/sunnylink/settings_ui.json

@@ -620,6 +620,58 @@
                 }
               ]
             },
+            {
+              "key": "AccelPersonalityEnabled",
+              "widget": "toggle",
+              "title": "Enable Accel Controller",
+              "description": "Begin slowing early and smoothly behind lead vehicles. Stock longitudinal control retains braking and stopping authority.",
+              "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 slows earliest and recovers gently, Normal balances comfort and response, and Sport reacts and recovers more quickly.",
+              "options": [
+                {
+                  "value": 0,
+                  "label": "Eco"
+                },
+                {
+                  "value": 1,
+                  "label": "Normal"
+                },
+                {
+                  "value": 2,
+                  "label": "Sport"
+                }
+              ],
+              "enablement": [
+                {
+                  "type": "capability",
+                  "field": "has_longitudinal_control",
+                  "equals": true
+                },
+                {
+                  "type": "param",
+                  "key": "AccelPersonalityEnabled",
+                  "equals": true
+                }
+              ]
+            },
             {
               "key": "IntelligentCruiseButtonManagement",
               "widget": "toggle",
@@ -2001,6 +2053,22 @@
                   "equals": true
                 }
               ]
+            },
+            {
+              "key": "PlanplusControl",
+              "widget": "option",
+              "title": "Plan Plus Controls",
+              "description": "Adjust planplus model recentering strength. The higher this number the more aggressively the model will recover to lane center; too high and it will ping-pong.",
+              "min": 0.0,
+              "max": 2.0,
+              "step": 0.1,
+              "enablement": [
+                {
+                  "type": "param",
+                  "key": "ShowAdvancedControls",
+                  "equals": true
+                }
+              ]
             }
           ]
         },
@@ -2204,6 +2272,62 @@
       "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": "ToyotaPriusTss2Pid",
+          "widget": "toggle",
+          "needs_onroad_cycle": true,
+          "title": "Toyota: Prius TSS2 PID Lateral Control (Alpha)",
+          "description": "Use a PID lateral controller instead of torque control on Prius TSS2. Overrides Neural Network Lateral Control and Enforce Torque Control for this car. Starting gains are unvalidated on a real Prius TSS2 \u2014 expect to need on-road tuning. Use at your own risk.",
+          "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,32 @@ sections:
       label: Relaxed
     enablement:
     - $ref: '#/macros/longitudinal'
+  - key: AccelPersonalityEnabled
+    widget: toggle
+    title: Enable Accel Controller
+    description: Begin slowing early and smoothly behind lead vehicles. Stock longitudinal control retains braking
+      and stopping authority.
+    visibility:
+    - $ref: '#/macros/longitudinal'
+    enablement:
+    - $ref: '#/macros/longitudinal'
+  - key: AccelPersonality
+    widget: multiple_button
+    title: Acceleration Profile
+    description: Eco slows earliest and recovers gently, Normal balances comfort and response, and Sport reacts
+      and recovers more quickly.
+    options:
+    - value: 0
+      label: Eco
+    - value: 1
+      label: Normal
+    - value: 2
+      label: Sport
+    enablement:
+    - $ref: '#/macros/longitudinal'
+    - type: param
+      key: AccelPersonalityEnabled
+      equals: true
   - key: IntelligentCruiseButtonManagement
     widget: toggle
     title: Intelligent Cruise Button Management (ICBM) (Alpha)

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

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

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

@@ -82,6 +82,39 @@ 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: ToyotaPriusTss2Pid
+    widget: toggle
+    needs_onroad_cycle: true
+    title: 'Toyota: Prius TSS2 PID Lateral Control (Alpha)'
+    description: Use a PID lateral controller instead of torque control on Prius TSS2. Overrides Neural
+      Network Lateral Control and Enforce Torque Control for this car. Starting gains are unvalidated on
+      a real Prius TSS2 — expect to need on-road tuning. Use at your own risk.
+    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/tests/test_settings_schema.py

@@ -272,6 +272,22 @@ class TestKnownPanels:
     nnlc_enable_keys = {r.get("key") for r in nnlc.get("enablement", []) if r.get("type") == "param"}
     assert "EnforceTorqueControl" in nnlc_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": "param",
+      "key": "AccelPersonalityEnabled",
+      "equals": True,
+    } in items["AccelPersonality"]["enablement"]
+
 
 class TestKnownVehicleSettings:
   def test_hyundai_has_longitudinal_tuning(self, schema):

openpilot/system/hardware/fan_controller.py

@@ -15,8 +15,13 @@ class FanController:
     self.controller = PIDController(k_p=0, k_i=4e-3, rate=rate)
 
   def update(self, cur_temp: float, ignition: bool) -> int:
-    self.controller.pos_limit = 100 if ignition else 30
-    self.controller.neg_limit = 30 if ignition else 0
+    if ignition:
+      # always run fan at max onroad, prioritize cooling over noise
+      self.last_ignition = ignition
+      return 100
+
+    self.controller.pos_limit = 30
+    self.controller.neg_limit = 0
 
     if ignition != self.last_ignition:
       self.controller.reset()

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
@@ -98,7 +99,7 @@ class GuiScrollPanel2:
       # simple exponential return if out of bounds
       # out of bounds is handled by snapping, so skip if set
       out_of_bounds = self.get_offset() > max_offset or self.get_offset() < min_offset
-      if out_of_bounds and snap_target is None:
+      if out_of_bounds and snap_target is None and self._handle_out_of_bounds:
         target = max_offset if self.get_offset() > max_offset else min_offset
 
         dt = rl.get_frame_time() or 1e-6

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,8 +190,12 @@ 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)
+      if self._horizontal:
+        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)
+      else:
+        center_pos = self._rect.y + self._rect.height / 2
+        closest_delta_pos = min((((item.rect.y + item.rect.height / 2) - 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)