mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-09-03 22:53:44 +08:00
firestar5683
94 lines
3.5 KiB
Python
94 lines
3.5 KiB
Python
import math
|
|
import pyray as rl
|
|
from openpilot.selfdrive.ui.mici.onroad import SIDE_PANEL_WIDTH
|
|
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
|
|
|
|
|
|
def draw_circle_gradient(center_x: float, center_y: float, radius: int,
|
|
top: rl.Color, bottom: rl.Color) -> None:
|
|
# Draw a square with the gradient
|
|
rl.draw_rectangle_gradient_v(int(center_x - radius), int(center_y - radius),
|
|
radius * 2, radius * 2,
|
|
top, bottom)
|
|
|
|
# Paint over square with a ring
|
|
outer_radius = math.ceil(radius * math.sqrt(2)) + 1
|
|
rl.draw_ring(rl.Vector2(int(center_x), int(center_y)), radius, outer_radius,
|
|
0.0, 360.0,
|
|
20, rl.BLACK)
|
|
|
|
|
|
class ConfidenceBall(Widget):
|
|
def __init__(self, demo: bool = False):
|
|
super().__init__()
|
|
self._demo = demo
|
|
self._confidence_filter = FirstOrderFilter(-0.5, 0.5, 1 / gui_app.target_fps)
|
|
|
|
def update_filter(self, value: float):
|
|
self._confidence_filter.update(value)
|
|
|
|
def _update_state(self):
|
|
if self._demo:
|
|
return
|
|
|
|
lateral_ui_active = ui_state.status != UIStatus.DISENGAGED or ui_state.always_on_lateral_active
|
|
|
|
# animate status dot in from bottom
|
|
if not lateral_ui_active:
|
|
self._confidence_filter.update(-0.5)
|
|
else:
|
|
self._confidence_filter.update((1 - max(ui_state.sm['modelV2'].meta.disengagePredictions.brakeDisengageProbs or [1])) *
|
|
(1 - max(ui_state.sm['modelV2'].meta.disengagePredictions.steerOverrideProbs or [1])))
|
|
|
|
def _dot_colors(self) -> tuple[rl.Color, rl.Color]:
|
|
if ui_state.status == UIStatus.ENGAGED or ui_state.always_on_lateral_active or self._demo:
|
|
if self._confidence_filter.x > 0.5:
|
|
top_dot_color = rl.Color(0, 255, 204, 255)
|
|
bottom_dot_color = rl.Color(0, 255, 38, 255)
|
|
elif self._confidence_filter.x > 0.2:
|
|
top_dot_color = rl.Color(255, 200, 0, 255)
|
|
bottom_dot_color = rl.Color(255, 115, 0, 255)
|
|
else:
|
|
top_dot_color = rl.Color(255, 0, 21, 255)
|
|
bottom_dot_color = rl.Color(255, 0, 89, 255)
|
|
|
|
elif ui_state.status == UIStatus.OVERRIDE:
|
|
top_dot_color = rl.Color(255, 255, 255, 255)
|
|
bottom_dot_color = rl.Color(82, 82, 82, 255)
|
|
|
|
else:
|
|
top_dot_color = rl.Color(50, 50, 50, 255)
|
|
bottom_dot_color = rl.Color(13, 13, 13, 255)
|
|
|
|
return top_dot_color, bottom_dot_color
|
|
|
|
def render_static(self, rect: rl.Rectangle, radius: int = 20) -> None:
|
|
self._update_state()
|
|
top_dot_color, bottom_dot_color = self._dot_colors()
|
|
draw_circle_gradient(rect.x + rect.width / 2,
|
|
rect.y + rect.height / 2,
|
|
radius,
|
|
top_dot_color,
|
|
bottom_dot_color)
|
|
|
|
def _render(self, _):
|
|
content_rect = rl.Rectangle(
|
|
self.rect.x + self.rect.width - SIDE_PANEL_WIDTH,
|
|
self.rect.y,
|
|
SIDE_PANEL_WIDTH,
|
|
self.rect.height,
|
|
)
|
|
|
|
status_dot_radius = 24
|
|
dot_height = (1 - self._confidence_filter.x) * (content_rect.height - 2 * status_dot_radius) + status_dot_radius
|
|
dot_height = self._rect.y + dot_height
|
|
|
|
top_dot_color, bottom_dot_color = self._dot_colors()
|
|
|
|
draw_circle_gradient(content_rect.x + content_rect.width - status_dot_radius,
|
|
dot_height, status_dot_radius,
|
|
top_dot_color, bottom_dot_color)
|