From c99a999f1a2ffd5d7ba58b375d33146576022297 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Wed, 20 May 2026 10:56:59 -0500 Subject: [PATCH] Add lateral resume delay settings surfaces and tests --- .../controls/tests/test_starpilot_planner.py | 119 ++++++++++++++++++ .../ui/layouts/settings/starpilot/lateral.py | 5 + starpilot/common/safe_mode.py | 1 + .../tools/device_settings_layout.json | 19 +++ 4 files changed, 144 insertions(+) create mode 100644 selfdrive/controls/tests/test_starpilot_planner.py diff --git a/selfdrive/controls/tests/test_starpilot_planner.py b/selfdrive/controls/tests/test_starpilot_planner.py new file mode 100644 index 000000000..6df046b29 --- /dev/null +++ b/selfdrive/controls/tests/test_starpilot_planner.py @@ -0,0 +1,119 @@ +import math +from pathlib import Path +from types import SimpleNamespace + +from openpilot.common.realtime import DT_MDL +from openpilot.starpilot.controls.starpilot_planner import StarPilotPlanner +import openpilot.starpilot.controls.starpilot_planner as starpilot_planner_module + + +class DummyThemeManager: + def update_wheel_image(self, *args, **kwargs): + pass + + +class FakeSM(dict): + def __init__(self, frame: int, services: dict): + super().__init__(services) + self.frame = frame + + +def make_toggles(**overrides): + defaults = { + "compass": False, + "conditional_experimental_mode": False, + "minimum_lane_change_speed": 100.0, + "pause_lateral_below_speed": 10.0, + "pause_lateral_below_signal": True, + "pause_lateral_signal_delay": 0.0, + "set_speed_offset": 0, + "stop_distance": 6.0, + "weather_presets": False, + } + defaults.update(overrides) + return SimpleNamespace(**defaults) + + +def make_sm(planner, *, frame: int, v_ego: float, left_blinker: bool, right_blinker: bool = False): + return FakeSM(frame, { + "radarState": SimpleNamespace( + leadOne=SimpleNamespace(status=False, dRel=float("inf"), vLead=0.0, modelProb=0.0, radar=False), + ), + "selfdriveState": SimpleNamespace(enabled=True), + "carState": SimpleNamespace( + vCruise=50.0, + vEgo=v_ego, + standstill=False, + leftBlinker=left_blinker, + rightBlinker=right_blinker, + ), + "controlsState": SimpleNamespace(curvature=0.0), + "modelV2": SimpleNamespace(position=SimpleNamespace(x=[0.0, 30.0]), laneLines=[None] * 4, roadEdges=[None] * 2), + "starpilotCarState": SimpleNamespace(pauseLateral=False, alwaysOnLateralEnabled=False), + planner.gps_location_service: SimpleNamespace(latitude=1.0, longitude=1.0, bearingDeg=90.0), + }) + + +def make_planner(monkeypatch): + planner = StarPilotPlanner(Path("/tmp/nonexistent"), DummyThemeManager()) + monkeypatch.setattr(starpilot_planner_module, "calculate_road_curvature", lambda model, v_ego: (0.01, 0.0)) + monkeypatch.setattr(planner.starpilot_acceleration, "update", lambda *args, **kwargs: None) + monkeypatch.setattr(planner.starpilot_events, "update", lambda *args, **kwargs: None) + monkeypatch.setattr(planner.starpilot_following, "update", lambda *args, **kwargs: None) + monkeypatch.setattr(planner.starpilot_vcruise, "update", lambda *args, **kwargs: 0.0) + monkeypatch.setattr(planner.starpilot_weather, "update_weather", lambda *args, **kwargs: None) + monkeypatch.setattr(planner.starpilot_cem, "stop_sign_and_light", lambda *args, **kwargs: None) + monkeypatch.setattr(planner, "update_lead_status", lambda *args, **kwargs: False) + return planner + + +def test_lateral_resume_delay_zero_keeps_immediate_resume(monkeypatch): + planner = make_planner(monkeypatch) + + try: + toggles = make_toggles(pause_lateral_signal_delay=0.0) + + planner.update(0.0, False, make_sm(planner, frame=1, v_ego=4.0, left_blinker=True), toggles) + assert planner.lateral_check is False + + planner.update(0.0, False, make_sm(planner, frame=2, v_ego=4.0, left_blinker=False), toggles) + assert planner.lateral_check is True + assert planner.blinker_delay_active is False + finally: + planner.shutdown() + + +def test_lateral_resume_delay_holds_resume_after_low_speed_turn(monkeypatch): + planner = make_planner(monkeypatch) + + try: + toggles = make_toggles(pause_lateral_signal_delay=0.5) + + planner.update(0.0, False, make_sm(planner, frame=1, v_ego=4.0, left_blinker=True), toggles) + planner.update(0.0, False, make_sm(planner, frame=2, v_ego=4.0, left_blinker=False), toggles) + + assert planner.lateral_check is False + assert planner.blinker_delay_active is True + + resume_frame = 2 + math.ceil(toggles.pause_lateral_signal_delay / DT_MDL) + planner.update(0.0, False, make_sm(planner, frame=resume_frame, v_ego=4.0, left_blinker=False), toggles) + + assert planner.lateral_check is True + assert planner.blinker_delay_active is False + finally: + planner.shutdown() + + +def test_lateral_resume_delay_ignores_signal_cycles_that_never_slow_enough(monkeypatch): + planner = make_planner(monkeypatch) + + try: + toggles = make_toggles(pause_lateral_signal_delay=0.5) + + planner.update(0.0, False, make_sm(planner, frame=1, v_ego=8.0, left_blinker=True), toggles) + planner.update(0.0, False, make_sm(planner, frame=2, v_ego=8.0, left_blinker=False), toggles) + + assert planner.lateral_check is True + assert planner.blinker_delay_active is False + finally: + planner.shutdown() diff --git a/selfdrive/ui/layouts/settings/starpilot/lateral.py b/selfdrive/ui/layouts/settings/starpilot/lateral.py index 63330e391..a86d5ad98 100644 --- a/selfdrive/ui/layouts/settings/starpilot/lateral.py +++ b/selfdrive/ui/layouts/settings/starpilot/lateral.py @@ -174,6 +174,11 @@ class StarPilotLateralLayout(_SettingsPage): get_state=lambda: self._params.get_bool("PauseLateralOnSignal"), set_state=lambda s: self._params.put_bool("PauseLateralOnSignal", s), visible=lambda: self._params.get_bool("QOLLateral") and self._params.get_int("PauseLateralSpeed") > 0), + SettingRow("LateralResumeDelay", "value", tr_noop("Lateral Resume Delay"), + subtitle=tr_noop("Delay before steering resumes after the turn signal turns off. Set to 0 to disable."), + get_value=lambda: "Off" if self._params.get_float("LateralResumeDelay") == 0 else f"{self._params.get_float('LateralResumeDelay'):.1f}s", + on_click=lambda: self._show_slider("LateralResumeDelay", 0.0, 5.0, step=0.1, unit=" s", value_type="float"), + visible=lambda: self._params.get_bool("QOLLateral") and self._params.get_int("PauseLateralSpeed") > 0 and self._params.get_bool("PauseLateralOnSignal")), ], tab_key="steering"), # ── Lane tab ── diff --git a/starpilot/common/safe_mode.py b/starpilot/common/safe_mode.py index 88cc4ee36..86355cdbd 100644 --- a/starpilot/common/safe_mode.py +++ b/starpilot/common/safe_mode.py @@ -44,6 +44,7 @@ SAFE_MODE_MANAGED_KEYS = ( "QOLLateral", "PauseLateralSpeed", "PauseLateralOnSignal", + "LateralResumeDelay", "LongitudinalTune", "AdvancedLongitudinalTune", "EVTuning", diff --git a/starpilot/system/the_pond/assets/components/tools/device_settings_layout.json b/starpilot/system/the_pond/assets/components/tools/device_settings_layout.json index 799709cde..c9c126426 100644 --- a/starpilot/system/the_pond/assets/components/tools/device_settings_layout.json +++ b/starpilot/system/the_pond/assets/components/tools/device_settings_layout.json @@ -234,6 +234,25 @@ "max": 99.0, "step": 1.0, "parent_key": "QOLLateral" + }, + { + "key": "PauseLateralOnSignal", + "label": "Turn Signal Only", + "description": "Only pause steering when the turn signal is active.", + "data_type": "bool", + "ui_type": "toggle", + "parent_key": "QOLLateral" + }, + { + "key": "LateralResumeDelay", + "label": "Lateral Resume Delay", + "description": "Delay before steering resumes after the turn signal turns off. Only applies when vehicle speed dropped below half the pause speed during the signal. Set to 0 to disable.", + "data_type": "float", + "ui_type": "numeric", + "min": 0.0, + "max": 5.0, + "step": 0.1, + "parent_key": "PauseLateralOnSignal" } ] },