Reapply "feat(dec): rework dynamic experimental controller"

This reverts commit cc8115141f.

.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/commaai/msgq.git

cereal/custom.capnp

@@ -194,6 +194,7 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
   aTarget @5 :Float32;
   events @6 :List(OnroadEventSP.Event);
   e2eAlerts @7 :E2eAlerts;
+  acceleration @8 :Acceleration;
 
   struct DynamicExperimentalControl {
     state @0 :DynamicExperimentalControlState;
@@ -296,6 +297,26 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
     greenLightAlert @0 :Bool;
     leadDepartAlert @1 :Bool;
   }
+
+  # Acceleration Personality (Eco / Normal / Sport)
+  struct Acceleration {
+    personality @0 :AccelerationPersonality;
+    enabled @1 :Bool;
+    maxAccel @2 :Float32;     # current speed-indexed accel ceiling
+    brakeNeed @3 :Float32;    # predicted decel demand from the lookahead (m/s^2, positive)
+    decelTarget @4 :Float32;  # early-soft comfort decel target (m/s^2, negative)
+    smoothActive @5 :Bool;    # early-soft braking currently shaping the target
+    bypassed @6 :Bool;        # passthrough to stock plan (hard brake / FCW / should_stop / closing lead / e2e)
+    comfortStopActive @7 :Bool;   # low-speed comfort decel-to-stop floor currently governing (behind a near-stopped lead)
+    comfortStopFloor @8 :Float32; # comfort-stop floor commanded (m/s^2, negative; 0 when not engaged)
+    leadUnstable @9 :Bool;        # RadarDistance lead-instability telemetry (bimodal/bouncing radar lead; informational, no control effect yet)
+  }
+
+  enum AccelerationPersonality {
+    eco @0;
+    normal @1;
+    sport @2;
+  }
 }
 
 struct OnroadEventSP @0xda96579883444c35 {
@@ -342,6 +363,7 @@ struct OnroadEventSP @0xda96579883444c35 {
     speedLimitChanged @21;
     speedLimitPending @22;
     e2eChime @23;
+    laneChangeRoadEdge @24;
   }
 }
 
@@ -448,6 +470,8 @@ struct LiveMapDataSP @0xf416ec09499d9d19 {
 
 struct ModelDataV2SP @0xa1680744031fdb2d {
   laneTurnDirection @0 :TurnDirection;
+  leftLaneChangeEdgeBlock @1 :Bool;
+  rightLaneChangeEdgeBlock @2 :Bool;
 
   enum TurnDirection {
     none @0;

common/params_keys.h

@@ -4,6 +4,7 @@
 #include <unordered_map>
 
 #include "cereal/gen/cpp/log.capnp.h"
+#include "cereal/gen/cpp/custom.capnp.h"
 
 inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"AccessToken", {CLEAR_ON_MANAGER_START | DONT_LOG, STRING}},
@@ -179,12 +180,19 @@ 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"}},
+
     // MADS params
     {"Mads", {PERSISTENT | BACKUP, BOOL, "1"}},
     {"MadsMainCruiseAllowed", {PERSISTENT | BACKUP, BOOL, "1"}},
@@ -228,6 +236,13 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"DynamicExperimentalControl", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"BlindSpot", {PERSISTENT | BACKUP, BOOL, "0"}},
 
+    // Acceleration Personality (Eco / Normal / Sport)
+    {"AccelPersonalityEnabled", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"AccelPersonality", {PERSISTENT | BACKUP, INT, std::to_string(static_cast<int>(cereal::LongitudinalPlanSP::AccelerationPersonality::NORMAL))}},
+
+    // Radar Distance: hold a lead through radar flicker/dropout so the MPC doesn't lose+regain it
+    {"RadarDistance", {PERSISTENT | BACKUP, BOOL, "0"}},
+
     // sunnypilot model params
     {"CameraOffset", {PERSISTENT | BACKUP, FLOAT, "0.0"}},
     {"LagdToggle", {PERSISTENT | BACKUP, BOOL, "1"}},

selfdrive/car/card.py

@@ -10,7 +10,7 @@ from cereal import car, log, custom
 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
@@ -121,7 +121,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)

selfdrive/controls/lib/desire_helper.py

@@ -56,7 +56,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
@@ -88,8 +88,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)
 

selfdrive/controls/lib/longitudinal_planner.py

@@ -110,7 +110,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     # No change cost when user is controlling the speed, or when standstill
     prev_accel_constraint = not (reset_state or sm['carState'].standstill)
 
-    accel_clip = [ACCEL_MIN, get_max_accel(v_ego)]
+    accel_clip = [ACCEL_MIN, self.accel.get_max_accel(v_ego)]
     steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg
     accel_clip = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_clip, self.CP)
 
@@ -138,7 +138,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
 
     self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality)
     self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
-    self.mpc.update(sm['radarState'], v_cruise, personality=sm['selfdriveState'].personality)
+    self.mpc.update(self.smooth_radarstate(sm['radarState']), v_cruise, personality=sm['selfdriveState'].personality)
 
     self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)
     self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution)
@@ -160,7 +160,8 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     output_a_target_e2e = sm['modelV2'].action.desiredAcceleration
     output_should_stop_e2e = sm['modelV2'].action.shouldStop
 
-    if self.is_e2e(sm):
+    is_e2e = 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:
@@ -169,8 +170,15 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
       output_a_target = output_a_target_mpc
       self.output_should_stop = output_should_stop_mpc
 
-    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)
+    # Acceleration Personality: early soft braking (never weaker than the plan). No-op when disabled.
+    output_a_target = self.accel.smooth_target_accel(output_a_target, self.a_desired_trajectory, CONTROL_N_T_IDX,
+                                                     self.output_should_stop or force_slow_decel, reset=reset_state, stock_brake=is_e2e,
+                                                     speed_trajectory=self.v_desired_trajectory)
+
+    # Lower (braking) bound and the ceiling's downward slew stay at the stock rate; only the ceiling's
+    # upward slew is tier-dependent (Acceleration Personality).
+    accel_clip[0] = np.clip(accel_clip[0], self.prev_accel_clip[0] - 0.05, self.prev_accel_clip[0] + 0.05)
+    accel_clip[1] = np.clip(accel_clip[1], self.prev_accel_clip[1] - 0.05, self.prev_accel_clip[1] + self.accel.get_rise_rate())
     self.output_a_target = np.clip(output_a_target, accel_clip[0], accel_clip[1])
     self.prev_accel_clip = accel_clip
 

selfdrive/controls/tests/test_longitudinal_smoothing_wiring.py

@@ -0,0 +1,52 @@
+import inspect
+import re
+from pathlib import Path
+
+from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
+
+
+REPO_ROOT = Path(__file__).resolve().parents[3]
+
+
+def test_smoothing_params_default_off():
+  params_keys = (REPO_ROOT / "common/params_keys.h").read_text()
+
+  assert re.search(r'"AccelPersonalityEnabled", \{PERSISTENT \| BACKUP, BOOL, "0"\}', params_keys)
+  assert re.search(r'"RadarDistance", \{PERSISTENT \| BACKUP, BOOL, "0"\}', params_keys)
+
+
+def test_longitudinal_smoothing_stays_planner_side():
+  update_src = inspect.getsource(LongitudinalPlanner.update)
+
+  accel_ceiling_idx = update_src.index("self.accel.get_max_accel(v_ego)")
+  radar_smoothing_idx = update_src.index("self.mpc.update(self.smooth_radarstate(sm['radarState'])")
+  accel_smoothing_idx = update_src.index("self.accel.smooth_target_accel(")
+
+  assert accel_ceiling_idx < radar_smoothing_idx
+  assert radar_smoothing_idx < accel_smoothing_idx
+
+
+# Tokens for the reverted input-side DEC model-stop-target (capped v_target into the MPC pre-solve). It was
+# superseded by DEC blended-mode and chased a source-fixed radar gate; it must not silently return.
+_DEC_MODEL_STOP_TOKENS = ("apply_model_stop_target", "force_stop_requested", "_update_model_stop", "MODEL_STOP_TARGET_TIME")
+
+
+def test_dec_model_stop_target_not_reintroduced():
+  this_file = Path(__file__).resolve()
+  for sub in ("selfdrive/controls", "sunnypilot/selfdrive/controls"):
+    for path in (REPO_ROOT / sub).rglob("*.py"):
+      if path.resolve() == this_file:
+        continue                                      # this guard names the tokens as strings
+      src = path.read_text()
+      for token in _DEC_MODEL_STOP_TOKENS:
+        assert token not in src, f"reverted DEC model-stop-target ({token}) re-introduced in {path}"
+
+
+def test_long_feature_gates():
+  # comfort_stop OFF: keep the stock smooth taper (flat-hold firms the end); farther-stop comes from the MPC
+  # stop-target shift instead. vLead speed-damp (B) stays OFF pending on-road proof.
+  from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import COMFORT_STOP_ENABLED
+  from openpilot.sunnypilot.selfdrive.controls.lib.radar_distance.radar_distance import VLEAD_DAMP_ENABLED
+
+  assert COMFORT_STOP_ENABLED is False
+  assert VLEAD_DAMP_ENABLED is False

selfdrive/modeld/modeld.py

@@ -28,6 +28,7 @@ from openpilot.selfdrive.modeld.helpers import usbgpu_present, modeld_pkl_path,
 
 from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
 from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
+from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController
 
 PROCESS_NAME = "selfdrive.modeld.modeld"
 SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
@@ -223,6 +224,7 @@ def main(demo=False):
   prev_action = log.ModelDataV2.Action()
 
   DH = DesireHelper()
+  RELC = RoadEdgeLaneChangeController(DH)
 
   while True:
     # Keep receiving frames until we are at least 1 frame ahead of previous extra frame
@@ -325,7 +327,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, modelv2_send.modelV2.roadEdges)
+      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

selfdrive/selfdrived/selfdrived.py

@@ -321,9 +321,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)

selfdrive/ui/layouts/settings/toggles.py

@@ -31,6 +31,15 @@ DESCRIPTIONS = {
     "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)."
   ),
+  "AccelPersonalityEnabled": tr_noop("Enable Eco/Normal/Sport acceleration profiles, including early soft braking."),
+  "AccelPersonality": tr_noop(
+    "Eco accelerates gently and brakes early and soft; Sport accelerates briskly. " +
+    "Hard-braking authority is always preserved."
+  ),
+  "RadarDistance": tr_noop(
+    "Hold a lead through brief radar flicker/dropout so sunnypilot does not lose and re-grab it, " +
+    "smoothing the hard/late brakes that radar drop-outs cause. Braking is never reduced below stock."
+  ),
   "AlwaysOnDM": tr_noop("Enable driver monitoring even when sunnypilot is not engaged."),
   'RecordFront': tr_noop("Upload data from the driver facing camera and help improve the driver monitoring algorithm."),
   "IsMetric": tr_noop("Display speed in km/h instead of mph."),
@@ -64,6 +73,12 @@ class TogglesLayout(Widget):
         "disengage_on_accelerator.png",
         False,
       ),
+      "RadarDistance": (
+        lambda: tr("Radar Distance"),
+        DESCRIPTIONS["RadarDistance"],
+        "speed_limit.png",
+        False,
+      ),
       "IsLdwEnabled": (
         lambda: tr("Enable Lane Departure Warnings"),
         DESCRIPTIONS["IsLdwEnabled"],
@@ -106,6 +121,24 @@ class TogglesLayout(Widget):
       icon="speed_limit.png"
     )
 
+    self._accel_personality_enabled = toggle_item(
+      lambda: tr("Enable Acceleration Profiles"),
+      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 +168,11 @@ class TogglesLayout(Widget):
 
       self._toggles[param] = toggle
 
-      # insert longitudinal personality after NDOG toggle
+      # insert longitudinal + acceleration personality 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)
@@ -176,11 +211,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(True)
       else:
         # no long for now
         self._toggles["ExperimentalMode"].action_item.set_enabled(False)
         self._toggles["ExperimentalMode"].action_item.set_state(False)
         self._long_personality_setting.action_item.set_enabled(False)
+        self._accel_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.")
@@ -247,3 +286,9 @@ class TogglesLayout(Widget):
 
   def _set_longitudinal_personality(self, button_index: int):
     self._params.put("LongitudinalPersonality", button_index, block=True)
+
+  def _set_accel_personality(self, button_index: int):
+    self._params.put("AccelPersonality", button_index, block=True)
+
+  def _set_accel_personality_enabled(self, state: bool):
+    self._params.put_bool("AccelPersonalityEnabled", state, block=True)

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

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

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

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)

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

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)

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

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)

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

sunnypilot/modeld_v2/modeld.py

@@ -40,6 +40,7 @@ from openpilot.sunnypilot.modeld_v2.camera_offset_helper import CameraOffsetHelp
 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 = "selfdrive.modeld.modeld_tinygrad"
 
@@ -329,6 +330,7 @@ def main(demo=False):
   prev_action = log.ModelDataV2.Action()
 
   DH = DesireHelper()
+  RELC = RoadEdgeLaneChangeController(DH)
   meta_constants = load_meta_constants()
 
   while True:
@@ -433,7 +435,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, modelv2_send.modelV2.roadEdges)
+      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

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

