diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 91161b8716..b2005aa788 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -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])] diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 8034957a8e..4e9323dcd1 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -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) diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index 17de11da16..3a0f506f6b 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -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, diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py index 620f51cc42..7902354818 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py @@ -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, diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py b/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py index b1f31a1a9b..43d11d8ddd 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py @@ -6,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)) diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller_interfaces.py b/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller_interfaces.py index aa369719f5..f2a6a7ca78 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller_interfaces.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller_interfaces.py @@ -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 diff --git a/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py b/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py index fd7df9c139..4d33f4be70 100644 --- a/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py +++ b/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py @@ -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,