mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-08-20 15:54:19 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix(long): filter v_ego

Browse Source
This commit is contained in:
rav4kumar
2026-08-17 19:05:10 -07:00
parent ec3013fda6
commit 7fb853d3fd
2 changed files with 24 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
openpilot/selfdrive/controls/lib/longitudinal_planner.py
+8 -3
View File
@@ -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),
openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py
+16
View File
@@ -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))