refactor(long):

common/params_keys.h

@@ -243,8 +243,8 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     // Radar Distance: hold a lead through radar flicker/dropout so the MPC doesn't lose+regain it
     {"RadarDistance", {PERSISTENT | BACKUP, BOOL, "0"}},
 
-    // Stop Settle Soften: ease the final brake-pressure build below walking speed for a smoother stop
-    {"StopSettleSoften", {PERSISTENT | BACKUP, BOOL, "0"}},
+    // Stop Gap Bias: stop a bit farther back from a stopped lead so it doesn't crawl in too close
+    {"StopGapBias", {PERSISTENT | BACKUP, BOOL, "0"}},
 
     // sunnypilot model params
     {"CameraOffset", {PERSISTENT | BACKUP, FLOAT, "0.0"}},

selfdrive/controls/controlsd.py

@@ -55,7 +55,6 @@ class Controls(ControlsExt):
     self.calibrated_pose: Pose | None = None
 
     self.LoC = LongControl(self.CP, self.CP_SP)
-    self.LoC = ControlsExt.initialize_longitudinal_control(self, self.LoC)
     self.VM = VehicleModel(self.CP)
     self.LaC: LatControl
     if self.CP.steerControlType == car.CarParams.SteerControlType.angle:

selfdrive/controls/tests/test_longitudinal_smoothing_wiring.py

@@ -13,6 +13,7 @@ def test_smoothing_params_default_off():
 
   assert re.search(r'"AccelPersonalityEnabled", \{PERSISTENT \| BACKUP, BOOL, "0"\}', params_keys)
   assert re.search(r'"RadarDistance", \{PERSISTENT \| BACKUP, BOOL, "0"\}', params_keys)
+  assert re.search(r'"StopGapBias", \{PERSISTENT \| BACKUP, BOOL, "0"\}', params_keys)
 
 
 def test_longitudinal_smoothing_stays_planner_side():
@@ -43,12 +44,11 @@ def test_dec_model_stop_target_not_reintroduced():
 
 
 def test_long_feature_gates():
-  # The surviving opt-in long features all default OFF (byte-stock until enabled + on-road verified):
-  # AccelController jerk-limiter, RadarDistance lead-smoother + stop-gap bias.
+  # The surviving opt-in long features default OFF (byte-stock until enabled + on-road verified):
+  # AccelController jerk-limiter and RadarDistance lead-smoother are module flags; the StopGapBias stop-gap
+  # rides on a param (its default is checked in test_smoothing_params_default_off).
   from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import JERK_LIMIT_ENABLED
-  from openpilot.sunnypilot.selfdrive.controls.lib.radar_distance.radar_distance import \
-    LEAD_SMOOTH_ENABLED, STOP_GAP_BIAS_ENABLED
+  from openpilot.sunnypilot.selfdrive.controls.lib.radar_distance.radar_distance import LEAD_SMOOTH_ENABLED
 
   assert JERK_LIMIT_ENABLED is False
   assert LEAD_SMOOTH_ENABLED is False
-  assert STOP_GAP_BIAS_ENABLED is False

sunnypilot/selfdrive/controls/controlsd_ext.py

@@ -17,7 +17,6 @@ from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
 from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
 from openpilot.sunnypilot.selfdrive.controls.lib.blinker_pause_lateral import BlinkerPauseLateral
 from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_torque_v0 import LatControlTorque as LatControlTorqueV0
-from openpilot.sunnypilot.selfdrive.controls.lib.longcontrol_ext import LongControlExt
 
 
 class ControlsExt(ModelStateBase):
@@ -35,10 +34,6 @@ class ControlsExt(ModelStateBase):
     self.sm_services_ext = ['radarState', 'selfdriveStateSP']
     self.pm_services_ext = ['carControlSP']
 
-  def initialize_longitudinal_control(self, _loc):
-    # The softener self-gates on the StopSettleSoften param (read live), so it is byte-stock when off.
-    return LongControlExt(self.CP, self.CP_SP, self.params)
-
   def initialize_lateral_control(self, lac, CI, dt):
     enforce_torque_control = self.params.get_bool("EnforceTorqueControl")
     torque_versions = self.params.get("TorqueControlTune")

sunnypilot/selfdrive/controls/lib/longcontrol_ext.py

