diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py index 7902354818..f1e88e11d0 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py @@ -74,6 +74,8 @@ CLEAR_LAUNCH_ACCEL_RATE = 3.0 INITIAL_LAUNCH_ACCEL_MAX = 0.95 BREAKAWAY_ACCEL_MAX = 1.15 HOLD_ACCEL_MAX = 0.10 +MATCHED_LEAD_CLOSING_SPEED = 0.02 +MATCHED_LEAD_PROFILE_ACCEL = -0.90 LAUNCH_PACE_RATE = 5.0 LEAD_ACQUISITION_INITIAL_AUTHORITY = 0.20 LEAD_ACQUISITION_TIME = 0.30 @@ -85,8 +87,12 @@ LEAD_ACQUISITION_MIN_LEAD_ACCEL = -0.50 LEAD_ACQUISITION_MIN_PLANNER_ACCEL = -0.10 LEAD_DEPARTURE_HANDOFF_TIME = 0.50 RELATIVE_PACE_PREVIEW_TIME = 3.0 -URGENT_BYPASS_REQUIRED_DECEL = 0.75 +URGENT_BYPASS_REQUIRED_DECEL = 0.45 URGENT_BYPASS_MIN_SPEED = 5.0 +URGENT_RELEASE_REQUIRED_DECEL = 0.35 +URGENT_LOW_SPEED_LEAD_BYPASS = 5.0 +URGENT_REJOIN_ACCEL_MAX = -0.15 +URGENT_REJOIN_ACCEL_RATE = 5.0 @dataclass(frozen=True) @@ -97,6 +103,7 @@ class EnergyEnvelope: usable_gap: float = math.inf closing_speed: float = 0.0 required_decel: float = 0.0 + conservative_required_decel: float = 0.0 has_nearly_stopped_lead: bool = False @@ -141,6 +148,8 @@ class _PacePath: accel_limit: float | None = None decel_limit_active: bool = False urgent_bypass_active: bool = False + urgent_recovery_active: bool = False + urgent_dropout_frames: int = 0 lead_seen: list[bool] = field(default_factory=lambda: [False, False]) lead_track_ids: list[int] = field(default_factory=lambda: [-1, -1]) lead_obstacle_weights: list[float] = field(default_factory=lambda: [1.0, 1.0]) @@ -158,6 +167,8 @@ class _PacePath: self.accel_limit = None self.decel_limit_active = False self.urgent_bypass_active = False + self.urgent_recovery_active = False + self.urgent_dropout_frames = 0 self.lead_seen = [False, False] self.lead_track_ids = [-1, -1] self.lead_obstacle_weights = [1.0, 1.0] @@ -260,6 +271,26 @@ class AccelController: else: required_decel = closing_speed * closing_speed / (2.0 * usable_gap) + # A newly acquired radar track can briefly report a large positive + # acceleration. Do not let that optimistic sample delay handing an + # urgent lead to stock MPC. This conservative value is used only for + # urgent acquisition classification; the energy cap and raw radar lead + # continue using the original stock-filtered acceleration. + conservative_required_decel = required_decel + if a_lead > 0.0: + conservative_lead_xv = LongitudinalMpc.extrapolate_lead(x_lead, v_lead, 0.0, a_lead_tau) + conservative_x_lead = float(np.interp(delay, T_IDXS, conservative_lead_xv[:, 0])) + conservative_v_lead = float(np.interp(delay, T_IDXS, conservative_lead_xv[:, 1])) + conservative_gap = max(conservative_x_lead - x_ego - STOP_DISTANCE - t_follow * conservative_v_lead, 0.0) + conservative_closing = max(v_ego_delay - conservative_v_lead, 0.0) + if conservative_closing == 0.0: + conservative_required_decel = 0.0 + elif conservative_gap == 0.0: + conservative_required_decel = math.inf + else: + conservative_required_decel = conservative_closing * conservative_closing / (2.0 * conservative_gap) + conservative_required_decel = max(required_decel, conservative_required_decel) + # Relative kinetic energy: the lead keeps moving while ego sheds closing speed. anticipated_gap = max(usable_gap - closing_speed * RELATIVE_PACE_PREVIEW_TIME, 0.0) cap = v_lead_delay + math.sqrt(2.0 * config.comfort_decel * anticipated_gap) @@ -269,6 +300,7 @@ class AccelController: usable_gap=usable_gap, closing_speed=closing_speed, required_decel=required_decel, + conservative_required_decel=conservative_required_decel, )) if not candidates: @@ -283,6 +315,7 @@ class AccelController: usable_gap=selected.usable_gap, closing_speed=selected.closing_speed, required_decel=selected.required_decel, + conservative_required_decel=selected.conservative_required_decel, has_nearly_stopped_lead=departure_lead_speed < STOPPED_LEAD_SPEED, ) @@ -535,6 +568,8 @@ class AccelController: planner_accel: float, profile_accel_max: float, config: ProfileConfig, + closing_speed: float, + lead_present: bool, ) -> tuple[float, float]: """Return telemetry effective max and the controller's pre-MPC upper bound.""" profile_limit = float(np.clip(profile_accel_max, 0.0, ACCEL_MAX)) @@ -545,10 +580,12 @@ class AccelController: # opens in time on departure instead of changing shape in one frame. path.accel_limit = 0.0 path.decel_limit_active = False + path.urgent_recovery_active = False return min(stock_accel_max, 0.0), 0.0 if path.departing_from_stop: path.decel_limit_active = False + path.urgent_recovery_active = False planner_seed = max(0.0, planner_accel) if path.departure_handoff_active: # Open every profile at the same bounded jerk rate until the command is @@ -568,7 +605,27 @@ class AccelController: path.accel_limit = min(BREAKAWAY_ACCEL_MAX, path.accel_limit + CLEAR_LAUNCH_ACCEL_RATE * self.dt) return min(stock_accel_max, path.accel_limit), path.accel_limit - if path.state == AccelControllerState.restrict: + if path.urgent_recovery_active: + # Once projected relative speed is matched, a persistent recovery floor + # would keep the car from accelerating back toward a moving lead. Hand + # off to the normal state machine from the already-slewed ceiling. A + # missing lead does not qualify: the median dropout guard must retain its + # no-gas behavior until current lead evidence returns or relief confirms. + if (lead_present and closing_speed <= 0.10) or path.state in (AccelControllerState.free, AccelControllerState.release): + path.urgent_recovery_active = False + else: + previous_limit = path.accel_limit if path.accel_limit is not None else ACCEL_MAX + rejoin_target = URGENT_REJOIN_ACCEL_MAX if path.state == AccelControllerState.restrict else HOLD_ACCEL_MAX + max_step = URGENT_REJOIN_ACCEL_RATE * self.dt + path.accel_limit = float(np.clip(rejoin_target, previous_limit - max_step, previous_limit + max_step)) + path.decel_limit_active = path.state == AccelControllerState.restrict + return min(stock_accel_max, path.accel_limit), path.accel_limit + + if path.state == AccelControllerState.restrict and (not lead_present or closing_speed > MATCHED_LEAD_CLOSING_SPEED): + # A negative full-horizon maximum is useful only while ego is still + # closing. Keeping it after matching relative speed forces continued + # braking below a slower moving lead. Once matched, retain the lowering + # pace target but let the selected positive profile ceiling recover. requested_limit = -config.comfort_decel if not path.decel_limit_active: # Preserve an existing pre-MPC ceiling when restriction begins. The @@ -580,10 +637,15 @@ class AccelController: if path.accel_limit is None: path.accel_limit = max(0.0, planner_accel) path.decel_limit_active = True - elif path.state == AccelControllerState.hold: + elif lead_present and closing_speed > MATCHED_LEAD_CLOSING_SPEED: + # Do not spend the remaining gap by accelerating toward a slower lead, + # regardless of whether the scalar pace state is currently releasing. + requested_limit = HOLD_ACCEL_MAX + path.decel_limit_active = False + elif path.state in (AccelControllerState.restrict, AccelControllerState.hold): # No gas while waiting for relief confirmation. This is the main # anti-rubber-band rule for a still-closing lead. - requested_limit = HOLD_ACCEL_MAX + requested_limit = profile_limit if lead_present and planner_accel >= MATCHED_LEAD_PROFILE_ACCEL else HOLD_ACCEL_MAX path.decel_limit_active = False else: requested_limit = profile_limit @@ -697,7 +759,15 @@ class AccelController: envelope.closing_speed, launch_delta_v, ) - self._update_accel_limit(self.shadow, stock_accel_max, planner_accel, profile_accel_max, config) + self._update_accel_limit( + self.shadow, + stock_accel_max, + planner_accel, + profile_accel_max, + config, + envelope.closing_speed, + envelope.selected_lead >= 0, + ) shadow_active = True else: self.shadow.reset() @@ -737,30 +807,110 @@ class AccelController: follow_personality, allow_blend=live_was_initialized, ) + urgent_was_active = self.live.urgent_bypass_active + pre_urgent_accel_limit = self.live.accel_limit + selected_lead_speed = math.inf + if envelope.selected_lead in (0, 1): + selected_lead_speed = float(getattr((radar_state.leadOne, radar_state.leadTwo)[envelope.selected_lead], "vLeadK", math.inf)) + selected_has_full_authority = ( + envelope.selected_lead in (0, 1) and lead_obstacle_weights[envelope.selected_lead] >= 1.0 + ) + urgent_required_decel = envelope.required_decel + if not live_was_initialized or not established_selected_lead: + urgent_required_decel = max(urgent_required_decel, envelope.conservative_required_decel) urgent_trigger = ( sanitized_v_ego >= URGENT_BYPASS_MIN_SPEED - and envelope.required_decel >= URGENT_BYPASS_REQUIRED_DECEL - and (not live_was_initialized or established_selected_lead) + and urgent_required_decel >= URGENT_BYPASS_REQUIRED_DECEL + and (not live_was_initialized or established_selected_lead or selected_has_full_authority) ) + if urgent_was_active and envelope.selected_lead < 0: + self.live.urgent_dropout_frames += 1 + else: + self.live.urgent_dropout_frames = 0 + urgent_dropout_guard = urgent_was_active and envelope.selected_lead < 0 and self.live.urgent_dropout_frames <= 2 urgent_bypass = urgent_trigger or ( - self.live.urgent_bypass_active and envelope.selected_lead >= 0 and envelope.closing_speed > 0.10 + urgent_was_active + and envelope.selected_lead >= 0 + and ( + envelope.required_decel > URGENT_RELEASE_REQUIRED_DECEL + or (selected_lead_speed < URGENT_LOW_SPEED_LEAD_BYPASS and self.live.state != AccelControllerState.stopHold) + ) ) - self.live.urgent_bypass_active = urgent_bypass - if urgent_bypass: + # Keep the urgent context latched across two missing observations, but do + # not call it a stock bypass below: with no raw lead, stock has no obstacle + # to preserve the braking plan. The dedicated dropout branch retains only + # scalar pace and acceleration state, never stale lead geometry. + self.live.urgent_bypass_active = urgent_bypass or urgent_dropout_guard + if ( + urgent_was_active + and not urgent_bypass + and not urgent_dropout_guard + and envelope.selected_lead >= 0 + and self.live.state != AccelControllerState.stopHold + ): + # Rejoin comfort control at the speed the stock lead plan has already + # reached. Keeping the pre-urgent pace here asks MPC to release a hard + # brake toward a stale, much higher target in one frame. + rejoin_speed = min(planner_speed, sanitized_v_ego) + if envelope.closing_speed <= 0.10 and math.isfinite(selected_lead_speed): + # A moving lead is the natural lower pace reference once projected + # relative speed is matched. Seeding below it makes the cruise + # obstacle reinforce stock's residual braking and creates a large + # undershoot before the release ramp can recover. + rejoin_speed = max(rejoin_speed, min(base_speed, selected_lead_speed)) + self.live.pace = rejoin_speed + else: + self.live.pace = min(self.live.pace if self.live.pace is not None else rejoin_speed, rejoin_speed) + self.live.state = AccelControllerState.hold + self.live.relief_time = 0.0 + # Reintroduce the custom horizon from stock's global ceiling. The + # existing pre-MPC slew then tightens it gradually instead of stepping + # directly from no bound to HOLD_ACCEL_MAX while braking hard. + self.live.accel_limit = ACCEL_MAX + self.live.urgent_recovery_active = True + if urgent_dropout_guard: + # A one- or two-frame all-lead dropout must not turn an urgent brake + # into gas. Hold the synchronized scalar pace and the current + # nonpositive planner ceiling while the median cap remains restrictive. + # This protects the handoff without retaining stale lead geometry. + held_limit = self.live.accel_limit if self.live.accel_limit is not None else planner_accel + self.live.accel_limit = float(np.clip(min(held_limit, 0.0), ACCEL_MIN, ACCEL_MAX)) + self.live.decel_limit_active = self.live.accel_limit < 0.0 + self.live.urgent_recovery_active = False + effective_accel_max = min(stock_accel_max, self.live.accel_limit) + mpc_accel_max = self._build_mpc_accel_max(self.live, self.live.accel_limit) + mpc_shape_cruise = True + lead_obstacle_weights = (1.0, 1.0) + target_speed = min(base_speed, self.live.pace if self.live.pace is not None else sanitized_v_ego) + elif urgent_bypass: # Comfort shaping must never compete with urgent braking. Hand the raw # leads, base cruise target, and stock acceleration bounds directly to - # MPC. Clearing the stored ceiling gives the later comfort re-entry a - # fresh non-restrictive seed instead of resurrecting an urgent bound. + # MPC. On the rising edge only, retain a prior nonnegative upper bound + # for one solve so introducing the raw obstacle does not simultaneously + # remove an optimizer constraint. A nonnegative maximum cannot weaken + # braking, and the following urgent frame returns to stock bounds. + urgent_entry_limit = None + if not urgent_was_active and pre_urgent_accel_limit is not None and math.isfinite(pre_urgent_accel_limit): + urgent_entry_limit = float(np.clip(max(pre_urgent_accel_limit, 0.0), 0.0, ACCEL_MAX)) self.live.accel_limit = None self.live.decel_limit_active = False - effective_accel_max = stock_accel_max - mpc_accel_max = None + self.live.urgent_recovery_active = False + if self.live.pace is not None: + self.live.pace = min(self.live.pace, planner_speed, sanitized_v_ego) + effective_accel_max = min(stock_accel_max, urgent_entry_limit) if urgent_entry_limit is not None else stock_accel_max + mpc_accel_max = self._build_mpc_accel_max(self.live, urgent_entry_limit) if urgent_entry_limit is not None else None mpc_shape_cruise = False lead_obstacle_weights = (1.0, 1.0) target_speed = base_speed else: effective_accel_max, controller_accel_max = self._update_accel_limit( - self.live, stock_accel_max, planner_accel, profile_accel_max, config + self.live, + stock_accel_max, + planner_accel, + profile_accel_max, + config, + envelope.closing_speed, + envelope.selected_lead >= 0, ) # Feed only the controller-owned ceiling into MPC. Stock's speed, turn, # coast, and no-throttle limits remain in their original output clip. @@ -778,6 +928,13 @@ class AccelController: # Give all profiles the same prompt stock breakaway. The raw lead still # owns obstacle braking, and profile separation begins after motion. target_speed = base_speed + elif self.live.urgent_recovery_active: + # Keep stock's cruise incentive while the positive recovery ceiling + # gently unwinds the hard lead-braking plan. Switching immediately + # to the synchronized pace can make MPC keep braking well below a + # moving lead even though the ceiling itself is nonnegative. + target_speed = base_speed + lead_obstacle_weights = (1.0, 1.0) else: target_speed = min(base_speed, self.live.pace if self.live.pace is not None else base_speed) else: 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 43d11d8ddd..f5c50f54cf 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 @@ -6,7 +6,7 @@ import numpy as np import pytest from cereal import log -from opendbc.car.interfaces import ACCEL_MAX +from opendbc.car.interfaces import ACCEL_MAX, ACCEL_MIN from openpilot.common.realtime import DT_MDL from openpilot.selfdrive.controls.lib.longitudinal_planner import get_max_accel from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import N, LongitudinalPlanSource, STOP_DISTANCE, get_T_FOLLOW @@ -17,11 +17,15 @@ from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_control BREAKAWAY_ACCEL_MAX, CLEAR_LAUNCH_ACCEL_RATE, DECEL_LIMIT_JERK, + HOLD_ACCEL_MAX, INITIAL_LAUNCH_ACCEL_MAX, LAUNCH_ACCEL_RATE, LAUNCH_DELTA_V, RELATIVE_PACE_PREVIEW_TIME, URGENT_BYPASS_REQUIRED_DECEL, + URGENT_REJOIN_ACCEL_MAX, + URGENT_REJOIN_ACCEL_RATE, + URGENT_RELEASE_REQUIRED_DECEL, AccelController, AccelControllerState, AccelProfile, @@ -147,7 +151,7 @@ class TestAccelProfileLimits: assert len(result.mpc_accel_max) == N + 1 assert_profile_trajectory(result, 0.90) - def test_ordinary_lead_keeps_profile_pre_mpc_accel_bound(self): + def test_ordinary_closing_lead_uses_no_gas_pre_mpc_bound(self): governor = make_governor() radar_state = make_radar(make_lead(status=True, d_rel=100.0, v_lead_k=15.0)) @@ -155,7 +159,7 @@ class TestAccelProfileLimits: assert result.active assert result.selected_lead == 0 - assert_profile_trajectory(result, result.profile_accel_max) + assert_profile_trajectory(result, 0.10) assert result.mpc_shape_cruise def test_filtered_lead_history_keeps_profile_bound_through_two_dropouts(self): @@ -499,7 +503,7 @@ class TestAccelControllerState: governor = make_governor() # Restrictive enough to start early comfort shaping, but below the urgent # stock-MPC bypass threshold. - radar_state = make_radar(make_lead(status=True, d_rel=100.0, v_lead_k=10.0)) + radar_state = make_radar(make_lead(status=True, d_rel=160.0, v_lead_k=10.0)) update(governor, radar_state) update(governor, radar_state) @@ -510,8 +514,7 @@ class TestAccelControllerState: assert first_restriction.live_pace == pytest.approx(20.0 - expected_step) assert next_restriction.live_pace == pytest.approx(first_restriction.live_pace - expected_step) assert next_restriction.state == AccelControllerState.restrict - initial_limit = AccelController.get_profile_accel_max(AccelProfile.normal, 20.0) - assert_profile_trajectory(first_restriction, initial_limit - DECEL_LIMIT_JERK * DT_MDL) + assert_profile_trajectory(first_restriction, HOLD_ACCEL_MAX - DECEL_LIMIT_JERK * DT_MDL) assert_profile_trajectory(next_restriction, first_restriction.mpc_accel_max[0] - DECEL_LIMIT_JERK * DT_MDL) def test_urgent_closing_bypasses_comfort_shaping_for_stock_mpc(self): @@ -523,6 +526,114 @@ class TestAccelControllerState: assert result.mpc_accel_max is None assert result.lead_obstacle_weights == (1.0, 1.0) + def test_positive_lead_accel_spike_cannot_delay_first_urgent_bypass(self): + governor = make_governor() + radar_state = make_radar(make_lead(status=True, d_rel=90.0, v_lead_k=14.0, a_lead_k=5.0)) + + result = update(governor, radar_state, base_speed=30.0, v_ego=22.0, planner_speed=22.0) + + assert result.required_decel < URGENT_BYPASS_REQUIRED_DECEL + assert governor.live.urgent_bypass_active + assert result.target_speed == result.base_speed + assert result.mpc_accel_max is None + assert result.lead_obstacle_weights == (1.0, 1.0) + + def test_urgent_entry_bridges_existing_nonnegative_ceiling_for_one_frame(self): + governor = make_governor() + clear = update(governor, base_speed=40.0, v_ego=34.8, planner_speed=34.8) + urgent_radar = make_radar(make_lead(status=True, d_rel=94.0, v_lead_k=23.4, radar_track_id=22)) + + entering = update(governor, urgent_radar, base_speed=40.0, v_ego=34.8, planner_speed=34.8) + established = update(governor, urgent_radar, base_speed=40.0, v_ego=34.8, planner_speed=34.8) + + assert clear.mpc_accel_max is not None + assert entering.required_decel > URGENT_BYPASS_REQUIRED_DECEL + assert entering.mpc_accel_max == clear.mpc_accel_max + assert not entering.mpc_shape_cruise + assert entering.lead_obstacle_weights == (1.0, 1.0) + assert established.mpc_accel_max is None + + def test_urgent_dropout_holds_pace_and_nonpositive_ceiling_without_lead_geometry(self): + governor = make_governor() + urgent_radar = make_radar(self.restrictive_lead) + for _ in range(3): + update(governor, urgent_radar, planner_accel=-0.5) + + dropout_one = update(governor, planner_accel=-0.5) + dropout_two = update(governor, planner_accel=-0.5) + expired = update(governor, planner_accel=-0.5) + + for result in (dropout_one, dropout_two): + assert result.mpc_accel_max is not None + assert max(result.mpc_accel_max) <= 0.0 + assert result.mpc_shape_cruise + assert result.target_speed == result.live_pace + assert result.target_speed < result.base_speed + assert result.lead_obstacle_weights == (1.0, 1.0) + assert not governor.live.urgent_bypass_active + assert not governor.live.urgent_recovery_active + assert expired.mpc_accel_max is not None + assert expired.target_speed == expired.live_pace + assert expired.target_speed < expired.base_speed + + def test_urgent_exit_slews_rejoin_ceiling_then_releases_after_speed_match(self): + governor = make_governor() + urgent_radar = make_radar(self.restrictive_lead) + for _ in range(3): + urgent = update(governor, urgent_radar) + assert urgent.required_decel > URGENT_BYPASS_REQUIRED_DECEL + assert governor.live.urgent_bypass_active + + moderate_lead = make_radar(make_lead(status=True, d_rel=180.0, v_lead_k=10.0)) + rejoin = update(governor, moderate_lead) + + assert rejoin.required_decel < URGENT_RELEASE_REQUIRED_DECEL + assert not governor.live.urgent_bypass_active + assert governor.live.urgent_recovery_active + assert rejoin.mpc_accel_max is not None + expected_first_ceiling = ACCEL_MAX - URGENT_REJOIN_ACCEL_RATE * DT_MDL + assert_profile_trajectory(rejoin, expected_first_ceiling) + assert rejoin.live_pace <= 20.0 + + recovery_limits = [rejoin.mpc_accel_max[0]] + while governor.live.urgent_recovery_active and len(recovery_limits) < 50: + result = update(governor, moderate_lead) + assert result.mpc_accel_max is not None + recovery_limits.append(result.mpc_accel_max[0]) + + max_rejoin_step = URGENT_REJOIN_ACCEL_RATE * DT_MDL + assert all(abs(current - previous) <= max_rejoin_step + 1e-9 for previous, current in zip(recovery_limits[:-1], recovery_limits[1:], strict=True)) + assert all(ACCEL_MIN <= limit <= ACCEL_MAX for limit in recovery_limits) + assert min(recovery_limits) == pytest.approx(URGENT_REJOIN_ACCEL_MAX) + + matched_lead = make_radar(make_lead(status=True, d_rel=100.0, v_lead_k=20.0)) + matched = update(governor, matched_lead) + assert not governor.live.urgent_recovery_active + assert matched.mpc_accel_max is not None + assert matched.mpc_accel_max[0] > URGENT_REJOIN_ACCEL_MAX + + def test_renewed_urgent_closing_cancels_rejoin_ceiling(self): + governor = make_governor() + urgent_radar = make_radar(self.restrictive_lead) + for _ in range(3): + update(governor, urgent_radar) + moderate_lead = make_radar(make_lead(status=True, d_rel=180.0, v_lead_k=10.0)) + update(governor, moderate_lead) + assert governor.live.urgent_recovery_active + + renewed = update(governor, urgent_radar) + + assert renewed.required_decel > URGENT_BYPASS_REQUIRED_DECEL + assert governor.live.urgent_bypass_active + assert not governor.live.urgent_recovery_active + assert renewed.mpc_accel_max is not None + assert min(renewed.mpc_accel_max) >= 0.0 + assert not renewed.mpc_shape_cruise + assert renewed.lead_obstacle_weights == (1.0, 1.0) + + established = update(governor, urgent_radar) + assert established.mpc_accel_max is None + def test_release_waits_for_confirmation_then_uses_profile_rate(self): governor = make_governor() radar_state = make_radar(self.restrictive_lead) diff --git a/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py b/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py index 4d33f4be70..447904db83 100644 --- a/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py +++ b/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py @@ -26,6 +26,8 @@ class ClosedLoopTrace: active: np.ndarray shadow_active: np.ndarray launching: np.ndarray + urgent_bypass: np.ndarray + urgent_recovery: np.ndarray pace: np.ndarray filtered_cap: np.ndarray selected_lead: np.ndarray @@ -90,6 +92,8 @@ def _run( controller.active, controller.shadow_active, controller.launching, + plant.planner.accel_controller.live.urgent_bypass_active, + plant.planner.accel_controller.live.urgent_recovery_active, controller.live_pace, controller.live_filtered_cap, controller.selected_lead, @@ -120,18 +124,20 @@ def _run( active=data[:, 8].astype(bool), shadow_active=data[:, 9].astype(bool), launching=data[:, 10].astype(bool), - pace=data[:, 11], - filtered_cap=data[:, 12], - selected_lead=data[:, 13].astype(int), - profile_accel_max=data[:, 14], - effective_accel_max=data[:, 15], - controller_fault=data[:, 16].astype(bool), - actuator_command=data[:, 17], - applied_actuator_command=data[:, 18], - observed_speed=data[:, 19], - observed_acceleration=data[:, 20], - lead_obstacle_weight_0=data[:, 21], - lead_obstacle_weight_1=data[:, 22], + urgent_bypass=data[:, 11].astype(bool), + urgent_recovery=data[:, 12].astype(bool), + pace=data[:, 13], + filtered_cap=data[:, 14], + selected_lead=data[:, 15].astype(int), + profile_accel_max=data[:, 16], + effective_accel_max=data[:, 17], + controller_fault=data[:, 18].astype(bool), + actuator_command=data[:, 19], + applied_actuator_command=data[:, 20], + observed_speed=data[:, 21], + observed_acceleration=data[:, 22], + lead_obstacle_weight_0=data[:, 23], + lead_obstacle_weight_1=data[:, 24], solver_failures=solver_failures, ) @@ -467,6 +473,103 @@ def test_severe_closing_never_delays_braking_or_reduces_clearance(): assert np.max(np.abs(np.diff(controlled.a_target)[onset] / DT_MDL)) < 4.0 +def test_slow_lead_urgent_rejoin_has_no_brake_release_jolt_or_safety_regression(): + common = dict( + duration=25.0, + lead_relevancy=True, + speed=20.0, + distance_lead=100.0, + v_lead=10.0, + v_cruise=30.0, + actuator_delay=0.20, + actuator_lag=0.25, + ) + baseline = _run(controller_enabled=False, **common) + controlled = _run(controller_enabled=True, profile=1, **common) + + assert controlled.urgent_bypass.any() + assert controlled.urgent_recovery.any() + assert controlled.solver_failures == 0 + assert controlled.solver_failures <= baseline.solver_failures + + command_jerk = _command_jerk(controlled, after=1.0) + assert np.max(command_jerk) < 3.0 + assert np.max(np.abs(command_jerk)) < 4.0 + assert not _has_propulsion_brake_reversal(controlled, after=1.0) + + baseline_gap = baseline.distance_lead - baseline.distance + controlled_gap = controlled.distance_lead - controlled.distance + # Clean-base acados can hit its known macOS solver edge late in this long + # fixture. Compare only the valid stock prefix, while requiring the + # controller to remain fault-free for the complete settle and recovery. + baseline_valid = ~baseline.controller_fault + if baseline.controller_fault.any(): + baseline_valid[np.flatnonzero(baseline.controller_fault)[0]:] = False + # This routine matching fixture trades less than one metre of the stock + # buffer for avoiding stock's late solver edge and large speed undershoot. + # The severe-closing regression above retains the exact no-clearance-loss + # safety gate. + assert np.min(controlled_gap[baseline_valid]) >= np.min(baseline_gap[baseline_valid]) - 1.0 + baseline_closing = np.maximum(baseline.speed - common["v_lead"], 0.0) + controlled_closing = np.maximum(controlled.speed - common["v_lead"], 0.0) + baseline_ttc = np.divide(baseline_gap, baseline_closing, out=np.full_like(baseline_gap, np.inf), where=baseline_closing > 0.0) + controlled_ttc = np.divide(controlled_gap, controlled_closing, out=np.full_like(controlled_gap, np.inf), where=controlled_closing > 0.0) + assert np.min(controlled_ttc[baseline_valid]) >= np.min(baseline_ttc[baseline_valid]) - 0.50 + assert np.min(controlled_gap) > 20.0 + assert np.min(controlled_ttc) > 6.0 + + # Rejoining comfort control must not command gas while ego still needs to + # match the slower lead, or over-slow materially compared with stock. + still_closing = controlled.speed > common["v_lead"] + 0.2 + assert np.max(controlled.a_target[still_closing]) <= 0.2 + controlled_undershoot = np.min(controlled.speed - common["v_lead"]) + assert controlled_undershoot >= -1.1 + assert abs(controlled.speed[-1] - common["v_lead"]) < 0.65 + + +@pytest.mark.parametrize("profile", range(3), ids=("eco", "normal", "sport")) +@pytest.mark.parametrize( + ("actuator_delay", "actuator_lag"), + [ + (0.10, 0.20), + (0.15, 0.25), + (0.20, 0.20), + (0.25, 0.30), + (0.30, 0.35), + ], + ids=("toyota", "honda", "gm", "hyundai", "ford"), +) +def test_slow_lead_rejoin_is_smooth_across_profiles_and_actuator_dynamics(profile, actuator_delay, actuator_lag): + lead_speed = 10.0 + trace = _run( + duration=10.0, + controller_enabled=True, + profile=profile, + lead_relevancy=True, + speed=20.0, + distance_lead=100.0, + v_lead=lead_speed, + v_cruise=30.0, + actuator_delay=actuator_delay, + actuator_lag=actuator_lag, + ) + + assert trace.urgent_bypass.any() + assert trace.solver_failures == 0 + command_jerk = _command_jerk(trace, after=1.0) + assert np.max(command_jerk) < 3.0 + assert np.max(np.abs(command_jerk)) < 4.0 + assert not _has_propulsion_brake_reversal(trace, after=1.0) + + gap = trace.distance_lead - trace.distance + closing = np.maximum(trace.speed - lead_speed, 0.0) + ttc = np.divide(gap, closing, out=np.full_like(gap, np.inf), where=closing > 0.0) + assert np.min(gap) > 20.0 + assert np.min(ttc) > 3.0 + assert np.min(trace.speed - lead_speed) > -1.25 + assert np.max(trace.a_target[trace.speed > lead_speed + 0.2]) <= 0.2 + + @pytest.mark.parametrize( ("actuator_delay", "actuator_lag"), [