mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-10-01 01:43:41 +08:00
Code Issues Packages Projects Releases Wiki Activity

Smooth slow-lead braking handoffs

Browse Source
This commit is contained in:
rav4kumar
2026-07-16 14:34:11 -07:00
parent 1dc2ed7901
commit 1aa85675d1
3 changed files with 404 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py
+172 -15
View File
@@ -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:
sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py
+117 -6
View File
@@ -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)
sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py
+115 -12
View File
@@ -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"),
[