Keep UI scheduling below planning and stabilize speed limit pulses

selfdrive/ui/lib/speed_limit_pulse.py

@@ -0,0 +1,32 @@
+import math
+
+
+class SpeedLimitPulse:
+  def __init__(self):
+    self.last_limit = 0.0
+    self.start_time = -math.inf
+    self._started_frame: int | None = None
+
+  def clear(self) -> None:
+    self.start_time = -math.inf
+
+  def reset(self) -> None:
+    self.last_limit = 0.0
+    self._started_frame = None
+    self.clear()
+
+  def update(self, source: str, limit: float, speed_conversion: float, now: float, started_frame: int) -> None:
+    if started_frame != self._started_frame:
+      self.reset()
+      self._started_frame = started_frame
+
+    if not math.isfinite(limit) or limit <= 0:
+      self.clear()
+      return
+
+    if source != "Vision":
+      self.clear()
+    elif round(limit * speed_conversion) != round(self.last_limit * speed_conversion):
+      self.start_time = now
+
+    self.last_limit = limit

selfdrive/ui/mici/onroad/hud_renderer.py

@@ -6,6 +6,7 @@ from openpilot.common.constants import CV
 from openpilot.selfdrive.ui.onroad.starpilot.torque_bar import TorqueBar
 from openpilot.selfdrive.ui.onroad.starpilot.rivian_lateral_mode import rivian_lateral_mode
 from openpilot.selfdrive.ui.mici.onroad.speed_limit_utils import resolve_display_speed_limit_ms
+from openpilot.selfdrive.ui.lib.speed_limit_pulse import SpeedLimitPulse
 from openpilot.selfdrive.ui.onroad.starpilot.navigation_card import NavigationCardRenderer
 from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus
 from openpilot.selfdrive.ui.onroad.exp_button import get_wheel_tint
@@ -133,9 +134,7 @@ class HudRenderer(Widget):
     self._show_speed_limit_offset: bool = False
     self._speed_limit_overridden: bool = False
     self._pending_speed_limit: float = 0.0
-    self._vision_speed_limit_active: bool = False
-    self._last_vision_speed_limit: float = 0.0
-    self._vision_speed_limit_pulse_start: float = -VISION_SPEED_LIMIT_PULSE_SECONDS
+    self._speed_limit_pulse = SpeedLimitPulse()
     self._prompt_visible: bool = False
     self._prompt_card_rect: rl.Rectangle = rl.Rectangle(0, 0, 0, 0)
     self._prompt_sign_rect: rl.Rectangle = rl.Rectangle(0, 0, 0, 0)
@@ -256,12 +255,9 @@ class HudRenderer(Widget):
           primary_priority=primary_priority,
           secondary_priority=secondary_priority,
         )
-        vision_speed_limit_active = starpilot_plan.slcSpeedLimitSource == "Vision" and resolved_speed_limit > 0.0
-        vision_speed_limit_changed = abs(resolved_speed_limit - self._last_vision_speed_limit) >= 0.1
-        if vision_speed_limit_active and (not self._vision_speed_limit_active or vision_speed_limit_changed):
-          self._vision_speed_limit_pulse_start = rl.get_time()
-        self._vision_speed_limit_active = vision_speed_limit_active
-        self._last_vision_speed_limit = resolved_speed_limit if vision_speed_limit_active else 0.0
+        self._speed_limit_pulse.update(
+          starpilot_plan.slcSpeedLimitSource, resolved_speed_limit, speed_conversion, rl.get_time(), ui_state.started_frame,
+        )
 
         display_speed_limit = starpilot_plan.slcOverriddenSpeed if starpilot_plan.slcOverriddenSpeed > 0 else resolved_speed_limit
         self._speed_limit = max(0.0, display_speed_limit * speed_conversion)
@@ -276,8 +272,7 @@ class HudRenderer(Widget):
         self._show_speed_limit_offset = False
         self._speed_limit_overridden = False
         self._pending_speed_limit = 0.0
-        self._vision_speed_limit_active = False
-        self._last_vision_speed_limit = 0.0
+        self._speed_limit_pulse.clear()
       self._prompt_visible = self._pending_speed_limit > 0
     else:
       self._show_speed_limit = False
