diff --git a/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py b/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py index f19d7658ed..8ca4d77681 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py +++ b/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py @@ -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: diff --git a/sunnypilot/selfdrive/controls/lib/accel_controller/constants.py b/sunnypilot/selfdrive/controls/lib/accel_controller/constants.py index 689fabb410..2343cc23b0 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_controller/constants.py +++ b/sunnypilot/selfdrive/controls/lib/accel_controller/constants.py @@ -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 diff --git a/sunnypilot/selfdrive/controls/lib/accel_controller/state.py b/sunnypilot/selfdrive/controls/lib/accel_controller/state.py index 74ae624e78..3f58a7a15c 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_controller/state.py +++ b/sunnypilot/selfdrive/controls/lib/accel_controller/state.py @@ -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() diff --git a/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py b/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py index 43cb53a880..b564633218 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py +++ b/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py @@ -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 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 329bc27aa7..d55ef40b14 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 @@ -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):