mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-07-29 15:32:13 +08:00
Compare commits
1 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| 5ebb02b22d |
@@ -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
|
||||
|
||||
@@ -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.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -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
|
||||
|
||||
+8
-140
@@ -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
|
||||
|
||||
+11
-85
@@ -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)
|
||||
|
||||
-83
@@ -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
|
||||
|
||||
@@ -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))
|
||||
|
||||
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user