tune

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

@@ -16,6 +16,7 @@ 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, SMOOTH_DECEL_BP, \
+  STOCK_A_CRUISE_MAX_V, STOCK_RISE_RATE, \
   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, STOP_IMMINENT_VEGO, STOP_IMMINENT_LOOKAHEAD_T, \
   ONSET_JERK0, ONSET_JERK_GAIN, ONSET_GAP_SOFT, ONSET_GAP_GAIN, ONSET_JERK_MAX, ONSET_HANDBACK_JERK, \
@@ -59,10 +60,13 @@ class AccelController:
     self._frame += 1
 
   def get_max_accel(self, v_ego: float) -> float:
-    return float(np.interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_V[self._personality]))
+    # 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:
-    return RISE_RATE[self._personality]
+    # 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]))

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

@@ -15,20 +15,20 @@ SPORT = AccelerationPersonality.sport
 PERSONALITY_MIN = min(AccelerationPersonality.schema.enumerants.values())
 PERSONALITY_MAX = max(AccelerationPersonality.schema.enumerants.values())
 
-# Accel ceiling. NORMAL is stock so a disabled controller (forced to NORMAL) is stock.
-# This is the POSITIVE-accel upper clip + its upward slew rate. It is the launch/cruise-accel side and
-# is independent of braking (which is the lower clip + the convex/SMOOTH_DECEL shaper) -- tuning it does
-# NOT change the gentle-brake goals. ECO launch (v=0) matches stock + stock rise rate so take-off from
-# a stop is prompt (no honking); only the 25/40 m/s cruise points stay gentle.
+# Accel ceiling + its upward slew rate (the POSITIVE-accel / launch + cruise-accel side; independent of
+# braking, so tuning here does NOT touch the gentle-brake goals). off==stock is enforced in accel_controller
+# (get_max_accel/get_rise_rate fall back to STOCK_* when disabled), so the NORMAL profile is free to differ
+# from stock -- all three tiers are now distinct. Start-from-stop is FAST: launch peak (v=0) is firm and the
+# rise rate (how fast the ceiling opens) is well above stock 0.05 in every tier, stepped ECO < NORMAL < SPORT.
 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.60, 0.60, 0.13, 0.05],   # stock launch (v=0), gentle cruise (25/40)
-  NORMAL: STOCK_A_CRUISE_MAX_V,
-  SPORT:  [1.90, 1.30, 0.60, 0.25],
+  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.05, NORMAL: STOCK_RISE_RATE, SPORT: 0.06}   # ECO rise = stock so launch ramps promptly
+RISE_RATE = {ECO: 0.10, NORMAL: 0.15, SPORT: 0.22}   # ceiling open-rate: all >> stock 0.05 for fast take-off
 
 # Early soft braking: predicted brake need (m/s^2) -> early decel target (m/s^2). Front-loads a gentle
 # decel as soon as the 3s plan lookahead predicts a brake, so decel is spread out instead of arriving as
@@ -44,7 +44,7 @@ SMOOTH_DECEL_V = {
 }
 BRAKE_DEEPENING_JERK = {ECO: 0.5, NORMAL: 0.8, SPORT: 1.0}
 BRAKE_RELEASE_JERK = 2.0
-ACCEL_RISE_JERK = {ECO: 0.7, NORMAL: 1.2, SPORT: 1.6}
+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.2

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

@@ -77,21 +77,21 @@ def test_disabled_passes_brake_through():
     assert out == pytest.approx(raw, abs=_EPS)
 
 
-def test_normal_matches_stock():
+def test_normal_is_distinct_from_stock():
+  # off==stock is enforced via the disabled path (see test_disabled_forces_normal_and_stock_ceiling), NOT by
+  # NORMAL==stock. So the enabled NORMAL tier is free to differ from stock -- and now does.
   ctrl = make_controller(personality=NORMAL)
