feat(long): tfllow

common/params_keys.h

@@ -243,6 +243,9 @@ 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"}},
+
     // sunnypilot model params
     {"CameraOffset", {PERSISTENT | BACKUP, FLOAT, "0.0"}},
     {"LagdToggle", {PERSISTENT | BACKUP, BOOL, "1"}},

selfdrive/controls/controlsd.py

@@ -55,6 +55,7 @@ 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:

sunnypilot/selfdrive/controls/controlsd_ext.py

@@ -17,6 +17,7 @@ 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):
@@ -34,6 +35,10 @@ 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

@@ -0,0 +1,43 @@
+"""
+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

@@ -9,11 +9,15 @@ 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.
   - lead-jitter smoother: de-jitter a churning (trackId-flipping) lead so the MPC does not hunt the gap.
   - stop-gap bias: report a near-stopped lead slightly closer so the MPC stops a touch farther back.
+  - speed-gap bias: at higher speed report the lead slightly nearer so the MPC settles a wider following
+    gap and reacts to a lead's deceleration sooner.
 Also publishes a read-only lead-instability flag (telemetry). Default off => stock passthrough.
 """
 
 from collections import deque
 
+import numpy as np
+
 from opendbc.car import structs
 from openpilot.common.params import Params
 from openpilot.common.realtime import DT_MDL
@@ -53,6 +57,14 @@ STOP_BIAS_REGIME_DREL = 12.0   # m: bias ramps in below this dRel
 STOP_BIAS_RAMP_BAND = 2.0     # m: ramp-in band (full offset below REGIME_DREL - RAMP_BAND)
 STOP_BIAS_MIN_DREL = 2.0      # m: never report a lead closer than this
 
+# Speed-gap bias: widen the following gap at higher speed by reporting the lead a touch nearer
+# (offset_m = dt(v) * v_ego). The MPC regulates the reported gap onto its target, so the real gap settles
+# wider and braking onto a slowing lead starts sooner. Only reports nearer, so braking stays >= stock.
+SPEED_GAP_V_BP = [14.0, 28.0]          # m/s: gap widening ramps in across this band, flat above
+SPEED_GAP_TF_V = [0.0, 0.25]           # s: follow-time added to the gap at the band ends
+SPEED_GAP_MAX_M = 5.0                  # m: cap on the reported reduction
+SPEED_GAP_MIN_DREL = 3.0               # m: never report the lead nearer than this
+
 
 class _BiasedLead:
   __slots__ = ('status', 'dRel', 'yRel', 'vRel', 'vLead', 'vLeadK', 'aLeadK', 'aLeadTau', 'modelProb')
@@ -252,6 +264,17 @@ class RadarDistanceController:
       return lead
     return _BiasedLead(lead, max(lead.dRel - offset, STOP_BIAS_MIN_DREL))
 
+  def _speed_gap_bias(self, lead):
+    # Report the lead nearer at speed so the MPC holds a wider gap and brakes sooner onto a slowing lead.
+    if not lead.status:
+      return lead
+    tf = float(np.interp(self._v_ego, SPEED_GAP_V_BP, SPEED_GAP_TF_V))
+    offset = min(tf * self._v_ego, SPEED_GAP_MAX_M)
+    new_dRel = max(lead.dRel - offset, SPEED_GAP_MIN_DREL)
+    if new_dRel >= lead.dRel - 0.05:
+      return lead
+    return _BiasedLead(lead, new_dRel)
+
   def smooth_radarstate(self, radarstate):
     self._stability.update(radarstate.leadOne, self._v_ego)   # telemetry, runs every cycle
     if not self._enabled:
@@ -262,6 +285,7 @@ class RadarDistanceController:
       one_b = self._stop_gap_bias(radarstate.leadOne)         # low speed = stock lead, only the stop-gap bias
       return radarstate if one_b is radarstate.leadOne else _RadarStateProxy(one_b, radarstate.leadTwo)
     one = self._stop_gap_bias(one)
+    one = self._speed_gap_bias(one)
     if self._lead_smooth_enabled:
       one = self._smoother.update(one, self._stability.churn)  # de-jitter a churning lead (anti follow-hunt)
     return _RadarStateProxy(one, two)

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

@@ -10,7 +10,8 @@ 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
+  RadarDistanceController, HOLD_MAX_FRAMES, FCW_PROB_CAP, LOW_SPEED_PASSTHROUGH_V, \
+  SPEED_GAP_MAX_M, SPEED_GAP_MIN_DREL
 
 COMFORT_BRAKE = 2.5
 
