ui: add accel boost indicator

openpilot/selfdrive/ui/mici/onroad/augmented_road_view.py

@@ -22,6 +22,7 @@ from openpilot.common.transformations.orientation import rot_from_euler
 from enum import IntEnum
 
 if gui_app.sunnypilot_ui():
+  from openpilot.selfdrive.ui.sunnypilot.mici.onroad.augmented_road_view import AugmentedRoadViewSP
   from openpilot.selfdrive.ui.sunnypilot.mici.onroad.hud_renderer import HudRendererSP as HudRenderer
   from openpilot.selfdrive.ui.sunnypilot.ui_state import OnroadTimerStatus
 
@@ -156,9 +157,10 @@ class BookmarkIcon(Widget):
         rl.draw_texture_ex(icon, rl.Vector2(icon_x, icon_y), 0.0, 1.0, rl.Color(255, 255, 255, round(255 * alpha)))
 
 
-class AugmentedRoadView(CameraView):
+class AugmentedRoadView(CameraView, AugmentedRoadViewSP):
   def __init__(self, bookmark_callback=None, stream_type: VisionStreamType = VisionStreamType.VISION_STREAM_NARROW_ROAD):
     super().__init__("camerad", stream_type)
+    AugmentedRoadViewSP.__init__(self)
     self._bookmark_callback = bookmark_callback
     self._set_placeholder_color(rl.BLACK)
 
@@ -275,7 +277,7 @@ class AugmentedRoadView(CameraView):
     if show_navigation:
       self._navigation.render(rl.Rectangle(self._content_rect.x + self._content_rect.width, self.rect.y, PANEL_WIDTH, self.rect.height))
     else:
-      self._confidence_ball.render(self.rect)
+      AugmentedRoadViewSP.render_side_panel(self, self.rect)
 
     self._bookmark_icon.render(self.rect)
 

openpilot/selfdrive/ui/sunnypilot/mici/onroad/accel_boost_indicator.py

@@ -0,0 +1,95 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+import math
+import pyray as rl
+
+from openpilot.cereal import log
+from openpilot.common.filter_simple import FirstOrderFilter
+from openpilot.selfdrive.ui.mici.onroad import SIDE_PANEL_WIDTH
+from openpilot.selfdrive.ui.ui_state import ui_state
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_boost import get_boost_max
+from openpilot.system.ui.lib.application import gui_app
+from openpilot.system.ui.widgets import Widget
+
+BAR_WIDTH = 26
+TRACK_TOP_RATIO = 0.08
+TRACK_BOTTOM_RATIO = 0.92
+CAP_BLEND_START = 0.45
+MAX_BOOST_BLEND = 0.55
+FADE_THRESHOLD = 0.01
+
+COLOR_TRACK = rl.Color(255, 255, 255, 28)
+COLOR_FILL = rl.Color(0, 230, 120, 255)
+
+
+def get_boost_fraction(boost: float, max_boost: float) -> float:
+  if max_boost <= 0.0:
+    return 0.0
+  return min(max(boost / max_boost, 0.0), 1.0)
+
+
+def lighten(color: rl.Color, amount: float) -> rl.Color:
+  amount = min(max(amount, 0.0), 1.0)
+  return rl.Color(int(color.r + (255 - color.r) * amount),
+                  int(color.g + (255 - color.g) * amount),
+                  int(color.b + (255 - color.b) * amount), color.a)
+
+
+def get_boost_color(fraction: float) -> rl.Color:
+  blend = max(0.0, (fraction - CAP_BLEND_START) / (1.0 - CAP_BLEND_START))
+  return lighten(COLOR_FILL, MAX_BOOST_BLEND * blend)
+
+
+class AccelBoostIndicator(Widget):
+  def __init__(self, panel_width: float = SIDE_PANEL_WIDTH, bar_width: float = BAR_WIDTH):
+    super().__init__()
+    self._panel_width = panel_width
+    self._bar_width = bar_width
+    fps = max(gui_app.target_fps, 1)
+    self._boost_filter = FirstOrderFilter(0.0, 0.15, 1 / fps)
+    self._alpha_filter = FirstOrderFilter(0.0, 0.1, 1 / fps)
+    self._max_boost_filter = FirstOrderFilter(get_boost_max(), 0.25, 1 / fps)
+
+  @property
+  def holding_panel(self) -> bool:
+    return self._alpha_filter.x > FADE_THRESHOLD
+
+  def _update_state(self):
+    sm = ui_state.sm
+    visible = (ui_state.accel_boost and ui_state.has_longitudinal_control and sm.alive['selfdriveState'] and
+               sm.alive['longitudinalPlan'] and sm['selfdriveState'].enabled)
+    plan = sm['longitudinalPlan']
+    applied = visible and plan.longitudinalPlanSource == log.LongitudinalPlan.LongitudinalPlanSource.e2e
+    self._boost_filter.update(float(plan.accelBoost) if applied else 0.0)
+    self._alpha_filter.update(1.0 if visible else 0.0)
+
+    if sm.alive['selfdriveState']:
+      self._max_boost_filter.update(get_boost_max(sm['selfdriveState'].personality))
+
+  def _render(self, _):
+    alpha = self._alpha_filter.x
+    if alpha < FADE_THRESHOLD:
+      return
+
+    panel_x = max(self.rect.x, self.rect.x + self.rect.width - self._panel_width)
+    bar_x = panel_x + (self._panel_width - self._bar_width) / 2
+    track_top = self.rect.y + self.rect.height * TRACK_TOP_RATIO
+    track = rl.Rectangle(bar_x, track_top, self._bar_width,
+                         max(1.0, self.rect.height * (TRACK_BOTTOM_RATIO - TRACK_TOP_RATIO)))
+    rl.draw_rectangle_rounded(track, 0.5, 12,
+                              rl.Color(COLOR_TRACK.r, COLOR_TRACK.g, COLOR_TRACK.b, int(COLOR_TRACK.a * alpha)))
+
+    fraction = get_boost_fraction(self._boost_filter.x, self._max_boost_filter.x)
+    fill_h = fraction * track.height
+    if fill_h < 1.0:
+      return
+
+    fill_color = get_boost_color(fraction)
+    tip = track.y + track.height - fill_h
+    rl.begin_scissor_mode(int(track.x), int(tip), int(math.ceil(track.width)) + 1, int(math.ceil(fill_h)))
+    rl.draw_rectangle_rounded(track, 0.5, 12, rl.Color(fill_color.r, fill_color.g, fill_color.b, int(235 * alpha)))
+    rl.end_scissor_mode()