@@ -0,0 +1,253 @@
+"""
+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.
+
+Acceleration personality: per-profile launch/cruise accel ceiling (ECO/NORMAL/SPORT), an anticipatory
+brake front-load, and a low-speed comfort stop. SAFETY: a firm/closing brake -- emergency (raw <=
+HARD_BRAKE_TARGET_ACCEL or brake_need >= HARD_BRAKE_NEED), FCW/crash, should_stop, or blended/e2e -- passes
+the plan straight through at full strength and rate, never softened/delayed/rate-limited. Only on the
+NON-emergency comfort path may the onset arrive spread by at most ONSET_SPREAD_MAX (a tightly bounded,
+transient lag) so a gentle brake does not land as a step. The front-load and comfort stop only ever ADD
+braking (min(., plan)). Disabled => byte-stock.
+"""
+
+from collections.abc import Sequence
+
+import numpy as np
+
+from cereal import messaging
+from opendbc.car import structs
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+from openpilot.sunnypilot import get_sanitize_int_param
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import \
+  NORMAL, PERSONALITY_MIN, PERSONALITY_MAX, A_CRUISE_MAX_BP, A_CRUISE_MAX_V, RISE_RATE, \
+  STOCK_A_CRUISE_MAX_V, STOCK_RISE_RATE, SMOOTH_DECEL_BP, SMOOTH_DECEL_V, BRAKE_DEEPENING_JERK, \
+  BRAKE_RELEASE_JERK, ACCEL_RISE_JERK, SMOOTH_DECEL_LOOKAHEAD_T, MIN_SMOOTH_BRAKE_NEED, \
+  HARD_BRAKE_TARGET_ACCEL, HARD_BRAKE_NEED, OVERBITE_CAP, STOP_PASSTHROUGH_V, \
+  STOP_IMMINENT_VEGO, STOP_IMMINENT_LOOKAHEAD_T, ONSET_SPREAD_MAX, ONSET_SPREAD_JERK, \
+  COMFORT_STOP_ENABLED, COMFORT_STOP_V, COMFORT_STOP_LEAD_V, COMFORT_STOP_GAP, \
+  COMFORT_STOP_MAX_DECEL, COMFORT_STOP_RELEASE_V, COMFORT_STOP_HOLD_GAP, \
+  GAS_SUPPRESS_ENABLED, GAS_SUPPRESS_DREL, GAS_SUPPRESS_VREL, GAS_SUPPRESS_CLOSE, \
+  GAS_SUPPRESS_RECENT_T, GAS_SUPPRESS_BRAKE_THR
+
+_ZERO_ACCEL_EPS = 1e-6
+
+
+class AccelController:
+  def __init__(self, CP: structs.CarParams, mpc, params=None):
+    self._CP = CP
+    self._mpc = mpc
+    self._params = params or Params()
+    self._frame = 0
+    self._enabled = self._params.get_bool("AccelPersonalityEnabled")
+    self._personality = NORMAL
+    self._v_ego = 0.0
+    self._last_target_accel = 0.0
+    self._brake_need = 0.0
+    self._decel_target = 0.0
+    self._smooth_active = False
+    self._bypassed = False
+    self._lead_status = False
+    self._lead_d = 0.0
+    self._lead_vlead = 0.0
+    self._stop_floor = 0.0       # comfort-stop floor latch (monotone within a stop episode, eased on release)
+    self._comfort_stop_enabled = COMFORT_STOP_ENABLED
+    self._gas_suppress_enabled = GAS_SUPPRESS_ENABLED
+    self._since_brake_frames = 10 ** 6                   # frames since last brake output (gas-suppress recency)
+    self._read_params()
+
+  def _read_params(self) -> None:
+    self._enabled = self._params.get_bool("AccelPersonalityEnabled")
+    if not self._enabled:
+      self._personality = NORMAL
+      return
+    self._personality = get_sanitize_int_param("AccelPersonality", PERSONALITY_MIN, PERSONALITY_MAX, self._params)
+
+  def update(self, sm: messaging.SubMaster) -> None:
+    if self._frame % int(1. / DT_MDL) == 0:
+      self._read_params()
+    self._v_ego = sm['carState'].vEgo
+    lead = sm['radarState'].leadOne          # raw radard lead (== what the MPC sees at crawl, where the enforcer acts)
+    self._lead_status = bool(lead.status)
+    self._lead_d = float(lead.dRel)
+    self._lead_vlead = float(lead.vLead)
+    self._frame += 1
+
+  def get_max_accel(self, v_ego: float) -> float:
+    # Disabled -> stock ceiling (off == stock, independent of the NORMAL profile so NORMAL is free to differ).
+    table = A_CRUISE_MAX_V[self._personality] if self._enabled else STOCK_A_CRUISE_MAX_V
+    return float(np.interp(v_ego, A_CRUISE_MAX_BP, table))
+
+  def get_rise_rate(self) -> float:
+    # Disabled -> stock rise rate (off == stock, independent of the NORMAL profile).
+    return RISE_RATE[self._personality] if self._enabled else STOCK_RISE_RATE
+
+  def get_decel_target(self, brake_need: float) -> float:
+    return float(np.interp(max(0.0, float(brake_need)), SMOOTH_DECEL_BP, SMOOTH_DECEL_V[self._personality]))
+
+  def smooth_target_accel(self, raw_target_accel: float, accel_trajectory: Sequence[float], t_idxs: Sequence[float],
+                          should_stop: bool, reset: bool = False, stock_brake: bool = False,
+                          speed_trajectory: Sequence[float] | None = None) -> float:
+    raw = float(raw_target_accel)
+    self._brake_need = self._compute_brake_need(raw, accel_trajectory, t_idxs)
+    self._decel_target = 0.0
+    self._smooth_active = False
+    self._bypassed = False
+
+    out = self._shape(raw, should_stop, reset, speed_trajectory, t_idxs, stock_brake)
+    out = self._comfort_stop(out, reset)   # low-speed monotone comfort decel-to-stop (replaces the self-releasing enforcer)
+    return self._finalize(out)
+
+  def _shape(self, raw: float, should_stop: bool, reset: bool, speed_trajectory, t_idxs, stock_brake: bool) -> float:
+    # --- Full stock passthroughs (output is exactly the plan, no shaping) ---
+    if reset or not self._enabled:
+      return raw                                               # disabled / reset
+    if self._v_ego < STOP_PASSTHROUGH_V and raw <= 0.0:
+      return raw                                               # stop/creep regime: braking is stock (no coast-in)
+    self._bypassed = self._emergency_bypass(raw, should_stop)
+    if self._bypassed or self._stop_imminent(speed_trajectory, t_idxs):
+      return raw                                               # emergency / coming stop: full strength, no delay
+
+    # Anticipatory front-load, capped at OVERBITE_CAP below the live plan (avoids an abrupt over-bite on a
+    # cut-in brake_need spike).
+    target = raw
+    if self._brake_need >= MIN_SMOOTH_BRAKE_NEED:
+      self._smooth_active = True
+      self._decel_target = max(self.get_decel_target(self._brake_need), raw - OVERBITE_CAP)
+      target = min(raw, self._decel_target)
+      if raw > 0.0:
+        target = max(target, 0.0)                              # plan wants throttle -> ease the gas early, never fabricate a brake
+    target = self._suppress_gas_near_lead(target, raw)
+    slewed = self._slew(target)
+    if raw >= 0.0:
+      return slewed
+    if stock_brake:
+      return min(slewed, raw)                                  # blended/e2e: the model owns the brake -> strict never-weaker
+    return self._onset_spread(slewed, raw)                     # non-emergency brake: bounded onset spread (<= ONSET_SPREAD_MAX weaker)
+
+  def _suppress_gas_near_lead(self, target: float, raw: float) -> float:
+    # Coast instead of accelerating toward a close lead: T1 recent brake + lead not pulling away, or T2 clearly
+    # closing. Only reduces accel, never a brake. Off => no-op.
+    if not self._gas_suppress_enabled or raw <= 0.0 or not self._lead_status:
+      return target
+    if not 0.1 < self._lead_d < GAS_SUPPRESS_DREL:
+      return target
+    closing = self._lead_vlead - self._v_ego
+    recent_brake = self._since_brake_frames * DT_MDL < GAS_SUPPRESS_RECENT_T
+    if (recent_brake and closing < GAS_SUPPRESS_VREL) or closing < GAS_SUPPRESS_CLOSE:
+      return min(target, 0.0)
+    return target
+
+  def _onset_spread(self, shaped: float, raw: float) -> float:
+    # Scoped softening: on a NON-emergency brake the onset may arrive spread instead of stepping to the plan.
+    # The output deepens toward the plan jerk-limited at ONSET_SPREAD_JERK and may lag it by at most
+    # ONSET_SPREAD_MAX -- a tightly bounded, transient weaker-than-plan window that smooths the felt onset.
+    # Emergency brakes never reach here (raw passthrough in _shape), so a genuine hard brake is never softened.
+    # The front-load still wins when it is deeper (anticipation preserved).
+    spread = max(raw, self._last_target_accel - ONSET_SPREAD_JERK * DT_MDL)   # deepen toward the plan, jerk-limited
+    spread = min(spread, raw + ONSET_SPREAD_MAX)                              # never more than the bounded lag weaker
+    return min(shaped, spread)
+
+  def _comfort_stop(self, out: float, reset: bool) -> float:
+    # Low-speed ANTI-CREEP HOLD behind a near-stopped lead. In the final-approach window it HOLDS the deepest
+    # decel the PLAN itself commanded this episode (gentle-capped at COMFORT_STOP_MAX_DECEL), so the brake does
+    # not ease off / creep in before the car is stopped (no roll, slightly roomier). It is NEVER firmer than the
+    # plan -- it only stops the brake from WEAKENING -- so it can never add a hard bite (the old kinematic
+    # enforcer demanded a firm ~-1.6 grab; this does not). Outside the window (gap opening as a creeping lead
+    # pulls away / lead moving / launch / standstill) the floor eases out at the release rate. min(out, floor)
+    # keeps it never weaker than the plan. Off => no-op (off==stock).
+    if reset or not self._enabled or not self._comfort_stop_enabled:
+      self._stop_floor = 0.0                                   # disabled/gated/reset: drop the latch, pure passthrough
+      return out
+    final_approach = (self._lead_status and self._lead_vlead < COMFORT_STOP_LEAD_V and self._lead_d > 0.1
+                      and COMFORT_STOP_RELEASE_V <= self._v_ego < COMFORT_STOP_V
+                      and self._lead_d - COMFORT_STOP_GAP <= COMFORT_STOP_HOLD_GAP)
+    if final_approach:
+      plan_hold = max(out, COMFORT_STOP_MAX_DECEL)             # the plan's own decel, gentle-capped (never firmer)
+      self._stop_floor = min(plan_hold, self._stop_floor)      # latch the deepest -> hold through the plan's ease
+    else:
+      # Not final approach (cruise / gap opening / lead moving / launch / standstill): ease the floor toward 0 at
+      # the release rate. Matches _shape's own _slew_up rate, so the floor decays in lockstep with the natural
+      # output -> no launch drag, no release-direction snap, no phantom brake into an opening gap.
+      self._stop_floor = min(0.0, self._stop_floor + BRAKE_RELEASE_JERK * DT_MDL)
+    return min(out, self._stop_floor) if self._stop_floor < 0.0 else out
+
+  def _stop_imminent(self, speed_trajectory: Sequence[float] | None, t_idxs: Sequence[float]) -> bool:
+    # plan predicts a near-stop within the lookahead -> a stop is coming (lead or light/sign).
+    if speed_trajectory is None:
+      return False
+    return any(float(s) < STOP_IMMINENT_VEGO
+               for s, t in zip(speed_trajectory, t_idxs, strict=False) if float(t) <= STOP_IMMINENT_LOOKAHEAD_T)
+
+  def _compute_brake_need(self, raw_target_accel: float, accel_trajectory: Sequence[float], t_idxs: Sequence[float]) -> float:
+    min_accel = float(raw_target_accel)
+    for accel, t in zip(accel_trajectory, t_idxs, strict=False):
+      if float(t) <= SMOOTH_DECEL_LOOKAHEAD_T:
+        min_accel = min(min_accel, float(accel))
+    return max(0.0, -min_accel)
+
+  def _emergency_bypass(self, raw_target_accel: float, should_stop: bool) -> bool:
+    return (self._mpc.crash_cnt > 0 or should_stop or
+            raw_target_accel <= HARD_BRAKE_TARGET_ACCEL or self._brake_need >= HARD_BRAKE_NEED)
+
+  def _slew(self, target_accel: float) -> float:
+    # Jerk-limit the brake DEEPENING (smooths the front-load's extra depth). On the brake side the caller
+    # clamps with min(., raw), so this NEVER delays a real brake -- when the plan is deeper than the slewed
+    # value, min(.) picks the plan and the brake passes through at full rate.
+    target_accel = float(target_accel)
+    if target_accel <= self._last_target_accel:
+      jmax = BRAKE_DEEPENING_JERK[self._personality]
+      return self._clean_accel(max(target_accel, self._last_target_accel - jmax * DT_MDL))
+    return self._slew_up(target_accel)
+
+  def _slew_up(self, target_accel: float) -> float:
+    # Releasing the brake / accelerating: rate-limit the rise (release jerk on the brake side, the
+    # personality accel-rise jerk on the throttle side).
+    if self._last_target_accel < 0.0:
+      released = min(target_accel, self._last_target_accel + BRAKE_RELEASE_JERK * DT_MDL)
+      if released <= 0.0:
+        return self._clean_accel(released)
+      return self._clean_accel(min(target_accel, ACCEL_RISE_JERK[self._personality] * DT_MDL))
+    step = ACCEL_RISE_JERK[self._personality] * DT_MDL
+    return self._clean_accel(min(target_accel, self._last_target_accel + step))
+
+  def _finalize(self, target_accel: float) -> float:
+    target_accel = self._clean_accel(target_accel)
+    self._last_target_accel = target_accel
+    self._since_brake_frames = 0 if target_accel < GAS_SUPPRESS_BRAKE_THR else self._since_brake_frames + 1
+    return target_accel
+
+  @staticmethod
+  def _clean_accel(accel: float) -> float:
+    accel = float(accel)
+    return 0.0 if abs(accel) < _ZERO_ACCEL_EPS else accel
+
+  def enabled(self) -> bool:
+    return self._enabled
+
+  def personality(self):
+    return self._personality
+
+  def max_accel(self) -> float:
+    return self.get_max_accel(self._v_ego)
+
+  def brake_need(self) -> float:
+    return self._brake_need
+
+  def decel_target(self) -> float:
+    return self._decel_target
+
+  def smooth_active(self) -> bool:
+    return self._smooth_active
+
+  def bypassed(self) -> bool:
+    return self._bypassed
+
+  def comfort_stop_floor(self) -> float:
+    return self._stop_floor
+
+  def comfort_stop_active(self) -> bool:
+    return self._stop_floor < 0.0

sunnypilot/selfdrive/controls/lib/accel_personality/constants.py

