fix(long): decouple

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),

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)