From 7fb853d3fde0aa24d3743dfb6dc4dfe376207bbe Mon Sep 17 00:00:00 2001 From: rav4kumar <36933347+rav4kumar@users.noreply.github.com> Date: Mon, 17 Aug 2026 19:05:10 -0700 Subject: [PATCH] fix(long): filter v_ego --- .../controls/lib/longitudinal_planner.py | 11 ++++++++--- .../tests/test_accel_controller.py | 16 ++++++++++++++++ 2 files changed, 24 insertions(+), 3 deletions(-) diff --git a/openpilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/selfdrive/controls/lib/longitudinal_planner.py index 44868dab45..323502a578 100755 --- a/openpilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/selfdrive/controls/lib/longitudinal_planner.py @@ -36,7 +36,7 @@ def get_coast_accel(pitch): return np.sin(pitch) * -5.65 - 0.3 # fitted from data using xx/projects/allow_throttle/compute_coast_accel.py def get_cruise_accel(e2e, v_cruise, v_ego, a_cruise_prev, angle_steers, CP, dt, accel_coast, allow_throttle, - max_accel_override=None, min_accel_override=None): + max_accel_override=None, min_accel_override=None, v_ego_filtered=None): if max_accel_override is not None: max_accel = max_accel_override else: @@ -53,7 +53,11 @@ def get_cruise_accel(e2e, v_cruise, v_ego, a_cruise_prev, angle_steers, CP, dt, coast_limit = np.interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [max_accel, clipped_accel_coast]) max_accel = min(max_accel, coast_limit) - target_accel = np.clip(v_cruise - v_ego, min_accel, max_accel) + # v_ego is noisy frame-to-frame (~0.05-0.1 m/s sensor jitter); at a held cruise speed with no + # lead this raw diff is the entire accel command, so unfiltered it reads straight through as + # gas/brake hunting. v_ego_filtered lets the caller supply an already-smoothed speed for this + # term only -- turn/coast derating above still uses the real-time v_ego on purpose. + target_accel = np.clip(v_cruise - (v_ego if v_ego_filtered is None else v_ego_filtered), min_accel, max_accel) if not e2e: 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)) @@ -154,7 +158,8 @@ class LongitudinalPlanner(LongitudinalPlannerSP): 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, steer_angle_without_offset, self.CP, self.dt, - accel_coast, self.allow_throttle, max_accel_override, min_accel_override) + accel_coast, self.allow_throttle, max_accel_override, min_accel_override, + self.v_desired_filter.x) cruise_should_stop = should_stop(v_ego, self.a_cruise) candidates = [(output_a_target_mpc, self.mpc.source, output_should_stop_mpc), 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 298fc12b81..428e2f3ac3 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 @@ -142,6 +142,22 @@ class TestOffEqualsStock(OpenpilotTestCase): args = (False, 10.0, 8.0, 0.5, 0.0, _fake_cp(), DT_MDL, 1.0, True) self.assertEqual(get_cruise_accel(*args), get_cruise_accel(*args, max_accel_override=None, min_accel_override=None)) + def test_v_ego_filtered_none_matches_no_kwarg(self): + # Regression guard: v_ego_filtered defaults to None and must not change any existing caller's output. + from openpilot.selfdrive.controls.lib.longitudinal_planner import get_cruise_accel + args = (False, 10.0, 8.0, 0.5, 0.0, _fake_cp(), DT_MDL, 1.0, True) + self.assertEqual(get_cruise_accel(*args), get_cruise_accel(*args, v_ego_filtered=None)) + + def test_v_ego_filtered_replaces_raw_v_ego_in_the_cruise_error_only(self): + # The cruise-hold term (v_cruise - v_ego) is the only thing that should move; turn/coast + # derating still has to react to the real, unfiltered v_ego (see get_cruise_accel's comment). + # dt=1.0 opens the jerk-limit window wide so it can't mask the v_ego_filtered swap below. + from openpilot.selfdrive.controls.lib.longitudinal_planner import get_cruise_accel + raw = get_cruise_accel(False, 8.6, 8.5, 0.1, 0.0, _fake_cp(), 1.0, 1.0, True) + filtered = get_cruise_accel(False, 8.6, 8.5, 0.1, 0.0, _fake_cp(), 1.0, 1.0, True, v_ego_filtered=8.3) + self.assertAlmostEqual(raw, 0.1, places=6) + self.assertAlmostEqual(filtered, 0.3, places=6) + def test_disabled_min_accel_override_is_none(self): planner = _bare_planner() self.assertIsNone(planner.get_min_accel_override(v_ego=5.0, e2e=False, force_decel=False))