@@ -38,7 +39,7 @@ def obstacle(ld):
 
 def ctrl(enabled=True):
   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._v_ego = 10.0  # above the low-speed gate (hold logic runs) but below the speed-gap onset (it stays inert here)
   return c
 
 
@@ -226,6 +227,48 @@ def test_stop_bias_via_smooth_radarstate_low_speed():
   assert out.leadOne.dRel < 8.0                                # biased proxy returned at low speed
 
 
+# --- speed-gap bias (wider gap at speed) -------------------------------------
+
+def _speed_ctrl(v_ego=20.0):
+  c = ctrl()                                                   # speed-gap rides on the controller being enabled
+  c._v_ego = v_ego
+  return c
+
+def test_speed_gap_reports_closer_at_speed():
+  out = _speed_ctrl(v_ego=20.0)._speed_gap_bias(lead(dRel=60.0))
+  assert out.dRel < 60.0                                        # reported closer => MPC keeps a wider real gap
+  assert out.status and out.vLead == 18.0                       # other fields preserved
+
+def test_speed_gap_monotone_never_farther():
+  c = _speed_ctrl(v_ego=25.0)
+  for dr in (10.0, 30.0, 60.0, 100.0):
+    assert c._speed_gap_bias(lead(dRel=dr)).dRel <= dr + 1e-6   # only ever closer (brake >= stock)
+
+def test_speed_gap_offset_capped():
+  out = _speed_ctrl(v_ego=40.0)._speed_gap_bias(lead(dRel=120.0))
+  assert out.dRel >= 120.0 - SPEED_GAP_MAX_M - 1e-6             # reduction never exceeds the cap
+
+def test_speed_gap_min_floor():
+  out = _speed_ctrl(v_ego=30.0)._speed_gap_bias(lead(dRel=4.0))
+  assert out.dRel >= SPEED_GAP_MIN_DREL - 1e-6                  # close cut-in not reported past the floor
+
+def test_speed_gap_off_when_controller_disabled():
+  c = ctrl(enabled=False)
+  c._v_ego = 20.0
+  r = rs(lead(dRel=60.0))
+  assert c.smooth_radarstate(r) is r                            # controller off -> no speed-gap, byte-stock
+
+def test_speed_gap_low_speed_no_change():
+  ld = lead(dRel=20.0)
+  assert _speed_ctrl(v_ego=7.0)._speed_gap_bias(ld) is ld       # below the bottom speed bp -> no offset (no step)
+
+def test_speed_gap_via_smooth_radarstate_keeps_obstacle_le():
+  c = _speed_ctrl(v_ego=22.0)
+  one = lead(dRel=70.0, vLead=20.0)
+  out = c.smooth_radarstate(rs(one))
+  assert obstacle(out.leadOne) <= obstacle(one) + 1e-6          # biased obstacle never farther (brake >= stock)
+
+
 def test_obstacle_monotone_during_hold():
   c = ctrl()
   for _ in range(3):

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

@@ -0,0 +1,81 @@
+"""
+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,6 +1259,10 @@
     "title": "[TIZI/TICI only] Standstill Timer",
     "description": "Show a timer on the HUD when the car is at a standstill."
   },
+  "StopSettleSoften": {
+    "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."
+  },
   "SubaruStopAndGo": {
     "title": "Subaru Stop and Go",
     "description": ""

sunnypilot/sunnylink/settings_ui.json

@@ -597,7 +597,27 @@
               "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.",
+              "description": "Holds a lead through brief radar flicker/dropout so sunnypilot does not lose and re-grab it, and at higher speed keeps a slightly wider following gap so it starts slowing sooner when the lead does. Smooths 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": "StopSettleSoften",
+              "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.",
               "visibility": [
                 {
                   "type": "capability",

sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml

@@ -28,7 +28,17 @@ sections:
     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.
+      it, and at higher speed keeps a slightly wider following gap so it starts slowing sooner when the
+      lead does. Smooths the hard/late brakes that radar drop-outs cause. Braking is never reduced below stock.
+    visibility:
+    - $ref: '#/macros/longitudinal'
+    enablement:
+    - $ref: '#/macros/longitudinal'
+  - key: StopSettleSoften
+    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.
     visibility:
     - $ref: '#/macros/longitudinal'
     enablement: