From 41cfac46d74ef75df6f1abfbdd80d59b73666736 Mon Sep 17 00:00:00 2001 From: rav4kumar <36933347+rav4kumar@users.noreply.github.com> Date: Fri, 17 Jul 2026 18:57:19 -0700 Subject: [PATCH] Prevent pace handoff lurches without damping acceleration --- .../lib/accel_personality/accel_controller.py | 115 ++++++++++------- .../tests/test_accel_controller.py | 118 ++++++++++++++---- .../test_accel_controller_closed_loop.py | 49 +++++++- 3 files changed, 204 insertions(+), 78 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py index 9d29fcc6e5..1cf4ca1012 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py @@ -37,15 +37,12 @@ class AccelControllerState(IntEnum): @dataclass(frozen=True) class ProfileConfig: comfort_decel: float - release_rate: float - release_confirm: float - departure_rate: float PROFILE_CONFIGS = { - AccelProfile.eco: ProfileConfig(comfort_decel=0.25, release_rate=1.55, release_confirm=0.50, departure_rate=12.0), - AccelProfile.normal: ProfileConfig(comfort_decel=0.32, release_rate=1.75, release_confirm=0.35, departure_rate=16.0), - AccelProfile.sport: ProfileConfig(comfort_decel=0.38, release_rate=2.00, release_confirm=0.20, departure_rate=20.0), + AccelProfile.eco: ProfileConfig(comfort_decel=0.25), + AccelProfile.normal: ProfileConfig(comfort_decel=0.32), + AccelProfile.sport: ProfileConfig(comfort_decel=0.38), } # Keep launch responsive, then separate the profiles above walking speed. @@ -60,6 +57,7 @@ CAP_FILTER_FRAMES = 5 RESTRICT_DEADBAND = 0.15 RESTRICT_EXIT_DEADBAND = 0.05 RELIEF_DEADBAND = 0.35 +RELIEF_CONFIRM_FRAMES = 2 PACE_ACQUISITION_MARGIN = 0.45 EARLY_APPROACH_HEADWAY = 5.0 EARLY_APPROACH_MIN_SPEED = 8.0 @@ -70,13 +68,14 @@ MOVING_LEAD_DECEL_CONFIRM_FRAMES = 3 MOVING_LEAD_DECEL_ENTER_REQUIRED_DECEL = 0.15 MOVING_LEAD_DECEL_ACCEL_MAX = -0.42 MOVING_LEAD_DECEL_ACCEL_SLEW_RATE = 0.50 +MOVING_LEAD_DECEL_RELEASE_RATE = 1.00 MOVING_LEAD_DECEL_EXIT_REQUIRED_DECEL = 0.05 MOVING_LEAD_DECEL_EXIT_FRAMES = 4 STOP_HOLD_EGO_SPEED = 0.30 STOP_HOLD_CAP = 0.50 -STOP_HOLD_ACCEL_MAX = 0.0 -STOP_HOLD_GUARD_ACCEL_MAX = -0.42 -STOP_HOLD_GUARD_EXTRA = 0.20 +STOP_HOLD_PRECONDITION_SPEED = 0.05 +STOP_HOLD_PRECONDITION_ACCEL_MAX = -0.25 +STOP_HOLD_PRECONDITION_EXTRA = 0.20 STOPPED_LEAD_SPEED = 0.30 STOP_HOLD_EXIT_FRAMES = 4 MAX_LEAD_ACCEL_TAU = 10.0 @@ -135,13 +134,14 @@ class _PacePath: cap_samples: deque[float] = field(default_factory=lambda: deque([math.inf] * CAP_FILTER_FRAMES, maxlen=CAP_FILTER_FRAMES)) pace: float | None = None state: AccelControllerState = AccelControllerState.inactive - relief_time: float = 0.0 + relief_frames: int = 0 departure_frames: int = 0 departing_from_stop: bool = False filtered_lead_active: bool = False previous_lead_speed: float | None = None lead_decel_frames: int = 0 moving_lead_decel: bool = False + moving_lead_release: bool = False moving_lead_relief_frames: int = 0 moving_lead_accel_max: float | None = None @@ -149,13 +149,14 @@ class _PacePath: self.cap_samples = deque([math.inf] * CAP_FILTER_FRAMES, maxlen=CAP_FILTER_FRAMES) self.pace = None self.state = AccelControllerState.inactive - self.relief_time = 0.0 + self.relief_frames = 0 self.departure_frames = 0 self.departing_from_stop = False self.filtered_lead_active = False self.previous_lead_speed = None self.lead_decel_frames = 0 self.moving_lead_decel = False + self.moving_lead_release = False self.moving_lead_relief_frames = 0 self.moving_lead_accel_max = None @@ -331,7 +332,9 @@ class AccelController: and envelope.closing_speed > CLOSING_SPEED_ENTER ): path.moving_lead_decel = True - path.moving_lead_accel_max = None + if not path.moving_lead_release: + path.moving_lead_accel_max = None + path.moving_lead_release = False matched_moving_lead = ( path.moving_lead_decel @@ -340,10 +343,15 @@ class AccelController: and envelope.required_decel <= MOVING_LEAD_DECEL_EXIT_REQUIRED_DECEL ) path.moving_lead_relief_frames = path.moving_lead_relief_frames + 1 if matched_moving_lead else 0 - if path.moving_lead_relief_frames >= MOVING_LEAD_DECEL_EXIT_FRAMES or envelope.selected_lead < 0 or path.state == AccelControllerState.stopHold: + if path.state == AccelControllerState.stopHold: path.moving_lead_decel = False + path.moving_lead_release = False path.moving_lead_relief_frames = 0 path.moving_lead_accel_max = None + elif path.moving_lead_relief_frames >= MOVING_LEAD_DECEL_EXIT_FRAMES or not path.filtered_lead_active: + path.moving_lead_decel = False + path.moving_lead_release = path.moving_lead_accel_max is not None and path.moving_lead_accel_max < 0.0 + path.moving_lead_relief_frames = 0 path.previous_lead_speed = lead_speed if math.isfinite(lead_speed) else None @@ -360,11 +368,11 @@ class AccelController: path.state = AccelControllerState.free early_approach = envelope.usable_gap >= EARLY_APPROACH_HEADWAY * max(v_ego, 1.0) - if acquired_restrictive_lead and filtered_cap < path.pace - RESTRICT_DEADBAND: - if not early_approach or v_ego >= EARLY_APPROACH_MIN_SPEED: - acquisition_margin = PACE_ACQUISITION_MARGIN if early_approach else 0.0 - acquisition_pace = max(envelope.selected_lead_speed, v_ego - acquisition_margin) - path.pace = min(path.pace, acquisition_pace) + if acquired_restrictive_lead and path.pace >= base_speed - RESTRICT_DEADBAND and filtered_cap < path.pace - RESTRICT_DEADBAND: + if not early_approach: + path.pace = min(path.pace, v_ego) + elif v_ego >= EARLY_APPROACH_MIN_SPEED: + path.pace = min(path.pace, max(envelope.selected_lead_speed, v_ego - PACE_ACQUISITION_MARGIN)) path.pace = min(path.pace, base_speed) if self._lead_source(previous_mpc_source) and not math.isfinite(raw_cap) and planner_speed < path.pace: @@ -378,10 +386,9 @@ class AccelController: path.pace = 0.0 return filtered_cap - departure_ceiling = min(base_speed, filtered_cap) if math.isfinite(filtered_cap) else base_speed - path.pace = max(departure_ceiling, 0.0) + path.pace = base_speed path.state = AccelControllerState.release - path.relief_time = 0.0 + path.relief_frames = 0 path.departure_frames = 0 path.departing_from_stop = True return filtered_cap @@ -393,7 +400,7 @@ class AccelController: if v_ego < STOP_HOLD_EGO_SPEED and (stop_evidence or stale_plan_stop): path.pace = 0.0 path.state = AccelControllerState.stopHold - path.relief_time = 0.0 + path.relief_frames = 0 path.departure_frames = 0 path.departing_from_stop = False return filtered_cap @@ -401,10 +408,16 @@ class AccelController: if path.departing_from_stop and v_ego >= STOP_HOLD_EGO_SPEED: path.departing_from_stop = False - transient_relief = math.isfinite(filtered_cap) and (not math.isfinite(raw_cap) or raw_cap > filtered_cap + RELIEF_DEADBAND) - if transient_relief and not path.departing_from_stop: + filter_warmup_relief = math.isfinite(raw_cap) and not math.isfinite(filtered_cap) and path.pace < base_speed - RESTRICT_DEADBAND + if filter_warmup_relief: path.state = AccelControllerState.hold - path.relief_time = 0.0 + path.relief_frames = 0 + return filtered_cap + + transient_relief = math.isfinite(filtered_cap) and (not math.isfinite(raw_cap) or raw_cap > filtered_cap + RELIEF_DEADBAND) + if transient_relief and not path.departing_from_stop and path.state != AccelControllerState.release: + path.state = AccelControllerState.hold + path.relief_frames = 0 return filtered_cap ceiling = base_speed if path.departing_from_stop else min(base_speed, filtered_cap) @@ -415,37 +428,33 @@ class AccelController: if cap_restriction: path.pace = max(ceiling, path.pace - config.comfort_decel * self.dt) path.state = AccelControllerState.restrict - path.relief_time = 0.0 + path.relief_frames = 0 return filtered_cap if closing_guard: path.state = AccelControllerState.hold - path.relief_time = 0.0 + path.relief_frames = 0 return filtered_cap relief = ceiling - path.pace - release_active = path.state == AccelControllerState.release and relief > RESTRICT_DEADBAND - waiting_for_release = False - if relief > RELIEF_DEADBAND and not release_active: - if path.state != AccelControllerState.release: - path.relief_time += self.dt - path.state = AccelControllerState.hold - if path.relief_time >= config.release_confirm: - path.state = AccelControllerState.release - release_active = path.state == AccelControllerState.release - waiting_for_release = not release_active + if relief > RELIEF_DEADBAND and path.state != AccelControllerState.release: + confirmed_lead_loss = not math.isfinite(raw_cap) and not math.isfinite(filtered_cap) + path.relief_frames = path.relief_frames + 1 if confirmed_lead_loss else RELIEF_CONFIRM_FRAMES + path.state = AccelControllerState.release if path.relief_frames >= RELIEF_CONFIRM_FRAMES else AccelControllerState.hold + elif path.state != AccelControllerState.release: + path.relief_frames = 0 + release_active = path.state == AccelControllerState.release and relief >= 0.0 if release_active: - release_rate = config.departure_rate if path.departing_from_stop else config.release_rate - path.pace = min(ceiling, path.pace + release_rate * self.dt) - elif waiting_for_release: + path.pace = ceiling + path.state = AccelControllerState.free if path.pace >= base_speed else AccelControllerState.release + path.relief_frames = 0 + elif path.state == AccelControllerState.release: pass elif not math.isfinite(raw_cap) and not math.isfinite(filtered_cap) and 0.0 < relief <= RELIEF_DEADBAND: path.pace = ceiling path.state = AccelControllerState.free - path.relief_time = 0.0 else: - path.relief_time = 0.0 path.state = AccelControllerState.free if path.pace >= ceiling - RESTRICT_DEADBAND else AccelControllerState.hold return filtered_cap @@ -474,15 +483,17 @@ class AccelController: requested_accel_max = min(profile_accel_max, stock_accel_max) if stop_hold: self._reset_accel_limit() - guard_time = self._delay() + STOP_HOLD_GUARD_EXTRA - hold_accel_max = tuple(STOP_HOLD_GUARD_ACCEL_MAX if t <= guard_time else ACCEL_MAX for t in T_IDXS) - return min(requested_accel_max, STOP_HOLD_ACCEL_MAX), hold_accel_max, True, True + if v_ego > STOP_HOLD_PRECONDITION_SPEED: + return requested_accel_max, None, False, False + guard_time = self._delay() + STOP_HOLD_PRECONDITION_EXTRA + hold_accel_max = tuple(STOP_HOLD_PRECONDITION_ACCEL_MAX if t <= guard_time else ACCEL_MAX for t in T_IDXS) + return STOP_HOLD_PRECONDITION_ACCEL_MAX, hold_accel_max, True, True if self.live.departing_from_stop: self._reset_accel_limit(requested_accel_max) return requested_accel_max, None, False, False - if self.live.moving_lead_decel and lead_present: + if self.live.moving_lead_decel and self.live.filtered_lead_active: if self.live.moving_lead_accel_max is None: self.live.moving_lead_accel_max = max(min(planner_accel, 0.0), MOVING_LEAD_DECEL_ACCEL_MAX) self.live.moving_lead_accel_max = max( @@ -492,6 +503,16 @@ class AccelController: moving_lead_accel_max = min(stock_accel_max, self.live.moving_lead_accel_max) return moving_lead_accel_max, build_trajectory(moving_lead_accel_max), True, True + if self.live.moving_lead_release: + self.live.moving_lead_accel_max = min(0.0, self.live.moving_lead_accel_max + MOVING_LEAD_DECEL_RELEASE_RATE * self.dt) + moving_lead_accel_max = min(stock_accel_max, self.live.moving_lead_accel_max) + if self.live.moving_lead_accel_max < 0.0: + return moving_lead_accel_max, build_trajectory(moving_lead_accel_max), True, True + self.live.moving_lead_release = False + self.live.moving_lead_accel_max = None + self._reset_accel_limit(requested_accel_max) + return moving_lead_accel_max, build_trajectory(moving_lead_accel_max), True, True + if requested_accel_max <= 0.0: self._reset_accel_limit() return requested_accel_max, None, False, False @@ -573,7 +594,7 @@ class AccelController: live_filtered_cap = self._update_path(self.live, envelope, base_speed, sanitized_v_ego, config, previous_mpc_source, planner_speed, previous_should_stop) self._update_moving_lead_decel(self.live, envelope, base_speed, sanitized_v_ego) - target_speed = base_speed if self.live.departing_from_stop else min(base_speed, self.live.pace if self.live.pace is not None else base_speed) + target_speed = min(base_speed, self.live.pace if self.live.pace is not None else base_speed) effective_accel_max, mpc_accel_max, mpc_shape_cruise, mpc_apply_accel_constraint = self._build_mpc_accel_max( profile_accel_max, stock_accel_max, planner_accel, sanitized_v_ego, envelope.selected_lead >= 0, envelope.closing_speed, self.live.state == AccelControllerState.stopHold, 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 930dc9b207..aaf7cd703b 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 @@ -16,9 +16,11 @@ from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_control ACCEL_SHAPE_WARMUP_FRAMES, CAP_FILTER_FRAMES, MOVING_LEAD_DECEL_ACCEL_MAX, - MOVING_LEAD_DECEL_ACCEL_SLEW_RATE, MOVING_LEAD_DECEL_CONFIRM_FRAMES, MOVING_LEAD_DECEL_EXIT_FRAMES, + MOVING_LEAD_DECEL_ACCEL_SLEW_RATE, + MOVING_LEAD_DECEL_RELEASE_RATE, + RELIEF_CONFIRM_FRAMES, STOP_HOLD_EXIT_FRAMES, AccelController, AccelControllerState, @@ -431,7 +433,7 @@ class TestPaceGovernor: assert all(not result.mpc_shape_cruise for result in results) assert all(not result.mpc_apply_accel_constraint for result in results) - def test_decelerating_moving_lead_uses_smooth_ceiling_until_confirmed_match(self): + def test_decelerating_moving_lead_uses_gradual_bounded_ceiling_until_confirmed_match(self): controller = make_controller() def moving_lead(speed): @@ -441,13 +443,7 @@ class TestPaceGovernor: confirmation = [] for frame in range(MOVING_LEAD_DECEL_CONFIRM_FRAMES): confirmation.append( - update( - controller, - moving_lead(15.0 - 0.01 * (frame + 1)), - base_speed=30.0, - v_ego=20.0, - planner_speed=20.0, - ) + update(controller, moving_lead(15.0 - 0.01 * (frame + 1)), base_speed=30.0, v_ego=20.0, planner_speed=20.0) ) if frame < MOVING_LEAD_DECEL_CONFIRM_FRAMES - 1: assert not controller.live.moving_lead_decel @@ -457,17 +453,55 @@ class TestPaceGovernor: assert confirmation[-1].mpc_apply_accel_constraint falling_speed = 15.0 - 0.01 * MOVING_LEAD_DECEL_CONFIRM_FRAMES - constrained = [update(controller, moving_lead(falling_speed - 0.01 * (frame + 1)), base_speed=30.0, v_ego=20.0, planner_speed=20.0) for frame in range(20)] - limits = np.array([result.effective_accel_max for result in [confirmation[-1], *constrained]]) + constrained = [ + update(controller, moving_lead(falling_speed - 0.01 * (frame + 1)), base_speed=30.0, v_ego=20.0, planner_speed=20.0) + for frame in range(20) + ] + limits = np.array([confirmation[-1].effective_accel_max, *(result.effective_accel_max for result in constrained)]) assert constrained[-1].effective_accel_max == MOVING_LEAD_DECEL_ACCEL_MAX assert np.all(np.diff(limits) <= 0.0) assert np.max(np.abs(np.diff(limits))) <= MOVING_LEAD_DECEL_ACCEL_SLEW_RATE * DT_MDL + 1e-9 + assert np.min(limits) >= MOVING_LEAD_DECEL_ACCEL_MAX + assert np.all(np.diff([result.live_pace for result in constrained]) <= 1e-9) matched = [update(controller, moving_lead(5.0), base_speed=30.0, v_ego=5.05, planner_speed=5.05) for _ in range(MOVING_LEAD_DECEL_EXIT_FRAMES)] assert all(result.effective_accel_max <= 0.0 for result in matched[:-1]) assert not controller.live.moving_lead_decel + assert controller.live.moving_lead_release + assert matched[-1].effective_accel_max < 0.0 + + unwind_frames = math.ceil(abs(matched[-1].effective_accel_max) / (MOVING_LEAD_DECEL_RELEASE_RATE * DT_MDL)) + unwinding = [update(controller, moving_lead(5.0), base_speed=30.0, v_ego=5.05, planner_speed=5.05) for _ in range(unwind_frames)] + unwind_limits = np.array([matched[-1].effective_accel_max, *(result.effective_accel_max for result in unwinding)]) + assert np.all(np.diff(unwind_limits) >= 0.0) + assert np.max(np.diff(unwind_limits)) <= MOVING_LEAD_DECEL_RELEASE_RATE * DT_MDL + 1e-9 + assert unwinding[-1].effective_accel_max == 0.0 + assert not controller.live.moving_lead_release assert controller.live.moving_lead_accel_max is None - assert matched[-1].effective_accel_max > 0.0 + + released = update(controller, moving_lead(5.0), base_speed=30.0, v_ego=5.05, planner_speed=5.05) + assert released.effective_accel_max == min(released.profile_accel_max, 1.2, ACCEL_MAX) + + def test_moving_lead_ceiling_unwinds_smoothly_after_filtered_dropout(self): + controller = make_controller() + + def moving_lead(speed): + return make_radar(make_lead(status=True, d_rel=100.0, v_lead_k=speed)) + + update(controller, moving_lead(15.0), base_speed=30.0, v_ego=20.0, planner_speed=20.0) + for frame in range(MOVING_LEAD_DECEL_CONFIRM_FRAMES + 20): + update(controller, moving_lead(14.99 - 0.01 * frame), base_speed=30.0, v_ego=20.0, planner_speed=20.0) + + assert controller.live.moving_lead_decel + assert controller.live.moving_lead_accel_max == MOVING_LEAD_DECEL_ACCEL_MAX + dropouts = [update(controller, make_radar(), base_speed=30.0, v_ego=20.0, planner_speed=20.0) for _ in range(3)] + assert all(result.effective_accel_max == MOVING_LEAD_DECEL_ACCEL_MAX for result in dropouts[:2]) + assert dropouts[-1].effective_accel_max == pytest.approx(MOVING_LEAD_DECEL_ACCEL_MAX + MOVING_LEAD_DECEL_RELEASE_RATE * DT_MDL) + assert controller.live.moving_lead_release + + stock_coast = update(controller, make_radar(), base_speed=30.0, v_ego=20.0, planner_speed=20.0, stock_accel_max=-0.5) + assert stock_coast.effective_accel_max == -0.5 + assert stock_coast.mpc_accel_max[-1] == -0.5 def test_far_or_noisy_decelerating_lead_does_not_force_braking(self): far_controller = make_controller() @@ -497,21 +531,31 @@ class TestPaceGovernor: assert not far_controller.live.moving_lead_decel assert not noisy_controller.live.moving_lead_decel assert all(result.effective_accel_max >= 0.0 for result in [*far_results, *noisy_results]) + assert all(result.mpc_accel_max is None or min(result.mpc_accel_max) >= 0.0 for result in [*far_results, *noisy_results]) - def test_release_waits_then_raises_pace_at_profile_rate(self): + def test_filtered_relief_returns_pace_authority_to_accel_profile(self): controller = make_controller() restricted = self.establish_gentle_restriction(controller, frames=30)[-1] restricted_pace = restricted.live_pace - confirmation_frames = math.ceil(PROFILE_CONFIGS[AccelProfile.normal].release_confirm / DT_MDL) - results = [update(controller) for _ in range(CAP_FILTER_FRAMES // 2 + confirmation_frames)] + results = [update(controller) for _ in range(CAP_FILTER_FRAMES // 2 + RELIEF_CONFIRM_FRAMES)] released = results[-1] - assert all(result.state == AccelControllerState.hold for result in results[:-1]) - assert all(result.live_pace == restricted_pace for result in results[:-1]) - assert released.state == AccelControllerState.release - assert released.live_pace > restricted_pace - assert released.live_pace == pytest.approx(restricted_pace + PROFILE_CONFIGS[AccelProfile.normal].release_rate * DT_MDL) + assert all(result.state != AccelControllerState.release for result in results[:-1]) + assert all(result.live_pace <= restricted_pace for result in results[:-1]) + assert released.state == AccelControllerState.free + assert released.live_pace == released.base_speed + + def test_restrictive_lead_reacquisition_holds_pace_until_filter_recovers(self): + controller = make_controller() + restricted = self.establish_gentle_restriction(controller, frames=30)[-1] + missing = [update(controller) for _ in range(CAP_FILTER_FRAMES // 2 + 1)] + reacquired = [update(controller, make_radar(self.gentle_restrictive_lead)) for _ in range(CAP_FILTER_FRAMES // 2 + 1)] + + assert math.isinf(missing[-1].live_filtered_cap) + assert all(result.live_pace <= restricted.live_pace for result in missing) + assert all(result.live_pace == missing[-1].live_pace for result in reacquired[:-1]) + assert reacquired[-1].live_pace == pytest.approx(missing[-1].live_pace - PROFILE_CONFIGS[AccelProfile.normal].comfort_decel * DT_MDL) def test_confirmed_clear_finishes_release_at_base(self): controller = make_controller() @@ -583,6 +627,28 @@ class TestPaceGovernor: assert handed_off.mpc_accel_max is None assert not handed_off.mpc_apply_accel_constraint + @pytest.mark.parametrize(("v_ego", "uses_standstill_preconditioner"), [(0.0, True), (0.05, True), (0.051, False), (0.1, False)]) + def test_stop_hold_negative_bound_is_limited_to_stationary_preconditioning(self, v_ego, uses_standstill_preconditioner): + controller = make_controller() + stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0)) + + result = update(controller, stopped, base_speed=8.0, v_ego=v_ego, planner_speed=v_ego, stock_accel_max=1.6) + + assert result.state == AccelControllerState.stopHold + assert result.target_speed == 0.0 + if uses_standstill_preconditioner: + guard_time = controller._delay() + 0.20 + expected = tuple(-0.25 if t <= guard_time else ACCEL_MAX for t in T_IDXS) + assert result.effective_accel_max == -0.25 + assert result.mpc_accel_max == expected + assert result.mpc_shape_cruise + assert result.mpc_apply_accel_constraint + else: + assert result.effective_accel_max > 0.0 + assert result.mpc_accel_max is None + assert not result.mpc_shape_cruise + assert not result.mpc_apply_accel_constraint + def test_stop_hold_requires_four_moving_lead_frames_then_targets_base(self): controller = make_controller() stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0)) @@ -595,14 +661,16 @@ class TestPaceGovernor: assert held.state == AccelControllerState.stopHold assert held.target_speed == 0.0 held_results = [held, *waiting, *confirmations[:-1]] - assert all(result.effective_accel_max == 0.0 for result in held_results) - assert all(result.mpc_accel_max[0] < 0.0 and result.mpc_accel_max[-1] == ACCEL_MAX for result in held_results) - assert all(result.mpc_shape_cruise for result in held_results) - assert all(result.mpc_apply_accel_constraint for result in held_results) + assert all(result.effective_accel_max > 0.0 for result in held_results) + assert all(result.mpc_accel_max is None for result in held_results) + assert all(not result.mpc_shape_cruise for result in held_results) + assert all(not result.mpc_apply_accel_constraint for result in held_results) assert all(result.target_speed == 0.0 for result in confirmations[:-1]) - assert confirmations[-1].target_speed > 0.0 + assert confirmations[-1].target_speed == confirmations[-1].base_speed assert confirmations[-1].effective_accel_max <= ACCEL_MAX assert confirmations[-1].mpc_accel_max is None + assert not confirmations[-1].mpc_shape_cruise + assert not confirmations[-1].mpc_apply_accel_constraint assert confirmations[-1].launching assert released.target_speed >= confirmations[-1].target_speed assert released.launching 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 1512ab42f4..f1515b3537 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 @@ -236,11 +236,13 @@ def test_active_controller_is_pre_mpc_and_preserves_stock_lead_authority(): lead_plant = Plant(lead_relevancy=True, speed=0.0, distance_lead=6.0, actuator_delay=0.15, actuator_lag=0.20) lead_plant.step(v_lead=0.0, v_cruise=15.0) controller = lead_plant.planner.accel_controller_result - assert controller.mpc_accel_max[0] < 0.0 + assert controller.target_speed == 0.0 + assert min(controller.mpc_accel_max) == -0.25 assert controller.mpc_accel_max[-1] == ACCEL_MAX assert controller.mpc_shape_cruise assert controller.mpc_apply_accel_constraint - assert np.max(lead_plant.planner.mpc.params[T_IDXS <= 0.40, 1]) <= 0.0 + preconditioned = lead_plant.planner.mpc.params[(T_IDXS > 0.0) & (T_IDXS <= 0.40), 1] + assert len(preconditioned) and np.max(preconditioned) <= -0.25 def test_clear_road_launch_is_immediate_and_profiles_separate(): @@ -453,7 +455,9 @@ def test_low_speed_stopped_lead_never_accelerates_during_stop_hold(): stop_hold = trace.state == int(AccelControllerState.stopHold) assert urgent_demand.any() and stop_hold.any() assert np.max(trace.a_target[urgent_demand]) < 0.0 - assert np.max(trace.a_target[stop_hold]) <= 0.0 + assert np.max(trace.a_target[stop_hold]) < 0.1 + assert np.max(trace.speed[stop_hold]) < 0.30 + assert np.max(trace.acceleration[stop_hold]) <= 0.0 assert not _has_brake_gas_brake(trace.a_target[trace.time >= 1.0]) assert _first_time_below(trace, -1.0) <= _first_time_below(baseline, -1.0) + 1e-9 assert np.min(trace.distance_lead - trace.distance) >= np.min(baseline.distance_lead - baseline.distance) - 1e-3 @@ -488,6 +492,37 @@ def test_moving_lead_dropout_and_false_relief_do_not_release_pace(): assert trace.solver_failures <= baseline.solver_failures +def test_confirmed_finite_relief_stays_latched_and_smooth(): + def lead_speed(current_time: float) -> float: + return 8.0 if current_time < 5.0 else min(15.0, 8.0 + 3.5 * (current_time - 5.0)) + + common = dict( + duration=9.0, + profile=1, + lead_relevancy=True, + speed=12.0, + distance_lead=50.0, + v_lead=lead_speed, + v_cruise=20.0, + actuator_delay=0.15, + actuator_lag=0.20, + ) + baseline = _run(controller_enabled=False, **common) + trace = _run(controller_enabled=True, **common) + + released = np.flatnonzero((trace.time >= 5.0) & (trace.state == int(AccelControllerState.release))) + assert len(released) + reached_base = np.flatnonzero((np.arange(len(trace.time)) >= released[0]) & (trace.target_speed >= 20.0 - 1e-6)) + release_end = reached_base[0] if len(reached_base) else len(trace.time) + rising = (np.arange(len(trace.time)) >= released[0]) & (np.arange(len(trace.time)) < release_end) + assert rising.any() + assert np.all(trace.state[rising] == int(AccelControllerState.release)) + assert np.all(np.diff(trace.target_speed[rising]) >= -1e-9) + assert not _has_brake_gas_brake(trace.a_target[trace.time >= 5.0]) + assert np.max(np.abs(_command_jerk(trace, after=5.0))) <= np.max(np.abs(_command_jerk(baseline, after=5.0))) + 0.1 + assert trace.solver_failures <= baseline.solver_failures + + def test_low_speed_far_lead_acquisition_does_not_fault_or_lurch(): acquisition_time = 5.0 @@ -601,9 +636,11 @@ def test_decelerating_moving_lead_unwinds_without_brake_gas_brake(profile): baseline = _run(controller_enabled=False, **common) trace = _run(controller_enabled=True, **common) after_lead_settles = trace.time >= 8.0 - moving_lead_decel = trace.effective_accel_max < 0.0 - assert moving_lead_decel.any() - assert np.min(trace.effective_accel_max[moving_lead_decel]) >= MOVING_LEAD_DECEL_ACCEL_MAX - 1e-9 + lead_decelerating = (trace.time >= 2.0) & (trace.time <= 8.0) & trace.active + moving_lead_guard = trace.effective_accel_max < 0.0 + assert moving_lead_guard.any() + assert np.min(trace.effective_accel_max[moving_lead_guard]) >= MOVING_LEAD_DECEL_ACCEL_MAX - 1e-9 + assert np.all(np.diff(trace.pace[lead_decelerating]) <= 1e-9) assert not trace.should_stop[after_lead_settles].any() assert np.max(np.abs(_command_jerk(trace, after=1.0))) < 3.5 assert np.min(trace.speed[after_lead_settles]) >= 2.0