From 56d1efda6470bc178eda111888d3087963f7af4d Mon Sep 17 00:00:00 2001 From: rav4kumar <36933347+rav4kumar@users.noreply.github.com> Date: Mon, 27 Jul 2026 21:50:31 -0700 Subject: [PATCH] Prevent false stop departures without damping takeoff --- .../lib/accel_personality/accel_controller.py | 104 ++++++++++---- .../lib/accel_personality/constants.py | 1 + .../tests/test_accel_controller.py | 134 ++++++++++++++++-- .../tests/test_accel_controller_interfaces.py | 22 +++ .../test_longcontrol_vehicle_interfaces.py | 83 +++++++++++ .../controls/lib/longitudinal_planner.py | 4 +- .../test_accel_controller_closed_loop.py | 93 ++++++++++++ 7 files changed, 403 insertions(+), 38 deletions(-) create mode 100644 sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_longcontrol_vehicle_interfaces.py diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py index 25fb9cc328..fa3a75dcdf 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py @@ -20,7 +20,7 @@ from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants imp PACE_RESTRICT_DEADBAND, PROFILE_CONFIGS, RADAR_STALE_TIMEOUT, STOP_GAP_RESERVE, STOP_GAP_RESERVE_DECEL_BP, STOP_GAP_RESERVE_LEAD_SPEED, STOP_HOLD_CREEP_DISTANCE, STOP_HOLD_CREEP_SPEED, STOP_HOLD_EGO_SPEED, STOP_HOLD_EXIT_FRAMES, STOP_HOLD_EXIT_SPEED, - STOP_HOLD_MAX_LEAD_DISTANCE, STOPPED_LEAD_SPEED, VEGO_NOISE_TOLERANCE, AccelProfile, + STOP_HOLD_FAST_DEPARTURE_DISTANCE, STOP_HOLD_MAX_LEAD_DISTANCE, STOPPED_LEAD_SPEED, VEGO_NOISE_TOLERANCE, AccelProfile, ) @@ -43,6 +43,9 @@ class EnergyEnvelope: departure_lead_index: int = -1 departure_lead_speed: float = math.inf departure_cap: float = math.inf + departure_lead_speeds: tuple[float, float] = (math.inf, math.inf) + departure_lead_distances: tuple[float, float] = (-math.inf, -math.inf) + departure_lead_track_ids: tuple[int, int] = (-1, -1) departure_lead_separations: tuple[float, float] = (-math.inf, -math.inf) usable_gap: float = math.inf closing_speed: float = 0.0 @@ -85,7 +88,9 @@ class _ControllerPath: departure_samples: tuple[deque[float], deque[float]] = field( default_factory=lambda: (deque(maxlen=CAP_FILTER_FRAMES), deque(maxlen=CAP_FILTER_FRAMES)), ) + departure_motion_samples: deque[float] = field(default_factory=lambda: deque(maxlen=CAP_FILTER_FRAMES)) departure_references: list[float | None] = field(default_factory=lambda: [None, None]) + departure_track_ids: list[int] = field(default_factory=lambda: [-1, -1]) pace: float | None = None state: AccelControllerState = AccelControllerState.inactive departure_frames: int = 0 @@ -124,7 +129,9 @@ class _ControllerPath: self.lead_speed_samples = deque([math.inf] * CAP_FILTER_FRAMES, maxlen=CAP_FILTER_FRAMES) self.lead_accel_samples = deque([0.0] * CAP_FILTER_FRAMES, maxlen=CAP_FILTER_FRAMES) self.departure_samples = (deque(maxlen=CAP_FILTER_FRAMES), deque(maxlen=CAP_FILTER_FRAMES)) + self.departure_motion_samples = deque(maxlen=CAP_FILTER_FRAMES) self.departure_references = [None, None] + self.departure_track_ids = [-1, -1] self.pace = None self.state = AccelControllerState.inactive self.departure_frames = 0 @@ -242,6 +249,8 @@ class AccelController: candidates: list[EnergyEnvelope] = [] departure_candidates: list[tuple[float, int]] = [] departure_speeds = [math.inf, math.inf] + departure_distances = [-math.inf, -math.inf] + departure_track_ids = [-1, -1] departure_separations = [-math.inf, -math.inf] departure_caps = [math.inf, math.inf] @@ -280,6 +289,8 @@ class AccelController: )) departure_candidates.append((departure_distance, lead_index)) departure_speeds[lead_index] = v_lead_delay + departure_distances[lead_index] = d_rel + departure_track_ids[lead_index] = self._lead_track_id(lead) departure_separations[lead_index] = separation departure_caps[lead_index] = departure_cap @@ -294,7 +305,9 @@ class AccelController: selected_lead_speed=selected.selected_lead_speed, selected_lead_accel=selected.selected_lead_accel, departure_lead_index=departure_lead_index, departure_lead_speed=departure_lead_speed, - departure_cap=departure_caps[departure_lead_index], departure_lead_separations=tuple(departure_separations), + departure_cap=departure_caps[departure_lead_index], departure_lead_speeds=tuple(departure_speeds), + departure_lead_distances=tuple(departure_distances), departure_lead_track_ids=tuple(departure_track_ids), + departure_lead_separations=tuple(departure_separations), usable_gap=selected.usable_gap, closing_speed=selected.closing_speed, required_decel=selected.required_decel, has_nearly_stopped_lead=departure_lead_speed < STOPPED_LEAD_SPEED, lead_status=lead_status, ) @@ -307,37 +320,71 @@ class AccelController: def _lead_source(source) -> bool: return source in (LongitudinalPlanSource.lead0, LongitudinalPlanSource.lead1) - @staticmethod - def _update_samples(path: _ControllerPath, envelope: EnergyEnvelope) -> bool: + def _update_samples(self, path: _ControllerPath, envelope: EnergyEnvelope) -> bool: had_filtered_lead = math.isfinite(path.filtered_cap) has_lead = envelope.selected_lead >= 0 path.cap_samples.append(envelope.cap if has_lead else math.inf) path.lead_speed_samples.append(envelope.selected_lead_speed if has_lead else math.inf) path.lead_accel_samples.append(envelope.selected_lead_accel if has_lead else 0.0) path.lead_loss_frames = 0 if has_lead else path.lead_loss_frames + 1 - for lead_index, separation in enumerate(envelope.departure_lead_separations): - if math.isfinite(separation): - path.departure_samples[lead_index].append(separation) + for lead_index, distance in enumerate(envelope.departure_lead_distances): + if not math.isfinite(distance): + continue + samples = path.departure_samples[lead_index] + track_id = envelope.departure_lead_track_ids[lead_index] + identity_changed = bool(samples) and track_id != path.departure_track_ids[lead_index] and (track_id >= 0 or path.departure_track_ids[lead_index] >= 0) + max_distance_step = max(STOP_HOLD_CREEP_DISTANCE / 2.0, 3.0 * envelope.departure_lead_speeds[lead_index] * self.dt) + geometry_jump = bool(samples) and abs(distance - samples[-1]) > max_distance_step + if identity_changed or geometry_jump: + samples.clear() + path.departure_references[lead_index] = distance + samples.append(distance) + path.departure_track_ids[lead_index] = track_id + lead_index = envelope.departure_lead_index + if lead_index >= 0: + distance = envelope.departure_lead_distances[lead_index] + samples = path.departure_motion_samples + max_distance_step = max(STOP_HOLD_CREEP_DISTANCE / 2.0, 3.0 * envelope.departure_lead_speed * self.dt) + if samples and abs(distance - samples[-1]) > max_distance_step: + samples.clear() + samples.append(distance) return not had_filtered_lead and math.isfinite(path.filtered_cap) @staticmethod def _seed_departure_tracking(path: _ControllerPath, envelope: EnergyEnvelope) -> None: path.departure_samples = (deque(maxlen=CAP_FILTER_FRAMES), deque(maxlen=CAP_FILTER_FRAMES)) + path.departure_motion_samples = deque(maxlen=CAP_FILTER_FRAMES) path.departure_references = [None, None] - for lead_index, separation in enumerate(envelope.departure_lead_separations): - if math.isfinite(separation): - path.departure_samples[lead_index].append(separation) - path.departure_references[lead_index] = separation + path.departure_track_ids = list(envelope.departure_lead_track_ids) + for lead_index, distance in enumerate(envelope.departure_lead_distances): + if math.isfinite(distance): + path.departure_samples[lead_index].append(distance) + path.departure_references[lead_index] = distance + if envelope.departure_lead_index >= 0: + path.departure_motion_samples.append(envelope.departure_lead_distances[envelope.departure_lead_index]) path.departure_frames = 0 @staticmethod - def _creep_departure(path: _ControllerPath, envelope: EnergyEnvelope) -> bool: + def _departure_progress(path: _ControllerPath, envelope: EnergyEnvelope, minimum_distance: float, *, robust: bool = True) -> bool: lead_index = envelope.departure_lead_index if lead_index < 0 or envelope.departure_lead_speed <= STOP_HOLD_CREEP_SPEED: return False reference = path.departure_references[lead_index] - separation = path.robust_departure_separation(lead_index) - return reference is not None and separation - reference >= STOP_HOLD_CREEP_DISTANCE + samples = path.departure_samples[lead_index] + distance = path.robust_departure_separation(lead_index) if robust else samples[-1] if samples else -math.inf + return reference is not None and distance - reference >= minimum_distance + + @classmethod + def _creep_departure(cls, path: _ControllerPath, envelope: EnergyEnvelope) -> bool: + return cls._departure_progress(path, envelope, STOP_HOLD_CREEP_DISTANCE) + + @staticmethod + def _recent_departure_motion(path: _ControllerPath) -> bool: + samples = tuple(path.departure_motion_samples)[-STOP_HOLD_EXIT_FRAMES:] + if len(samples) < STOP_HOLD_EXIT_FRAMES: + return False + deltas = np.diff(samples) + return samples[-1] - samples[0] >= STOP_HOLD_FAST_DEPARTURE_DISTANCE and np.count_nonzero(deltas > 0.005) >= 2 def _enter_stop_hold(self, path: _ControllerPath, envelope: EnergyEnvelope) -> None: if path.state != AccelControllerState.stopHold: @@ -390,8 +437,8 @@ class AccelController: stop_evidence = (stopped_lead_hold or envelope.cap < 0.50 or filtered_cap < 0.50 or (previous_stop and not path.launching) or invalid_lead) confirmed_creep_departure = (path.launching and path.departure_launch and has_lead - and (envelope.departure_lead_speed > STOP_HOLD_CREEP_SPEED - or self._creep_departure(path, envelope))) + and (self._departure_progress(path, envelope, STOP_HOLD_FAST_DEPARTURE_DISTANCE) + or self._recent_departure_motion(path))) 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 @@ -423,13 +470,17 @@ class AccelController: separation = path.robust_departure_separation(lead_index) if math.isfinite(separation) and path.departure_references[lead_index] is None: path.departure_references[lead_index] = separation - raw_departure = ((has_lead and min(envelope.selected_lead_speed, envelope.departure_lead_speed) > STOP_HOLD_CREEP_SPEED - and envelope.departure_cap > STOP_HOLD_CREEP_SPEED) + fast_departure = (has_lead and min(envelope.selected_lead_speed, envelope.departure_lead_speed) > STOP_HOLD_EXIT_SPEED + and envelope.departure_cap > STOP_HOLD_EXIT_SPEED) + raw_departure = (fast_departure or (not envelope.lead_status and path.lead_loss_frames >= self.lead_loss_hold_frames)) departed = self._creep_departure(path, envelope) or raw_departure + if fast_departure and path.departure_frames == 0 and path.departure_motion_samples: + path.departure_motion_samples = deque([path.departure_motion_samples[-1]], maxlen=CAP_FILTER_FRAMES) path.departure_frames = path.departure_frames + 1 if departed else 0 path.pace = 0.0 - if path.departure_frames < STOP_HOLD_EXIT_FRAMES: + fast_departure_confirmed = fast_departure and self._recent_departure_motion(path) + if path.departure_frames < STOP_HOLD_EXIT_FRAMES or (fast_departure and not fast_departure_confirmed): return path.pace path.pace = base_speed path.state = AccelControllerState.release @@ -589,28 +640,29 @@ class AccelController: planner_speed = sanitized_v_ego if planner_speed is None else planner_speed valid_context = self._valid_context(base_speed, sanitized_v_ego, a_ego, planner_speed, planner_accel, stock_accel_max, self._delay(), engaged, cruise_initialized) - if valid_context and radar_fresh: + feature_context = valid_context and bool(enabled) + if feature_context and radar_fresh: envelope = self.calculate_energy_envelope(radar_state, sanitized_v_ego, a_ego, selected_profile, follow_personality) self._held_envelope = envelope - elif valid_context and self._held_envelope is not None: + elif feature_context and self._held_envelope is not None: envelope = self._held_envelope else: envelope = EnergyEnvelope(lead_status=self._radar_has_lead(radar_state)) - if not valid_context: + if not feature_context: self._held_envelope = None - shadow_fresh = self._update_freshness(self.shadow, radar_fresh) if valid_context else False - if valid_context and radar_fresh: + shadow_fresh = self._update_freshness(self.shadow, radar_fresh) if feature_context else False + if feature_context and radar_fresh: self._update_path(self.shadow, envelope, base_speed, sanitized_v_ego, selected_profile, profile_accel_max, previous_should_stop, previous_mpc_source, planner_speed, planner_accel) shadow_active = True - elif valid_context and not shadow_fresh and self.shadow.pace is not None: + elif feature_context and not shadow_fresh and self.shadow.pace is not None: shadow_active = True else: self.shadow.reset() shadow_active = False - live_context = valid_context and bool(enabled) and bool(acc_selected) + live_context = feature_context and bool(acc_selected) live_fresh = self._update_freshness(self.live, radar_fresh) if live_context else False if live_context and radar_fresh: pace_target = self._update_path(self.live, envelope, base_speed, sanitized_v_ego, selected_profile, profile_accel_max, diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py b/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py index 7314dc3254..ca985f0b14 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py @@ -53,6 +53,7 @@ STOP_HOLD_EXIT_SPEED = 0.80 STOP_HOLD_EXIT_FRAMES = 4 STOP_HOLD_CREEP_SPEED = 0.15 STOP_HOLD_CREEP_DISTANCE = 0.30 +STOP_HOLD_FAST_DEPARTURE_DISTANCE = 0.03 STOP_HOLD_MAX_LEAD_DISTANCE = 30.0 STOP_GAP_RESERVE = 0.75 STOP_GAP_RESERVE_LEAD_SPEED = 2.0 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 d970c01f47..b224177532 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 @@ -183,10 +183,13 @@ class TestMpcCeiling: assert np.all(ceiling >= 0.0) def test_inactive_controller_has_no_custom_ceiling(self): - result = update(make_controller(), enabled=False) + controller = make_controller() + result = update(controller, enabled=False) assert not result.active + assert not result.shadow_active assert result.mpc_accel_max is None assert math.isinf(result.effective_accel_max) + assert controller.live.pace is None and controller.shadow.pace is None def test_profile_ceiling_does_not_interfere_while_planner_is_braking(self): controller = make_controller() @@ -513,8 +516,8 @@ class TestPaceAndLifecycle: controller = make_controller() held = enter_stop_hold(controller) assert controller.live.pace == 0.0 - departing = make_radar(make_lead(status=True, d_rel=8.0, v_lead_k=2.0)) - results = [update(controller, departing, base_speed=8.0, v_ego=0.1) for _ in range(CAP_FILTER_FRAMES + STOP_HOLD_EXIT_FRAMES)] + results = [update(controller, make_radar(make_lead(status=True, d_rel=6.0 + (frame + 1) * 0.1, v_lead_k=2.0)), + base_speed=8.0, v_ego=0.1) for frame in range(CAP_FILTER_FRAMES + STOP_HOLD_EXIT_FRAMES)] launch_index = next(index for index, result in enumerate(results) if result.launching) assert held.state == AccelControllerState.stopHold @@ -539,19 +542,126 @@ class TestPaceAndLifecycle: assert held.state == AccelControllerState.stopHold assert held.target_speed == 0.0 and not held.launching - departing = make_radar(make_lead(status=True, d_rel=6.4, v_lead_k=2.0, radar_track_id=4887), - make_lead(status=True, d_rel=6.48, v_lead_k=2.0, radar_track_id=4905)) - results = [update(controller, departing, base_speed=8.0, v_ego=0.0) for _ in range(STOP_HOLD_EXIT_FRAMES)] + results = [ + update(controller, make_radar(make_lead(status=True, d_rel=6.04 + (frame + 1) * 0.1, v_lead_k=2.0, radar_track_id=4887), + make_lead(status=True, d_rel=6.12 + (frame + 1) * 0.1, v_lead_k=2.0, radar_track_id=4905)), + base_speed=8.0, v_ego=0.0) + for frame in range(STOP_HOLD_EXIT_FRAMES) + ] assert all(result.state == AccelControllerState.stopHold for result in results[:-1]) assert results[-1].launching and results[-1].departure_launching + def test_route_520_slow_lead_pulse_cannot_release_stop_hold_but_real_departure_can(self): + controller = make_controller() + enter_stop_hold(controller, v_ego=0.0) + speeds = (0.01, 0.03, 0.07, 0.10, 0.14, 0.20, 0.26, 0.32, 0.34, 0.33, 0.31, 0.28, 0.24, 0.20, 0.15, 0.09, 0.05, 0.01) + offsets = (0.00, 0.00, 0.00, 0.01, 0.01, 0.02, 0.03, 0.04, 0.06, 0.07, 0.09, 0.11, 0.12, 0.13, 0.14, 0.15, 0.15, 0.16) + + for offset, speed in zip(offsets, speeds, strict=True): + pulse = make_radar(make_lead(status=True, d_rel=6.0 + offset, v_lead_k=speed, radar_track_id=2133)) + held = update(controller, pulse, base_speed=8.0, v_ego=0.0) + assert held.state == AccelControllerState.stopHold + assert held.target_speed == 0.0 and not held.launching + + stopped = make_radar(make_lead(status=True, d_rel=6.2, v_lead_k=0.0, radar_track_id=2133)) + assert update(controller, stopped, base_speed=8.0, v_ego=0.0).state == AccelControllerState.stopHold + results = [update(controller, make_radar(make_lead(status=True, d_rel=6.2 + (frame + 1) * 0.1, v_lead_k=2.0, radar_track_id=2133)), + base_speed=8.0, v_ego=0.0) for frame in range(STOP_HOLD_EXIT_FRAMES)] + + assert all(result.state == AccelControllerState.stopHold for result in results[:-1]) + assert results[-1].launching and results[-1].departure_launching + + def test_fast_speed_signal_that_slows_without_separating_never_releases_stop_hold(self): + controller = make_controller() + enter_stop_hold(controller, v_ego=0.0) + departing = make_radar(make_lead(status=True, d_rel=5.9, v_lead_k=2.0)) + results = [update(controller, departing, base_speed=8.0, v_ego=0.0) for _ in range(STOP_HOLD_EXIT_FRAMES)] + slowed = update(controller, make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.2)), base_speed=8.0, v_ego=0.0) + + assert all(result.state == AccelControllerState.stopHold and not result.launching for result in results) + assert slowed.state == AccelControllerState.stopHold + assert slowed.target_speed == 0.0 and not slowed.launching + + def test_stop_hold_reseeds_departure_distance_when_radar_track_is_replaced(self): + controller = make_controller() + original = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0, radar_track_id=100)) + update(controller, original, base_speed=8.0, v_ego=0.0, previous_should_stop=True) + replacement = make_radar(make_lead(status=True, d_rel=6.4, v_lead_k=0.2, radar_track_id=200)) + results = [update(controller, replacement, base_speed=8.0, v_ego=0.0) for _ in range(CAP_FILTER_FRAMES + STOP_HOLD_EXIT_FRAMES)] + + assert all(result.state == AccelControllerState.stopHold for result in results) + assert all(result.target_speed == 0.0 and not result.launching for result in results) + + def test_stop_hold_rejects_persistent_same_track_distance_step(self): + controller = make_controller() + original = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0, radar_track_id=100)) + update(controller, original, base_speed=8.0, v_ego=0.0, previous_should_stop=True) + stepped = make_radar(make_lead(status=True, d_rel=6.4, v_lead_k=0.2, radar_track_id=100)) + results = [update(controller, stepped, base_speed=8.0, v_ego=0.0) for _ in range(CAP_FILTER_FRAMES + STOP_HOLD_EXIT_FRAMES)] + + assert all(result.state == AccelControllerState.stopHold for result in results) + assert all(result.target_speed == 0.0 and not result.launching for result in results) + + def test_stop_hold_reseeds_non_selected_departure_lead_when_its_track_is_replaced(self): + controller = make_controller() + original = make_radar(make_lead(status=True, d_rel=3.0, v_lead_k=0.2, radar_track_id=100), + make_lead(status=True, d_rel=6.0, v_lead_k=0.1, radar_track_id=200)) + update(controller, original, base_speed=8.0, v_ego=0.0, previous_should_stop=True) + replacement = make_radar(make_lead(status=True, d_rel=3.4, v_lead_k=0.2, radar_track_id=101), + make_lead(status=True, d_rel=6.0, v_lead_k=0.1, radar_track_id=200)) + envelope = controller.calculate_energy_envelope(replacement, 0.0, 0.0, AccelProfile.normal) + results = [update(controller, replacement, base_speed=8.0, v_ego=0.0) for _ in range(CAP_FILTER_FRAMES + STOP_HOLD_EXIT_FRAMES)] + + assert envelope.selected_lead == 1 and envelope.departure_lead_index == 0 + assert all(result.state == AccelControllerState.stopHold for result in results) + assert all(result.target_speed == 0.0 and not result.launching for result in results) + + def test_genuine_departure_survives_lead_slot_and_track_flicker(self): + controller = make_controller() + enter_stop_hold(controller, v_ego=0.0) + results = [] + for frame in range(STOP_HOLD_EXIT_FRAMES): + moving = make_lead(status=True, d_rel=6.0 + (frame + 1) * 0.1, v_lead_k=2.0, radar_track_id=100) + secondary = make_lead(status=True, d_rel=7.0, v_lead_k=2.0, radar_track_id=200) + results.append(update(controller, make_radar(moving, secondary) if frame % 2 == 0 else make_radar(secondary, moving), + base_speed=8.0, v_ego=0.0)) + + assert all(result.state == AccelControllerState.stopHold for result in results[:-1]) + assert results[-1].launching and results[-1].departure_launching + + def test_fast_speed_glitch_without_distance_progress_stays_in_stop_hold(self): + controller = make_controller() + stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0, radar_track_id=100)) + update(controller, stopped, base_speed=8.0, v_ego=0.0, previous_should_stop=True) + glitch = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.9, radar_track_id=100)) + results = [update(controller, glitch, base_speed=8.0, v_ego=0.0) for _ in range(STOP_HOLD_EXIT_FRAMES)] + results.append(update(controller, stopped, base_speed=8.0, v_ego=0.0)) + + assert all(result.state == AccelControllerState.stopHold for result in results) + assert all(result.target_speed == 0.0 and not result.launching for result in results) + + def test_moving_departure_does_not_reenter_stop_hold_when_speed_crosses_exit_threshold(self): + controller = make_controller() + stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0, radar_track_id=100)) + update(controller, stopped, base_speed=8.0, v_ego=0.0, previous_should_stop=True) + distance = 6.0 + results = [] + for speed in (0.81, 0.82, 0.83, 0.84, 0.79, 0.76, 0.74, 0.72): + distance += speed * DT_MDL + radar = make_radar(make_lead(status=True, d_rel=distance, v_lead_k=speed, radar_track_id=100)) + results.append(update(controller, radar, base_speed=8.0, v_ego=0.0)) + + launch_index = next(index for index, result in enumerate(results) if result.launching) + assert all(result.state != AccelControllerState.stopHold for result in results[launch_index:]) + assert all(result.target_speed > 0.0 and result.departure_launching for result in results[launch_index:]) + def test_reused_radar_does_not_pulse_stop_hold_or_departure_target(self): controller = make_controller() enter_stop_hold(controller) - departing = make_radar(make_lead(status=True, d_rel=8.0, v_lead_k=2.0)) for frame in range(STOP_HOLD_EXIT_FRAMES): + departing = make_radar(make_lead(status=True, d_rel=6.0 + (frame + 1) * 0.1, v_lead_k=2.0)) fresh = update(controller, departing, base_speed=8.0, v_ego=0.1) held = update(controller, departing, base_speed=8.0, v_ego=0.1, radar_fresh=False, previous_mpc_source=LongitudinalPlanSource.lead0, planner_speed=0.01) @@ -623,14 +733,15 @@ class TestPaceAndLifecycle: results.append(update(controller, creeping, base_speed=8.0, v_ego=0.0)) launch_index = next(index for index, result in enumerate(results) if result.launching) + assert launch_index * DT_MDL <= 2.0 assert all(result.state != AccelControllerState.stopHold for result in results[launch_index:]) assert all(result.target_speed > 0.0 for result in results[launch_index:]) def test_departure_dropout_holds_without_resurrecting_stop_hold(self): controller = make_controller() enter_stop_hold(controller) - departing = make_radar(make_lead(status=True, d_rel=8.0, v_lead_k=2.0)) - results = [update(controller, departing, base_speed=8.0, v_ego=0.1) for _ in range(CAP_FILTER_FRAMES + STOP_HOLD_EXIT_FRAMES)] + results = [update(controller, make_radar(make_lead(status=True, d_rel=6.0 + (frame + 1) * 0.1, v_lead_k=2.0)), + base_speed=8.0, v_ego=0.1) for frame in range(CAP_FILTER_FRAMES + STOP_HOLD_EXIT_FRAMES)] launched = next(result for result in results if result.launching) before_dropout = results[-1] dropout = [update(controller, base_speed=8.0, v_ego=0.1) for _ in range(controller.lead_loss_hold_frames + 1)] @@ -644,8 +755,8 @@ class TestPaceAndLifecycle: def test_invalid_departure_geometry_returns_to_stop_hold(self): controller = make_controller() enter_stop_hold(controller) - departing = make_radar(make_lead(status=True, d_rel=8.0, v_lead_k=2.0)) - for _ in range(CAP_FILTER_FRAMES + STOP_HOLD_EXIT_FRAMES): + for frame in range(CAP_FILTER_FRAMES + STOP_HOLD_EXIT_FRAMES): + departing = make_radar(make_lead(status=True, d_rel=6.0 + (frame + 1) * 0.1, v_lead_k=2.0)) launched = update(controller, departing, base_speed=8.0, v_ego=0.1) invalid = make_radar(make_lead(status=True, d_rel=math.nan, v_lead_k=2.0)) guarded = update(controller, invalid, base_speed=8.0, v_ego=0.1) @@ -702,6 +813,7 @@ class TestPaceAndLifecycle: assert path.pace is None and path.matched_accel_limit is None assert path.state == AccelControllerState.inactive assert path.departure_frames == path.active_frames == path.lead_loss_frames == path.stale_frames == 0 + assert not path.departure_motion_samples assert not path.launching and not path.departure_launch and not path.matched_lead assert not path.braking_limited and not path.braking_handoff and not path.pace_reserve_armed assert math.isinf(path.filtered_cap) and math.isinf(path.filtered_lead_speed) and path.filtered_lead_accel == 0.0 diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller_interfaces.py b/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller_interfaces.py index 0abeb7f13f..87d2f7dcc4 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller_interfaces.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller_interfaces.py @@ -75,6 +75,7 @@ def test_profile_enum_keeps_toyota_importable(): assert AccelPersonality.schema.enumerants == expected assert CarState.__module__ == "opendbc.car.toyota.carstate" + assert "Params()" not in inspect.getsource(CarState.update) def test_mpc_accepts_optional_acceleration_ceiling_without_changing_stock_bounds(): @@ -356,6 +357,7 @@ def test_controller_receives_previous_mpc_state_and_cached_radar_freshness(): planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP) planner.accel_personality = int(AccelProfile.normal) planner.accel_personality_enabled = True + planner.accel_personality_available = True planner._radar_fresh_this_cycle = True planner.a_desired = -0.4 planner.v_desired_filter = SimpleNamespace(x=9.5) @@ -377,6 +379,25 @@ def test_controller_receives_previous_mpc_state_and_cached_radar_freshness(): assert received["radar_fresh"] is True +def test_controller_is_disabled_when_openpilot_longitudinal_control_is_unavailable(): + planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP) + planner.accel_personality = int(AccelProfile.normal) + planner.accel_personality_enabled = True + planner.accel_personality_available = False + planner._radar_fresh_this_cycle = True + planner.accel_controller = SimpleNamespace( + update=lambda *_args, **kwargs: SimpleNamespace(target_speed=20.0, received_enabled=kwargs["enabled"]), + ) + planner.a_desired = 0.0 + planner.v_desired_filter = SimpleNamespace(x=10.0) + planner.mpc = SimpleNamespace(source=MpcLongitudinalPlanSource.cruise) + sm = {"radarState": radar_state(), "carState": SimpleNamespace(vEgo=10.0, aEgo=0.0), "selfdriveState": SimpleNamespace(personality=0)} + + planner.update_accel_controller(sm, 20.0, True, True, True, ACCEL_MAX, False) + + assert not planner.accel_controller_result.received_enabled + + def test_radar_freshness_is_computed_once_and_shared_with_dec_and_controller(): planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP) planner._radar_log_mono_time = None @@ -388,6 +409,7 @@ def test_radar_freshness_is_computed_once_and_shared_with_dec_and_controller(): planner.e2e_alerts_helper = SimpleNamespace(update=lambda *_args: None) planner.accel_personality = int(AccelProfile.normal) planner.accel_personality_enabled = True + planner.accel_personality_available = True planner.output_a_target = 0.0 planner.a_desired = 0.0 planner.v_desired_filter = SimpleNamespace(x=10.0) diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_longcontrol_vehicle_interfaces.py b/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_longcontrol_vehicle_interfaces.py new file mode 100644 index 0000000000..407a15dffe --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_longcontrol_vehicle_interfaces.py @@ -0,0 +1,83 @@ +import pytest + +from opendbc.car import DT_CTRL, gen_empty_fingerprint, structs +from opendbc.car.car_helpers import interfaces +from opendbc.car.ford.values import CAR as FORD +from opendbc.car.gm.values import CAR as GM +from opendbc.car.honda.values import CAR as HONDA +from opendbc.car.hyundai.values import CAR as HYUNDAI +from opendbc.car.toyota.values import CAR as TOYOTA +from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan +from openpilot.selfdrive.controls.lib.longcontrol import LongControl, LongCtrlState, long_control_state_trans + + +VEHICLES = [ + pytest.param(TOYOTA.TOYOTA_RAV4_TSS2, (True, False, 0.0, -2.0, 0.25, 0.25, 0.3), id="toyota-rav4-tss2"), + pytest.param(HONDA.HONDA_ACCORD, (True, False, 0.0, -2.0, 0.5, 0.5, 0.8), id="honda-accord"), + pytest.param(GM.CHEVROLET_BOLT_EUV, (True, False, 0.0, -2.0, 0.25, 0.25, 2.0), id="gm-bolt-euv"), + pytest.param(HYUNDAI.HYUNDAI_SONATA, (True, True, 1.0, -2.0, 0.1, 0.5, 0.8), id="hyundai-sonata"), + pytest.param(FORD.FORD_ESCAPE_MK4, (True, False, 0.0, -2.0, 0.5, 0.5, 0.8), id="ford-escape"), +] + + +def get_car_params(candidate): + fingerprint = gen_empty_fingerprint() + interface = interfaces[candidate] + CP = interface.get_params(candidate, fingerprint, [], True, False, False) + CP_SP = interface.get_params_sp(CP, candidate, fingerprint, [], True, False, False) + return CP, CP_SP + + +@pytest.mark.parametrize(("candidate", "expected"), VEHICLES) +def test_real_vehicle_longcontrol_stop_and_start(candidate, expected): + CP, CP_SP = get_car_params(candidate) + expected_long, expected_starting, *expected_tuning = expected + + assert CP.openpilotLongitudinalControl is expected_long + assert CP.startingState is expected_starting + assert (CP.startAccel, CP.stopAccel, CP.vEgoStarting, CP.vEgoStopping, CP.stoppingDecelRate) == pytest.approx(expected_tuning) + + stop_speeds = [CP.vEgoStopping - 0.01] * 2 + drive_speeds = [CP.vEgoStopping + 0.01] * 2 + _, should_stop = get_accel_from_plan(stop_speeds, [0.0, 0.0], [0.0, 1.0], vEgoStopping=CP.vEgoStopping) + _, should_drive = get_accel_from_plan(drive_speeds, [0.0, 0.0], [0.0, 1.0], vEgoStopping=CP.vEgoStopping) + assert should_stop + assert not should_drive + + departure_state = long_control_state_trans( + CP, + CP_SP, + True, + LongCtrlState.stopping, + CP.vEgoStarting - 0.01, + should_drive, + brake_pressed=False, + cruise_standstill=False, + ) + assert departure_state == (LongCtrlState.starting if CP.startingState else LongCtrlState.pid) + assert ( + long_control_state_trans( + CP, + CP_SP, + True, + departure_state, + CP.vEgoStarting + 0.01, + should_drive, + brake_pressed=False, + cruise_standstill=False, + ) + == LongCtrlState.pid + ) + + CS = structs.CarState() + CS.vEgo = 0.0 + CS.aEgo = 0.0 + control = LongControl(CP, CP_SP) + + stopping_accel = control.update(True, CS, 0.0, should_stop, (-3.0, 2.0)) + assert control.long_control_state == LongCtrlState.stopping + assert stopping_accel == pytest.approx(-CP.stoppingDecelRate * DT_CTRL) + + departure_accel = control.update(True, CS, 0.0, should_drive, (-3.0, 2.0)) + assert control.long_control_state == departure_state + assert departure_accel == pytest.approx(CP.startAccel) diff --git a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index e15caa8982..6f47c96f97 100644 --- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -46,6 +46,7 @@ class LongitudinalPlannerSP: self.e2e_alerts_helper = E2EAlertsHelper() self.accel_controller = AccelController(CP, dt=dt) self.accel_controller_result = None + self.accel_personality_available = bool(CP.openpilotLongitudinalControl) self._accel_jerk_smoothing_blocked = False self._accel_required_decel_samples = deque(maxlen=4) self._accel_required_decel_lead = -1 @@ -125,7 +126,8 @@ class LongitudinalPlannerSP: self.accel_controller_result = self.accel_controller.update( sm['radarState'], base_speed=base_speed, v_ego=sm['carState'].vEgo, a_ego=sm['carState'].aEgo, profile=self.accel_personality, follow_personality=sm['selfdriveState'].personality, - enabled=self.accel_personality_enabled, acc_selected=acc_selected, engaged=engaged, cruise_initialized=cruise_initialized, + enabled=self.accel_personality_enabled and self.accel_personality_available, + acc_selected=acc_selected, engaged=engaged, cruise_initialized=cruise_initialized, stock_accel_max=stock_accel_max, previous_should_stop=previous_should_stop, radar_fresh=getattr(self, '_radar_fresh_this_cycle', True), previous_mpc_source=getattr(getattr(self, 'mpc', None), 'source', None), 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 93a99491fc..6ada7ce130 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 @@ -563,6 +563,59 @@ def test_stop_hold_survives_short_full_field_dropout(): _assert_no_new_solver_failures(trace, baseline) +@pytest.mark.parametrize("replacement_track_id", (100, 200), ids=("same-track", "replacement")) +def test_stop_hold_rejects_persistent_same_slot_range_step(replacement_track_id): + step_time = 1.0 + + def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None: + if lead_name == "leadTwo": + return None + stepped = current_time >= step_time + speed = 0.2 if stepped else 0.0 + return truth | {"dRel": truth["dRel"] + 0.4 * stepped, "vLead": speed, "vLeadK": speed, "vRel": speed, + "aLeadK": 0.0, "radarTrackId": replacement_track_id if stepped else 100, "radar": True} + + trace = _run( + duration=2.5, controller_enabled=True, lead_relevancy=True, speed=0.0, distance_lead=6.0, + v_lead=0.0, v_cruise=8.0, lead_observation_fn=observe, actuator_delay=0.15, actuator_lag=0.20, + ) + + assert np.all(trace.state == int(AccelControllerState.stopHold)) + assert np.all(trace.target_speed == 0.0) + assert trace.should_stop.all() + assert np.max(trace.speed) < 1e-3 + assert not trace.fcw.any() + assert trace.solver_failures == 0 + + +def test_moving_departure_crossing_exit_speed_releases_once(): + departure_time = 1.0 + speeds = (0.81, 0.82, 0.83, 0.84, 0.79, 0.76, 0.74, 0.72) + + def lead_speed(current_time: float) -> float: + frame = round((current_time - departure_time) / DT_MDL) + return 0.0 if frame < 0 else speeds[min(frame, len(speeds) - 1)] + + def observe(_current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None: + return None if lead_name == "leadTwo" else truth | {"aLeadK": 0.0, "radarTrackId": 100, "radar": True} + + trace = _run( + duration=3.0, controller_enabled=True, lead_relevancy=True, speed=0.0, distance_lead=6.0, + v_lead=lead_speed, v_cruise=8.0, lead_observation_fn=observe, actuator_delay=0.15, actuator_lag=0.20, + ) + after_departure = trace.time >= departure_time + stop_hold = int(AccelControllerState.stopHold) + releases = np.flatnonzero((trace.state[:-1] == stop_hold) & (trace.state[1:] != stop_hold)) + 1 + + assert len(releases) == 1 + assert trace.launching[releases[0]] + assert not np.any(trace.state[releases[0]:] == stop_hold) + assert np.count_nonzero(np.diff(trace.should_stop[after_departure].astype(int))) == 1 + assert not _has_propulsion_brake_cycle(trace.a_target[after_departure]) + assert not trace.fcw.any() + assert trace.solver_failures == 0 + + def test_route_51d_duplicate_lead_speed_pulse_cannot_release_stop_hold(): pulse_start = 1.0 departure_time = 2.0 @@ -597,6 +650,45 @@ def test_route_51d_duplicate_lead_speed_pulse_cannot_release_stop_hold(): assert trace.solver_failures == 0 +def test_route_520_slow_lead_pulse_cannot_release_stop_hold_or_dampen_real_departure(): + pulse_start = 1.0 + departure_time = 2.5 + pulse_speeds = (0.01, 0.03, 0.07, 0.10, 0.14, 0.20, 0.26, 0.32, 0.34, 0.33, 0.31, 0.28, 0.24, 0.20, 0.15, 0.09, 0.05, 0.01) + pulse_offsets = (0.00, 0.00, 0.00, 0.01, 0.01, 0.02, 0.03, 0.04, 0.06, 0.07, 0.09, 0.11, 0.12, 0.13, 0.14, 0.15, 0.15, 0.16) + + def lead_speed(current_time: float) -> float: + return 0.0 if current_time < departure_time else 2.0 + + def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None: + if lead_name == "leadTwo": + return None + pulse_frame = round((current_time - pulse_start) / DT_MDL) + if 0 <= pulse_frame < len(pulse_speeds): + speed = pulse_speeds[pulse_frame] + return truth | {"dRel": 6.0 + pulse_offsets[pulse_frame], "vLead": speed, "vLeadK": speed, "vRel": speed, + "aLeadK": 0.0, "radarTrackId": 2133, "radar": True} + return truth | {"aLeadK": 0.0, "radarTrackId": 2133, "radar": True} + + common = dict( + duration=4.0, lead_relevancy=True, speed=0.0, distance_lead=6.0, v_lead=lead_speed, v_cruise=8.0, + lead_observation_fn=observe, actuator_model=PRIUS_TSS2_ROUTE_MODEL, + ) + baseline = _run(controller_enabled=False, **common) + trace = _run(controller_enabled=True, **common) + pulse = (trace.time >= pulse_start) & (trace.time < pulse_start + len(pulse_speeds) * DT_MDL) + release = np.flatnonzero((trace.time >= departure_time) & trace.launching) + + assert np.all(trace.state[pulse] == int(AccelControllerState.stopHold)) + assert np.all(trace.target_speed[pulse] == 0.0) + assert np.max(trace.speed[trace.time < departure_time]) < 0.01 + assert len(release) and trace.time[release[0]] <= departure_time + STOP_HOLD_EXIT_FRAMES * DT_MDL + 1e-9 + assert trace.a_target[release[0]] > 0.05 + assert np.allclose(trace.a_target[release[0]:], baseline.a_target[release[0]:], atol=1e-5, rtol=0.0) + assert not _has_propulsion_brake_cycle(trace.a_target[trace.time >= departure_time]) + assert not trace.fcw.any() + assert trace.solver_failures == 0 + + @pytest.mark.parametrize(("actuator_delay", "actuator_lag"), ACTUATOR_DYNAMICS, ids=ACTUATOR_IDS) def test_stopped_lead_requires_four_departure_frames_and_launches_within_one_second(actuator_delay, actuator_lag): departure_time = 1.0 @@ -723,6 +815,7 @@ def test_constant_creep_departure_does_not_pulse_between_launch_and_stop_hold(): ) launched = np.flatnonzero((trace.time >= departure_time) & trace.launching) assert len(launched) + assert trace.time[launched[0]] <= departure_time + 2.0 after_launch = slice(launched[0], None) assert not np.any(trace.state[after_launch] == int(AccelControllerState.stopHold))