fix(long): comfort_stop and vLead

This reverts commit 83de89e253.

.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,25 @@ 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)
+  }
+
+  enum AccelerationPersonality {
+    eco @0;
+    normal @1;
+    sport @2;
+  }
 }
 
 struct OnroadEventSP @0xda96579883444c35 {
@@ -342,6 +362,7 @@ struct OnroadEventSP @0xda96579883444c35 {
     speedLimitChanged @21;
     speedLimitPending @22;
     e2eChime @23;
+    laneChangeRoadEdge @24;
   }
 }
 
@@ -448,6 +469,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_comfort_stop_and_vlead_damp_gated_off():
+  # Strategy invariants (tn @ 2026-06-26): final-approach stop passes through stock (goal 6 stock-met), and the
+  # input-side vLead speed-damp (B) stays off pending on-road proof. Flicker-hold (A) is unaffected by either.
+  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,235 @@
+"""
+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 (gated OFF by default via COMFORT_STOP_ENABLED -- stops pass
+through stock). 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
+
+_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   # gated OFF: stops pass through stock (goal 6 stock-met)
+    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
+    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 _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
+    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,93 @@
+"""
+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.
+# Gated OFF by default and independently of AccelPersonalityEnabled: the final approach passes through stock,
+# because goal 6 (smooth coming to stop) is already met by the stock/Toyota tune (parks ~4.2m smoothly) and the
+# old stops-too-close complaint traced to the radar_distance modelProb gate, since fixed at source. Re-enable
+# (flip True) only if an on-road roll-in / creep is observed, ideally with a gentler COMFORT_STOP_MAX_DECEL (~-1.2).
+COMFORT_STOP_ENABLED = False
+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)

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

@@ -0,0 +1,368 @@
+"""
+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):
+  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.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
+
+
+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/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,17 @@ 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())
+
+
     pm.send('longitudinalPlanSP', plan_sp_send)

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

@@ -0,0 +1,162 @@
+"""
+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 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) is gated OFF: it lags a speeding-up lead (input-side) and a prior version caused phantom
+# braking + a launch rubber-band (lead measured up to ~11 m/s slower than real). Flicker-hold (A) runs alone.
+# Flip True only behind an on-road check of a lead accelerating away above LOW_SPEED_PASSTHROUGH_V; if a
+# failure-to-release appears, prefer an output-side accel slew over re-enabling this input-side lag.
+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
+
+
+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 _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 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   # speed-damp (B) gated off; flicker-hold (A) runs alone
+    self._one = _LeadHold()
+    self._two = _LeadHold()
+
+  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 smooth_radarstate(self, radarstate):
+    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:
+      return radarstate
+    if not self._vlead_damp_enabled:
+      return _RadarStateProxy(one, two)                       # flicker-hold (A) only; speed-damp (B) gated off
+    return _RadarStateProxy(self._one.smooth(one), self._two.smooth(two))

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

@@ -0,0 +1,189 @@
+"""
+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):
+  return SimpleNamespace(status=status, dRel=dRel, yRel=0.0, vRel=vRel, vLead=vLead, vLeadK=vLead,
+                         aLeadK=aLeadK, aLeadTau=aLeadTau, modelProb=modelProb)
+
+
+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
+
+
+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)