@@ -286,8 +281,7 @@ class HudRenderer(Widget):
       self._show_speed_limit_offset = False
       self._speed_limit_overridden = False
       self._pending_speed_limit = 0.0
-      self._vision_speed_limit_active = False
-      self._last_vision_speed_limit = 0.0
+      self._speed_limit_pulse.clear()
       self._prompt_visible = False
 
   def prepare(self, rect: rl.Rectangle) -> None:
@@ -514,7 +508,7 @@ class HudRenderer(Widget):
 
   def _speed_limit_pulse_color(self, base_color, alpha: int) -> rl.Color:
     base = rl.Color(base_color.r, base_color.g, base_color.b, alpha)
-    elapsed = rl.get_time() - self._vision_speed_limit_pulse_start
+    elapsed = rl.get_time() - self._speed_limit_pulse.start_time
     if elapsed < 0.0 or elapsed >= VISION_SPEED_LIMIT_PULSE_SECONDS:
       return base
 

selfdrive/ui/onroad/starpilot/slc_speed_limit.py

@@ -17,6 +17,7 @@ from openpilot.selfdrive.ui.onroad.starpilot.source_bubble_layout import (
   source_content_metrics, source_value_text, visible_source_rows,
 )
 from openpilot.selfdrive.ui.lib.starpilot_state import starpilot_state
+from openpilot.selfdrive.ui.lib.speed_limit_pulse import SpeedLimitPulse
 
 _WHITE = rl.Color(255, 255, 255, 255)
 
@@ -53,41 +54,16 @@ FONT_EU_OFFSET = 40
 # is "Vision" and the resolved value just changed.
 VISION_SPEED_LIMIT_PULSE_SECONDS = 1.0
 VISION_SPEED_LIMIT_PULSE_COLOR = rl.Color(188, 132, 255, 255)
-VISION_SPEED_LIMIT_CHANGE_THRESHOLD = 0.1  # m/s
-
-
-# ── Vision speed-limit pulse state (one-shot highlight) ────────────────
-# Persists across frames so we can detect a just-changed vision limit and
-# animate the sign colors toward VISION_SPEED_LIMIT_PULSE_COLOR for
-# VISION_SPEED_LIMIT_PULSE_SECONDS. Held at module scope because this file
-# is a procedural API consumed once per frame by StarPilotOnroadView.
-
-_pulse = {
-  "last": 0.0,            # last resolved vision limit (m/s) seen while active
-  "start": -VISION_SPEED_LIMIT_PULSE_SECONDS,  # get_time() stamp of the last change
-}
+_pulse = SpeedLimitPulse()
 
 
 def _reset_pulse() -> None:
-  """Clear pulse state when SLC goes hidden or stale."""
-  _pulse["last"] = 0.0
-  _pulse["start"] = -VISION_SPEED_LIMIT_PULSE_SECONDS
+  _pulse.reset()
 
 
 def _tick_pulse(source: str, resolved_ms: float) -> None:
-  """Update pulse state once per frame from the resolved speed limit.
-
-  The pulse fires when the active source is "Vision" and its resolved value
-  changed by at least VISION_SPEED_LIMIT_CHANGE_THRESHOLD (m/s).
-  """
-  vision_active = source == "Vision" and resolved_ms > 0.0
-  if not vision_active:
-    _pulse["start"] = -VISION_SPEED_LIMIT_PULSE_SECONDS
-    return
-
-  if abs(resolved_ms - _pulse["last"]) >= VISION_SPEED_LIMIT_CHANGE_THRESHOLD:
-    _pulse["start"] = rl.get_time()
-  _pulse["last"] = resolved_ms
+  speed_conversion = CV.MS_TO_KPH if ui_state.is_metric else CV.MS_TO_MPH
+  _pulse.update(source, resolved_ms, speed_conversion, rl.get_time(), ui_state.started_frame)
 
 
 def _speed_limit_pulse_color(base: rl.Color, alpha: int) -> rl.Color:
