From d56679b2e3cbcf20077099c563e8e5249edef418 Mon Sep 17 00:00:00 2001 From: rav4kumar <36933347+rav4kumar@users.noreply.github.com> Date: Tue, 18 Aug 2026 23:26:22 -0700 Subject: [PATCH] fix(accel): deacel floor --- .../controls/lib/longitudinal_planner.py | 12 +++++++++--- .../lib/accel_controller/accel_controller.py | 9 ++++++++- .../tests/test_accel_controller.py | 16 +++++++++++++--- 3 files changed, 30 insertions(+), 7 deletions(-) diff --git a/openpilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/selfdrive/controls/lib/longitudinal_planner.py index 22e68d5545..fd9516644b 100755 --- a/openpilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/selfdrive/controls/lib/longitudinal_planner.py @@ -54,10 +54,17 @@ def get_cruise_accel(e2e, v_cruise, v_ego, a_cruise_prev, angle_steers, CP, dt, max_accel = min(max_accel, coast_limit) target_accel = np.clip(v_cruise - v_ego, min_accel, max_accel) + + # An override only counts as "active" if it's the bound that actually determined target_accel here -- + # turn/coast derating can shrink max_accel back below max_accel_override, and either bound can simply + # not be reached if v_cruise - v_ego already sits inside [min_accel, max_accel] on its own. + accel_controller_active = ((max_accel_override is not None and max_accel == max_accel_override and target_accel == max_accel) or + (min_accel_override is not None and min_accel == min_accel_override and target_accel == min_accel)) + j_cruise = np.interp(v_ego, A_CRUISE_MAX_BP, J_CRUISE_VALS) target_accel = float(np.clip(target_accel, a_cruise_prev - j_cruise * dt, a_cruise_prev + j_cruise * dt)) - return target_accel + return target_accel, accel_controller_active class LongitudinalPlanner(LongitudinalPlannerSP): @@ -149,8 +156,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP): max_accel_override = self.get_max_accel_override(v_ego) min_accel_override = self.get_min_accel_override(v_ego, is_e2e, force_decel) - self.accel_controller_active = max_accel_override is not None or min_accel_override is not None - self.a_cruise = get_cruise_accel(is_e2e, v_cruise, v_ego, + self.a_cruise, self.accel_controller_active = get_cruise_accel(is_e2e, v_cruise, v_ego, self.a_cruise, steer_angle_without_offset, self.CP, self.dt, accel_coast, self.allow_throttle, max_accel_override, min_accel_override) cruise_should_stop = should_stop(v_ego, self.a_cruise) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py index 64e050b748..f7b0a6854d 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py @@ -38,6 +38,7 @@ class AccelController: self.last_max_accel = 2.0 self.last_min_accel = -0.01 self.first_run = True + self.min_accel_first_run = True self._profile = get_sanitize_int_param("AccelPersonality", AccelProfile.eco, AccelProfile.sport, self.params) self._enabled = self.params.get_bool("AccelPersonalityEnabled") @@ -69,6 +70,12 @@ class AccelController: def get_min_accel(self, v_ego: float) -> float: v_ego = max(0.0, v_ego) target_min = np.interp(v_ego, MIN_ACCEL_BREAKPOINTS, MIN_ACCEL_PROFILES[self._profile]) - self.last_min_accel = DECEL_SMOOTH_ALPHA * target_min + (1 - DECEL_SMOOTH_ALPHA) * self.last_min_accel + + if self.min_accel_first_run: + self.last_min_accel = target_min + self.min_accel_first_run = False + else: + self.last_min_accel = DECEL_SMOOTH_ALPHA * target_min + (1 - DECEL_SMOOTH_ALPHA) * self.last_min_accel + self.last_min_accel = min(self.last_min_accel, self.last_max_accel - 0.1) return float(self.last_min_accel) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py index 1a8572efd6..c7bb622e74 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py @@ -23,7 +23,7 @@ from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL from openpilot.common.test import OpenpilotTestCase from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import ( - AccelController, AccelProfile, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES, MIN_ACCEL_PROFILES, + AccelController, AccelProfile, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES, MIN_ACCEL_BREAKPOINTS, MIN_ACCEL_PROFILES, ) @@ -39,6 +39,14 @@ class TestAccelControllerCeiling(OpenpilotTestCase): expected_max = np.interp(20.0, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[AccelProfile.normal]) self.assertAlmostEqual(max_a, expected_max, places=6) + def test_min_accel_first_call_snaps_to_table_not_the_neg0p01_seed(self): + # Regression guard: get_min_accel used to have no first-run snap (unlike get_max_accel), + # so its very first call blended the table target against a hardcoded -0.01 seed and + # commanded a much-weaker-than-any-profile floor for the first ~10-15 frames of every drive. + min_a = self.controller.get_min_accel(20.0) + expected_min = np.interp(20.0, MIN_ACCEL_BREAKPOINTS, MIN_ACCEL_PROFILES[AccelProfile.normal]) + self.assertAlmostEqual(min_a, expected_min, places=6) + def test_table_lookup_matches_breakpoints_per_profile(self): for profile, table in MAX_ACCEL_PROFILES.items(): self.params.put("AccelPersonality", profile, block=True) @@ -181,8 +189,9 @@ class TestOffEqualsStock(OpenpilotTestCase): from openpilot.selfdrive.controls.lib.longitudinal_planner import get_cruise_accel, A_CRUISE_MIN args = {"v_cruise": -100.0, "v_ego": 20.0, "a_cruise_prev": 0.0, "angle_steers": 0.0, "CP": _fake_cp(), "dt": 10.0, "accel_coast": 1.0, "allow_throttle": True} - target = get_cruise_accel(True, **args, min_accel_override=-0.3) + target, active = get_cruise_accel(True, **args, min_accel_override=-0.3) self.assertAlmostEqual(target, A_CRUISE_MIN, places=6) + self.assertFalse(active) # controller's floor was ignored in favor of stock -- not "active" def test_blended_max_accel_uses_controller_override(self): # jerk-limiting now applies unconditionally (even in e2e) -- dt=10.0 opens the jerk-limit @@ -190,8 +199,9 @@ class TestOffEqualsStock(OpenpilotTestCase): from openpilot.selfdrive.controls.lib.longitudinal_planner import get_cruise_accel args = {"v_cruise": 100.0, "v_ego": 20.0, "a_cruise_prev": 0.0, "angle_steers": 0.0, "CP": _fake_cp(), "dt": 10.0, "accel_coast": 1.0, "allow_throttle": True} - target = get_cruise_accel(True, **args, max_accel_override=0.4) + target, active = get_cruise_accel(True, **args, max_accel_override=0.4) self.assertAlmostEqual(target, 0.4, places=6) + self.assertTrue(active)