Keep navigation animation timing consistent under onroad load

selfdrive/ui/tests/test_ui_state_performance.py

@@ -1,6 +1,8 @@
 import time
 from types import SimpleNamespace
 
+import pytest
+
 from openpilot.selfdrive.ui.lib.ui_param_cache import UIParamCache
 from openpilot.selfdrive.ui import ui_state as ui_state_module
 
@@ -77,3 +79,73 @@ def test_ui_update_reports_offroad_callback(monkeypatch):
     "ui.update.before_offroad_callback.<lambda>",
     "ui.update.after_offroad_callback.<lambda>",
   ]
+
+
+@pytest.fixture
+def toggles_state(monkeypatch):
+  class SubMaster(dict):
+    frame = 0
+    updated = {"pandaStates": False, "wideRoadCameraState": False, "starpilotPlan": True}
+    valid = {"starpilotCarState": False}
+    alive = {"wideRoadCameraState": False}
+    recv_frame = {"pandaStates": 0}
+
+  state = object.__new__(ui_state_module.UIState)
+  state.sm = SubMaster({
+    "deviceState": SimpleNamespace(started=True, chestnutPresent=False),
+    "starpilotPlan": SimpleNamespace(starpilotToggles=""),
+  })
+  state.ui_params = SimpleNamespace(get_bool=lambda key: False)
+  state.params_memory = SimpleNamespace(get_bool=lambda key: False, get_int=lambda key, default=0: default)
+  state.usbgpu = False
+  state.starpilot_toggles = {"standby_mode": False}
+  state._last_starpilot_toggles = ""
+  monkeypatch.setattr(ui_state_module.rl, "get_fps", lambda: 60)
+  return state
+
+
+def test_identical_starpilot_toggles_are_decoded_once(toggles_state, monkeypatch):
+  payloads = []
+  original_loads = ui_state_module.json.loads
+
+  def loads(payload):
+    payloads.append(payload)
+    return original_loads(payload)
+
+  monkeypatch.setattr(ui_state_module.json, "loads", loads)
+  toggles_state.sm["starpilotPlan"].starpilotToggles = '{"standby_mode": true, "path_width": 6.1}'
+
+  toggles_state._update_state()
+  toggles_state._update_state()
+  toggles_state.sm["starpilotPlan"].starpilotToggles = ""
+  toggles_state._update_state()
+  toggles_state.sm["starpilotPlan"].starpilotToggles = '{"standby_mode": true, "path_width": 6.1}'
+  toggles_state._update_state()
+
+  assert len(payloads) == 1
+  assert toggles_state.starpilot_toggles["standby_mode"] is True
+  assert toggles_state.starpilot_toggles["path_width"] == 6.1
+
+
+def test_changed_starpilot_toggles_apply_immediately(toggles_state):
+  toggles_state.sm["starpilotPlan"].starpilotToggles = '{"standby_mode": true, "path_width": 6.1}'
+  toggles_state._update_state()
+  toggles_state.sm["starpilotPlan"].starpilotToggles = '{"standby_mode": false}'
+  toggles_state._update_state()
+
+  assert toggles_state.starpilot_toggles["standby_mode"] is False
+  assert toggles_state.starpilot_toggles["path_width"] == 6.1
+
+
+@pytest.mark.parametrize("invalid_payload", ['{"standby_mode":', '[]', 'null'])
+def test_invalid_starpilot_toggles_preserve_state_and_recover(toggles_state, monkeypatch, invalid_payload):
+  monkeypatch.setattr(ui_state_module.cloudlog, "warning", lambda message: None)
+  toggles_state.sm["starpilotPlan"].starpilotToggles = '{"standby_mode": true}'
+  toggles_state._update_state()
+  toggles_state.sm["starpilotPlan"].starpilotToggles = invalid_payload
+  toggles_state._update_state()
+  assert toggles_state.starpilot_toggles["standby_mode"] is True
+
+  toggles_state.sm["starpilotPlan"].starpilotToggles = '{"standby_mode": false}'
+  toggles_state._update_state()
+  assert toggles_state.starpilot_toggles["standby_mode"] is False

selfdrive/ui/ui_state.py

@@ -112,6 +112,7 @@ class UIState:
     self.switchback_mode_enabled: bool = False
     self.traffic_mode_enabled: bool = False
     self.conditional_status: int = 0
