Add lateral resume delay settings surfaces and tests

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()

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 ──

starpilot/common/safe_mode.py

@@ -44,6 +44,7 @@ SAFE_MODE_MANAGED_KEYS = (
   "QOLLateral",
   "PauseLateralSpeed",
   "PauseLateralOnSignal",
+  "LateralResumeDelay",
   "LongitudinalTune",
   "AdvancedLongitudinalTune",
   "EVTuning",

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"
       }
     ]
   },