Prevent accel shaping from starving planner inputs

sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py

@@ -15,7 +15,8 @@ from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants imp
   ACCEL_PROFILE_MAX_BP, ACCEL_PROFILE_MAX_V, APPROACH_CLOSING_SPEED, APPROACH_LEAD_DECEL, APPROACH_LEAD_SPEED_MARGIN, APPROACH_MIN_SPEED,
   BRAKE_CAP_MARGIN, CAP_FILTER_FRAMES, CAP_RELAX_JERK, CAP_TIGHTEN_JERK, COAST_MATCH_CLOSING_SPEED, COAST_MATCH_USABLE_GAP,
   DROPOUT_ACTION_ACCEL_MARGIN, HORIZON_DOWN_JERK, HORIZON_HOLD_TIME, HORIZON_SPEED_BUDGET, HORIZON_UP_JERK, MAX_LEAD_ACCEL_TAU,
-  MIN_LEAD_SPEED, POSITIVE_MPC_HEADROOM, PROFILE_CONFIGS, PROFILE_TRANSITION_JERK, RADAR_STALE_TIMEOUT, RELIEF_CAP_MARGIN,
+  MIN_LEAD_SPEED, POSITIVE_MPC_ALWAYS_ACTIVE_SPEED, POSITIVE_MPC_ENTER_MARGIN, POSITIVE_MPC_EXIT_MARGIN, POSITIVE_MPC_HEADROOM, PROFILE_CONFIGS,
+  PROFILE_TRANSITION_JERK, RADAR_STALE_TIMEOUT, RELIEF_CAP_MARGIN,
   RELIEF_CONFIRM_FRAMES, RELIEF_LEAD_SPEED_STEP, RELIEF_MPC_JERK, REQUIRED_DECEL_MARGIN, ROUTINE_DECEL_MAX, STOP_HOLD_EGO_SPEED,
   SHALLOW_BRAKE_BOUND, SHALLOW_BRAKE_RELIEF_TIME, STOP_GAP_RESERVE, STOP_GAP_RESERVE_DECEL_BP, STOP_GAP_RESERVE_LEAD_SPEED,
   STOP_HOLD_CREEP_ABORT_FRAMES, STOP_HOLD_CREEP_DISTANCE, STOP_HOLD_CREEP_SPEED,
@@ -102,6 +103,8 @@ class _ControllerPath:
   departing_from_stop: bool = False
   previous_lead_speed: float | None = None
   lead_speed_relief: bool = False
+  positive_limit_active: bool = False
+  positive_limit_initialized: bool = False
 
   @property
   def filtered_cap(self) -> float:
@@ -139,6 +142,8 @@ class _ControllerPath:
     self.departing_from_stop = False
     self.previous_lead_speed = None
     self.lead_speed_relief = False
+    self.positive_limit_active = False
+    self.positive_limit_initialized = False
 
 
 class AccelController:
@@ -592,8 +597,23 @@ class AccelController:
       effective_accel_max = float(np.clip(self.live.bound, ACCEL_MIN, ACCEL_MAX))
       if self.live.bound_relief_frames and self.live.lead_speed_relief:
         effective_accel_max = min(effective_accel_max, action_accel + RELIEF_MPC_JERK * self.dt)
-      mpc_accel_max = self._build_accel_ceiling(effective_accel_max, sanitized_v_ego, planner_accel, self._delay())
+      if effective_accel_max > 0.0:
+        if radar_fresh:
+          margin = POSITIVE_MPC_EXIT_MARGIN if self.live.positive_limit_active else POSITIVE_MPC_ENTER_MARGIN
+          self.live.positive_limit_active = (not self.live.positive_limit_initialized
+                                             or sanitized_v_ego <= POSITIVE_MPC_ALWAYS_ACTIVE_SPEED
+                                             or envelope.selected_lead >= 0
+                                             or math.isfinite(self.live.filtered_cap)
+                                             or max(planner_accel, action_accel) >= effective_accel_max - margin)
+          self.live.positive_limit_initialized = True
+      else:
+        self.live.positive_limit_active = False
+        self.live.positive_limit_initialized = False
+      mpc_accel_max = (self._build_accel_ceiling(effective_accel_max, sanitized_v_ego, planner_accel, self._delay())
+                       if effective_accel_max <= 0.0 or self.live.positive_limit_active else None)
     else:
+      self.live.positive_limit_active = False
+      self.live.positive_limit_initialized = False
       effective_accel_max = math.inf
       mpc_accel_max = None
 

sunnypilot/selfdrive/controls/lib/accel_personality/constants.py

@@ -60,6 +60,9 @@ RELIEF_LEAD_SPEED_STEP = 0.05
 DROPOUT_ACTION_ACCEL_MARGIN = 0.08
 PROFILE_TRANSITION_JERK = 1.50
 POSITIVE_MPC_HEADROOM = 0.02
+POSITIVE_MPC_ENTER_MARGIN = 0.10
+POSITIVE_MPC_EXIT_MARGIN = 0.20
+POSITIVE_MPC_ALWAYS_ACTIVE_SPEED = 3.0
 URGENT_CLOSING_SPEED = 12.0
 URGENT_REQUIRED_DECEL = 1.0
 URGENT_TTC = 3.2

sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py

@@ -115,6 +115,43 @@ class TestProfiles:
     assert result.effective_accel_max == 0.0
     np.testing.assert_array_equal(result.mpc_accel_max, 0.0)
 
+  def test_nonbinding_clear_road_ceiling_releases_after_initialization(self):
+    controller = make_controller()
+    initial = update(controller, v_ego=10.0)
+    released = update(controller, v_ego=10.0)
+
+    assert initial.mpc_accel_max is not None
+    assert released.mpc_accel_max is None
+    assert released.effective_accel_max == released.profile_accel_max
+
+  def test_positive_ceiling_uses_demand_hysteresis(self):
+    controller = make_controller()
+    initial = update(controller, v_ego=10.0)
+    bound = initial.effective_accel_max
+    released = update(controller, v_ego=10.0)
+    entered = update(controller, v_ego=10.0, planner_accel=bound - 0.09)
+    retained = update(controller, v_ego=10.0, planner_accel=bound - 0.15)
+    exited = update(controller, v_ego=10.0, planner_accel=bound - 0.21)
+
+    assert released.mpc_accel_max is None
+    assert entered.mpc_accel_max is not None
+    assert retained.mpc_accel_max is not None
+    assert exited.mpc_accel_max is None
+
+  def test_low_speed_and_valid_leads_keep_positive_ceiling_active(self):
+    low_speed_controller = make_controller()
+    update(low_speed_controller, v_ego=3.0)
+    low_speed = update(low_speed_controller, v_ego=3.0)
+
+    lead_controller = make_controller()
+    far_lead = make_radar(make_lead(status=True, d_rel=200.0, v_lead_k=10.0))
+    for _ in range(CAP_FILTER_FRAMES):
+      with_lead = update(lead_controller, far_lead, v_ego=10.0)
+
+    assert low_speed.mpc_accel_max is not None
+    assert with_lead.state == AccelControllerState.free
+    assert with_lead.mpc_accel_max is not None
+
   def test_profile_switch_changes_ceiling_without_a_step(self):
     controller = make_controller()
     sport = update(controller, profile=AccelProfile.sport, v_ego=10.0)
@@ -404,6 +441,31 @@ class TestAccelControllerState:
     timed_out = update(controller, radar_fresh=False)
     assert not timed_out.active and timed_out.mpc_accel_max is None
 