@@ -0,0 +1,99 @@
+"""
+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 cereal import custom
+
+AccelerationPersonality = custom.LongitudinalPlanSP.AccelerationPersonality
+ECO = AccelerationPersonality.eco
+NORMAL = AccelerationPersonality.normal
+SPORT = AccelerationPersonality.sport
+
+PERSONALITY_MIN = min(AccelerationPersonality.schema.enumerants.values())
+PERSONALITY_MAX = max(AccelerationPersonality.schema.enumerants.values())
+
+# Positive-accel ceiling + its upward slew rate (launch/cruise side; independent of braking). off==stock is
+# enforced in accel_controller (falls back to STOCK_* when disabled), so the tiers are free to differ.
+A_CRUISE_MAX_BP = [0., 14., 25., 40.]
+STOCK_A_CRUISE_MAX_V = [1.6, 0.7, 0.2, 0.08]
+STOCK_RISE_RATE = 0.05
+A_CRUISE_MAX_V = {
+  ECO:    [1.70, 0.75, 0.25, 0.10],   # prompt launch, efficient cruise
+  NORMAL: [2.10, 1.10, 0.50, 0.18],   # quick launch, balanced cruise
+  SPORT:  [2.60, 1.55, 0.85, 0.35],   # fast launch, strong cruise
+}
+RISE_RATE = {ECO: 0.10, NORMAL: 0.15, SPORT: 0.22}   # ceiling open-rate: all >> stock 0.05 for fast take-off
+
+# Anticipatory front-load: predicted brake need (m/s^2) -> early decel target (m/s^2). Starts a gentle
+# decel early when a brake is predicted, so it arrives spread out, not as one late firm onset. The first
+# knot sits AT the MIN_SMOOTH_BRAKE_NEED gate (0.00 there): below the gate there is no front-load, so there
+# is no dead [0, gate) anchor and no step at the gate (the old [0.0 -> 0.00] knot was never evaluated).
+SMOOTH_DECEL_BP = [0.4, 0.8, 1.2, 1.6, 2.0, 2.4]
+SMOOTH_DECEL_V = {
+  ECO:    [0.00, -0.20, -0.35, -0.55, -0.78, -1.00],
+  NORMAL: [0.00, -0.30, -0.55, -0.84, -1.12, -1.40],
+  SPORT:  [0.00, -0.40, -0.72, -1.05, -1.35, -1.65],
+}
+BRAKE_DEEPENING_JERK = {ECO: 0.5, NORMAL: 0.8, SPORT: 1.0}
+BRAKE_RELEASE_JERK = 2.0
+ACCEL_RISE_JERK = {ECO: 1.0, NORMAL: 1.5, SPORT: 2.2}   # accel-onset jerk: higher = snappier take-off, stepped per tier
+
+SMOOTH_DECEL_LOOKAHEAD_T = 3.0
+MIN_SMOOTH_BRAKE_NEED = 0.4   # below this no front-load (kills the faint low-brake_need drag + the gate-crossing toggle)
+
+# Cap how much DEEPER than the live plan the front-load may bite -> no abrupt over-bite on a cut-in
+# brake_need spike (binds only when the plan still wants throttle; once it brakes, the table wins).
+OVERBITE_CAP = 0.30   # m/s^2 max front-load depth below the live plan
+
+# Hard brake: at/below this accel, or this predicted brake_need within the lookahead, the controller hands
+# the plan straight through at full strength and rate (no front-load, no rate limit) -- a firm/closing-lead
+# brake must never be delayed, softened or rate-limited.
+HARD_BRAKE_TARGET_ACCEL = -1.5
+HARD_BRAKE_NEED = 2.6
+
+# Stop-imminent stand-down. When the plan predicts a near-stop within the lookahead, hand the plan straight
+# through (stock decel) so the car stops at the proper gap with no front-load coast-in. Keyed on the
+# PREDICTED speed reaching ~0 (covers lead AND light/sign stops), not raw ego speed.
+STOP_IMMINENT_VEGO = 1.0          # m/s  plan-predicted speed below this within the lookahead == stop coming
+STOP_IMMINENT_LOOKAHEAD_T = 3.0   # s
+
+# Below this ego speed the brake side is stock passthrough (the comfort stop below adds the only low-speed
+# shaping); the bounded onset-spread does not run here, so a stock stop is not rate-limited.
+STOP_PASSTHROUGH_V = 5.0          # m/s
+
+# Scoped onset-spread -- the ONLY place the output may be transiently WEAKER than the plan. On a NON-emergency
+# brake the onset may arrive spread over a bounded ramp instead of stepping straight to the plan: the output
+# may lag the plan by at most ONSET_SPREAD_MAX, deepening toward it at ONSET_SPREAD_JERK. A firm/closing brake
+# (raw <= HARD_BRAKE_TARGET_ACCEL or brake_need >= HARD_BRAKE_NEED, FCW/crash, should_stop, blended/e2e) skips
+# this entirely (raw passthrough), so a real hard brake is never softened or delayed.
+ONSET_SPREAD_MAX = 0.25           # m/s^2: max the output may lag (be weaker than) the live plan, non-emergency only
+ONSET_SPREAD_JERK = 2.5           # m/s^3: rate the spread output deepens back toward the plan
+
+# Low-speed comfort stop = ANTI-CREEP HOLD (not a brake adder). In the final approach behind a (near-)stopped
+# lead it HOLDS the deepest decel the PLAN itself has commanded (gentle-capped), so the brake does not ease
+# off / creep in before the car is stopped (no roll, slightly roomier). It is NEVER firmer than the plan, so
+# it can never add a hard bite -- the stop stays as gentle as the plan's own decel. Outside the final approach
+# (cruising / gap opening as a creeping lead pulls away / lead moving / launch) the floor eases out at the
+# release rate. min(plan, floor) keeps it never weaker than the plan. Replaces the old kinematic v^2/(2*gap)
+# enforcer, which engaged late and demanded a firm ~-1.6 grab to hit a fixed gap. Off => no-op.
+COMFORT_STOP_ENABLED = False      # off: keeps the stock smooth taper (flat-hold firms the end). Farther-stop is via the MPC stop-target shift, not this.
+COMFORT_STOP_V = 4.0              # m/s: only engage at/below this ego speed
+COMFORT_STOP_LEAD_V = 1.0         # m/s: only behind a (near-)stopped lead
+COMFORT_STOP_GAP = 5.0            # m: reference standstill gap (radar dRel) for the final-approach window
+COMFORT_STOP_MAX_DECEL = -1.6     # m/s^2: backstop cap on the held decel (a brief plan spike is not held firmer than this)
+COMFORT_STOP_RELEASE_V = 0.3      # m/s: below this, ease the floor out (release rate) -> smooth stock standstill handoff
+COMFORT_STOP_HOLD_GAP = 2.0       # m: within this of the reference gap = final-approach window where the hold applies;
+                                  # beyond it the floor eases out (a creeping lead opening the gap -> no phantom brake)
+
+# Gas suppression near a lead: coast instead of accelerating toward a close lead, in two cases (OR) --
+# T1 we braked for it within RECENT_T and it is still not pulling away (closing < VREL); T2 we are clearly
+# gaining on it (closing < CLOSE). Only reduces accel, never a brake; opening/far lead keeps its gas.
+GAS_SUPPRESS_ENABLED = False
+GAS_SUPPRESS_DREL = 60.0          # m: lead within this distance
+GAS_SUPPRESS_VREL = 0.5           # m/s: "not pulling away" bound for the rebound trigger (vLead - vEgo)
+GAS_SUPPRESS_CLOSE = -1.5         # m/s: closing rate below which gas is suppressed outright
+GAS_SUPPRESS_RECENT_T = 3.0       # s: a brake within this long counts as recent
+GAS_SUPPRESS_BRAKE_THR = -0.30    # m/s^2: output below this is a "brake" for the recency latch

sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py

@@ -0,0 +1,422 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+
+from types import SimpleNamespace
+
+import numpy as np
+import pytest
+
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_controller import AccelController
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import \
+  ECO, NORMAL, SPORT, PERSONALITY_MIN, PERSONALITY_MAX, A_CRUISE_MAX_BP, RISE_RATE, \
+  STOCK_A_CRUISE_MAX_V, STOCK_RISE_RATE, HARD_BRAKE_TARGET_ACCEL, OVERBITE_CAP, \
+  STOP_PASSTHROUGH_V, ONSET_SPREAD_MAX, AccelerationPersonality
+
+T_IDXS = [0.0, 0.2, 0.4, 0.6, 0.8, 1.0, 1.25, 1.5, 1.75, 2.0, 2.5, 3.0, 4.0]
+_EPS = 1e-6
+
+
+class FakeParams:
+  def __init__(self, store=None):
+    self.store = dict(store or {})
+
+  def get_bool(self, key):
+    return bool(self.store.get(key, False))
+
+  def get(self, key, return_default=False):
+    return int(self.store.get(key, 1))
+
+  def put(self, key, val, block=False):
+    self.store[key] = val
+
+
+def make_sm(v_ego=20.0, lead_status=False, lead_d=0.0, lead_vlead=0.0):
+  lead = SimpleNamespace(status=lead_status, dRel=lead_d, vLead=lead_vlead)
+  return {'carState': SimpleNamespace(vEgo=v_ego), 'radarState': SimpleNamespace(leadOne=lead)}
+
+
+def make_controller(enabled=True, personality=NORMAL, crash_cnt=0, comfort_stop=False, gas_suppress=False):
+  store = {"AccelPersonalityEnabled": enabled, "AccelPersonality": int(personality)}
+  ctrl = AccelController(CP=SimpleNamespace(), mpc=SimpleNamespace(crash_cnt=crash_cnt), params=FakeParams(store))
+  ctrl._comfort_stop_enabled = comfort_stop   # comfort_stop is gated off in production; opt in per-test
+  ctrl._gas_suppress_enabled = gas_suppress   # gas-suppression is gated off in production; opt in per-test
+  ctrl.update(make_sm())
+  return ctrl
+
+
+def flat_traj(value):
+  return [float(value)] * len(T_IDXS)
+
+
+# --- Profiles / off==stock ---------------------------------------------------
+
+def test_enum_source_parity():
+  assert (ECO, NORMAL, SPORT) == (AccelerationPersonality.eco, AccelerationPersonality.normal, AccelerationPersonality.sport)
+  assert (PERSONALITY_MIN, PERSONALITY_MAX) == (0, 2)
+
+
+def test_disabled_forces_normal_and_stock_ceiling():
+  ctrl = make_controller(enabled=False, personality=SPORT)
+  assert ctrl.personality() == NORMAL
+  assert not ctrl.enabled()
+  for v in (0.0, 10.0, 25.0, 40.0):
+    assert ctrl.get_max_accel(v) == pytest.approx(np.interp(v, A_CRUISE_MAX_BP, STOCK_A_CRUISE_MAX_V))
+  assert ctrl.get_rise_rate() == STOCK_RISE_RATE
+
+
+def test_disabled_passes_brake_through():
+  ctrl = make_controller(enabled=False)
+  for raw in (-3.0, -1.5, -0.5, 0.0, 1.0):
+    out = ctrl.smooth_target_accel(raw, flat_traj(raw), T_IDXS, should_stop=False)
+    assert out == pytest.approx(raw, abs=_EPS)
+
+
+def test_normal_is_distinct_from_stock():
+  # off==stock is enforced via the disabled path, NOT by NORMAL==stock, so enabled NORMAL is free to differ.
+  ctrl = make_controller(personality=NORMAL)
+  assert ctrl.get_max_accel(0.0) != pytest.approx(np.interp(0.0, A_CRUISE_MAX_BP, STOCK_A_CRUISE_MAX_V))
+  assert ctrl.get_rise_rate() != STOCK_RISE_RATE
+
+
+def test_ceiling_ordering_eco_lt_normal_lt_sport():
+  eco, normal, sport = (make_controller(personality=p) for p in (ECO, NORMAL, SPORT))
+  for v in (0.0, 14.0, 25.0, 40.0):
+    assert eco.get_max_accel(v) < normal.get_max_accel(v) < sport.get_max_accel(v)
+  assert eco.get_rise_rate() < normal.get_rise_rate() < sport.get_rise_rate()
+
+
+def test_rise_rate_ordering():
+  assert RISE_RATE[ECO] < RISE_RATE[NORMAL] < RISE_RATE[SPORT]
+
+
+# --- SAFETY: never weaker than the plan, hard brakes never delayed --------------
+
+@pytest.mark.parametrize("personality", [ECO, NORMAL, SPORT])
+def test_never_weaker_than_plan_sustained(personality):
+  # Safety: an EMERGENCY brake is never weaker than the plan (strict). A non-emergency brake may lag the plan
+  # by at most ONSET_SPREAD_MAX (the bounded onset-spread) and no more.
+  ctrl = make_controller(personality=personality)
+  for raw in [0.0, -0.2, -0.5, -0.9, -1.2, -1.5, -2.0] + [-2.0] * 20:
+    out = ctrl.smooth_target_accel(raw, flat_traj(raw), T_IDXS, should_stop=False)
+    if raw <= HARD_BRAKE_TARGET_ACCEL:
+      assert out <= raw + _EPS                                 # emergency: strict never-weaker
+    elif raw < 0.0:
+      assert out <= raw + ONSET_SPREAD_MAX + _EPS              # non-emergency: bounded onset-spread only
+
+
+@pytest.mark.parametrize("personality", [ECO, NORMAL, SPORT])
+def test_never_weaker_random_walk(personality):
+  rng = np.random.default_rng(0)
+  ctrl = make_controller(personality=personality)
+  for _ in range(500):
+    raw = float(rng.uniform(-2.5, 1.5))
+    traj = flat_traj(raw - float(rng.uniform(0.0, 0.6)))
+    out = ctrl.smooth_target_accel(raw, traj, T_IDXS, should_stop=False)
+    if raw < 0.0:
+      assert out <= raw + ONSET_SPREAD_MAX + _EPS              # never more than the bounded onset-spread weaker
+
+
+@pytest.mark.parametrize("personality", [ECO, NORMAL, SPORT])
+def test_hard_brake_passes_through_immediately(personality):
+  # Regression for route 00000466 near-crash: a sudden hard brake (plan steps deep) must reach FULL depth
+  # on the FIRST frame -- never rate-limited / delayed, or the car under-brakes into a closing lead.
+  ctrl = make_controller(personality=personality)
+  out = ctrl.smooth_target_accel(-3.5, flat_traj(-3.5), T_IDXS, should_stop=False)
+  assert out == pytest.approx(-3.5, abs=_EPS)
+  assert ctrl.bypassed()
+
+
+def test_sudden_lead_no_brake_delay():
+  # The exact 466 shape: cruising (plan +1.7, no brake) then a fast lead appears and the plan steps to max
+  # brake. The commanded brake must hit full depth immediately, not ramp in over time.
+  ctrl = make_controller(personality=ECO)
+  for _ in range(5):
+    ctrl.smooth_target_accel(1.7, flat_traj(1.7), T_IDXS, should_stop=False)   # cruising, no lead
+  out = ctrl.smooth_target_accel(-3.5, flat_traj(-3.5), T_IDXS, should_stop=False)  # lead appears
+  assert out == pytest.approx(-3.5, abs=_EPS)                                  # full brake, zero delay
+
+
+def test_should_stop_passes_through():
+  ctrl = make_controller(personality=ECO)
+  out = ctrl.smooth_target_accel(-1.0, flat_traj(-1.0), T_IDXS, should_stop=True)
+  assert out == pytest.approx(-1.0, abs=_EPS)
+  assert ctrl.bypassed()
+
+
+def test_fcw_crash_passes_through():
+  ctrl = make_controller(personality=ECO, crash_cnt=3)
+  out = ctrl.smooth_target_accel(-1.0, flat_traj(-1.0), T_IDXS, should_stop=False)
+  assert out == pytest.approx(-1.0, abs=_EPS)
+  assert ctrl.bypassed()
+
+
+def test_blended_never_weaker():
+  # Blended/e2e (stock_brake): never weaker than the plan (may anticipate via the never-weaker front-load).
+  ctrl = make_controller(personality=ECO)
+  for raw in [0.0, -0.3, -0.6, -0.9, -1.0, -1.0, -1.0]:
+    out = ctrl.smooth_target_accel(raw, flat_traj(raw), T_IDXS, should_stop=False, stock_brake=True)
+    assert out <= raw + _EPS
+
+
+# --- Anticipatory front-load (never weaker, capped) ------------------------------
+
+def test_front_load_brakes_before_plan():
+  # A deeper brake is predicted ahead (brake_need=1.0) while the live plan is still flat -> front-load
+  # brakes early (output goes negative), but the smooth branch keeps it never weaker than the plan.
+  ctrl = make_controller(personality=ECO)
+  out = ctrl.smooth_target_accel(0.0, flat_traj(-1.0), T_IDXS, should_stop=False)
+  assert out < 0.0
+  assert ctrl.smooth_active()
+  assert ctrl.brake_need() == pytest.approx(1.0)
+
+
+def test_front_load_anticipates_below_live_plan():
+  # When the live plan is gently braking and a deeper brake is predicted, the front-load deepens below the
+  # live plan (anticipatory early brake), settling within OVERBITE_CAP of it.
+  ctrl = make_controller(personality=ECO)
+  out = 0.0
+  for _ in range(20):
+    out = ctrl.smooth_target_accel(-0.2, flat_traj(-1.5), T_IDXS, should_stop=False)
+  assert out < -0.2 - _EPS                                   # deeper than the live -0.2 plan
+  assert out >= -0.2 - OVERBITE_CAP - _EPS                   # but never more than the cap below it
+
+
+def test_overbite_cap_limits_frontload_vs_live_plan():
+  # Cut-in/merge: plan still wants throttle (+0.5) while a deep brake is predicted -> front-load may not
+  # settle more than OVERBITE_CAP below the live plan (no abrupt early over-bite).
+  ctrl = make_controller(personality=ECO)
+  traj = [0.5, 0.3, 0.0, -0.5, -1.5, -2.0] + [-2.0] * (len(T_IDXS) - 6)
+  out = 0.0
+  for _ in range(10):
+    out = ctrl.smooth_target_accel(0.5, traj, T_IDXS, should_stop=False)
+  assert ctrl.smooth_active()
+  assert out == pytest.approx(0.5 - OVERBITE_CAP, abs=1e-3)
+
+
+# --- Stop / low-speed neutrality -------------------------------------------------
+
+def test_low_speed_brake_is_stock_passthrough():
+  # Stop/creep regime (vEgo < STOP_PASSTHROUGH_V): braking is stock so the stop distance matches OFF.
+  ctrl = make_controller(personality=ECO)
+  ctrl.update(make_sm(v_ego=STOP_PASSTHROUGH_V - 0.1))
+  for raw in (-0.3, -1.0):
+    out = ctrl.smooth_target_accel(raw, flat_traj(-1.5), T_IDXS, should_stop=False)
+    assert out == pytest.approx(raw, abs=_EPS)
+    assert not ctrl.smooth_active()
+
+
+def test_low_speed_launch_still_shapes():
+  # The low-speed brake passthrough must NOT neutralize positive-accel (launch) shaping.
+  ctrl = make_controller(personality=ECO)
+  ctrl.update(make_sm(v_ego=STOP_PASSTHROUGH_V - 0.1))
+  ctrl.smooth_target_accel(0.0, flat_traj(0.0), T_IDXS, should_stop=False)
+  out = ctrl.smooth_target_accel(1.5, flat_traj(1.5), T_IDXS, should_stop=False)
+  assert out < 1.5                                           # rise-rate limited (shaped)
+
+
+def test_stop_imminent_passthrough_but_moving_follow_shapes():
+  # Stop coming (plan speed -> ~0): stock passthrough (no coast-in). Slowing to a moving follow: front-load
+  # stays active so the early-brake goal is preserved.
+  ctrl = make_controller(personality=ECO)
+  stopping = [3.0, 2.0, 1.0, 0.4, 0.0] + [0.0] * (len(T_IDXS) - 5)
+  out = ctrl.smooth_target_accel(-0.1, flat_traj(-1.0), T_IDXS, should_stop=False, speed_trajectory=stopping)
+  assert not ctrl.smooth_active()
+  assert out == pytest.approx(-0.1, abs=_EPS)
+  moving = [8.0] * len(T_IDXS)
+  ctrl.smooth_target_accel(-0.1, flat_traj(-1.0), T_IDXS, should_stop=False, speed_trajectory=moving)
+  assert ctrl.smooth_active()
+
+
+def test_comfort_stop_holds_through_plan_ease():
+  # Plan brakes to a peak then eases off near the stop (the stock creep). The hold keeps the deeper decel so
+  # the brake does not ease in (no roll) -- but NEVER firmer than the plan's own peak (no added hard bite).
+  ctrl = make_controller(personality=ECO, comfort_stop=True)
+  out = 0.0
+  for plan in [-0.4, -0.8, -1.1, -1.1, -0.6, -0.3, -0.1]:   # decel to a -1.1 peak, then ease (creep)
+    ctrl.update(make_sm(v_ego=2.0, lead_status=True, lead_d=6.0, lead_vlead=0.0))
+    out = ctrl.smooth_target_accel(plan, flat_traj(plan), T_IDXS, should_stop=False)
+  assert out < -0.3 - _EPS                              # held deeper than the easing plan (-0.1) -> no creep-in
+  assert out >= -1.1 - _EPS                             # but never firmer than the plan's own peak (no -1.6 bite)
+
+
+def test_comfort_stop_never_firmer_than_plan():
+  # The hold can only stop the brake from WEAKENING; it never commands a decel firmer than the plan itself.
+  ctrl = make_controller(personality=ECO, comfort_stop=True)
+  for plan in [-0.2, -0.5, -0.9, -0.9, -0.9]:           # steady (no ease) -> hold matches plan, adds nothing
+    ctrl.update(make_sm(v_ego=2.0, lead_status=True, lead_d=6.0, lead_vlead=0.0))
+    out = ctrl.smooth_target_accel(plan, flat_traj(plan), T_IDXS, should_stop=False)
+    assert out == pytest.approx(plan, abs=_EPS)         # never firmer than the (non-easing) plan -> no bite/grab
+
+
+def test_comfort_stop_monotone_no_early_release():
+  # While still moving, the comfort floor never WEAKENS frame-to-frame (the old enforcer self-released -> roll).
+  ctrl = make_controller(personality=ECO, comfort_stop=True)
+  floors = []
+  for v in [3.0, 2.6, 2.2, 1.8, 1.4, 1.0, 0.6]:         # decelerating toward the lead
+    ctrl.update(make_sm(v_ego=v, lead_status=True, lead_d=max(0.5, 7.0 - (3.0 - v) * 2), lead_vlead=0.0))
+    ctrl.smooth_target_accel(-0.5, flat_traj(-0.5), T_IDXS, should_stop=False)
+    floors.append(ctrl._stop_floor)
+  for a, b in zip(floors, floors[1:], strict=False):
+    assert b <= a + _EPS                                # monotone non-weakening while approaching
+
+
+def test_comfort_stop_off_when_disabled():
+  ctrl = make_controller(enabled=False, personality=ECO)
+  ctrl.update(make_sm(v_ego=2.0, lead_status=True, lead_d=4.0, lead_vlead=0.0))
+  out = ctrl.smooth_target_accel(-0.1, flat_traj(-0.1), T_IDXS, should_stop=False)
+  assert out == pytest.approx(-0.1, abs=_EPS)
+
+
+def test_comfort_stop_gated_off_is_stock_passthrough():
+  # Production default (COMFORT_STOP_ENABLED off, even with AccelController enabled): the final approach is stock
+  # passthrough -- output follows the easing plan, no anti-creep hold, floor stays 0 (goal 6 met by stock).
+  ctrl = make_controller(personality=ECO)                      # comfort_stop defaults False (production)
+  out = 0.0
+  for plan in [-0.4, -0.8, -1.1, -0.6, -0.1]:                  # decel to a peak then ease (stock creep)
+    ctrl.update(make_sm(v_ego=2.0, lead_status=True, lead_d=6.0, lead_vlead=0.0))
+    out = ctrl.smooth_target_accel(plan, flat_traj(plan), T_IDXS, should_stop=False)
+  assert out == pytest.approx(-0.1, abs=_EPS)                  # follows the easing plan -> no hold
+  assert ctrl._stop_floor == 0.0                               # never latched
+
+
+def test_comfort_stop_no_op_moving_lead():
+  # Moving lead (vLead high): no comfort stop (only behind a near-stopped lead).
+  ctrl = make_controller(personality=ECO, comfort_stop=True)
+  ctrl.update(make_sm(v_ego=2.0, lead_status=True, lead_d=6.0, lead_vlead=5.0))
+  out = ctrl.smooth_target_accel(-0.1, flat_traj(-0.1), T_IDXS, should_stop=False)
+  assert out == pytest.approx(-0.1, abs=_EPS)
+
+
+def test_comfort_stop_never_weaker():
+  # The comfort floor only ever ADDS braking: output never weaker than the plan.
+  ctrl = make_controller(personality=ECO, comfort_stop=True)
+  for raw in (-0.05, -0.3, -1.0, -2.5):
+    ctrl.update(make_sm(v_ego=2.0, lead_status=True, lead_d=5.5, lead_vlead=0.0))
+    out = ctrl.smooth_target_accel(raw, flat_traj(raw), T_IDXS, should_stop=False)
+    assert out <= raw + _EPS
+
+
+def test_comfort_stop_weakens_when_gap_opens():
+  # Creeping stop-and-go lead (vLead stays < COMFORT_STOP_LEAD_V) that pulls away: once the gap opens well past
+  # the target the floor must WEAKEN, not hold a phantom brake into an opening gap.
+  ctrl = make_controller(personality=ECO, comfort_stop=True)
+  for _ in range(15):                                          # approach close -> deep floor (final-approach hold)
+    ctrl.update(make_sm(v_ego=2.0, lead_status=True, lead_d=5.5, lead_vlead=0.3))
+    ctrl.smooth_target_accel(-0.5, flat_traj(-0.5), T_IDXS, should_stop=False)
+  deep = ctrl._stop_floor
+  assert deep < -0.3
+  for _ in range(25):                                          # lead creeps away (still vLead<1): gap opens wide
+    ctrl.update(make_sm(v_ego=2.0, lead_status=True, lead_d=12.0, lead_vlead=0.5))
+    ctrl.smooth_target_accel(-0.05, flat_traj(-0.05), T_IDXS, should_stop=False)
+  assert ctrl._stop_floor > deep + 0.3                         # floor weakened as the gap opened (no phantom brake)
+
+
+def test_comfort_stop_releases_on_launch():
+  # Stop-and-go GO: after holding a comfort floor at a stop, once the lead moves and the plan wants throttle the
+  # floor must release (track the plan up) and not hold the output below the natural plan -> the car launches.
+  ctrl = make_controller(personality=ECO, comfort_stop=True)
+  for _ in range(20):                                          # hold the plan's -1.0 decel approaching a stopped lead
+    ctrl.update(make_sm(v_ego=1.5, lead_status=True, lead_d=6.0, lead_vlead=0.0))
+    ctrl.smooth_target_accel(-1.0, flat_traj(-1.0), T_IDXS, should_stop=False)
+  assert ctrl._stop_floor < -0.5                               # floor holds the plan's decel (engaged/deep)
+  out = 0.0
+  for _ in range(30):                                          # lead launches, plan wants throttle
+    ctrl.update(make_sm(v_ego=2.0, lead_status=True, lead_d=8.0, lead_vlead=4.0))
+    out = ctrl.smooth_target_accel(0.8, flat_traj(0.8), T_IDXS, should_stop=False)
+  assert out > 0.0                                             # launches (floor did not hold it back)
+  assert ctrl._stop_floor == 0.0                               # floor fully released
+
+
+# --- gas suppression near a non-opening lead ---------------------------------
+
+def _gas(ctrl, v_ego, lead_d, lead_vlead, raw=0.4):
+  ctrl.update(make_sm(v_ego=v_ego, lead_status=True, lead_d=lead_d, lead_vlead=lead_vlead))
+  return ctrl.smooth_target_accel(raw, flat_traj(raw), T_IDXS, should_stop=False)
+
+def _brake_then(ctrl, v_ego, lead_d, lead_vlead, raw):
+  for _ in range(3):                                           # brake for the lead -> set the recency latch
+    ctrl.update(make_sm(v_ego=v_ego, lead_status=True, lead_d=lead_d, lead_vlead=lead_vlead))
+    ctrl.smooth_target_accel(-0.5, flat_traj(-0.5), T_IDXS, should_stop=False)
+  ctrl.update(make_sm(v_ego=v_ego, lead_status=True, lead_d=lead_d, lead_vlead=lead_vlead))
+  return ctrl.smooth_target_accel(raw, flat_traj(raw), T_IDXS, should_stop=False)
+
+def test_gas_suppress_T2_closing_lead_coasts():
+  # T2: clearly gaining on the lead (closing -2.5) -> suppress outright (the 0480 t448 case).
+  ctrl = make_controller(gas_suppress=True)
+  out = _gas(ctrl, v_ego=23.0, lead_d=54.0, lead_vlead=20.5)
+  assert out <= _EPS                                           # coast, never a fabricated brake
+  assert out >= -0.5 - _EPS
+
+def test_gas_suppress_T1_rebound_after_brake():
+  # T1: braked for a matched lead, then plan wants gas again within RECENT_T -> suppress the rebound (t1302 case).
+  ctrl = make_controller(gas_suppress=True)
+  out = _brake_then(ctrl, v_ego=20.0, lead_d=50.0, lead_vlead=20.0, raw=0.3)
+  assert out <= _EPS
+
+def test_gas_suppress_allows_matched_lead_without_recent_brake():
+  # The deliberate narrowing: a steady matched lead we did NOT just brake for keeps its gas (no normal-following drag).
+  ctrl = make_controller(gas_suppress=True)
+  out = _gas(ctrl, v_ego=20.0, lead_d=50.0, lead_vlead=20.0)   # closing 0, no recent brake
+  assert out > 0.0
+
+def test_gas_suppress_allows_gas_when_lead_opening():
+  ctrl = make_controller(gas_suppress=True)
+  out = _gas(ctrl, v_ego=20.0, lead_d=50.0, lead_vlead=24.0)   # lead pulling away +4 -> keep up (neither trigger)
+  assert out > 0.0
+
+def test_gas_suppress_allows_gas_when_lead_far():
+  ctrl = make_controller(gas_suppress=True)
+  out = _gas(ctrl, v_ego=23.0, lead_d=80.0, lead_vlead=20.5)   # closing -2.5 but beyond GAS_SUPPRESS_DREL -> allow
+  assert out > 0.0
+
+def test_gas_suppress_off_by_default_is_stock_gas():
+  ctrl = make_controller()                                     # gas_suppress defaults False (production)
+  out = _gas(ctrl, v_ego=23.0, lead_d=54.0, lead_vlead=20.5)
+  assert out > 0.0                                             # stock gas passes through
+
+def test_gas_suppress_does_not_touch_brake():
+  ctrl = make_controller(gas_suppress=True)
+  out = _gas(ctrl, v_ego=23.0, lead_d=54.0, lead_vlead=20.5, raw=-0.5)   # plan brakes
+  assert out <= 0.0                                            # suppression no-ops on a brake (never weakens it)
+
+
+def test_onset_spread_bounded_and_skipped_for_emergency():
+  # Non-emergency brake onset is spread (lagged) but never by more than ONSET_SPREAD_MAX; an emergency brake
+  # is instant full depth (no spread).
+  ctrl = make_controller(personality=ECO)
+  for _ in range(3):
+    ctrl.smooth_target_accel(0.0, flat_traj(0.0), T_IDXS, should_stop=False)
+  out = ctrl.smooth_target_accel(-1.0, flat_traj(-1.0), T_IDXS, should_stop=False)   # non-emergency step
+  assert out > -1.0 + _EPS                              # lagged (spread), not an instant step
+  assert out <= -1.0 + ONSET_SPREAD_MAX + _EPS          # but bounded
+  ctrl2 = make_controller(personality=ECO)
+  for _ in range(3):
+    ctrl2.smooth_target_accel(0.0, flat_traj(0.0), T_IDXS, should_stop=False)
+  out2 = ctrl2.smooth_target_accel(-2.0, flat_traj(-2.0), T_IDXS, should_stop=False)  # emergency (<= -1.5)
+  assert out2 == pytest.approx(-2.0, abs=_EPS)
+
+
+def test_disabled_hard_brake_is_instant_stock():
+  ctrl = make_controller(enabled=False, personality=ECO)
+  out = ctrl.smooth_target_accel(-3.0, flat_traj(-3.0), T_IDXS, should_stop=False)
+  assert out == pytest.approx(-3.0, abs=_EPS)
+
+
+# --- Misc ------------------------------------------------------------------------
+
+def test_out_of_range_personality_clamps():
+  ctrl = AccelController(CP=SimpleNamespace(), mpc=SimpleNamespace(crash_cnt=0),
+                         params=FakeParams({"AccelPersonalityEnabled": True, "AccelPersonality": 99}))
+  ctrl.update(make_sm())
+  assert ctrl.personality() == PERSONALITY_MAX
+
+
+def test_reset_passes_through():
+  ctrl = make_controller(personality=ECO)
+  out = ctrl.smooth_target_accel(0.0, flat_traj(-1.0), T_IDXS, should_stop=False, reset=True)
+  assert out == pytest.approx(0.0, abs=_EPS)
+  assert not ctrl.bypassed()

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

@@ -1,17 +1,46 @@
+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_ACC_PROB = 0.5
+  RADAR_LEAD_ACC_EXIT_PROB = 0.4
+  RADAR_LEAD_ACC_RISE_RATE = 1.0
+  RADAR_LEAD_ACC_FALL_RATE = 0.25
+  RADAR_LEAD_ACC_MAX_DREL = 80.0
+  RADAR_LEAD_ACC_MAX_TTC = 6.0
+  RADAR_LEAD_ACC_MIN_CLOSING_SPEED = -0.5
+
+  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
+  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

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

@@ -6,129 +6,116 @@ See the LICENSE.md file in the root directory for more details.
 """
 # Version = 2025-6-30
 
