From 705804be72cb067defefbbbd55d816a17ef93c1b Mon Sep 17 00:00:00 2001 From: infiniteCable <75014343+infiniteCable@users.noreply.github.com> Date: Sun, 15 Feb 2026 15:03:00 +0100 Subject: [PATCH] Simple Acceleration Bar (#6) Simple Acceleration Bar --- common/params_keys.h | 1 + .../ui/mici/layouts/settings/ictoggles.py | 3 + selfdrive/ui/mici/onroad/hud_renderer.py | 7 +- selfdrive/ui/mici/onroad/long_accel_bar.py | 229 ++++++++++++++++++ selfdrive/ui/ui_state.py | 2 + 5 files changed, 241 insertions(+), 1 deletion(-) create mode 100644 selfdrive/ui/mici/onroad/long_accel_bar.py diff --git a/common/params_keys.h b/common/params_keys.h index d35c39879..64e074656 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -146,6 +146,7 @@ inline static std::unordered_map keys = { {"ForceRHDForBSM", {PERSISTENT, BOOL}}, {"DisableCarSteerAlerts", {PERSISTENT, BOOL}}, {"ForceShowTorqueBar", {PERSISTENT, BOOL}}, + {"ShowAccelBar", {PERSISTENT, BOOL}}, // --- sunnypilot params --- // {"ApiCache_DriveStats", {PERSISTENT, JSON}}, diff --git a/selfdrive/ui/mici/layouts/settings/ictoggles.py b/selfdrive/ui/mici/layouts/settings/ictoggles.py index fcbbd4363..d82ac5908 100644 --- a/selfdrive/ui/mici/layouts/settings/ictoggles.py +++ b/selfdrive/ui/mici/layouts/settings/ictoggles.py @@ -27,6 +27,7 @@ class ICTogglesLayoutMici(NavWidget): enable_dark_mode = BigParamControl("Dark Mode", "DarkMode") enable_onroad_screen_timer = BigParamControl("Onroad Screen Timeout", "DisableScreenTimer") force_enable_torque_bar = BigParamControl("Force Enable Torque Bar", "ForceShowTorqueBar") + enable_accel_bar = BigParamControl("Enable Accel Bar", "ShowAccelBar") self._scroller = Scroller([ @@ -42,6 +43,7 @@ class ICTogglesLayoutMici(NavWidget): enable_dark_mode, enable_onroad_screen_timer, force_enable_torque_bar, + enable_accel_bar, ], snap_items=False) # Toggle lists @@ -58,6 +60,7 @@ class ICTogglesLayoutMici(NavWidget): ("DarkMode", enable_dark_mode), ("DisableScreenTimer", enable_onroad_screen_timer), ("ForceShowTorqueBar", force_enable_torque_bar), + ("ShowAccelBar", enable_accel_bar), ) if ui_state.params.get_bool("ShowDebugInfo"): diff --git a/selfdrive/ui/mici/onroad/hud_renderer.py b/selfdrive/ui/mici/onroad/hud_renderer.py index 5d80af461..b942e2fad 100644 --- a/selfdrive/ui/mici/onroad/hud_renderer.py +++ b/selfdrive/ui/mici/onroad/hud_renderer.py @@ -2,6 +2,7 @@ import pyray as rl from dataclasses import dataclass from openpilot.common.constants import CV from openpilot.selfdrive.ui.mici.onroad.torque_bar import TorqueBar +from openpilot.selfdrive.ui.mici.onroad.long_accel_bar import LongitudinalAccelBar from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus from openpilot.system.ui.lib.application import gui_app, FontWeight from openpilot.system.ui.lib.multilang import tr @@ -117,6 +118,7 @@ class HudRenderer(Widget): self._turn_intent = TurnIntent() self._torque_bar = TorqueBar() + self._long_accel_bar = LongitudinalAccelBar() self._txt_wheel: rl.Texture = gui_app.texture('icons_mici/wheel.png', 50, 50) self._txt_wheel_critical: rl.Texture = gui_app.texture('icons_mici/wheel_critical.png', 50, 50) @@ -174,7 +176,10 @@ class HudRenderer(Widget): if ui_state.sm['controlsState'].lateralControlState.which() != 'angleState' or ui_state.force_enable_torque_bar: self._torque_bar.render(rect) - + + if ui_state.enable_accel_bar: + self._long_accel_bar.render(rect) + if self.is_cruise_set: self._draw_set_speed(rect) diff --git a/selfdrive/ui/mici/onroad/long_accel_bar.py b/selfdrive/ui/mici/onroad/long_accel_bar.py new file mode 100644 index 000000000..143f831ba --- /dev/null +++ b/selfdrive/ui/mici/onroad/long_accel_bar.py @@ -0,0 +1,229 @@ +from functools import wraps +from collections import OrderedDict + +import numpy as np +import pyray as rl +from cereal import car + +from openpilot.selfdrive.ui.mici.onroad import blend_colors +from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus +from openpilot.system.ui.widgets import Widget +from openpilot.system.ui.lib.application import gui_app +from openpilot.common.filter_simple import FirstOrderFilter +from openpilot.system.ui.lib.shader_polygon import draw_polygon, Gradient + + +ACCEL_MAX = 2.0 +ACCEL_MIN = -3.5 + +GearShifter = car.CarState.GearShifter + + +def clamp(x: float, lo: float, hi: float) -> float: + return lo if x < lo else hi if x > hi else x + + +def quantized_lru_cache(maxsize=256): + def decorator(func): + cache = OrderedDict() + + @wraps(func) + def wrapper(*args, **kwargs): + q_args = [] + for a in args: + if isinstance(a, float): + q_args.append(round(a * 2) / 2) + else: + q_args.append(a) + key = (tuple(q_args), tuple(sorted(kwargs.items()))) + if key in cache: + cache.move_to_end(key) + else: + if len(cache) >= maxsize: + cache.popitem(last=False) + cache[key] = func(*args, **kwargs) + return cache[key] + + return wrapper + return decorator + + +def _arc(cx: float, cy: float, r: float, a0_deg: float, a1_deg: float, segs: int) -> np.ndarray: + a = np.deg2rad(np.linspace(a0_deg, a1_deg, max(2, segs))) + return np.c_[cx + np.cos(a) * r, cy + np.sin(a) * r] + + +@quantized_lru_cache(maxsize=256) +def rounded_rect_pts(x: float, y: float, w: float, h: float, r: float, segs: int = 9) -> np.ndarray: + r = max(0.0, min(r, w * 0.5, h * 0.5)) + if r <= 0.01: + return np.array([[x, y], [x + w, y], [x + w, y + h], [x, y + h], [x, y]], dtype=np.float32) + + tr = _arc(x + w - r, y + r, r, 270, 360, segs) + br = _arc(x + w - r, y + h - r, r, 0, 90, segs) + bl = _arc(x + r, y + h - r, r, 90, 180, segs) + tl = _arc(x + r, y + r, r, 180, 270, segs) + return np.vstack([tr, br, bl, tl, tr[:1]]).astype(np.float32) + + +@quantized_lru_cache(maxsize=256) +def rounded_cap_segment_pts(x: float, y: float, w: float, h: float, r: float, *, cap: str, segs: int = 9) -> np.ndarray: + r = max(0.0, min(r, w * 0.5, h * 0.5)) + if r <= 0.01: + return np.array([[x, y], [x + w, y], [x + w, y + h], [x, y + h], [x, y]], dtype=np.float32) + + if cap == "top": + tr = _arc(x + w - r, y + r, r, 270, 360, segs) + tl = _arc(x + r, y + r, r, 180, 270, segs) + return np.vstack([ + tr, + [x + w, y + r], + [x + w, y + h], + [x, y + h], + [x, y + r], + tl, + tr[:1], + ]).astype(np.float32) + + br = _arc(x + w - r, y + h - r, r, 0, 90, segs) + bl = _arc(x + r, y + h - r, r, 90, 180, segs) + return np.vstack([ + [x + w, y], + [x + w, y + h - r], + br, + bl, + [x, y + h - r], + [x, y], + [x + w, y], + ]).astype(np.float32) + + +class LongitudinalAccelBar(Widget): + def __init__(self, demo: bool = False, scale: float = 1.0, always: bool = False): + super().__init__() + self._demo = demo + self._scale = scale + self._always = always + + # filtered effect and request values + self._aego_f = FirstOrderFilter(0.0, 0.15, 1 / gui_app.target_fps) + self._ades_f = FirstOrderFilter(0.0, 0.15, 1 / gui_app.target_fps) + + # load / visibility filters + self._mag_f = FirstOrderFilter(0.0, 0.20, 1 / gui_app.target_fps) + self._alpha_f = FirstOrderFilter(0.0, 0.10, 1 / gui_app.target_fps) + + def update_filter(self, aego: float, ades: float = 0.0): + self._aego_f.update(aego) + self._ades_f.update(ades) + + def _update_state(self): + if self._demo: + return + + cs = ui_state.sm['carState'] + cc = ui_state.sm['carControl'] + + aego = float(cs.aEgo) + if cs.gearShifter == GearShifter.reverse: + aego = -aego + + self._aego_f.update(aego) + self._ades_f.update(float(cc.actuators.accel)) + + @staticmethod + def _norm_acc(a: float) -> float: + return a / max(1e-3, (ACCEL_MAX if a >= 0.0 else -ACCEL_MIN)) + + def _render(self, rect: rl.Rectangle): + # alignment + bar_w = int(19 * self._scale) + right_margin = int(24 * self._scale) + bar_x = int(rect.x + rect.width - bar_w - right_margin) + + # vertical span + status_dot_radius = int(24 * self._scale) + bar_h_max = int(rect.height - 2 * status_dot_radius) + bar_h_min = int(160 * self._scale) + bar_h = int(clamp(bar_h_max, bar_h_min, max(bar_h_min, bar_h_max))) + bar_y = int(rect.y + status_dot_radius) + + # visibility + if self._demo: + self._alpha_f.update(1.0) + else: + visible = self._always or (ui_state.status != UIStatus.DISENGAGED) + self._alpha_f.update(1.0 if visible else 0.0) + + alpha = clamp(self._alpha_f.x, 0.0, 1.0) + if alpha <= 0.001: + return + + # color mode + colored = self._demo or (ui_state.status in (UIStatus.ENGAGED, UIStatus.LONG_ONLY)) + dim = 1.0 if colored else 0.55 + + aego = clamp(self._aego_f.x, ACCEL_MIN, ACCEL_MAX) + ades = clamp(self._ades_f.x, ACCEL_MIN, ACCEL_MAX) + + naego = clamp(self._norm_acc(aego), -1.0, 1.0) + nades = clamp(self._norm_acc(ades), -1.0, 1.0) + + # load scaling (TorqueBar-like growth) + self._mag_f.update(clamp(abs(nades), 0.0, 1.0)) + load = self._mag_f.x + + extra_w = int(np.interp(load, [0.5, 1.0], [0, 4]) * self._scale) + bw = bar_w + extra_w + bx = bar_x - extra_w + + radius = max(2.0, 6.0 * self._scale) + + # background + bg_alpha = int(255 * (0.18 + 0.10 * load) * alpha * dim) + bg_pts = rounded_rect_pts(float(bx), float(bar_y), float(bw), float(bar_h), float(radius), segs=9) + draw_polygon(rect, bg_pts, color=rl.Color(255, 255, 255, bg_alpha)) + + # zero line + mid_y = bar_y + bar_h // 2 + mid_alpha = int(255 * 0.30 * alpha * dim) + rl.draw_line(bx, mid_y, bx + bw, mid_y, rl.Color(255, 255, 255, mid_alpha)) + + # desired accel fill + half = bar_h / 2.0 + fill_h = int(abs(nades) * half) + + if colored: + t = clamp((abs(nades) - 0.75) * 4.0, 0.0, 1.0) + base = rl.Color(255, 255, 255, int(255 * 0.9 * alpha * dim)) + hi = rl.Color(255, 200, 0, int(255 * alpha * dim)) if nades >= 0 else \ + rl.Color(255, 115, 0, int(255 * alpha * dim)) + fill_start = blend_colors(base, hi, t) + fill_end = blend_colors(base, hi, t) + else: + fill_start = fill_end = rl.Color(255, 255, 255, int(255 * 0.32 * alpha * dim)) + + if fill_h > 0: + if nades >= 0: + fy, fh, cap = int(mid_y - fill_h), int(fill_h), "top" + else: + fy, fh, cap = int(mid_y), int(fill_h), "bottom" + + seg_r = float(min(radius, fh * 0.5)) + seg_pts = rounded_cap_segment_pts(float(bx), float(fy), float(bw), float(fh), float(seg_r), cap=cap, segs=9) + + cx = ((bx + bw / 2.0) - rect.x) / rect.width + ex = (bx - rect.x) / rect.width if (nades < 0) else ((bx + bw) - rect.x) / rect.width + + grad = Gradient(start=(cx, 0), end=(ex, 0), colors=[fill_start, fill_end], stops=[0.0, 1.0]) + draw_polygon(rect, seg_pts, gradient=grad) + + # actual accel dot + dot_alpha = int(255 * (0.75 if colored else 0.50) * alpha * dim) + dot_color = rl.Color(255, 255, 255, dot_alpha) + + a_off = int((-naego) * half) + a_y = int(mid_y + a_off) + + dot_r = int((6 + 3 * load) * self._scale) + rl.draw_circle(int(bx + bw / 2), a_y, dot_r, dot_color) diff --git a/selfdrive/ui/ui_state.py b/selfdrive/ui/ui_state.py index f30789a7e..2e49ccdfe 100644 --- a/selfdrive/ui/ui_state.py +++ b/selfdrive/ui/ui_state.py @@ -89,6 +89,7 @@ class UIState(UIStateSP): self.dark_mode: bool = False self.onroad_screen_timeout: bool = False self.force_enable_torque_bar: bool = False + self.enable_accel_bar: bool = False self.has_alert: bool = False self.has_status_change: bool = False self._status_prev: UIStatus = self.status @@ -207,6 +208,7 @@ class UIState(UIStateSP): self.dark_mode = self.params.get_bool("DarkMode") self.onroad_screen_timeout = self.params.get_bool("DisableScreenTimer") self.force_enable_torque_bar = self.params.get_bool("ForceShowTorqueBar") + self.enable_accel_bar = self.params.get_bool("ShowAccelBar") class Device(DeviceSP):