From 0ef6fa48d768fcbac3c1ba4ebf90dc5dc46d0b0c Mon Sep 17 00:00:00 2001 From: rav4kumar <36933347+rav4kumar@users.noreply.github.com> Date: Mon, 17 Aug 2026 19:24:00 -0700 Subject: [PATCH] fix(long): decouple --- .../controls/lib/longitudinal_planner.py | 8 +- .../controls/tests/test_v_ego_filter.py | 103 ++++++++++++++++++ 2 files changed, 110 insertions(+), 1 deletion(-) create mode 100644 openpilot/selfdrive/controls/tests/test_v_ego_filter.py diff --git a/openpilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/selfdrive/controls/lib/longitudinal_planner.py index 323502a578..183a43b0cf 100755 --- a/openpilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/selfdrive/controls/lib/longitudinal_planner.py @@ -76,6 +76,10 @@ class LongitudinalPlanner(LongitudinalPlannerSP): self.a_desired = init_a self.v_desired_filter = FirstOrderFilter(init_v, 2.0, self.dt) + # v_desired_filter.x is a trajectory-integration state (fed back from output_a_target each + # frame, see below) not a clean v_ego low-pass -- keep the cruise-hold error term's smoothing + # on its own filter so it can't pick up feedback from the planner's own output. + self.v_ego_filter = FirstOrderFilter(init_v, 2.0, self.dt) self.a_cruise = 0.0 self.output_a_target = 0.0 self.output_should_stop = False @@ -116,10 +120,12 @@ class LongitudinalPlanner(LongitudinalPlannerSP): if reset_state: self.v_desired_filter.x = v_ego + self.v_ego_filter.x = v_ego self.a_desired = np.clip(sm['carState'].aEgo, ACCEL_MIN, ACCEL_MAX) # Prevent divergence, smooth in current v_ego self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego)) + self.v_ego_filter.x = max(0.0, self.v_ego_filter.update(v_ego)) # No change cost when user is controlling the speed, or when standstill prev_accel_constraint = not (reset_state or sm['carState'].standstill) @@ -159,7 +165,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP): 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, - self.v_desired_filter.x) + self.v_ego_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/selfdrive/controls/tests/test_v_ego_filter.py b/openpilot/selfdrive/controls/tests/test_v_ego_filter.py new file mode 100644 index 0000000000..6bc315ab11 --- /dev/null +++ b/openpilot/selfdrive/controls/tests/test_v_ego_filter.py @@ -0,0 +1,103 @@ +from typing import cast + +from openpilot.cereal import custom, messaging +from opendbc.car import structs +from openpilot.common.test import OpenpilotTestCase +from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner +from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController + +V_EGO = 20.0 +E2E_ACCEL = -3.0 # sustained hard braking candidate, picked to win the min() every frame + + +class MockDec: + def __init__(self): + pass + + def update(self, sm): + pass + + def active(self) -> bool: + return False + + def mode(self) -> str: + return "acc" + + def enabled(self) -> bool: + return True + + +class MockSubMaster(dict): + def __init__(self, services: dict): + super().__init__(services) + self.valid = dict.fromkeys(services, True) + self.logMonoTime = dict.fromkeys(services, 0) + self.updated = dict.fromkeys(services, True) + self.recv_frame = dict.fromkeys(services, 1) + + def all_checks(self, service_list=None) -> bool: + return True + + +def build_sm(experimental_mode: bool) -> MockSubMaster: + services = {} + for service in ("radarState", "controlsState", "vehicleParameters", "carStateSP", + "liveMapDataSP", "gpsLocationExternal", "gpsLocation"): + services[service] = getattr(messaging.new_message(service), service) + + car_state = messaging.new_message('carState') + car_state.carState.vEgo = V_EGO + car_state.carState.vCruise = 100.0 + car_state.carState.vCruiseCluster = 100.0 + services['carState'] = car_state.carState.as_reader() + + selfdrive_state = messaging.new_message('selfdriveState') + selfdrive_state.selfdriveState.experimentalMode = experimental_mode + selfdrive_state.selfdriveState.enabled = True + services['selfdriveState'] = selfdrive_state.selfdriveState.as_reader() + + car_control = messaging.new_message('carControl') + car_control.carControl.enabled = True + services['carControl'] = car_control.carControl.as_reader() + + model = messaging.new_message('modelV2') + model.modelV2.orientationRate.z = [0.01] * 33 # nonzero: a straight path divides by zero in SCC vision + model.modelV2.velocity.x = [V_EGO] * 33 + model.modelV2.position.x = [float(i) for i in range(33)] + model.modelV2.action.desiredAcceleration = E2E_ACCEL + services['modelV2'] = model.modelV2.as_reader() + + return MockSubMaster(services) + + +def build_planner() -> LongitudinalPlanner: + CP = structs.CarParams() + CP.steerRatio = 15.0 + CP.wheelbase = 2.7 + CP.longitudinalActuatorDelay = 0.2 + CP_SP = custom.CarParamsSP.new_message().as_reader() + + planner = LongitudinalPlanner(CP, CP_SP, init_v=V_EGO) + planner.dec = cast(DynamicExperimentalController, MockDec()) + return planner + + +class TestVEgoFilter(OpenpilotTestCase): + """ + v_desired_filter.x is a trajectory-integration state, not a plain v_ego low-pass -- its last + update()-line nudges it by output_a_target/a_prev every frame, so a sustained e2e/mpc + candidate that wins the min() can drag it away from a real, held-constant v_ego. get_cruise_ + accel's cruise-hold error term must be smoothed by the dedicated v_ego_filter instead, which + only ever sees v_ego and can't pick up that feedback. + """ + + def test_v_ego_filter_tracks_constant_v_ego_even_when_output_a_target_diverges(self): + planner = build_planner() + sm = build_sm(experimental_mode=True) # e2e candidate (E2E_ACCEL) wins the min() every frame + for _ in range(300): # 15s at DT_MDL=0.05 -- several v_desired_filter time constants + planner.update(sm) + + # the trajectory-integration state got dragged well away from the real, constant v_ego + self.assertLess(planner.v_desired_filter.x, V_EGO - 1.0) + # the dedicated filter, seeing only v_ego, stays converged on it regardless + self.assertAlmostEqual(planner.v_ego_filter.x, V_EGO, places=1)