-  for v in (0.0, 5.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
+  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_le_normal_lt_sport():
-  # ECO launch (v=0) intentionally matches stock for prompt take-off; ECO is strictly gentler only at
-  # cruise speeds. So ECO <= NORMAL everywhere, strictly below at cruise; SPORT strictly above NORMAL.
+def test_ceiling_ordering_eco_lt_normal_lt_sport():
+  # All three tiers are distinct: ECO < NORMAL < SPORT at every speed (launch and cruise). Each launches
+  # promptly (peak + rise rate above stock), stepped by tier.
   eco, normal, sport = (make_controller(personality=p) for p in (ECO, NORMAL, SPORT))
-  for v in (0.0, 10.0, 25.0, 40.0):
-    assert eco.get_max_accel(v) <= normal.get_max_accel(v) < sport.get_max_accel(v)
-  for v in (14.0, 25.0, 40.0):
-    assert eco.get_max_accel(v) < normal.get_max_accel(v)
+  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():

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

@@ -19,6 +19,13 @@ DROPOUT_DREL = 1.0
 FCW_PROB_CAP = 0.9       # held lead can't reach the FCW gate (>0.9) -> no false FCW
 MIN_HELD_DREL = 0.5
 
+# Roomier stops. Stock long MPC targets STOP_DISTANCE (6m) but soft-coasts in to ~3.5-4m on a real stop,
+# which feels close. We pull the reported lead in by STOP_MARGIN as the car slows so the MPC settles that
+# much farther back. Scoped to the stop regime by a v_ego ramp (full at/below STOP_MARGIN_V[0], zero at/above
+# STOP_MARGIN_V[1]) so it never touches mid/high-speed following. Set STOP_MARGIN=0 to disable.
+STOP_MARGIN = 1.5        # m extra standstill/low-speed gap
+STOP_MARGIN_V = (1.0, 4.0)  # m/s ramp: full <=1.0, none >=4.0
+
 
 class _HeldLead:
   __slots__ = ('status', 'dRel', 'yRel', 'vRel', 'vLead', 'vLeadK', 'aLeadK', 'aLeadTau', 'modelProb')
@@ -43,6 +50,23 @@ class _RadarStateProxy:
     self.leadTwo = lead_two
 
 
+class _LeadView:
+  # mirror the fields the MPC reads from a lead, with dRel pulled in by `pull` (>=0 m) so the car keeps that
+  # much extra gap. Only used in the low-speed stop regime (see STOP_MARGIN); high-speed leads pass through.
+  __slots__ = ('status', 'dRel', 'yRel', 'vRel', 'vLead', 'vLeadK', 'aLeadK', 'aLeadTau', 'modelProb')
+
+  def __init__(self, src, pull):
+    self.status = src.status
+    self.dRel = max(MIN_HELD_DREL, src.dRel - pull)
+    self.yRel = src.yRel
+    self.vRel = src.vRel
+    self.vLead = src.vLead
+    self.vLeadK = src.vLeadK
+    self.aLeadK = src.aLeadK
+    self.aLeadTau = src.aLeadTau
+    self.modelProb = src.modelProb
+
+
 class _LeadHold:
   def __init__(self):
     self._last = None
@@ -85,6 +109,7 @@ class RadarDistanceController:
     self._CP = CP
     self._params = params or Params()
     self._frame = 0
+    self._v_ego = 0.0
     self._enabled = self._params.get_bool("RadarDistance")
     self._one = _LeadHold()
     self._two = _LeadHold()
@@ -99,12 +124,29 @@ class RadarDistanceController:
   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 _stop_margin(self) -> float:
+    if STOP_MARGIN <= 0.0:
+      return 0.0
+    lo, hi = STOP_MARGIN_V
+    frac = (hi - self._v_ego) / (hi - lo)
+    frac = 0.0 if frac < 0.0 else (1.0 if frac > 1.0 else frac)
+    return STOP_MARGIN * frac
+
   def smooth_radarstate(self, radarstate):
     if not self._enabled:
       return radarstate
-    return _RadarStateProxy(self._one.step(radarstate.leadOne), self._two.step(radarstate.leadTwo))
+    one = self._one.step(radarstate.leadOne)
+    two = self._two.step(radarstate.leadTwo)
+    pull = self._stop_margin()
+    if pull > 0.0:
+      if one.status:
+        one = _LeadView(one, pull)
+      if two.status:
+        two = _LeadView(two, pull)
+    return _RadarStateProxy(one, two)

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

@@ -10,7 +10,7 @@ from types import SimpleNamespace
 import pytest
 
 from openpilot.sunnypilot.selfdrive.controls.lib.radar_distance.radar_distance import \
-  RadarDistanceController, HOLD_MAX_FRAMES, FCW_PROB_CAP
+  RadarDistanceController, HOLD_MAX_FRAMES, FCW_PROB_CAP, STOP_MARGIN, STOP_MARGIN_V
 
 COMFORT_BRAKE = 2.5
 
@@ -37,7 +37,9 @@ def obstacle(ld):
 
 
 def ctrl(enabled=True):
-  return RadarDistanceController(CP=SimpleNamespace(), params=FakeParams({'RadarDistance': enabled}))
+  c = RadarDistanceController(CP=SimpleNamespace(), params=FakeParams({'RadarDistance': enabled}))
+  c._v_ego = STOP_MARGIN_V[1] + 10.0  # default to a no-margin (cruise) speed so hold-logic tests are isolated
+  return c
 
 
 def test_disabled_is_identity():
@@ -83,6 +85,23 @@ def test_low_speed_override_lead_arms_hold():
   assert held.status is True                         # armed off the prob=0 lead, holds through dropout
 
 
+def test_stop_margin_pulls_lead_in_near_stop():
+  # at standstill the reported lead is pulled in by STOP_MARGIN so the MPC keeps extra real gap (roomier stop)
+  c = ctrl()
+  c._v_ego = STOP_MARGIN_V[0]            # full-margin regime
+  out = c.smooth_radarstate(rs(lead(status=True, dRel=6.0, vRel=0.0, vLead=0.0)))
+  assert out.leadOne.dRel == pytest.approx(6.0 - STOP_MARGIN, abs=1e-6)
+
+
+def test_stop_margin_off_at_speed():
+  # no margin at cruise speed -> untouched passthrough (full-fidelity real lead to the MPC)
+  c = ctrl()
+  c._v_ego = STOP_MARGIN_V[1] + 5.0
+  one = lead(status=True, dRel=40.0)
+  out = c.smooth_radarstate(rs(one))
+  assert out.leadOne is one
+
+
 def test_obstacle_monotone_during_hold():
   c = ctrl()
   for _ in range(3):