@@ -98,7 +74,7 @@ def _speed_limit_pulse_color(base: rl.Color, alpha: int) -> rl.Color:
   where t is elapsed / VISION_SPEED_LIMIT_PULSE_SECONDS.
   """
   base_with_alpha = rl.Color(base.r, base.g, base.b, alpha)
-  elapsed = rl.get_time() - _pulse["start"]
+  elapsed = rl.get_time() - _pulse.start_time
   if elapsed < 0.0 or elapsed >= VISION_SPEED_LIMIT_PULSE_SECONDS:
     return base_with_alpha
 
@@ -118,7 +94,7 @@ def _get_slc_state():
   """Extract SLC state from SubMaster. Returns dict or None if stale/hidden."""
   sm = ui_state.sm
   if sm.recv_frame["starpilotPlan"] < ui_state.started_frame:
-    _reset_pulse()
+    _pulse.clear()
     return None
 
   plan = sm["starpilotPlan"]
@@ -129,7 +105,7 @@ def _get_slc_state():
   unconfirmed_valid = plan.unconfirmedSlcSpeedLimit > 1
 
   if not show_slc:
-    _reset_pulse()
+    _pulse.clear()
     return None
 
   speed_conversion = CV.MS_TO_KPH if ui_state.is_metric else CV.MS_TO_MPH

selfdrive/ui/tests/test_speed_limit_pulse.py

@@ -0,0 +1,139 @@
+import math
+from types import SimpleNamespace
+
+import pyray as rl
+import pytest
+
+from openpilot.common.constants import CV
+from openpilot.selfdrive.ui.lib.speed_limit_pulse import SpeedLimitPulse
+
+
+@pytest.mark.parametrize("conversion", [CV.MS_TO_MPH, CV.MS_TO_KPH])
+def test_pulse_tracks_visible_number_without_delaying_real_changes(conversion):
+  pulse = SpeedLimitPulse()
+  pulse.update("Vision", 35 / conversion, conversion, 0.0, 1)
+
+  for frame in range(1, 100):
+    limit = (35.3 if frame % 2 else 35.0) / conversion
+    pulse.update("Vision", limit, conversion, frame / 20, 1)
+    assert pulse.start_time == 0.0
+
+  pulse.update("Vision", 30 / conversion, conversion, 5.0, 1)
+  assert pulse.start_time == 5.0
+
+
+def test_source_reacquisition_does_not_repeat_an_already_visible_limit():
+  pulse = SpeedLimitPulse()
+  for index, source in enumerate(["Map Data", "Vision", "Dashboard", "Vision"] * 10):
+    pulse.update(source, 15.6464, CV.MS_TO_MPH, float(index), 1)
+    assert pulse.start_time == -math.inf
+
+
+def test_new_drive_resets_pulse_and_unit_change_does_not():
+  pulse = SpeedLimitPulse()
+  pulse.update("Vision", 15.6464, CV.MS_TO_MPH, 1.0, 10)
+  pulse.update("Vision", 15.6464, CV.MS_TO_KPH, 2.0, 10)
+  assert pulse.start_time == 1.0
+
+  pulse.update("Vision", 15.6464, CV.MS_TO_KPH, 3.0, 20)
+  assert pulse.start_time == 3.0
+
+
+@pytest.mark.parametrize("missing", [0.0, float("nan"), float("inf")])
+def test_missing_reading_cancels_pulse_without_forgetting_limit(missing):
+  pulse = SpeedLimitPulse()
+  pulse.update("Vision", 15.6464, CV.MS_TO_MPH, 0.0, 1)
+  pulse.update("Vision", missing, CV.MS_TO_MPH, 0.2, 1)
+  pulse.update("Vision", 15.6464, CV.MS_TO_MPH, 0.3, 1)
+  assert pulse.start_time == -math.inf
+
+
+class FakeParams(dict):
+  def get(self, key, encoding=None):
+    return super().get(key)
+
+  def get_bool(self, key):
+    return bool(self.get(key))
+
+
+class FakeSubMaster(dict):
+  recv_frame = {"carState": 20, "starpilotPlan": 20}
+  valid = {"starpilotCarState": True}
+
+
+@pytest.fixture(params=["big", "small"])
+def renderer(request, monkeypatch):
+  from openpilot.selfdrive.ui.mici.onroad import hud_renderer
+  from openpilot.selfdrive.ui.onroad.starpilot import slc_speed_limit
+
+  plan = SimpleNamespace(
+    slcSpeedLimit=15.6464, slcSpeedLimitSource="Vision", slcOverriddenSpeed=0.0, slcSpeedLimitOffset=0.0,
+    slcMapSpeedLimit=15.6464, slcMapboxSpeedLimit=0.0, slcNextSpeedLimit=0.0,
+    unconfirmedSlcSpeedLimit=0.0, speedLimitChanged=False,
+  )
+  sm = FakeSubMaster({
+    "starpilotPlan": plan,
+    "starpilotCarState": SimpleNamespace(dashboardSpeedLimit=15.6464),
+    "carState": SimpleNamespace(vCruiseCluster=100.0, vEgoCluster=0.0, vEgo=0.0, standstill=True),
+    "controlsState": SimpleNamespace(),
+    "selfdriveState": SimpleNamespace(enabled=False),
+  })
+  sm.recv_frame = sm.recv_frame.copy()
+  params = FakeParams({"ShowSpeedLimits": True, "ShowSLCOffset": True, "VisionSpeedLimitDetection": True,
+                       "SLCPriority1": "Vision", "SLCPriority2": "Map Data"})
+  ui = SimpleNamespace(sm=sm, started_frame=10, is_metric=False, ui_params=params,
+                       params_memory=SimpleNamespace(get_float=lambda _: plan.slcSpeedLimit))
+  pulse = SpeedLimitPulse()
+  clock = [0.0]
+  monkeypatch.setattr(rl, "get_time", lambda: clock[0])
+
+  if request.param == "big":
+    monkeypatch.setattr(slc_speed_limit, "ui_state", ui)
+    monkeypatch.setattr(slc_speed_limit, "_pulse", pulse)
+    update = slc_speed_limit._get_slc_state
+  else:
+    monkeypatch.setattr(hud_renderer, "ui_state", ui)
+    monkeypatch.setattr(hud_renderer, "rivian_lateral_mode", SimpleNamespace(update=lambda: None, wheel_tint=None))
+    hud = object.__new__(hud_renderer.HudRenderer)
+    hud._engaged = False
+    hud.set_speed = 100.0
+    hud.v_ego_cluster_seen = False
+    hud._speed_limit_pulse = pulse
+    update = hud._update_state
+
+  return SimpleNamespace(update=update, pulse=pulse, clock=clock, plan=plan, sm=sm, params=params)
+
+
+@pytest.mark.parametrize("gap", ["hidden", "stale", "source"])
+def test_both_renderers_preserve_pulse_history_at_standstill(renderer, gap):
+  renderer.update()
+  assert renderer.pulse.start_time == 0.0
+  renderer.clock[0] = 0.5
+
+  if gap == "hidden":
+    renderer.params["ShowSpeedLimits"] = False
+  elif gap == "stale":
+    renderer.sm.recv_frame["starpilotPlan"] = 9
+  else:
+    renderer.plan.slcSpeedLimitSource = "Map Data"
+  renderer.update()
+
+  renderer.params["ShowSpeedLimits"] = True
+  renderer.sm.recv_frame["starpilotPlan"] = 20
+  renderer.plan.slcSpeedLimitSource = "Vision"
+  renderer.clock[0] = 0.6
+  renderer.update()
+  assert renderer.pulse.start_time == -math.inf
+
+  renderer.plan.slcSpeedLimit = 30 * CV.MPH_TO_MS
+  renderer.clock[0] = 0.7
+  renderer.update()
+  assert renderer.pulse.start_time == 0.7
+
+
+def test_both_renderers_do_not_pulse_for_jitter_with_same_sign_number(renderer):
+  renderer.update()
+  renderer.plan.slcSpeedLimit = 35.3 * CV.MPH_TO_MS
+  renderer.clock[0] = 2.0
+  renderer.update()
+  assert renderer.pulse.start_time == 0.0

selfdrive/ui/ui.py

@@ -45,7 +45,7 @@ def _stall_context() -> dict[str, object]:
 
 def main():
   cores = {5, }
-  config_realtime_process(0, Priority.CTRL_HIGH)
+  config_realtime_process(0, Priority.UI)
 
   stall_monitor = UIStallMonitor("raylib_ui")
   stall_monitor.progress("ui.before_init_window")