-from cereal import messaging
-from opendbc.car import structs
-from numpy import interp
-from openpilot.common.params import Params
-from openpilot.common.realtime import DT_MDL
-from openpilot.sunnypilot.selfdrive.controls.lib.dec.constants import WMACConstants
 from typing import Literal
 
-# d-e2e, from modeldata.h
-TRAJECTORY_SIZE = 33
-SET_MODE_TIMEOUT = 15
+from cereal import messaging
+from numpy import interp
+from opendbc.car import structs
+from openpilot.common.params import Params
+from openpilot.sunnypilot.selfdrive.controls.lib.dec.constants import WMACConstants
 
-# 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)
+      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 +129,33 @@ 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._radar_acc_lead_tracker = HysteresisSignal(
+      enter_threshold=WMACConstants.RADAR_LEAD_ACC_PROB,
+      exit_threshold=WMACConstants.RADAR_LEAD_ACC_EXIT_PROB,
+      rise_rate=WMACConstants.RADAR_LEAD_ACC_RISE_RATE,
+      fall_rate=WMACConstants.RADAR_LEAD_ACC_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_radar_acc_lead = False
     self._has_slow_down = False
     self._has_slowness = False
     self._has_mpc_fcw = False
@@ -179,13 +164,14 @@ 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
 
   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,7 +184,6 @@ 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:
@@ -210,179 +195,109 @@ class DynamicExperimentalController:
     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.status))
