mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-08-22 02:33:47 +08:00
Code Issues Packages Projects Releases Wiki Activity

Deliver prompt takeoffs without sacrificing smooth lead approaches

Browse Source
This commit is contained in:
rav4kumar
2026-07-16 13:23:06 -07:00
parent 52d7dd58a7
commit 1dc2ed7901
7 changed files with 1026 additions and 183 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
+25 -4
View File
@@ -243,6 +243,7 @@ class LongitudinalMpc:
self.last_cloudlog_t = 0
self.status = False
self.crash_cnt = 0.0
self.lead_obstacle_weights = np.ones(2)
self.solution_status = 0
# timers
self.solve_time = 0.0
@@ -315,7 +316,8 @@ class LongitudinalMpc:
return lead_xv
def update(self, radarstate, v_cruise, personality=log.LongitudinalPersonality.standard,
accel_max: float | tuple[float, ...] | np.ndarray | None = None, shape_accel_max_in_cruise: bool = False):
accel_max: float | tuple[float, ...] | np.ndarray | None = None, shape_accel_max_in_cruise: bool = False,
lead_obstacle_weights: tuple[float, float] | np.ndarray | None = None):
t_follow = get_T_FOLLOW(personality)
v_ego = self.x0[1]
self.status = radarstate.leadOne.status or radarstate.leadTwo.status
@@ -326,8 +328,8 @@ class LongitudinalMpc:
# To estimate a safe distance from a moving lead, we calculate how much stopping
# distance that lead needs as a minimum. We can add that to the current distance
# and then treat that as a stopped car/obstacle at this new distance.
lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1])
lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1])
raw_lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1])
raw_lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1])
custom_accel_max = False
accel_max_traj = ACCEL_MAX * np.ones(N + 1)
@@ -337,7 +339,7 @@ class LongitudinalMpc:
accel_max_input = np.full(N + 1, float(accel_max_input))
custom_accel_max = accel_max_input.shape == (N + 1,) and np.all(np.isfinite(accel_max_input))
if custom_accel_max:
accel_max_traj = np.clip(accel_max_input, 0.0, ACCEL_MAX)
accel_max_traj = np.clip(accel_max_input, ACCEL_MIN, ACCEL_MAX)
# Fake an obstacle for cruise, this ensures smooth acceleration to set speed
# when the leads are no factor.
@@ -351,6 +353,25 @@ class LongitudinalMpc:
v_cruise_clipped = np.clip(v_cruise * np.ones(N+1), v_lower, v_upper)
cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, t_follow)
# The acceleration controller may gradually introduce a benign newly
# acquired obstacle to avoid a one-frame optimizer/source discontinuity.
# Raw lead trajectories remain untouched for FCW below, and missing or
# invalid weights preserve stock behavior exactly.
self.lead_obstacle_weights = np.ones(2)
if lead_obstacle_weights is not None:
weight_input = np.asarray(lead_obstacle_weights, dtype=float)
if weight_input.shape == (2,) and np.all(np.isfinite(weight_input)):
self.lead_obstacle_weights = np.clip(weight_input, 0.0, 1.0)
if np.array_equal(self.lead_obstacle_weights, np.ones(2)):
# Preserve the original arrays bit-for-bit on every bypass. Even an
# algebraically equivalent subtract/add can perturb the one-iteration
# solver at a standstill.
lead_0_obstacle = raw_lead_0_obstacle
lead_1_obstacle = raw_lead_1_obstacle
else:
lead_0_obstacle = cruise_obstacle + self.lead_obstacle_weights[0] * (raw_lead_0_obstacle - cruise_obstacle)
lead_1_obstacle = cruise_obstacle + self.lead_obstacle_weights[1] * (raw_lead_1_obstacle - cruise_obstacle)
x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle])
self.source = MPC_SOURCES[np.argmin(x_obstacles[0])]
selfdrive/controls/lib/longitudinal_planner.py
+1
View File
@@ -151,6 +151,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
sm['radarState'], v_cruise, personality=sm['selfdriveState'].personality,
accel_max=self.accel_controller_result.mpc_accel_max,
shape_accel_max_in_cruise=self.accel_controller_result.mpc_shape_cruise,
lead_obstacle_weights=self.accel_controller_result.lead_obstacle_weights,
)
self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)
selfdrive/test/longitudinal_maneuvers/plant.py
+121 -9
View File
@@ -1,6 +1,7 @@
#!/usr/bin/env python3
from collections import deque
from collections.abc import Callable
from dataclasses import dataclass
import math
import time
from typing import Any
@@ -19,6 +20,48 @@ from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
LeadObservation = dict[str, Any]
LeadObservationFn = Callable[[float, str, LeadObservation], LeadObservation | None]
ModelActionFn = Callable[[float, float, float], tuple[float, bool]]
EgoObservationFn = Callable[[float, float, float], tuple[float, float]]
@dataclass(frozen=True)
class ActuatorModel:
planner_delay: float
transport_delay: float
actuator_lag: float
command_rate_limit: float
stopping_acceleration: float
standstill_breakaway_acceleration: float
standstill_breakaway_time: float
def __post_init__(self):
nonnegative_fields = {
"planner_delay": self.planner_delay,
"transport_delay": self.transport_delay,
"actuator_lag": self.actuator_lag,
"standstill_breakaway_acceleration": self.standstill_breakaway_acceleration,
"standstill_breakaway_time": self.standstill_breakaway_time,
}
if any(not math.isfinite(value) or value < 0.0 for value in nonnegative_fields.values()):
raise ValueError(f"ActuatorModel fields must be finite and non-negative: {nonnegative_fields}")
if not math.isfinite(self.command_rate_limit) or self.command_rate_limit <= 0.0:
raise ValueError("command_rate_limit must be finite and positive")
if not math.isfinite(self.stopping_acceleration) or self.stopping_acceleration > 0.0:
raise ValueError("stopping_acceleration must be finite and non-positive")
# Route-derived conservative Prius TSS2 stress model for the acceleration-controller
# regression suite. The 1.0 m/s² gate represents prompt takeoffs, not a universal
# physical threshold: the supplied routes also contain low-command creep departures.
# This models vehicle response only and does not emulate Toyota's CAN controller.
PRIUS_TSS2_ROUTE_MODEL = ActuatorModel(
planner_delay=0.05,
transport_delay=0.0,
actuator_lag=0.20,
command_rate_limit=4.0,
stopping_acceleration=-2.0,
standstill_breakaway_acceleration=1.0,
standstill_breakaway_time=0.05,
)
class Plant:
@@ -37,8 +80,10 @@ class Plant:
force_decel=False,
lead_observation_fn: LeadObservationFn | None = None,
model_action_fn: ModelActionFn | None = None,
ego_observation_fn: EgoObservationFn | None = None,
actuator_delay: float | None = None,
actuator_lag: float = 0.0,
actuator_model: ActuatorModel | None = None,
):
"""Closed-loop longitudinal planner plant.
@@ -50,11 +95,20 @@ class Plant:
``model_action_fn(time, v_ego, a_ego)`` returns
``(desired_acceleration, should_stop)``.
``ego_observation_fn(time, true_v_ego, true_a_ego)`` returns the observed
``(v_ego, a_ego)`` published in ``carState``. It can inject measurement noise
without changing the physical plant state.
Passing ``actuator_delay`` both overrides ``CP.longitudinalActuatorDelay`` and
adds the corresponding command transport delay to the plant. ``None`` keeps the
historical Honda planner delay with instantaneous plant response. ``actuator_lag``
is an optional first-order acceleration-response time constant. Both defaults keep
historical plant dynamics unchanged.
``actuator_model`` opts into a staged vehicle-response model. Its planner delay
is used by MPC, while its independent transport delay is used by the command
queue before rate limiting, standstill breakaway confirmation, and first-order
lag. Leaving it unset preserves the historical actuator path.
"""
if actuator_delay is not None and (not math.isfinite(actuator_delay) or actuator_delay < 0.0):
raise ValueError("actuator_delay must be finite and non-negative")
@@ -79,6 +133,9 @@ class Plant:
self.acceleration = 0.0
self.a_target = 0.0
self.actuator_command = 0.0
self.applied_actuator_command = 0.0
self.breakaway_confirmed = False
self._breakaway_timer = 0.0
# lead car
self.lead_relevancy = lead_relevancy
@@ -91,9 +148,13 @@ class Plant:
self.force_decel = force_decel
self.lead_observation_fn = lead_observation_fn
self.model_action_fn = model_action_fn
self.actuator_delay = actuator_delay
self.actuator_lag = actuator_lag
self.publish_realized_a_ego = any((lead_observation_fn is not None, model_action_fn is not None, actuator_delay is not None, actuator_lag > 0.0))
self.ego_observation_fn = ego_observation_fn
self.actuator_model = actuator_model
self.actuator_delay = actuator_model.planner_delay if actuator_model is not None else actuator_delay
self.transport_delay = actuator_model.transport_delay if actuator_model is not None else actuator_delay
self.actuator_lag = actuator_model.actuator_lag if actuator_model is not None else actuator_lag
self.publish_realized_a_ego = any((lead_observation_fn is not None, model_action_fn is not None, ego_observation_fn is not None,
actuator_delay is not None, actuator_lag > 0.0, actuator_model is not None))
self.rk = Ratekeeper(self.rate, print_delay_threshold=100.0)
self.ts = 1.0 / self.rate
@@ -109,7 +170,9 @@ class Plant:
CP_SP = CarInterface.get_non_essential_params_sp(CP, CAR.HONDA_CIVIC)
self.planner = LongitudinalPlanner(CP, CP_SP, init_v=self.speed)
delay_steps = 0 if self.actuator_delay is None else round(self.actuator_delay / self.ts)
if self.actuator_model is not None and self.speed >= 0.01:
self.breakaway_confirmed = True
delay_steps = 0 if self.transport_delay is None else round(self.transport_delay / self.ts)
self._actuator_delay_queue = deque([self.acceleration] * delay_steps)
@property
@@ -144,11 +207,41 @@ class Plant:
else:
delayed_command = command
if self.actuator_model is not None:
max_command_delta = self.actuator_model.command_rate_limit * self.ts
self.applied_actuator_command = float(np.clip(delayed_command,
self.applied_actuator_command - max_command_delta,
self.applied_actuator_command + max_command_delta))
if self.speed < 0.01:
if self.applied_actuator_command <= 0.0:
self.breakaway_confirmed = False
self._breakaway_timer = 0.0
elif not self.breakaway_confirmed:
breakaway_ready = self.applied_actuator_command + 1e-9 >= self.actuator_model.standstill_breakaway_acceleration
if breakaway_ready:
self._breakaway_timer += self.ts
else:
self._breakaway_timer = 0.0
self.breakaway_confirmed = breakaway_ready and self._breakaway_timer + 1e-9 >= self.actuator_model.standstill_breakaway_time
if not self.breakaway_confirmed:
self.acceleration = 0.0
return delayed_command, self.acceleration
else:
self.breakaway_confirmed = True
response_command = self.applied_actuator_command
else:
# Preserve the historical response path exactly when no staged model is used.
self.applied_actuator_command = delayed_command
response_command = delayed_command
if self.actuator_lag > 0.0:
alpha = 1.0 - math.exp(-self.ts / self.actuator_lag)
self.acceleration += alpha * (delayed_command - self.acceleration)
self.acceleration += alpha * (response_command - self.acceleration)
else:
self.acceleration = delayed_command
self.acceleration = response_command
return delayed_command, self.acceleration
def step(self, v_lead=0.0, prob_lead=1.0, v_cruise=50.0, pitch=0.0, prob_throttle=1.0):
@@ -232,8 +325,13 @@ class Plant:
ss.selfdriveState.experimentalMode = self.e2e
ss.selfdriveState.personality = self.personality
control.controlsState.forceDecel = self.force_decel
car_state.carState.vEgo = float(self.speed)
published_a_ego = self.acceleration if self.publish_realized_a_ego else 0.0
true_v_ego = self.speed
true_a_ego = self.acceleration
published_v_ego = true_v_ego
published_a_ego = true_a_ego if self.publish_realized_a_ego else 0.0
if self.ego_observation_fn is not None:
published_v_ego, published_a_ego = self.ego_observation_fn(self.current_time, true_v_ego, true_a_ego)
car_state.carState.vEgo = float(published_v_ego)
car_state.carState.aEgo = float(published_a_ego)
car_state.carState.standstill = bool(self.speed < 0.01)
car_state.carState.vCruise = float(v_cruise * 3.6)
@@ -256,7 +354,8 @@ class Plant:
self.a_target = self.planner.output_a_target
self.actuator_command = self.a_target
if self.planner.output_should_stop:
self.actuator_command = min(-0.5, self.actuator_command)
stopping_acceleration = -0.5 if self.actuator_model is None else self.actuator_model.stopping_acceleration
self.actuator_command = min(stopping_acceleration, self.actuator_command)
delayed_actuator_command, _ = self._update_actuator(self.actuator_command)
self.speed = self.speed + self.acceleration * self.ts
self.should_stop = self.planner.output_should_stop
@@ -293,9 +392,22 @@ class Plant:
"acceleration": self.acceleration,
"realized_acceleration": self.acceleration,
"a_target": self.a_target,
"planner_acceleration": self.a_target,
"actuator_command": self.actuator_command,
"stop_clamped_actuator_command": self.actuator_command,
"delayed_actuator_command": delayed_actuator_command,
"applied_actuator_command": self.applied_actuator_command,
"vehicle_actuator_command": self.applied_actuator_command,
"true_v_ego": true_v_ego,
"true_a_ego": true_a_ego,
"published_a_ego": published_a_ego,
"published_v_ego": published_v_ego,
"observed_a_ego": published_a_ego,
"observed_v_ego": published_v_ego,
"planner_delay": self.actuator_delay,
"transport_delay": self.transport_delay,
"breakaway_confirmed": self.breakaway_confirmed,
"breakaway_time": self._breakaway_timer,
"should_stop": self.should_stop,
"distance_lead": self.distance_lead,
"fcw": fcw,
sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py
+332 -100
View File
@@ -7,7 +7,7 @@ import math
import numpy as np
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_mpc_lib.long_mpc import (
LongitudinalMpc,
@@ -15,6 +15,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import (
STOP_DISTANCE,
T_IDXS,
get_T_FOLLOW,
get_stopped_equivalence_factor,
)
@@ -47,10 +48,12 @@ PROFILE_CONFIGS = {
}
ACCEL_PROFILE_MAX_BP = [0.0, 10.0, 25.0, 40.0]
# These are pre-MPC profile requests. Values remain inside global ACCEL_MAX;
# the planner's stock speed/turn/coast limit remains the final output authority.
ACCEL_PROFILE_MAX_V = {
AccelProfile.eco: [0.95, 0.70, 0.42, 0.28],
AccelProfile.normal: [1.30, 1.00, 0.65, 0.45],
AccelProfile.sport: [1.55, 1.15, 0.78, 0.58],
AccelProfile.eco: [1.55, 0.30, 0.20, 0.10],
AccelProfile.normal: [1.70, 0.90, 0.40, 0.20],
AccelProfile.sport: [2.00, 1.70, 1.20, 0.90],
}
LAUNCH_DELTA_V = 3.0
@@ -60,22 +63,37 @@ RELIEF_DEADBAND = 0.35
STOP_HOLD_EGO_SPEED = 0.30
STOP_HOLD_CAP = 0.50
STOPPED_LEAD_SPEED = 0.30
STOP_HOLD_EXIT_CAP = 0.80
LEAD_DEPARTURE_SPEED = 0.30
STOP_HOLD_EXIT_FRAMES = 4
CLEAR_ROAD_PROFILE_SPEED = 0.20
LAUNCH_PROFILE_HANDOFF_SPEED = 0.05
VEGO_NOISE_TOLERANCE = 0.10
ACCEL_LIMIT_JERK = 1.0
LAUNCH_ACCEL_JERK = 3.0
DECEL_LIMIT_JERK = 1.10
LAUNCH_ACCEL_RATE = 4.0
CLEAR_LAUNCH_ACCEL_RATE = 3.0
INITIAL_LAUNCH_ACCEL_MAX = 0.95
BREAKAWAY_ACCEL_MAX = 1.15
HOLD_ACCEL_MAX = 0.10
LAUNCH_PACE_RATE = 5.0
MPC_LAUNCH_BOUND_NODES = 2
MPC_STOP_WARM_BLEND = 0.0
MPC_CONFIRM_WARM_BLEND = 0.10
MPC_DEPART_WARM_BLEND = 0.25
LEAD_ACQUISITION_INITIAL_AUTHORITY = 0.20
LEAD_ACQUISITION_TIME = 0.30
LEAD_ACQUISITION_MIN_DISTANCE = 60.0
LEAD_ACQUISITION_MIN_HEADWAY = 3.0
LEAD_ACQUISITION_MIN_TTC = 10.0
LEAD_ACQUISITION_MAX_DECEL = 0.25
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_MIN_SPEED = 5.0
@dataclass(frozen=True)
class EnergyEnvelope:
cap: float = math.inf
selected_lead: int = -1
departure_lead_speed: float = math.inf
usable_gap: float = math.inf
closing_speed: float = 0.0
required_decel: float = 0.0
@@ -94,6 +112,7 @@ class AccelControllerResult:
effective_accel_max: float
mpc_accel_max: tuple[float, ...] | None
mpc_shape_cruise: bool
lead_obstacle_weights: tuple[float, float]
state: AccelControllerState
shadow_state: AccelControllerState
base_speed: float
@@ -116,8 +135,15 @@ class _PacePath:
relief_time: float = 0.0
departure_frames: int = 0
departing_from_stop: bool = False
departure_handoff_active: bool = False
stop_departure_confirmed: bool = False
stopped_lead_hold: bool = False
accel_limit: float | None = None
decel_limit_active: bool = False
urgent_bypass_active: bool = False
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])
def reset(self) -> None:
self.cap_samples = deque([math.inf] * CAP_FILTER_FRAMES, maxlen=CAP_FILTER_FRAMES)
@@ -126,8 +152,15 @@ class _PacePath:
self.relief_time = 0.0
self.departure_frames = 0
self.departing_from_stop = False
self.departure_handoff_active = False
self.stop_departure_confirmed = False
self.stopped_lead_hold = False
self.accel_limit = None
self.decel_limit_active = False
self.urgent_bypass_active = False
self.lead_seen = [False, False]
self.lead_track_ids = [-1, -1]
self.lead_obstacle_weights = [1.0, 1.0]
def update_filter(self, cap: float) -> float:
self.cap_samples.append(cap)
@@ -139,7 +172,7 @@ class _PacePath:
class AccelController:
"""A relative-pace governor with a positive-acceleration comfort ceiling."""
"""A relative-pace governor with a pre-MPC acceleration comfort ceiling."""
def __init__(self, CP, dt: float = DT_MDL):
if not math.isfinite(dt) or dt <= 0.0:
@@ -202,7 +235,7 @@ class AccelController:
x_ego, v_ego_delay = self._project_ego(v_ego, a_ego, delay)
candidates: list[EnergyEnvelope] = []
nearly_stopped = False
departure_candidates: list[tuple[float, float]] = []
for lead_index, lead in enumerate((radar_state.leadOne, radar_state.leadTwo)):
if not self._valid_lead(lead):
@@ -215,7 +248,7 @@ class AccelController:
lead_xv = LongitudinalMpc.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau)
x_lead_delay = float(np.interp(delay, T_IDXS, lead_xv[:, 0]))
v_lead_delay = float(np.interp(delay, T_IDXS, lead_xv[:, 1]))
nearly_stopped = nearly_stopped or v_lead_delay < STOPPED_LEAD_SPEED
departure_candidates.append((x_lead_delay + float(get_stopped_equivalence_factor(v_lead_delay)), v_lead_delay))
match_gap = STOP_DISTANCE + t_follow * v_lead_delay
usable_gap = max(x_lead_delay - x_ego - match_gap, 0.0)
@@ -228,14 +261,144 @@ class AccelController:
required_decel = closing_speed * closing_speed / (2.0 * usable_gap)
# Relative kinetic energy: the lead keeps moving while ego sheds closing speed.
cap = v_lead_delay + math.sqrt(2.0 * config.comfort_decel * usable_gap)
candidates.append(EnergyEnvelope(cap, lead_index, usable_gap, closing_speed, required_decel))
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)
candidates.append(EnergyEnvelope(
cap=cap,
selected_lead=lead_index,
usable_gap=usable_gap,
closing_speed=closing_speed,
required_decel=required_decel,
))
if not candidates:
return EnergyEnvelope(has_nearly_stopped_lead=nearly_stopped)
return EnergyEnvelope()
selected = min(candidates, key=lambda candidate: candidate.cap)
return EnergyEnvelope(selected.cap, selected.selected_lead, selected.usable_gap, selected.closing_speed, selected.required_decel, nearly_stopped)
departure_lead_speed = min(departure_candidates, key=lambda candidate: candidate[0])[1]
return EnergyEnvelope(
cap=selected.cap,
selected_lead=selected.selected_lead,
departure_lead_speed=departure_lead_speed,
usable_gap=selected.usable_gap,
closing_speed=selected.closing_speed,
required_decel=selected.required_decel,
has_nearly_stopped_lead=departure_lead_speed < STOPPED_LEAD_SPEED,
)
def _lead_acquisition_is_benign(
self,
lead,
v_ego: float,
a_ego: float,
planner_accel: float,
follow_personality,
) -> bool:
"""Return whether a new lead can enter the optimizer gradually without delaying needed braking."""
if not self._valid_lead(lead) or planner_accel <= LEAD_ACQUISITION_MIN_PLANNER_ACCEL:
return False
try:
t_follow = get_T_FOLLOW(follow_personality)
except (NotImplementedError, TypeError, ValueError):
t_follow = get_T_FOLLOW(log.LongitudinalPersonality.standard)
delay = self._delay()
x_ego, v_ego_delay = self._project_ego(v_ego, a_ego, delay)
lead_xv = LongitudinalMpc.extrapolate_lead(
float(lead.dRel),
float(lead.vLeadK),
float(np.clip(lead.aLeadK, -10.0, 5.0)),
float(lead.aLeadTau),
)
x_lead_delay = float(np.interp(delay, T_IDXS, lead_xv[:, 0]))
v_lead_delay = float(np.interp(delay, T_IDXS, lead_xv[:, 1]))
separation = max(x_lead_delay - x_ego, 0.0)
closing_speed = max(v_ego_delay - v_lead_delay, 0.0)
ttc = separation / closing_speed if closing_speed > 0.0 else math.inf
usable_gap = max(separation - STOP_DISTANCE - t_follow * v_lead_delay, 0.0)
if closing_speed == 0.0:
required_decel = 0.0
elif usable_gap == 0.0:
required_decel = math.inf
else:
required_decel = closing_speed * closing_speed / (2.0 * usable_gap)
return (
separation > max(LEAD_ACQUISITION_MIN_DISTANCE, LEAD_ACQUISITION_MIN_HEADWAY * v_ego_delay)
and ttc > LEAD_ACQUISITION_MIN_TTC
and required_decel < LEAD_ACQUISITION_MAX_DECEL
and float(lead.aLeadK) > LEAD_ACQUISITION_MIN_LEAD_ACCEL
)
def _update_lead_obstacle_weights(
self,
path: _PacePath,
radar_state,
v_ego: float,
a_ego: float,
planner_accel: float,
follow_personality,
*,
allow_blend: bool,
) -> tuple[float, float]:
"""Ramp benign new obstacles into MPC while making every urgent lead immediate."""
authority_step = (1.0 - LEAD_ACQUISITION_INITIAL_AUTHORITY) * self.dt / LEAD_ACQUISITION_TIME
departure_step = self.dt / LEAD_DEPARTURE_HANDOFF_TIME
for lead_index, lead in enumerate((radar_state.leadOne, radar_state.leadTwo)):
if not self._valid_lead(lead):
path.lead_seen[lead_index] = False
path.lead_track_ids[lead_index] = -1
path.lead_obstacle_weights[lead_index] = 1.0
continue
track_id = int(getattr(lead, "radarTrackId", -1))
previous_track_id = path.lead_track_ids[lead_index]
positive_track_change = track_id >= 0 and previous_track_id >= 0 and track_id != previous_track_id
new_lead = not path.lead_seen[lead_index] or positive_track_change
benign = self._lead_acquisition_is_benign(lead, v_ego, a_ego, planner_accel, follow_personality)
safe_departure_handoff = (
path.departure_handoff_active
and float(lead.vLeadK) > LEAD_DEPARTURE_SPEED
and v_ego <= float(lead.vLeadK)
)
if safe_departure_handoff:
if path.departing_from_stop:
path.lead_obstacle_weights[lead_index] = 0.0
else:
path.lead_obstacle_weights[lead_index] = min(1.0, path.lead_obstacle_weights[lead_index] + departure_step)
elif new_lead:
path.lead_obstacle_weights[lead_index] = LEAD_ACQUISITION_INITIAL_AUTHORITY if allow_blend and benign else 1.0
elif path.lead_obstacle_weights[lead_index] < 1.0:
if benign:
path.lead_obstacle_weights[lead_index] = min(1.0, path.lead_obstacle_weights[lead_index] + authority_step)
else:
path.lead_obstacle_weights[lead_index] = 1.0
path.lead_seen[lead_index] = True
path.lead_track_ids[lead_index] = track_id
if path.state == AccelControllerState.stopHold:
try:
v_ego_stopping = float(self.CP.vEgoStopping)
except (AttributeError, TypeError, ValueError):
v_ego_stopping = STOP_HOLD_EGO_SPEED
if not math.isfinite(v_ego_stopping) or v_ego_stopping < 0.0:
v_ego_stopping = STOP_HOLD_EGO_SPEED
# Once stock's shouldStop threshold is reachable, use the zero-speed
# cruise obstacle to keep the stopped solver warm. Above that threshold,
# retain full raw-lead authority: a zero acceleration ceiling alone does
# not require enough braking for a newly detected close stopped lead.
hold_weight = 0.0 if v_ego < v_ego_stopping else 1.0
path.lead_obstacle_weights = [hold_weight, hold_weight]
elif path.departure_handoff_active and all(
not seen or weight >= 1.0 for seen, weight in zip(path.lead_seen, path.lead_obstacle_weights, strict=True)
):
path.departure_handoff_active = False
return tuple(path.lead_obstacle_weights)
def reset(self) -> None:
self.live.reset()
@@ -256,20 +419,27 @@ class AccelController:
planner_speed: float,
previous_should_stop: bool,
has_nearly_stopped_lead: bool,
departure_lead_speed: float,
closing_speed: float,
launch_delta_v: float,
) -> float:
filtered_cap = path.update_filter(raw_cap)
just_initialized = path.pace is None
if just_initialized:
path.pace = min(base_speed, v_ego)
# A clear road has no prior restriction to release from, so expose base
# cruise immediately. With any lead present, seed at ego instead: the
# first radar frame can contain a large aLeadK spike, and using that
# transient energy cap as pace caused several seconds of acceleration
# before a late brake.
path.pace = base_speed if not math.isfinite(raw_cap) else min(base_speed, v_ego)
path.state = AccelControllerState.free
# A clear-road standstill engagement should request motion immediately. A
# stopped/previously-stopping lead still goes through stop-hold confirmation.
if just_initialized and v_ego < STOP_HOLD_EGO_SPEED and not math.isfinite(raw_cap) and not previous_should_stop:
path.pace = min(base_speed, v_ego + launch_delta_v)
path.pace = base_speed
path.state = AccelControllerState.release
path.relief_time = config.release_confirm
path.relief_time = 0.0
path.departing_from_stop = True
return filtered_cap
@@ -281,26 +451,33 @@ class AccelController:
if v_ego < STOP_HOLD_EGO_SPEED and (filtered_cap < STOP_HOLD_CAP or has_nearly_stopped_lead):
path.stopped_lead_hold = True
clear_road_launch_complete = path.departing_from_stop and not path.stopped_lead_hold and v_ego >= CLEAR_ROAD_PROFILE_SPEED
clear_road_launch_complete = path.departing_from_stop and not path.stopped_lead_hold and v_ego >= LAUNCH_PROFILE_HANDOFF_SPEED
if v_ego >= STOP_HOLD_EGO_SPEED or clear_road_launch_complete:
path.departing_from_stop = False
path.stopped_lead_hold = False
if v_ego >= STOP_HOLD_EGO_SPEED:
path.stop_departure_confirmed = False
renewed_stop_evidence = filtered_cap < STOP_HOLD_CAP or has_nearly_stopped_lead
enter_stop_hold = v_ego < STOP_HOLD_EGO_SPEED and (renewed_stop_evidence or (previous_should_stop and not path.departing_from_stop))
stale_plan_stop = previous_should_stop and not path.departing_from_stop and not path.stop_departure_confirmed
enter_stop_hold = v_ego < STOP_HOLD_EGO_SPEED and (renewed_stop_evidence or stale_plan_stop)
if enter_stop_hold and path.state != AccelControllerState.stopHold:
path.pace = 0.0
path.state = AccelControllerState.stopHold
path.relief_time = 0.0
path.departure_frames = 0
path.departing_from_stop = False
path.departure_handoff_active = False
path.stop_departure_confirmed = False
return filtered_cap
if path.state == AccelControllerState.stopHold:
# A continuously observed moving lead exits after exactly four raw frames.
# Total lead loss still waits for the five-frame median dropout guard first.
raw_departure = math.isfinite(raw_cap) and raw_cap > STOP_HOLD_EXIT_CAP and not has_nearly_stopped_lead
guarded_lead_loss = not math.isfinite(raw_cap) and filtered_cap > STOP_HOLD_EXIT_CAP
# Departure is a perception fact, not a comfort-profile decision. Confirm
# the selected lead's projected motion directly so Eco cannot wait longer
# merely because its energy envelope is lower. Total lead loss still waits
# for the five-frame median dropout guard before confirmation begins.
raw_departure = math.isfinite(departure_lead_speed) and departure_lead_speed > LEAD_DEPARTURE_SPEED
guarded_lead_loss = not math.isfinite(raw_cap) and not math.isfinite(filtered_cap)
if raw_departure or guarded_lead_loss:
path.departure_frames += 1
else:
@@ -314,15 +491,24 @@ class AccelController:
path.relief_time = config.release_confirm
path.departure_frames = 0
path.departing_from_stop = True
path.departure_handoff_active = True
path.stop_departure_confirmed = True
path.stopped_lead_hold = False
path.pace = min(base_speed, filtered_cap, v_ego + launch_delta_v)
return filtered_cap
ceiling = min(base_speed, filtered_cap)
if math.isfinite(raw_cap) and closing_speed > 0.0:
# Never spend stored gap by accelerating toward a slower lead. Hold the
# current pace until relative speed is matched; the energy envelope may
# still lower it at the configured comfort rate.
ceiling = min(ceiling, path.pace)
if ceiling <= path.pace - RESTRICT_DEADBAND:
path.pace = max(ceiling, path.pace - config.comfort_decel * self.dt)
path.state = AccelControllerState.restrict
path.relief_time = 0.0
path.departing_from_stop = False
path.departure_handoff_active = False
return filtered_cap
relief = ceiling - path.pace
@@ -348,32 +534,69 @@ class AccelController:
stock_accel_max: float,
planner_accel: float,
profile_accel_max: float,
config: ProfileConfig,
) -> tuple[float, float]:
"""Return telemetry effective max and the controller's pre-MPC positive bound."""
requested_limit = float(np.clip(profile_accel_max, 0.0, ACCEL_MAX))
"""Return telemetry effective max and the controller's pre-MPC upper bound."""
profile_limit = float(np.clip(profile_accel_max, 0.0, ACCEL_MAX))
if path.state == AccelControllerState.stopHold:
# Keep the entire reachable cruise trajectory at standstill. Unlike the
# old mixed zero/warm-node horizon, this is internally consistent and
# opens in time on departure instead of changing shape in one frame.
path.accel_limit = 0.0
path.decel_limit_active = False
return min(stock_accel_max, 0.0), 0.0
if path.departing_from_stop:
if path.stopped_lead_hold:
# A confirmed lead departure opens quickly but continuously from zero.
path.decel_limit_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
# high enough to overcome measured standstill deadband. Normal and
# Sport may continue above that common floor, but all profiles begin
# physical motion at the same time.
launch_target = min(ACCEL_MAX, max(BREAKAWAY_ACCEL_MAX, profile_limit, planner_seed))
previous_limit = path.accel_limit if path.accel_limit is not None else 0.0
path.accel_limit = min(requested_limit, previous_limit + LAUNCH_ACCEL_JERK * self.dt)
path.accel_limit = min(launch_target, previous_limit + LAUNCH_ACCEL_RATE * self.dt)
else:
# The MPC stays completely stock for the first few centimeters of a
# clear-road launch. Seed the selected table value for a smooth handoff.
path.accel_limit = requested_limit
# Start below the solver's standstill cold-start edge, then reach the
# common breakaway floor over two controller frames. The lookup table
# takes over after the first few centimeters.
if path.accel_limit is None:
path.accel_limit = min(INITIAL_LAUNCH_ACCEL_MAX, BREAKAWAY_ACCEL_MAX)
else:
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:
requested_limit = -config.comfort_decel
if not path.decel_limit_active:
# Preserve an existing pre-MPC ceiling when restriction begins. The
# planner's scalar acceleration is the near-time state, while the
# commanded target is sampled later for actuator delay; replacing the
# ceiling with that scalar can therefore create a one-frame command
# drop. A newly initialized path has no prior ceiling to preserve and
# is safely seeded from the current planner state.
if path.accel_limit is None:
path.accel_limit = max(0.0, planner_accel)
path.decel_limit_active = True
elif path.state == 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
path.decel_limit_active = False
else:
requested_limit = profile_limit
path.decel_limit_active = False
if path.accel_limit is None:
# Avoid a discontinuity when enabling around an already-positive command.
# The global OP limit bounds this seed; dynamic stock output constraints
# still retain their existing output-side enforcement and slew.
path.accel_limit = min(ACCEL_MAX, max(requested_limit, max(0.0, planner_accel)))
else:
max_step = ACCEL_LIMIT_JERK * self.dt
transition_jerk = DECEL_LIMIT_JERK if path.decel_limit_active or path.accel_limit < 0.0 else ACCEL_LIMIT_JERK
max_step = transition_jerk * self.dt
path.accel_limit = float(np.clip(requested_limit, path.accel_limit - max_step, path.accel_limit + max_step))
effective_limit = min(stock_accel_max, path.accel_limit)
@@ -382,49 +605,14 @@ class AccelController:
def _build_mpc_accel_max(
self,
path: _PacePath,
envelope: EnergyEnvelope,
filtered_cap: float,
previous_mpc_source,
accel_limit: float,
) -> tuple[float, ...] | None:
"""Build a short pre-MPC bound while leaving the future horizon stock-warm."""
# Stock tip-in removes launch delay and gives every profile the same initial
# response. The lookup table becomes active once the car is barely rolling.
if path.departing_from_stop and not path.stopped_lead_hold:
return None
# A short total-lead dropout has no obstacle to hold stock MPC at zero.
# Bound the whole horizon only while the median guard still says "stopped";
# genuine loss transitions to the tapered confirmation path below.
if path.state == AccelControllerState.stopHold and envelope.selected_lead < 0 and path.departure_frames == 0:
return tuple(0.0 for _ in T_IDXS)
special_launch_state = path.state == AccelControllerState.stopHold or path.departing_from_stop
# Ordinary lead following must retain stock MPC constraints and obstacle
# behavior. Include filtered and previous-source state so a radar dropout
# cannot switch the profile bound on for only one or two frames.
lead_guarded = envelope.selected_lead >= 0 or math.isfinite(filtered_cap) or self._lead_source(previous_mpc_source)
if not special_launch_state and lead_guarded:
return None
"""Build the controller's pre-MPC acceleration upper-bound trajectory."""
if not math.isfinite(accel_limit):
return None
bounded_limit = float(np.clip(accel_limit, 0.0, ACCEL_MAX))
accel_max = np.full(len(T_IDXS), bounded_limit, dtype=float)
if special_launch_state:
# A hard low bound across the full action-delay horizon cold-soaks the
# stop solver. Two bounded nodes plus one tapered warm-up node holds the
# vehicle through confirmation while preserving a ready future solution.
accel_max[MPC_LAUNCH_BOUND_NODES:] = ACCEL_MAX
if len(accel_max) > MPC_LAUNCH_BOUND_NODES:
if path.state == AccelControllerState.stopHold:
warm_blend = MPC_CONFIRM_WARM_BLEND if path.departure_frames > 0 else MPC_STOP_WARM_BLEND
else:
warm_blend = MPC_DEPART_WARM_BLEND
accel_max[MPC_LAUNCH_BOUND_NODES] = bounded_limit + warm_blend * (ACCEL_MAX - bounded_limit)
return tuple(float(value) for value in accel_max)
bounded_limit = float(np.clip(accel_limit, ACCEL_MIN, ACCEL_MAX))
return tuple(bounded_limit for _ in T_IDXS)
@staticmethod
def _valid_context(
@@ -444,7 +632,7 @@ class AccelController:
and cruise_initialized
and not controller_fault
and base_speed >= 0.0
and v_ego >= 0.0
and v_ego >= -VEGO_NOISE_TOLERANCE
and planner_speed >= 0.0
and delay >= 0.0
and all(math.isfinite(value) for value in (base_speed, v_ego, a_ego, planner_speed, stock_accel_max, planner_accel, delay))
@@ -472,13 +660,16 @@ class AccelController:
) -> AccelControllerResult:
"""Update live and shadow acceleration controllers and return the target and additive telemetry."""
profile = self._profile(profile)
# Toyota wheel-speed filtering can report a few cm/s negative at a stop.
# Treat that as zero without allowing a real invalid state to persist.
sanitized_v_ego = max(v_ego, 0.0) if math.isfinite(v_ego) and v_ego >= -VEGO_NOISE_TOLERANCE else v_ego
config = PROFILE_CONFIGS[profile]
profile_accel_max = self.get_profile_accel_max(profile, v_ego)
profile_accel_max = self.get_profile_accel_max(profile, sanitized_v_ego)
launch_delta_v = LAUNCH_DELTA_V
delay = self._delay()
valid_context = self._valid_context(
base_speed,
v_ego,
sanitized_v_ego,
a_ego,
planner_speed,
stock_accel_max,
@@ -489,22 +680,24 @@ class AccelController:
controller_fault,
)
envelope = self.calculate_energy_envelope(radar_state, v_ego, a_ego, profile, follow_personality) if valid_context else EnergyEnvelope()
envelope = self.calculate_energy_envelope(radar_state, sanitized_v_ego, a_ego, profile, follow_personality) if valid_context else EnergyEnvelope()
if valid_context:
shadow_filtered_cap = self._update_path(
self.shadow,
envelope.cap,
base_speed,
v_ego,
sanitized_v_ego,
config,
previous_mpc_source,
planner_speed,
previous_should_stop,
envelope.has_nearly_stopped_lead,
envelope.departure_lead_speed,
envelope.closing_speed,
launch_delta_v,
)
self._update_accel_limit(self.shadow, stock_accel_max, planner_accel, profile_accel_max)
self._update_accel_limit(self.shadow, stock_accel_max, planner_accel, profile_accel_max, config)
shadow_active = True
else:
self.shadow.reset()
@@ -513,43 +706,80 @@ class AccelController:
live_active = valid_context and bool(enabled) and bool(acc_selected)
if live_active:
live_was_initialized = self.live.pace is not None
established_selected_lead = False
if envelope.selected_lead in (0, 1):
selected_lead = (radar_state.leadOne, radar_state.leadTwo)[envelope.selected_lead]
selected_track_id = int(getattr(selected_lead, "radarTrackId", -1))
previous_track_id = self.live.lead_track_ids[envelope.selected_lead]
positive_track_change = selected_track_id >= 0 and previous_track_id >= 0 and selected_track_id != previous_track_id
established_selected_lead = self.live.lead_seen[envelope.selected_lead] and not positive_track_change
live_filtered_cap = self._update_path(
self.live,
envelope.cap,
base_speed,
v_ego,
sanitized_v_ego,
config,
previous_mpc_source,
planner_speed,
previous_should_stop,
envelope.has_nearly_stopped_lead,
envelope.departure_lead_speed,
envelope.closing_speed,
launch_delta_v,
)
effective_accel_max, controller_accel_max = self._update_accel_limit(
self.live, stock_accel_max, planner_accel, profile_accel_max
lead_obstacle_weights = self._update_lead_obstacle_weights(
self.live,
radar_state,
sanitized_v_ego,
a_ego,
planner_accel,
follow_personality,
allow_blend=live_was_initialized,
)
# Feed only the controller-owned ceiling into MPC. Stock's speed, turn,
# coast, and no-throttle limits remain in their original output clip.
mpc_accel_max = self._build_mpc_accel_max(
self.live, envelope, live_filtered_cap, previous_mpc_source, controller_accel_max,
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)
)
mpc_shape_cruise = (
mpc_accel_max is not None
and self.live.state != AccelControllerState.stopHold
and not self.live.departing_from_stop
urgent_bypass = urgent_trigger or (
self.live.urgent_bypass_active and envelope.selected_lead >= 0 and envelope.closing_speed > 0.10
)
if mpc_accel_max is None:
self.live.urgent_bypass_active = urgent_bypass
if 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.
self.live.accel_limit = None
self.live.decel_limit_active = False
effective_accel_max = stock_accel_max
if self.live.state == AccelControllerState.stopHold:
# Bounds provide the dropout/creep guard while the stock cruise target
# keeps the solver ready for a confirmed departure.
target_speed = base_speed
elif self.live.departing_from_stop and v_ego < STOP_HOLD_EGO_SPEED and envelope.selected_lead >= 0:
# A moving lead keeps stock MPC well-conditioned during a confirmed
# departure. Clear-road launches retain the bounded live pace below.
mpc_accel_max = None
mpc_shape_cruise = False
lead_obstacle_weights = (1.0, 1.0)
target_speed = base_speed
else:
target_speed = min(base_speed, self.live.pace if self.live.pace is not None else base_speed)
effective_accel_max, controller_accel_max = self._update_accel_limit(
self.live, stock_accel_max, planner_accel, profile_accel_max, config
)
# Feed only the controller-owned ceiling into MPC. Stock's speed, turn,
# coast, and no-throttle limits remain in their original output clip.
mpc_accel_max = self._build_mpc_accel_max(self.live, controller_accel_max)
mpc_shape_cruise = mpc_accel_max is not None
if mpc_accel_max is None:
effective_accel_max = stock_accel_max
if self.live.state == AccelControllerState.stopHold:
# Pin the cruise obstacle to zero as well as the acceleration upper
# bound. Some platforms declare shouldStop below this controller's
# 0.30 m/s hold threshold; keeping base cruise there can otherwise
# permit a slow coast while lead authority is intentionally muted.
target_speed = 0.0
elif self.live.departing_from_stop:
# 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
else:
target_speed = min(base_speed, self.live.pace if self.live.pace is not None else base_speed)
else:
self.live.reset()
live_filtered_cap = math.inf
@@ -558,6 +788,7 @@ class AccelController:
effective_accel_max = math.inf
mpc_accel_max = None
mpc_shape_cruise = False
lead_obstacle_weights = (1.0, 1.0)
return AccelControllerResult(
target_speed=target_speed,
@@ -570,6 +801,7 @@ class AccelController:
effective_accel_max=effective_accel_max,
mpc_accel_max=mpc_accel_max,
mpc_shape_cruise=mpc_shape_cruise,
lead_obstacle_weights=lead_obstacle_weights,
state=self.live.state,
shadow_state=self.shadow.state,
base_speed=base_speed,
sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py
+320 -69
View File
@@ -6,6 +6,7 @@ import numpy as np
import pytest
from cereal import log
from opendbc.car.interfaces import ACCEL_MAX
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
@@ -13,11 +14,14 @@ from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_control
ACCEL_LIMIT_JERK,
ACCEL_PROFILE_MAX_BP,
ACCEL_PROFILE_MAX_V,
LAUNCH_ACCEL_JERK,
BREAKAWAY_ACCEL_MAX,
CLEAR_LAUNCH_ACCEL_RATE,
DECEL_LIMIT_JERK,
INITIAL_LAUNCH_ACCEL_MAX,
LAUNCH_ACCEL_RATE,
LAUNCH_DELTA_V,
MPC_CONFIRM_WARM_BLEND,
MPC_DEPART_WARM_BLEND,
MPC_LAUNCH_BOUND_NODES,
RELATIVE_PACE_PREVIEW_TIME,
URGENT_BYPASS_REQUIRED_DECEL,
AccelController,
AccelControllerState,
AccelProfile,
@@ -25,8 +29,16 @@ from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_control
)
def make_lead(*, status: bool = False, d_rel: float = 0.0, v_lead_k: float = 0.0, a_lead_k: float = 0.0, a_lead_tau: float = 1.5):
return SimpleNamespace(status=status, dRel=d_rel, vLeadK=v_lead_k, aLeadK=a_lead_k, aLeadTau=a_lead_tau)
def make_lead(*, status: bool = False, d_rel: float = 0.0, v_lead_k: float = 0.0, a_lead_k: float = 0.0, a_lead_tau: float = 1.5,
radar_track_id: int = -1):
return SimpleNamespace(
status=status,
dRel=d_rel,
vLeadK=v_lead_k,
aLeadK=a_lead_k,
aLeadTau=a_lead_tau,
radarTrackId=radar_track_id,
)
def make_radar(lead_one=None, lead_two=None):
@@ -64,6 +76,14 @@ def assert_profile_trajectory(result, expected: float) -> None:
class TestAccelProfileLimits:
def test_profile_table_matches_tuned_values(self):
assert ACCEL_PROFILE_MAX_BP == [0.0, 10.0, 25.0, 40.0]
assert ACCEL_PROFILE_MAX_V == {
AccelProfile.eco: [1.55, 0.30, 0.20, 0.10],
AccelProfile.normal: [1.70, 0.90, 0.40, 0.20],
AccelProfile.sport: [2.00, 1.70, 1.20, 0.90],
}
@pytest.mark.parametrize("profile", list(AccelProfile))
def test_profile_accel_max_matches_lookup_table(self, profile):
for speed, expected in zip(ACCEL_PROFILE_MAX_BP, ACCEL_PROFILE_MAX_V[profile], strict=True):
@@ -84,17 +104,38 @@ class TestAccelProfileLimits:
assert limits[AccelProfile.eco] < limits[AccelProfile.normal] < limits[AccelProfile.sport]
@pytest.mark.parametrize("profile", list(AccelProfile))
def test_profile_table_never_exceeds_stock_speed_limit(self, profile):
def test_profile_table_stays_within_global_accel_limit(self, profile):
for step in range(161):
speed = step * 0.25
assert AccelController.get_profile_accel_max(profile, speed) <= get_max_accel(speed)
assert 0.0 <= AccelController.get_profile_accel_max(profile, speed) <= ACCEL_MAX
@pytest.mark.parametrize("profile", list(AccelProfile))
@pytest.mark.parametrize("speed", ACCEL_PROFILE_MAX_BP)
def test_stock_dynamic_output_limit_remains_authoritative(self, profile, speed):
governor = make_governor()
stock_limit = get_max_accel(speed)
result = update(governor, profile=profile, v_ego=speed, planner_speed=speed, stock_accel_max=stock_limit)
assert result.effective_accel_max <= stock_limit
@pytest.mark.parametrize("speed", ACCEL_PROFILE_MAX_BP[1:])
def test_effective_profiles_remain_distinct_below_stock_output_limit(self, speed):
stock_limit = get_max_accel(speed)
limits = []
for profile in AccelProfile:
governor = make_governor()
result = update(governor, profile=profile, v_ego=speed, planner_speed=speed, stock_accel_max=stock_limit)
limits.append(result.effective_accel_max)
assert limits[AccelProfile.eco] < limits[AccelProfile.normal] < limits[AccelProfile.sport]
def test_active_result_exposes_profile_accel_max(self):
governor = make_governor()
result = update(governor, profile=AccelProfile.eco, v_ego=17.5, planner_speed=17.5)
assert result.profile_accel_max == pytest.approx(0.56)
assert result.profile_accel_max == pytest.approx(0.25)
def test_clear_road_profile_is_a_separate_pre_mpc_trajectory(self):
governor = make_governor()
@@ -104,9 +145,9 @@ class TestAccelProfileLimits:
assert result.mpc_accel_max is not None
assert result.mpc_shape_cruise
assert len(result.mpc_accel_max) == N + 1
assert_profile_trajectory(result, 1.0)
assert_profile_trajectory(result, 0.90)
def test_ordinary_lead_keeps_stock_mpc_accel_bounds(self):
def test_ordinary_lead_keeps_profile_pre_mpc_accel_bound(self):
governor = make_governor()
radar_state = make_radar(make_lead(status=True, d_rel=100.0, v_lead_k=15.0))
@@ -114,10 +155,10 @@ class TestAccelProfileLimits:
assert result.active
assert result.selected_lead == 0
assert result.mpc_accel_max is None
assert not result.mpc_shape_cruise
assert_profile_trajectory(result, result.profile_accel_max)
assert result.mpc_shape_cruise
def test_filtered_lead_history_keeps_stock_mpc_bounds_through_two_dropouts(self):
def test_filtered_lead_history_keeps_profile_bound_through_two_dropouts(self):
governor = make_governor()
radar_state = make_radar(make_lead(status=True, d_rel=100.0, v_lead_k=15.0))
for _ in range(3):
@@ -126,9 +167,10 @@ class TestAccelProfileLimits:
dropouts = [update(governor), update(governor)]
assert all(math.isfinite(result.live_filtered_cap) for result in dropouts)
assert all(result.mpc_accel_max is None for result in dropouts)
assert all(result.mpc_accel_max is not None for result in dropouts)
assert all(result.mpc_shape_cruise for result in dropouts)
def test_stop_hold_warms_only_after_departure_evidence(self):
def test_stop_hold_pins_zero_target_with_coherent_zero_accel_horizon(self):
governor = make_governor()
stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0))
moving = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=5.0))
@@ -137,24 +179,36 @@ class TestAccelProfileLimits:
confirming = update(governor, moving, base_speed=5.0, v_ego=0.1, planner_speed=0.1)
assert held.state == AccelControllerState.stopHold
assert held.mpc_accel_max is not None
assert not held.mpc_shape_cruise
np.testing.assert_array_equal(held.mpc_accel_max[:MPC_LAUNCH_BOUND_NODES + 1], 0.0)
assert held.target_speed == 0.0
assert held.effective_accel_max == 0.0
assert_profile_trajectory(held, 0.0)
assert held.mpc_shape_cruise
assert held.lead_obstacle_weights == (0.0, 0.0)
assert confirming.state == AccelControllerState.stopHold
assert confirming.mpc_accel_max is not None
assert not confirming.mpc_shape_cruise
np.testing.assert_array_equal(confirming.mpc_accel_max[:MPC_LAUNCH_BOUND_NODES], 0.0)
assert confirming.mpc_accel_max[MPC_LAUNCH_BOUND_NODES] == pytest.approx(MPC_CONFIRM_WARM_BLEND * 2.0)
np.testing.assert_array_equal(confirming.mpc_accel_max[MPC_LAUNCH_BOUND_NODES + 1:], 2.0)
assert confirming.target_speed == 0.0
assert confirming.effective_accel_max == 0.0
assert_profile_trajectory(confirming, 0.0)
assert confirming.mpc_shape_cruise
assert confirming.lead_obstacle_weights == (0.0, 0.0)
def test_stop_hold_keeps_raw_lead_above_vehicle_should_stop_threshold(self):
governor = AccelController(SimpleNamespace(longitudinalActuatorDelay=0.10, vEgoStopping=0.25))
stopped = make_radar(make_lead(status=True, d_rel=2.0, v_lead_k=0.0))
held = update(governor, stopped, base_speed=5.0, v_ego=0.28, planner_speed=0.28)
assert held.state == AccelControllerState.stopHold
assert held.target_speed == 0.0
assert held.lead_obstacle_weights == (1.0, 1.0)
def test_normal_active_limits_are_bounded_by_stock_and_profile(self):
governor = make_governor()
result = update(governor, profile=AccelProfile.normal, v_ego=10.0, planner_speed=10.0, stock_accel_max=1.40)
assert result.profile_accel_max == 1.0
assert result.effective_accel_max == 1.0
assert_profile_trajectory(result, 1.0)
assert result.profile_accel_max == 0.90
assert result.effective_accel_max == 0.90
assert_profile_trajectory(result, 0.90)
def test_first_enable_seeds_from_positive_planner_accel_within_stock(self):
governor = make_governor()
@@ -185,7 +239,7 @@ class TestAccelProfileLimits:
eco = update(governor, profile=AccelProfile.eco, v_ego=10.0, planner_speed=10.0, stock_accel_max=2.0)
assert sport.effective_accel_max == 1.15
assert sport.effective_accel_max == 1.70
assert eco.effective_accel_max == pytest.approx(sport.effective_accel_max - ACCEL_LIMIT_JERK * DT_MDL)
assert eco.effective_accel_max > eco.profile_accel_max
@@ -197,9 +251,9 @@ class TestAccelProfileLimits:
released = update(governor, profile=AccelProfile.normal, v_ego=10.0, planner_speed=10.0, stock_accel_max=1.40)
assert tightened.effective_accel_max == 0.40
assert_profile_trajectory(tightened, 1.0)
assert released.effective_accel_max == 1.0
assert_profile_trajectory(released, 1.0)
assert_profile_trajectory(tightened, 0.90)
assert released.effective_accel_max == 0.90
assert_profile_trajectory(released, 0.90)
def test_negative_stock_max_remains_authoritative_outside_the_mpc_profile_bound(self):
governor = make_governor()
@@ -217,7 +271,7 @@ class TestAccelProfileLimits:
results = [
update(governor, profile=AccelProfile.eco, v_ego=10.0, planner_speed=10.0, stock_accel_max=2.0, planner_accel=1.15)
for _ in range(10)
for _ in range(30)
]
assert results[-1].effective_accel_max == pytest.approx(ACCEL_PROFILE_MAX_V[AccelProfile.eco][1])
@@ -248,6 +302,76 @@ class TestAccelProfileLimits:
assert not result.mpc_shape_cruise
class TestLeadObstacleAcquisition:
benign_lead = make_lead(status=True, d_rel=126.0, v_lead_k=17.0, radar_track_id=7)
def test_lead_already_present_at_enable_has_full_authority(self):
governor = make_governor()
result = update(governor, make_radar(self.benign_lead), base_speed=30.0, v_ego=20.0, planner_speed=20.0)
assert result.lead_obstacle_weights == (1.0, 1.0)
def test_benign_new_lead_reaches_full_authority_in_point_three_seconds(self):
governor = make_governor()
args = {"base_speed": 30.0, "v_ego": 20.0, "planner_speed": 20.0}
update(governor, **args)
results = [update(governor, make_radar(self.benign_lead), **args) for _ in range(7)]
expected = np.linspace(0.2, 1.0, 7)
np.testing.assert_allclose([result.lead_obstacle_weights[0] for result in results], expected, atol=1e-12, rtol=0.0)
assert all(result.mpc_accel_max is not None for result in results)
def test_route_shaped_urgent_acquisition_is_immediate(self):
governor = make_governor()
args = {"base_speed": 40.0, "v_ego": 34.8, "planner_speed": 34.8}
update(governor, **args)
route_lead = make_lead(status=True, d_rel=93.6, v_lead_k=23.4, radar_track_id=22)
result = update(governor, make_radar(route_lead), **args)
assert result.required_decel > 1.0
assert result.lead_obstacle_weights == (1.0, 1.0)
@pytest.mark.parametrize(
"lead",
[
make_lead(status=True, d_rel=25.0, v_lead_k=0.0),
make_lead(status=True, d_rel=126.0, v_lead_k=17.0, a_lead_k=-0.6),
],
)
def test_close_or_braking_new_lead_is_immediate(self, lead):
governor = make_governor()
args = {"base_speed": 30.0, "v_ego": 20.0, "planner_speed": 20.0}
update(governor, **args)
result = update(governor, make_radar(lead), **args)
assert result.lead_obstacle_weights == (1.0, 1.0)
def test_dropout_discards_authority_state_and_reacquisition_starts_fresh(self):
governor = make_governor()
args = {"base_speed": 30.0, "v_ego": 20.0, "planner_speed": 20.0}
update(governor, **args)
first = update(governor, make_radar(self.benign_lead), **args)
dropout = update(governor, **args)
reacquired = update(governor, make_radar(self.benign_lead), **args)
assert first.lead_obstacle_weights[0] == pytest.approx(0.2)
assert dropout.lead_obstacle_weights == (1.0, 1.0)
assert reacquired.lead_obstacle_weights[0] == pytest.approx(0.2)
@pytest.mark.parametrize("bypass", [{"enabled": False}, {"acc_selected": False}])
def test_non_actuating_mode_always_requests_raw_lead_authority(self, bypass):
governor = make_governor()
update(governor)
result = update(governor, make_radar(self.benign_lead), **bypass)
assert result.lead_obstacle_weights == (1.0, 1.0)
class TestEnergyEnvelope:
def test_correct_relative_energy_formula_and_lead_selection(self):
governor = make_governor()
@@ -261,8 +385,11 @@ class TestEnergyEnvelope:
x_ego = 20.0 * delay
x_lead = lead_one.dRel + lead_one.vLeadK * delay
usable_gap = x_lead - x_ego - STOP_DISTANCE - get_T_FOLLOW() * lead_one.vLeadK
expected = lead_one.vLeadK + math.sqrt(2.0 * PROFILE_CONFIGS[AccelProfile.normal].comfort_decel * usable_gap)
incorrect_fixed_target_formula = math.sqrt(lead_one.vLeadK**2 + 2.0 * PROFILE_CONFIGS[AccelProfile.normal].comfort_decel * usable_gap)
anticipated_gap = max(usable_gap - (20.0 - lead_one.vLeadK) * RELATIVE_PACE_PREVIEW_TIME, 0.0)
expected = lead_one.vLeadK + math.sqrt(2.0 * PROFILE_CONFIGS[AccelProfile.normal].comfort_decel * anticipated_gap)
incorrect_fixed_target_formula = math.sqrt(
lead_one.vLeadK**2 + 2.0 * PROFILE_CONFIGS[AccelProfile.normal].comfort_decel * anticipated_gap,
)
assert envelope.selected_lead == 0
assert envelope.usable_gap == pytest.approx(usable_gap)
@@ -285,7 +412,8 @@ class TestEnergyEnvelope:
radar_state = make_radar(make_lead(status=True, d_rel=60.0, v_lead_k=10.0))
envelope = governor.calculate_energy_envelope(radar_state, 20.0, 0.0, profile)
expected = 10.0 + math.sqrt(2.0 * PROFILE_CONFIGS[profile].comfort_decel * envelope.usable_gap)
anticipated_gap = max(envelope.usable_gap - envelope.closing_speed * RELATIVE_PACE_PREVIEW_TIME, 0.0)
expected = 10.0 + math.sqrt(2.0 * PROFILE_CONFIGS[profile].comfort_decel * anticipated_gap)
assert envelope.cap == pytest.approx(expected)
@@ -336,6 +464,17 @@ class TestEnergyEnvelope:
class TestAccelControllerState:
restrictive_lead = make_lead(status=True, d_rel=40.0, v_lead_k=5.0)
@pytest.mark.parametrize("v_ego", [4.25, 9.39])
def test_clear_road_rolling_engagement_immediately_targets_base_speed(self, v_ego):
governor = make_governor()
result = update(governor, base_speed=20.0, v_ego=v_ego, planner_speed=v_ego)
assert result.state == AccelControllerState.free
assert result.live_pace == result.base_speed
assert result.target_speed == result.base_speed
assert not result.launching
def test_five_frame_median_requires_three_observations_and_holds_two_dropouts(self):
governor = make_governor()
restrictive_radar = make_radar(self.restrictive_lead)
@@ -358,7 +497,9 @@ class TestAccelControllerState:
def test_restriction_is_limited_by_profile_deceleration(self):
governor = make_governor()
radar_state = make_radar(self.restrictive_lead)
# 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))
update(governor, radar_state)
update(governor, radar_state)
@@ -369,6 +510,18 @@ 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(next_restriction, first_restriction.mpc_accel_max[0] - DECEL_LIMIT_JERK * DT_MDL)
def test_urgent_closing_bypasses_comfort_shaping_for_stock_mpc(self):
governor = make_governor()
result = update(governor, make_radar(self.restrictive_lead))
assert result.required_decel > URGENT_BYPASS_REQUIRED_DECEL
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_release_waits_for_confirmation_then_uses_profile_rate(self):
governor = make_governor()
@@ -431,30 +584,31 @@ class TestAccelControllerState:
result = update(governor, stopped, **stop_args)
assert result.state == AccelControllerState.stopHold
assert result.live_pace == 0.0
assert result.target_speed == stop_args["base_speed"]
assert result.target_speed == 0.0
assert result.effective_accel_max == 0.0
assert result.mpc_accel_max is not None
assert_profile_trajectory(result, 0.0)
assert result.lead_obstacle_weights == (0.0, 0.0)
for _ in range(3):
result = update(governor, moving, **stop_args)
assert result.state == AccelControllerState.stopHold
assert not result.launching
assert result.live_pace == 0.0
assert result.target_speed == stop_args["base_speed"]
assert result.target_speed == 0.0
assert result.effective_accel_max == 0.0
assert result.mpc_accel_max is not None
assert_profile_trajectory(result, 0.0)
assert result.lead_obstacle_weights == (0.0, 0.0)
departed = update(governor, moving, **stop_args)
assert departed.state == AccelControllerState.release
assert departed.launching
assert departed.live_pace == pytest.approx(stop_args["v_ego"] + LAUNCH_DELTA_V)
assert departed.target_speed == stop_args["base_speed"]
assert departed.effective_accel_max == pytest.approx(LAUNCH_ACCEL_JERK * DT_MDL)
np.testing.assert_allclose(departed.mpc_accel_max[:MPC_LAUNCH_BOUND_NODES], departed.effective_accel_max)
expected_warm = departed.effective_accel_max + MPC_DEPART_WARM_BLEND * (2.0 - departed.effective_accel_max)
assert departed.mpc_accel_max[MPC_LAUNCH_BOUND_NODES] == pytest.approx(expected_warm)
assert departed.effective_accel_max == pytest.approx(LAUNCH_ACCEL_RATE * DT_MDL)
assert_profile_trajectory(departed, departed.effective_accel_max)
assert departed.lead_obstacle_weights == (0.0, 1.0)
def test_second_nearly_stopped_lead_blocks_departure_confirmation(self):
def test_far_irrelevant_stopped_lead_does_not_block_departure(self):
governor = make_governor()
stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0))
mixed = make_radar(
@@ -464,15 +618,33 @@ class TestAccelControllerState:
args = {"base_speed": 5.0, "v_ego": 0.1, "planner_speed": 0.0}
update(governor, stopped, **args)
results = [update(governor, mixed, **args) for _ in range(5)]
results = [update(governor, mixed, **args) for _ in range(4)]
assert all(result.raw_energy_cap > 0.8 for result in results)
assert all(result.state == AccelControllerState.stopHold for result in results[:3])
assert all(not result.launching for result in results[:3])
assert results[3].state == AccelControllerState.release
assert results[3].launching
assert governor.live.departure_frames == 0
def test_stock_relevant_stopped_lead_two_blocks_departure(self):
governor = make_governor()
stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0))
mixed = make_radar(
make_lead(status=True, d_rel=20.0, v_lead_k=5.0),
make_lead(status=True, d_rel=5.0, v_lead_k=0.0),
)
args = {"base_speed": 5.0, "v_ego": 0.1, "planner_speed": 0.0}
update(governor, stopped, **args)
results = [update(governor, mixed, **args) for _ in range(5)]
assert all(result.state == AccelControllerState.stopHold for result in results)
assert all(not result.launching for result in results)
assert governor.live.departure_frames == 0
@pytest.mark.parametrize("profile", list(AccelProfile))
def test_confirmed_departure_launch_is_immediate_bounded_and_profiled(self, profile):
def test_confirmed_departure_ramps_to_common_breakaway_then_profiles(self, profile):
governor = make_governor()
stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0))
moving = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=5.0))
@@ -482,12 +654,56 @@ class TestAccelControllerState:
update(governor, stopped, **args)
departure = [update(governor, moving, **args) for _ in range(4)]
assert [result.target_speed for result in departure[:3]] == [args["base_speed"]] * 3
assert [result.target_speed for result in departure[:3]] == [0.0] * 3
assert all(result.effective_accel_max == 0.0 for result in departure[:3])
assert all(result.mpc_accel_max is not None for result in departure[:3])
expected_launch_pace = min(args["base_speed"], departure[-1].live_filtered_cap, args["v_ego"] + LAUNCH_DELTA_V)
assert departure[-1].live_pace == pytest.approx(expected_launch_pace)
assert departure[-1].target_speed == args["base_speed"]
assert departure[-1].effective_accel_max == pytest.approx(LAUNCH_ACCEL_JERK * DT_MDL)
assert departure[-1].mpc_accel_max is not None
assert departure[-1].effective_accel_max == pytest.approx(LAUNCH_ACCEL_RATE * DT_MDL)
assert_profile_trajectory(departure[-1], departure[-1].effective_accel_max)
still_breaking_away = update(governor, moving, **(args | {"v_ego": 0.04, "planner_speed": 0.04}))
assert still_breaking_away.launching
assert still_breaking_away.target_speed == args["base_speed"]
assert still_breaking_away.effective_accel_max == pytest.approx(2.0 * LAUNCH_ACCEL_RATE * DT_MDL)
assert_profile_trajectory(still_breaking_away, still_breaking_away.effective_accel_max)
moving_result = update(governor, moving, **(args | {"v_ego": 0.05, "planner_speed": 0.05}))
assert not moving_result.launching
assert moving_result.mpc_accel_max is not None
assert moving_result.mpc_shape_cruise
@pytest.mark.parametrize("profile", list(AccelProfile))
def test_departure_confirmation_uses_controlling_lead_speed_not_energy_cap(self, profile):
governor = make_governor()
stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0))
barely_moving = make_radar(make_lead(status=True, d_rel=5.0, v_lead_k=0.35))
args = {"base_speed": 5.0, "v_ego": 0.1, "planner_speed": 0.0, "profile": profile}
update(governor, stopped, **args)
confirmation = [update(governor, barely_moving, **args) for _ in range(4)]
assert all(result.raw_energy_cap < 0.8 for result in confirmation)
assert all(result.state == AccelControllerState.stopHold for result in confirmation[:3])
assert confirmation[3].state == AccelControllerState.release
assert confirmation[3].launching
def test_departure_confirmation_must_be_four_consecutive_frames(self):
governor = make_governor()
stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0))
moving = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=5.0))
args = {"base_speed": 5.0, "v_ego": 0.1, "planner_speed": 0.0}
update(governor, stopped, **args)
assert all(update(governor, moving, **args).state == AccelControllerState.stopHold for _ in range(2))
interrupted = update(governor, stopped, **args)
assert interrupted.state == AccelControllerState.stopHold
assert governor.live.departure_frames == 0
confirmation = [update(governor, moving, **args) for _ in range(4)]
assert all(result.state == AccelControllerState.stopHold for result in confirmation[:3])
assert confirmation[3].state == AccelControllerState.release
def test_stopped_lead_departure_releases_while_mpc_source_remains_lead(self):
governor = make_governor()
@@ -506,7 +722,7 @@ class TestAccelControllerState:
departure = [update(governor, moving, **lead_args) for _ in range(4)]
assert [result.live_pace for result in departure[:3]] == [0.0] * 3
assert departure[3].live_pace > 0.0
assert [result.target_speed for result in departure[:3]] == [lead_args["base_speed"]] * 3
assert [result.target_speed for result in departure[:3]] == [0.0] * 3
assert departure[-1].target_speed == lead_args["base_speed"]
assert len(departure) * DT_MDL < 1.0
@@ -559,8 +775,9 @@ class TestAccelControllerState:
renewed_stop = update(governor, stopped, **stale_stop_args)
assert renewed_stop.state == AccelControllerState.stopHold
assert renewed_stop.live_pace == 0.0
assert renewed_stop.target_speed == stale_stop_args["base_speed"]
assert renewed_stop.mpc_accel_max is not None
assert renewed_stop.target_speed == 0.0
assert renewed_stop.effective_accel_max == 0.0
assert_profile_trajectory(renewed_stop, 0.0)
assert not governor.live.departing_from_stop
def test_low_speed_moving_lead_never_bypasses_bounded_pace(self):
@@ -590,19 +807,23 @@ class TestAccelControllerState:
assert initial_noise.target_speed == initial_noise.live_pace
assert stopped_evidence.state == AccelControllerState.stopHold
assert governor.live.stopped_lead_hold
assert stopped_evidence.target_speed == stopped_evidence.base_speed
assert repeated_noise.target_speed == args["base_speed"]
assert stopped_evidence.target_speed == 0.0
assert repeated_noise.target_speed == 0.0
assert_profile_trajectory(stopped_evidence, 0.0)
assert_profile_trajectory(repeated_noise, 0.0)
def test_later_continuously_moving_lead_does_not_latch_stopped_hold(self):
governor = make_governor()
moving_lead = make_radar(make_lead(status=True, d_rel=10.0, v_lead_k=1.5))
update(governor, base_speed=5.0, v_ego=1.0, planner_speed=1.0)
settled = update(governor, moving_lead, base_speed=5.0, v_ego=0.0, planner_speed=0.0)
observations = [update(governor, moving_lead, base_speed=5.0, v_ego=0.0, planner_speed=0.0) for _ in range(3)]
settled = observations[-1]
assert settled.selected_lead == 0
assert not governor.live.stopped_lead_hold
assert settled.target_speed == settled.live_pace
assert observations[0].target_speed == observations[0].base_speed
assert settled.target_speed < settled.base_speed
def test_stop_hold_dropout_pins_target_without_losing_hold_state(self):
@@ -617,21 +838,28 @@ class TestAccelControllerState:
assert dropout.selected_lead == -1
assert dropout.state == AccelControllerState.stopHold
assert dropout.live_pace == 0.0
assert dropout.target_speed == dropout.base_speed
np.testing.assert_array_equal(dropout.mpc_accel_max, 0.0)
assert dropout.target_speed == 0.0
assert_profile_trajectory(dropout, 0.0)
assert governor.live.stopped_lead_hold
def test_no_lead_start_launches_immediately_with_profile_limit(self):
@pytest.mark.parametrize("profile", list(AccelProfile))
def test_no_lead_start_uses_solver_safe_seed_then_common_breakaway_floor(self, profile):
governor = make_governor()
result = update(governor, base_speed=5.0, v_ego=0.1, planner_speed=0.1)
args = {"base_speed": 5.0, "v_ego": 0.0, "planner_speed": 0.0, "profile": profile}
first = update(governor, **args)
second = update(governor, **args)
third = update(governor, **args)
assert result.selected_lead == -1
assert result.launching
assert result.live_pace == pytest.approx(0.1 + LAUNCH_DELTA_V)
assert result.target_speed == result.live_pace
assert result.effective_accel_max == 2.0
assert result.mpc_accel_max is None
assert first.selected_lead == -1
assert first.launching
assert first.live_pace == first.base_speed
assert first.target_speed == first.base_speed
assert first.effective_accel_max == INITIAL_LAUNCH_ACCEL_MAX
assert second.effective_accel_max == pytest.approx(INITIAL_LAUNCH_ACCEL_MAX + CLEAR_LAUNCH_ACCEL_RATE * DT_MDL)
assert third.effective_accel_max == BREAKAWAY_ACCEL_MAX
assert_profile_trajectory(first, INITIAL_LAUNCH_ACCEL_MAX)
assert_profile_trajectory(third, BREAKAWAY_ACCEL_MAX)
def test_confirmed_departure_has_no_later_pace_jump(self):
governor = make_governor()
@@ -639,7 +867,7 @@ class TestAccelControllerState:
moving = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=5.0))
lead_args = {
"base_speed": 5.0,
"v_ego": 0.1,
"v_ego": 0.04,
"planner_speed": 0.0,
"previous_mpc_source": LongitudinalPlanSource.lead0,
"previous_should_stop": True,
@@ -650,7 +878,7 @@ class TestAccelControllerState:
departing = update(governor, moving, **lead_args)
assert governor.live.departing_from_stop
handed_back = update(governor, moving, **(lead_args | {"v_ego": 0.31, "planner_speed": 0.31}))
handed_back = update(governor, moving, **(lead_args | {"v_ego": 0.05, "planner_speed": 0.05}))
assert not governor.live.departing_from_stop
assert not governor.live.stopped_lead_hold
@@ -658,7 +886,8 @@ class TestAccelControllerState:
assert handed_back.live_pace == pytest.approx(departing.live_pace + expected_step)
assert handed_back.live_pace < min(handed_back.base_speed, handed_back.live_filtered_cap)
assert handed_back.target_speed == handed_back.live_pace
assert handed_back.mpc_accel_max is None
assert handed_back.mpc_accel_max is not None
assert handed_back.mpc_shape_cruise
@pytest.mark.parametrize(
"bypass",
@@ -706,6 +935,28 @@ class TestAccelControllerState:
assert result.profile == AccelProfile.normal
def test_small_negative_ego_speed_is_sanitized_without_resetting_state(self):
governor = make_governor()
update(governor)
cap_samples = governor.live.cap_samples
result = update(governor, v_ego=-0.04, planner_speed=0.0)
assert result.active
assert math.isfinite(result.live_pace)
assert governor.live.cap_samples is cap_samples
def test_negative_ego_speed_below_noise_tolerance_resets_live_state(self):
governor = make_governor()
update(governor)
cap_samples = governor.live.cap_samples
result = update(governor, v_ego=-0.101, planner_speed=0.0)
assert not result.active
assert math.isinf(result.live_pace)
assert governor.live.cap_samples is not cap_samples
def test_invalid_delay_resets_and_bypasses(self):
governor = AccelController(SimpleNamespace(longitudinalActuatorDelay=None))
sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller_interfaces.py
+67
View File
@@ -60,6 +60,26 @@ def test_mpc_preshape_keeps_current_accel_feasible_only_at_initial_node():
np.testing.assert_array_equal(shaped_params[:, 2:], stock_params[:, 2:])
def test_mpc_negative_preshape_constrains_upper_bound_without_weakening_safety_bound():
radar_state = messaging.new_message('radarState').radarState
mpc = LongitudinalMpc()
mpc.set_cur_state(10.0, ACCEL_MIN)
mpc.run = lambda: None
requested_accel_max = np.linspace(ACCEL_MIN - 1.0, -0.2, N + 1)
expected_accel_max = np.clip(requested_accel_max, ACCEL_MIN, ACCEL_MAX)
mpc.update(radar_state, 30.0, accel_max=requested_accel_max, shape_accel_max_in_cruise=True)
shaped_params = mpc.params.copy()
mpc.update(radar_state, 30.0)
stock_params = mpc.params.copy()
np.testing.assert_array_equal(shaped_params[:, 0], ACCEL_MIN)
np.testing.assert_array_equal(shaped_params[:, 0], stock_params[:, 0])
np.testing.assert_array_equal(shaped_params[:, 1], expected_accel_max)
assert np.all(shaped_params[:, 1] < stock_params[:, 1])
assert np.all((ACCEL_MIN <= shaped_params[:, 1]) & (shaped_params[:, 1] <= ACCEL_MAX))
def test_mpc_last_solve_failure_survives_internal_solver_reset():
mpc = LongitudinalMpc()
mpc.last_solution_status = 3
@@ -84,6 +104,53 @@ def test_mpc_missing_or_invalid_preshape_is_exact_stock(accel_max):
np.testing.assert_array_equal(mpc.params, stock_params)
def test_mpc_benign_lead_weight_softens_only_optimization_obstacle():
radar_state = messaging.new_message('radarState').radarState
radar_state.leadOne.status = True
radar_state.leadOne.dRel = 60.0
radar_state.leadOne.vLead = 15.0
radar_state.leadOne.vLeadK = 15.0
radar_state.leadOne.aLeadK = 0.0
radar_state.leadOne.aLeadTau = 1.0
mpc = LongitudinalMpc()
mpc.set_cur_state(20.0, 0.0)
mpc.run = lambda: None
mpc.update(radar_state, 30.0, lead_obstacle_weights=(1.0, 1.0))
full_authority_params = mpc.params.copy()
lead_before = (radar_state.leadOne.dRel, radar_state.leadOne.vLead, radar_state.leadOne.aLeadK)
mpc.update(radar_state, 30.0, lead_obstacle_weights=(0.2, 1.0))
softened_params = mpc.params.copy()
assert softened_params[0, 2] > full_authority_params[0, 2]
np.testing.assert_array_equal(softened_params[:, :2], full_authority_params[:, :2])
np.testing.assert_array_equal(softened_params[:, 3:], full_authority_params[:, 3:])
np.testing.assert_array_equal(mpc.lead_obstacle_weights, [0.2, 1.0])
assert (radar_state.leadOne.dRel, radar_state.leadOne.vLead, radar_state.leadOne.aLeadK) == lead_before
@pytest.mark.parametrize("weights", [(1.0,), (np.nan, 1.0), (np.inf, 1.0)])
def test_mpc_invalid_lead_weights_are_exact_full_authority(weights):
radar_state = messaging.new_message('radarState').radarState
radar_state.leadOne.status = True
radar_state.leadOne.dRel = 60.0
radar_state.leadOne.vLead = 15.0
radar_state.leadOne.aLeadK = 0.0
radar_state.leadOne.aLeadTau = 1.0
mpc = LongitudinalMpc()
mpc.set_cur_state(20.0, 0.0)
mpc.run = lambda: None
mpc.update(radar_state, 30.0)
stock_params = mpc.params.copy()
stock_source = mpc.source
mpc.update(radar_state, 30.0, lead_obstacle_weights=weights)
np.testing.assert_array_equal(mpc.params, stock_params)
assert mpc.source == stock_source
np.testing.assert_array_equal(mpc.lead_obstacle_weights, [1.0, 1.0])
def test_shadow_target_telemetry_publishes_filtered_cap():
planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
planner.source = LongitudinalPlanSource.cruise
sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py
+160 -1
View File
@@ -8,7 +8,8 @@ from opendbc.car.interfaces import ACCEL_MIN
from openpilot.common.params import Params
from openpilot.common.realtime import DT_MDL
from openpilot.selfdrive.controls.lib.longitudinal_planner import get_max_accel
from openpilot.selfdrive.test.longitudinal_maneuvers.plant import LeadObservation, Plant
from openpilot.selfdrive.test.longitudinal_maneuvers.plant import PRIUS_TSS2_ROUTE_MODEL, LeadObservation, Plant
from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality import AccelControllerState
@dataclass
@@ -31,6 +32,12 @@ class ClosedLoopTrace:
profile_accel_max: np.ndarray
effective_accel_max: np.ndarray
controller_fault: np.ndarray
actuator_command: np.ndarray
applied_actuator_command: np.ndarray
observed_speed: np.ndarray
observed_acceleration: np.ndarray
lead_obstacle_weight_0: np.ndarray
lead_obstacle_weight_1: np.ndarray
solver_failures: int
@@ -89,6 +96,12 @@ def _run(
controller.profile_accel_max,
controller.effective_accel_max,
controller_fault,
result["actuator_command"],
result["applied_actuator_command"],
result["observed_v_ego"],
result["observed_a_ego"],
controller.lead_obstacle_weights[0],
controller.lead_obstacle_weights[1],
)
)
sources.append(result["mpc_source"])
@@ -113,6 +126,12 @@ def _run(
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],
solver_failures=solver_failures,
)
@@ -288,6 +307,82 @@ def test_lead_slot_handoff_does_not_resurrect_stale_relief():
assert not _has_propulsion_brake_reversal(trace, after=1.0)
def test_benign_far_lead_acquisition_ramps_optimizer_authority_without_jerk():
acquisition_time = 1.0
def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None:
if current_time < acquisition_time or lead_name == "leadTwo":
return None
return truth | {"radarTrackId": 7}
trace = _run(
duration=3.0,
controller_enabled=True,
lead_relevancy=True,
speed=20.0,
distance_lead=126.0,
v_lead=17.0,
v_cruise=30.0,
lead_observation_fn=observe,
actuator_delay=0.15,
actuator_lag=0.20,
)
acquired = np.flatnonzero((trace.time >= acquisition_time) & (trace.selected_lead == 0))
assert len(acquired)
first = acquired[0]
assert trace.lead_obstacle_weight_0[first] == pytest.approx(0.2)
authority = trace.lead_obstacle_weight_0[first:first + 7]
np.testing.assert_allclose(authority, np.linspace(0.2, 1.0, 7), atol=1e-9, rtol=0.0)
jerk_window = (trace.time[1:] >= acquisition_time) & (trace.time[1:] <= acquisition_time + 0.6)
assert np.max(np.abs(np.diff(trace.a_target)[jerk_window] / DT_MDL)) < 3.0
assert trace.solver_failures == 0
def test_route_shaped_urgent_lead_acquisition_is_immediate_and_does_not_delay_braking(record_property):
acquisition_time = 1.0
def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None:
if current_time < acquisition_time or lead_name == "leadTwo":
return None
return truth | {"radarTrackId": 22}
common = dict(
duration=10.0,
lead_relevancy=True,
speed=34.8,
# The 11.4 m/s closing speed removes about 11.4 m before acquisition,
# reproducing the route's observed ~93.6 m lead distance.
distance_lead=105.0,
v_lead=23.4,
v_cruise=40.0,
lead_observation_fn=observe,
actuator_delay=0.15,
actuator_lag=0.20,
)
baseline = _run(controller_enabled=False, **common)
trace = _run(controller_enabled=True, **common)
acquired = np.flatnonzero((trace.time >= acquisition_time) & (trace.selected_lead == 0))
assert len(acquired)
assert trace.lead_obstacle_weight_0[acquired[0]] == 1.0
for threshold in (-1.0, -2.0):
assert _first_time_below(trace, threshold) <= _first_time_below(baseline, threshold) + 1e-9
assert trace.solver_failures == 0
record_property("clean_base_solver_failures", baseline.solver_failures)
if baseline.solver_failures:
pytest.xfail("provisional route gate: clean-base MPC loses the abrupt 34.8-to-23.4 m/s lead-acquisition solve")
baseline_gap = baseline.distance_lead - baseline.distance
controlled_gap = trace.distance_lead - trace.distance
assert np.min(controlled_gap) >= np.min(baseline_gap) - 1e-3
baseline_closing = np.maximum(baseline.speed - common["v_lead"], 0.0)
controlled_closing = np.maximum(trace.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) >= np.min(baseline_ttc) - 1e-3
def test_alternating_full_lead_range_glitch_has_bounded_jerk_and_no_reversal():
glitch_start = 5.0
glitch_end = 5.5
@@ -446,6 +541,35 @@ def test_stopped_lead_noise_requires_four_departure_frames_and_launches_within_o
assert not _has_propulsion_brake_reversal(trace, after=departure_time)
@pytest.mark.parametrize("profile", range(3), ids=("eco", "normal", "sport"))
def test_route_derived_prius_prompt_launch_gate(profile):
departure_time = 1.0
def lead_speed(current_time: float) -> float:
return 0.0 if current_time < departure_time else 2.0
trace = _run(
duration=3.0,
controller_enabled=True,
profile=profile,
lead_relevancy=True,
speed=0.0,
distance_lead=6.0,
v_lead=lead_speed,
# Dominant post-SCC/SLA target in the supplied Prius routes (50 mph).
v_cruise=22.352,
actuator_model=PRIUS_TSS2_ROUTE_MODEL,
)
first_three = (trace.time > departure_time) & (trace.time <= departure_time + 3 * DT_MDL + 1e-9)
assert not trace.launching[first_three].any()
assert np.max(trace.speed[first_three]) < 1e-3
moving = np.flatnonzero((trace.time >= departure_time) & (trace.speed > 0.05))
assert len(moving)
assert trace.time[moving[0]] - departure_time <= 1.0 + 1e-9
assert trace.solver_failures == 0
def test_stop_hold_two_frame_total_lead_dropout_cannot_launch():
def observe(current_time: float, _lead_name: str, truth: LeadObservation) -> LeadObservation | None:
return None if 1.0 <= current_time < 1.1 else truth
@@ -469,6 +593,41 @@ def test_stop_hold_two_frame_total_lead_dropout_cannot_launch():
assert not _has_propulsion_brake_reversal(trace, after=0.5)
@pytest.mark.parametrize("v_ego_stopping", [0.25, 0.10], ids=("toyota-like", "tesla-like"))
def test_stop_hold_above_vehicle_should_stop_threshold_keeps_close_lead_authority(v_ego_stopping):
_set_accel_controller_params(enabled=True, profile=1)
initial_gap = 0.25
plant = Plant(
lead_relevancy=True,
speed=0.28,
distance_lead=initial_gap,
actuator_delay=0.10,
actuator_lag=0.20,
)
plant.planner.CP.vEgoStopping = v_ego_stopping
gaps = []
should_stop = []
solver_statuses = []
first_controller = None
for _ in range(round(2.0 / DT_MDL)):
result = plant.step(v_lead=0.0, v_cruise=5.0)
controller = plant.planner.accel_controller_result
first_controller = first_controller or controller
gaps.append(result["distance_lead"] - result["distance"])
should_stop.append(result["should_stop"])
solver_statuses.append(plant.planner.mpc.last_solution_status)
assert first_controller is not None
assert first_controller.state == AccelControllerState.stopHold
assert first_controller.target_speed == 0.0
assert first_controller.lead_obstacle_weights == (1.0, 1.0)
assert np.flatnonzero(should_stop)[0] * DT_MDL <= 1.0
assert min(gaps) > 0.05
assert plant.speed == 0.0
assert not any(solver_statuses)
def test_clear_road_launch_is_immediate_bounded_and_profiles_feel_distinct():
common = dict(
duration=6.0,