diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index ff5cc1f625..10283b62c5 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -178,7 +178,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP): # 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()) + 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(v_ego)) self.output_a_target = np.clip(output_a_target, accel_clip[0], accel_clip[1]) self.prev_accel_clip = accel_clip diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py index 013614abc4..9775acf86a 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py @@ -5,7 +5,8 @@ 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 (ECO / NORMAL / SPORT). Tunes only MPC INPUTS, never the output: - * positive-accel ceiling + per-cycle open-rate -> tier-scaled take-off from a stop; + * positive-accel ceiling + speed-dependent per-cycle open-rate -> tier-scaled take-off from a stop + (the open-rate is fast near v=0 so launch is never delayed, tapering to a steady-state rate at speed); * add-only, speed-dependent follow-gap widen on the MPC t_follow -> earlier/gentler braking, roomier gap; * sticky should_stop hysteresis -> no stop-and-go gas-brake-gas-brake. Add-only gap => desired distance >= stock => braking >= stock. Disabled => stock everywhere (byte-stock). @@ -20,7 +21,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, STOCK_A_CRUISE_MAX_V, \ - RISE_RATE, STOCK_RISE_RATE, TF_WIDEN_V_BP, TF_WIDEN_BASE_V, TF_WIDEN_TIER, TF_WIDEN_MAX, \ + RISE_RATE_BP, RISE_RATE_V, STOCK_RISE_RATE, TF_WIDEN_V_BP, TF_WIDEN_BASE_V, TF_WIDEN_TIER, TF_WIDEN_MAX, \ TF_SLEW_PER_S, TF_DECEL_HOLD_A @@ -60,9 +61,12 @@ class AccelController: 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: + def get_rise_rate(self, v_ego: float) -> float: # Disabled -> stock ceiling open-rate (off == stock, independent of the NORMAL profile). - return RISE_RATE[self._personality] if self._enabled else STOCK_RISE_RATE + # Speed-dependent: fast near a stop (non-binding, no launch delay), tapering to the steady-state rate. + if not self._enabled: + return STOCK_RISE_RATE + return float(np.interp(v_ego, RISE_RATE_BP, RISE_RATE_V[self._personality])) def get_t_follow(self, t_follow: float, v_ego: float) -> float: # MPC t_follow hook. Adds a slewed, decel-held, speed-dependent comfort widen on top of the stock diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py b/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py index 61c3bd717e..687d889f53 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py @@ -28,14 +28,22 @@ A_CRUISE_MAX_BP = [0., 10., 25., 40.] # m/s (matches upstream A_CRU STOCK_A_CRUISE_MAX_V = [1.6, 1.2, 0.8, 0.6] # upstream A_CRUISE_MAX_VALS -> off == byte-stock ceiling STOCK_RISE_RATE = 0.05 # upstream ceiling open-rate (m/s^2 per cycle) A_CRUISE_MAX_V = { - ECO: [1.55, 0.95, 0.45, 0.30], # responsive off the line, LAZY at highway speed (mileage) + ECO: [1.55, 0.75, 0.35, 0.20], # responsive off the line, LAZY at highway speed (mileage) NORMAL: [2.00, 1.40, 0.95, 0.70], # brisk launch, balanced cruise SPORT: [2.00, 1.70, 1.20, 0.90], # strong launch (ACCEL_MAX caps the 0 m/s knot), assertive cruise } -# Ceiling open-rate: how fast the accel ceiling may rise per cycle. Above stock 0.05 so the permitted ceiling -# opens promptly off the line; ECO stays close to stock for the gentlest onset. The MPC's own jerk/a_change -# cost still smooths the actual accel. -RISE_RATE = {ECO: 0.07, NORMAL: 0.16, SPORT: 0.24} +# Ceiling open-rate: how fast the accel ceiling may rise per cycle, speed-dependent (decoupled from the +# ceiling magnitude above). Near a stop (v=0) the rate is set high enough to be non-binding within ~2 cycles +# (DT_MDL=0.05s @ 20Hz) so launch is never delayed waiting on the ceiling to open, regardless of personality. +# By the v=5 knot the rate settles to the telemetry-verified steady-state value (unchanged from before) so +# cruise/resume behavior at speed is preserved exactly. The MPC's own jerk/a_change cost still smooths the +# actual accel. +RISE_RATE_BP = [0., 5.] # m/s +RISE_RATE_V = { + ECO: [0.80, 0.07], + NORMAL: [1.00, 0.16], + SPORT: [1.20, 0.24], +} # --- Follow-gap widen (add-only, fed to the MPC t_follow) ------------------------------------------------ # Add a small speed-dependent widen to the stock t_follow (the driver's gap-button value). Wider gap -> 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 e84f1a0748..dfc9f359ef 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 @@ -18,7 +18,7 @@ import pytest from openpilot.common.realtime import DT_MDL 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, \ + ECO, NORMAL, SPORT, PERSONALITY_MIN, PERSONALITY_MAX, A_CRUISE_MAX_BP, RISE_RATE_V, \ STOCK_A_CRUISE_MAX_V, STOCK_RISE_RATE, TF_WIDEN_V_BP, TF_WIDEN_BASE_V, TF_WIDEN_TIER, TF_WIDEN_MAX, \ TF_SLEW_PER_S, TF_DECEL_HOLD_A, AccelerationPersonality @@ -73,7 +73,9 @@ def test_disabled_forces_normal_and_stock_ceiling(): 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 + # off == stock, regardless of v_ego (the speed-dependent open-rate table is bypassed entirely when disabled) + for v in (0.0, 5.0, 20.0, 40.0): + assert ctrl.get_rise_rate(v) == STOCK_RISE_RATE def test_disabled_t_follow_is_identity(): @@ -103,15 +105,39 @@ def test_ceiling_ordering_eco_le_normal_le_sport(): def test_rise_rate_ordering_and_above_stock(): - assert RISE_RATE[ECO] < RISE_RATE[NORMAL] < RISE_RATE[SPORT] - assert RISE_RATE[ECO] > STOCK_RISE_RATE # every tier opens the ceiling faster than stock (fast take-off) + # ordering holds at both knots: near a stop (v=0) and at the steady-state speed (v=5) + assert RISE_RATE_V[ECO][0] < RISE_RATE_V[NORMAL][0] < RISE_RATE_V[SPORT][0] + assert RISE_RATE_V[ECO][1] < RISE_RATE_V[NORMAL][1] < RISE_RATE_V[SPORT][1] + # every tier opens the ceiling faster than stock at both knots (fast take-off, never slower than stock) + assert RISE_RATE_V[ECO][0] > STOCK_RISE_RATE + assert RISE_RATE_V[ECO][1] > STOCK_RISE_RATE + + +def test_rise_rate_fast_near_stop_tapers_to_steady_state(): + # Near a stop (v=0) the open-rate must be large/non-binding (NOT the old flat 0.07/0.16/0.24) so launch + # is never delayed. At/above the v=5 knot it must match the old flat, telemetry-verified steady-state + # values exactly, so cruise/resume behavior at speed is unchanged. + for personality, steady_state in ((ECO, 0.07), (NORMAL, 0.16), (SPORT, 0.24)): + ctrl = make_controller(personality=personality) + assert ctrl.get_rise_rate(0.0) >= 0.5 + assert ctrl.get_rise_rate(0.0) > steady_state + assert ctrl.get_rise_rate(5.0) == pytest.approx(steady_state) + assert ctrl.get_rise_rate(20.0) == pytest.approx(steady_state) # flat above the v=5 knot def test_normal_is_distinct_from_stock(): nrm = make_controller(personality=NORMAL) # enabled NORMAL differs from stock (so NORMAL is a real profile, not a stock alias) assert nrm.get_max_accel(25.0) != pytest.approx(np.interp(25.0, A_CRUISE_MAX_BP, STOCK_A_CRUISE_MAX_V)) - assert nrm.get_rise_rate() != STOCK_RISE_RATE + assert nrm.get_rise_rate(0.0) != STOCK_RISE_RATE + assert nrm.get_rise_rate(5.0) != STOCK_RISE_RATE + + +def test_eco_ceiling_matches_lowered_table(): + # ECO's cruise/resume-range ceiling was lowered (launch knot at v=0 unchanged at 1.55). + eco = make_controller(personality=ECO) + for v, expected in zip(A_CRUISE_MAX_BP, (1.55, 0.75, 0.35, 0.20), strict=True): + assert eco.get_max_accel(v) == pytest.approx(expected) # --- t_follow: add-only speed widen -----------------------------------------------------------------------