-    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._has_lead_filtered = self._lead_tracker.update(float(lead_one.status))
+    self._has_radar_acc_lead = self._radar_acc_lead_tracker.update(self._radar_acc_lead_score(lead_one))
+    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
+    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
-      return
+    self._raw_urgency = clip01(urgency)
+    self._has_slow_down = self._slow_down_tracker.update(self._raw_urgency)
+    self._urgency = self._slow_down_tracker.value
 
-    # We have a valid full trajectory
-    self._trajectory_valid = True
+  def _radar_acc_lead_score(self, lead_one) -> float:
+    if not lead_one.status:
+      return 0.0
 
-    # Use the exact endpoint (33rd point, index 32)
-    endpoint_x = md.position.x[TRAJECTORY_SIZE - 1]
-    self._endpoint_x = endpoint_x
+    d_rel = float(getattr(lead_one, 'dRel', float('inf')))
+    v_rel = float(getattr(lead_one, 'vRel', 0.0))
+    if d_rel <= WMACConstants.RADAR_LEAD_ACC_MAX_DREL:
+      return 1.0
+    if v_rel <= WMACConstants.RADAR_LEAD_ACC_MIN_CLOSING_SPEED and d_rel / max(-v_rel, 0.1) <= WMACConstants.RADAR_LEAD_ACC_MAX_TTC:
+      return 1.0
+    return 0.0
 
-    # 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 _desired_mode(self) -> tuple[ModeType, bool]:
+    if not self._CP.radarUnavailable and self._has_radar_acc_lead:
+      return 'acc', False
 
-    # EMERGENCY: MPC FCW - immediate blended mode
     if self._has_mpc_fcw:
-      self._mode_manager.request_mode('blended', confidence=1.0, emergency=True)
-      return
+      return 'blended', True
 
-    # Standstill: use blended
-    if self._standstill_count > 3:
-      self._mode_manager.request_mode('blended', confidence=0.9)
-      return
+    standstill = self._standstill_count > WMACConstants.STANDSTILL_FRAMES
+    urgent_slow_down = self._has_slow_down and self._raw_urgency > WMACConstants.URGENT_SLOW_DOWN_PROB
 
-    # 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
+    if self._CP.radarUnavailable:
+      if standstill or self._has_slow_down:
+        return 'blended', urgent_slow_down
+      return 'acc', False
 
-    # 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
+    if standstill or self._has_slow_down:
+      return 'blended', urgent_slow_down
 
-    # 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)
+    return 'acc', False
 
   def update(self, sm: messaging.SubMaster) -> None:
     self._read_params()
-
     self.set_mpc_fcw_crash_cnt()
-
     self._update_calculations(sm)
 
-    if self._CP.radarUnavailable:
-      self._radarless_mode()
-    else:
-      self._radar_mode()
-
+    mode, immediate = self._desired_mode()
+    self._mode_manager.request_mode(mode, immediate=immediate, hold_frames=WMACConstants.EMERGENCY_HOLD_FRAMES,
+                                    cancel_hold=self._has_radar_acc_lead)
     self._mode_manager.update()
+
     self._active = sm['selfdriveState'].experimentalMode and self._enabled
     self._frame += 1

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)  # Ensure urgency triggers slowdown
-  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"

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

@@ -0,0 +1,235 @@
+import pytest
+
+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):
+    self.status = status
+    self.dRel = dRel
+    self.vRel = vRel
+
+
+class MockRadarState:
+  def __init__(self, status=0.0, dRel=30.0, vRel=0.0):
+    self.leadOne = MockLeadOne(status=status, dRel=dRel, vRel=vRel)
+
+
+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),
+    '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()
+
+
+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_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)
+  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_allows_blended_until_acc_relevant(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)
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
+
+  controller.update(default_sm)
+
+  assert controller._has_lead_filtered
+  assert not controller._has_radar_acc_lead
+  assert controller.mode() == "blended"
+
+
+def test_relevant_radar_lead_smoothly_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=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=45.0, vRel=0.0)
+  for _ in range(20):
+    controller.update(default_sm)
+
+  assert controller._has_radar_acc_lead
+  assert controller.mode() == "acc"
+
+
+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)
+  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)
+  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)
+  controller.update(default_sm)
+
+  default_sm['radarState'] = MockRadarState(status=0.0)
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
+  controller.update(default_sm)
+
+  assert controller._has_lead_filtered
+  assert controller.mode() == "acc"

sunnypilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -9,6 +9,8 @@ from cereal import messaging, custom
 from opendbc.car import structs
 from openpilot.common.constants import CV
 from openpilot.selfdrive.car.cruise import V_CRUISE_MAX
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_controller import AccelController
+from openpilot.sunnypilot.selfdrive.controls.lib.radar_distance.radar_distance import RadarDistanceController
 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
@@ -26,6 +28,8 @@ class LongitudinalPlannerSP:
     self.events_sp = EventsSP()
     self.resolver = SpeedLimitResolver()
     self.dec = DynamicExperimentalController(CP, mpc)
+    self.accel = AccelController(CP, mpc)
+    self.radar_distance = RadarDistanceController(CP)
     self.scc = SmartCruiseControl()
     self.resolver = SpeedLimitResolver()
     self.sla = SpeedLimitAssist(CP, CP_SP)
@@ -76,8 +80,13 @@ class LongitudinalPlannerSP:
   def update(self, sm: messaging.SubMaster) -> None:
     self.events_sp.clear()
     self.dec.update(sm)
+    self.accel.update(sm)
+    self.radar_distance.update(sm)
     self.e2e_alerts_helper.update(sm, self.events_sp)
 
+  def smooth_radarstate(self, radarstate):
+    return self.radar_distance.smooth_radarstate(radarstate)
+
   def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
     plan_sp_send = messaging.new_message('longitudinalPlanSP')
 
@@ -138,4 +147,18 @@ class LongitudinalPlannerSP:
     e2eAlerts.greenLightAlert = self.e2e_alerts_helper.green_light_alert
     e2eAlerts.leadDepartAlert = self.e2e_alerts_helper.lead_depart_alert
 
+    # Acceleration Personality
+    acceleration = longitudinalPlanSP.acceleration
+    acceleration.personality = self.accel.personality()
+    acceleration.enabled = self.accel.enabled()
+    acceleration.maxAccel = float(self.accel.max_accel())
+    acceleration.brakeNeed = float(self.accel.brake_need())
+    acceleration.decelTarget = float(self.accel.decel_target())
+    acceleration.smoothActive = self.accel.smooth_active()
+    acceleration.bypassed = bool(self.accel.bypassed())
+    acceleration.comfortStopActive = bool(self.accel.comfort_stop_active())
+    acceleration.comfortStopFloor = float(self.accel.comfort_stop_floor())
+    acceleration.leadUnstable = bool(self.radar_distance.lead_unstable())
+
+
     pm.send('longitudinalPlanSP', plan_sp_send)

sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py