+  def test_stale_radar_preserves_positive_lead_ceiling_until_timeout(self):
+    controller = make_controller()
+    far_lead = make_radar(make_lead(status=True, d_rel=200.0, v_lead_k=10.0))
+    for _ in range(CAP_FILTER_FRAMES):
+      active = update(controller, far_lead, v_ego=10.0)
+    hold_frames = math.ceil(RADAR_STALE_TIMEOUT / DT_MDL) - 1
+    frozen = [update(controller, far_lead, radar_fresh=False, v_ego=10.0) for _ in range(hold_frames)]
+
+    assert active.state == AccelControllerState.free and active.mpc_accel_max is not None
+    assert all(result.active and result.mpc_accel_max is not None for result in frozen)
+    timed_out = update(controller, far_lead, radar_fresh=False, v_ego=10.0)
+    assert not timed_out.active and timed_out.mpc_accel_max is None
+
+  def test_filtered_lead_dropout_guard_preserves_positive_ceiling(self):
+    controller = make_controller()
+    far_lead = make_radar(make_lead(status=True, d_rel=200.0, v_lead_k=10.0))
+    for _ in range(CAP_FILTER_FRAMES):
+      with_lead = update(controller, far_lead, v_ego=10.0)
+    guarded = [update(controller, v_ego=10.0) for _ in range(CAP_FILTER_FRAMES // 2)]
+    released = update(controller, v_ego=10.0)
+
+    assert with_lead.state == AccelControllerState.free and with_lead.mpc_accel_max is not None
+    assert all(result.mpc_accel_max is not None for result in guarded)
+    assert released.mpc_accel_max is None
+
   def test_stale_radar_preserves_urgent_stock_passthrough_until_timeout(self):
     controller = make_controller()
     urgent = make_radar(make_lead(status=True, d_rel=18.0, v_lead_k=0.0))

sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller_interfaces.py

@@ -182,7 +182,8 @@ def test_e2e_to_acc_handoff_preserves_braking_only_when_controller_is_active(con
   planner.update_accel_controller = lambda *_args, **_kwargs: setattr(
     planner, "accel_controller_result",
     SimpleNamespace(enabled=controller_active, active=controller_active, shadow_active=controller_active,
-                    stock_mode=not controller_active, target_speed=20.0, mpc_accel_max=None,
+                    stock_mode=not controller_active, target_speed=20.0,
+                    mpc_accel_max=(0.8,) * (N + 1) if controller_active else None,
                     state=AccelControllerState.free, effective_accel_max=0.8 if controller_active else np.inf),
   )
   calls = []

sunnypilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -17,7 +17,7 @@ from openpilot.selfdrive.car.cruise import V_CRUISE_MAX
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import N, T_IDXS
 from openpilot.sunnypilot import get_sanitize_int_param
 from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality import AccelController, AccelControllerState, AccelProfile
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import MPC_SEED_RISE_RATE
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import MPC_SEED_RISE_RATE, POSITIVE_MPC_ALWAYS_ACTIVE_SPEED
 from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
 from openpilot.sunnypilot.selfdrive.controls.lib.e2e_alerts_helper import E2EAlertsHelper
 from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.smart_cruise_control import SmartCruiseControl
@@ -187,8 +187,9 @@ class LongitudinalPlannerSP:
     self._previous_is_e2e = is_e2e and handoff_context
     controller_actuating = result.active and not result.stock_mode and not force_decel
     accel_max = result.mpc_accel_max if controller_actuating else None
-    free_profile_limit = controller_actuating and result.state == AccelControllerState.free and result.effective_accel_max > 0.0
-    seed_target = result.effective_accel_max if free_profile_limit and handoff_accel is None else None
+    free_profile_context = controller_actuating and result.state == AccelControllerState.free and result.effective_accel_max > 0.0
+    positive_profile_mpc = free_profile_context and (accel_max is not None or sm['carState'].vEgo <= POSITIVE_MPC_ALWAYS_ACTIVE_SPEED)
+    seed_target = result.effective_accel_max if positive_profile_mpc and handoff_accel is None else None
     custom_mpc = handoff_accel is not None or (controller_actuating and (accel_max is not None or seed_target is not None))
     retry_state = (self.mpc.a_prev.copy(), self.mpc.crash_cnt)
     controller_v_cruise = min(mpc_v_cruise, result.target_speed)