diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py index 8d3232aa01..a8cea1da05 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py @@ -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 diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py b/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py index 0a4a955d3f..235db9b69d 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py @@ -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 diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py b/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py index 1618fefb3d..d925cd4532 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py @@ -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): diff --git a/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py b/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py index 2338f5e849..9f4dcf5702 100644 --- a/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py +++ b/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py @@ -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