@@ -0,0 +1,307 @@
+"""
+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.
+
+RadarDistance smooths the lead the longitudinal MPC follows on a noisy radar, never reporting a
+farther-or-faster lead than reality, so braking is always >= stock:
+  - flicker-hold: keep a just-dropped, recently-sustained lead alive through a radar dropout.
+  - speed damp: lag the lead speeding up (instant on slow-down) to damp the catch-up surge / rubber-band,
+    reset on a track switch so it never carries a stale-slow value across a different track.
+Active only above LOW_SPEED_PASSTHROUGH_V; at/below it returns the raw radarstate (byte-stock stops).
+Default off => stock passthrough.
+"""
+
+from collections import deque
+
+from opendbc.car import structs
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+
+HOLD_MAX_FRAMES = 20        # ~1.0s flicker-hold cap, since the last sustained lead
+SUSTAIN_FRAMES = 2
+DROPOUT_DREL = 1.0
+FCW_PROB_CAP = 0.9
+MIN_HELD_DREL = 0.5
+
+LOW_SPEED_PASSTHROUGH_V = 5.0   # m/s
+
+# Speed-damp (B) gated off (caused phantom braking + launch rubber-band before); flicker-hold (A) runs alone.
+VLEAD_DAMP_ENABLED = False
+VLEAD_TAU = 0.4                 # s, lag on a speeding-up lead
+_VLEAD_ALPHA = DT_MDL / VLEAD_TAU
+SWITCH_DREL = 8.0              # m, dRel jump that means the radar switched to a different track -> reset the filter
+
+# Lead-instability detector (telemetry only): flags a bimodal/bouncing radar lead.
+STABILITY_WINDOW = 5            # frames (~0.25s @ 20Hz)
+VLEAD_SPREAD = 4.0             # m/s, vLead range over the window above which the lead is "unstable"
+ID_CHURN_WINDOW = 10           # frames (~0.5s) for radarTrackId-churn detection (steady lead, flipping track ids)
+ID_CHURN = 3                   # trackId switches in the window above which the lead is "unstable" (follow-hunting)
+
+# Lead jitter smoother (B2): during trackId churn the per-track dRel/vRel jitter makes the MPC hunt the follow
+# gap. A short SYMMETRIC EMA on the churning lead removes the jitter so the MPC sees a steady lead and stops
+# hunting. Active ONLY during churn (NOT bimodal vLead -> never averages two real tracks). Bounded symmetric
+# lag ~LEAD_SMOOTH_TAU. Gated OFF by default.
+LEAD_SMOOTH_ENABLED = False
+LEAD_SMOOTH_TAU = 0.5          # s, EMA time constant
+LEAD_SMOOTH_HOLD = 20          # frames (~1s): keep smoothing through brief churn gaps (churn toggles on/off)
+
+# Stop-gap bias: near a (near-)stopped lead at low speed, report dRel up to STOP_GAP_BIAS_M closer so the MPC
+# runs its own smooth stop but terminates that much farther back (stock crawl-creeps to ~2m). Monotone (closer
+# => brake >= stock). Ramps in over the regime edge and out as the lead moves (no step, releases on launch).
+STOP_GAP_BIAS_ENABLED = False
+STOP_GAP_BIAS_M = 2.0          # m: max dRel reduction = added standstill gap
+STOP_BIAS_VEGO = 8.0           # m/s: only below this ego speed
+STOP_BIAS_VLEAD = 1.5         # m/s: only behind a (near-)stopped lead; ramps out as vLead rises to this
+STOP_BIAS_REGIME_DREL = 12.0   # m: bias ramps in below this dRel
+STOP_BIAS_RAMP_BAND = 2.0     # m: ramp-in band (full offset below REGIME_DREL - RAMP_BAND)
+STOP_BIAS_MIN_DREL = 2.0      # m: never report a lead closer than this
+
+
+class _LeadView:
+  __slots__ = ('status', 'dRel', 'yRel', 'vRel', 'vLead', 'vLeadK', 'aLeadK', 'aLeadTau', 'modelProb')
+
+  def __init__(self, src, vlead):
+    self.status = src.status
+    self.dRel = src.dRel
+    self.yRel = src.yRel
+    self.vRel = src.vRel
+    self.vLead = vlead
+    self.vLeadK = vlead
+    self.aLeadK = src.aLeadK
+    self.aLeadTau = src.aLeadTau
+    self.modelProb = src.modelProb
+
+
+class _BiasedLead:
+  __slots__ = ('status', 'dRel', 'yRel', 'vRel', 'vLead', 'vLeadK', 'aLeadK', 'aLeadTau', 'modelProb')
+
+  def __init__(self, src, dRel):
+    self.status = src.status
+    self.dRel = dRel
+    self.yRel = src.yRel
+    self.vRel = src.vRel
+    self.vLead = src.vLead
+    self.vLeadK = src.vLeadK
+    self.aLeadK = src.aLeadK
+    self.aLeadTau = src.aLeadTau
+    self.modelProb = src.modelProb
+
+
+class _SmoothedLead:
+  __slots__ = ('status', 'dRel', 'yRel', 'vRel', 'vLead', 'vLeadK', 'aLeadK', 'aLeadTau', 'modelProb')
+
+  def __init__(self, src, dRel, vLead, vRel):
+    self.status = src.status
+    self.dRel = dRel
+    self.yRel = src.yRel
+    self.vRel = vRel
+    self.vLead = vLead
+    self.vLeadK = vLead
+    self.aLeadK = src.aLeadK
+    self.aLeadTau = src.aLeadTau
+    self.modelProb = src.modelProb
+
+
+class _LeadSmoother:
+  # Short symmetric EMA on a churning lead's dRel/vLead/vRel (jitter removal). A hold keeps it active through
+  # brief churn gaps (churn toggles); passthrough + reset only after the hold lapses.
+  def __init__(self):
+    self._d = None
+    self._vl = None
+    self._vr = None
+    self._hold = 0
+
+  def reset(self):
+    self._d = None
+    self._vl = None
+    self._vr = None
+    self._hold = 0
+
+  def update(self, lead, churn: bool):
+    self._hold = LEAD_SMOOTH_HOLD if churn else self._hold - 1
+    if self._hold <= 0 or not lead.status:
+      self.reset()
+      return lead
+    if self._d is None:
+      self._d, self._vl, self._vr = lead.dRel, lead.vLead, lead.vRel
+      return lead
+    a = DT_MDL / LEAD_SMOOTH_TAU
+    self._d += (lead.dRel - self._d) * a
+    self._vl += (lead.vLead - self._vl) * a
+    self._vr += (lead.vRel - self._vr) * a
+    return _SmoothedLead(lead, self._d, self._vl, self._vr)
+
+
+class _HeldLead:
+  __slots__ = ('status', 'dRel', 'yRel', 'vRel', 'vLead', 'vLeadK', 'aLeadK', 'aLeadTau', 'modelProb')
+
+  def __init__(self, dRel, vRel, vLead, aLeadK, aLeadTau, modelProb):
+    self.status = True
+    self.dRel = dRel
+    self.vRel = vRel
+    self.vLead = vLead
+    self.vLeadK = vLead
+    self.aLeadK = aLeadK
+    self.aLeadTau = aLeadTau
+    self.modelProb = modelProb
+    self.yRel = 0.0
+
+
+class _RadarStateProxy:
+  __slots__ = ('leadOne', 'leadTwo')
+
+  def __init__(self, lead_one, lead_two):
+    self.leadOne = lead_one
+    self.leadTwo = lead_two
+
+
+class _LeadHold:
+  def __init__(self):
+    self._last = None
+    self._sustained = 0
+    self._since_real = 0
+    self._armed = False
+    self._held_dRel = 0.0
+    self._vlead_f = None
+    self._last_dRel = None
+
+  def reset(self):
+    self.__init__()
+
+  def step(self, raw):
+    if raw.status and raw.dRel > DROPOUT_DREL:
+      self._last = (raw.dRel, raw.vRel, raw.vLead, raw.aLeadK, raw.aLeadTau, raw.modelProb)
+      self._sustained += 1
+      if self._sustained >= SUSTAIN_FRAMES:
+        self._since_real = 0
+        self._armed = True
+      return raw
+
+    self._sustained = 0
+    self._since_real += 1
+    if self._armed and self._last is not None and self._since_real <= HOLD_MAX_FRAMES:
+      dRel0, vRel0, vLead0, aLeadK0, aLeadTau0, prob0 = self._last
+      if self._since_real == 1:
+        self._held_dRel = dRel0
+      self._held_dRel = max(MIN_HELD_DREL, self._held_dRel - max(-vRel0, 0.0) * DT_MDL)
+      return _HeldLead(self._held_dRel, vRel0, vLead0, min(aLeadK0, 0.0), aLeadTau0, min(prob0, FCW_PROB_CAP))
+
+    self._armed = False
+    return raw
+
+  def smooth(self, lead):
+    if not lead.status:
+      self._vlead_f = None
+      self._last_dRel = None
+      return lead
+    if self._last_dRel is None or abs(lead.dRel - self._last_dRel) > SWITCH_DREL:
+      self._vlead_f = lead.vLead
+    self._last_dRel = lead.dRel
+    v = float(lead.vLead)
+    if self._vlead_f is None or v <= self._vlead_f:
+      self._vlead_f = v
+      return lead
+    self._vlead_f += (v - self._vlead_f) * _VLEAD_ALPHA
+    return _LeadView(lead, self._vlead_f)
+
+
+class _LeadStability:
+  # Read-only monitor: flags an unstable leadOne -- bimodal/bouncing vLead, dRel track-switch jumps, or
+  # radarTrackId churn (a steady lead flipping track ids -> vRel jitter -> follow-hunting). Telemetry only.
+  def __init__(self):
+    self._v = deque(maxlen=STABILITY_WINDOW)
+    self._d = deque(maxlen=STABILITY_WINDOW)
+    self._id = deque(maxlen=ID_CHURN_WINDOW)
+    self.unstable = False
+    self.churn = False
+
+  def reset(self):
+    self._v.clear()
+    self._d.clear()
+    self._id.clear()
+    self.unstable = False
+    self.churn = False
+
+  def update(self, lead, v_ego: float) -> None:
+    if not lead.status or v_ego < LOW_SPEED_PASSTHROUGH_V:
+      self.reset()
+      return
+    self._v.append(float(lead.vLead))
+    self._d.append(float(lead.dRel))
+    self._id.append(int(getattr(lead, 'radarTrackId', -1)))
+    if len(self._v) < STABILITY_WINDOW:
+      self.unstable = False
+      return
+    v_spread = max(self._v) - min(self._v)
+    d_jumps = sum(abs(b - a) > SWITCH_DREL for a, b in zip(self._d, list(self._d)[1:], strict=False))
+    ids = list(self._id)
+    id_churn = sum(1 for a, b in zip(ids, ids[1:], strict=False) if a != b and a > 0 and b > 0)
+    self.churn = id_churn >= ID_CHURN and v_spread <= VLEAD_SPREAD   # steady lead, flipping ids (not bimodal)
+    self.unstable = v_spread > VLEAD_SPREAD or d_jumps >= 2 or self.churn
+
+
+class RadarDistanceController:
+  def __init__(self, CP: structs.CarParams, params=None):
+    self._CP = CP
+    self._params = params or Params()
+    self._frame = 0
+    self._v_ego = 0.0
+    self._enabled = self._params.get_bool("RadarDistance")
+    self._vlead_damp_enabled = VLEAD_DAMP_ENABLED
+    self._stop_gap_bias_enabled = STOP_GAP_BIAS_ENABLED
+    self._lead_smooth_enabled = LEAD_SMOOTH_ENABLED
+    self._one = _LeadHold()
+    self._two = _LeadHold()
+    self._stability = _LeadStability()
+    self._smoother = _LeadSmoother()
+
+  def _read_params(self) -> None:
+    enabled = self._params.get_bool("RadarDistance")
+    if enabled and not self._enabled:
+      self._one.reset()
+      self._two.reset()
+    self._enabled = enabled
+
+  def update(self, sm) -> None:
+    if self._frame % int(1. / DT_MDL) == 0:
+      self._read_params()
+    self._v_ego = float(sm['carState'].vEgo)
+    self._frame += 1
+
+  def enabled(self) -> bool:
+    return self._enabled
+
+  def lead_unstable(self) -> bool:
+    return self._stability.unstable
+
+  def _stop_gap_bias(self, lead):
+    # Report a (near-)stopped lead up to STOP_GAP_BIAS_M closer at low speed, so the MPC's own smooth stop ends
+    # that much farther back. Monotone (only ever reports closer). No-op outside the regime / when disabled.
+    if not self._stop_gap_bias_enabled or not lead.status:
+      return lead
+    if lead.vLead > STOP_BIAS_VLEAD or self._v_ego > STOP_BIAS_VEGO or lead.dRel <= STOP_BIAS_MIN_DREL:
+      return lead
+    d_ramp = min(max((STOP_BIAS_REGIME_DREL - lead.dRel) / STOP_BIAS_RAMP_BAND, 0.0), 1.0)
+    v_ramp = min(max((STOP_BIAS_VLEAD - lead.vLead) / STOP_BIAS_VLEAD, 0.0), 1.0)
+    offset = STOP_GAP_BIAS_M * d_ramp * v_ramp
+    if offset < 0.05:
+      return lead
+    return _BiasedLead(lead, max(lead.dRel - offset, STOP_BIAS_MIN_DREL))
+
+  def smooth_radarstate(self, radarstate):
+    self._stability.update(radarstate.leadOne, self._v_ego)   # telemetry, runs every cycle
+    if not self._enabled:
+      return radarstate
+    one = self._one.step(radarstate.leadOne)
+    two = self._two.step(radarstate.leadTwo)
+    if self._v_ego < LOW_SPEED_PASSTHROUGH_V:
+      one_b = self._stop_gap_bias(radarstate.leadOne)         # low speed = stock lead, only the stop-gap bias
+      return radarstate if one_b is radarstate.leadOne else _RadarStateProxy(one_b, radarstate.leadTwo)
+    one = self._stop_gap_bias(one)
+    if self._lead_smooth_enabled:
+      one = self._smoother.update(one, self._stability.churn)  # de-jitter a churning lead (anti follow-hunt)
+    if not self._vlead_damp_enabled:
+      return _RadarStateProxy(one, two)                       # flicker-hold (A) only
+    return _RadarStateProxy(self._one.smooth(one), self._two.smooth(two))

sunnypilot/selfdrive/controls/lib/radar_distance/tests/test_radar_distance.py

