From b98a62c1fdc4306c80d14c9b0865fb4856ee8c6a Mon Sep 17 00:00:00 2001 From: rav4kumar <36933347+rav4kumar@users.noreply.github.com> Date: Tue, 21 Jul 2026 17:56:38 -0700 Subject: [PATCH] Prevent accel shaping from starving planner inputs --- .../lib/accel_personality/accel_controller.py | 24 ++++++- .../lib/accel_personality/constants.py | 3 + .../tests/test_accel_controller.py | 62 +++++++++++++++++++ .../tests/test_accel_controller_interfaces.py | 3 +- .../controls/lib/longitudinal_planner.py | 7 ++- 5 files changed, 93 insertions(+), 6 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py index 30445d7e34..a4f103c517 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py +++ b/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 diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py b/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py index 508dcfb981..52be391802 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py +++ b/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 diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py b/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py index ee99a211cd..37d3116406 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py +++ b/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)) diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller_interfaces.py b/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller_interfaces.py index b9817fe901..48287d3477 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller_interfaces.py +++ b/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 = [] diff --git a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index f00d76b7e4..1ed0c5e8ff 100644 --- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/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)