Compare commits

..

1 Commits

Author SHA1 Message Date
github-actions[bot] 5ebb02b22d sunnypilot v2026.07.24-4587
version: sunnypilot v2026.002.000 (feature-branch)
date: 2026-07-24T17:30:38
master commit: 15dc075560
2026-07-24 17:30:38 +00:00
32 changed files with 93 additions and 978 deletions
@@ -205,7 +205,6 @@ opendbc/car/volkswagen/mebcan.py
opendbc/car/volkswagen/mlbcan.py
opendbc/car/volkswagen/mqbcan.py
opendbc/car/volkswagen/pqcan.py
opendbc/car/volkswagen/radar_interface.py
opendbc/car/volkswagen/values.py
opendbc/car/volkswagen/tests/__init__.py
opendbc/car/volkswagen/tests/test_volkswagen.py
@@ -13,7 +13,6 @@ from opendbc.car.toyota.values import CAR, NO_STOP_TIMER_CAR, TSS2_CAR, \
CarControllerParams, ToyotaFlags, \
UNSUPPORTED_DSU_CAR
from opendbc.can import CANPacker
from opendbc.sunnypilot.car.toyota.auto_brake_hold import AutoBrakeHoldCarController
from opendbc.sunnypilot.car.toyota.enhanced_bsm import EnhancedBsmCarController
from opendbc.sunnypilot.car.toyota.gas_interceptor import GasInterceptorCarController
from opendbc.sunnypilot.car.toyota.values import ToyotaFlagsSP
@@ -93,10 +92,15 @@ class CarController(CarControllerBase, GasInterceptorCarController):
self.secoc_prev_reset_counter = 0
self.enhanced_bsm = EnhancedBsmCarController(CP, CP_SP)
self.auto_brake_hold = AutoBrakeHoldCarController(CP, CP_SP)
self._auto_lock_speed = 0.0
if CP_SP.flags & ToyotaFlagsSP.SP_AUTO_BRAKE_HOLD:
self.brake_hold_active: bool = False
self._brake_hold_counter: int = 0
self._brake_hold_reset: bool = False
self._prev_brake_pressed: bool = False
self._auto_lock_once = False
self._gear_prev = GearShifter.park
@@ -215,8 +219,8 @@ class CarController(CarControllerBase, GasInterceptorCarController):
self.last_standstill = CS.out.standstill
if self.auto_brake_hold.enabled:
can_sends.extend(self.auto_brake_hold.update(CS, self.frame, self.packer))
if self.CP_SP.flags & ToyotaFlagsSP.SP_AUTO_BRAKE_HOLD:
can_sends.extend(self.create_auto_brake_hold_messages(CS))
# handle UI messages
fcw_alert = hud_control.visualAlert == VisualAlert.fcw
@@ -353,3 +357,25 @@ class CarController(CarControllerBase, GasInterceptorCarController):
self.frame += 1
return new_actuators, can_sends
# auto brake hold (https://github.com/AlexandreSato/)
def create_auto_brake_hold_messages(self, CS: structs.CarState, brake_hold_allowed_timer: int = 100):
can_sends = []
disallowed_gears = [GearShifter.park, GearShifter.reverse]
brake_hold_allowed = CS.out.standstill and CS.out.cruiseState.available and not CS.out.gasPressed and \
not CS.out.cruiseState.enabled and (CS.out.gearShifter not in disallowed_gears)
if brake_hold_allowed:
self._brake_hold_counter += 1
self.brake_hold_active = self._brake_hold_counter > brake_hold_allowed_timer and not self._brake_hold_reset
self._brake_hold_reset = not self._prev_brake_pressed and CS.out.brakePressed and not self._brake_hold_reset
else:
self._brake_hold_counter = 0
self.brake_hold_active = False
self._brake_hold_reset = False
self._prev_brake_pressed = CS.out.brakePressed
if self.frame % 2 == 0:
can_sends.append(toyotacan.create_brake_hold_command(self.packer, self.frame, CS.pre_collision_2, self.brake_hold_active))
return can_sends
+3 -11
View File
@@ -69,7 +69,6 @@ static bool toyota_secoc = false;
static bool toyota_alt_brake = false;
static bool toyota_stock_longitudinal = false;
static bool toyota_lta = false;
static bool toyota_cruise_engaged = false; // SP: PCM_CRUISE.CRUISE_ACTIVE, narrows the auto brake hold AEB window below
static int toyota_dbc_eps_torque_factor = 100; // conversion factor for STEER_TORQUE_EPS in %: see dbc file
static uint32_t toyota_compute_checksum(const CANPacket_t *msg) {
@@ -152,7 +151,6 @@ static void toyota_rx_hook(const CANPacket_t *msg) {
if (msg->addr == 0x176U) {
bool cruise_engaged = GET_BIT(msg, 5U); // PCM_CRUISE.CRUISE_ACTIVE
pcm_cruise_check(cruise_engaged);
toyota_cruise_engaged = cruise_engaged;
}
if (msg->addr == 0x116U) {
gas_pressed = msg->data[1] != 0U; // GAS_PEDAL.GAS_PEDAL_USER
@@ -164,7 +162,6 @@ static void toyota_rx_hook(const CANPacket_t *msg) {
if (msg->addr == 0x1D2U) {
bool cruise_engaged = GET_BIT(msg, 5U); // PCM_CRUISE.CRUISE_ACTIVE
pcm_cruise_check(cruise_engaged);
toyota_cruise_engaged = cruise_engaged;
if (!enable_gas_interceptor) {
gas_pressed = !GET_BIT(msg, 4U); // PCM_CRUISE.GAS_RELEASED
@@ -392,7 +389,7 @@ static bool toyota_tx_hook(const CANPacket_t *msg) {
// SP: auto brake hold https://github.com/AlexandreSato
if ((msg->addr == 0x344U) && (alternative_experience & ALT_EXP_ALLOW_AEB)) {
if (vehicle_moving || gas_pressed || !acc_main_on || toyota_cruise_engaged) {
if (vehicle_moving || gas_pressed || !acc_main_on) {
tx = false;
}
}
@@ -593,14 +590,9 @@ static safety_config toyota_init(uint16_t param) {
static bool toyota_fwd_hook(int bus_num, int addr) {
bool block_msg = false;
if (bus_num == 2) {
// SP: block AEB when auto brake hold is active, unblock AEB when auto brake hold is not active.
// Narrowed to match auto brake hold's own precondition (cruise must be off) - previously this
// blocked native AEB forwarding, forcing a slower software relay, any time the car was simply
// stopped with the gas released and ACC main on, even while cruise was actively engaged and
// auto brake hold couldn't be active at all.
// SP: block AEB when auto brake hold is active, unblock AEB when auto brake hold is not active
bool is_aeb_msg = (addr == 0x344);
block_msg = (is_aeb_msg && (alternative_experience & ALT_EXP_ALLOW_AEB) && !vehicle_moving && !gas_pressed && acc_main_on &&
!toyota_cruise_engaged);
block_msg = (is_aeb_msg && (alternative_experience & ALT_EXP_ALLOW_AEB) && !vehicle_moving && !gas_pressed && acc_main_on);
}
return block_msg;
@@ -1,79 +0,0 @@
"""
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
This file is part of sunnypilot and is licensed under the MIT License.
See the LICENSE.md file in the root directory for more details.
"""
from opendbc.car import structs
from opendbc.car.toyota import toyotacan
from opendbc.sunnypilot.car.toyota.values import ToyotaFlagsSP
GearShifter = structs.CarState.GearShifter
# frames of confirmed hold-eligible standstill required before engaging
BRAKE_HOLD_ALLOWED_TIMER = 100
DISALLOWED_GEARS = (GearShifter.park, GearShifter.reverse)
# PRE_COLLISION_2 fields that go high when the camera's own PCS/AEB is genuinely intervening this
# frame (PCSALM mirrors PRECOLLISION_ACTIVE; IBTRGR/PBATRGR/PREFILL/AVSTRGR/PBRTRGR/PPTRGR are its
# actuation triggers - see create_pcs_commands for the same field set on the stock-DSU PCS path).
# Deliberately over-inclusive: a false positive here just means we pass a quiescent frame through
# instead of holding it, never the other way around, so err toward checking more fields, not fewer.
PCS_TRIGGER_FIELDS = ("PCSALM", "IBTRGR", "PBATRGR", "PREFILL", "AVSTRGR", "PBRTRGR", "PPTRGR")
def pcs_is_active(pre_collision_2: dict) -> bool:
return any(pre_collision_2.get(field, 0) for field in PCS_TRIGGER_FIELDS) or pre_collision_2.get("DSS1GDRV", 0) != 0
class AutoBrakeHold:
def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP):
self.CP = CP
self.CP_SP = CP_SP
@property
def enabled(self):
return bool(self.CP_SP.flags & ToyotaFlagsSP.SP_AUTO_BRAKE_HOLD)
# Auto Brake Hold (@AlexandreSato, @rav4kumar): holds the car at a stop with cruise off by
# overriding PRE_COLLISION_2 - the only channel on this platform that can command the brake
# independent of ACC engagement, since PCS/AEB is an always-on active safety system by design.
# Yields to any genuine PCS activation this frame - the real message is only ever overridden while
# it's quiescent - and releases for the rest of the current standstill episode on a brake press,
# rather than for a single frame.
class AutoBrakeHoldCarController(AutoBrakeHold):
def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP):
super().__init__(CP, CP_SP)
self.active = False
self._counter = 0
self._released = False
self._prev_brake_pressed = False
def update(self, CS: structs.CarState, frame: int, packer) -> list:
hold_allowed = (CS.out.standstill and CS.out.cruiseState.available and not CS.out.cruiseState.enabled and
not CS.out.gasPressed and CS.out.gearShifter not in DISALLOWED_GEARS)
if hold_allowed:
# only a fresh press releases hold - the press that caused the stop is already reflected in
# _prev_brake_pressed by the time standstill is reached, so it doesn't count as a release
if CS.out.brakePressed and not self._prev_brake_pressed:
self._released = True
self._counter += 1
self.active = self._counter > BRAKE_HOLD_ALLOWED_TIMER and not self._released
else:
self._counter = 0
self.active = False
self._released = False
self._prev_brake_pressed = CS.out.brakePressed
can_sends = []
if frame % 2 == 0:
override = self.active and not pcs_is_active(CS.pre_collision_2)
can_sends.append(toyotacan.create_brake_hold_command(packer, frame, CS.pre_collision_2, override))
return can_sends
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
+1 -1
View File
@@ -1 +1 @@
#define SUNNYPILOT_VERSION "2026.07.28-4599"
#define SUNNYPILOT_VERSION "2026.07.24-4587"
@@ -1,3 +1,4 @@
#!/usr/bin/env python3
from collections import deque
from dataclasses import dataclass, field
from enum import IntEnum
@@ -20,7 +21,7 @@ from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants imp
PACE_RESTRICT_DEADBAND, PROFILE_CONFIGS, RADAR_STALE_TIMEOUT, STOP_GAP_RESERVE, STOP_GAP_RESERVE_DECEL_BP,
STOP_GAP_RESERVE_LEAD_SPEED,
STOP_HOLD_CREEP_DISTANCE, STOP_HOLD_CREEP_SPEED, STOP_HOLD_EGO_SPEED, STOP_HOLD_EXIT_FRAMES, STOP_HOLD_EXIT_SPEED,
STOP_HOLD_FAST_DEPARTURE_DISTANCE, STOP_HOLD_MAX_LEAD_DISTANCE, STOPPED_LEAD_SPEED, VEGO_NOISE_TOLERANCE, AccelProfile,
STOP_HOLD_MAX_LEAD_DISTANCE, STOPPED_LEAD_SPEED, VEGO_NOISE_TOLERANCE, AccelProfile,
)
@@ -43,9 +44,6 @@ class EnergyEnvelope:
departure_lead_index: int = -1
departure_lead_speed: float = math.inf
departure_cap: float = math.inf
departure_lead_speeds: tuple[float, float] = (math.inf, math.inf)
departure_lead_distances: tuple[float, float] = (-math.inf, -math.inf)
departure_lead_track_ids: tuple[int, int] = (-1, -1)
departure_lead_separations: tuple[float, float] = (-math.inf, -math.inf)
usable_gap: float = math.inf
closing_speed: float = 0.0
@@ -88,9 +86,7 @@ class _ControllerPath:
departure_samples: tuple[deque[float], deque[float]] = field(
default_factory=lambda: (deque(maxlen=CAP_FILTER_FRAMES), deque(maxlen=CAP_FILTER_FRAMES)),
)
departure_motion_samples: deque[float] = field(default_factory=lambda: deque(maxlen=CAP_FILTER_FRAMES))
departure_references: list[float | None] = field(default_factory=lambda: [None, None])
departure_track_ids: list[int] = field(default_factory=lambda: [-1, -1])
pace: float | None = None
state: AccelControllerState = AccelControllerState.inactive
departure_frames: int = 0
@@ -129,9 +125,7 @@ class _ControllerPath:
self.lead_speed_samples = deque([math.inf] * CAP_FILTER_FRAMES, maxlen=CAP_FILTER_FRAMES)
self.lead_accel_samples = deque([0.0] * CAP_FILTER_FRAMES, maxlen=CAP_FILTER_FRAMES)
self.departure_samples = (deque(maxlen=CAP_FILTER_FRAMES), deque(maxlen=CAP_FILTER_FRAMES))
self.departure_motion_samples = deque(maxlen=CAP_FILTER_FRAMES)
self.departure_references = [None, None]
self.departure_track_ids = [-1, -1]
self.pace = None
self.state = AccelControllerState.inactive
self.departure_frames = 0
@@ -249,8 +243,6 @@ class AccelController:
candidates: list[EnergyEnvelope] = []
departure_candidates: list[tuple[float, int]] = []
departure_speeds = [math.inf, math.inf]
departure_distances = [-math.inf, -math.inf]
departure_track_ids = [-1, -1]
departure_separations = [-math.inf, -math.inf]
departure_caps = [math.inf, math.inf]
@@ -289,8 +281,6 @@ class AccelController:
))
departure_candidates.append((departure_distance, lead_index))
departure_speeds[lead_index] = v_lead_delay
departure_distances[lead_index] = d_rel
departure_track_ids[lead_index] = self._lead_track_id(lead)
departure_separations[lead_index] = separation
departure_caps[lead_index] = departure_cap
@@ -305,9 +295,7 @@ class AccelController:
selected_lead_speed=selected.selected_lead_speed,
selected_lead_accel=selected.selected_lead_accel,
departure_lead_index=departure_lead_index, departure_lead_speed=departure_lead_speed,
departure_cap=departure_caps[departure_lead_index], departure_lead_speeds=tuple(departure_speeds),
departure_lead_distances=tuple(departure_distances), departure_lead_track_ids=tuple(departure_track_ids),
departure_lead_separations=tuple(departure_separations),
departure_cap=departure_caps[departure_lead_index], departure_lead_separations=tuple(departure_separations),
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, lead_status=lead_status,
)
@@ -320,71 +308,37 @@ class AccelController:
def _lead_source(source) -> bool:
return source in (LongitudinalPlanSource.lead0, LongitudinalPlanSource.lead1)
def _update_samples(self, path: _ControllerPath, envelope: EnergyEnvelope) -> bool:
@staticmethod
def _update_samples(path: _ControllerPath, envelope: EnergyEnvelope) -> bool:
had_filtered_lead = math.isfinite(path.filtered_cap)
has_lead = envelope.selected_lead >= 0
path.cap_samples.append(envelope.cap if has_lead else math.inf)
path.lead_speed_samples.append(envelope.selected_lead_speed if has_lead else math.inf)
path.lead_accel_samples.append(envelope.selected_lead_accel if has_lead else 0.0)
path.lead_loss_frames = 0 if has_lead else path.lead_loss_frames + 1
for lead_index, distance in enumerate(envelope.departure_lead_distances):
if not math.isfinite(distance):
continue
samples = path.departure_samples[lead_index]
track_id = envelope.departure_lead_track_ids[lead_index]
identity_changed = bool(samples) and track_id != path.departure_track_ids[lead_index] and (track_id >= 0 or path.departure_track_ids[lead_index] >= 0)
max_distance_step = max(STOP_HOLD_CREEP_DISTANCE / 2.0, 3.0 * envelope.departure_lead_speeds[lead_index] * self.dt)
geometry_jump = bool(samples) and abs(distance - samples[-1]) > max_distance_step
if identity_changed or geometry_jump:
samples.clear()
path.departure_references[lead_index] = distance
samples.append(distance)
path.departure_track_ids[lead_index] = track_id
lead_index = envelope.departure_lead_index
if lead_index >= 0:
distance = envelope.departure_lead_distances[lead_index]
samples = path.departure_motion_samples
max_distance_step = max(STOP_HOLD_CREEP_DISTANCE / 2.0, 3.0 * envelope.departure_lead_speed * self.dt)
if samples and abs(distance - samples[-1]) > max_distance_step:
samples.clear()
samples.append(distance)
for lead_index, separation in enumerate(envelope.departure_lead_separations):
if math.isfinite(separation):
path.departure_samples[lead_index].append(separation)
return not had_filtered_lead and math.isfinite(path.filtered_cap)
@staticmethod
def _seed_departure_tracking(path: _ControllerPath, envelope: EnergyEnvelope) -> None:
path.departure_samples = (deque(maxlen=CAP_FILTER_FRAMES), deque(maxlen=CAP_FILTER_FRAMES))
path.departure_motion_samples = deque(maxlen=CAP_FILTER_FRAMES)
path.departure_references = [None, None]
path.departure_track_ids = list(envelope.departure_lead_track_ids)
for lead_index, distance in enumerate(envelope.departure_lead_distances):
if math.isfinite(distance):
path.departure_samples[lead_index].append(distance)
path.departure_references[lead_index] = distance
if envelope.departure_lead_index >= 0:
path.departure_motion_samples.append(envelope.departure_lead_distances[envelope.departure_lead_index])
for lead_index, separation in enumerate(envelope.departure_lead_separations):
if math.isfinite(separation):
path.departure_samples[lead_index].append(separation)
path.departure_references[lead_index] = separation
path.departure_frames = 0
@staticmethod
def _departure_progress(path: _ControllerPath, envelope: EnergyEnvelope, minimum_distance: float, *, robust: bool = True) -> bool:
def _creep_departure(path: _ControllerPath, envelope: EnergyEnvelope) -> bool:
lead_index = envelope.departure_lead_index
if lead_index < 0 or envelope.departure_lead_speed <= STOP_HOLD_CREEP_SPEED:
return False
reference = path.departure_references[lead_index]
samples = path.departure_samples[lead_index]
distance = path.robust_departure_separation(lead_index) if robust else samples[-1] if samples else -math.inf
return reference is not None and distance - reference >= minimum_distance
@classmethod
def _creep_departure(cls, path: _ControllerPath, envelope: EnergyEnvelope) -> bool:
return cls._departure_progress(path, envelope, STOP_HOLD_CREEP_DISTANCE)
@staticmethod
def _recent_departure_motion(path: _ControllerPath) -> bool:
samples = tuple(path.departure_motion_samples)[-STOP_HOLD_EXIT_FRAMES:]
if len(samples) < STOP_HOLD_EXIT_FRAMES:
return False
deltas = np.diff(samples)
return samples[-1] - samples[0] >= STOP_HOLD_FAST_DEPARTURE_DISTANCE and np.count_nonzero(deltas > 0.005) >= 2
separation = path.robust_departure_separation(lead_index)
return reference is not None and separation - reference >= STOP_HOLD_CREEP_DISTANCE
def _enter_stop_hold(self, path: _ControllerPath, envelope: EnergyEnvelope) -> None:
if path.state != AccelControllerState.stopHold:
@@ -437,8 +391,8 @@ class AccelController:
stop_evidence = (stopped_lead_hold or envelope.cap < 0.50 or filtered_cap < 0.50
or (previous_stop and not path.launching) or invalid_lead)
confirmed_creep_departure = (path.launching and path.departure_launch and has_lead
and (self._departure_progress(path, envelope, STOP_HOLD_FAST_DEPARTURE_DISTANCE)
or self._recent_departure_motion(path)))
and (envelope.departure_lead_speed > STOP_HOLD_CREEP_SPEED
or self._creep_departure(path, envelope)))
if (path.active_frames >= self.lead_loss_hold_frames and math.isfinite(filtered_cap)
and has_lead and planner_accel <= BRAKING_ACCEL_LIMIT_THRESHOLD):
path.braking_limited = True
@@ -470,17 +424,13 @@ class AccelController:
separation = path.robust_departure_separation(lead_index)
if math.isfinite(separation) and path.departure_references[lead_index] is None:
path.departure_references[lead_index] = separation
fast_departure = (has_lead and min(envelope.selected_lead_speed, envelope.departure_lead_speed) > STOP_HOLD_EXIT_SPEED
and envelope.departure_cap > STOP_HOLD_EXIT_SPEED)
raw_departure = (fast_departure
raw_departure = ((has_lead and envelope.departure_lead_speed > STOP_HOLD_CREEP_SPEED
and envelope.departure_cap > STOP_HOLD_CREEP_SPEED)
or (not envelope.lead_status and path.lead_loss_frames >= self.lead_loss_hold_frames))
departed = self._creep_departure(path, envelope) or raw_departure
if fast_departure and path.departure_frames == 0 and path.departure_motion_samples:
path.departure_motion_samples = deque([path.departure_motion_samples[-1]], maxlen=CAP_FILTER_FRAMES)
path.departure_frames = path.departure_frames + 1 if departed else 0
path.pace = 0.0
fast_departure_confirmed = fast_departure and self._recent_departure_motion(path)
if path.departure_frames < STOP_HOLD_EXIT_FRAMES or (fast_departure and not fast_departure_confirmed):
if path.departure_frames < STOP_HOLD_EXIT_FRAMES:
return path.pace
path.pace = base_speed
path.state = AccelControllerState.release
@@ -640,29 +590,28 @@ class AccelController:
planner_speed = sanitized_v_ego if planner_speed is None else planner_speed
valid_context = self._valid_context(base_speed, sanitized_v_ego, a_ego, planner_speed, planner_accel, stock_accel_max, self._delay(),
engaged, cruise_initialized)
feature_context = valid_context and bool(enabled)
if feature_context and radar_fresh:
if valid_context and radar_fresh:
envelope = self.calculate_energy_envelope(radar_state, sanitized_v_ego, a_ego, selected_profile, follow_personality)
self._held_envelope = envelope
elif feature_context and self._held_envelope is not None:
elif valid_context and self._held_envelope is not None:
envelope = self._held_envelope
else:
envelope = EnergyEnvelope(lead_status=self._radar_has_lead(radar_state))
if not feature_context:
if not valid_context:
self._held_envelope = None
shadow_fresh = self._update_freshness(self.shadow, radar_fresh) if feature_context else False
if feature_context and radar_fresh:
shadow_fresh = self._update_freshness(self.shadow, radar_fresh) if valid_context else False
if valid_context and radar_fresh:
self._update_path(self.shadow, envelope, base_speed, sanitized_v_ego, selected_profile, profile_accel_max, previous_should_stop,
previous_mpc_source, planner_speed, planner_accel)
shadow_active = True
elif feature_context and not shadow_fresh and self.shadow.pace is not None:
elif valid_context and not shadow_fresh and self.shadow.pace is not None:
shadow_active = True
else:
self.shadow.reset()
shadow_active = False
live_context = feature_context and bool(acc_selected)
live_context = valid_context and bool(enabled) and bool(acc_selected)
live_fresh = self._update_freshness(self.live, radar_fresh) if live_context else False
if live_context and radar_fresh:
pace_target = self._update_path(self.live, envelope, base_speed, sanitized_v_ego, selected_profile, profile_accel_max,
@@ -44,7 +44,6 @@ MATCHED_PACE_DECEL_RATE = 0.50
BRAKING_ACCEL_LIMIT_THRESHOLD = -0.11
MPC_DECEL_JERK_COST_MULTIPLIER = 1.05
MPC_DECEL_JERK_MAX_REQUIRED_DECEL = 0.80
MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE = 0.35
MPC_DECEL_JERK_MAX_TARGET_REDUCTION = 9.0
STOP_HOLD_EGO_SPEED = 0.30
@@ -53,7 +52,6 @@ STOP_HOLD_EXIT_SPEED = 0.80
STOP_HOLD_EXIT_FRAMES = 4
STOP_HOLD_CREEP_SPEED = 0.15
STOP_HOLD_CREEP_DISTANCE = 0.30
STOP_HOLD_FAST_DEPARTURE_DISTANCE = 0.03
STOP_HOLD_MAX_LEAD_DISTANCE = 30.0
STOP_GAP_RESERVE = 0.75
STOP_GAP_RESERVE_LEAD_SPEED = 2.0
@@ -183,13 +183,10 @@ class TestMpcCeiling:
assert np.all(ceiling >= 0.0)
def test_inactive_controller_has_no_custom_ceiling(self):
controller = make_controller()
result = update(controller, enabled=False)
result = update(make_controller(), enabled=False)
assert not result.active
assert not result.shadow_active
assert result.mpc_accel_max is None
assert math.isinf(result.effective_accel_max)
assert controller.live.pace is None and controller.shadow.pace is None
def test_profile_ceiling_does_not_interfere_while_planner_is_braking(self):
controller = make_controller()
@@ -516,8 +513,8 @@ class TestPaceAndLifecycle:
controller = make_controller()
held = enter_stop_hold(controller)
assert controller.live.pace == 0.0
results = [update(controller, make_radar(make_lead(status=True, d_rel=6.0 + (frame + 1) * 0.1, v_lead_k=2.0)),
base_speed=8.0, v_ego=0.1) for frame in range(CAP_FILTER_FRAMES + STOP_HOLD_EXIT_FRAMES)]
departing = make_radar(make_lead(status=True, d_rel=8.0, v_lead_k=2.0))
results = [update(controller, departing, base_speed=8.0, v_ego=0.1) for _ in range(CAP_FILTER_FRAMES + STOP_HOLD_EXIT_FRAMES)]
launch_index = next(index for index, result in enumerate(results) if result.launching)
assert held.state == AccelControllerState.stopHold
@@ -529,139 +526,12 @@ class TestPaceAndLifecycle:
assert results[launch_index].departure_launching
assert results[launch_index].effective_accel_max == pytest.approx(results[launch_index].positive_accel_max)
def test_stopped_governing_lead_rejects_route_51d_radar_speed_pulse_without_delaying_departure(self):
controller = make_controller()
enter_stop_hold(controller, v_ego=0.0)
speed_pulse = (0.1361, 0.1731, 0.2146, 0.2253, 0.2137, 0.1877)
distances = (6.0, 6.0, 6.0, 5.96, 6.04, 6.04)
for distance, speed in zip(distances, speed_pulse, strict=True):
radar = make_radar(make_lead(status=True, d_rel=distance, v_lead_k=speed, radar_track_id=4887),
make_lead(status=True, d_rel=6.08, v_lead_k=0.0, radar_track_id=4905))
held = update(controller, radar, base_speed=8.0, v_ego=0.0)
assert held.state == AccelControllerState.stopHold
assert held.target_speed == 0.0 and not held.launching
results = [
update(controller, make_radar(make_lead(status=True, d_rel=6.04 + (frame + 1) * 0.1, v_lead_k=2.0, radar_track_id=4887),
make_lead(status=True, d_rel=6.12 + (frame + 1) * 0.1, v_lead_k=2.0, radar_track_id=4905)),
base_speed=8.0, v_ego=0.0)
for frame in range(STOP_HOLD_EXIT_FRAMES)
]
assert all(result.state == AccelControllerState.stopHold for result in results[:-1])
assert results[-1].launching and results[-1].departure_launching
def test_route_520_slow_lead_pulse_cannot_release_stop_hold_but_real_departure_can(self):
controller = make_controller()
enter_stop_hold(controller, v_ego=0.0)
speeds = (0.01, 0.03, 0.07, 0.10, 0.14, 0.20, 0.26, 0.32, 0.34, 0.33, 0.31, 0.28, 0.24, 0.20, 0.15, 0.09, 0.05, 0.01)
offsets = (0.00, 0.00, 0.00, 0.01, 0.01, 0.02, 0.03, 0.04, 0.06, 0.07, 0.09, 0.11, 0.12, 0.13, 0.14, 0.15, 0.15, 0.16)
for offset, speed in zip(offsets, speeds, strict=True):
pulse = make_radar(make_lead(status=True, d_rel=6.0 + offset, v_lead_k=speed, radar_track_id=2133))
held = update(controller, pulse, base_speed=8.0, v_ego=0.0)
assert held.state == AccelControllerState.stopHold
assert held.target_speed == 0.0 and not held.launching
stopped = make_radar(make_lead(status=True, d_rel=6.2, v_lead_k=0.0, radar_track_id=2133))
assert update(controller, stopped, base_speed=8.0, v_ego=0.0).state == AccelControllerState.stopHold
results = [update(controller, make_radar(make_lead(status=True, d_rel=6.2 + (frame + 1) * 0.1, v_lead_k=2.0, radar_track_id=2133)),
base_speed=8.0, v_ego=0.0) for frame in range(STOP_HOLD_EXIT_FRAMES)]
assert all(result.state == AccelControllerState.stopHold for result in results[:-1])
assert results[-1].launching and results[-1].departure_launching
def test_fast_speed_signal_that_slows_without_separating_never_releases_stop_hold(self):
controller = make_controller()
enter_stop_hold(controller, v_ego=0.0)
departing = make_radar(make_lead(status=True, d_rel=5.9, v_lead_k=2.0))
results = [update(controller, departing, base_speed=8.0, v_ego=0.0) for _ in range(STOP_HOLD_EXIT_FRAMES)]
slowed = update(controller, make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.2)), base_speed=8.0, v_ego=0.0)
assert all(result.state == AccelControllerState.stopHold and not result.launching for result in results)
assert slowed.state == AccelControllerState.stopHold
assert slowed.target_speed == 0.0 and not slowed.launching
def test_stop_hold_reseeds_departure_distance_when_radar_track_is_replaced(self):
controller = make_controller()
original = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0, radar_track_id=100))
update(controller, original, base_speed=8.0, v_ego=0.0, previous_should_stop=True)
replacement = make_radar(make_lead(status=True, d_rel=6.4, v_lead_k=0.2, radar_track_id=200))
results = [update(controller, replacement, base_speed=8.0, v_ego=0.0) for _ in range(CAP_FILTER_FRAMES + STOP_HOLD_EXIT_FRAMES)]
assert all(result.state == AccelControllerState.stopHold for result in results)
assert all(result.target_speed == 0.0 and not result.launching for result in results)
def test_stop_hold_rejects_persistent_same_track_distance_step(self):
controller = make_controller()
original = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0, radar_track_id=100))
update(controller, original, base_speed=8.0, v_ego=0.0, previous_should_stop=True)
stepped = make_radar(make_lead(status=True, d_rel=6.4, v_lead_k=0.2, radar_track_id=100))
results = [update(controller, stepped, base_speed=8.0, v_ego=0.0) for _ in range(CAP_FILTER_FRAMES + STOP_HOLD_EXIT_FRAMES)]
assert all(result.state == AccelControllerState.stopHold for result in results)
assert all(result.target_speed == 0.0 and not result.launching for result in results)
def test_stop_hold_reseeds_non_selected_departure_lead_when_its_track_is_replaced(self):
controller = make_controller()
original = make_radar(make_lead(status=True, d_rel=3.0, v_lead_k=0.2, radar_track_id=100),
make_lead(status=True, d_rel=6.0, v_lead_k=0.1, radar_track_id=200))
update(controller, original, base_speed=8.0, v_ego=0.0, previous_should_stop=True)
replacement = make_radar(make_lead(status=True, d_rel=3.4, v_lead_k=0.2, radar_track_id=101),
make_lead(status=True, d_rel=6.0, v_lead_k=0.1, radar_track_id=200))
envelope = controller.calculate_energy_envelope(replacement, 0.0, 0.0, AccelProfile.normal)
results = [update(controller, replacement, base_speed=8.0, v_ego=0.0) for _ in range(CAP_FILTER_FRAMES + STOP_HOLD_EXIT_FRAMES)]
assert envelope.selected_lead == 1 and envelope.departure_lead_index == 0
assert all(result.state == AccelControllerState.stopHold for result in results)
assert all(result.target_speed == 0.0 and not result.launching for result in results)
def test_genuine_departure_survives_lead_slot_and_track_flicker(self):
controller = make_controller()
enter_stop_hold(controller, v_ego=0.0)
results = []
for frame in range(STOP_HOLD_EXIT_FRAMES):
moving = make_lead(status=True, d_rel=6.0 + (frame + 1) * 0.1, v_lead_k=2.0, radar_track_id=100)
secondary = make_lead(status=True, d_rel=7.0, v_lead_k=2.0, radar_track_id=200)
results.append(update(controller, make_radar(moving, secondary) if frame % 2 == 0 else make_radar(secondary, moving),
base_speed=8.0, v_ego=0.0))
assert all(result.state == AccelControllerState.stopHold for result in results[:-1])
assert results[-1].launching and results[-1].departure_launching
def test_fast_speed_glitch_without_distance_progress_stays_in_stop_hold(self):
controller = make_controller()
stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0, radar_track_id=100))
update(controller, stopped, base_speed=8.0, v_ego=0.0, previous_should_stop=True)
glitch = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.9, radar_track_id=100))
results = [update(controller, glitch, base_speed=8.0, v_ego=0.0) for _ in range(STOP_HOLD_EXIT_FRAMES)]
results.append(update(controller, stopped, base_speed=8.0, v_ego=0.0))
assert all(result.state == AccelControllerState.stopHold for result in results)
assert all(result.target_speed == 0.0 and not result.launching for result in results)
def test_moving_departure_does_not_reenter_stop_hold_when_speed_crosses_exit_threshold(self):
controller = make_controller()
stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0, radar_track_id=100))
update(controller, stopped, base_speed=8.0, v_ego=0.0, previous_should_stop=True)
distance = 6.0
results = []
for speed in (0.81, 0.82, 0.83, 0.84, 0.79, 0.76, 0.74, 0.72):
distance += speed * DT_MDL
radar = make_radar(make_lead(status=True, d_rel=distance, v_lead_k=speed, radar_track_id=100))
results.append(update(controller, radar, base_speed=8.0, v_ego=0.0))
launch_index = next(index for index, result in enumerate(results) if result.launching)
assert all(result.state != AccelControllerState.stopHold for result in results[launch_index:])
assert all(result.target_speed > 0.0 and result.departure_launching for result in results[launch_index:])
def test_reused_radar_does_not_pulse_stop_hold_or_departure_target(self):
controller = make_controller()
enter_stop_hold(controller)
departing = make_radar(make_lead(status=True, d_rel=8.0, v_lead_k=2.0))
for frame in range(STOP_HOLD_EXIT_FRAMES):
departing = make_radar(make_lead(status=True, d_rel=6.0 + (frame + 1) * 0.1, v_lead_k=2.0))
fresh = update(controller, departing, base_speed=8.0, v_ego=0.1)
held = update(controller, departing, base_speed=8.0, v_ego=0.1, radar_fresh=False,
previous_mpc_source=LongitudinalPlanSource.lead0, planner_speed=0.01)
@@ -733,15 +603,14 @@ class TestPaceAndLifecycle:
results.append(update(controller, creeping, base_speed=8.0, v_ego=0.0))
launch_index = next(index for index, result in enumerate(results) if result.launching)
assert launch_index * DT_MDL <= 2.0
assert all(result.state != AccelControllerState.stopHold for result in results[launch_index:])
assert all(result.target_speed > 0.0 for result in results[launch_index:])
def test_departure_dropout_holds_without_resurrecting_stop_hold(self):
controller = make_controller()
enter_stop_hold(controller)
results = [update(controller, make_radar(make_lead(status=True, d_rel=6.0 + (frame + 1) * 0.1, v_lead_k=2.0)),
base_speed=8.0, v_ego=0.1) for frame in range(CAP_FILTER_FRAMES + STOP_HOLD_EXIT_FRAMES)]
departing = make_radar(make_lead(status=True, d_rel=8.0, v_lead_k=2.0))
results = [update(controller, departing, base_speed=8.0, v_ego=0.1) for _ in range(CAP_FILTER_FRAMES + STOP_HOLD_EXIT_FRAMES)]
launched = next(result for result in results if result.launching)
before_dropout = results[-1]
dropout = [update(controller, base_speed=8.0, v_ego=0.1) for _ in range(controller.lead_loss_hold_frames + 1)]
@@ -755,8 +624,8 @@ class TestPaceAndLifecycle:
def test_invalid_departure_geometry_returns_to_stop_hold(self):
controller = make_controller()
enter_stop_hold(controller)
for frame in range(CAP_FILTER_FRAMES + STOP_HOLD_EXIT_FRAMES):
departing = make_radar(make_lead(status=True, d_rel=6.0 + (frame + 1) * 0.1, v_lead_k=2.0))
departing = make_radar(make_lead(status=True, d_rel=8.0, v_lead_k=2.0))
for _ in range(CAP_FILTER_FRAMES + STOP_HOLD_EXIT_FRAMES):
launched = update(controller, departing, base_speed=8.0, v_ego=0.1)
invalid = make_radar(make_lead(status=True, d_rel=math.nan, v_lead_k=2.0))
guarded = update(controller, invalid, base_speed=8.0, v_ego=0.1)
@@ -813,7 +682,6 @@ class TestPaceAndLifecycle:
assert path.pace is None and path.matched_accel_limit is None
assert path.state == AccelControllerState.inactive
assert path.departure_frames == path.active_frames == path.lead_loss_frames == path.stale_frames == 0
assert not path.departure_motion_samples
assert not path.launching and not path.departure_launch and not path.matched_lead
assert not path.braking_limited and not path.braking_handoff and not path.pace_reserve_armed
assert math.isinf(path.filtered_cap) and math.isinf(path.filtered_lead_speed) and path.filtered_lead_accel == 0.0
@@ -1,4 +1,3 @@
from collections import deque
import inspect
import math
from types import SimpleNamespace
@@ -8,7 +7,6 @@ import pytest
from cereal import custom, log, messaging
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 N, LongitudinalMpc
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalPlanSource as MpcLongitudinalPlanSource
from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality import AccelControllerState, AccelProfile
@@ -42,9 +40,6 @@ def planner_for_mpc_test(*, target_speed=15.0, active=True, is_e2e=False, mpc_ac
is_e2e_calls = []
planner.is_e2e = lambda _sm: is_e2e_calls.append(True) or is_e2e
planner._accel_jerk_smoothing_blocked = False
planner._accel_required_decel_samples = deque(maxlen=4)
planner._accel_required_decel_lead = -1
planner._dt = DT_MDL
planner.mpc = SimpleNamespace(source=mpc_source, last_solution_status=0)
planner.update_accel_controller = lambda *_args, **_kwargs: setattr(
planner, "accel_controller_result",
@@ -75,7 +70,6 @@ def test_profile_enum_keeps_toyota_importable():
assert AccelPersonality.schema.enumerants == expected
assert CarState.__module__ == "opendbc.car.toyota.carstate"
assert "Params()" not in inspect.getsource(CarState.update)
def test_mpc_accepts_optional_acceleration_ceiling_without_changing_stock_bounds():
@@ -239,34 +233,30 @@ def test_previous_mpc_failure_gets_one_stock_recovery_cycle():
assert recovered_calls == [(({}, 15.0, True, ceiling), {"jerk_cost_multiplier": 1.0})]
@pytest.mark.parametrize(
"mpc_source",
(MpcLongitudinalPlanSource.cruise, MpcLongitudinalPlanSource.lead0, MpcLongitudinalPlanSource.lead1),
)
def test_routine_governor_restriction_forwards_the_jerk_cost_multiplier(mpc_source):
def test_routine_governor_restriction_forwards_the_jerk_cost_multiplier():
planner, _ = planner_for_mpc_test(
state=AccelControllerState.restrict, selected_lead=0, required_decel=0.30,
mpc_source=mpc_source,
mpc_source=MpcLongitudinalPlanSource.cruise,
)
_, calls = run_controller_mpc(planner)
assert calls == [(({}, 15.0, True, None), {"jerk_cost_multiplier": MPC_DECEL_JERK_COST_MULTIPLIER})]
def test_ineligible_required_decel_blocks_smoothing_only_until_the_restriction_episode_ends():
def test_lead_source_blocks_smoothing_only_until_the_restriction_episode_ends():
planner, _ = planner_for_mpc_test(
state=AccelControllerState.restrict, selected_lead=0, required_decel=0.30,
mpc_source=MpcLongitudinalPlanSource.cruise,
)
_, initial_calls = run_controller_mpc(planner)
routine_result = planner.accel_controller_result
assert initial_calls[0][1] == {"jerk_cost_multiplier": MPC_DECEL_JERK_COST_MULTIPLIER}
ineligible_result = SimpleNamespace(**(vars(routine_result) | {"required_decel": MPC_DECEL_JERK_MAX_REQUIRED_DECEL}))
planner.update_accel_controller = lambda *_args, **_kwargs: setattr(planner, "accel_controller_result", ineligible_result)
_, ineligible_calls = run_controller_mpc(planner)
assert ineligible_calls[0][1] == {"jerk_cost_multiplier": 1.0}
planner.mpc.source = MpcLongitudinalPlanSource.lead0
_, lead_calls = run_controller_mpc(planner)
assert lead_calls[0][1] == {"jerk_cost_multiplier": 1.0}
planner.update_accel_controller = lambda *_args, **_kwargs: setattr(planner, "accel_controller_result", routine_result)
planner.mpc.source = MpcLongitudinalPlanSource.cruise
_, flicker_calls = run_controller_mpc(planner)
assert flicker_calls[0][1] == {"jerk_cost_multiplier": 1.0}
@@ -278,50 +268,6 @@ def test_ineligible_required_decel_blocks_smoothing_only_until_the_restriction_e
assert rearmed_calls[0][1] == {"jerk_cost_multiplier": MPC_DECEL_JERK_COST_MULTIPLIER}
def test_consistently_tightening_lead_releases_smoothing_until_the_restriction_ends():
planner, _ = planner_for_mpc_test(
state=AccelControllerState.restrict, selected_lead=0, required_decel=0.18,
)
_, calls = run_controller_mpc(planner)
routine_result = planner.accel_controller_result
multipliers = [calls[0][1]["jerk_cost_multiplier"]]
for required_decel in (0.20, 0.23, 0.25):
result = SimpleNamespace(**(vars(routine_result) | {"required_decel": required_decel}))
planner.update_accel_controller = lambda *_args, result=result, **_kwargs: setattr(planner, "accel_controller_result", result)
_, calls = run_controller_mpc(planner)
multipliers.append(calls[0][1]["jerk_cost_multiplier"])
assert multipliers == [MPC_DECEL_JERK_COST_MULTIPLIER] * 3 + [1.0]
easing_result = SimpleNamespace(**(vars(routine_result) | {"required_decel": 0.20}))
planner.update_accel_controller = lambda *_args, **_kwargs: setattr(planner, "accel_controller_result", easing_result)
_, calls = run_controller_mpc(planner)
assert calls[0][1] == {"jerk_cost_multiplier": 1.0}
free_result = SimpleNamespace(**(vars(routine_result) | {"state": AccelControllerState.free, "target_speed": 20.0}))
planner.update_accel_controller = lambda *_args, **_kwargs: setattr(planner, "accel_controller_result", free_result)
run_controller_mpc(planner)
planner.update_accel_controller = lambda *_args, **_kwargs: setattr(planner, "accel_controller_result", routine_result)
_, calls = run_controller_mpc(planner)
assert calls[0][1] == {"jerk_cost_multiplier": MPC_DECEL_JERK_COST_MULTIPLIER}
def test_one_frame_required_decel_noise_does_not_disable_routine_smoothing():
planner, _ = planner_for_mpc_test(
state=AccelControllerState.restrict, selected_lead=0, required_decel=0.18,
)
_, calls = run_controller_mpc(planner)
routine_result = planner.accel_controller_result
multipliers = [calls[0][1]["jerk_cost_multiplier"]]
for required_decel in (0.24, 0.19, 0.22):
result = SimpleNamespace(**(vars(routine_result) | {"required_decel": required_decel}))
planner.update_accel_controller = lambda *_args, result=result, **_kwargs: setattr(planner, "accel_controller_result", result)
_, calls = run_controller_mpc(planner)
multipliers.append(calls[0][1]["jerk_cost_multiplier"])
assert multipliers == [MPC_DECEL_JERK_COST_MULTIPLIER] * 4
@pytest.mark.parametrize(
("state", "selected_lead", "launching", "required_decel", "target_speed", "mpc_source"),
[
@@ -338,6 +284,8 @@ def test_one_frame_required_decel_noise_does_not_disable_routine_smoothing():
(AccelControllerState.restrict, 0, False, 0.30, 20.0 - MPC_DECEL_JERK_MAX_TARGET_REDUCTION, MpcLongitudinalPlanSource.cruise),
(AccelControllerState.restrict, 0, False, 0.30, 20.0, MpcLongitudinalPlanSource.cruise),
(AccelControllerState.restrict, 0, False, 0.30, 25.0, MpcLongitudinalPlanSource.cruise),
(AccelControllerState.restrict, 0, False, 0.30, 15.0, MpcLongitudinalPlanSource.lead0),
(AccelControllerState.restrict, 0, False, 0.30, 15.0, MpcLongitudinalPlanSource.lead1),
],
)
def test_non_routine_or_stock_lead_states_keep_stock_jerk_cost(
@@ -357,7 +305,6 @@ def test_controller_receives_previous_mpc_state_and_cached_radar_freshness():
planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
planner.accel_personality = int(AccelProfile.normal)
planner.accel_personality_enabled = True
planner.accel_personality_available = True
planner._radar_fresh_this_cycle = True
planner.a_desired = -0.4
planner.v_desired_filter = SimpleNamespace(x=9.5)
@@ -379,25 +326,6 @@ def test_controller_receives_previous_mpc_state_and_cached_radar_freshness():
assert received["radar_fresh"] is True
def test_controller_is_disabled_when_openpilot_longitudinal_control_is_unavailable():
planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
planner.accel_personality = int(AccelProfile.normal)
planner.accel_personality_enabled = True
planner.accel_personality_available = False
planner._radar_fresh_this_cycle = True
planner.accel_controller = SimpleNamespace(
update=lambda *_args, **kwargs: SimpleNamespace(target_speed=20.0, received_enabled=kwargs["enabled"]),
)
planner.a_desired = 0.0
planner.v_desired_filter = SimpleNamespace(x=10.0)
planner.mpc = SimpleNamespace(source=MpcLongitudinalPlanSource.cruise)
sm = {"radarState": radar_state(), "carState": SimpleNamespace(vEgo=10.0, aEgo=0.0), "selfdriveState": SimpleNamespace(personality=0)}
planner.update_accel_controller(sm, 20.0, True, True, True, ACCEL_MAX, False)
assert not planner.accel_controller_result.received_enabled
def test_radar_freshness_is_computed_once_and_shared_with_dec_and_controller():
planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
planner._radar_log_mono_time = None
@@ -405,12 +333,10 @@ def test_radar_freshness_is_computed_once_and_shared_with_dec_and_controller():
planner._read_accel_controller_params = lambda: None
planner.events_sp = SimpleNamespace(clear=lambda: None)
dec_freshness = []
planner.dec = SimpleNamespace(update=lambda _sm, *, radar_fresh, planner_accel: dec_freshness.append(radar_fresh))
planner.dec = SimpleNamespace(update=lambda _sm, *, radar_fresh: dec_freshness.append(radar_fresh))
planner.e2e_alerts_helper = SimpleNamespace(update=lambda *_args: None)
planner.accel_personality = int(AccelProfile.normal)
planner.accel_personality_enabled = True
planner.accel_personality_available = True
planner.output_a_target = 0.0
planner.a_desired = 0.0
planner.v_desired_filter = SimpleNamespace(x=10.0)
planner.mpc = SimpleNamespace(source=log.LongitudinalPlan.LongitudinalPlanSource.cruise)
@@ -1,83 +0,0 @@
import pytest
from opendbc.car import DT_CTRL, gen_empty_fingerprint, structs
from opendbc.car.car_helpers import interfaces
from opendbc.car.ford.values import CAR as FORD
from opendbc.car.gm.values import CAR as GM
from opendbc.car.honda.values import CAR as HONDA
from opendbc.car.hyundai.values import CAR as HYUNDAI
from opendbc.car.toyota.values import CAR as TOYOTA
from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan
from openpilot.selfdrive.controls.lib.longcontrol import LongControl, LongCtrlState, long_control_state_trans
VEHICLES = [
pytest.param(TOYOTA.TOYOTA_RAV4_TSS2, (True, False, 0.0, -2.0, 0.25, 0.25, 0.3), id="toyota-rav4-tss2"),
pytest.param(HONDA.HONDA_ACCORD, (True, False, 0.0, -2.0, 0.5, 0.5, 0.8), id="honda-accord"),
pytest.param(GM.CHEVROLET_BOLT_EUV, (True, False, 0.0, -2.0, 0.25, 0.25, 2.0), id="gm-bolt-euv"),
pytest.param(HYUNDAI.HYUNDAI_SONATA, (True, True, 1.0, -2.0, 0.1, 0.5, 0.8), id="hyundai-sonata"),
pytest.param(FORD.FORD_ESCAPE_MK4, (True, False, 0.0, -2.0, 0.5, 0.5, 0.8), id="ford-escape"),
]
def get_car_params(candidate):
fingerprint = gen_empty_fingerprint()
interface = interfaces[candidate]
CP = interface.get_params(candidate, fingerprint, [], True, False, False)
CP_SP = interface.get_params_sp(CP, candidate, fingerprint, [], True, False, False)
return CP, CP_SP
@pytest.mark.parametrize(("candidate", "expected"), VEHICLES)
def test_real_vehicle_longcontrol_stop_and_start(candidate, expected):
CP, CP_SP = get_car_params(candidate)
expected_long, expected_starting, *expected_tuning = expected
assert CP.openpilotLongitudinalControl is expected_long
assert CP.startingState is expected_starting
assert (CP.startAccel, CP.stopAccel, CP.vEgoStarting, CP.vEgoStopping, CP.stoppingDecelRate) == pytest.approx(expected_tuning)
stop_speeds = [CP.vEgoStopping - 0.01] * 2
drive_speeds = [CP.vEgoStopping + 0.01] * 2
_, should_stop = get_accel_from_plan(stop_speeds, [0.0, 0.0], [0.0, 1.0], vEgoStopping=CP.vEgoStopping)
_, should_drive = get_accel_from_plan(drive_speeds, [0.0, 0.0], [0.0, 1.0], vEgoStopping=CP.vEgoStopping)
assert should_stop
assert not should_drive
departure_state = long_control_state_trans(
CP,
CP_SP,
True,
LongCtrlState.stopping,
CP.vEgoStarting - 0.01,
should_drive,
brake_pressed=False,
cruise_standstill=False,
)
assert departure_state == (LongCtrlState.starting if CP.startingState else LongCtrlState.pid)
assert (
long_control_state_trans(
CP,
CP_SP,
True,
departure_state,
CP.vEgoStarting + 0.01,
should_drive,
brake_pressed=False,
cruise_standstill=False,
)
== LongCtrlState.pid
)
CS = structs.CarState()
CS.vEgo = 0.0
CS.aEgo = 0.0
control = LongControl(CP, CP_SP)
stopping_accel = control.update(True, CS, 0.0, should_stop, (-3.0, 2.0))
assert control.long_control_state == LongCtrlState.stopping
assert stopping_accel == pytest.approx(-CP.stoppingDecelRate * DT_CTRL)
departure_accel = control.update(True, CS, 0.0, should_drive, (-3.0, 2.0))
assert control.long_control_state == departure_state
assert departure_accel == pytest.approx(CP.startAccel)
@@ -29,12 +29,6 @@ class WMACConstants:
MODEL_DECEL_START = -0.5
MODEL_DECEL_RANGE = 2.0
MODEL_DECEL_TREND_FRAMES = 4
MODEL_DECEL_TREND_ACCEL = -0.075
MODEL_DECEL_TREND_RATE = 0.35
MODEL_DECEL_TREND_MAX_MPC_ACCEL = 0.075
MODEL_DECEL_TREND_MAX_COMMAND_STEP = 0.15
MODEL_DECEL_TREND_RELEASE_ACCEL = -0.02
ENDPOINT_URGENCY_GAIN = 1.3
CRITICAL_ENDPOINT_FACTOR = 0.3
CRITICAL_URGENCY_GAIN = 1.5
+2 -60
View File
@@ -6,15 +6,12 @@ See the LICENSE.md file in the root directory for more details.
"""
# Version = 2025-6-30
from collections import deque
import math
from typing import Literal
from cereal import messaging
from numpy import interp
from opendbc.car import structs
from openpilot.common.params import Params
from openpilot.common.realtime import DT_MDL
from openpilot.sunnypilot.selfdrive.controls.lib.dec.constants import WMACConstants
ModeType = Literal['acc', 'blended']
@@ -171,10 +168,6 @@ class DynamicExperimentalController:
self._expected_distance = 0.0
self._trajectory_valid = False
self._raw_urgency = 0.0
self._model_accel_samples = deque(maxlen=WMACConstants.MODEL_DECEL_TREND_FRAMES)
self._model_decel_trending = False
self._model_decel_latched = False
self._planner_accel = math.nan
def _read_params(self) -> None:
if self._frame % WMACConstants.PARAM_READ_FRAMES == 0:
@@ -241,7 +234,6 @@ class DynamicExperimentalController:
self._expected_distance = 0.0
self._trajectory_valid = False
self._update_model_decel_trend(md)
urgency = self._model_action_urgency(md)
position_valid = len(md.position.x) == WMACConstants.TRAJECTORY_SIZE
@@ -255,31 +247,6 @@ class DynamicExperimentalController:
self._has_slow_down = self._slow_down_tracker.update(self._raw_urgency)
self._urgency = self._slow_down_tracker.value
def _update_model_decel_trend(self, md) -> None:
try:
desired_accel = float(md.action.desiredAcceleration)
except (AttributeError, OverflowError, TypeError, ValueError):
desired_accel = math.nan
if not math.isfinite(desired_accel):
self._reset_model_decel_trend()
else:
self._model_accel_samples.append(desired_accel)
history = tuple(self._model_accel_samples)
self._model_decel_trending = (len(history) == self._model_accel_samples.maxlen
and history[-1] <= WMACConstants.MODEL_DECEL_TREND_ACCEL
and (history[0] - history[-1]) / (DT_MDL * (len(history) - 1)) > WMACConstants.MODEL_DECEL_TREND_RATE
and all(after <= before for before, after in zip(history[:-1], history[1:], strict=True))
and sum(after < before for before, after in zip(history[:-1], history[1:], strict=True)) >= 2)
if len(history) == self._model_accel_samples.maxlen and all(
accel >= WMACConstants.MODEL_DECEL_TREND_RELEASE_ACCEL for accel in history
):
self._model_decel_latched = False
def _reset_model_decel_trend(self) -> None:
self._model_accel_samples.clear()
self._model_decel_trending = False
self._model_decel_latched = False
def _radar_acc_lead_score(self, lead_one) -> float:
radar_track_id = int(getattr(lead_one, 'radarTrackId', -1))
return float(lead_one.status and (bool(getattr(lead_one, 'radar', False)) or radar_track_id >= 0))
@@ -323,21 +290,11 @@ class DynamicExperimentalController:
return urgency
def _model_decel_handoff_ready(self) -> bool:
try:
mpc_accel = float(self._mpc.a_solution[1])
return (math.isfinite(mpc_accel) and mpc_accel <= WMACConstants.MODEL_DECEL_TREND_MAX_MPC_ACCEL
and math.isfinite(self._planner_accel) and self._planner_accel <= WMACConstants.MODEL_DECEL_TREND_MAX_MPC_ACCEL
and self._planner_accel - self._model_accel_samples[-1] <= WMACConstants.MODEL_DECEL_TREND_MAX_COMMAND_STEP)
except (AttributeError, IndexError, OverflowError, TypeError, ValueError):
return False
def _desired_mode(self) -> tuple[ModeType, bool]:
standstill = self._standstill_count > WMACConstants.STANDSTILL_FRAMES
urgent_slow_down = self._has_slow_down and self._raw_urgency > WMACConstants.URGENT_SLOW_DOWN_PROB
if not self._CP.radarUnavailable and self._has_current_radar_acc_lead:
self._reset_model_decel_trend()
return 'acc', True
radar_stale = not self._radar_fresh if self._has_mpc_fcw else self._radar_stale_frames > 1
@@ -347,14 +304,8 @@ class DynamicExperimentalController:
return 'blended', True
if not self._CP.radarUnavailable and self._has_radar_acc_lead:
self._reset_model_decel_trend()
return 'acc', True
entering_model_slowdown = self._model_decel_trending and self._model_decel_handoff_ready() and not self._model_decel_latched
self._model_decel_latched |= entering_model_slowdown
if self._model_decel_latched:
return 'blended', entering_model_slowdown
if self._has_mpc_fcw:
return 'blended', True
@@ -368,24 +319,15 @@ class DynamicExperimentalController:
return 'acc', False
def update(self, sm: messaging.SubMaster, *, radar_fresh: bool = True, planner_accel: float | None = None) -> None:
def update(self, sm: messaging.SubMaster, *, radar_fresh: bool = True) -> None:
self._read_params()
self.set_mpc_fcw_crash_cnt()
try:
self._planner_accel = float(planner_accel)
except (OverflowError, TypeError, ValueError):
self._planner_accel = math.nan
self._update_calculations(sm, radar_fresh)
self._active = sm['selfdriveState'].experimentalMode and self._enabled
if not self._active:
model_decel_latched = self._model_decel_latched
self._reset_model_decel_trend()
if model_decel_latched:
self._mode_manager.request_mode('acc', immediate=True)
mode, immediate = self._desired_mode()
self._mode_manager.request_mode(mode, immediate=immediate, hold_frames=WMACConstants.EMERGENCY_HOLD_FRAMES,
cancel_hold=not self._CP.radarUnavailable and self._has_radar_acc_lead)
self._mode_manager.update()
self._active = sm['selfdriveState'].experimentalMode and self._enabled
self._frame += 1
@@ -77,7 +77,6 @@ def mock_cp():
def mock_mpc():
class MPC:
crash_cnt = 0
a_solution = [0.0, 0.0]
return MPC()
@@ -160,159 +159,6 @@ def test_model_should_stop_triggers_blended_without_valid_trajectory(mock_cp, mo
assert controller.mode() == "blended"
def test_confirmed_model_decel_trend_enters_blended_before_a_large_command(mock_cp, mock_mpc, default_sm):
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
default_sm['radarState'] = MockRadarState(status=0.0)
for desired_acceleration in (-0.02, -0.05, -0.08):
default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
controller.update(default_sm, planner_accel=0.0)
assert controller.mode() == "acc"
default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=-0.12)
controller.update(default_sm, planner_accel=0.0)
assert controller._model_decel_trending
assert not controller._has_slow_down
assert controller.mode() == "blended"
def test_confirmed_model_decel_handoff_stays_latched_through_a_plateau(mock_cp, mock_mpc, default_sm):
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
default_sm['radarState'] = MockRadarState(status=0.0)
for desired_acceleration in (-0.02, -0.05, -0.08, -0.12):
default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
controller.update(default_sm, planner_accel=0.0)
for _ in range(WMACConstants.EMERGENCY_HOLD_FRAMES + WMACConstants.EXIT_BLENDED_FRAMES + 1):
default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=-0.12)
controller.update(default_sm, planner_accel=0.0)
assert not controller._model_decel_trending
assert controller._model_decel_latched
assert controller.mode() == "blended"
for _ in range(WMACConstants.MODEL_DECEL_TREND_FRAMES + WMACConstants.EXIT_BLENDED_FRAMES):
default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=0.0)
controller.update(default_sm, planner_accel=0.0)
assert not controller._model_decel_latched
assert controller.mode() == "acc"
def test_model_decel_trend_never_overrides_a_radar_lead(mock_cp, mock_mpc, default_sm):
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
for desired_acceleration in (-0.02, -0.05, -0.08, -0.12):
default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
controller.update(default_sm)
assert not controller._model_accel_samples
assert not controller._model_decel_latched
assert controller._has_radar_acc_lead
assert controller.mode() == "acc"
def test_radar_acquisition_clears_a_latched_model_decel_handoff(mock_cp, mock_mpc, default_sm):
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
default_sm['radarState'] = MockRadarState(status=0.0)
for desired_acceleration in (-0.02, -0.05, -0.08, -0.12):
default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
controller.update(default_sm, planner_accel=0.0)
assert controller._model_decel_latched
default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
controller.update(default_sm, planner_accel=0.0)
assert not controller._model_accel_samples
assert not controller._model_decel_latched
assert controller.mode() == "acc"
def test_model_decel_trend_does_not_accumulate_while_dec_is_inactive(mock_cp, mock_mpc, default_sm):
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
default_sm['radarState'] = MockRadarState(status=0.0)
default_sm['selfdriveState'].experimentalMode = False
for desired_acceleration in (-0.02, -0.05, -0.08, -0.12):
default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
controller.update(default_sm, planner_accel=0.0)
assert not controller._model_accel_samples
assert not controller._model_decel_latched
default_sm['selfdriveState'].experimentalMode = True
controller.update(default_sm, planner_accel=0.0)
assert not controller._model_decel_trending
assert controller.mode() == "acc"
def test_disabling_dec_clears_a_latched_model_decel_mode(mock_cp, mock_mpc, default_sm):
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
default_sm['radarState'] = MockRadarState(status=0.0)
for desired_acceleration in (-0.02, -0.05, -0.08, -0.12):
default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
controller.update(default_sm, planner_accel=0.0)
assert controller._model_decel_latched
assert controller.mode() == "blended"
default_sm['selfdriveState'].experimentalMode = False
default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=0.0)
controller.update(default_sm, planner_accel=0.0)
assert not controller._model_decel_latched
assert controller.mode() == "acc"
def test_model_decel_trend_waits_while_mpc_is_accelerating(mock_cp, mock_mpc, default_sm):
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
default_sm['radarState'] = MockRadarState(status=0.0)
mock_mpc.a_solution[1] = 0.5
for desired_acceleration in (-0.02, -0.05, -0.08, -0.12):
default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
controller.update(default_sm, planner_accel=0.0)
assert controller._model_decel_trending
assert controller.mode() == "acc"
def test_steep_model_decel_trend_defers_to_the_existing_urgent_path(mock_cp, mock_mpc, default_sm):
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
default_sm['radarState'] = MockRadarState(status=0.0)
for desired_acceleration in (0.0, -0.2, -0.4, -0.6):
default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
controller.update(default_sm, planner_accel=0.05)
assert controller._model_decel_trending
assert controller.mode() == "acc"
def test_model_decel_trend_waits_while_the_planner_is_accelerating(mock_cp, mock_mpc, default_sm):
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
default_sm['radarState'] = MockRadarState(status=0.0)
for desired_acceleration in (-0.02, -0.05, -0.08, -0.12):
default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
controller.update(default_sm, planner_accel=0.2)
assert controller._model_decel_trending
assert controller.mode() == "acc"
def test_alternating_model_accel_noise_does_not_trigger_an_early_handoff(mock_cp, mock_mpc, default_sm):
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
default_sm['radarState'] = MockRadarState(status=0.0)
for desired_acceleration in (0.0, -0.2, 0.0, -0.2):
default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
controller.update(default_sm)
assert not controller._model_decel_trending
assert controller.mode() == "acc"
def test_radar_lead_keeps_acc_over_model_slowdown(mock_cp, mock_mpc, default_sm):
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
@@ -5,20 +5,17 @@ This file is part of sunnypilot and is licensed under the MIT License.
See the LICENSE.md file in the root directory for more details.
"""
from collections import deque
import math
from cereal import messaging, custom
from opendbc.car import structs
from openpilot.common.constants import CV
from openpilot.common.params import Params
from openpilot.common.realtime import DT_MDL
from openpilot.selfdrive.car.cruise import V_CRUISE_MAX
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalPlanSource as MpcLongitudinalPlanSource
from openpilot.sunnypilot import get_sanitize_int_param
from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality import AccelController, AccelControllerState, AccelProfile
from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import (
MPC_DECEL_JERK_COST_MULTIPLIER, MPC_DECEL_JERK_MAX_REQUIRED_DECEL, MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE,
MPC_DECEL_JERK_MAX_TARGET_REDUCTION,
MPC_DECEL_JERK_COST_MULTIPLIER, MPC_DECEL_JERK_MAX_REQUIRED_DECEL, MPC_DECEL_JERK_MAX_TARGET_REDUCTION,
)
from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
from openpilot.sunnypilot.selfdrive.controls.lib.e2e_alerts_helper import E2EAlertsHelper
@@ -46,11 +43,7 @@ class LongitudinalPlannerSP:
self.e2e_alerts_helper = E2EAlertsHelper()
self.accel_controller = AccelController(CP, dt=dt)
self.accel_controller_result = None
self.accel_personality_available = bool(CP.openpilotLongitudinalControl)
self._accel_jerk_smoothing_blocked = False
self._accel_required_decel_samples = deque(maxlen=4)
self._accel_required_decel_lead = -1
self._dt = dt
self._radar_log_mono_time = None
self._radar_fresh_this_cycle = True
@@ -126,8 +119,7 @@ class LongitudinalPlannerSP:
self.accel_controller_result = self.accel_controller.update(
sm['radarState'], base_speed=base_speed, v_ego=sm['carState'].vEgo, a_ego=sm['carState'].aEgo,
profile=self.accel_personality, follow_personality=sm['selfdriveState'].personality,
enabled=self.accel_personality_enabled and self.accel_personality_available,
acc_selected=acc_selected, engaged=engaged, cruise_initialized=cruise_initialized,
enabled=self.accel_personality_enabled, acc_selected=acc_selected, engaged=engaged, cruise_initialized=cruise_initialized,
stock_accel_max=stock_accel_max, previous_should_stop=previous_should_stop,
radar_fresh=getattr(self, '_radar_fresh_this_cycle', True),
previous_mpc_source=getattr(getattr(self, 'mpc', None), 'source', None),
@@ -167,24 +159,14 @@ class LongitudinalPlannerSP:
actuating and prev_accel_constraint and result.state == AccelControllerState.restrict and result.selected_lead >= 0
and not result.launching and target_reduction > 1e-6
)
if not lead_restriction or result.selected_lead != self._accel_required_decel_lead or not math.isfinite(result.required_decel):
self._accel_required_decel_samples.clear()
if lead_restriction and math.isfinite(result.required_decel):
self._accel_required_decel_samples.append(result.required_decel)
self._accel_required_decel_lead = result.selected_lead if lead_restriction else -1
required_decel_history = tuple(self._accel_required_decel_samples)
tightening_lead = (len(required_decel_history) == self._accel_required_decel_samples.maxlen
and (required_decel_history[-1] - required_decel_history[0]) /
(self._dt * (len(required_decel_history) - 1)) > MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE
and sum(after > before for before, after in zip(required_decel_history[:-1], required_decel_history[1:], strict=True)) >= 2)
smoothing_eligible = (lead_restriction and target_reduction < MPC_DECEL_JERK_MAX_TARGET_REDUCTION
and 0.0 < result.required_decel < MPC_DECEL_JERK_MAX_REQUIRED_DECEL and not tightening_lead)
and 0.0 < result.required_decel < MPC_DECEL_JERK_MAX_REQUIRED_DECEL)
smoothing_blocked = getattr(self, '_accel_jerk_smoothing_blocked', False)
if previous_mpc_failed:
smoothing_blocked = True
elif not lead_restriction:
smoothing_blocked = False
elif not smoothing_blocked and not smoothing_eligible:
elif not smoothing_blocked and (getattr(self.mpc, 'source', None) != MpcLongitudinalPlanSource.cruise or not smoothing_eligible):
smoothing_blocked = True
self._accel_jerk_smoothing_blocked = smoothing_blocked
jerk_cost_multiplier = MPC_DECEL_JERK_COST_MULTIPLIER if smoothing_eligible and not smoothing_blocked else 1.0
@@ -204,7 +186,7 @@ class LongitudinalPlannerSP:
self._radar_fresh_this_cycle = self._update_radar_freshness(sm)
self._read_accel_controller_params()
self.events_sp.clear()
self.dec.update(sm, radar_fresh=self._radar_fresh_this_cycle, planner_accel=self.output_a_target)
self.dec.update(sm, radar_fresh=self._radar_fresh_this_cycle)
self.e2e_alerts_helper.update(sm, self.events_sp)
def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
@@ -7,14 +7,13 @@ import pytest
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 LongitudinalPlanSource, STOP_DISTANCE, get_T_FOLLOW
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import STOP_DISTANCE, get_T_FOLLOW
from openpilot.selfdrive.controls.lib.longitudinal_planner import get_max_accel
from openpilot.selfdrive.test.longitudinal_maneuvers.plant import PRIUS_TSS2_ROUTE_MODEL, LeadObservation, Plant
from openpilot.sunnypilot.selfdrive.controls.lib import longitudinal_planner as longitudinal_planner_sp
from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality import AccelControllerState, AccelProfile
from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import (
MATCHED_PACE_DECEL_RATE, MPC_DECEL_JERK_COST_MULTIPLIER, MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE, PACE_TARGET_RESERVE,
STOP_HOLD_EXIT_FRAMES,
MATCHED_PACE_DECEL_RATE, MPC_DECEL_JERK_COST_MULTIPLIER, PACE_TARGET_RESERVE,
)
ACTUATOR_DYNAMICS = (
@@ -388,35 +387,6 @@ def test_dec_retains_acc_through_route_like_radar_marker_dropout():
assert trace.solver_failures == 0
def test_dec_uses_confirmed_model_slowdown_while_the_handoff_is_still_gentle():
def model_action(current_time: float, _v_ego: float, _a_ego: float) -> tuple[float, bool]:
if current_time < 1.0:
return 0.0, False
if current_time < 1.75:
return -0.5 * (current_time - 1.0), False
if current_time < 3.25:
return -0.375, False
return -0.375 - 0.5 * (current_time - 3.25), False
trace = _run(
duration=4.5, controller_enabled=True, dec_enabled=True, e2e=True, lead_relevancy=False, speed=22.0,
v_cruise=22.0, model_action_fn=model_action, actuator_delay=0.15, actuator_lag=0.20,
)
mode_changes = np.flatnonzero(np.asarray(trace.dec_mode)[1:] != np.asarray(trace.dec_mode)[:-1]) + 1
response = trace.time >= 0.5
assert len(mode_changes) == 1
assert trace.dec_mode[mode_changes[0]] == "blended"
assert trace.time[mode_changes[0]] <= 1.20 + 1e-9
assert -0.10 < trace.a_target[mode_changes[0]] < 0.0
assert np.all(np.asarray(trace.dec_mode)[(trace.time >= 1.75) & (trace.time < 3.25)] == "blended")
assert np.max(np.abs(_command_jerk(trace)[response[1:]])) < 3.0
assert not trace.fcw.any()
assert trace.raw_radar_passthrough.all()
assert np.all(trace.mpc_calls == 1)
assert trace.solver_failures == 0
def test_clear_road_launch_is_prompt_and_profiles_separate_above_launch_speed():
traces = [
_run(
@@ -468,72 +438,6 @@ def test_decel_smoothing_does_not_change_clear_road_acceleration_at_representati
assert smoothed.solver_failures == stock_weight.solver_failures == 0
def test_lead_bound_routine_decel_uses_smoothing_without_delaying_initial_braking(monkeypatch):
def lead_speed(current_time: float) -> float:
return max(22.5, 25.0 - 0.4 * current_time)
common = dict(
duration=8.0, controller_enabled=True, profile=AccelProfile.eco, lead_relevancy=True, speed=29.0,
distance_lead=80.0, v_lead=lead_speed, v_cruise=33.528, actuator_delay=0.15, actuator_lag=0.20,
)
monkeypatch.setattr(longitudinal_planner_sp, "MPC_DECEL_JERK_COST_MULTIPLIER", 1.0)
baseline = _run(**common)
monkeypatch.setattr(longitudinal_planner_sp, "MPC_DECEL_JERK_COST_MULTIPLIER", MPC_DECEL_JERK_COST_MULTIPLIER)
smoothed = _run(**common)
response = smoothed.time >= 0.5
baseline_gap = baseline.distance_lead - baseline.distance
gap = smoothed.distance_lead - smoothed.distance
assert set(np.asarray(smoothed.source)[response]) <= {LongitudinalPlanSource.lead0, LongitudinalPlanSource.lead1}
assert np.max(smoothed.required_decel[response]) < 0.80
assert float(np.percentile(np.abs(_filtered_realized_jerk(smoothed)), 95)) < float(np.percentile(np.abs(_filtered_realized_jerk(baseline)), 95))
assert float(np.percentile(np.abs(_command_jerk(smoothed, after=0.5)), 95)) < float(np.percentile(np.abs(_command_jerk(baseline, after=0.5)), 95))
assert _first_time_below(smoothed, -0.2) <= _first_time_below(baseline, -0.2) + 1e-6
assert _first_time_below(smoothed, -0.5) <= _first_time_below(baseline, -0.5) + 0.25 + 1e-6
assert np.min(gap) >= np.min(baseline_gap) - 0.25
assert np.max(np.abs(_command_jerk(smoothed, after=0.5))) < 3.0
assert not _has_propulsion_brake_cycle(smoothed.a_target[response])
assert not smoothed.fcw.any()
assert smoothed.solver_failures == 0
def test_tightening_lead_releases_smoothing_before_late_catchup(monkeypatch):
event_time = 3.0
lead_jerk = 1.02
max_lead_decel = 2.22
ramp_time = max_lead_decel / lead_jerk
def lead_speed(current_time: float) -> float:
braking_time = max(current_time - event_time, 0.0)
ramp = min(braking_time, ramp_time)
return 16.9 - 0.5 * lead_jerk * ramp**2 - max_lead_decel * max(braking_time - ramp_time, 0.0)
common = dict(
duration=7.0, profile=AccelProfile.eco, lead_relevancy=True, speed=15.9, distance_lead=35.7,
v_lead=lead_speed, v_cruise=17.4, actuator_delay=0.15, actuator_lag=0.20,
)
stock = _run(controller_enabled=False, **common)
monkeypatch.setattr(longitudinal_planner_sp, "MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE", np.inf)
always_smoothed = _run(controller_enabled=True, **common)
monkeypatch.setattr(longitudinal_planner_sp, "MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE", MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE)
trace = _run(controller_enabled=True, **common)
response = trace.time >= event_time
response_jerk = trace.time[1:] >= event_time
required_decel_rate = (trace.required_decel[3:] - trace.required_decel[:-3]) / (3 * DT_MDL)
gap = trace.distance_lead - trace.distance
always_smoothed_gap = always_smoothed.distance_lead - always_smoothed.distance
assert np.max(required_decel_rate[trace.required_decel[3:] >= 0.15]) > MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE
assert _first_time_below(trace, -0.5) <= _first_time_below(stock, -0.5) + 1e-9
assert _first_time_below(trace, -0.5) <= _first_time_below(always_smoothed, -0.5) - DT_MDL + 1e-9
assert np.max(np.abs(np.diff(trace.a_target)[response_jerk] / DT_MDL)) < 3.0
assert not _has_brake_coast_brake(trace.a_target[response])
assert not _has_propulsion_brake_cycle(trace.a_target[response])
assert np.min(gap) >= np.min(always_smoothed_gap) - 1e-6
assert not stock.fcw.any() and not always_smoothed.fcw.any() and not trace.fcw.any()
assert stock.solver_failures == always_smoothed.solver_failures == trace.solver_failures == 0
def test_prius_route_model_launches_without_a_dead_pedal():
trace = _run(
duration=3.0, controller_enabled=True, profile=1, lead_relevancy=False, speed=0.0,
@@ -563,132 +467,6 @@ def test_stop_hold_survives_short_full_field_dropout():
_assert_no_new_solver_failures(trace, baseline)
@pytest.mark.parametrize("replacement_track_id", (100, 200), ids=("same-track", "replacement"))
def test_stop_hold_rejects_persistent_same_slot_range_step(replacement_track_id):
step_time = 1.0
def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None:
if lead_name == "leadTwo":
return None
stepped = current_time >= step_time
speed = 0.2 if stepped else 0.0
return truth | {"dRel": truth["dRel"] + 0.4 * stepped, "vLead": speed, "vLeadK": speed, "vRel": speed,
"aLeadK": 0.0, "radarTrackId": replacement_track_id if stepped else 100, "radar": True}
trace = _run(
duration=2.5, controller_enabled=True, lead_relevancy=True, speed=0.0, distance_lead=6.0,
v_lead=0.0, v_cruise=8.0, lead_observation_fn=observe, actuator_delay=0.15, actuator_lag=0.20,
)
assert np.all(trace.state == int(AccelControllerState.stopHold))
assert np.all(trace.target_speed == 0.0)
assert trace.should_stop.all()
assert np.max(trace.speed) < 1e-3
assert not trace.fcw.any()
assert trace.solver_failures == 0
def test_moving_departure_crossing_exit_speed_releases_once():
departure_time = 1.0
speeds = (0.81, 0.82, 0.83, 0.84, 0.79, 0.76, 0.74, 0.72)
def lead_speed(current_time: float) -> float:
frame = round((current_time - departure_time) / DT_MDL)
return 0.0 if frame < 0 else speeds[min(frame, len(speeds) - 1)]
def observe(_current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None:
return None if lead_name == "leadTwo" else truth | {"aLeadK": 0.0, "radarTrackId": 100, "radar": True}
trace = _run(
duration=3.0, controller_enabled=True, lead_relevancy=True, speed=0.0, distance_lead=6.0,
v_lead=lead_speed, v_cruise=8.0, lead_observation_fn=observe, actuator_delay=0.15, actuator_lag=0.20,
)
after_departure = trace.time >= departure_time
stop_hold = int(AccelControllerState.stopHold)
releases = np.flatnonzero((trace.state[:-1] == stop_hold) & (trace.state[1:] != stop_hold)) + 1
assert len(releases) == 1
assert trace.launching[releases[0]]
assert not np.any(trace.state[releases[0]:] == stop_hold)
assert np.count_nonzero(np.diff(trace.should_stop[after_departure].astype(int))) == 1
assert not _has_propulsion_brake_cycle(trace.a_target[after_departure])
assert not trace.fcw.any()
assert trace.solver_failures == 0
def test_route_51d_duplicate_lead_speed_pulse_cannot_release_stop_hold():
pulse_start = 1.0
departure_time = 2.0
pulse_speeds = (0.1361, 0.1731, 0.2146, 0.2253, 0.2137, 0.1877)
pulse_distances = (6.0, 6.0, 6.0, 5.96, 6.04, 6.04)
def lead_speed(current_time: float) -> float:
return 0.0 if current_time < departure_time else 2.0
def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation:
result = truth | {"radar": True, "radarTrackId": 4887 if lead_name == "leadOne" else 4905}
pulse_frame = round((current_time - pulse_start) / DT_MDL)
if 0 <= pulse_frame < len(pulse_speeds):
speed = pulse_speeds[pulse_frame] if lead_name == "leadOne" else 0.0
distance = pulse_distances[pulse_frame] if lead_name == "leadOne" else 6.08
result |= {"dRel": distance, "vLead": speed, "vLeadK": speed, "vRel": speed, "aLeadK": 0.0}
return result
trace = _run(
duration=3.0, controller_enabled=True, lead_relevancy=True, speed=0.0, distance_lead=6.0,
v_lead=lead_speed, v_cruise=8.0, lead_observation_fn=observe, actuator_delay=0.15, actuator_lag=0.20,
)
pulse = (trace.time >= pulse_start) & (trace.time < pulse_start + len(pulse_speeds) * DT_MDL)
launched = np.flatnonzero((trace.time >= departure_time) & trace.launching)
assert np.all(trace.state[pulse] == int(AccelControllerState.stopHold))
assert np.all(trace.target_speed[pulse] == 0.0)
assert np.max(trace.speed[pulse]) < 0.01
assert len(launched) and trace.time[launched[0]] <= departure_time + STOP_HOLD_EXIT_FRAMES * DT_MDL + 1e-9
assert not _has_propulsion_brake_cycle(trace.a_target[trace.time >= departure_time])
assert not trace.fcw.any()
assert trace.solver_failures == 0
def test_route_520_slow_lead_pulse_cannot_release_stop_hold_or_dampen_real_departure():
pulse_start = 1.0
departure_time = 2.5
pulse_speeds = (0.01, 0.03, 0.07, 0.10, 0.14, 0.20, 0.26, 0.32, 0.34, 0.33, 0.31, 0.28, 0.24, 0.20, 0.15, 0.09, 0.05, 0.01)
pulse_offsets = (0.00, 0.00, 0.00, 0.01, 0.01, 0.02, 0.03, 0.04, 0.06, 0.07, 0.09, 0.11, 0.12, 0.13, 0.14, 0.15, 0.15, 0.16)
def lead_speed(current_time: float) -> float:
return 0.0 if current_time < departure_time else 2.0
def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None:
if lead_name == "leadTwo":
return None
pulse_frame = round((current_time - pulse_start) / DT_MDL)
if 0 <= pulse_frame < len(pulse_speeds):
speed = pulse_speeds[pulse_frame]
return truth | {"dRel": 6.0 + pulse_offsets[pulse_frame], "vLead": speed, "vLeadK": speed, "vRel": speed,
"aLeadK": 0.0, "radarTrackId": 2133, "radar": True}
return truth | {"aLeadK": 0.0, "radarTrackId": 2133, "radar": True}
common = dict(
duration=4.0, lead_relevancy=True, speed=0.0, distance_lead=6.0, v_lead=lead_speed, v_cruise=8.0,
lead_observation_fn=observe, actuator_model=PRIUS_TSS2_ROUTE_MODEL,
)
baseline = _run(controller_enabled=False, **common)
trace = _run(controller_enabled=True, **common)
pulse = (trace.time >= pulse_start) & (trace.time < pulse_start + len(pulse_speeds) * DT_MDL)
release = np.flatnonzero((trace.time >= departure_time) & trace.launching)
assert np.all(trace.state[pulse] == int(AccelControllerState.stopHold))
assert np.all(trace.target_speed[pulse] == 0.0)
assert np.max(trace.speed[trace.time < departure_time]) < 0.01
assert len(release) and trace.time[release[0]] <= departure_time + STOP_HOLD_EXIT_FRAMES * DT_MDL + 1e-9
assert trace.a_target[release[0]] > 0.05
assert np.allclose(trace.a_target[release[0]:], baseline.a_target[release[0]:], atol=1e-5, rtol=0.0)
assert not _has_propulsion_brake_cycle(trace.a_target[trace.time >= departure_time])
assert not trace.fcw.any()
assert trace.solver_failures == 0
@pytest.mark.parametrize(("actuator_delay", "actuator_lag"), ACTUATOR_DYNAMICS, ids=ACTUATOR_IDS)
def test_stopped_lead_requires_four_departure_frames_and_launches_within_one_second(actuator_delay, actuator_lag):
departure_time = 1.0
@@ -815,7 +593,6 @@ def test_constant_creep_departure_does_not_pulse_between_launch_and_stop_hold():
)
launched = np.flatnonzero((trace.time >= departure_time) & trace.launching)
assert len(launched)
assert trace.time[launched[0]] <= departure_time + 2.0
after_launch = slice(launched[0], None)
assert not np.any(trace.state[after_launch] == int(AccelControllerState.stopHold))
+3 -3
View File
@@ -1,6 +1,6 @@
Metadata-Version: 2.4
Name: tinygrad
Version: 0.13.0
Version: 0.12.0
Summary: You like pytorch? You like micrograd? You love tinygrad! <3
Author: George Hotz
License-Expression: MIT
@@ -205,8 +205,8 @@ Documentation along with a quick start guide can be found on the [docs website](
```python
from tinygrad import Tensor
x = Tensor.eye(3)
y = Tensor([[2.0,0,-2.0]])
x = Tensor.eye(3, requires_grad=True)
y = Tensor([[2.0,0,-2.0]], requires_grad=True)
z = y.matmul(x).sum()
z.backward()
@@ -19,12 +19,10 @@ tinygrad.egg-info/top_level.txt
tinygrad/codegen/__init__.py
tinygrad/codegen/gpudims.py
tinygrad/codegen/simplify.py
tinygrad/codegen/late/__init__.py
tinygrad/codegen/late/devectorizer.py
tinygrad/codegen/late/expander.py
tinygrad/codegen/late/gater.py
tinygrad/codegen/late/linearizer.py
tinygrad/codegen/late/regalloc.py
tinygrad/codegen/opt/__init__.py
tinygrad/codegen/opt/heuristic.py
tinygrad/codegen/opt/postrange.py
@@ -35,7 +33,6 @@ tinygrad/engine/jit.py
tinygrad/engine/realize.py
tinygrad/llm/__init__.py
tinygrad/llm/__main__.py
tinygrad/llm/chat.html
tinygrad/llm/cli.py
tinygrad/llm/gguf.py
tinygrad/llm/model.py
@@ -62,8 +59,6 @@ tinygrad/renderer/amd/dsl.py
tinygrad/renderer/amd/elf.py
tinygrad/renderer/amd/generate.py
tinygrad/renderer/amd/sqtt.py
tinygrad/renderer/isa/__init__.py
tinygrad/renderer/isa/x86.py
tinygrad/runtime/__init__.py
tinygrad/runtime/ops_amd.py
tinygrad/runtime/ops_cl.py
@@ -137,7 +132,6 @@ tinygrad/runtime/autogen/am/soc_11.py
tinygrad/runtime/autogen/am/soc_12.py
tinygrad/runtime/autogen/am/soc_9.py
tinygrad/runtime/autogen/am/vega_offsets.py
tinygrad/runtime/autogen/amd/__init__.py
tinygrad/runtime/autogen/amd/common.py
tinygrad/runtime/autogen/amd/cdna/__init__.py
tinygrad/runtime/autogen/amd/cdna/enum.py
@@ -154,21 +148,6 @@ tinygrad/runtime/autogen/amd/rdna4/enum.py
tinygrad/runtime/autogen/amd/rdna4/ins.py
tinygrad/runtime/autogen/amd/rdna4/operands.py
tinygrad/runtime/autogen/amd/rdna4/str_pcode.py
tinygrad/runtime/autogen/nv_regs/__init__.py
tinygrad/runtime/autogen/nv_regs/dev_bus.py
tinygrad/runtime/autogen/nv_regs/dev_falcon_second_pri.py
tinygrad/runtime/autogen/nv_regs/dev_falcon_v4.py
tinygrad/runtime/autogen/nv_regs/dev_fb.py
tinygrad/runtime/autogen/nv_regs/dev_fbif_v4.py
tinygrad/runtime/autogen/nv_regs/dev_fsp_pri.py
tinygrad/runtime/autogen/nv_regs/dev_gc6_island.py
tinygrad/runtime/autogen/nv_regs/dev_gsp.py
tinygrad/runtime/autogen/nv_regs/dev_mmu.py
tinygrad/runtime/autogen/nv_regs/dev_riscv_pri.py
tinygrad/runtime/autogen/nv_regs/dev_sec_pri.py
tinygrad/runtime/autogen/nv_regs/dev_therm.py
tinygrad/runtime/autogen/nv_regs/dev_vm.py
tinygrad/runtime/autogen/nv_regs/nv_ref.py
tinygrad/runtime/graph/__init__.py
tinygrad/runtime/graph/cuda.py
tinygrad/runtime/graph/hcq.py
@@ -211,7 +190,6 @@ tinygrad/uop/spec.py
tinygrad/uop/symbolic.py
tinygrad/uop/upat.py
tinygrad/uop/validate.py
tinygrad/viz/__init__.py
tinygrad/viz/cli.py
tinygrad/viz/index.html
tinygrad/viz/serve.py