@@ -0,0 +1,308 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+
+from types import SimpleNamespace
+
+import pytest
+
+from openpilot.sunnypilot.selfdrive.controls.lib.radar_distance.radar_distance import \
+  RadarDistanceController, HOLD_MAX_FRAMES, FCW_PROB_CAP, LOW_SPEED_PASSTHROUGH_V
+
+COMFORT_BRAKE = 2.5
+
+
+class FakeParams:
+  def __init__(self, store=None):
+    self.store = dict(store or {})
+
+  def get_bool(self, key):
+    return bool(self.store.get(key, False))
+
+
+def lead(status=True, dRel=40.0, vRel=-2.0, vLead=18.0, aLeadK=0.0, aLeadTau=1.5, modelProb=0.95, radarTrackId=-1):
+  return SimpleNamespace(status=status, dRel=dRel, yRel=0.0, vRel=vRel, vLead=vLead, vLeadK=vLead,
+                         aLeadK=aLeadK, aLeadTau=aLeadTau, modelProb=modelProb, radarTrackId=radarTrackId)
+
+
+def rs(one, two=None):
+  return SimpleNamespace(leadOne=one, leadTwo=two or lead(status=False, dRel=0.0, modelProb=0.0))
+
+
+def obstacle(ld):
+  return ld.dRel + ld.vLead ** 2 / (2 * COMFORT_BRAKE)
+
+
+def ctrl(enabled=True, vlead_damp=False):
+  c = RadarDistanceController(CP=SimpleNamespace(), params=FakeParams({'RadarDistance': enabled}))
+  c._v_ego = LOW_SPEED_PASSTHROUGH_V + 10.0  # default above the gate so hold-logic tests exercise the flicker-hold
+  c._vlead_damp_enabled = vlead_damp         # speed-damp (B) is gated off in production; opt in per-test
+  return c
+
+
+def test_disabled_is_identity():
+  c = ctrl(enabled=False)
+  r = rs(lead())
+  assert c.smooth_radarstate(r) is r  # byte-stock passthrough
+
+
+def test_valid_lead_passthrough():
+  c = ctrl()
+  one = lead(dRel=40.0)
+  out = c.smooth_radarstate(rs(one))
+  assert out.leadOne is one
+
+
+def test_holds_after_sustained_dropout():
+  c = ctrl()
+  for _ in range(3):  # sustain (>= SUSTAIN_FRAMES)
+    c.smooth_radarstate(rs(lead(dRel=30.0, vRel=-4.0, vLead=16.0)))
+  out = c.smooth_radarstate(rs(lead(status=False, dRel=0.0, modelProb=0.0)))
+  held = out.leadOne
+  assert held.status is True
+  assert held.dRel < 30.0          # dead-reckoned closer
+  assert held.dRel == pytest.approx(30.0 - 4.0 * 0.05, abs=1e-6)
+
+
+def test_low_speed_override_lead_passthrough():
+  # radard low_speed_override emits a real closest-track lead with modelProb=0.0. It must be honored as a
+  # real lead (passthrough), NOT rejected and replaced by a stale farther held lead (would under-brake at
+  # stop-and-go and stop too close).
+  c = ctrl()
+  one = lead(status=True, dRel=2.5, vRel=0.0, vLead=0.0, modelProb=0.0)
+  out = c.smooth_radarstate(rs(one))
+  assert out.leadOne is one                         # passed straight through, not substituted
+
+
+def test_low_speed_override_lead_arms_hold():
+  # a sustained prob=0 real lead should arm the hold like any real lead
+  c = ctrl()
+  for _ in range(3):
+    c.smooth_radarstate(rs(lead(status=True, dRel=3.0, vRel=-0.5, vLead=1.0, modelProb=0.0)))
+  held = c.smooth_radarstate(rs(lead(status=False, dRel=0.0, modelProb=0.0))).leadOne
+  assert held.status is True                         # armed off the prob=0 lead, holds through dropout
+
+
+def test_low_speed_returns_raw_object():
+  # Stop/creep regime: ENABLED returns the EXACT raw radarstate object (byte-identical to OFF), so the
+  # lead the MPC sees -- and thus the stop distance -- is stock. This is the core stop-neutrality guarantee.
+  c = ctrl()
+  c._v_ego = LOW_SPEED_PASSTHROUGH_V - 0.1
+  r = rs(lead(status=True, dRel=6.0, vRel=0.0, vLead=0.0))
+  assert c.smooth_radarstate(r) is r                 # object identity == stock
+
+
+def test_low_speed_passthrough_but_hold_warmed_for_highway():
+  # At low speed the raw radarstate is returned, but the hold is still stepped (state kept warm) so the
+  # flicker-hold engages the moment speed rises above the gate.
+  c = ctrl()
+  for _ in range(3):                                 # sustain a real lead while in the low-speed regime
+    c._v_ego = LOW_SPEED_PASSTHROUGH_V - 0.1
+    r = rs(lead(dRel=30.0, vRel=-4.0, vLead=16.0))
+    assert c.smooth_radarstate(r) is r               # returned object stays raw at low speed
+  c._v_ego = LOW_SPEED_PASSTHROUGH_V + 10.0          # rise above the gate -> dropout now held (proxy, not raw)
+  out = c.smooth_radarstate(rs(lead(status=False, dRel=0.0, modelProb=0.0)))
+  assert out.leadOne.status is True
+
+
+def test_vlead_lags_rise_instant_fall():
+  c = ctrl(vlead_damp=True)                                     # speed-damp (B) under test; gated off in production
+  c.smooth_radarstate(rs(lead(dRel=30.0, vLead=15.0)))           # seed at 15
+  rising = c.smooth_radarstate(rs(lead(dRel=30.0, vLead=25.0))).leadOne
+  assert 15.0 <= rising.vLead < 25.0                            # rise lagged (<= real -> never faster than real)
+  falling = c.smooth_radarstate(rs(lead(dRel=30.0, vLead=8.0))).leadOne
+  assert falling.vLead == pytest.approx(8.0, abs=1e-6)          # slow-down instant
+
+
+def test_vlead_resets_on_track_switch_no_phantom_slow():
+  # the old bug: a slow lead's filtered speed carried across a switch to a fast farther track, reporting it
+  # near-stopped. A dRel jump (track switch) now resets the filter -> the new track's real speed is reported.
+  c = ctrl(vlead_damp=True)                                     # speed-damp (B) under test; gated off in production
+  for _ in range(3):
+    c.smooth_radarstate(rs(lead(dRel=12.0, vLead=0.5)))         # slow close lead
+  switched = c.smooth_radarstate(rs(lead(dRel=80.0, vLead=18.0))).leadOne  # different, far, fast track
+  assert switched.vLead == pytest.approx(18.0, abs=1e-6)        # real speed, not the stale ~0.5
+
+
+def test_vlead_damp_gated_off_reports_real_speed():
+  # Production default (VLEAD_DAMP_ENABLED off): a speeding-up lead is NOT lagged -> real vLead passes through
+  # (flicker-hold A only). This is the on-by-default behavior; B is opt-in pending on-road proof.
+  c = ctrl()                                                    # vlead_damp defaults off (production)
+  c.smooth_radarstate(rs(lead(dRel=30.0, vLead=15.0)))
+  rising = c.smooth_radarstate(rs(lead(dRel=30.0, vLead=25.0))).leadOne
+  assert rising.vLead == pytest.approx(25.0, abs=1e-6)          # no damp -> real speed
+
+
+# --- lead-instability detector (telemetry) -----------------------------------
+
+def test_stability_quiet_on_clean_lead():
+  c = ctrl()
+  for v in (18.0, 18.2, 17.9, 18.1, 18.0, 17.8):                # steady lead, small noise
+    c.smooth_radarstate(rs(lead(dRel=40.0, vLead=v)))
+  assert not c.lead_unstable()                                  # range < VLEAD_SPREAD -> stable
+
+def test_stability_flags_bimodal_lead():
+  c = ctrl()
+  for v in (12.0, 2.0, 12.0, 2.0, 12.0):                        # bouncing between two tracks
+    c.smooth_radarstate(rs(lead(dRel=60.0, vLead=v)))
+  assert c.lead_unstable()                                      # range 10 m/s > VLEAD_SPREAD -> unstable
+
+def test_stability_resets_on_dropout():
+  c = ctrl()
+  for v in (12.0, 2.0, 12.0, 2.0, 12.0):
+    c.smooth_radarstate(rs(lead(dRel=60.0, vLead=v)))
+  assert c.lead_unstable()
+  c.smooth_radarstate(rs(lead(status=False, dRel=0.0, modelProb=0.0)))   # lead drops
+  assert not c.lead_unstable()                                 # buffer cleared -> stable
+
+def test_stability_runs_even_when_disabled():
+  c = ctrl(enabled=False)                                      # telemetry runs regardless of RadarDistance gate
+  for v in (12.0, 2.0, 12.0, 2.0, 12.0):
+    c.smooth_radarstate(rs(lead(dRel=60.0, vLead=v)))
+  assert c.lead_unstable()
+
+def test_stability_flags_trackid_churn():
+  c = ctrl()
+  for tid in (10, 20, 10, 20, 10, 20, 10, 20, 10, 20):         # steady lead, radarTrackId flipping (follow-hunt)
+    c.smooth_radarstate(rs(lead(dRel=44.0, vLead=27.0, radarTrackId=tid)))
+  assert c.lead_unstable()
+
+def test_stability_steady_id_quiet():
+  c = ctrl()
+  for _ in range(10):
+    c.smooth_radarstate(rs(lead(dRel=44.0, vLead=27.0, radarTrackId=10)))
+  assert not c.lead_unstable()                                 # steady lead + steady id -> stable
+
+
+# --- lead jitter smoother (B2: anti follow-hunt) -----------------------------
+
+def _churn_feed(c, n=20):
+  out = []
+  for k in range(n):
+    dr = 42.0 if k % 2 == 0 else 46.0                          # steady ~44m lead, dRel jitter
+    tid = 10 if k % 2 == 0 else 20                             # radarTrackId churning
+    out.append(c.smooth_radarstate(rs(lead(dRel=dr, vLead=27.0, vRel=0.0, radarTrackId=tid))).leadOne.dRel)
+  return out
+
+def test_lead_smooth_removes_churn_jitter():
+  c = ctrl()
+  c._lead_smooth_enabled = True
+  tail = _churn_feed(c)[12:]
+  assert max(tail) - min(tail) < 3.0                           # raw range is 4.0 -> jitter reduced
+  assert all(42.5 < x < 45.5 for x in tail)                   # pulled toward the mean ~44
+
+def test_lead_smooth_off_passthrough():
+  c = ctrl()                                                   # smoother off (default)
+  tail = _churn_feed(c)[12:]
+  assert {round(x, 1) for x in tail} <= {42.0, 46.0}           # raw dRel, no smoothing
+
+def test_lead_smooth_inactive_without_churn():
+  c = ctrl()
+  c._lead_smooth_enabled = True
+  out = None
+  for _ in range(12):
+    out = c.smooth_radarstate(rs(lead(dRel=44.0, vLead=27.0, radarTrackId=10)))   # steady id -> no churn
+  assert out.leadOne.dRel == pytest.approx(44.0, abs=1e-6)     # smoother inactive -> exact dRel
+
+
+# --- stop-gap bias (smooth farther stop) -------------------------------------
+
+def _biased_ctrl(v_ego=2.0):
+  c = ctrl()
+  c._stop_gap_bias_enabled = True
+  c._v_ego = v_ego
+  return c
+
+def _bias(c, dRel, vLead):
+  return c._stop_gap_bias(lead(dRel=dRel, vLead=vLead))
+
+def test_stop_bias_pulls_stopped_lead_closer():
+  out = _bias(_biased_ctrl(), 8.0, 0.0)
+  assert 2.0 <= out.dRel < 8.0                                 # reported closer (farther stop), floored
+  assert out.vLead == 0.0 and out.status                       # other fields preserved
+
+def test_stop_bias_monotone_never_farther():
+  c = _biased_ctrl()
+  for dr in (4.0, 6.0, 8.0, 10.0, 12.0, 20.0):
+    assert _bias(c, dr, 0.0).dRel <= dr + 1e-6
+
+def test_stop_bias_min_floor():
+  assert _bias(_biased_ctrl(), 2.5, 0.0).dRel == pytest.approx(2.0, abs=1e-6)
+
+def test_stop_bias_off_no_change():
+  c = ctrl()
+  c._v_ego = 2.0
+  ld = lead(dRel=8.0, vLead=0.0)
+  assert c._stop_gap_bias(ld) is ld                            # default off -> exact passthrough
+
+def test_stop_bias_moving_lead_no_change():
+  ld = lead(dRel=8.0, vLead=5.0)
+  assert _biased_ctrl()._stop_gap_bias(ld) is ld
+
+def test_stop_bias_high_speed_no_change():
+  ld = lead(dRel=8.0, vLead=0.0)
+  assert _biased_ctrl(v_ego=15.0)._stop_gap_bias(ld) is ld
+
+def test_stop_bias_far_lead_no_change():
+  ld = lead(dRel=30.0, vLead=0.0)
+  assert _biased_ctrl()._stop_gap_bias(ld) is ld               # beyond regime -> no bias
+
+def test_stop_bias_via_smooth_radarstate_low_speed():
+  out = _biased_ctrl().smooth_radarstate(rs(lead(dRel=8.0, vLead=0.0, vRel=-2.0)))
+  assert out.leadOne.dRel < 8.0                                # biased proxy returned at low speed
+
+
+def test_obstacle_monotone_during_hold():
+  c = ctrl()
+  for _ in range(3):
+    c.smooth_radarstate(rs(lead(dRel=30.0, vRel=-4.0, vLead=16.0)))
+  last_obs = obstacle(lead(dRel=30.0, vLead=16.0))
+  prev = last_obs
+  for _ in range(HOLD_MAX_FRAMES):
+    held = c.smooth_radarstate(rs(lead(status=False, dRel=0.0, modelProb=0.0))).leadOne
+    if not held.status:
+      break
+    o = obstacle(held)
+    assert o <= last_obs + 1e-6     # never farther than the last real obstacle (brakes >= last real)
+    assert o <= prev + 1e-6         # monotonically non-increasing -> brakes more over the hold
+    prev = o
+
+
+def test_releases_after_hold_cap():
+  c = ctrl()
+  for _ in range(3):
+    c.smooth_radarstate(rs(lead(dRel=30.0)))
+  statuses = [c.smooth_radarstate(rs(lead(status=False, dRel=0.0, modelProb=0.0))).leadOne.status
+              for _ in range(HOLD_MAX_FRAMES + 3)]
+  assert all(statuses[:HOLD_MAX_FRAMES])        # held through the cap
+  assert statuses[HOLD_MAX_FRAMES] is False     # released after
+
+
+def test_no_hold_without_sustained_lead():
+  c = ctrl()
+  c.smooth_radarstate(rs(lead(dRel=30.0)))       # single valid frame (< SUSTAIN_FRAMES)
+  out = c.smooth_radarstate(rs(lead(status=False, dRel=0.0, modelProb=0.0)))
+  assert out.leadOne.status is False             # not armed -> no hold
+
+
+def test_flicker_does_not_reset_wall_clock():
+  c = ctrl()
+  for _ in range(3):
+    c.smooth_radarstate(rs(lead(dRel=30.0)))
+  # 1/0/1/0 flicker: lone valid frames must NOT reset the wall-clock (sustained < SUSTAIN_FRAMES)
+  for _ in range(4):
+    c.smooth_radarstate(rs(lead(status=False, dRel=0.0, modelProb=0.0)))  # dropout
+    c.smooth_radarstate(rs(lead(dRel=31.0)))                              # lone valid
+  assert c._one._since_real > 0                  # wall-clock kept climbing through the flicker
+
+
+def test_fcw_prob_capped_and_aleadk_not_positive():
+  c = ctrl()
+  for _ in range(3):
+    c.smooth_radarstate(rs(lead(dRel=30.0, aLeadK=1.0, modelProb=0.99)))
+  held = c.smooth_radarstate(rs(lead(status=False, dRel=0.0, modelProb=0.0))).leadOne
+  assert held.modelProb <= FCW_PROB_CAP          # no false FCW from a held phantom
+  assert held.aLeadK <= 0.0                       # never project the held lead as accelerating