@@ -1,43 +0,0 @@
-"""
-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.
-
-Eases the final brake-pressure build as the car settles to a stop. In the stopping state the output ramps
-toward the hold accel at a fixed rate; below SETTLE_V_BP[-1] this scales that per-step build down so the
-last fraction of a m/s tapers in instead of clamping on (approach braking is untouched). This is the one
-regime where the output is intentionally softer than stock. Gated by the StopSettleSoften param (read live).
-"""
-
-import numpy as np
-
-from openpilot.common.params import Params
-from openpilot.common.realtime import DT_CTRL
-from openpilot.selfdrive.controls.lib.longcontrol import LongControl, LongCtrlState
-
-SETTLE_V_BP = [0.3, 1.2, 2.5]      # m/s: the build step is eased below the top point, full rate at/above it
-SETTLE_SCALE_V = [0.25, 0.6, 1.0]  # fraction of the per-step brake build applied across the band
-
-
-class LongControlExt(LongControl):
-  def __init__(self, CP, CP_SP, params=None):
-    super().__init__(CP, CP_SP)
-    self._params = params or Params()
-    self._frame = 0
-    self._settle_soft = self._params.get_bool("StopSettleSoften")
-
-  def update(self, active, CS, a_target, should_stop, accel_limits):
-    if self._frame % int(1.0 / DT_CTRL) == 0:
-      self._settle_soft = self._params.get_bool("StopSettleSoften")
-    self._frame += 1
-
-    prev_accel = self.last_output_accel
-    accel = super().update(active, CS, a_target, should_stop, accel_limits)
-    # Soften only while the stop ramp is adding brake (accel going more negative) from an already-coasting
-    # output, so we shrink the build step without ever turning it into throttle or reducing held brake.
-    if self._settle_soft and self.long_control_state == LongCtrlState.stopping and prev_accel <= 0.0 and accel < prev_accel:
-      scale = float(np.interp(CS.vEgo, SETTLE_V_BP, SETTLE_SCALE_V))
-      accel = float(np.clip(prev_accel + (accel - prev_accel) * scale, accel_limits[0], accel_limits[1]))
-      self.last_output_accel = accel
-    return self.last_output_accel

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

@@ -49,7 +49,7 @@ LEAD_SMOOTH_HOLD = 20          # frames (~1s): keep smoothing through brief chur
 # Stop-gap bias: near a (near-)stopped lead at low speed, report dRel up to STOP_GAP_BIAS_M closer so the MPC
 # runs its own smooth stop but terminates that much farther back (stock crawl-creeps to ~2m). Monotone (closer
 # => brake >= stock). Ramps in over the regime edge and out as the lead moves (no step, releases on launch).
-STOP_GAP_BIAS_ENABLED = False
+# Gated by the StopGapBias param, independent of the rest of the controller.
 STOP_GAP_BIAS_M = 2.0          # m: max dRel reduction = added standstill gap
 STOP_BIAS_VEGO = 8.0           # m/s: only below this ego speed
 STOP_BIAS_VLEAD = 1.5         # m/s: only behind a (near-)stopped lead; ramps out as vLead rises to this
@@ -224,7 +224,7 @@ class RadarDistanceController:
     self._frame = 0
     self._v_ego = 0.0
     self._enabled = self._params.get_bool("RadarDistance")
-    self._stop_gap_bias_enabled = STOP_GAP_BIAS_ENABLED
+    self._stop_gap_bias_enabled = self._params.get_bool("StopGapBias")
     self._lead_smooth_enabled = LEAD_SMOOTH_ENABLED
     self._one = _LeadHold()
     self._two = _LeadHold()
@@ -237,6 +237,7 @@ class RadarDistanceController:
       self._one.reset()
       self._two.reset()
     self._enabled = enabled
+    self._stop_gap_bias_enabled = self._params.get_bool("StopGapBias")
 
   def update(self, sm) -> None:
     if self._frame % int(1. / DT_MDL) == 0:
@@ -278,7 +279,8 @@ class RadarDistanceController:
   def smooth_radarstate(self, radarstate):
     self._stability.update(radarstate.leadOne, self._v_ego)   # telemetry, runs every cycle
     if not self._enabled:
-      return radarstate
+      one_b = self._stop_gap_bias(radarstate.leadOne)          # stop-gap bias works standalone; else passthrough
+      return radarstate if one_b is radarstate.leadOne else _RadarStateProxy(one_b, radarstate.leadTwo)
     one = self._one.step(radarstate.leadOne)
     two = self._two.step(radarstate.leadTwo)
     if self._v_ego < LOW_SPEED_PASSTHROUGH_V:

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

@@ -226,6 +226,13 @@ def test_stop_bias_via_smooth_radarstate_low_speed():
   out = _biased_ctrl().smooth_radarstate(rs(lead(dRel=8.0, vLead=0.0, vRel=-2.0)))
   assert out.leadOne.dRel < 8.0                                # biased proxy returned at low speed
 
+def test_stop_bias_independent_of_radar_distance():
+  c = ctrl(enabled=False)                                      # RadarDistance off ...
+  c._stop_gap_bias_enabled = True                              # ... but StopGapBias on
+  c._v_ego = 2.0
+  out = c.smooth_radarstate(rs(lead(dRel=8.0, vLead=0.0, vRel=-2.0)))
+  assert out.leadOne.dRel < 8.0                                # stop-gap still applies standalone
+
 
 # --- speed-gap bias (wider gap at speed) -------------------------------------
 

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

