diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py index 82868c8402..dd27742063 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py +++ b/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])) diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py b/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py index f81fb6a6fa..219b750c41 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py +++ b/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 diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py b/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py index e84f840f8e..9f7da942dd 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py +++ b/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(): diff --git a/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py b/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py index 1ed1ea8656..a3506fda02 100644 --- a/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py +++ b/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) diff --git a/sunnypilot/selfdrive/controls/lib/radar_distance/tests/test_radar_distance.py b/sunnypilot/selfdrive/controls/lib/radar_distance/tests/test_radar_distance.py index 87e5777611..518d006e80 100644 --- a/sunnypilot/selfdrive/controls/lib/radar_distance/tests/test_radar_distance.py +++ b/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):