openpilot/selfdrive/ui/sunnypilot/mici/onroad/augmented_road_view.py

@@ -0,0 +1,19 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+import pyray as rl
+
+from openpilot.selfdrive.ui.sunnypilot.mici.onroad.accel_boost_indicator import AccelBoostIndicator
+
+
+class AugmentedRoadViewSP:
+  def __init__(self):
+    self._accel_boost_indicator = AccelBoostIndicator()
+
+  def render_side_panel(self, rect: rl.Rectangle) -> None:
+    self._accel_boost_indicator.render(rect)
+    if not self._accel_boost_indicator.holding_panel:
+      self._confidence_ball.render(rect)

openpilot/selfdrive/ui/sunnypilot/mici/tests/test_accel_boost_indicator.py

@@ -0,0 +1,36 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+from openpilot.cereal import log
+from openpilot.common.test import OpenpilotTestCase
+from openpilot.selfdrive.ui.sunnypilot.mici.onroad.accel_boost_indicator import (
+  CAP_BLEND_START, COLOR_FILL, get_boost_color, get_boost_fraction,
+)
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_boost import ACCEL_BOOST_MAX, get_boost_max
+
+
+class TestAccelBoostIndicator(OpenpilotTestCase):
+  def test_bar_fills_at_the_active_profile_cap(self):
+    for personality in (log.LongitudinalPersonality.aggressive, log.LongitudinalPersonality.standard,
+                        log.LongitudinalPersonality.relaxed):
+      cap = get_boost_max(personality)
+      assert get_boost_fraction(0.0, cap) == 0.0
+      assert get_boost_fraction(cap * 0.5, cap) == 0.5
+      assert get_boost_fraction(cap, cap) == 1.0
+
+    assert get_boost_fraction(max(ACCEL_BOOST_MAX), min(ACCEL_BOOST_MAX)) == 1.0
+    assert get_boost_fraction(0.0, 0.0) == 0.0
+    assert get_boost_fraction(-1.0, get_boost_max()) == 0.0
+
+  def test_fill_lightens_as_boost_builds(self):
+    base = get_boost_color(0.0)
+    assert (base.r, base.g, base.b) == (COLOR_FILL.r, COLOR_FILL.g, COLOR_FILL.b)
+    assert (get_boost_color(CAP_BLEND_START).r, get_boost_color(CAP_BLEND_START).b) == (COLOR_FILL.r, COLOR_FILL.b)
+
+    full = get_boost_color(1.0)
+    assert full.r > base.r
+    assert full.b > base.b
+    assert full.g > full.r and full.g > full.b

openpilot/selfdrive/ui/sunnypilot/ui_state.py

@@ -44,6 +44,7 @@ class UIStateSP:
     self.screensaver = ScreenSaverSP(params=self.params)
     self.screensaver_enabled: bool = False
 
+    self.accel_boost: bool = False
     self.active_bundle = None
     self.model_runner_tinygrad: bool = False
     self.active_runner_tinygrad: bool = False
@@ -171,6 +172,7 @@ class UIStateSP:
     # stock only counts the default big model's compiled pkl. a downloaded big bundle runs on the
     # chestnut just the same, so ChestnutState has to see it as available too.
     self.chestnut_compiled = self.chestnut_compiled or self.model_runner_tinygrad
+    self.accel_boost = self.params.get_bool("AccelBoostSP")
     self.blindspot = self.params.get_bool("BlindSpot")
     self.chevron_metrics = self.params.get("ChevronInfo")
     self.custom_interactive_timeout = self.params.get("InteractivityTimeout", return_default=True)