mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-08-23 19:43:44 +08:00
Bound matched-lead pace through radar handoffs
This commit is contained in:
rav4kumar
@@ -16,7 +16,7 @@ from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import (
|
||||
ACCEL_LIMIT_HORIZON_JERK, ACCEL_LIMIT_TRANSITION_JERK, ACCEL_PROFILE_MAX_BP, ACCEL_PROFILE_MAX_V, BRAKING_ACCEL_LIMIT_THRESHOLD,
|
||||
CAP_FILTER_FRAMES, LAUNCH_END_SPEED, LAUNCH_TARGET_HEADROOM, LAUNCH_TARGET_SLEW, LEAD_LOSS_HOLD_TIME, LEAD_MATCH_ACCEL_GAIN,
|
||||
LEAD_MATCH_ACCEL_SLEW, LEAD_MATCH_GAP_GAIN, LEAD_MATCH_SPEED_HEADROOM, MAX_LEAD_ACCEL_TAU, MIN_LEAD_SPEED,
|
||||
LEAD_MATCH_ACCEL_SLEW, LEAD_MATCH_GAP_GAIN, LEAD_MATCH_SPEED_HEADROOM, MATCHED_PACE_DECEL_RATE, MAX_LEAD_ACCEL_TAU, MIN_LEAD_SPEED,
|
||||
PACE_RELIEF_DEADBAND, 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,
|
||||
@@ -38,6 +38,7 @@ class AccelControllerState(IntEnum):
|
||||
class EnergyEnvelope:
|
||||
cap: float = math.inf
|
||||
selected_lead: int = -1
|
||||
selected_lead_track_id: int = -1
|
||||
selected_lead_speed: float = math.inf
|
||||
selected_lead_accel: float = 0.0
|
||||
departure_lead_index: int = -1
|
||||
@@ -90,6 +91,9 @@ class _ControllerPath:
|
||||
departure_frames: int = 0
|
||||
active_frames: int = 0
|
||||
lead_loss_frames: int = 0
|
||||
lead_switch_guard_frames: int = 0
|
||||
selected_lead: int = -1
|
||||
selected_lead_track_id: int = -1
|
||||
stale_frames: int = 0
|
||||
launching: bool = False
|
||||
departure_launch: bool = False
|
||||
@@ -125,6 +129,9 @@ class _ControllerPath:
|
||||
self.departure_frames = 0
|
||||
self.active_frames = 0
|
||||
self.lead_loss_frames = 0
|
||||
self.lead_switch_guard_frames = 0
|
||||
self.selected_lead = -1
|
||||
self.selected_lead_track_id = -1
|
||||
self.stale_frames = 0
|
||||
self.launching = False
|
||||
self.departure_launch = False
|
||||
@@ -202,6 +209,13 @@ class AccelController:
|
||||
a_lead_tau = _LEAD_ACCEL_TAU
|
||||
return d_rel, max(v_lead, 0.0), float(np.clip(a_lead, -10.0, 5.0)), a_lead_tau
|
||||
|
||||
@staticmethod
|
||||
def _lead_track_id(lead) -> int:
|
||||
try:
|
||||
return max(int(lead.radarTrackId), -1)
|
||||
except (AttributeError, OverflowError, TypeError, ValueError):
|
||||
return -1
|
||||
|
||||
def calculate_energy_envelope(self, radar_state, v_ego: float, a_ego: float, profile: int | AccelProfile,
|
||||
follow_personality=log.LongitudinalPersonality.standard) -> EnergyEnvelope:
|
||||
delay = self._delay()
|
||||
@@ -258,7 +272,8 @@ class AccelController:
|
||||
continue
|
||||
|
||||
candidates.append(EnergyEnvelope(
|
||||
cap=cap, selected_lead=lead_index, selected_lead_speed=v_lead_delay, selected_lead_accel=a_lead,
|
||||
cap=cap, selected_lead=lead_index, selected_lead_track_id=self._lead_track_id(lead),
|
||||
selected_lead_speed=v_lead_delay, selected_lead_accel=a_lead,
|
||||
usable_gap=usable_gap, closing_speed=closing_speed, required_decel=required_decel, lead_status=lead_status,
|
||||
))
|
||||
departure_candidates.append((departure_distance, lead_index))
|
||||
@@ -273,7 +288,8 @@ class AccelController:
|
||||
departure_lead_index = min(departure_candidates, key=lambda candidate: candidate[0])[1]
|
||||
departure_lead_speed = departure_speeds[departure_lead_index]
|
||||
return EnergyEnvelope(
|
||||
cap=selected.cap, selected_lead=selected.selected_lead, selected_lead_speed=selected.selected_lead_speed,
|
||||
cap=selected.cap, selected_lead=selected.selected_lead, selected_lead_track_id=selected.selected_lead_track_id,
|
||||
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_separations=tuple(departure_separations),
|
||||
@@ -333,12 +349,29 @@ class AccelController:
|
||||
path.matched_accel_limit = None
|
||||
|
||||
def _update_path(self, path: _ControllerPath, envelope: EnergyEnvelope, base_speed: float, v_ego: float,
|
||||
profile: AccelProfile, positive_accel_max: float, stock_accel_max: float, previous_should_stop: bool,
|
||||
profile: AccelProfile, profile_accel_max: float, previous_should_stop: bool,
|
||||
previous_mpc_source, planner_speed: float, planner_accel: float) -> float:
|
||||
confirmed_lead = self._update_samples(path, envelope)
|
||||
path.active_frames += 1
|
||||
has_lead = envelope.selected_lead >= 0
|
||||
filtered_cap = path.filtered_cap
|
||||
slot_changed = has_lead and path.selected_lead >= 0 and envelope.selected_lead != path.selected_lead
|
||||
track_changed = (has_lead and path.selected_lead >= 0 and envelope.selected_lead == path.selected_lead
|
||||
and envelope.selected_lead_track_id != path.selected_lead_track_id
|
||||
and (path.selected_lead_track_id >= 0 or envelope.selected_lead_track_id >= 0))
|
||||
false_relief = (has_lead and math.isfinite(filtered_cap)
|
||||
and envelope.cap >= filtered_cap + PACE_RELIEF_DEADBAND)
|
||||
if (slot_changed or track_changed) and false_relief and path.lead_switch_guard_frames == 0 and planner_accel <= BRAKING_ACCEL_LIMIT_THRESHOLD:
|
||||
path.lead_switch_guard_frames = self.lead_loss_hold_frames
|
||||
elif path.lead_switch_guard_frames > 0:
|
||||
path.lead_switch_guard_frames -= 1
|
||||
if has_lead:
|
||||
path.selected_lead = envelope.selected_lead
|
||||
path.selected_lead_track_id = envelope.selected_lead_track_id
|
||||
elif path.lead_loss_frames >= self.lead_loss_hold_frames:
|
||||
path.lead_switch_guard_frames = 0
|
||||
path.selected_lead = -1
|
||||
path.selected_lead_track_id = -1
|
||||
departure_separation = (envelope.departure_lead_separations[envelope.departure_lead_index]
|
||||
if envelope.departure_lead_index >= 0 else math.inf)
|
||||
stopped_lead_hold = (has_lead and envelope.has_nearly_stopped_lead
|
||||
@@ -353,9 +386,9 @@ class AccelController:
|
||||
confirmed_creep_departure = path.launching and path.departure_launch and self._creep_departure(path, envelope)
|
||||
|
||||
if path.accel_limit is None:
|
||||
path.accel_limit = positive_accel_max
|
||||
path.accel_limit = profile_accel_max
|
||||
else:
|
||||
path.accel_limit = min(stock_accel_max, self._move(path.accel_limit, positive_accel_max, ACCEL_LIMIT_TRANSITION_JERK, self.dt))
|
||||
path.accel_limit = self._move(path.accel_limit, profile_accel_max, ACCEL_LIMIT_TRANSITION_JERK, self.dt)
|
||||
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
|
||||
@@ -454,14 +487,22 @@ class AccelController:
|
||||
if path.matched_accel_limit is None:
|
||||
path.matched_accel_limit = path.accel_limit
|
||||
path.matched_accel_limit = min(path.accel_limit, self._move(path.matched_accel_limit, desired_accel_limit, LEAD_MATCH_ACCEL_SLEW, self.dt))
|
||||
path.pace = min(base_speed, path.pace + path.accel_limit * self.dt)
|
||||
path.state = AccelControllerState.free if path.pace >= base_speed - PACE_RESTRICT_DEADBAND else AccelControllerState.release
|
||||
matched_ceiling = min(base_speed, filtered_cap)
|
||||
if matched_ceiling <= path.pace - PACE_RESTRICT_DEADBAND:
|
||||
path.pace = max(matched_ceiling, path.pace - MATCHED_PACE_DECEL_RATE * self.dt)
|
||||
path.state = AccelControllerState.restrict
|
||||
elif path.lead_switch_guard_frames == 0 and matched_ceiling >= path.pace + PACE_RELIEF_DEADBAND:
|
||||
path.pace = min(matched_ceiling, path.pace + path.accel_limit * self.dt)
|
||||
path.state = AccelControllerState.free if path.pace >= base_speed - PACE_RESTRICT_DEADBAND else AccelControllerState.release
|
||||
else:
|
||||
path.state = AccelControllerState.free if path.pace >= base_speed - PACE_RESTRICT_DEADBAND else AccelControllerState.hold
|
||||
return path.pace
|
||||
path.matched_accel_limit = None
|
||||
|
||||
ceiling = min(base_speed, filtered_cap)
|
||||
if confirmed_lead and not path.launching:
|
||||
path.pace = min(path.pace, planner_speed)
|
||||
if (confirmed_lead and path.active_frames == CAP_FILTER_FRAMES // 2 + 1 and not path.launching
|
||||
and planner_speed < path.pace):
|
||||
path.pace = max(planner_speed, path.pace - comfort_decel * self.dt)
|
||||
|
||||
if self._lead_source(previous_mpc_source) and not has_lead and planner_speed < path.pace:
|
||||
path.pace = min(path.pace, planner_speed)
|
||||
@@ -482,7 +523,8 @@ class AccelController:
|
||||
confirmed_clear_road = not math.isfinite(filtered_cap) and not guarded_lead_loss
|
||||
relief = not has_lead or envelope.closing_speed <= 0.0
|
||||
if relief and (ceiling >= path.pace + PACE_RELIEF_DEADBAND or (confirmed_clear_road and ceiling > path.pace)):
|
||||
path.pace = min(ceiling, path.pace + path.accel_limit * self.dt)
|
||||
if path.lead_switch_guard_frames == 0:
|
||||
path.pace = min(ceiling, path.pace + path.accel_limit * self.dt)
|
||||
path.state = AccelControllerState.free if path.pace >= base_speed - PACE_RESTRICT_DEADBAND else AccelControllerState.release
|
||||
else:
|
||||
path.state = AccelControllerState.free if path.pace >= base_speed - PACE_RESTRICT_DEADBAND else AccelControllerState.hold
|
||||
@@ -547,7 +589,7 @@ class AccelController:
|
||||
|
||||
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, positive_accel_max, stock_accel_max, previous_should_stop,
|
||||
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 valid_context and not shadow_fresh and self.shadow.pace is not None:
|
||||
@@ -559,7 +601,7 @@ class AccelController:
|
||||
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, positive_accel_max, stock_accel_max,
|
||||
pace_target = self._update_path(self.live, envelope, base_speed, sanitized_v_ego, selected_profile, profile_accel_max,
|
||||
previous_should_stop,
|
||||
previous_mpc_source, planner_speed, planner_accel)
|
||||
live_active = True
|
||||
|
||||
@@ -39,6 +39,7 @@ LEAD_MATCH_GAP_GAIN = 0.04
|
||||
LEAD_MATCH_SPEED_HEADROOM = 2.50
|
||||
LEAD_MATCH_ACCEL_GAIN = 0.20
|
||||
LEAD_MATCH_ACCEL_SLEW = 0.25
|
||||
MATCHED_PACE_DECEL_RATE = 0.50
|
||||
BRAKING_ACCEL_LIMIT_THRESHOLD = -0.11
|
||||
|
||||
STOP_HOLD_EGO_SPEED = 0.30
|
||||
|
||||
@@ -13,13 +13,14 @@ from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import (
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality import AccelController, AccelControllerState, AccelProfile
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import (
|
||||
ACCEL_LIMIT_HORIZON_JERK, ACCEL_LIMIT_TRANSITION_JERK, ACCEL_PROFILE_MAX_BP, ACCEL_PROFILE_MAX_V, CAP_FILTER_FRAMES,
|
||||
LAUNCH_END_SPEED, LAUNCH_TARGET_HEADROOM, LAUNCH_TARGET_SLEW, LEAD_MATCH_ACCEL_SLEW, PROFILE_CONFIGS, RADAR_STALE_TIMEOUT,
|
||||
STOP_GAP_RESERVE, STOP_HOLD_ACCEL_MAX, STOP_HOLD_DEPARTURE_ACCEL_MAX, STOP_HOLD_EXIT_FRAMES,
|
||||
LAUNCH_END_SPEED, LAUNCH_TARGET_HEADROOM, LAUNCH_TARGET_SLEW, LEAD_MATCH_ACCEL_SLEW, MATCHED_PACE_DECEL_RATE, PROFILE_CONFIGS,
|
||||
RADAR_STALE_TIMEOUT, STOP_GAP_RESERVE, STOP_HOLD_ACCEL_MAX, STOP_HOLD_DEPARTURE_ACCEL_MAX, STOP_HOLD_EXIT_FRAMES,
|
||||
)
|
||||
|
||||
|
||||
def make_lead(*, status=False, d_rel=0.0, v_lead_k=0.0, a_lead_k=0.0, a_lead_tau=1.5):
|
||||
return SimpleNamespace(status=status, dRel=d_rel, vLeadK=v_lead_k, aLeadK=a_lead_k, aLeadTau=a_lead_tau)
|
||||
def make_lead(*, status=False, d_rel=0.0, v_lead_k=0.0, a_lead_k=0.0, a_lead_tau=1.5, radar_track_id=-1):
|
||||
return SimpleNamespace(status=status, dRel=d_rel, vLeadK=v_lead_k, aLeadK=a_lead_k, aLeadTau=a_lead_tau,
|
||||
radarTrackId=radar_track_id)
|
||||
|
||||
|
||||
def make_radar(lead_one=None, lead_two=None):
|
||||
@@ -131,6 +132,19 @@ class TestProfiles:
|
||||
assert reduced.mpc_accel_max is not None
|
||||
assert max(reduced.mpc_accel_max) <= 0.30 + 1e-9
|
||||
|
||||
def test_one_frame_stock_zero_does_not_poison_profile_recovery(self):
|
||||
clean_controller, glitch_controller = make_controller(), make_controller()
|
||||
for _ in range(clean_controller.lead_loss_hold_frames + 10):
|
||||
clean = update(clean_controller, v_ego=10.0, stock_accel_max=1.5)
|
||||
recovered = update(glitch_controller, v_ego=10.0, stock_accel_max=1.5)
|
||||
|
||||
limited = update(glitch_controller, v_ego=10.0, stock_accel_max=0.0)
|
||||
clean = update(clean_controller, v_ego=10.0, stock_accel_max=1.5)
|
||||
recovered = update(glitch_controller, v_ego=10.0, stock_accel_max=1.5)
|
||||
|
||||
assert limited.effective_accel_max == 0.0
|
||||
assert recovered.effective_accel_max == pytest.approx(clean.effective_accel_max)
|
||||
|
||||
@pytest.mark.parametrize("radar_fresh", (True, False), ids=("dropout", "stale"))
|
||||
def test_matched_lead_ceiling_obeys_current_stock_limit(self, radar_fresh):
|
||||
controller = make_controller()
|
||||
@@ -204,7 +218,9 @@ class TestMpcCeiling:
|
||||
assert controller.live.matched_lead
|
||||
assert braking.mpc_accel_max is not None and accelerating.mpc_accel_max is not None
|
||||
assert abs(accelerating.effective_accel_max - braking.effective_accel_max) <= LEAD_MATCH_ACCEL_SLEW * DT_MDL + 1e-9
|
||||
assert accelerating.target_speed >= braking.target_speed
|
||||
target_drop = braking.target_speed - accelerating.target_speed
|
||||
assert 0.0 <= target_drop <= MATCHED_PACE_DECEL_RATE * DT_MDL + 1e-9
|
||||
assert accelerating.state == AccelControllerState.restrict
|
||||
|
||||
def test_matched_lead_ignores_two_frame_speed_jump(self):
|
||||
clean_controller, noisy_controller = make_controller(), make_controller()
|
||||
@@ -274,7 +290,9 @@ class TestEnergyEnvelope:
|
||||
radar = make_radar(make_lead(status=True, d_rel=70.0, v_lead_k=12.0), make_lead(status=True, d_rel=25.0, v_lead_k=8.0))
|
||||
assert make_controller().calculate_energy_envelope(radar, 10.0, 0.0, AccelProfile.normal).selected_lead == 1
|
||||
|
||||
@pytest.mark.parametrize("field,value", [("aLeadK", math.nan), ("aLeadK", math.inf), ("aLeadTau", math.nan), ("aLeadTau", -1.0)])
|
||||
@pytest.mark.parametrize("field,value", [
|
||||
("aLeadK", math.nan), ("aLeadK", math.inf), ("aLeadTau", math.nan), ("aLeadTau", -1.0), ("radarTrackId", math.nan),
|
||||
])
|
||||
def test_nonessential_invalid_lead_fields_are_sanitized(self, field, value):
|
||||
lead = make_lead(status=True, d_rel=30.0, v_lead_k=8.0)
|
||||
setattr(lead, field, value)
|
||||
@@ -315,6 +333,66 @@ class TestPaceAndLifecycle:
|
||||
assert results[-1].state == AccelControllerState.restrict
|
||||
assert results[-1].target_speed < results[0].target_speed
|
||||
|
||||
@pytest.mark.parametrize("clear_frames", (1, 2, CAP_FILTER_FRAMES + 1))
|
||||
def test_lead_acquired_after_clear_road_cannot_step_pace_to_planner(self, clear_frames):
|
||||
controller = make_controller()
|
||||
for _ in range(clear_frames):
|
||||
update(controller, base_speed=25.0, v_ego=20.0, planner_speed=25.0)
|
||||
|
||||
results = [update(controller, restrictive_radar(), base_speed=25.0, v_ego=20.0, planner_speed=20.0)
|
||||
for _ in range(CAP_FILTER_FRAMES)]
|
||||
targets = np.asarray([25.0, *(result.target_speed for result in results)])
|
||||
max_step = PROFILE_CONFIGS[AccelProfile.normal].comfort_decel * DT_MDL
|
||||
|
||||
assert np.max(-np.diff(targets)) <= max_step + 1e-9
|
||||
|
||||
def test_lead_slot_is_forgotten_before_reacquisition(self):
|
||||
controller = make_controller()
|
||||
lead_one = restrictive_radar()
|
||||
for _ in range(CAP_FILTER_FRAMES + 10):
|
||||
update(controller, lead_one, base_speed=25.0, v_ego=20.0, planner_speed=20.0, planner_accel=-0.2)
|
||||
|
||||
for _ in range(controller.lead_loss_hold_frames):
|
||||
before = update(controller, base_speed=25.0, v_ego=20.0, planner_speed=20.0, planner_accel=-0.2)
|
||||
assert controller.live.selected_lead == -1
|
||||
|
||||
lead_two = make_radar(lead_two=make_lead(status=True, d_rel=20.0, v_lead_k=8.0, a_lead_k=-0.5))
|
||||
results = [update(controller, lead_two, base_speed=25.0, v_ego=20.0, planner_speed=5.0, planner_accel=-0.2)
|
||||
for _ in range(CAP_FILTER_FRAMES)]
|
||||
targets = np.asarray([before.target_speed, *(result.target_speed for result in results)])
|
||||
max_step = PROFILE_CONFIGS[AccelProfile.normal].comfort_decel * DT_MDL
|
||||
|
||||
assert np.max(-np.diff(targets)) <= max_step + 1e-9
|
||||
|
||||
@pytest.mark.parametrize("replacement_track_id", (200, -1), ids=("radar-track", "vision-track"))
|
||||
def test_false_relief_track_replacement_freezes_bounded_pace_release(self, replacement_track_id):
|
||||
controller = make_controller()
|
||||
original = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0, radar_track_id=100))
|
||||
for _ in range(CAP_FILTER_FRAMES + 10):
|
||||
update(controller, original, base_speed=25.0, v_ego=10.0, planner_speed=10.0, planner_accel=-0.2)
|
||||
for _ in range(20):
|
||||
before = update(controller, original, base_speed=25.0, v_ego=8.0, planner_speed=8.0, planner_accel=-0.2)
|
||||
assert controller.live.matched_lead
|
||||
|
||||
replacement = make_radar(make_lead(status=True, d_rel=40.0, v_lead_k=12.0, radar_track_id=replacement_track_id))
|
||||
switched = update(controller, replacement, base_speed=25.0, v_ego=8.0, planner_speed=5.0, planner_accel=-0.2)
|
||||
|
||||
target_drop = before.target_speed - switched.target_speed
|
||||
assert 0.0 <= target_drop <= MATCHED_PACE_DECEL_RATE * DT_MDL + 1e-9
|
||||
assert switched.target_speed < switched.base_speed
|
||||
assert controller.live.lead_switch_guard_frames == controller.lead_loss_hold_frames
|
||||
|
||||
def test_track_id_churn_without_false_relief_does_not_arm_guard(self):
|
||||
controller = make_controller()
|
||||
original = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0, radar_track_id=100))
|
||||
for _ in range(CAP_FILTER_FRAMES + 10):
|
||||
update(controller, original, base_speed=25.0, v_ego=10.0, planner_speed=10.0, planner_accel=-0.2)
|
||||
|
||||
replacement = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0, radar_track_id=200))
|
||||
update(controller, replacement, base_speed=25.0, v_ego=10.0, planner_speed=10.0, planner_accel=-0.2)
|
||||
|
||||
assert controller.live.lead_switch_guard_frames == 0
|
||||
|
||||
def test_short_dropout_holds_then_releases_at_profile_rate(self):
|
||||
controller = make_controller()
|
||||
for _ in range(CAP_FILTER_FRAMES + 20):
|
||||
|
||||
@@ -11,7 +11,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import STOP_
|
||||
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.accel_personality import AccelControllerState, AccelProfile
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import LEAD_MATCH_ACCEL_SLEW
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import LEAD_MATCH_ACCEL_SLEW, MATCHED_PACE_DECEL_RATE
|
||||
|
||||
ACTUATOR_DYNAMICS = (
|
||||
(0.10, 0.20),
|
||||
@@ -493,21 +493,22 @@ def test_short_false_departure_does_not_launch_the_vehicle(departure_frames):
|
||||
assert trace.solver_failures == 0
|
||||
|
||||
|
||||
def test_matched_lead_recovery_preserves_profile_ordering():
|
||||
@pytest.mark.parametrize(("actuator_delay", "actuator_lag"), ACTUATOR_DYNAMICS, ids=ACTUATOR_IDS)
|
||||
def test_matched_lead_recovery_preserves_profile_ordering(actuator_delay, actuator_lag):
|
||||
traces = [
|
||||
_run(
|
||||
duration=32.0, controller_enabled=True, profile=profile, lead_relevancy=True, speed=20.0,
|
||||
distance_lead=100.0, v_lead=10.0, v_cruise=30.0, actuator_delay=0.15, actuator_lag=0.25,
|
||||
distance_lead=100.0, v_lead=10.0, v_cruise=30.0, actuator_delay=actuator_delay, actuator_lag=actuator_lag,
|
||||
)
|
||||
for profile in range(3)
|
||||
]
|
||||
response = (traces[0].time >= 23.5) & (traces[0].time <= 29.0)
|
||||
response = (traces[0].time >= 15.0) & (traces[0].time <= 28.5)
|
||||
mean_accel = [float(np.mean(trace.a_target[response])) for trace in traces]
|
||||
final_speed = [float(trace.speed[np.flatnonzero(response)[-1]]) for trace in traces]
|
||||
|
||||
assert mean_accel[0] + 0.06 < mean_accel[1]
|
||||
assert mean_accel[1] + 0.025 < mean_accel[2]
|
||||
assert max(final_speed) - min(final_speed) < 0.10
|
||||
assert max(final_speed) - min(final_speed) < 0.15
|
||||
assert all(not _has_propulsion_brake_cycle(trace.a_target[response]) for trace in traces)
|
||||
assert all(trace.solver_failures == 0 for trace in traces)
|
||||
|
||||
@@ -635,6 +636,55 @@ def test_false_range_relief_matches_clean_controller_response():
|
||||
_assert_no_new_solver_failures(trace, baseline)
|
||||
|
||||
|
||||
@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):
|
||||
glitch_start = 67.0
|
||||
glitch_end = 67.5
|
||||
|
||||
def lead_speed(current_time: float) -> float:
|
||||
braking_time = np.clip(current_time - 60.0, 0.0, 7.0)
|
||||
return 10.0 - 0.42 * braking_time
|
||||
|
||||
def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None:
|
||||
if glitch_start <= current_time < glitch_end:
|
||||
if lead_name == "leadOne":
|
||||
return None
|
||||
return truth | {
|
||||
"dRel": truth["dRel"] + 20.0,
|
||||
"vLead": truth["vLead"] + 4.0,
|
||||
"vLeadK": truth["vLeadK"] + 4.0,
|
||||
"vRel": truth["vRel"] + 4.0,
|
||||
"aLeadK": 0.0,
|
||||
"radar": True,
|
||||
"radarTrackId": 200,
|
||||
}
|
||||
if lead_name == "leadTwo":
|
||||
return None
|
||||
return truth | {"aLeadK": -0.42 if 60.0 <= current_time < glitch_start else 0.0, "radar": True, "radarTrackId": 100}
|
||||
|
||||
common = dict(
|
||||
duration=73.0, controller_enabled=True, profile=profile, lead_relevancy=True, speed=20.0,
|
||||
distance_lead=100.0, v_lead=lead_speed, v_cruise=30.0, actuator_delay=actuator_delay, actuator_lag=actuator_lag,
|
||||
)
|
||||
clean = _run(**common)
|
||||
trace = _run(lead_observation_fn=observe, **common)
|
||||
response = (trace.time >= 66.0) & (trace.time <= 72.0)
|
||||
jerk_response = (trace.time[1:] >= 66.0) & (trace.time[1:] <= 72.0)
|
||||
clean_gap = clean.distance_lead - clean.distance
|
||||
gap = trace.distance_lead - trace.distance
|
||||
|
||||
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(np.diff(trace.a_target)[jerk_response] / DT_MDL)) < 3.0
|
||||
assert np.max(-np.diff(trace.target_speed)[jerk_response]) <= MATCHED_PACE_DECEL_RATE * DT_MDL + 1e-9
|
||||
assert np.min(gap[response]) >= np.min(clean_gap[response]) - DROPOUT_GAP_TOLERANCE
|
||||
assert not trace.fcw.any()
|
||||
assert trace.solver_failures == 0
|
||||
assert trace.raw_radar_passthrough.all()
|
||||
assert np.all(trace.mpc_calls == 1)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("profile", range(3), ids=("eco", "normal", "sport"))
|
||||
def test_profile_ceiling_and_pace_stay_smooth_through_slot_switch_noise(profile):
|
||||
glitch_start = 24.0
|
||||
|
||||
Reference in New Issue
Block a user