sunnypilot/selfdrive/controls/lib/relc.py

@@ -0,0 +1,106 @@
+"""
+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.25
+EDGE_PROB = 0.35
+EDGE_REACTION_TIME = 1.0
+EDGE_CLEAR_TIME = 0.3
+MIN_SPEED = 20 * CV.MPH_TO_MS
+NEAR_EDGE_DISTANCE = 4.5
+LEFT_NEARSIDE_LANE_IDX = 1
+RIGHT_NEARSIDE_LANE_IDX = 2
+
+
+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
+
+  @staticmethod
+  def _road_edge_y(road_edges, idx: int) -> float | None:
+    if road_edges is None or len(road_edges) <= idx or len(road_edges[idx].y) == 0:
+      return None
+    return road_edges[idx].y[0]
+
+  @staticmethod
+  def _edge_is_near(edge_y: float | None, left: bool) -> bool:
+    if edge_y is None:
+      return False
+    if left:
+      return bool(-NEAR_EDGE_DISTANCE < edge_y < 0.0)
+    return bool(0.0 < edge_y < NEAR_EDGE_DISTANCE)
+
+  def update(self, road_edge_stds, lane_line_probs, v_ego: float, road_edges=None) -> 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[LEFT_NEARSIDE_LANE_IDX]
+    right_lane_prob = lane_line_probs[RIGHT_NEARSIDE_LANE_IDX]
+
+    left_edge_y = self._road_edge_y(road_edges, 0)
+    right_edge_y = self._road_edge_y(road_edges, 1)
+    left_edge_near = self._edge_is_near(left_edge_y, True)
+    right_edge_near = self._edge_is_near(right_edge_y, False)
+
+    left_cond = left_edge_prob > EDGE_PROB and (left_edge_near or (left_edge_y is None and left_lane_prob < NEARSIDE_PROB))
+    right_cond = right_edge_prob > EDGE_PROB and (right_edge_near or (right_edge_y is None and right_lane_prob < NEARSIDE_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

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

@@ -5,6 +5,8 @@ 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 +109,11 @@ 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
+

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

@@ -0,0 +1,133 @@
+"""
+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
+
+
+class DummyRoadEdge:
+  def __init__(self, y):
+    self.y = [y]
+
+
+@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 make_road_edges(left_y=-3.0, right_y=3.0):
+  return [DummyRoadEdge(left_y), DummyRoadEdge(right_y)]
+
+
+def drive(controller, road_edge_stds, lane_line_probs, seconds, v_ego=V_HIGH, road_edges=None):
+  for _ in range(int(seconds / DT_MDL) + 1):
+    controller.update(road_edge_stds, lane_line_probs, v_ego, road_edges)
+
+
+@pytest.mark.parametrize("road_edge_stds,lane_line_probs,attr", [
+  ([0.0, 0.9], [0.8, 0.0, 0.8, 0.8], "left_edge_detected"),
+  ([0.9, 0.0], [0.8, 0.8, 0.0, 0.8], "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.8, 0.0, 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.8, 0.0, 0.0, 0.8], 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.8, 0.0, 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.8, 0.0, 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.8, 0.0, 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.8, 0.0, 0.8, 0.8], EDGE_REACTION_TIME + 0.1)
+  assert relc.left_edge_detected
+
+  relc.update([0.0, 0.9], [0.8, 0.0, 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.8, 0.0, 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.8, 0.0, 0.8, 0.8], V_HIGH)
+  assert not relc.left_edge_detected
+  assert not relc.right_edge_detected
+
+
+@pytest.mark.parametrize("lane_line_probs", [
+  [0.0, 0.8, 0.8, 0.8],
+  [0.8, 0.8, 0.8, 0.0],
+])
+def test_outer_lane_lines_do_not_drive_edge_detection(relc, lane_line_probs):
+  drive(relc, [0.0, 0.0], lane_line_probs, EDGE_REACTION_TIME + 0.1)
+  assert not relc.left_edge_detected
+  assert not relc.right_edge_detected
+
+
+@pytest.mark.parametrize("road_edge_stds,road_edges,attr", [
+  ([0.0, 0.9], make_road_edges(left_y=-3.0, right_y=8.0), "left_edge_detected"),
+  ([0.9, 0.0], make_road_edges(left_y=-8.0, right_y=3.0), "right_edge_detected"),
+])
+def test_near_road_edge_geometry_blocks_with_visible_lane_lines(relc, road_edge_stds, road_edges, attr):
+  drive(relc, road_edge_stds, [0.8, 0.8, 0.8, 0.8], EDGE_REACTION_TIME + 0.1, road_edges=road_edges)
+  assert getattr(relc, attr)
+
+
+def test_far_road_edge_geometry_does_not_block(relc):
+  drive(relc, [0.0, 0.0], [0.8, 0.0, 0.0, 0.8], EDGE_REACTION_TIME + 0.1, road_edges=make_road_edges(left_y=-8.0, right_y=8.0))
+  assert not relc.left_edge_detected
+  assert not relc.right_edge_detected

sunnypilot/selfdrive/selfdrived/events.py

@@ -243,4 +243,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),
+  },
 }

sunnypilot/sunnylink/params_metadata.json

@@ -1,4 +1,12 @@
 {
+  "AccelPersonality": {
+    "title": "Acceleration Profile",
+    "description": "Eco accelerates gently and brakes early and soft; Sport accelerates briskly. Hard-braking authority is always preserved."
+  },
+  "AccelPersonalityEnabled": {
+    "title": "Enable Acceleration Profiles",
+    "description": "Enable Eco/Normal/Sport acceleration profiles, including early soft braking."
+  },
   "AccessToken": {
     "title": "AccessTokenIsNice",
     "description": ""
@@ -1098,6 +1106,10 @@
     "title": "Quiet Mode",
     "description": ""
   },
+  "RadarDistance": {
+    "title": "Radar Distance",
+    "description": "Holds a lead through brief radar flicker/dropout so sunnypilot does not lose and re-grab it, smoothing the hard/late brakes that radar drop-outs cause. Braking is never reduced below stock."
+  },
   "RainbowMode": {
     "title": "Rainbow Mode",
     "description": ""
@@ -1118,6 +1130,10 @@
     "title": "Record Front Lock",
     "description": ""
   },
+  "RoadEdgeLaneChangeEnabled": {
+    "title": "Block Lane Change: Road Edge Detection",
+    "description": "Blocks lane changes when the model sees a road edge on the signaled side."
+  },
   "RoadName": {
     "title": "Road Name",
     "description": ""
@@ -1324,14 +1340,30 @@
     "step": 0.1,
     "unit": "m/s\u00b2"
   },
+  "ToyotaAutoHold": {
+    "title": "Toyota: Auto Brake Hold FOR TSS2 HYBRID CARS",
+    "description": ""
+  },
+  "ToyotaDriveMode": {
+    "title": "Enable drive mode btn link",
+    "description": ""
+  },
   "ToyotaEnforceStockLongitudinal": {
     "title": "Toyota: Enforce Factory Longitudinal Control",
     "description": "When enabled, sunnypilot will not take over control of gas and brakes. Factory Toyota longitudinal control will be used."
   },
+  "ToyotaEnhancedBsm": {
+    "title": "Toyota: Prius TSS2 BSM and some tssp",
+    "description": ""
+  },
   "ToyotaStopAndGoHack": {
     "title": "Toyota: Stop and Go Hack (Alpha)",
     "description": "sunnypilot will allow some Toyota/Lexus cars to auto resume during stop and go traffic. This feature is only applicable to certain models that are able to use longitudinal control. This is an alpha feature. Use at your own risk."
   },
+  "ToyotaTSS2Long": {
+    "title": "Toyota: custom longitudinal for TSS2",
+    "description": ""
+  },
   "TrainingVersion": {
     "title": "Training Version",
     "description": ""

sunnypilot/sunnylink/settings_ui.json

@@ -537,6 +537,12 @@
                   "value": 0
                 }
               ]
+            },
+            {
+              "key": "RoadEdgeLaneChangeEnabled",
+              "widget": "toggle",
+              "title": "Block Lane Change: Road Edge Detection",
+              "description": "Blocks the lane change if the model sees a road edge on your signaled side."
             }
           ]
         }
@@ -587,6 +593,26 @@
                 }
               ]
             },
+            {
+              "key": "RadarDistance",
+              "widget": "toggle",
+              "title": "Radar Distance",
+              "description": "Holds a lead through brief radar flicker/dropout so sunnypilot does not lose and re-grab it, smoothing the hard/late brakes that radar drop-outs cause. Braking is never reduced below stock.",
+              "visibility": [
+                {
+                  "type": "capability",
+                  "field": "has_longitudinal_control",
+                  "equals": true
+                }
+              ],
+              "enablement": [
+                {
+                  "type": "capability",
+                  "field": "has_longitudinal_control",
+                  "equals": true
+                }
+              ]
+            },
             {
               "key": "DisengageOnAccelerator",
               "widget": "toggle",
@@ -620,6 +646,58 @@
                 }
               ]
             },
+            {
+              "key": "AccelPersonalityEnabled",
+              "widget": "toggle",
+              "title": "Enable Acceleration Profiles",
+              "description": "Enables Eco/Normal/Sport acceleration profiles for longitudinal control, including early soft braking.",
+              "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 accelerates gently and brakes early and soft; Sport accelerates briskly. Hard-braking authority is always preserved.",
+              "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 +2079,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
+                }
+              ]
             }
           ]
         },
@@ -2168,6 +2262,50 @@
       "title": "Toyota / Lexus Settings",
       "description": "",
       "items": [
+        {
+          "key": "ToyotaAutoHold",
+          "widget": "toggle",
+          "needs_onroad_cycle": true,
+          "title": "Toyota: Auto Brake Hold FOR TSS2 HYBRID CARS",
+          "enablement": [
+            {
+              "type": "not_engaged"
+            }
+          ]
+        },
+        {
+          "key": "ToyotaEnhancedBsm",
+          "widget": "toggle",
+          "needs_onroad_cycle": true,
+          "title": "Toyota: Prius TSS2 BSM and some tssp",
+          "enablement": [
+            {
+              "type": "not_engaged"
+            }
+          ]
+        },
+        {
+          "key": "ToyotaTSS2Long",
+          "widget": "toggle",
+          "needs_onroad_cycle": true,
+          "title": "Toyota: custom longitudinal for TSS2",
+          "enablement": [
+            {
+              "type": "not_engaged"
+            }
+          ]
+        },
+        {
+          "key": "ToyotaDriveMode",
+          "widget": "toggle",
+          "needs_onroad_cycle": true,
+          "title": "Enable drive mode btn link",
+          "enablement": [
+            {
+              "type": "not_engaged"
+            }
+          ]
+        },
         {
           "key": "ToyotaEnforceStockLongitudinal",
           "widget": "toggle",

sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml

@@ -24,6 +24,15 @@ sections:
     - $ref: '#/macros/longitudinal'
     enablement:
     - $ref: '#/macros/longitudinal'
+  - key: RadarDistance
+    widget: toggle
+    title: Radar Distance
+    description: Holds a lead through brief radar flicker/dropout so sunnypilot does not lose and re-grab
+      it, smoothing the hard/late brakes that radar drop-outs cause. Braking is never reduced below stock.
+    visibility:
+    - $ref: '#/macros/longitudinal'
+    enablement:
+    - $ref: '#/macros/longitudinal'
   - key: DisengageOnAccelerator
     widget: toggle
     title: Disengage Cruise on Accelerator Pedal
@@ -43,6 +52,31 @@ sections:
       label: Relaxed
     enablement:
     - $ref: '#/macros/longitudinal'
+  - key: AccelPersonalityEnabled
+    widget: toggle
+    title: Enable Acceleration Profiles
+    description: Enables Eco/Normal/Sport acceleration profiles for longitudinal control, including early soft braking.
+    visibility:
+    - $ref: '#/macros/longitudinal'
+    enablement:
+    - $ref: '#/macros/longitudinal'
+  - key: AccelPersonality
+    widget: multiple_button
+    title: Acceleration Profile
+    description: Eco accelerates gently and brakes early and soft; Sport accelerates briskly. Hard-braking
+      authority is always preserved.
+    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)

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

sunnypilot/sunnylink/settings_ui_src/pages/steering.yaml

@@ -255,3 +255,7 @@ sections:
       key: AutoLaneChangeTimer
       op: '>'
       value: 0
+  - key: RoadEdgeLaneChangeEnabled
+    widget: toggle
+    title: 'Block Lane Change: Road Edge Detection'
+    description: Blocks the lane change if the model sees a road edge on your signaled side.

sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml

@@ -60,6 +60,30 @@ sections:
   title: Toyota / Lexus Settings
   description: ''
   items:
+  - key: ToyotaAutoHold
+    widget: toggle
+    needs_onroad_cycle: true
+    title: 'Toyota: Auto Brake Hold FOR TSS2 HYBRID CARS'
+    enablement:
+    - $ref: '#/macros/not_engaged'
+  - key: ToyotaEnhancedBsm
+    widget: toggle
+    needs_onroad_cycle: true
+    title: 'Toyota: Prius TSS2 BSM and some tssp'
+    enablement:
+    - $ref: '#/macros/not_engaged'
+  - key: ToyotaTSS2Long
+    widget: toggle
+    needs_onroad_cycle: true
+    title: 'Toyota: custom longitudinal for TSS2'
+    enablement:
+    - $ref: '#/macros/not_engaged'
+  - key: ToyotaDriveMode
+    widget: toggle
+    needs_onroad_cycle: true
+    title: Enable drive mode btn link
+    enablement:
+    - $ref: '#/macros/not_engaged'
   - key: ToyotaEnforceStockLongitudinal
     widget: toggle
     needs_onroad_cycle: true

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

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)