@@ -1,81 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-
-from types import SimpleNamespace
-
-from openpilot.selfdrive.controls.lib.longcontrol import LongControl
-from openpilot.sunnypilot.selfdrive.controls.lib.longcontrol_ext import LongControlExt, SETTLE_V_BP
-
-
-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 _CP():
-  tuning = SimpleNamespace(kpBP=[0.0], kpV=[1.0], kiBP=[0.0], kiV=[0.0])
-  return SimpleNamespace(longitudinalTuning=tuning, stopAccel=-2.0, stoppingDecelRate=0.8,
-                         startAccel=0.0, vEgoStarting=0.5, startingState=False)
-
-
-def _CS(v_ego, brake=False, standstill=False):
-  return SimpleNamespace(vEgo=v_ego, aEgo=0.0, brakePressed=brake,
-                         cruiseState=SimpleNamespace(standstill=standstill))
-
-
-CP_SP = SimpleNamespace(enableGasInterceptor=False)
-LIMITS = (-3.0, 2.0)
-
-
-def _stock():
-  return LongControl(_CP(), CP_SP)
-
-
-def _ext(enabled=True):
-  return LongControlExt(_CP(), CP_SP, params=FakeParams({"StopSettleSoften": enabled}))
-
-
-def _run(c, v_ego, frames):
-  return [c.update(True, _CS(v_ego), 0.0, True, LIMITS) for _ in range(frames)]
-
-
-def test_disabled_matches_stock():
-  assert _run(_ext(enabled=False), 0.3, 30) == _run(_stock(), 0.3, 30)  # off => byte-stock
-
-
-def test_low_speed_softens_brake_build():
-  soft = _run(_ext(), 0.3, 30)
-  stock = _run(_stock(), 0.3, 30)
-  assert soft[-1] > stock[-1]                                # softer (less brake) at the final settle
-  assert all(s >= b - 1e-9 for s, b in zip(soft, stock, strict=True))   # never harder than stock anywhere
-
-
-def test_high_speed_unchanged():
-  v = SETTLE_V_BP[-1] + 0.5                                  # above the band => full stock rate
-  assert _run(_ext(), v, 20) == _run(_stock(), v, 20)
-
-
-def test_never_adds_throttle_or_releases_brake():
-  c = _ext()
-  prev = c.last_output_accel
-  for _ in range(40):
-    a = c.update(True, _CS(0.3), 0.0, True, LIMITS)
-    assert a <= 1e-9                                         # never turns the stop into throttle
-    assert a <= prev + 1e-9                                  # only ever builds brake, never releases it
-    prev = a
-
-
-def test_only_acts_in_stopping_state():
-  # moving, not stopping => pid state => identical to stock
-  ext = _ext()
-  stock = _stock()
-  out_ext = [ext.update(True, _CS(15.0), -0.5, False, LIMITS) for _ in range(10)]
-  out_stock = [stock.update(True, _CS(15.0), -0.5, False, LIMITS) for _ in range(10)]
-  assert out_ext == out_stock

sunnypilot/sunnylink/params_metadata.json

@@ -1259,9 +1259,9 @@
     "title": "[TIZI/TICI only] Standstill Timer",
     "description": "Show a timer on the HUD when the car is at a standstill."
   },
-  "StopSettleSoften": {
+  "StopGapBias": {
     "title": "Smooth Stop",
-    "description": "Eases the brake pressure as the car settles the last few mph to a stop, so it stops with a soft taper instead of a firm catch. Only affects the final crawl, not normal braking."
+    "description": "Stops a touch farther back from a stopped lead at low speed, so sunnypilot settles smoothly instead of crawling in close. Braking is never reduced below stock."
   },
   "SubaruStopAndGo": {
     "title": "Subaru Stop and Go",

sunnypilot/sunnylink/settings_ui.json

@@ -614,10 +614,10 @@
               ]
             },
             {
-              "key": "StopSettleSoften",
+              "key": "StopGapBias",
               "widget": "toggle",
               "title": "Smooth Stop",
-              "description": "Eases the brake pressure as the car settles the last few mph to a stop, so it stops with a soft taper instead of a firm catch. Only affects the final crawl, not normal braking.",
+              "description": "Stops a touch farther back from a stopped lead at low speed, so sunnypilot settles smoothly instead of crawling in close. Braking is never reduced below stock.",
               "visibility": [
                 {
                   "type": "capability",

sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml

@@ -34,11 +34,11 @@ sections:
     - $ref: '#/macros/longitudinal'
     enablement:
     - $ref: '#/macros/longitudinal'
-  - key: StopSettleSoften
+  - key: StopGapBias
     widget: toggle
     title: Smooth Stop
-    description: Eases the brake pressure as the car settles the last few mph to a stop, so it stops with
-      a soft taper instead of a firm catch. Only affects the final crawl, not normal braking.
+    description: Stops a touch farther back from a stopped lead at low speed, so sunnypilot settles smoothly
+      instead of crawling in close. Braking is never reduced below stock.
     visibility:
     - $ref: '#/macros/longitudinal'
     enablement: