mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-08-05 00:25:40 +08:00
long: prevent abrupt accel
This commit is contained in:
rav4kumar
@@ -10,9 +10,10 @@ from openpilot.sunnypilot import get_sanitize_int_param
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import (
|
||||
CAP_FILTER_FRAMES, COMFORT_DECEL, DEPARTURE_MOTION_NOISE_FLOOR, LAUNCH_END_SPEED, LAUNCH_TARGET_HEADROOM, LAUNCH_TARGET_SLEW,
|
||||
LEAD_BRAKING_ACCEL_THRESHOLD, LEAD_LOSS_HOLD_TIME, LEAD_MATCH_ACCEL_SLEW, LEAD_MATCH_GAP_GAIN, LEAD_MATCH_SPEED_HEADROOM,
|
||||
LEAD_SWITCH_MAX_HOLD_TIME,
|
||||
MATCHED_SPEED_DECEL_RATE, 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_TREND_FRAMES, SPEED_RELIEF_DEADBAND, SPEED_RESTRICT_DEADBAND, TARGET_SPEED_ARM_MARGIN,
|
||||
TARGET_SPEED_RESERVE, PLANNER_BRAKING_ACCEL_THRESHOLD, RADAR_STALE_TIMEOUT, STOP_HOLD_CREEP_DISTANCE, STOP_HOLD_EGO_SPEED,
|
||||
TARGET_RELEASE_SLEW, TARGET_SPEED_RESERVE, PLANNER_BRAKING_ACCEL_THRESHOLD, RADAR_STALE_TIMEOUT, STOP_HOLD_CREEP_DISTANCE, STOP_HOLD_EGO_SPEED,
|
||||
STOP_HOLD_EXIT_FRAMES, STOP_HOLD_EXIT_SPEED, STOP_HOLD_MAX_LEAD_DISTANCE, VEGO_NOISE_TOLERANCE, PARAM_READ_INTERVAL, AccelProfile,
|
||||
profile_accel_max, sanitize_profile,
|
||||
)
|
||||
@@ -29,6 +30,7 @@ class AccelController:
|
||||
self.dt = dt
|
||||
self.delay = float(CP.longitudinalActuatorDelay) + DT_MDL
|
||||
self.lead_loss_hold_frames = max(CAP_FILTER_FRAMES, math.ceil(LEAD_LOSS_HOLD_TIME / dt))
|
||||
self.lead_switch_max_hold_frames = max(self.lead_loss_hold_frames, math.ceil(LEAD_SWITCH_MAX_HOLD_TIME / dt))
|
||||
self.radar_stale_frames = max(1, math.ceil(RADAR_STALE_TIMEOUT / dt))
|
||||
self.params = Params()
|
||||
self.available = bool(CP.openpilotLongitudinalControl)
|
||||
@@ -88,10 +90,8 @@ class AccelController:
|
||||
and (guarded_restriction or planner_accel <= PLANNER_BRAKING_ACCEL_THRESHOLD))
|
||||
confirmed_relief = (not has_lead or (state.target_speed is not None and lead_plan.closing_speed <= 0.0
|
||||
and lead_plan.cap >= state.target_speed + SPEED_RELIEF_DEADBAND))
|
||||
if switched_to_relief and state.lead_switch_guard_frames == 0:
|
||||
state.lead_switch_guard_frames = self.lead_loss_hold_frames
|
||||
elif state.lead_switch_guard_frames > 0:
|
||||
state.lead_switch_guard_frames = (state.lead_switch_guard_frames - 1 if confirmed_relief else self.lead_loss_hold_frames)
|
||||
state.update_lead_switch_guard(switched_to_relief, confirmed_relief, slot_changed or track_changed or false_relief,
|
||||
self.lead_loss_hold_frames, self.lead_switch_max_hold_frames)
|
||||
if slot_changed or track_changed:
|
||||
state.matched_lead = False
|
||||
state.matched_accel_limit = None
|
||||
@@ -99,7 +99,7 @@ class AccelController:
|
||||
state.selected_lead = lead_plan.selected_lead
|
||||
state.selected_lead_track_id = lead_plan.selected_lead_track_id
|
||||
elif state.lead_loss_frames >= self.lead_loss_hold_frames:
|
||||
state.lead_switch_guard_frames = 0
|
||||
state.reset_lead_switch_guard()
|
||||
state.selected_lead = state.selected_lead_track_id = -1
|
||||
departure_separation = (lead_plan.departure_lead_separations[lead_plan.departure_lead_index]
|
||||
if lead_plan.departure_lead_index >= 0 else math.inf)
|
||||
@@ -120,6 +120,8 @@ class AccelController:
|
||||
e2e_handoff = previous_mpc_source == LongitudinalPlanSource.e2e
|
||||
seed_from_ego = has_lead and planner_accel > PLANNER_BRAKING_ACCEL_THRESHOLD and not e2e_handoff
|
||||
state.target_speed = min(base_speed, v_ego) if seed_from_ego else base_speed
|
||||
if seed_from_ego and v_ego >= LAUNCH_END_SPEED and lead_plan.closing_speed > 0.0:
|
||||
state.arm_release_slew()
|
||||
state.e2e_braking_handoff = e2e_handoff and planner_accel < 0.0
|
||||
state.state = AccelControllerState.free
|
||||
if v_ego < STOP_HOLD_EGO_SPEED and not stop_evidence:
|
||||
@@ -197,6 +199,7 @@ class AccelController:
|
||||
if not has_lead and (state.matched_lead or lost_lead_source):
|
||||
if lost_lead_source:
|
||||
state.target_speed = max(planner_speed, state.target_speed - MATCHED_SPEED_DECEL_RATE * self.dt)
|
||||
state.arm_release_slew()
|
||||
state.state = AccelControllerState.hold
|
||||
return state.target_speed
|
||||
|
||||
@@ -219,12 +222,18 @@ class AccelController:
|
||||
matched_ceiling = min(base_speed, filtered_cap)
|
||||
if matched_ceiling <= state.target_speed - SPEED_RESTRICT_DEADBAND:
|
||||
state.target_speed = max(matched_ceiling, state.target_speed - MATCHED_SPEED_DECEL_RATE * self.dt)
|
||||
state.arm_release_slew()
|
||||
state.state = AccelControllerState.restrict
|
||||
elif state.lead_switch_guard_frames == 0 and matched_ceiling >= state.target_speed + SPEED_RELIEF_DEADBAND:
|
||||
elif (state.lead_switch_guard_frames == 0 and matched_ceiling >= state.target_speed + SPEED_RELIEF_DEADBAND
|
||||
and (state.lead_switch_elapsed_frames < self.lead_switch_max_hold_frames or planner_accel > PLANNER_BRAKING_ACCEL_THRESHOLD)):
|
||||
state.target_speed = min(matched_ceiling, state.target_speed + profile_max_accel * self.dt)
|
||||
state.state = AccelControllerState.free if state.target_speed >= base_speed - SPEED_RESTRICT_DEADBAND else AccelControllerState.release
|
||||
else:
|
||||
state.state = AccelControllerState.free if state.target_speed >= base_speed - SPEED_RESTRICT_DEADBAND else AccelControllerState.hold
|
||||
if state.state == AccelControllerState.free:
|
||||
state.reset_release_slew(state.target_speed)
|
||||
else:
|
||||
state.update_release_slew(matched_ceiling, math.isfinite(matched_ceiling) and state.target_speed == matched_ceiling)
|
||||
return state.target_speed
|
||||
state.matched_accel_limit = None
|
||||
|
||||
@@ -234,6 +243,7 @@ class AccelController:
|
||||
|
||||
if ceiling <= state.target_speed - SPEED_RESTRICT_DEADBAND or (state.state == AccelControllerState.restrict and ceiling < state.target_speed):
|
||||
state.target_speed = max(ceiling, state.target_speed - comfort_decel * self.dt)
|
||||
state.arm_release_slew()
|
||||
state.state = AccelControllerState.restrict
|
||||
return state.target_speed
|
||||
|
||||
@@ -244,14 +254,27 @@ class AccelController:
|
||||
return state.target_speed
|
||||
|
||||
confirmed_clear_road = not math.isfinite(filtered_cap) and not guarded_lead_loss
|
||||
relief = not has_lead or lead_plan.closing_speed <= 0.0
|
||||
if relief and (ceiling >= state.target_speed + SPEED_RELIEF_DEADBAND or (confirmed_clear_road and ceiling > state.target_speed)):
|
||||
relief = (not has_lead or lead_plan.closing_speed <= 0.0) and planner_accel > PLANNER_BRAKING_ACCEL_THRESHOLD
|
||||
continuing_release = state.release_slew_armed and ceiling > state.target_speed
|
||||
if relief and (continuing_release or ceiling >= state.target_speed + SPEED_RELIEF_DEADBAND
|
||||
or (confirmed_clear_road and ceiling > state.target_speed)):
|
||||
if state.lead_switch_guard_frames == 0:
|
||||
state.target_speed = ceiling
|
||||
state.state = (AccelControllerState.hold if state.lead_switch_guard_frames > 0 else
|
||||
AccelControllerState.free if state.target_speed >= base_speed - SPEED_RESTRICT_DEADBAND else AccelControllerState.release)
|
||||
timed_out = state.lead_switch_elapsed_frames >= self.lead_switch_max_hold_frames
|
||||
if not state.release_slew_armed and (timed_out or (state.release_settle_speed is not None
|
||||
and ceiling - state.target_speed > TARGET_RELEASE_SLEW * self.dt)):
|
||||
state.arm_release_slew(force=True)
|
||||
release_rate = comfort_decel if timed_out else TARGET_RELEASE_SLEW
|
||||
state.target_speed = min(ceiling, state.target_speed + release_rate * self.dt) if state.release_slew_armed else ceiling
|
||||
if state.release_slew_armed and state.target_speed < ceiling:
|
||||
state.state = AccelControllerState.release
|
||||
else:
|
||||
state.state = AccelControllerState.free if state.target_speed >= base_speed - SPEED_RESTRICT_DEADBAND else AccelControllerState.hold
|
||||
else:
|
||||
state.state = AccelControllerState.free if state.target_speed >= base_speed - SPEED_RESTRICT_DEADBAND else AccelControllerState.hold
|
||||
if state.target_speed >= base_speed:
|
||||
state.reset_release_slew(state.target_speed)
|
||||
else:
|
||||
state.update_release_slew(ceiling, math.isfinite(ceiling) and state.target_speed == ceiling)
|
||||
return state.target_speed
|
||||
|
||||
def _update_freshness(self, radar_fresh: bool) -> None:
|
||||
|
||||
@@ -23,8 +23,10 @@ ACCEL_PROFILE_MAX_V = {
|
||||
|
||||
CAP_FILTER_FRAMES = 5
|
||||
LEAD_LOSS_HOLD_TIME = 0.50
|
||||
LEAD_SWITCH_MAX_HOLD_TIME = 6.0
|
||||
SPEED_RESTRICT_DEADBAND = 0.15
|
||||
SPEED_RELIEF_DEADBAND = 0.35
|
||||
TARGET_RELEASE_SLEW = 8.75
|
||||
TARGET_SPEED_ARM_MARGIN = 1.0
|
||||
TARGET_SPEED_RESERVE = 0.10
|
||||
LAUNCH_TARGET_HEADROOM = 3.0
|
||||
|
||||
@@ -5,7 +5,8 @@ import numpy as np
|
||||
|
||||
from cereal import custom
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import (
|
||||
CAP_FILTER_FRAMES, DEPARTURE_MOTION_NOISE_FLOOR, DEPARTURE_MOTION_STEP_MIN, STOP_HOLD_CREEP_DISTANCE, STOP_HOLD_CREEP_SPEED, STOP_HOLD_EXIT_FRAMES,
|
||||
CAP_FILTER_FRAMES, DEPARTURE_MOTION_NOISE_FLOOR, DEPARTURE_MOTION_STEP_MIN, SPEED_RELIEF_DEADBAND, STOP_HOLD_CREEP_DISTANCE,
|
||||
STOP_HOLD_CREEP_SPEED, STOP_HOLD_EXIT_FRAMES,
|
||||
)
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.lead import LeadPlan
|
||||
|
||||
@@ -97,13 +98,61 @@ class TargetState:
|
||||
self.departure = DepartureTracker()
|
||||
self.target_speed: float | None = None
|
||||
self.state = AccelControllerState.inactive
|
||||
self.departure_frames = self.active_frames = self.lead_loss_frames = 0
|
||||
self.lead_switch_guard_frames = self.stale_frames = 0
|
||||
self.departure_frames = self.active_frames = self.lead_loss_frames = self.release_settle_frames = 0
|
||||
self.lead_switch_guard_frames = self.lead_switch_elapsed_frames = self.lead_switch_stable_frames = self.stale_frames = 0
|
||||
self.selected_lead = self.selected_lead_track_id = -1
|
||||
self.launching = self.departure_launch = self.matched_lead = False
|
||||
self.launching = self.departure_launch = self.matched_lead = self.release_slew_armed = False
|
||||
self.lead_braking = self.e2e_braking_handoff = self.speed_reserve_armed = False
|
||||
self.speed_reserve_suppressed = False
|
||||
self.matched_accel_limit: float | None = None
|
||||
self.release_settle_speed: float | None = None
|
||||
|
||||
def reset_lead_switch_guard(self) -> None:
|
||||
self.lead_switch_guard_frames = self.lead_switch_elapsed_frames = self.lead_switch_stable_frames = 0
|
||||
|
||||
def arm_release_slew(self, force: bool = False) -> None:
|
||||
if self.release_slew_armed:
|
||||
self.release_settle_frames = 0
|
||||
return
|
||||
if not force and self.release_settle_speed is not None and self.target_speed is not None:
|
||||
if self.release_settle_speed - self.target_speed < SPEED_RELIEF_DEADBAND:
|
||||
return
|
||||
self.release_slew_armed = True
|
||||
self.release_settle_frames = 0
|
||||
self.release_settle_speed = None
|
||||
|
||||
def reset_release_slew(self, settled_speed: float | None = None) -> None:
|
||||
self.release_slew_armed = False
|
||||
self.release_settle_frames = 0
|
||||
self.release_settle_speed = settled_speed
|
||||
|
||||
def update_release_slew(self, ceiling: float, settled: bool) -> None:
|
||||
if not self.release_slew_armed:
|
||||
return
|
||||
if not settled:
|
||||
self.release_settle_frames = 0
|
||||
elif self.release_settle_speed is None or ceiling > self.release_settle_speed:
|
||||
self.release_settle_frames = 1
|
||||
self.release_settle_speed = ceiling
|
||||
else:
|
||||
self.release_settle_frames += 1
|
||||
if self.release_settle_frames >= CAP_FILTER_FRAMES:
|
||||
self.release_slew_armed = False
|
||||
self.release_settle_frames = 0
|
||||
|
||||
def update_lead_switch_guard(self, arm: bool, confirmed: bool, unstable: bool, hold_frames: int, max_frames: int) -> None:
|
||||
if self.lead_switch_elapsed_frames > 0:
|
||||
self.lead_switch_stable_frames = 0 if unstable else self.lead_switch_stable_frames + 1
|
||||
if self.lead_switch_guard_frames == 0 and self.lead_switch_stable_frames >= hold_frames:
|
||||
self.reset_lead_switch_guard()
|
||||
if arm and self.lead_switch_elapsed_frames == 0:
|
||||
self.lead_switch_guard_frames, self.lead_switch_elapsed_frames = hold_frames, 1
|
||||
elif self.lead_switch_guard_frames > 0:
|
||||
self.lead_switch_elapsed_frames += 1
|
||||
if self.lead_switch_elapsed_frames >= max_frames:
|
||||
self.lead_switch_guard_frames = 0
|
||||
else:
|
||||
self.lead_switch_guard_frames = self.lead_switch_guard_frames - 1 if confirmed else hold_frames
|
||||
|
||||
@property
|
||||
def filtered_cap(self) -> float:
|
||||
@@ -136,5 +185,7 @@ class TargetState:
|
||||
self.state = AccelControllerState.stopHold
|
||||
self.departure_frames = 0
|
||||
self.launching = self.departure_launch = False
|
||||
self.reset_release_slew()
|
||||
self.matched_lead = self.speed_reserve_armed = self.speed_reserve_suppressed = False
|
||||
self.matched_accel_limit = None
|
||||
self.reset_lead_switch_guard()
|
||||
|
||||
+137
-10
@@ -13,11 +13,12 @@ from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import (
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController, AccelControllerState
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import (
|
||||
ACCEL_LIMIT_HORIZON_JERK, ACCEL_PROFILE_MAX_BP, ACCEL_PROFILE_MAX_V, ACCEL_PROFILES, CAP_FILTER_FRAMES, LAUNCH_END_SPEED,
|
||||
COMFORT_DECEL, LAUNCH_TARGET_HEADROOM, LAUNCH_TARGET_SLEW, LEAD_MATCH_ACCEL_SLEW, MATCHED_SPEED_DECEL_RATE,
|
||||
MPC_DECEL_JERK_COST_MULTIPLIER, TARGET_SPEED_RESERVE, RADAR_STALE_TIMEOUT, STOP_GAP_RESERVE, STOP_HOLD_EXIT_FRAMES, AccelProfile,
|
||||
COMFORT_DECEL, LAUNCH_TARGET_HEADROOM, LAUNCH_TARGET_SLEW, LEAD_MATCH_ACCEL_SLEW, LEAD_SWITCH_MAX_HOLD_TIME, MATCHED_SPEED_DECEL_RATE,
|
||||
MPC_DECEL_JERK_COST_MULTIPLIER, RADAR_STALE_TIMEOUT, STOP_GAP_RESERVE, STOP_HOLD_EXIT_FRAMES, TARGET_RELEASE_SLEW,
|
||||
TARGET_SPEED_RESERVE, AccelProfile,
|
||||
)
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.helpers import build_accel_ceiling
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.lead import _project_ego, calculate_lead_plan
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.lead import LeadPlan, _project_ego, calculate_lead_plan
|
||||
|
||||
|
||||
def make_lead(*, status=False, d_rel=0.0, v_lead_k=0.0, a_lead_k=0.0, a_lead_tau=1.5, radar_track_id=-1):
|
||||
@@ -84,6 +85,8 @@ class TestProfiles:
|
||||
AccelProfile.normal: [1.80, 1.50, 0.97, 0.48, 0.30],
|
||||
AccelProfile.sport: [2.00, 1.90, 1.15, 0.68, 0.42],
|
||||
}
|
||||
assert TARGET_RELEASE_SLEW == 8.75
|
||||
assert LEAD_SWITCH_MAX_HOLD_TIME == 6.0
|
||||
|
||||
@pytest.mark.parametrize("profile", ACCEL_PROFILES)
|
||||
def test_lookup_interpolates_and_stays_inside_global_limit(self, profile):
|
||||
@@ -465,10 +468,130 @@ class TestTargetLifecycle:
|
||||
churn = update(controller, radar, base_speed=25.0, v_ego=10.0, planner_speed=10.0, planner_accel=0.2)
|
||||
assert churn.target_speed <= before.target_speed + 1e-9
|
||||
|
||||
for _ in range(controller.lead_loss_hold_frames + 1):
|
||||
released = update(controller, replacement, base_speed=25.0, v_ego=10.0, planner_speed=10.0, planner_accel=0.2)
|
||||
released = [update(controller, replacement, base_speed=25.0, v_ego=10.0, planner_speed=10.0, planner_accel=0.2)
|
||||
for _ in range(controller.lead_loss_hold_frames + 1)]
|
||||
assert controller.target_state.lead_switch_guard_frames == 0
|
||||
assert released.target_speed > before.target_speed
|
||||
assert released[-1].target_speed > before.target_speed
|
||||
assert np.max(np.diff([before.target_speed, *(result.target_speed for result in released)])) <= TARGET_RELEASE_SLEW * DT_MDL + 1e-9
|
||||
|
||||
def test_initial_lead_target_release_is_slewed(self):
|
||||
controller = make_controller()
|
||||
closing = make_radar(make_lead(status=True, d_rel=90.0, v_lead_k=8.0, radar_track_id=100))
|
||||
relief = make_radar(make_lead(status=True, d_rel=90.0, v_lead_k=12.0, radar_track_id=100))
|
||||
first = update(controller, closing)
|
||||
results = [update(controller, relief) for _ in range(CAP_FILTER_FRAMES)]
|
||||
targets = [first.target_speed, *(result.target_speed for result in results)]
|
||||
|
||||
assert np.max(np.diff(targets)) > 0.0
|
||||
assert np.max(np.diff(targets)) <= TARGET_RELEASE_SLEW * DT_MDL + 1e-9
|
||||
assert controller.target_state.target_speed < controller.target_state.filtered_cap
|
||||
assert controller.target_state.release_slew_armed
|
||||
|
||||
def test_direct_relief_slews_to_a_moving_ceiling(self):
|
||||
controller = make_controller()
|
||||
state = controller.target_state
|
||||
state.target_speed, state.state, state.release_slew_armed = 18.0, AccelControllerState.hold, True
|
||||
state.active_frames, state.selected_lead, state.selected_lead_track_id = 20, 0, 100
|
||||
state.cap_samples = [20.0] * CAP_FILTER_FRAMES
|
||||
targets = []
|
||||
|
||||
for frame in range(70):
|
||||
cap = min(23.0, 20.0 + 0.1 * frame)
|
||||
lead_plan = LeadPlan(cap=cap, selected_lead=0, selected_lead_track_id=100, selected_lead_speed=20.0, lead_status=True)
|
||||
targets.append(controller._update_target(lead_plan, 25.0, 20.0, AccelProfile.normal, 0.48, False,
|
||||
LongitudinalPlanSource.cruise, 20.0, 0.0))
|
||||
|
||||
steps = np.diff(targets)
|
||||
assert np.all(steps >= -1e-9)
|
||||
assert np.max(steps) <= TARGET_RELEASE_SLEW * DT_MDL + 1e-9
|
||||
assert targets[-1] == 23.0
|
||||
assert state.state == AccelControllerState.hold and not state.release_slew_armed
|
||||
|
||||
for cap in (23.1, 23.2) * CAP_FILTER_FRAMES:
|
||||
lead_plan = LeadPlan(cap=cap, selected_lead=0, selected_lead_track_id=100, selected_lead_speed=20.0, lead_status=True)
|
||||
controller._update_target(lead_plan, 25.0, 20.0, AccelProfile.normal, 0.48, False,
|
||||
LongitudinalPlanSource.cruise, 20.0, 0.0)
|
||||
assert state.target_speed == 23.0
|
||||
|
||||
def test_settled_release_does_not_follow_subdeadband_cap_churn(self):
|
||||
controller = make_controller()
|
||||
state = controller.target_state
|
||||
state.target_speed, state.state = 20.0, AccelControllerState.hold
|
||||
state.active_frames, state.selected_lead, state.selected_lead_track_id = 20, 0, 100
|
||||
state.arm_release_slew()
|
||||
targets = [state.target_speed]
|
||||
|
||||
for cap in (19.8, 20.2) * (2 * CAP_FILTER_FRAMES):
|
||||
state.cap_samples = [cap] * CAP_FILTER_FRAMES
|
||||
lead_plan = LeadPlan(cap=cap, selected_lead=0, selected_lead_track_id=100, selected_lead_speed=25.0, lead_status=True)
|
||||
targets.append(controller._update_target(lead_plan, 25.0, 20.0, AccelProfile.normal, 0.48, False,
|
||||
LongitudinalPlanSource.cruise, 20.0, 0.0))
|
||||
|
||||
assert np.max(np.diff(targets)) <= TARGET_RELEASE_SLEW * DT_MDL + 1e-9
|
||||
assert state.release_slew_armed and state.release_settle_frames == 1
|
||||
|
||||
settle_updates = math.ceil((state.target_speed - 19.8) / (COMFORT_DECEL[AccelProfile.normal] * DT_MDL)) + CAP_FILTER_FRAMES
|
||||
for _ in range(settle_updates):
|
||||
state.cap_samples = [19.8] * CAP_FILTER_FRAMES
|
||||
lead_plan = LeadPlan(cap=19.8, selected_lead=0, selected_lead_track_id=100, selected_lead_speed=25.0, lead_status=True)
|
||||
controller._update_target(lead_plan, 25.0, 20.0, AccelProfile.normal, 0.48, False,
|
||||
LongitudinalPlanSource.cruise, 20.0, 0.0)
|
||||
assert not state.release_slew_armed
|
||||
|
||||
def test_guard_timeout_uses_wall_clock_and_does_not_rearm_during_churn(self):
|
||||
controller = make_controller()
|
||||
state = controller.target_state
|
||||
state.target_speed, state.state, state.release_slew_armed = 18.0, AccelControllerState.hold, True
|
||||
state.active_frames, state.selected_lead, state.selected_lead_track_id = 20, 0, 100
|
||||
guard_history = []
|
||||
|
||||
for frame in range(2 * controller.lead_switch_max_hold_frames):
|
||||
state.cap_samples = [20.0] * CAP_FILTER_FRAMES
|
||||
track_id = 200 if frame % 2 == 0 else 100
|
||||
lead_plan = LeadPlan(cap=25.0, selected_lead=0, selected_lead_track_id=track_id, selected_lead_speed=15.0,
|
||||
closing_speed=1.0, lead_status=True)
|
||||
controller._update_target(lead_plan, 25.0, 20.0, AccelProfile.normal, 0.48, False,
|
||||
LongitudinalPlanSource.cruise, 18.0, -0.2)
|
||||
guard_history.append(state.lead_switch_guard_frames)
|
||||
|
||||
first_zero = next(index for index, guard in enumerate(guard_history[1:], 1) if guard == 0)
|
||||
assert first_zero <= controller.lead_switch_max_hold_frames
|
||||
assert all(guard == 0 for guard in guard_history[first_zero:])
|
||||
assert state.lead_switch_elapsed_frames == controller.lead_switch_max_hold_frames
|
||||
|
||||
stable = LeadPlan(cap=25.0, selected_lead=0, selected_lead_track_id=100, selected_lead_speed=25.0, lead_status=True)
|
||||
for _ in range(controller.lead_loss_hold_frames + 20):
|
||||
controller._update_target(stable, 25.0, 20.0, AccelProfile.normal, 0.48, False,
|
||||
LongitudinalPlanSource.cruise, 20.0, 0.0)
|
||||
assert state.target_speed == 25.0 and state.state == AccelControllerState.free
|
||||
assert state.lead_switch_elapsed_frames == 0
|
||||
|
||||
state.cap_samples = [15.0] * CAP_FILTER_FRAMES
|
||||
restrictive = LeadPlan(cap=15.0, selected_lead=0, selected_lead_track_id=100, selected_lead_speed=15.0,
|
||||
closing_speed=5.0, lead_status=True)
|
||||
controller._update_target(restrictive, 25.0, 20.0, AccelProfile.normal, 0.48, False,
|
||||
LongitudinalPlanSource.cruise, 20.0, -0.2)
|
||||
replacement = restrictive._replace(cap=25.0, selected_lead_track_id=200, closing_speed=0.0)
|
||||
controller._update_target(replacement, 25.0, 20.0, AccelProfile.normal, 0.48, False,
|
||||
LongitudinalPlanSource.cruise, 20.0, -0.2)
|
||||
assert state.lead_switch_guard_frames == controller.lead_loss_hold_frames
|
||||
|
||||
def test_guard_timeout_does_not_release_while_planner_is_braking(self):
|
||||
controller = make_controller()
|
||||
state = controller.target_state
|
||||
state.target_speed, state.state, state.release_slew_armed = 18.0, AccelControllerState.hold, True
|
||||
state.active_frames, state.selected_lead, state.selected_lead_track_id = 20, 0, 100
|
||||
targets = []
|
||||
|
||||
for frame in range(controller.lead_switch_max_hold_frames + controller.lead_loss_hold_frames):
|
||||
state.cap_samples = [20.0] * CAP_FILTER_FRAMES
|
||||
lead_plan = LeadPlan(cap=25.0, selected_lead=0, selected_lead_track_id=200 if frame % 2 == 0 else 100,
|
||||
selected_lead_speed=20.0, closing_speed=0.0, lead_status=True)
|
||||
targets.append(controller._update_target(lead_plan, 25.0, 20.0, AccelProfile.normal, 0.48, False,
|
||||
LongitudinalPlanSource.cruise, 18.0, -0.2))
|
||||
|
||||
assert state.lead_switch_guard_frames == 0
|
||||
assert max(targets) == 18.0
|
||||
|
||||
def test_track_id_churn_without_false_relief_does_not_arm_guard(self):
|
||||
controller = make_controller()
|
||||
@@ -481,7 +604,7 @@ class TestTargetLifecycle:
|
||||
|
||||
assert controller.target_state.lead_switch_guard_frames == 0
|
||||
|
||||
def test_short_dropout_holds_then_releases_without_a_second_accel_cap(self):
|
||||
def test_short_dropout_holds_then_releases_at_a_bounded_target_rate(self):
|
||||
controller = make_controller()
|
||||
for _ in range(CAP_FILTER_FRAMES + 20):
|
||||
restricted = update(controller, restrictive_radar())
|
||||
@@ -489,8 +612,11 @@ class TestTargetLifecycle:
|
||||
held = [update(controller) for _ in range(controller.lead_loss_hold_frames - 1)]
|
||||
assert all(result.target_speed <= restricted.target_speed + 1e-9 for result in held)
|
||||
|
||||
released = update(controller)
|
||||
assert released.target_speed == 25.0
|
||||
released = [update(controller) for _ in range(40)]
|
||||
targets = np.asarray([restricted.target_speed, *(result.target_speed for result in released)])
|
||||
assert np.max(np.diff(targets)) <= TARGET_RELEASE_SLEW * DT_MDL + TARGET_SPEED_RESERVE + 1e-9
|
||||
assert released[-1].target_speed == 25.0
|
||||
assert released[-1].state == AccelControllerState.free
|
||||
|
||||
def test_previous_lead_source_synchronizes_down_to_planner(self):
|
||||
controller = make_controller()
|
||||
@@ -905,7 +1031,8 @@ class TestTargetLifecycle:
|
||||
assert target_state.target_speed is None and target_state.matched_accel_limit is None
|
||||
assert target_state.state == AccelControllerState.inactive
|
||||
assert target_state.departure_frames == target_state.active_frames == target_state.lead_loss_frames == target_state.stale_frames == 0
|
||||
assert target_state.lead_switch_guard_frames == 0
|
||||
assert target_state.lead_switch_guard_frames == target_state.lead_switch_elapsed_frames == target_state.lead_switch_stable_frames == 0
|
||||
assert target_state.release_settle_frames == 0 and target_state.release_settle_speed is None and not target_state.release_slew_armed
|
||||
assert target_state.selected_lead == target_state.selected_lead_track_id == -1
|
||||
assert target_state.cap_samples == [math.inf] * CAP_FILTER_FRAMES
|
||||
assert target_state.lead_speed_samples == [math.inf] * CAP_FILTER_FRAMES
|
||||
|
||||
@@ -14,8 +14,8 @@ from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PRI
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller import accel_controller as accel_controller_module
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelControllerState
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import (
|
||||
MATCHED_SPEED_DECEL_RATE, MPC_DECEL_JERK_COST_MULTIPLIER, MPC_DECEL_JERK_MAX_REQUIRED_DECEL,
|
||||
MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE, MPC_DECEL_TREND_FRAMES, TARGET_SPEED_RESERVE, STOP_HOLD_EXIT_FRAMES, AccelProfile,
|
||||
LEAD_LOSS_HOLD_TIME, MATCHED_SPEED_DECEL_RATE, MPC_DECEL_JERK_COST_MULTIPLIER, MPC_DECEL_JERK_MAX_REQUIRED_DECEL,
|
||||
MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE, MPC_DECEL_TREND_FRAMES, TARGET_RELEASE_SLEW, TARGET_SPEED_RESERVE, STOP_HOLD_EXIT_FRAMES, AccelProfile,
|
||||
)
|
||||
|
||||
ACTUATOR_DYNAMICS = (
|
||||
@@ -196,8 +196,8 @@ def _run(
|
||||
return trace
|
||||
|
||||
|
||||
def _first_time_below(trace: ClosedLoopTrace, threshold: float) -> float:
|
||||
indices = np.flatnonzero(trace.a_target <= threshold)
|
||||
def _first_time_below(trace: ClosedLoopTrace, threshold: float, after: float = 0.0) -> float:
|
||||
indices = np.flatnonzero((trace.time >= after) & (trace.a_target <= threshold))
|
||||
assert len(indices), f"never reached {threshold} m/s²"
|
||||
return float(trace.time[indices[0]])
|
||||
|
||||
@@ -430,10 +430,17 @@ def test_clear_road_launch_is_prompt_and_profiles_separate_above_launch_speed():
|
||||
for trace in traces:
|
||||
positive = np.flatnonzero(trace.a_target > 0.05)
|
||||
moving = np.flatnonzero(trace.speed > 0.01)
|
||||
target_steps = np.diff(trace.target_speed)
|
||||
release = int(np.argmax(target_steps))
|
||||
assert len(positive) and trace.time[positive[0]] <= 4 * DT_MDL
|
||||
assert len(moving) and trace.time[moving[0]] <= 1.0
|
||||
assert np.interp(1.0, trace.time, trace.speed) >= 0.33
|
||||
assert target_steps[release] > TARGET_RELEASE_SLEW * DT_MDL
|
||||
assert trace.time[release + 1] <= 0.5
|
||||
assert abs(_command_jerk(trace)[release]) < 5.0
|
||||
assert abs(np.diff(trace.acceleration)[release] / DT_MDL) < 1.0
|
||||
assert not np.any(trace.a_target < -0.05)
|
||||
assert not _has_propulsion_brake_cycle(trace.a_target)
|
||||
assert trace.solver_failures == 0
|
||||
|
||||
launch_window = traces[0].time <= 0.5
|
||||
@@ -449,6 +456,36 @@ def test_clear_road_launch_is_prompt_and_profiles_separate_above_launch_speed():
|
||||
assert ceiling_at_ten[0] < ceiling_at_ten[1] < ceiling_at_ten[2]
|
||||
|
||||
|
||||
@pytest.mark.parametrize(("actuator_delay", "actuator_lag"), ACTUATOR_DYNAMICS, ids=ACTUATOR_IDS)
|
||||
def test_high_speed_lead_seed_release_has_no_target_snap(actuator_delay, actuator_lag):
|
||||
dropout_time = 5.0
|
||||
|
||||
def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None:
|
||||
return None if lead_name == "leadTwo" or current_time >= dropout_time else truth
|
||||
|
||||
common = dict(
|
||||
duration=7.0, profile=AccelProfile.eco, lead_relevancy=True, speed=22.0,
|
||||
distance_lead=100.0, v_lead=20.0, v_cruise=30.0, lead_observation_fn=observe,
|
||||
actuator_delay=actuator_delay, actuator_lag=actuator_lag,
|
||||
)
|
||||
baseline = _run(controller_enabled=False, **common)
|
||||
trace = _run(controller_enabled=True, **common)
|
||||
response = (trace.time >= dropout_time - 0.5) & (trace.time <= dropout_time + 2.0)
|
||||
response_steps = (trace.time[1:] >= dropout_time) & (trace.time[1:] <= dropout_time + 2.0)
|
||||
steps = np.diff(trace.target_speed)
|
||||
release = np.flatnonzero(response_steps & (steps > 1e-6))
|
||||
|
||||
assert len(release) and trace.time[release[0] + 1] <= dropout_time + LEAD_LOSS_HOLD_TIME + DT_MDL + 1e-9
|
||||
assert np.max(steps[response_steps]) <= TARGET_RELEASE_SLEW * DT_MDL + TARGET_SPEED_RESERVE + 1e-9
|
||||
assert trace.target_speed[trace.time >= dropout_time + 2.0][0] == 30.0
|
||||
assert np.max(np.abs(_command_jerk(trace)[response_steps])) <= np.max(np.abs(_command_jerk(baseline)[response_steps])) + 1e-9
|
||||
release_response = response_steps.copy()
|
||||
release_response[:release[0]] = False
|
||||
assert np.max(np.abs(_command_jerk(trace)[release_response])) < 1.0
|
||||
assert not _has_propulsion_brake_cycle(trace.a_target[response])
|
||||
assert not trace.fcw.any() and trace.solver_failures == 0
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
("speed", "v_cruise"),
|
||||
((0.0, 22.352), (25.0, 30.0), (35.0, 35.0)),
|
||||
@@ -1029,7 +1066,8 @@ def test_false_range_relief_matches_clean_controller_response():
|
||||
_assert_no_new_solver_failures(trace, baseline)
|
||||
|
||||
|
||||
def test_route_52f_radar_vision_switch_does_not_release_restricted_pace():
|
||||
@pytest.mark.parametrize(("actuator_delay", "actuator_lag"), ACTUATOR_DYNAMICS, ids=ACTUATOR_IDS)
|
||||
def test_route_52f_radar_vision_switch_does_not_release_restricted_pace(actuator_delay, actuator_lag):
|
||||
glitch_start = 20.0
|
||||
glitch_end = 24.0
|
||||
|
||||
@@ -1053,7 +1091,7 @@ def test_route_52f_radar_vision_switch_does_not_release_restricted_pace():
|
||||
|
||||
common = dict(
|
||||
duration=28.0, controller_enabled=True, profile=AccelProfile.eco, lead_relevancy=True, speed=28.0,
|
||||
distance_lead=40.0, v_lead=lead_speed, v_cruise=34.72, actuator_delay=0.15, actuator_lag=0.25,
|
||||
distance_lead=40.0, v_lead=lead_speed, v_cruise=34.72, actuator_delay=actuator_delay, actuator_lag=actuator_lag,
|
||||
)
|
||||
baseline = _run(**common)
|
||||
trace = _run(lead_observation_fn=observe, **common)
|
||||
@@ -1068,6 +1106,7 @@ def test_route_52f_radar_vision_switch_does_not_release_restricted_pace():
|
||||
assert {LongitudinalPlanSource.cruise, LongitudinalPlanSource.lead0} <= glitch_sources
|
||||
assert np.max(trace.target_speed[glitch]) <= before + 0.05
|
||||
assert np.max(trace.target_speed[recovered]) > before + 0.1
|
||||
assert np.max(np.diff(trace.target_speed)[response[1:]]) <= TARGET_RELEASE_SLEW * DT_MDL + TARGET_SPEED_RESERVE + 1e-9
|
||||
assert not _has_propulsion_brake_cycle(trace.a_target[response])
|
||||
assert np.max(np.abs(_command_jerk(trace)[response[1:]])) < 3.0
|
||||
assert np.min(gap[response]) >= np.min(baseline_gap[response]) - DROPOUT_GAP_TOLERANCE
|
||||
@@ -1077,6 +1116,147 @@ def test_route_52f_radar_vision_switch_does_not_release_restricted_pace():
|
||||
_assert_no_new_solver_failures(trace, baseline)
|
||||
|
||||
|
||||
@pytest.mark.parametrize(("actuator_delay", "actuator_lag"), ACTUATOR_DYNAMICS, ids=ACTUATOR_IDS)
|
||||
def test_route_533_same_track_relief_has_no_target_snap(actuator_delay, actuator_lag):
|
||||
glitch_start = 20.0
|
||||
glitch_end = 24.0
|
||||
|
||||
def lead_speed(current_time: float) -> float:
|
||||
return 25.0 if current_time < glitch_end else min(33.0, 25.0 + 2.0 * (current_time - glitch_end))
|
||||
|
||||
def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None:
|
||||
if lead_name == "leadTwo":
|
||||
return None
|
||||
observed = truth | {"radar": True, "radarTrackId": 1119}
|
||||
if not glitch_start <= current_time < glitch_end:
|
||||
return observed
|
||||
phase = int((current_time - glitch_start) / 0.20)
|
||||
if phase % 2 == 0:
|
||||
return observed | {"vLeadK": truth["vLeadK"] - 0.3, "vRel": truth["vRel"] - 0.3}
|
||||
return observed | {"dRel": truth["dRel"] + (8.0, 12.0, 18.0)[phase % 3], "vLead": truth["vLead"] + 0.5,
|
||||
"vLeadK": truth["vLeadK"] + 0.5, "vRel": truth["vRel"] + 0.5}
|
||||
|
||||
common = dict(
|
||||
duration=28.0, controller_enabled=True, profile=AccelProfile.eco, lead_relevancy=True, speed=28.0,
|
||||
distance_lead=40.0, v_lead=lead_speed, v_cruise=34.72, actuator_delay=actuator_delay, actuator_lag=actuator_lag,
|
||||
)
|
||||
clean = _run(**common)
|
||||
trace = _run(lead_observation_fn=observe, **common)
|
||||
glitch = (trace.time >= glitch_start) & (trace.time < glitch_end)
|
||||
response = (trace.time >= glitch_start - 0.5) & (trace.time <= glitch_end + 1.0)
|
||||
clean_gap = clean.distance_lead - clean.distance
|
||||
gap = trace.distance_lead - trace.distance
|
||||
glitch_sources = {trace.source[index] for index in np.flatnonzero(glitch)}
|
||||
|
||||
assert {LongitudinalPlanSource.cruise, LongitudinalPlanSource.lead0} <= glitch_sources
|
||||
assert np.max(np.diff(trace.target_speed)[response[1:]]) <= TARGET_RELEASE_SLEW * DT_MDL + TARGET_SPEED_RESERVE + 1e-9
|
||||
assert not _has_propulsion_brake_cycle(trace.a_target[response])
|
||||
assert not _has_brake_coast_brake(trace.a_target[response])
|
||||
assert np.max(np.abs(_command_jerk(trace)[response[1:]])) < 3.0
|
||||
assert float(np.percentile(np.abs(_filtered_realized_jerk(trace)), 95)) <= float(np.percentile(np.abs(_filtered_realized_jerk(clean)), 95)) + 0.02
|
||||
assert np.min(gap[response]) >= np.min(clean_gap[response]) - DROPOUT_GAP_TOLERANCE
|
||||
assert trace.raw_radar_passthrough.all()
|
||||
assert np.all(trace.mpc_calls == 1)
|
||||
assert not trace.fcw.any()
|
||||
_assert_no_new_solver_failures(trace, clean)
|
||||
|
||||
|
||||
@pytest.mark.parametrize(("actuator_delay", "actuator_lag"), ACTUATOR_DYNAMICS, ids=ACTUATOR_IDS)
|
||||
def test_route_532_sustained_switch_churn_has_no_target_snap(actuator_delay, actuator_lag):
|
||||
glitch_start = 20.0
|
||||
glitch_end = 32.0
|
||||
|
||||
def lead_speed(current_time: float) -> float:
|
||||
return 25.0 if current_time < glitch_end else min(33.0, 25.0 + 2.0 * (current_time - glitch_end))
|
||||
|
||||
def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None:
|
||||
if lead_name == "leadTwo":
|
||||
return None
|
||||
if not glitch_start <= current_time < glitch_end:
|
||||
return truth | {"radar": True, "radarTrackId": 1119}
|
||||
phase = int((current_time - glitch_start) / 0.20)
|
||||
if phase % 2 == 0:
|
||||
return truth | {"vLeadK": truth["vLeadK"] - 0.3, "vRel": truth["vRel"] - 0.3,
|
||||
"radar": True, "radarTrackId": 1119 if phase % 4 == 0 else 1176}
|
||||
return truth | {"dRel": truth["dRel"] + (15.0, 30.0, 60.0)[phase % 3], "vLead": truth["vLead"] + 1.0,
|
||||
"vLeadK": truth["vLeadK"] + 1.0, "vRel": truth["vRel"] + 1.0, "radar": False, "radarTrackId": -1}
|
||||
|
||||
common = dict(
|
||||
duration=36.0, profile=AccelProfile.eco, lead_relevancy=True, speed=28.0,
|
||||
distance_lead=40.0, v_lead=lead_speed, v_cruise=34.72, actuator_delay=actuator_delay, actuator_lag=actuator_lag,
|
||||
)
|
||||
baseline = _run(controller_enabled=False, lead_observation_fn=observe, **common)
|
||||
trace = _run(controller_enabled=True, lead_observation_fn=observe, **common)
|
||||
before = trace.target_speed[np.flatnonzero(trace.time < glitch_start)[-1]]
|
||||
protected = (trace.time >= glitch_start) & (trace.time < glitch_start + 4.0)
|
||||
response = (trace.time >= glitch_start - 0.5) & (trace.time <= glitch_end + 1.0)
|
||||
gap = trace.distance_lead - trace.distance
|
||||
|
||||
assert np.max(trace.target_speed[protected]) <= before + 0.05
|
||||
assert np.max(np.diff(trace.target_speed)[response[1:]]) <= TARGET_RELEASE_SLEW * DT_MDL + TARGET_SPEED_RESERVE + 1e-9
|
||||
assert not _has_propulsion_brake_cycle(trace.a_target[response])
|
||||
assert not _has_brake_coast_brake(trace.a_target[response])
|
||||
assert np.max(np.abs(_command_jerk(trace)[response[1:]])) < 3.0
|
||||
assert np.min(gap[response]) > STOP_DISTANCE + 10.0
|
||||
assert trace.raw_radar_passthrough.all()
|
||||
assert np.all(trace.mpc_calls == 1)
|
||||
assert not trace.fcw.any()
|
||||
_assert_no_new_solver_failures(trace, baseline)
|
||||
|
||||
|
||||
@pytest.mark.parametrize(("actuator_delay", "actuator_lag"), ACTUATOR_DYNAMICS, ids=ACTUATOR_IDS)
|
||||
def test_sustained_switch_churn_timeout_preserves_braking_safety(monkeypatch, actuator_delay, actuator_lag):
|
||||
churn_start = 20.0
|
||||
braking_start = 26.0
|
||||
churn_end = 32.0
|
||||
|
||||
def lead_speed(current_time: float) -> float:
|
||||
progress = np.clip((current_time - braking_start) / 4.0, 0.0, 1.0)
|
||||
return float(25.0 - 3.0 * (3.0 * progress**2 - 2.0 * progress**3))
|
||||
|
||||
def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None:
|
||||
if lead_name == "leadTwo":
|
||||
return None
|
||||
observed = truth | {"aLeadK": 0.0}
|
||||
if not churn_start <= current_time < churn_end:
|
||||
return observed | {"radar": True, "radarTrackId": 1119}
|
||||
phase = int((current_time - churn_start) / 0.20)
|
||||
if phase % 2 == 0:
|
||||
return observed | {"vLeadK": truth["vLeadK"] - 0.3, "vRel": truth["vRel"] - 0.3,
|
||||
"radar": True, "radarTrackId": 1119 if phase % 4 == 0 else 1176}
|
||||
return observed | {"dRel": truth["dRel"] + (15.0, 30.0, 60.0)[phase % 3], "vLead": truth["vLead"] + 1.0,
|
||||
"vLeadK": truth["vLeadK"] + 1.0, "vRel": truth["vRel"] + 1.0, "radar": False, "radarTrackId": -1}
|
||||
|
||||
common = dict(
|
||||
duration=churn_end, controller_enabled=True, profile=AccelProfile.eco, lead_relevancy=True, speed=28.0,
|
||||
distance_lead=50.0, v_lead=lead_speed, v_cruise=34.72, lead_observation_fn=observe,
|
||||
actuator_delay=actuator_delay, actuator_lag=actuator_lag,
|
||||
)
|
||||
with monkeypatch.context() as patch:
|
||||
patch.setattr(accel_controller_module, "LEAD_SWITCH_MAX_HOLD_TIME", common["duration"])
|
||||
guarded = _run(**common)
|
||||
trace = _run(**common)
|
||||
after_timeout = (trace.time[1:] >= churn_start + accel_controller_module.LEAD_SWITCH_MAX_HOLD_TIME) & (trace.time[1:] < churn_end)
|
||||
planner_braking = trace.planner_seed_accel[1:] <= accel_controller_module.PLANNER_BRAKING_ACCEL_THRESHOLD
|
||||
gap = trace.distance_lead - trace.distance
|
||||
guarded_gap = guarded.distance_lead - guarded.distance
|
||||
lead_speeds = np.asarray([lead_speed(current_time) for current_time in trace.time])
|
||||
closing = trace.speed - lead_speeds
|
||||
guarded_closing = guarded.speed - lead_speeds
|
||||
ttc = np.min(gap[closing > 0.1] / closing[closing > 0.1])
|
||||
guarded_ttc = np.min(guarded_gap[guarded_closing > 0.1] / guarded_closing[guarded_closing > 0.1])
|
||||
|
||||
assert np.any(after_timeout & planner_braking)
|
||||
assert np.max(np.diff(trace.target_speed)[after_timeout & planner_braking]) <= 1e-9
|
||||
for threshold in (-0.5, -1.0):
|
||||
timeout = churn_start + accel_controller_module.LEAD_SWITCH_MAX_HOLD_TIME
|
||||
assert _first_time_below(trace, threshold, timeout) <= _first_time_below(guarded, threshold, timeout) + 1e-9
|
||||
assert np.min(gap) >= np.min(guarded_gap) - 0.02
|
||||
assert ttc >= guarded_ttc - 0.02
|
||||
assert not trace.fcw.any()
|
||||
assert trace.solver_failures == guarded.solver_failures == 0
|
||||
|
||||
|
||||
@pytest.mark.parametrize("profile", range(3), ids=("eco", "normal", "sport"))
|
||||
@pytest.mark.parametrize(("actuator_delay", "actuator_lag"), ACTUATOR_DYNAMICS, ids=ACTUATOR_IDS)
|
||||
def test_route_507_braking_lead_slot_switch_has_no_false_relief_cycle(profile, actuator_delay, actuator_lag):
|
||||
|
||||
Reference in New Issue
Block a user