+    self._last_starpilot_toggles: str = ""
     self.starpilot_toggles: dict = {
       "debug_mode": False,
       "driver_camera_in_reverse": False,
@@ -236,11 +237,12 @@ class UIState:
     if self.sm.updated["starpilotPlan"]:
       plan = self.sm["starpilotPlan"]
       toggles_str = plan.starpilotToggles
-      if toggles_str:
+      if toggles_str and toggles_str != self._last_starpilot_toggles:
         try:
           parsed = json.loads(toggles_str)
           if isinstance(parsed, dict):
             self.starpilot_toggles.update(parsed)
+            self._last_starpilot_toggles = toggles_str
         except Exception as e:
           cloudlog.warning(f"Error parsing starpilot_toggles: {e}")
 

system/ui/tests/test_nav_widget.py

@@ -2,8 +2,9 @@ import pyray as rl
 import pytest
 
 from openpilot.system.ui.lib.application import MouseEvent, MousePos, gui_app
+from openpilot.common.filter_simple import BounceFilter
 from openpilot.system.ui.widgets import Widget
-from openpilot.system.ui.widgets.nav_widget import NavWidget
+from openpilot.system.ui.widgets.nav_widget import NavBar, NavWidget, NAV_BAR_MARGIN, NAV_BAR_HEIGHT
 
 
 class NavScreen(NavWidget):
@@ -25,6 +26,8 @@ def viewport():
 def screen(monkeypatch, viewport):
   monkeypatch.setattr(rl, "draw_rectangle_rec", lambda *_: None)
   monkeypatch.setattr(rl, "get_time", lambda: 10.0)
+  monkeypatch.setattr(rl, "get_frame_time", lambda: 1 / 60)
+  monkeypatch.setattr(gui_app, "_target_fps", 60)
   monkeypatch.setattr(gui_app, "_show_touches", False)
   monkeypatch.setattr(gui_app, "_mouse_events", [])
   screen = NavScreen()
@@ -126,3 +129,93 @@ def test_programmatic_dismiss_uncovers_background_before_first_moving_frame(scre
   assert not screen.covers_background(viewport)
   screen.render(viewport)
   assert screen.rect.y > 0
+
+
+@pytest.mark.parametrize("fps", [20, 30, 60])
+def test_show_animation_duration_tracks_elapsed_time(monkeypatch, screen, viewport, fps):
+  monkeypatch.setattr(rl, "get_frame_time", lambda: 1 / fps)
+  shown = []
+  screen.set_shown_callback(lambda: shown.append(True))
+  screen.show_event()
+  for _frames in range(1, fps * 3):
+    screen.render(viewport)
+    if screen.covers_background(viewport):
+      break
+
+  assert screen.covers_background(viewport)
+  assert screen._y_pos_filter.x == screen._y_pos_filter.velocity.x == 0
+  assert _frames / fps == pytest.approx(35 / 60, abs=1 / fps)
+  assert shown == [True]
+
+
+@pytest.mark.parametrize("fps", [20, 30, 60])
+def test_dismiss_animation_duration_tracks_elapsed_time(monkeypatch, screen, viewport, fps):
+  monkeypatch.setattr(rl, "get_frame_time", lambda: 1 / fps)
+  popped, dismissed, backed = [], [], []
+  monkeypatch.setattr(gui_app, "pop_widget", lambda: popped.append(True))
+  screen.set_back_callback(lambda: backed.append(True))
+  screen.dismiss(lambda: dismissed.append(True))
+  for _frames in range(1, fps * 3):
+    screen.render(viewport)
+    if popped:
+      break
+
+  assert _frames / fps == pytest.approx(13 / 60, abs=1 / fps)
+  assert popped == dismissed == [True]
+  assert not backed
+
+
+def test_show_animation_retains_original_sixty_fps_motion(screen):
+  reference = BounceFilter(gui_app.height, 0.1, 1 / 60, bounce=1)
+  screen.show_event()
+  for _ in range(20):
+    reference.update(0.0)
+    screen._update_state()
+    assert screen._y_pos_filter.x == pytest.approx(reference.x)
+    assert screen._y_pos_filter.velocity.x == pytest.approx(reference.velocity.x)
+
+
+@pytest.mark.parametrize("fps", [20, 30, 60])
+def test_navigation_bar_fade_tracks_elapsed_time(monkeypatch, screen, fps):
+  monkeypatch.setattr(rl, "get_frame_time", lambda: 1 / fps)
+  monkeypatch.setattr(rl, "draw_rectangle_rounded", lambda *_: None)
+  monkeypatch.setattr(rl, "draw_rectangle_rounded_lines_ex", lambda *_: None)
+  bar = NavBar()
+  bar.set_alpha(0.0)
+  for _ in range(fps // 2):
+    bar._render(bar.rect)
+  assert bar._alpha_filter.x == pytest.approx((1 - bar._alpha_filter.alpha) ** 30)
+
+
+@pytest.mark.parametrize("fps", [20, 30, 60])
+def test_navigation_bar_slide_tracks_elapsed_time(monkeypatch, screen, viewport, fps):
+  monkeypatch.setattr(rl, "get_frame_time", lambda: 1 / fps)
+  screen._nav_bar_y_filter.x = -NAV_BAR_MARGIN - NAV_BAR_HEIGHT
+  for _ in range(fps // 2):
+    screen.render(viewport)
+  remaining = (1 - screen._nav_bar_y_filter.alpha) ** 30
+  assert screen._nav_bar_y_filter.x == pytest.approx(NAV_BAR_MARGIN - (2 * NAV_BAR_MARGIN + NAV_BAR_HEIGHT) * remaining)
+
+
+def test_long_frame_uses_bounded_spring_steps(monkeypatch, screen):
+  screen.show_event()
+  monkeypatch.setattr(rl, "get_frame_time", lambda: 0.1)
+  screen._update_state()
+  expected = screen._y_pos_filter.x, screen._y_pos_filter.velocity.x
+
+  screen.show_event()
+  monkeypatch.setattr(rl, "get_frame_time", lambda: 5.0)
+  screen._update_state()
+  assert (screen._y_pos_filter.x, screen._y_pos_filter.velocity.x) == pytest.approx(expected)
+
+
+@pytest.mark.parametrize("elapsed", [0.0, -1.0, float("nan"), float("inf")])
+def test_invalid_frame_duration_uses_default_step(monkeypatch, screen, elapsed):
+  screen.show_event()
+  screen._update_state()
+  expected = screen._y_pos_filter.x, screen._y_pos_filter.velocity.x
+
+  screen.show_event()
+  monkeypatch.setattr(rl, "get_frame_time", lambda: elapsed)
+  screen._update_state()
+  assert (screen._y_pos_filter.x, screen._y_pos_filter.velocity.x) == pytest.approx(expected)

system/ui/widgets/nav_widget.py

@@ -1,6 +1,7 @@
 from __future__ import annotations
 
 import abc
+import math
 import pyray as rl
 from collections.abc import Callable
 from openpilot.system.ui.widgets import Widget
@@ -19,6 +20,32 @@ DISMISS_PUSH_OFFSET = NAV_BAR_MARGIN + NAV_BAR_HEIGHT + 50  # px extra to push d
 DISMISS_ANIMATION_RC = 0.2  # slightly slower for non-user triggered dismiss animation
 
 
+def _frame_dt(default: float) -> float:
+  elapsed = rl.get_frame_time()
+  return min(elapsed if math.isfinite(elapsed) and elapsed > 0 else default, 0.1)
+
+
+def _update_filter(animation: FirstOrderFilter, target: float, elapsed: float) -> float:
+  alpha = 1 - (1 - animation.alpha) ** (elapsed / animation.dt)
+  animation.x += alpha * (target - animation.x)
+  return animation.x
+
+
+def _update_bounce(animation: BounceFilter, target: float, elapsed: float) -> float:
+  steps = max(1, math.ceil(elapsed / animation.dt))
+  for _ in range(steps):
+    step_dt = min(elapsed, animation.dt)
+    scale = step_dt / animation.dt
+    _update_filter(animation, target, step_dt)
+    animation.velocity.x += (target - animation.x) * animation.bounce * (animation.dt * 60) * step_dt
+    _update_filter(animation.velocity, 0.0, step_dt)
+    if abs(animation.velocity.x) < 1e-5:
+      animation.velocity.x = 0.0
+    animation.x += animation.velocity.x * scale
+    elapsed -= step_dt
+  return animation.x
+
+
 class NavBar(Widget):
   FADE_AFTER_SECONDS = 2.0
 
@@ -42,7 +69,7 @@ class NavBar(Widget):
   def _render(self, _):
     if rl.get_time() - self._fade_time > self.FADE_AFTER_SECONDS:
       self._alpha = 0.0
-    alpha = self._alpha_filter.update(self._alpha)
+    alpha = _update_filter(self._alpha_filter, self._alpha, _frame_dt(self._alpha_filter.dt))
 
     # white bar with black border
     rl.draw_rectangle_rounded(self._rect, 1.0, 6, rl.Color(255, 255, 255, int(255 * 0.9 * alpha)))
@@ -156,7 +183,7 @@ class NavWidget(Widget, abc.ABC):
     if self._playing_dismiss_animation:
       new_y = self._rect.height + DISMISS_PUSH_OFFSET
 
-    new_y = self._y_pos_filter.update(new_y)
+    new_y = _update_bounce(self._y_pos_filter, new_y, _frame_dt(self._y_pos_filter.dt))
     if abs(new_y) < 1 and abs(self._y_pos_filter.velocity.x) < 0.5:
       new_y = self._y_pos_filter.x = 0.0
       self._y_pos_filter.velocity.x = 0.0
@@ -202,7 +229,7 @@ class NavWidget(Widget, abc.ABC):
       self._nav_bar_y_filter.x = -NAV_BAR_MARGIN - NAV_BAR_HEIGHT
     # Animate back to top
     else:
-      self._nav_bar_y_filter.update(NAV_BAR_MARGIN)
+      _update_filter(self._nav_bar_y_filter, NAV_BAR_MARGIN, _frame_dt(self._nav_bar_y_filter.dt))
 
     self._nav_bar.set_position(bar_x, self._nav_bar_y_filter.x)
     self._nav_bar.render()