diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 31499adf4..674aec923 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -208,7 +208,11 @@ class LatControlTorque(LatControl): roll_offset_fade = np.interp(CS.vEgo, FF_ROLL_OFFSET_FADE_BP, FF_ROLL_OFFSET_FADE_V) roll_compensation = params.roll * ACCELERATION_DUE_TO_GRAVITY * roll_offset_fade - curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0)) + flm_center_deadband_deg = ( + get_flm_full_surface_center_deadband_deg(self.flm_surface_profile_key, CS.vEgo) if flm_surface_active else 0.0 + ) + effective_deadband_deg = self.steering_angle_deadzone_deg + flm_center_deadband_deg + curvature_deadzone = abs(VM.calc_curvature(math.radians(effective_deadband_deg), CS.vEgo, 0.0)) lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2 delay_frames = int(np.clip(lat_delay / self.dt, 1, self.request_buffer_len)) diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index 8e4a45860..e35922176 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py @@ -2709,6 +2709,11 @@ FLM_FULL_SURFACE_SUFFIX_METADATA = { "unwind_taper_right": {"min": 0.0, "max": 12.0, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True}, "center_taper_max": {"min": 0.0, "max": 0.18, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True}, "highway_center_taper_max": {"min": 0.0, "max": 0.18, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True}, + "center_deadband_crawl_deg": {"min": 0.0, "max": 0.30, "precision": 0.005, "deltaType": "absolute", "safeLiveTrial": True}, + "center_deadband_low_deg": {"min": 0.0, "max": 0.30, "precision": 0.005, "deltaType": "absolute", "safeLiveTrial": True}, + "center_deadband_mid_deg": {"min": 0.0, "max": 0.20, "precision": 0.005, "deltaType": "absolute", "safeLiveTrial": True}, + "center_deadband_fast_deg": {"min": 0.0, "max": 0.12, "precision": 0.005, "deltaType": "absolute", "safeLiveTrial": True}, + "center_deadband_highway_deg": {"min": 0.0, "max": 0.08, "precision": 0.005, "deltaType": "absolute", "safeLiveTrial": True}, "turn_in_threshold_reduction_left": {"min": 0.0, "max": 2.00, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True}, "turn_in_threshold_reduction_right": {"min": 0.0, "max": 2.00, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True}, "unwind_threshold_increase_left": {"min": 0.0, "max": 12.0, "precision": 0.001, "deltaType": "absolute", "safeLiveTrial": True}, @@ -2737,6 +2742,11 @@ FLM_FULL_SURFACE_NEUTRAL_DEFAULTS = { "unwind_taper_right": 0.0, "center_taper_max": 0.0, "highway_center_taper_max": 0.0, + "center_deadband_crawl_deg": 0.0, + "center_deadband_low_deg": 0.0, + "center_deadband_mid_deg": 0.0, + "center_deadband_fast_deg": 0.0, + "center_deadband_highway_deg": 0.0, "turn_in_threshold_reduction_left": 0.0, "turn_in_threshold_reduction_right": 0.0, "unwind_threshold_increase_left": 0.0, @@ -2823,6 +2833,24 @@ def get_flm_full_surface_center_taper_scale(profile_key: str | None, desired_lat return 1.0 - min(reduction, 0.20) +def get_flm_full_surface_center_deadband_deg(profile_key: str | None, v_ego: float) -> float: + if not profile_key: + return 0.0 + + suffixes = ( + "center_deadband_crawl_deg", + "center_deadband_low_deg", + "center_deadband_mid_deg", + "center_deadband_fast_deg", + "center_deadband_highway_deg", + ) + values = [ + _flm_vehicle_knob(_flm_profile_symbol(profile_key, suffix), 0.0) + for suffix in suffixes + ] + return float(np.interp(max(v_ego, 0.0), FLM_FRICTION_SPEED_KNOTS, values)) + + def get_flm_full_surface_ff_scale(profile_key: str | None, desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float, include_base_ff: bool = False) -> float: if not profile_key or desired_lateral_accel == 0.0: diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 6aedc6483..fc554c541 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -323,6 +323,50 @@ class TestLatControl: assert get_flm_runtime_overrides() == {} assert get_standard_friction_threshold(10.0) == pytest.approx(base) + def test_flm_center_deadband_curve_interpolates_by_speed(self): + overrides = normalize_flm_overrides({ + "vehicleKnobs": { + "torque_universal.center_deadband_crawl_deg": 0.0, + "torque_universal.center_deadband_low_deg": 0.04, + "torque_universal.center_deadband_mid_deg": 0.08, + "torque_universal.center_deadband_fast_deg": 0.04, + "torque_universal.center_deadband_highway_deg": 0.02, + }, + }) + try: + set_flm_runtime_overrides(overrides) + helper = latcontrol_vehicle_tunes.get_flm_full_surface_center_deadband_deg + assert helper("torque_universal", 0.0) == pytest.approx(0.0) + assert helper("torque_universal", 10.0) == pytest.approx(0.08) + assert helper("torque_universal", 12.5) == pytest.approx(0.06) + assert helper("torque_universal", 25.0) == pytest.approx(0.02) + finally: + clear_flm_runtime_overrides() + + def test_flm_center_deadband_only_reaches_controller_with_active_trial(self, monkeypatch): + controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(GM.CHEVROLET_BOLT_ACC_2022_2023) + symbol = f"{controller.flm_surface_profile_key}.center_deadband_highway_deg" + recorded_deadzones = [] + + def record_deadzone(_error, deadzone, _threshold, _torque_params): + recorded_deadzones.append(deadzone) + return 0.0 + + monkeypatch.setattr(latcontrol_torque, "get_friction", record_deadzone) + starpilot_toggles.flm_active_overrides = {"vehicleKnobs": {symbol: 0.08}} + starpilot_toggles.flm_active_profile_id = "" + starpilot_toggles.flm_trial_applied = False + controller.update(True, CS, VM, params, False, 0.0025, False, 0.2, None, None, starpilot_toggles) + inactive_deadzone = recorded_deadzones[-1] + + starpilot_toggles.flm_active_profile_id = "report:cleanup:recommended" + starpilot_toggles.flm_trial_applied = True + try: + controller.update(True, CS, VM, params, False, 0.0025, False, 0.2, None, None, starpilot_toggles) + assert recorded_deadzones[-1] > inactive_deadzone + finally: + clear_flm_runtime_overrides() + def test_flm_vehicle_knob_override_ioniq6_center_taper(self): baseline = get_ioniq_6_center_taper_scale(0.0, 32.0) overrides = normalize_flm_overrides({ diff --git a/selfdrive/ui/lib/ui_param_cache.py b/selfdrive/ui/lib/ui_param_cache.py index 9324fe9ef..0e2ab2e92 100644 --- a/selfdrive/ui/lib/ui_param_cache.py +++ b/selfdrive/ui/lib/ui_param_cache.py @@ -1,6 +1,6 @@ """Small, shared cache for read-mostly UI parameters. -Parameter reads are file-backed. The BIG UI asks for the same values from +Parameter reads are file-backed. The raylib UIs ask for the same values from multiple widgets during a frame, so a short cache avoids repeated open/read/ close cycles without making settings changes sticky: every write invalidates the affected key immediately and the short TTL bounds visibility of writes @@ -100,7 +100,7 @@ _SHARED_UI_PARAMS: UIParamCache | None = None def shared_ui_params() -> UIParamCache: - """Return the cache shared by BIG UI views and settings panels.""" + """Return the cache shared by raylib UI views and settings panels.""" global _SHARED_UI_PARAMS if _SHARED_UI_PARAMS is None: _SHARED_UI_PARAMS = UIParamCache() diff --git a/selfdrive/ui/mici/onroad/augmented_road_view.py b/selfdrive/ui/mici/onroad/augmented_road_view.py index ef539775f..e4e46709d 100644 --- a/selfdrive/ui/mici/onroad/augmented_road_view.py +++ b/selfdrive/ui/mici/onroad/augmented_road_view.py @@ -19,7 +19,7 @@ from openpilot.selfdrive.ui.mici.onroad.starpilot_status import ( TRAFFIC_COLOR, get_border_color, ) -from openpilot.selfdrive.ui.mici.onroad.cameraview import CameraView +from openpilot.selfdrive.ui.onroad.cameraview import CameraView from openpilot.selfdrive.ui.lib.starpilot_visuals import get_border_width from openpilot.starpilot.common.favorite_slots import is_favorite_action_key, load_favorite_slots, toggle_favorite_slot from openpilot.system.ui.lib.application import FontWeight, gui_app, MousePos, MouseEvent @@ -507,7 +507,7 @@ class StandstillTimerOverlay: return minute_text, second_text def _draw_centered_text(self, rect: rl.Rectangle, text: str, y: float, font: rl.Font, font_size: int, color: rl.Color) -> None: - text_size = rl.measure_text_ex(font, text, font_size, 0) + text_size = measure_text_cached(font, text, font_size) text_pos = rl.Vector2(rect.x + rect.width / 2 - text_size.x / 2, rect.y + y - text_size.y / 2) shadow_pos = rl.Vector2(text_pos.x + 2, text_pos.y + 2) rl.draw_text_ex(font, text, shadow_pos, font_size, 0, rl.Color(0, 0, 0, 170)) @@ -516,7 +516,7 @@ class StandstillTimerOverlay: @staticmethod def _fit_font_size(font: rl.Font, text: str, initial_size: int, max_width: float, minimum_size: int) -> int: font_size = max(initial_size, minimum_size) - while font_size > minimum_size and rl.measure_text_ex(font, text, font_size, 0).x > max_width: + while font_size > minimum_size and measure_text_cached(font, text, font_size).x > max_width: font_size -= 2 return font_size @@ -830,14 +830,16 @@ class AugmentedRoadView(CameraView): def _switch_stream_if_needed(self, sm, camera_view: int): if camera_view == CAMERA_VIEW_NONE: + self._cancel_pending_switch() self._reverse_driver_camera_frames = 0 self._reverse_driver_camera_active = False return + if getattr(self, "_onroad_reentry_pending", False): + self._refresh_available_streams() + if self._update_reverse_driver_camera_state(): - target = DRIVER_CAM - if self.stream_type != target: - self.switch_stream(target) + self.switch_stream(DRIVER_CAM) return wide_available = WIDE_CAM in self.available_streams @@ -859,7 +861,8 @@ class AugmentedRoadView(CameraView): else: target = ROAD_CAM - if self.stream_type != target: + if (getattr(self, "_onroad_reentry_pending", False) or + self.stream_type != target or (self._switching and self._target_stream_type != target)): self.switch_stream(target) def _update_calibration(self): diff --git a/selfdrive/ui/mici/onroad/cameraview.py b/selfdrive/ui/mici/onroad/cameraview.py index 1cfc11a12..1aa0694ff 100644 --- a/selfdrive/ui/mici/onroad/cameraview.py +++ b/selfdrive/ui/mici/onroad/cameraview.py @@ -1,486 +1,5 @@ -import os -import platform -import weakref -import numpy as np -import pyray as rl +"""Compatibility import for the shared CameraView implementation.""" -from msgq.visionipc import VisionIpcClient, VisionStreamType, VisionBuf -from openpilot.common.swaglog import cloudlog -from openpilot.system.hardware import TICI -from openpilot.system.ui.lib.application import gui_app -from openpilot.system.ui.lib.egl import (init_egl, create_egl_image, destroy_egl_image, bind_egl_image_to_texture, - create_external_texture, destroy_external_texture, EGLImage) -from openpilot.system.ui.widgets import Widget -from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus +from openpilot.selfdrive.ui.onroad.cameraview import CameraView -CONNECTION_RETRY_INTERVAL = 0.2 # seconds between connection attempts -MICI_FORCE_TEXTURE_CAMERA = os.getenv("MICI_FORCE_TEXTURE_CAMERA", "0") == "1" - -VERSION = """ -#version 300 es -precision mediump float; -""" -if platform.system() == "Darwin": - VERSION = """ - #version 330 core - """ - - -VERTEX_SHADER = VERSION + """ -in vec3 vertexPosition; -in vec2 vertexTexCoord; -in vec3 vertexNormal; -in vec4 vertexColor; -uniform mat4 mvp; -out vec2 fragTexCoord; -out vec4 fragColor; -void main() { - fragTexCoord = vertexTexCoord; - fragColor = vertexColor; - gl_Position = mvp * vec4(vertexPosition, 1.0); -} -""" - -FRAME_FRAGMENT_SHADER_EXTERNAL = """ - #version 300 es - #extension GL_OES_EGL_image_external_essl3 : enable - precision mediump float; - in vec2 fragTexCoord; - uniform samplerExternalOES texture0; - out vec4 fragColor; - uniform int engaged; - uniform int enhance_driver; - - void main() { - vec4 color = texture(texture0, fragTexCoord); - // Keep the onroad camera feed full-color in every driving state. - if (engaged == 1) { - color.rgb = color.rgb; - } - if (enhance_driver == 1) { - float brightness = 1.1; - color.rgb = color.rgb + 0.15; - color.rgb = clamp((color.rgb - 0.5) * (brightness * 0.8) + 0.5, 0.0, 1.0); - color.rgb = color.rgb * color.rgb * (3.0 - 2.0 * color.rgb); - color.rgb = pow(color.rgb, vec3(0.8)); - } - fragColor = vec4(color.rgb, color.a); - } - """ - -FRAME_FRAGMENT_SHADER_YUV = VERSION + """ - in vec2 fragTexCoord; - uniform sampler2D texture0; - uniform sampler2D texture1; - out vec4 fragColor; - uniform int engaged; - uniform int enhance_driver; - - void main() { - float y = texture(texture0, fragTexCoord).r; - vec2 uv = texture(texture1, fragTexCoord).ra - 0.5; - vec3 rgb = vec3(y + 1.402*uv.y, y - 0.344*uv.x - 0.714*uv.y, y + 1.772*uv.x); - // Keep the onroad camera feed full-color in every driving state. - if (engaged == 1) { - rgb = rgb; - } - // TODO: the images out of camerad need some more correction and - // the ui should apply a gamma curve for the device display - if (enhance_driver == 1) { - float brightness = 1.1; - rgb = rgb + 0.15; - rgb = clamp((rgb - 0.5) * (brightness * 0.8) + 0.5, 0.0, 1.0); - rgb = rgb * rgb * (3.0 - 2.0 * rgb); - rgb = pow(rgb, vec3(0.8)); - } - fragColor = vec4(rgb, 1.0); - } - """ - - -class CameraView(Widget): - def __init__(self, name: str, stream_type: VisionStreamType): - super().__init__() - self._name = name - # Primary stream - self.client = VisionIpcClient(name, stream_type, conflate=True) - self._stream_type = stream_type - self.available_streams: list[VisionStreamType] = [] - - # Target stream for switching - self._target_client: VisionIpcClient | None = None - self._target_stream_type: VisionStreamType | None = None - self._switching: bool = False - - self._texture_needs_update = True - self.last_connection_attempt: float = 0.0 - self._use_egl = TICI and not MICI_FORCE_TEXTURE_CAMERA and init_egl() - if TICI and MICI_FORCE_TEXTURE_CAMERA: - cloudlog.warning("CameraView EGL disabled by MICI_FORCE_TEXTURE_CAMERA, using texture rendering") - elif TICI and not self._use_egl: - cloudlog.error("CameraView EGL init failed, falling back to texture rendering") - - frame_shader = FRAME_FRAGMENT_SHADER_EXTERNAL if self._use_egl else FRAME_FRAGMENT_SHADER_YUV - self.shader = rl.load_shader_from_memory(VERTEX_SHADER, frame_shader) - self._texture1_loc: int = rl.get_shader_location(self.shader, "texture1") if not self._use_egl else -1 - self._engaged_loc = rl.get_shader_location(self.shader, "engaged") - self._engaged_val = rl.ffi.new("int[1]", [1]) - self._enhance_driver_loc = rl.get_shader_location(self.shader, "enhance_driver") - self._enhance_driver_val = rl.ffi.new("int[1]", [1 if stream_type == VisionStreamType.VISION_STREAM_DRIVER else 0]) - - self.frame: VisionBuf | None = None - self._last_frame_id = -1 - self._regressive_frame_count = 0 - self.texture_y: rl.Texture | None = None - self.texture_uv: rl.Texture | None = None - - # EGL resources - self.egl_images: dict[int, EGLImage] = {} - self.egl_texture: rl.Texture | None = None - self._external_texture_id = 0 - - self._placeholder_color: rl.Color | None = None - self._closed = False - - # Initialize EGL for zero-copy rendering when available. - if self._use_egl: - self._create_egl_texture() - - self_ref = weakref.ref(self) - - def offroad_transition_callback(): - if (view := self_ref()) is not None: - view._offroad_transition() - - self._offroad_transition_callback = offroad_transition_callback - ui_state.add_offroad_transition_callback(self._offroad_transition_callback) - - def _offroad_transition(self): - self._reset_camera_connection() - - def _reset_camera_connection(self): - # EGL images and VisionBuf objects both retain the imported camera buffer. - # Release them on every road-state transition instead of pinning the old - # camerad allocation until this view happens to render again. - self._clear_textures() - self.frame = None - self._last_frame_id = -1 - self.available_streams.clear() - self.client = VisionIpcClient(self._name, self._stream_type, conflate=True) - self._target_client = None - self._target_stream_type = None - self._switching = False - self._texture_needs_update = True - self.last_connection_attempt = 0.0 - - def _set_placeholder_color(self, color: rl.Color): - """Set a placeholder color to be drawn when no frame is available.""" - self._placeholder_color = color - - def switch_stream(self, stream_type: VisionStreamType) -> None: - if self._stream_type == stream_type: - return - - if self._switching and self._target_stream_type == stream_type: - return - - cloudlog.debug(f'Preparing switch from {self._stream_type} to {stream_type}') - - if self._target_client: - del self._target_client - - self._target_stream_type = stream_type - self._target_client = VisionIpcClient(self._name, stream_type, conflate=True) - self._switching = True - - @property - def stream_type(self) -> VisionStreamType: - return self._stream_type - - def close(self) -> None: - if self._closed: - return - self._closed = True - - callback = getattr(self, "_offroad_transition_callback", None) - if callback is not None: - ui_state.remove_offroad_transition_callback(callback) - self._offroad_transition_callback = None - self._clear_textures() - - # Clean up shader - if self.shader and self.shader.id: - rl.unload_shader(self.shader) - self.shader.id = 0 - - self.frame = None - self._last_frame_id = -1 - self.available_streams.clear() - self.client = None - self._target_client = None - - def __del__(self): - self.close() - - def _calc_frame_matrix(self, rect: rl.Rectangle) -> np.ndarray: - if not self.frame: - return np.eye(3) - - # Calculate aspect ratios - widget_aspect_ratio = rect.width / rect.height - frame_aspect_ratio = self.frame.width / self.frame.height - - # Calculate scaling factors to maintain aspect ratio - zx = min(frame_aspect_ratio / widget_aspect_ratio, 1.0) - zy = min(widget_aspect_ratio / frame_aspect_ratio, 1.0) - - return np.array([ - [zx, 0.0, 0.0], - [0.0, zy, 0.0], - [0.0, 0.0, 1.0] - ]) - - def _render(self, rect: rl.Rectangle): - if self._switching: - self._handle_switch() - - if not self._ensure_connection(): - self._draw_placeholder(rect) - return - - if self._use_egl: - self._observe_displayed_frame() - - # Try to get a new buffer without blocking - buffer = self.client.recv(timeout_ms=0) - if buffer: - self._accept_frame(buffer, self.client.frame_id) - elif not self.client.is_connected(): - # ensure we clear the displayed frame when the connection is lost - self.frame = None - - if not self.frame: - self._draw_placeholder(rect) - return - - transform = self._calc_frame_matrix(rect) - src_rect = rl.Rectangle(0, 0, float(self.frame.width), float(self.frame.height)) - # Flip driver camera horizontally - if self._stream_type == VisionStreamType.VISION_STREAM_DRIVER: - src_rect.width = -src_rect.width - - # Calculate scale - scale_x = rect.width * transform[0, 0] # zx - scale_y = rect.height * transform[1, 1] # zy - - # Calculate base position (centered) - x_offset = rect.x + (rect.width - scale_x) / 2 - y_offset = rect.y + (rect.height - scale_y) / 2 - - x_offset += transform[0, 2] * rect.width / 2 - y_offset += transform[1, 2] * rect.height / 2 - - dst_rect = rl.Rectangle(x_offset, y_offset, scale_x, scale_y) - - # Render with appropriate method - if self._use_egl: - self._render_egl(src_rect, dst_rect) - else: - self._render_textures(src_rect, dst_rect) - - def _draw_placeholder(self, rect: rl.Rectangle): - if self._placeholder_color: - rl.draw_rectangle_rec(rect, self._placeholder_color) - - def _observe_displayed_frame(self) -> None: - if self.frame is not None: - client_frame_id = getattr(self.client, "frame_id", -1) if hasattr(self, "client") and self.client is not None else -1 - frame_id = getattr(self.frame, "frame_id", client_frame_id) - self._last_frame_id = max(self._last_frame_id, int(frame_id)) - - def _accept_frame(self, frame: VisionBuf, packet_frame_id: int) -> bool: - content_frame_id = int(getattr(frame, "frame_id", packet_frame_id)) - if content_frame_id < self._last_frame_id: - self._regressive_frame_count += 1 - if self._regressive_frame_count == 1 or self._regressive_frame_count % 100 == 0: - message = f"Dropping regressive {self._name} frame: content={content_frame_id}, packet={packet_frame_id}, " - message += f"displayed={self._last_frame_id}, idx={frame.idx}, count={self._regressive_frame_count}" - cloudlog.warning(message) - return False - - self.frame = frame - self._last_frame_id = content_frame_id - self._texture_needs_update = True - return True - - def _render_egl(self, src_rect: rl.Rectangle, dst_rect: rl.Rectangle) -> None: - """Render using EGL for direct buffer access""" - if self.frame is None or self.egl_texture is None or not self._external_texture_id: - return - - idx = self.frame.idx - egl_image = self.egl_images.get(idx) - - # Create EGL image if needed - if egl_image is None: - egl_image = create_egl_image(self.frame.width, self.frame.height, self.frame.stride, self.frame.fd, self.frame.uv_offset) - if egl_image: - self.egl_images[idx] = egl_image - else: - return - - # Update texture dimensions to match current frame - self.egl_texture.width = self.frame.width - self.egl_texture.height = self.frame.height - - # Bind the EGL image to our texture - bind_egl_image_to_texture(self._external_texture_id, egl_image) - - # Render with shader - rl.begin_shader_mode(self.shader) - self._update_texture_color_filtering() - rl.draw_texture_pro(self.egl_texture, src_rect, dst_rect, rl.Vector2(0, 0), 0.0, rl.WHITE) - rl.end_shader_mode() - - def _render_textures(self, src_rect: rl.Rectangle, dst_rect: rl.Rectangle) -> None: - """Render using texture copies""" - if not self.texture_y or not self.texture_uv or self.frame is None: - return - - # Update textures with new frame data - if self._texture_needs_update: - y_data = self.frame.data[: self.frame.uv_offset] - uv_data = self.frame.data[self.frame.uv_offset:] - - rl.update_texture(self.texture_y, rl.ffi.cast("void *", y_data.ctypes.data)) - rl.update_texture(self.texture_uv, rl.ffi.cast("void *", uv_data.ctypes.data)) - self._texture_needs_update = False - - # Render with shader - rl.begin_shader_mode(self.shader) - self._update_texture_color_filtering() - rl.set_shader_value_texture(self.shader, self._texture1_loc, self.texture_uv) - rl.draw_texture_pro(self.texture_y, src_rect, dst_rect, rl.Vector2(0, 0), 0.0, rl.WHITE) - rl.end_shader_mode() - - def _update_texture_color_filtering(self): - self._engaged_val[0] = 1 if ui_state.status != UIStatus.DISENGAGED else 0 - if self._engaged_loc >= 0: - rl.set_shader_value(self.shader, self._engaged_loc, self._engaged_val, rl.ShaderUniformDataType.SHADER_UNIFORM_INT) - if self._enhance_driver_loc >= 0: - rl.set_shader_value(self.shader, self._enhance_driver_loc, self._enhance_driver_val, rl.ShaderUniformDataType.SHADER_UNIFORM_INT) - - def _ensure_connection(self) -> bool: - if not self.client.is_connected(): - self.frame = None - self._last_frame_id = -1 - self.available_streams.clear() - - # Throttle connection attempts - current_time = rl.get_time() - if current_time - self.last_connection_attempt < CONNECTION_RETRY_INTERVAL: - return False - self.last_connection_attempt = current_time - - # A GL texture can retain the last EGL image after camerad exits. Release - # it before connect() frees and replaces the client's imported buffers. - self._clear_textures() - if not self.client.connect(False) or not self.client.num_buffers: - return False - - cloudlog.debug(f"Connected to {self._name} stream: {self._stream_type}, buffers: {self.client.num_buffers}") - self._initialize_textures() - self.available_streams = self.client.available_streams(self._name, block=False) - - return True - - def _handle_switch(self) -> None: - """Check if target stream is ready and switch immediately.""" - if not self._target_client or not self._switching: - return - - # Try to connect target if needed - if not self._target_client.is_connected(): - if not self._target_client.connect(False) or not self._target_client.num_buffers: - return - - cloudlog.debug(f"Target stream connected: {self._target_stream_type}") - - # Check if target has frames ready - target_frame = self._target_client.recv(timeout_ms=0) - if target_frame: - self.frame = target_frame # Update current frame to target frame - self._complete_switch() - - def _complete_switch(self) -> None: - """Instantly switch to target stream.""" - cloudlog.debug(f"Switching to {self._target_stream_type}") - # Delete the GL texture before releasing the old client. Merely destroying - # the EGLImage handle leaves its storage alive while a texture sibling exists. - self._clear_textures() - - # Switch to target - self.client = self._target_client - self._stream_type = self._target_stream_type - enhance_driver_val = getattr(self, "_enhance_driver_val", None) - if enhance_driver_val is not None: - enhance_driver_val[0] = 1 if self._stream_type == VisionStreamType.VISION_STREAM_DRIVER else 0 - client_frame_id = getattr(self.client, "frame_id", -1) if hasattr(self, "client") and self.client is not None else -1 - frame = getattr(self, "frame", None) - self._last_frame_id = int(getattr(frame, "frame_id", client_frame_id)) if frame is not None else -1 - self._texture_needs_update = True - - # Reset state - self._target_client = None - self._target_stream_type = None - self._switching = False - - # Initialize textures for new stream - self._initialize_textures() - - def _initialize_textures(self): - self._clear_textures() - if self._use_egl: - self._create_egl_texture() - else: - self.texture_y = rl.load_texture_from_image(rl.Image(None, int(self.client.stride), - int(self.client.height), 1, rl.PixelFormat.PIXELFORMAT_UNCOMPRESSED_GRAYSCALE)) - self.texture_uv = rl.load_texture_from_image(rl.Image(None, int(self.client.stride // 2), - int(self.client.height // 2), 1, rl.PixelFormat.PIXELFORMAT_UNCOMPRESSED_GRAY_ALPHA)) - - def _create_egl_texture(self): - temp_image = rl.gen_image_color(1, 1, rl.BLACK) - self.egl_texture = rl.load_texture_from_image(temp_image) - rl.unload_image(temp_image) - self._external_texture_id = create_external_texture() - if not self._external_texture_id: - raise RuntimeError("Failed to create external camera texture") - - def _clear_textures(self): - if self.texture_y and self.texture_y.id: - rl.unload_texture(self.texture_y) - self.texture_y = None - - if self.texture_uv and self.texture_uv.id: - rl.unload_texture(self.texture_uv) - self.texture_uv = None - - if self._use_egl: - if self._external_texture_id: - destroy_external_texture(self._external_texture_id) - self._external_texture_id = 0 - - if self.egl_texture and self.egl_texture.id: - rl.unload_texture(self.egl_texture) - self.egl_texture = None - - for data in self.egl_images.values(): - destroy_egl_image(data) - self.egl_images = {} - - -if __name__ == "__main__": - gui_app.init_window("camera view") - road = CameraView("camerad", VisionStreamType.VISION_STREAM_ROAD) - for _ in gui_app.render(): - road.render(rl.Rectangle(0, 0, gui_app.width, gui_app.height)) +__all__ = ["CameraView"] diff --git a/selfdrive/ui/mici/onroad/driver_camera_dialog.py b/selfdrive/ui/mici/onroad/driver_camera_dialog.py index 7ac9953e3..367dc783f 100644 --- a/selfdrive/ui/mici/onroad/driver_camera_dialog.py +++ b/selfdrive/ui/mici/onroad/driver_camera_dialog.py @@ -1,7 +1,7 @@ import pyray as rl -from cereal import car, log, messaging +from cereal import log, messaging from msgq.visionipc import VisionStreamType -from openpilot.selfdrive.ui.mici.onroad.cameraview import CameraView +from openpilot.selfdrive.ui.onroad.cameraview import CameraView from openpilot.selfdrive.ui.mici.onroad.driver_state import DriverStateRenderer from openpilot.selfdrive.ui.ui_state import ui_state, device from openpilot.system.ui.lib.application import gui_app, FontWeight diff --git a/selfdrive/ui/mici/tests/test_camera_cleanup.py b/selfdrive/ui/mici/tests/test_camera_cleanup.py index 081806d5f..1a6412fa1 100644 --- a/selfdrive/ui/mici/tests/test_camera_cleanup.py +++ b/selfdrive/ui/mici/tests/test_camera_cleanup.py @@ -2,14 +2,13 @@ import gc from types import SimpleNamespace import weakref -import pytest - -from openpilot.selfdrive.ui.mici.onroad import cameraview as mici_cameraview from openpilot.selfdrive.ui.onroad import cameraview as big_cameraview +from openpilot.selfdrive.ui.mici.onroad import augmented_road_view as mici_augmented_road_view -@pytest.mark.parametrize("module", (mici_cameraview, big_cameraview)) -def test_road_transition_releases_camera_buffers(monkeypatch, module): +def test_road_transition_releases_camera_buffers(monkeypatch): + module = big_cameraview + class FakeClient: pass @@ -25,12 +24,12 @@ def test_road_transition_releases_camera_buffers(monkeypatch, module): view._target_stream_type = object() view._switching = True view._texture_needs_update = False + view._regressive_frame_count = 2 view.last_connection_attempt = 123.0 view._closed = True cleared = [] view._clear_textures = lambda: cleared.append(True) - monkeypatch.setattr(module, "VisionIpcClient", lambda *_args, **_kwargs: FakeClient()) del old_client view._offroad_transition() @@ -44,11 +43,13 @@ def test_road_transition_releases_camera_buffers(monkeypatch, module): assert view._target_stream_type is None assert view._switching is False assert view._texture_needs_update + assert view._regressive_frame_count == 0 assert view.last_connection_attempt == 0.0 -@pytest.mark.parametrize("module", (mici_cameraview, big_cameraview)) -def test_transition_callback_does_not_retain_camera_view(monkeypatch, module): +def test_transition_callback_does_not_retain_camera_view(monkeypatch): + module = big_cameraview + class FakeClient: pass @@ -72,61 +73,50 @@ def test_transition_callback_does_not_retain_camera_view(monkeypatch, module): assert callbacks == [] -@pytest.mark.parametrize("module", (mici_cameraview, big_cameraview)) -def test_stream_switch_releases_graphics_before_old_client(module): +def test_stream_switch_releases_graphics_before_old_client(): + module = big_cameraview + events = [] class FakeClient: pass + class FakeFrame: + pass + view = module.CameraView.__new__(module.CameraView) - view.client = FakeClient() - old_client_finalizer = weakref.finalize(view.client, events.append, "client") + old_client = FakeClient() + old_client_finalizer = weakref.finalize(old_client, events.append, "client") + old_frame = FakeFrame() + old_frame.frame_id = 10 + old_frame.owner = old_client + old_frame_finalizer = weakref.finalize(old_frame, events.append, "frame") + view.client = old_client view._target_client = FakeClient() view._target_stream_type = object() view._stream_type = object() view._switching = True + view.frame = old_frame + view._regressive_frame_count = 2 view._texture_needs_update = False view._closed = True view._clear_textures = lambda: events.append("graphics") view._initialize_textures = lambda: events.append("initialize") + del old_frame + del old_client - view._complete_switch() + view._complete_switch(SimpleNamespace(frame_id=11)) gc.collect() assert old_client_finalizer.alive is False - assert events == ["graphics", "client", "initialize"] + assert old_frame_finalizer.alive is False + assert events == ["graphics", "frame", "client", "initialize"] + assert view._regressive_frame_count == 0 -@pytest.mark.parametrize(("target_stream", "expected"), ( - (mici_cameraview.VisionStreamType.VISION_STREAM_DRIVER, 1), - (mici_cameraview.VisionStreamType.VISION_STREAM_ROAD, 0), - (mici_cameraview.VisionStreamType.VISION_STREAM_WIDE_ROAD, 0), -)) -def test_mici_stream_switch_updates_driver_enhancement(target_stream, expected): - class FakeClient: - frame_id = 42 +def test_egl_cleanup_deletes_texture_before_images(monkeypatch): + module = big_cameraview - view = mici_cameraview.CameraView.__new__(mici_cameraview.CameraView) - view.client = FakeClient() - view._target_client = FakeClient() - view._target_stream_type = target_stream - view._stream_type = mici_cameraview.VisionStreamType.VISION_STREAM_DRIVER - view._switching = True - view._texture_needs_update = False - view._enhance_driver_val = [-1] - view._closed = True - view._clear_textures = lambda: None - view._initialize_textures = lambda: None - - view._complete_switch() - - assert view._enhance_driver_val[0] == expected - assert view._last_frame_id == -1 - - -@pytest.mark.parametrize("module", (mici_cameraview, big_cameraview)) -def test_egl_cleanup_deletes_texture_before_images(monkeypatch, module): events = [] view = module.CameraView.__new__(module.CameraView) view.texture_y = None @@ -136,10 +126,7 @@ def test_egl_cleanup_deletes_texture_before_images(monkeypatch, module): view.egl_images = {0: object(), 1: object()} view._closed = True - if module is mici_cameraview: - view._use_egl = True - else: - monkeypatch.setattr(module, "TICI", True) + view._use_egl = True monkeypatch.setattr(module.rl, "unload_texture", lambda _texture: events.append("texture")) monkeypatch.setattr(module, "destroy_external_texture", lambda _texture: events.append("external")) @@ -153,28 +140,72 @@ def test_egl_cleanup_deletes_texture_before_images(monkeypatch, module): assert view.egl_images == {} -@pytest.mark.parametrize("module", (mici_cameraview, big_cameraview)) -def test_egl_render_keeps_external_and_raylib_texture_targets_separate(monkeypatch, module): - frame = SimpleNamespace(idx=3, width=1928, height=1208, stride=2048, fd=9, uv_offset=2473984) - image = object() +def test_egl_cleanup_synchronizes_after_backend_switch(monkeypatch): + module = big_cameraview + + events = [] view = module.CameraView.__new__(module.CameraView) - view.frame = frame - view.egl_texture = SimpleNamespace(id=7, width=1, height=1) + view.texture_y = None + view.texture_uv = None + view.egl_texture = SimpleNamespace(id=7) view._external_texture_id = 11 - view.egl_images = {frame.idx: image} - view.shader = object() + view.egl_images = {0: object()} + view._use_egl = False view._closed = True - view._update_texture_color_filtering = lambda: None - bound = [] - drawn = [] - monkeypatch.setattr(module, "bind_egl_image_to_texture", lambda texture_id, egl_image: bound.append((texture_id, egl_image))) - monkeypatch.setattr(module.rl, "begin_shader_mode", lambda _shader: None) - monkeypatch.setattr(module.rl, "end_shader_mode", lambda: None) - monkeypatch.setattr(module.rl, "draw_texture_pro", lambda texture, *_args: drawn.append(texture.id)) + monkeypatch.setattr(module, "is_egl_initialized", lambda: True) + monkeypatch.setattr(module.rl, "rl_draw_render_batch_active", lambda: events.append("flush")) + monkeypatch.setattr(module, "finish_gl", lambda: events.append("finish")) + monkeypatch.setattr(module.rl, "unload_texture", lambda _texture: events.append("texture")) + monkeypatch.setattr(module, "destroy_external_texture", lambda _texture: events.append("external")) + monkeypatch.setattr(module, "destroy_egl_image", lambda _image: events.append("image")) - rect = SimpleNamespace() - view._render_egl(rect, rect) + view._clear_textures() - assert bound == [(11, image)] - assert drawn == [7] + assert events == ["flush", "finish", "external", "texture", "image"] + + +def test_reverse_activation_cancels_mismatched_pending_switch(): + view = mici_augmented_road_view.AugmentedRoadView.__new__(mici_augmented_road_view.AugmentedRoadView) + view._stream_type = mici_augmented_road_view.DRIVER_CAM + view._target_stream_type = mici_augmented_road_view.WIDE_CAM + view._target_client = object() + view._switching = True + view._closed = True + view._update_reverse_driver_camera_state = lambda: True + + view._switch_stream_if_needed(None, mici_augmented_road_view.CAMERA_VIEW_AUTO) + + assert view._target_client is None + assert view._target_stream_type is None + assert not view._switching + + +def test_onroad_transition_marks_camera_reentry(monkeypatch): + module = big_cameraview + + class FakeClient: + pass + + view = module.CameraView.__new__(module.CameraView) + view._name = "camerad" + view._stream_type = object() + view.client = FakeClient() + view.frame = None + view.available_streams = [] + view._target_client = None + view._target_stream_type = None + view._switching = False + view._texture_needs_update = False + view._regressive_frame_count = 1 + view._closed = True + view._onroad_reentry_pending = False + view._reentry_stream_selected = False + view._clear_textures = lambda: None + + monkeypatch.setattr(module.ui_state, "is_onroad", lambda: True) + + view._offroad_transition() + + assert view._onroad_reentry_pending + assert not view._reentry_stream_selected diff --git a/selfdrive/ui/onroad/augmented_road_view.py b/selfdrive/ui/onroad/augmented_road_view.py index 36c475f8c..a07f4419b 100644 --- a/selfdrive/ui/onroad/augmented_road_view.py +++ b/selfdrive/ui/onroad/augmented_road_view.py @@ -200,10 +200,14 @@ class AugmentedRoadView(CameraView): def _switch_stream_if_needed(self, sm, camera_view: int): if camera_view == CAMERA_VIEW_NONE: + self._cancel_pending_switch() self._reverse_driver_camera_frames = 0 self._reverse_driver_camera_active = False return + if getattr(self, "_onroad_reentry_pending", False): + self._refresh_available_streams() + if self._update_reverse_driver_camera_state(): target = DRIVER_CAM elif camera_view == CAMERA_VIEW_DRIVER: @@ -224,7 +228,8 @@ class AugmentedRoadView(CameraView): else: target = ROAD_CAM - if self.stream_type != target: + if (getattr(self, "_onroad_reentry_pending", False) or + self.stream_type != target or (self._switching and self._target_stream_type != target)): self.switch_stream(target) def _update_calibration(self): diff --git a/selfdrive/ui/onroad/cameraview.py b/selfdrive/ui/onroad/cameraview.py index e3cdeafbe..624fd1300 100644 --- a/selfdrive/ui/onroad/cameraview.py +++ b/selfdrive/ui/onroad/cameraview.py @@ -1,3 +1,4 @@ +import os import platform import weakref import numpy as np @@ -7,12 +8,17 @@ from msgq.visionipc import VisionIpcClient, VisionStreamType, VisionBuf from openpilot.common.swaglog import cloudlog from openpilot.system.hardware import TICI from openpilot.system.ui.lib.application import gui_app -from openpilot.system.ui.lib.egl import (init_egl, create_egl_image, destroy_egl_image, bind_egl_image_to_texture, - create_external_texture, destroy_external_texture, EGLImage) +from openpilot.system.ui.lib.egl import ( + init_egl, is_egl_initialized, finish_gl, create_egl_image, destroy_egl_image, + bind_egl_image_to_texture, create_external_texture, destroy_external_texture, EGLImage, +) from openpilot.system.ui.widgets import Widget from openpilot.selfdrive.ui.ui_state import ui_state CONNECTION_RETRY_INTERVAL = 0.2 # seconds between connection attempts +MICI_FORCE_TEXTURE_CAMERA = os.getenv("MICI_FORCE_TEXTURE_CAMERA", "0") == "1" +# One stale frame can be normal ring-buffer reuse; repeated consecutive regressions demote EGL. +EGL_REGRESSIVE_FRAME_FALLBACK_THRESHOLD = 3 VERSION = """ #version 300 es @@ -39,32 +45,48 @@ void main() { } """ -# Choose fragment shader based on platform capabilities -if TICI: - FRAME_FRAGMENT_SHADER = """ - #version 300 es - #extension GL_OES_EGL_image_external_essl3 : enable - precision mediump float; - in vec2 fragTexCoord; - uniform samplerExternalOES texture0; - out vec4 fragColor; - void main() { - vec4 color = texture(texture0, fragTexCoord); - fragColor = vec4(pow(color.rgb, vec3(1.0/1.28)), color.a); +FRAME_FRAGMENT_SHADER_EXTERNAL = """ + #version 300 es + #extension GL_OES_EGL_image_external_essl3 : enable + precision mediump float; + in vec2 fragTexCoord; + uniform samplerExternalOES texture0; + uniform int enhance_driver; + out vec4 fragColor; + void main() { + vec4 color = texture(texture0, fragTexCoord); + color.rgb = pow(color.rgb, vec3(1.0/1.28)); + if (enhance_driver == 1) { + float brightness = 1.1; + color.rgb = color.rgb + 0.15; + color.rgb = clamp((color.rgb - 0.5) * (brightness * 0.8) + 0.5, 0.0, 1.0); + color.rgb = color.rgb * color.rgb * (3.0 - 2.0 * color.rgb); + color.rgb = pow(color.rgb, vec3(0.8)); } - """ -else: - FRAME_FRAGMENT_SHADER = VERSION + """ - in vec2 fragTexCoord; - uniform sampler2D texture0; - uniform sampler2D texture1; - out vec4 fragColor; - void main() { - float y = texture(texture0, fragTexCoord).r; - vec2 uv = texture(texture1, fragTexCoord).ra - 0.5; - fragColor = vec4(y + 1.402*uv.y, y - 0.344*uv.x - 0.714*uv.y, y + 1.772*uv.x, 1.0); + fragColor = vec4(color.rgb, color.a); + } + """ + +FRAME_FRAGMENT_SHADER_YUV = VERSION + """ + in vec2 fragTexCoord; + uniform sampler2D texture0; + uniform sampler2D texture1; + uniform int enhance_driver; + out vec4 fragColor; + void main() { + float y = texture(texture0, fragTexCoord).r; + vec2 uv = texture(texture1, fragTexCoord).ra - 0.5; + vec3 rgb = vec3(y + 1.402*uv.y, y - 0.344*uv.x - 0.714*uv.y, y + 1.772*uv.x); + if (enhance_driver == 1) { + float brightness = 1.1; + rgb = rgb + 0.15; + rgb = clamp((rgb - 0.5) * (brightness * 0.8) + 0.5, 0.0, 1.0); + rgb = rgb * rgb * (3.0 - 2.0 * rgb); + rgb = pow(rgb, vec3(0.8)); } - """ + fragColor = vec4(rgb, 1.0); + } + """ class CameraView(Widget): @@ -72,7 +94,7 @@ class CameraView(Widget): super().__init__() self._name = name # Primary stream - self.client = VisionIpcClient(name, stream_type, conflate=True) + self.client: VisionIpcClient | None = None self._stream_type = stream_type self.available_streams: list[VisionStreamType] = [] @@ -83,8 +105,18 @@ class CameraView(Widget): self._texture_needs_update = True self.last_connection_attempt: float = 0.0 - self.shader = rl.load_shader_from_memory(VERTEX_SHADER, FRAME_FRAGMENT_SHADER) - self._texture1_loc: int = rl.get_shader_location(self.shader, "texture1") if not TICI else -1 + self._use_egl = TICI and not MICI_FORCE_TEXTURE_CAMERA and init_egl() + if TICI and MICI_FORCE_TEXTURE_CAMERA: + cloudlog.warning("CameraView EGL disabled by MICI_FORCE_TEXTURE_CAMERA, using texture rendering") + elif TICI and not self._use_egl: + cloudlog.error("CameraView EGL init failed, falling back to texture rendering") + + self._enhance_driver_val = rl.ffi.new("int[1]", [0]) + self._load_frame_shader() + if self._use_egl and not self.shader.id: + cloudlog.error("CameraView EGL shader failed, falling back to texture rendering") + self._use_egl = False + self._load_frame_shader() self.frame: VisionBuf | None = None self._last_frame_id = -1 @@ -99,12 +131,17 @@ class CameraView(Widget): self._placeholder_color: rl.Color | None = None self._closed = False + self._onroad_reentry_pending = False + self._reentry_stream_selected = False - # Initialize EGL for zero-copy rendering on TICI - if TICI: - if not init_egl(): - raise RuntimeError("Failed to initialize EGL") - self._create_egl_texture() + if self._use_egl and not self._create_egl_texture(): + cloudlog.error("CameraView EGL texture creation failed, falling back to texture rendering") + self._use_egl = False + if self.shader and self.shader.id: + rl.unload_shader(self.shader) + self.shader.id = 0 + self._load_frame_shader() + cloudlog.info(f"CameraView using {'EGL zero-copy' if self._use_egl else 'texture-copy'} rendering for {stream_type}") self_ref = weakref.ref(self) @@ -118,30 +155,43 @@ class CameraView(Widget): def _offroad_transition(self): self._reset_camera_connection() - def _reset_camera_connection(self): - # EGL images and VisionBuf objects both retain the imported camera buffer. - # Release them on every road-state transition instead of pinning the old - # camerad allocation until this view happens to render again. + def _retire_active_client(self) -> None: + """Release graphics, frame, and client as one camera generation.""" self._clear_textures() self.frame = None + self.client = None + + def _reset_camera_connection(self): + self._cancel_pending_switch() + self._retire_active_client() self._last_frame_id = -1 + self._regressive_frame_count = 0 self.available_streams.clear() - self.client = VisionIpcClient(self._name, self._stream_type, conflate=True) - self._target_client = None - self._target_stream_type = None - self._switching = False self._texture_needs_update = True self.last_connection_attempt = 0.0 + self._onroad_reentry_pending = ui_state.is_onroad() + self._reentry_stream_selected = False def _set_placeholder_color(self, color: rl.Color): """Set a placeholder color to be drawn when no frame is available.""" self._placeholder_color = color + def _refresh_available_streams(self) -> None: + streams = VisionIpcClient.available_streams(self._name, block=False) + if streams: + self.available_streams = list(streams) + def switch_stream(self, stream_type: VisionStreamType) -> None: - if self._stream_type == stream_type: + if getattr(self, "_onroad_reentry_pending", False): + self._select_reentry_stream(stream_type) return - if self._switching and self._target_stream_type == stream_type: + if self._switching: + if self._target_stream_type == stream_type: + return + self._cancel_pending_switch() + + if self._stream_type == stream_type: return cloudlog.debug(f'Preparing switch from {self._stream_type} to {stream_type}') @@ -153,6 +203,32 @@ class CameraView(Widget): self._target_client = VisionIpcClient(self._name, stream_type, conflate=True) self._switching = True + def _cancel_pending_switch(self) -> None: + if self._target_client is not None: + cloudlog.debug(f"Cancelling pending camera switch to {self._target_stream_type}") + self._target_client = None + self._target_stream_type = None + self._switching = False + + def _discard_pending_client(self) -> None: + """Discard a failed candidate while retaining the requested stream.""" + self._target_client = None + self._switching = False + + def _select_reentry_stream(self, stream_type: VisionStreamType) -> None: + """Select the desired stream before displaying any post-transition frame.""" + self._cancel_pending_switch() + + if self._stream_type != stream_type: + self._retire_active_client() + self._stream_type = stream_type + + self.frame = None + self._last_frame_id = -1 + self._regressive_frame_count = 0 + self._texture_needs_update = True + self._reentry_stream_selected = True + @property def stream_type(self) -> VisionStreamType: return self._stream_type @@ -166,17 +242,19 @@ class CameraView(Widget): if callback is not None: ui_state.remove_offroad_transition_callback(callback) self._offroad_transition_callback = None - self._clear_textures() + self._cancel_pending_switch() + self._retire_active_client() # Clean up shader if self.shader and self.shader.id: rl.unload_shader(self.shader) + self.shader.id = 0 self.frame = None self._last_frame_id = -1 self.available_streams.clear() - self.client = None - self._target_client = None + self._onroad_reentry_pending = False + self._reentry_stream_selected = False def __del__(self): self.close() @@ -203,13 +281,15 @@ class CameraView(Widget): if self._switching: self._handle_switch() + if self._onroad_reentry_pending and not self._reentry_stream_selected: + # Standalone CameraView users have no higher-level stream selector. + self._select_reentry_stream(self._stream_type) + if not self._ensure_connection(): self._draw_placeholder(rect) return - # An EGL image references camerad's reusable ring-buffer slot. Account for - # that slot advancing before accepting another (possibly older) slot. - if TICI: + if self._use_egl: self._observe_displayed_frame() # Try to get a new buffer without blocking @@ -243,16 +323,34 @@ class CameraView(Widget): dst_rect = rl.Rectangle(x_offset, y_offset, scale_x, scale_y) - # Render with appropriate method - if TICI: - self._render_egl(src_rect, dst_rect) - else: + if self._use_egl: + try: + rendered = self._render_egl(src_rect, dst_rect) + except Exception: + cloudlog.exception("CameraView EGL rendering failed") + rendered = False + if not rendered: + self._fallback_to_textures("EGL frame rendering failed") + + if not self._use_egl: self._render_textures(src_rect, dst_rect) def _draw_placeholder(self, rect: rl.Rectangle): if self._placeholder_color: rl.draw_rectangle_rec(rect, self._placeholder_color) + def _load_frame_shader(self) -> None: + frame_shader = FRAME_FRAGMENT_SHADER_EXTERNAL if self._use_egl else FRAME_FRAGMENT_SHADER_YUV + self.shader = rl.load_shader_from_memory(VERTEX_SHADER, frame_shader) + self._texture1_loc = -1 if self._use_egl else rl.get_shader_location(self.shader, "texture1") + self._enhance_driver_loc = rl.get_shader_location(self.shader, "enhance_driver") + + def _update_shader_state(self) -> None: + self._enhance_driver_val[0] = 1 if self._stream_type == VisionStreamType.VISION_STREAM_DRIVER else 0 + if self._enhance_driver_loc >= 0: + rl.set_shader_value(self.shader, self._enhance_driver_loc, self._enhance_driver_val, + rl.ShaderUniformDataType.SHADER_UNIFORM_INT) + def _observe_displayed_frame(self) -> None: if self.frame is not None: client_frame_id = getattr(self.client, "frame_id", -1) if hasattr(self, "client") and self.client is not None else -1 @@ -261,50 +359,88 @@ class CameraView(Widget): def _accept_frame(self, frame: VisionBuf, packet_frame_id: int) -> bool: content_frame_id = int(getattr(frame, "frame_id", packet_frame_id)) + if content_frame_id != packet_frame_id: + cloudlog.debug( + f"Dropping inconsistent {self._name} frame: content={content_frame_id}, packet={packet_frame_id}" + ) + return False if content_frame_id < self._last_frame_id: self._regressive_frame_count += 1 if self._regressive_frame_count == 1 or self._regressive_frame_count % 100 == 0: message = f"Dropping regressive {self._name} frame: content={content_frame_id}, packet={packet_frame_id}, " message += f"displayed={self._last_frame_id}, idx={frame.idx}, count={self._regressive_frame_count}" cloudlog.warning(message) + if getattr(self, "_use_egl", False) and self._regressive_frame_count >= EGL_REGRESSIVE_FRAME_FALLBACK_THRESHOLD: + self._fallback_to_textures("repeated regressive frames") return False self.frame = frame self._last_frame_id = content_frame_id + self._regressive_frame_count = 0 self._texture_needs_update = True + self._onroad_reentry_pending = False + self._reentry_stream_selected = False return True - def _render_egl(self, src_rect: rl.Rectangle, dst_rect: rl.Rectangle) -> None: - """Render using EGL for direct buffer access""" - if self.frame is None or self.egl_texture is None or not self._external_texture_id: - return + def _render_egl(self, src_rect: rl.Rectangle, dst_rect: rl.Rectangle) -> bool: + """Render using EGL for direct buffer access.""" + if self.frame is None or self.egl_texture is None or not self.egl_texture.id or not self._external_texture_id: + return False idx = self.frame.idx egl_image = self.egl_images.get(idx) - - # Create EGL image if needed if egl_image is None: egl_image = create_egl_image(self.frame.width, self.frame.height, self.frame.stride, self.frame.fd, self.frame.uv_offset) - if egl_image: - self.egl_images[idx] = egl_image - else: - return + if egl_image is None: + return False + self.egl_images[idx] = egl_image - # Update texture dimensions to match current frame self.egl_texture.width = self.frame.width self.egl_texture.height = self.frame.height - - # Bind the EGL image to our texture bind_egl_image_to_texture(self._external_texture_id, egl_image) - # Render with shader rl.begin_shader_mode(self.shader) - rl.draw_texture_pro(self.egl_texture, src_rect, dst_rect, rl.Vector2(0, 0), 0.0, rl.WHITE) - rl.end_shader_mode() + try: + self._update_shader_state() + rl.draw_texture_pro(self.egl_texture, src_rect, dst_rect, rl.Vector2(0, 0), 0.0, rl.WHITE) + finally: + rl.end_shader_mode() + return True + + def _fallback_to_textures(self, reason: str) -> None: + if not self._use_egl: + return + + cloudlog.error(f"CameraView switching from EGL to texture rendering: {reason}") + self._use_egl = False + try: + self._clear_textures() + except Exception: + cloudlog.exception("CameraView EGL cleanup failed during texture fallback") + + if self.shader and self.shader.id: + try: + rl.unload_shader(self.shader) + except Exception: + cloudlog.exception("CameraView EGL shader cleanup failed during texture fallback") + self.shader.id = 0 + try: + self._load_frame_shader() + self._initialize_textures() + self._texture_needs_update = True + except Exception: + cloudlog.exception("CameraView texture fallback initialization failed") def _render_textures(self, src_rect: rl.Rectangle, dst_rect: rl.Rectangle) -> None: - """Render using texture copies""" - if not self.texture_y or not self.texture_uv or self.frame is None: + """Copy camera data into ordinary Raylib textures before drawing. + + Raylib batches camera draws as GL_TEXTURE_2D. Imported EGL images are + GL_TEXTURE_EXTERNAL_OES objects and cannot safely pass through that path; + copying also prevents the GPU from sampling camerad's reusable buffers + after they have been handed back to the producer. + """ + if (self.texture_y is None or not self.texture_y.id or + self.texture_uv is None or not self.texture_uv.id or self.frame is None): return # Update textures with new frame data @@ -318,33 +454,45 @@ class CameraView(Widget): # Render with shader rl.begin_shader_mode(self.shader) - rl.set_shader_value_texture(self.shader, self._texture1_loc, self.texture_uv) - rl.draw_texture_pro(self.texture_y, src_rect, dst_rect, rl.Vector2(0, 0), 0.0, rl.WHITE) - rl.end_shader_mode() + try: + self._update_shader_state() + rl.set_shader_value_texture(self.shader, self._texture1_loc, self.texture_uv) + rl.draw_texture_pro(self.texture_y, src_rect, dst_rect, rl.Vector2(0, 0), 0.0, rl.WHITE) + finally: + rl.end_shader_mode() def _ensure_connection(self) -> bool: - if not self.client.is_connected(): - self.frame = None - self._last_frame_id = -1 - self.available_streams.clear() + if self.client is not None and self.client.is_connected(): + return True - # Throttle connection attempts - current_time = rl.get_time() - if current_time - self.last_connection_attempt < CONNECTION_RETRY_INTERVAL: - return False - self.last_connection_attempt = current_time + # A pending candidate owns the connection attempt. Poll it until its first + # frame arrives instead of reconnecting the same client in place. + if self._switching: + self._handle_switch() + return self.client is not None and self.client.is_connected() - # A GL texture can retain the last EGL image after camerad exits. Release - # it before connect() frees and replaces the client's imported buffers. - self._clear_textures() - if not self.client.connect(False) or not self.client.num_buffers: - return False + if self.client is not None: + self._retire_active_client() + self._last_frame_id = -1 + self._regressive_frame_count = 0 + self.available_streams.clear() - cloudlog.debug(f"Connected to {self._name} stream: {self._stream_type}, buffers: {self.client.num_buffers}") - self._initialize_textures() - self.available_streams = self.client.available_streams(self._name, block=False) + # Throttle connection attempts + current_time = rl.get_time() + if current_time - self.last_connection_attempt < CONNECTION_RETRY_INTERVAL: + return False + self.last_connection_attempt = current_time - return True + # Do not create a client until camerad advertises the requested stream. + stream_type = self._target_stream_type or self._stream_type + if stream_type not in VisionIpcClient.available_streams(self._name, block=False): + return False + + self._target_stream_type = stream_type + self._target_client = VisionIpcClient(self._name, stream_type, conflate=True) + self._switching = True + self._handle_switch() + return self.client is not None and self.client.is_connected() def _handle_switch(self) -> None: """Check if target stream is ready and switch immediately.""" @@ -354,6 +502,7 @@ class CameraView(Widget): # Try to connect target if needed if not self._target_client.is_connected(): if not self._target_client.connect(False) or not self._target_client.num_buffers: + self._discard_pending_client() return cloudlog.debug(f"Target stream connected: {self._target_stream_type}") @@ -361,71 +510,126 @@ class CameraView(Widget): # Check if target has frames ready target_frame = self._target_client.recv(timeout_ms=0) if target_frame: - self.frame = target_frame # Update current frame to target frame - self._complete_switch() + packet_frame_id = int(getattr(self._target_client, "frame_id", -1)) + content_frame_id = int(getattr(target_frame, "frame_id", packet_frame_id)) + if content_frame_id != packet_frame_id: + message = f"Discarding inconsistent {self._name} target frame: content={content_frame_id}, " + message += f"packet={packet_frame_id}, stream={self._target_stream_type}" + cloudlog.warning(message) + self._discard_pending_client() + return + self._complete_switch(target_frame) + elif not self._target_client.is_connected(): + # A failed recv can invalidate the server/buffer generation. Never + # reconnect this client; the next attempt must use a fresh candidate. + self._discard_pending_client() - def _complete_switch(self) -> None: + def _complete_switch(self, target_frame: VisionBuf) -> None: """Instantly switch to target stream.""" cloudlog.debug(f"Switching to {self._target_stream_type}") - # Delete the GL texture before releasing the old client. Merely destroying - # the EGLImage handle leaves its storage alive while a texture sibling exists. - self._clear_textures() - # Switch to target - self.client = self._target_client - self._stream_type = self._target_stream_type - client_frame_id = getattr(self.client, "frame_id", -1) if hasattr(self, "client") and self.client is not None else -1 - frame = getattr(self, "frame", None) - self._last_frame_id = int(getattr(frame, "frame_id", client_frame_id)) if frame is not None else -1 - self._texture_needs_update = True - - # Reset state + target_client = self._target_client + target_stream_type = self._target_stream_type self._target_client = None self._target_stream_type = None self._switching = False + # Retire the old generation before exposing the new client and frame. + self._retire_active_client() + + # Switch to target + self.client = target_client + self._stream_type = target_stream_type + self.frame = target_frame + client_frame_id = getattr(self.client, "frame_id", -1) if self.client is not None else -1 + self._last_frame_id = int(getattr(self.frame, "frame_id", client_frame_id)) if self.frame is not None else -1 + self._regressive_frame_count = 0 + self._texture_needs_update = True + self._onroad_reentry_pending = False + self._reentry_stream_selected = False + # Initialize textures for new stream self._initialize_textures() + available_streams = getattr(self.client, "available_streams", None) + if available_streams is not None: + self.available_streams = available_streams(self._name, block=False) def _initialize_textures(self): self._clear_textures() - if TICI: - self._create_egl_texture() + if self._use_egl: + if not self._create_egl_texture(): + self._fallback_to_textures("EGL texture creation failed") else: self.texture_y = rl.load_texture_from_image(rl.Image(None, int(self.client.stride), int(self.client.height), 1, rl.PixelFormat.PIXELFORMAT_UNCOMPRESSED_GRAYSCALE)) self.texture_uv = rl.load_texture_from_image(rl.Image(None, int(self.client.stride // 2), int(self.client.height // 2), 1, rl.PixelFormat.PIXELFORMAT_UNCOMPRESSED_GRAY_ALPHA)) + if not self.texture_y.id or not self.texture_uv.id: + cloudlog.error("CameraView texture-copy texture creation failed") + self._clear_textures() - def _create_egl_texture(self): - temp_image = rl.gen_image_color(1, 1, rl.BLACK) - self.egl_texture = rl.load_texture_from_image(temp_image) - rl.unload_image(temp_image) - self._external_texture_id = create_external_texture() - if not self._external_texture_id: - raise RuntimeError("Failed to create external camera texture") - - def _clear_textures(self): - if self.texture_y and self.texture_y.id: - rl.unload_texture(self.texture_y) - self.texture_y = None - - if self.texture_uv and self.texture_uv.id: - rl.unload_texture(self.texture_uv) - self.texture_uv = None - - if TICI: + def _create_egl_texture(self) -> bool: + temp_image = None + try: + temp_image = rl.gen_image_color(1, 1, rl.BLACK) + texture = rl.load_texture_from_image(temp_image) + if texture is None or not texture.id: + self.egl_texture = None + return False + self.egl_texture = texture + self._external_texture_id = create_external_texture() + if not self._external_texture_id: + rl.unload_texture(self.egl_texture) + self.egl_texture = None + return False + return True + except Exception: if self._external_texture_id: destroy_external_texture(self._external_texture_id) self._external_texture_id = 0 - - if self.egl_texture and self.egl_texture.id: + if self.egl_texture is not None and self.egl_texture.id: rl.unload_texture(self.egl_texture) self.egl_texture = None + cloudlog.exception("CameraView failed to create EGL texture") + return False + finally: + if temp_image is not None: + try: + rl.unload_image(temp_image) + except Exception: + cloudlog.exception("CameraView failed to unload temporary EGL image") - for data in self.egl_images.values(): - destroy_egl_image(data) - self.egl_images = {} + def _clear_textures(self): + if ((self._external_texture_id or self.egl_texture is not None or self.egl_images) and is_egl_initialized()): + try: + # Raylib queues draw calls. Submit them before waiting for the GPU so + # no pending batch can still reference an EGL-backed texture. + rl.rl_draw_render_batch_active() + finish_gl() + except Exception: + cloudlog.exception("CameraView failed to synchronize EGL resources") + + if self.texture_y is not None: + if self.texture_y.id: + rl.unload_texture(self.texture_y) + self.texture_y = None + + if self.texture_uv is not None: + if self.texture_uv.id: + rl.unload_texture(self.texture_uv) + self.texture_uv = None + + if self._external_texture_id: + destroy_external_texture(self._external_texture_id) + self._external_texture_id = 0 + + if self.egl_texture and self.egl_texture.id: + rl.unload_texture(self.egl_texture) + self.egl_texture = None + + for data in self.egl_images.values(): + destroy_egl_image(data) + self.egl_images = {} if __name__ == "__main__": diff --git a/selfdrive/ui/stall_monitor.py b/selfdrive/ui/stall_monitor.py index 5e18c456c..1109cf949 100644 --- a/selfdrive/ui/stall_monitor.py +++ b/selfdrive/ui/stall_monitor.py @@ -1,10 +1,12 @@ import os +import resource import sys import time import traceback import threading -from collections import deque +from collections import Counter, deque from pathlib import Path +from typing import Any from openpilot.common.swaglog import cloudlog @@ -26,6 +28,10 @@ class UIStallMonitor: self._name = name self._threshold_s = float(os.getenv("UI_STALL_PROBE_MAX_DT", "5")) self._poll_s = float(os.getenv("UI_STALL_PROBE_POLL_DT", "0.25")) + self._hitch_threshold_s = float(os.getenv("UI_HITCH_PROBE_MAX_DT", "0.25")) + self._hitch_report_interval_s = max(self._poll_s, float(os.getenv("UI_HITCH_REPORT_INTERVAL", "300"))) + self._hitch_report_min_count = max(1, int(os.getenv("UI_HITCH_REPORT_MIN_COUNT", "3"))) + self._hitch_log_interval_s = max(0.0, float(os.getenv("UI_HITCH_LOG_INTERVAL", "10"))) self._dump_dir = _default_dump_dir() self._main_thread_id = threading.get_ident() @@ -36,6 +42,14 @@ class UIStallMonitor: self._stall_reported = False self._stalled_since = now self._stalled_phase = self._phase + self._context: dict[str, Any] = {} + + self._hitch_counts: Counter[str] = Counter() + self._hitch_max_s: dict[str, float] = {} + self._recent_hitches = deque(maxlen=max(1, int(os.getenv("UI_HITCH_HISTORY_LEN", "16")))) + self._hitch_window_started = now + self._last_hitch_report = now + self._last_hitch_log = now - self._hitch_log_interval_s self._lock = threading.Lock() self._history = deque(maxlen=max(1, int(os.getenv("UI_STALL_HISTORY_LEN", "64")))) @@ -44,31 +58,55 @@ class UIStallMonitor: self._thread = threading.Thread(target=self._run, name=f"{name}_stall_probe", daemon=True) def start(self) -> None: - if self._threshold_s <= 0.0: + if self._threshold_s <= 0.0 and self._hitch_threshold_s <= 0.0: return self._thread.start() def stop(self) -> None: - if self._threshold_s <= 0.0: + if self._threshold_s <= 0.0 and self._hitch_threshold_s <= 0.0: return self._stop_event.set() self._thread.join(timeout=1.0) + def set_context(self, context: dict[str, Any]) -> None: + with self._lock: + self._context = dict(context) + def progress(self, phase: str) -> None: now = time.monotonic() recovered = None + hitch_warning = None with self._lock: + previous_phase = self._phase + phase_duration_s = now - self._last_progress if phase != self._phase: self._phase = phase self._phase_entered = now self._history.append((now, phase)) self._last_progress = now + if self._hitch_threshold_s > 0.0 and phase_duration_s >= self._hitch_threshold_s: + self._hitch_counts[previous_phase] += 1 + self._hitch_max_s[previous_phase] = max(phase_duration_s, self._hitch_max_s.get(previous_phase, 0.0)) + self._recent_hitches.append({ + "phase": previous_phase, + "next_phase": phase, + "duration_ms": round(phase_duration_s * 1000.0, 1), + "monotonic": round(now, 3), + }) + if now - self._last_hitch_log >= self._hitch_log_interval_s: + self._last_hitch_log = now + hitch_warning = (previous_phase, phase, phase_duration_s) + if self._stall_reported: recovered = (now - self._stalled_since, self._stalled_phase, phase) self._stall_reported = False + if hitch_warning is not None: + previous_phase, current_phase, duration_s = hitch_warning + cloudlog.warning(f"{self._name} frame hitch {duration_s * 1000.0:.0f}ms in phase={previous_phase} (next_phase={current_phase})") + if recovered is not None: stalled_for_s, stalled_phase, current_phase = recovered cloudlog.warning(f"{self._name} stall recovered after {stalled_for_s:.1f}s (stalled_phase={stalled_phase}, current_phase={current_phase})") @@ -82,28 +120,43 @@ class UIStallMonitor: phase_for_s = now - self._phase_entered already_reported = self._stall_reported - if stalled_for_s < self._threshold_s or already_reported: - continue + should_report_stall = self._threshold_s > 0.0 and stalled_for_s >= self._threshold_s and not already_reported + if should_report_stall: + self._stall_reported = True + self._stalled_since = self._last_progress + self._stalled_phase = phase - dump = self._build_dump(now, phase, stalled_for_s, phase_for_s) - dump_path = self._write_dump(dump) - with self._lock: - self._stall_reported = True - self._stalled_since = now - self._stalled_phase = phase + if should_report_stall: + frames = sys._current_frames() + preview = self._main_thread_preview(frames) + dump = self._build_dump(now, phase, stalled_for_s, phase_for_s, frames=frames) + dump_path = self._write_dump(dump) + self._report_stall(dump, dump_path, phase, stalled_for_s, phase_for_s, preview=preview) - self._report_stall(dump, dump_path, phase, stalled_for_s, phase_for_s) + hitch_report = self._take_hitch_report(now) + if hitch_report is not None: + self._report_hitches(hitch_report) - def _report_stall(self, dump: str, dump_path: Path | None, phase: str, stalled_for_s: float, phase_for_s: float) -> None: - preview = self._main_thread_preview() + def _report_stall(self, dump: str, dump_path: Path | None, phase: str, stalled_for_s: float, phase_for_s: float, + preview: str | None = None) -> None: + preview = preview if preview is not None else self._main_thread_preview() path_s = str(dump_path) if dump_path is not None else "" + with self._lock: + context = dict(self._context) + cloudlog.error(f"{self._name} main loop stalled for {stalled_for_s:.1f}s in phase={phase} (phase_for={phase_for_s:.1f}s) dump={path_s}\n{preview}") + tags = { + "ui_stall_name": self._name, + "ui_stall_phase": phase, + } + if "ui_mode" in context: + tags["ui_mode"] = str(context["ui_mode"]) + if "started" in context: + tags["ui_onroad"] = str(bool(context["started"])).lower() + _capture_message( "raylib UI main loop stalled", - tags={ - "ui_stall_name": self._name, - "ui_stall_phase": phase, - }, + tags=tags, extras={ "pid": os.getpid(), "stalled_for_s": round(stalled_for_s, 3), @@ -111,13 +164,74 @@ class UIStallMonitor: "dump_path": path_s, "main_thread_stack": preview, "thread_dump": dump, + "ui_context": context, + "runtime_metrics": self._runtime_metrics(), }, attachment_path=dump_path, flush_timeout=2.0, ) - def _build_dump(self, now: float, phase: str, stalled_for_s: float, phase_for_s: float) -> str: - frames = sys._current_frames() + def _take_hitch_report(self, now: float) -> dict[str, Any] | None: + with self._lock: + count = sum(self._hitch_counts.values()) + if now - self._last_hitch_report < self._hitch_report_interval_s or count < self._hitch_report_min_count: + return None + + report = { + "window_s": round(now - self._hitch_window_started, 3), + "hitch_threshold_ms": round(self._hitch_threshold_s * 1000.0, 1), + "total_hitches": count, + "phase_counts": dict(self._hitch_counts), + "phase_max_ms": {phase: round(duration_s * 1000.0, 1) for phase, duration_s in self._hitch_max_s.items()}, + "recent_hitches": list(self._recent_hitches), + "ui_context": dict(self._context), + } + self._hitch_counts.clear() + self._hitch_max_s.clear() + self._recent_hitches.clear() + self._hitch_window_started = now + self._last_hitch_report = now + return report + + def _report_hitches(self, report: dict[str, Any]) -> None: + phase_max_ms = report["phase_max_ms"] + worst_phase = max(phase_max_ms, key=phase_max_ms.get) + context = report["ui_context"] + tags = { + "ui_stall_name": self._name, + "ui_hitch_worst_phase": worst_phase, + } + if "ui_mode" in context: + tags["ui_mode"] = str(context["ui_mode"]) + if "started" in context: + tags["ui_onroad"] = str(bool(context["started"])).lower() + + _capture_message( + "raylib UI frame hitches", + level="warning", + tags=tags, + extras={**report, "runtime_metrics": self._runtime_metrics()}, + flush_timeout=0.25, + ) + + @staticmethod + def _runtime_metrics() -> dict[str, Any]: + usage = resource.getrusage(resource.RUSAGE_SELF) + try: + load_average = [round(value, 3) for value in os.getloadavg()] + except OSError: + load_average = [] + return { + "load_average": load_average, + "max_rss_kb": usage.ru_maxrss, + "user_cpu_s": round(usage.ru_utime, 3), + "system_cpu_s": round(usage.ru_stime, 3), + "thread_count": threading.active_count(), + } + + def _build_dump(self, now: float, phase: str, stalled_for_s: float, phase_for_s: float, + frames: dict[int, Any] | None = None) -> str: + frames = frames if frames is not None else sys._current_frames() threads = {thread.ident: thread for thread in threading.enumerate()} lines = [ f"name={self._name}", @@ -150,8 +264,9 @@ class UIStallMonitor: return "".join(line if line.endswith("\n") else f"{line}\n" for line in lines) - def _main_thread_preview(self) -> str: - frame = sys._current_frames().get(self._main_thread_id) + def _main_thread_preview(self, frames: dict[int, Any] | None = None) -> str: + frames = frames if frames is not None else sys._current_frames() + frame = frames.get(self._main_thread_id) if frame is None: return "main_thread_stack=" stack_lines = traceback.format_stack(frame) diff --git a/selfdrive/ui/tests/test_camera_frame_order.py b/selfdrive/ui/tests/test_camera_frame_order.py index 002f37695..0c4de889c 100644 --- a/selfdrive/ui/tests/test_camera_frame_order.py +++ b/selfdrive/ui/tests/test_camera_frame_order.py @@ -1,3 +1,5 @@ +from types import SimpleNamespace + import pytest from openpilot.selfdrive.ui.mici.onroad import cameraview as mici_cameraview @@ -10,21 +12,101 @@ class FakeFrame: self.idx = idx -def _camera_view(cameraview): - view = cameraview.CameraView.__new__(cameraview.CameraView) +def _camera_view(): + view = big_cameraview.CameraView.__new__(big_cameraview.CameraView) view._name = "camerad" + view._stream_type = big_cameraview.VisionStreamType.VISION_STREAM_ROAD view.frame = None view._last_frame_id = -1 view._regressive_frame_count = 0 view._texture_needs_update = False + view._external_texture_id = 0 view._closed = True return view -@pytest.mark.parametrize("cameraview", [big_cameraview, mici_cameraview]) -def test_reused_egl_slot_cannot_move_camera_backwards(monkeypatch, cameraview): - monkeypatch.setattr(cameraview.cloudlog, "warning", lambda *_args, **_kwargs: None) - view = _camera_view(cameraview) +def test_mici_uses_shared_camera_view(): + assert mici_cameraview.CameraView is big_cameraview.CameraView + + +def test_pending_switch_is_cancelled_when_requested_stream_is_current(): + view = _camera_view() + view._stream_type = big_cameraview.VisionStreamType.VISION_STREAM_ROAD + view._target_stream_type = big_cameraview.VisionStreamType.VISION_STREAM_DRIVER + view._target_client = object() + view._switching = True + + view.switch_stream(big_cameraview.VisionStreamType.VISION_STREAM_ROAD) + + assert view._target_client is None + assert view._target_stream_type is None + assert not view._switching + + +def test_onroad_reentry_selects_requested_stream_before_rendering(monkeypatch): + view = _camera_view() + view._name = "camerad" + view._stream_type = big_cameraview.VisionStreamType.VISION_STREAM_WIDE_ROAD + view.client = object() + view._target_client = object() + view._target_stream_type = big_cameraview.VisionStreamType.VISION_STREAM_ROAD + view._switching = True + view._onroad_reentry_pending = True + view._reentry_stream_selected = False + view._clear_textures = lambda: None + clients = [] + + class FakeClient: + def __init__(self, name, stream_type, conflate): + self.name = name + self.stream_type = stream_type + self.conflate = conflate + clients.append(self) + + monkeypatch.setattr(big_cameraview, "VisionIpcClient", FakeClient) + view.switch_stream(big_cameraview.VisionStreamType.VISION_STREAM_ROAD) + + assert clients == [] + assert view.client is None + assert view.stream_type == big_cameraview.VisionStreamType.VISION_STREAM_ROAD + assert view._target_client is None + assert view._target_stream_type is None + assert not view._switching + assert view._reentry_stream_selected + + +def test_onroad_reentry_guard_clears_on_first_fresh_frame(): + view = _camera_view() + view._onroad_reentry_pending = True + view._reentry_stream_selected = True + + assert view._accept_frame(FakeFrame(frame_id=1, idx=0), packet_frame_id=1) + assert not view._onroad_reentry_pending + assert not view._reentry_stream_selected + + +def test_standalone_camera_reentry_selects_configured_stream(): + view = _camera_view() + view._switching = False + view._onroad_reentry_pending = True + view._reentry_stream_selected = False + selected = [] + placeholders = [] + view._select_reentry_stream = lambda stream_type: ( + selected.append(stream_type), setattr(view, "_reentry_stream_selected", True) + ) + view._draw_placeholder = lambda rect: placeholders.append(rect) + view._ensure_connection = lambda: False + + view._render(object()) + + assert selected == [view._stream_type] + assert len(placeholders) == 1 + + +def test_reused_egl_slot_cannot_move_camera_backwards(monkeypatch): + monkeypatch.setattr(big_cameraview.cloudlog, "warning", lambda *_args, **_kwargs: None) + view = _camera_view() displayed = FakeFrame(frame_id=10, idx=0) assert view._accept_frame(displayed, packet_frame_id=10) @@ -40,13 +122,357 @@ def test_reused_egl_slot_cannot_move_camera_backwards(monkeypatch, cameraview): assert view._regressive_frame_count == 1 -@pytest.mark.parametrize("cameraview", [big_cameraview, mici_cameraview]) -def test_newer_camera_frame_is_accepted(cameraview): - view = _camera_view(cameraview) +def test_newer_camera_frame_is_accepted(): + view = _camera_view() view._last_frame_id = 30 + view._regressive_frame_count = 2 newer = FakeFrame(frame_id=31, idx=2) assert view._accept_frame(newer, packet_frame_id=31) assert view.frame is newer assert view._last_frame_id == 31 + assert view._regressive_frame_count == 0 assert view._texture_needs_update + + +def test_shared_camera_has_upstream_shaders_and_driver_enhancement(): + assert "samplerExternalOES" in big_cameraview.FRAME_FRAGMENT_SHADER_EXTERNAL + assert "pow(color.rgb, vec3(1.0/1.28))" in big_cameraview.FRAME_FRAGMENT_SHADER_EXTERNAL + assert "uniform sampler2D texture0" in big_cameraview.FRAME_FRAGMENT_SHADER_YUV + assert "uniform sampler2D texture1" in big_cameraview.FRAME_FRAGMENT_SHADER_YUV + assert "uniform int enhance_driver" in big_cameraview.FRAME_FRAGMENT_SHADER_EXTERNAL + assert "uniform int enhance_driver" in big_cameraview.FRAME_FRAGMENT_SHADER_YUV + assert "uniform int engaged" not in big_cameraview.FRAME_FRAGMENT_SHADER_EXTERNAL + assert "uniform int engaged" not in big_cameraview.FRAME_FRAGMENT_SHADER_YUV + assert hasattr(big_cameraview.CameraView, "_render_egl") + assert hasattr(big_cameraview.CameraView, "_fallback_to_textures") + + +def test_shared_camera_falls_back_after_repeated_regressive_frames(monkeypatch): + monkeypatch.setattr(big_cameraview.cloudlog, "warning", lambda *_args, **_kwargs: None) + view = _camera_view() + view._use_egl = True + view.frame = FakeFrame(frame_id=30, idx=0) + view._last_frame_id = 30 + fallback_reasons = [] + view._fallback_to_textures = fallback_reasons.append + + for frame_id in (20, 19, 18): + assert not view._accept_frame(FakeFrame(frame_id=frame_id, idx=1), packet_frame_id=frame_id) + + assert fallback_reasons == ["repeated regressive frames"] + assert view.frame.frame_id == 30 + + +def test_shared_camera_fallback_reloads_texture_backend(monkeypatch): + view = _camera_view() + view._use_egl = True + view.shader = SimpleNamespace(id=1) + events = [] + view._clear_textures = lambda: events.append("clear") + view._load_frame_shader = lambda: events.append(("shader", view._use_egl)) + view._initialize_textures = lambda: events.append("textures") + monkeypatch.setattr(big_cameraview.cloudlog, "error", lambda *_args, **_kwargs: None) + monkeypatch.setattr(big_cameraview.rl, "unload_shader", lambda _shader: events.append("unload_shader")) + + view._fallback_to_textures("test") + + assert events == ["clear", "unload_shader", ("shader", False), "textures"] + assert not view._use_egl + + +def test_connection_retry_discards_failed_client_and_uses_fresh_candidate(monkeypatch): + view = _camera_view() + view._name = "camerad" + view._clear_textures = lambda: None + view.client = SimpleNamespace(is_connected=lambda: False) + view._target_client = None + view._target_stream_type = None + view._switching = False + view.available_streams = [] + view.last_connection_attempt = 0.0 + + candidates = [] + + class FakeClient: + @staticmethod + def available_streams(_name, block=False): + return [view._stream_type] + + def __init__(self, *_args, **_kwargs): + candidates.append(self) + self.connected = False + self.num_buffers = 0 + + def is_connected(self): + return self.connected + + def connect(self, _block): + return False + + monkeypatch.setattr(big_cameraview, "VisionIpcClient", FakeClient) + monkeypatch.setattr(big_cameraview.rl, "get_time", lambda: 1.0) + + assert not view._ensure_connection() + assert view.client is None + assert len(candidates) == 1 + + monkeypatch.setattr(big_cameraview.rl, "get_time", lambda: 1.3) + assert not view._ensure_connection() + assert len(candidates) == 2 + assert candidates[0] is not candidates[1] + + +def test_candidate_is_not_active_until_first_consistent_frame(monkeypatch): + view = _camera_view() + view._name = "camerad" + view._clear_textures = lambda: None + view._initialize_textures = lambda: None + view.client = None + view._target_client = None + view._target_stream_type = None + view._switching = False + view.available_streams = [] + view.last_connection_attempt = 0.0 + + class FakeClient: + @staticmethod + def available_streams(_name, block=False): + return [view._stream_type] + + def __init__(self, *_args, **_kwargs): + self.connected = False + self.num_buffers = 1 + self.frame_id = -1 + self.frames = [None, FakeFrame(frame_id=42, idx=0)] + + def is_connected(self): + return self.connected + + def connect(self, _block): + self.connected = True + return True + + def recv(self, timeout_ms=0): + frame = self.frames.pop(0) + if frame is not None: + self.frame_id = frame.frame_id + return frame + + monkeypatch.setattr(big_cameraview, "VisionIpcClient", FakeClient) + monkeypatch.setattr(big_cameraview.rl, "get_time", lambda: 1.0) + + assert not view._ensure_connection() + assert view.client is None + candidate = view._target_client + assert candidate is not None + + assert view._ensure_connection() + assert view.client is candidate + assert view.frame.frame_id == 42 + assert view._target_client is None + assert not view._switching + + +def test_inconsistent_candidate_frame_is_discarded(monkeypatch): + view = _camera_view() + view._name = "camerad" + view._clear_textures = lambda: None + view.client = None + view._target_client = None + view._target_stream_type = None + view._switching = False + view.available_streams = [] + view.last_connection_attempt = 0.0 + + class FakeClient: + @staticmethod + def available_streams(_name, block=False): + return [view._stream_type] + + def __init__(self, *_args, **_kwargs): + self.connected = False + self.num_buffers = 1 + self.frame_id = 10 + + def is_connected(self): + return self.connected + + def connect(self, _block): + self.connected = True + return True + + def recv(self, timeout_ms=0): + return FakeFrame(frame_id=9, idx=0) + + monkeypatch.setattr(big_cameraview, "VisionIpcClient", FakeClient) + monkeypatch.setattr(big_cameraview.rl, "get_time", lambda: 1.0) + + assert not view._ensure_connection() + assert view.client is None + assert view._target_client is None + assert view._target_stream_type == view._stream_type + assert not view._switching + + +def test_disconnected_candidate_is_discarded_without_reconnect(): + view = _camera_view() + + class Candidate: + num_buffers = 1 + + def __init__(self): + self.connected = True + + def is_connected(self): + return self.connected + + def connect(self, _block): + pytest.fail("discarded candidate was reconnected") + + def recv(self, timeout_ms=0): + self.connected = False + return None + + candidate = Candidate() + view._target_client = candidate + view._target_stream_type = view._stream_type + view._switching = True + + view._handle_switch() + + assert view._target_client is None + assert not view._switching + assert view._target_stream_type == view._stream_type + + +def test_steady_state_packet_content_mismatch_is_rejected(): + view = _camera_view() + displayed = FakeFrame(frame_id=10, idx=0) + assert view._accept_frame(displayed, packet_frame_id=10) + + delayed = FakeFrame(frame_id=12, idx=1) + assert not view._accept_frame(delayed, packet_frame_id=11) + assert view.frame is displayed + assert view._last_frame_id == 10 + + +def test_egl_image_creation_failure_is_reported(monkeypatch): + view = _camera_view() + view.frame = SimpleNamespace(idx=0, width=1928, height=1208, stride=2048, fd=7, uv_offset=2473984) + view.egl_texture = SimpleNamespace(id=1) + view._external_texture_id = 11 + view.egl_images = {} + monkeypatch.setattr(big_cameraview, "create_egl_image", lambda *_args: None) + + assert not view._render_egl(None, None) + assert view.egl_images == {} + + +def test_invalid_egl_texture_is_reported_without_binding(monkeypatch): + view = _camera_view() + view.frame = SimpleNamespace(idx=0) + view.egl_texture = SimpleNamespace(id=0) + view._external_texture_id = 11 + view.egl_images = {0: object()} + monkeypatch.setattr(big_cameraview, "bind_egl_image_to_texture", + lambda *_args: pytest.fail("invalid EGL texture was bound")) + + assert not view._render_egl(None, None) + + +def test_invalid_external_texture_is_reported_without_binding(monkeypatch): + view = _camera_view() + view.frame = SimpleNamespace(idx=0) + view.egl_texture = SimpleNamespace(id=7) + view.egl_images = {0: object()} + monkeypatch.setattr(big_cameraview, "bind_egl_image_to_texture", + lambda *_args: pytest.fail("invalid external texture was bound")) + + assert not view._render_egl(None, None) + + +def test_egl_render_always_ends_shader_mode(monkeypatch): + view = _camera_view() + view.frame = SimpleNamespace(idx=0, width=1928, height=1208) + view.egl_texture = SimpleNamespace(id=1, width=0, height=0) + view._external_texture_id = 11 + view.egl_images = {0: object()} + view.shader = SimpleNamespace(id=1) + view._update_shader_state = lambda: None + events = [] + monkeypatch.setattr(big_cameraview, "bind_egl_image_to_texture", lambda *_args: None) + monkeypatch.setattr(big_cameraview.rl, "begin_shader_mode", lambda *_args: events.append("begin")) + def fail_draw(*_args): + raise RuntimeError("draw failed") + + monkeypatch.setattr(big_cameraview.rl, "draw_texture_pro", fail_draw) + monkeypatch.setattr(big_cameraview.rl, "end_shader_mode", lambda: events.append("end")) + + with pytest.raises(RuntimeError, match="draw failed"): + view._render_egl(None, None) + + assert events == ["begin", "end"] + + +def test_egl_render_keeps_external_and_raylib_texture_targets_separate(monkeypatch): + view = _camera_view() + image = object() + view.frame = SimpleNamespace(idx=3, width=1928, height=1208) + view.egl_texture = SimpleNamespace(id=7, width=1, height=1) + view._external_texture_id = 11 + view.egl_images = {3: image} + view.shader = object() + view._update_shader_state = lambda: None + bound = [] + drawn = [] + monkeypatch.setattr(big_cameraview, "bind_egl_image_to_texture", + lambda texture_id, egl_image: bound.append((texture_id, egl_image))) + monkeypatch.setattr(big_cameraview.rl, "begin_shader_mode", lambda _shader: None) + monkeypatch.setattr(big_cameraview.rl, "end_shader_mode", lambda: None) + monkeypatch.setattr(big_cameraview.rl, "draw_texture_pro", lambda texture, *_args: drawn.append(texture.id)) + + assert view._render_egl(None, None) + assert bound == [(11, image)] + assert drawn == [7] + + +def test_driver_enhancement_tracks_active_stream(monkeypatch): + view = _camera_view() + view.shader = SimpleNamespace(id=1) + view._enhance_driver_loc = 2 + view._enhance_driver_val = [0] + values = [] + monkeypatch.setattr(big_cameraview.rl, "set_shader_value", + lambda _shader, _loc, value, _type: values.append(value[0])) + + view._stream_type = big_cameraview.VisionStreamType.VISION_STREAM_ROAD + view._update_shader_state() + view._stream_type = big_cameraview.VisionStreamType.VISION_STREAM_DRIVER + view._update_shader_state() + view._stream_type = big_cameraview.VisionStreamType.VISION_STREAM_WIDE_ROAD + view._update_shader_state() + + assert values == [0, 1, 0] + + +def test_texture_fallback_survives_egl_cleanup_failure(monkeypatch): + view = _camera_view() + view._use_egl = True + view.shader = SimpleNamespace(id=1) + events = [] + + def fail_cleanup(): + raise RuntimeError("cleanup failed") + + view._clear_textures = fail_cleanup + view._load_frame_shader = lambda: events.append(("shader", view._use_egl)) + view._initialize_textures = lambda: events.append("textures") + monkeypatch.setattr(big_cameraview.cloudlog, "error", lambda *_args, **_kwargs: None) + monkeypatch.setattr(big_cameraview.cloudlog, "exception", lambda *_args, **_kwargs: None) + monkeypatch.setattr(big_cameraview.rl, "unload_shader", lambda _shader: events.append("unload_shader")) + + view._fallback_to_textures("test") + + assert not view._use_egl + assert events == ["unload_shader", ("shader", False), "textures"] diff --git a/selfdrive/ui/tests/test_stall_monitor.py b/selfdrive/ui/tests/test_stall_monitor.py index 500c5ab6d..3b38cb4eb 100644 --- a/selfdrive/ui/tests/test_stall_monitor.py +++ b/selfdrive/ui/tests/test_stall_monitor.py @@ -23,3 +23,74 @@ def test_stall_report_is_sent_to_bugsink(monkeypatch, tmp_path): assert "test_stall_report_is_sent_to_bugsink" in report["extras"]["main_thread_stack"] assert report["extras"]["thread_dump"] == dump assert report["attachment_path"] == dump_path + + +def test_phase_hitches_are_aggregated(monkeypatch): + now = [100.0] + monkeypatch.setattr(stall_monitor.time, "monotonic", lambda: now[0]) + monkeypatch.setattr(stall_monitor.cloudlog, "warning", lambda *_args, **_kwargs: None) + + monitor = stall_monitor.UIStallMonitor("raylib_ui") + monitor._hitch_report_interval_s = 1.0 + monitor._hitch_report_min_count = 2 + monitor.set_context({"ui_mode": "small", "started": True}) + monitor.progress("gui_app.before_widget_render") + + now[0] += 0.4 + monitor.progress("gui_app.after_widget_render") + now[0] += 0.3 + monitor.progress("gui_app.before_end_drawing") + now[0] += 0.4 + + report = monitor._take_hitch_report(now[0]) + + assert report is not None + assert report["total_hitches"] == 2 + assert report["phase_counts"] == { + "gui_app.before_widget_render": 1, + "gui_app.after_widget_render": 1, + } + assert report["phase_max_ms"]["gui_app.before_widget_render"] == 400.0 + assert report["ui_context"] == {"ui_mode": "small", "started": True} + + +def test_hitch_report_is_rate_limited_and_sent_to_bugsink(monkeypatch): + reports = [] + monkeypatch.setattr(stall_monitor, "_capture_message", lambda message, **kwargs: reports.append((message, kwargs))) + monitor = stall_monitor.UIStallMonitor("raylib_ui") + monitor._hitch_counts.update({"gui_app.before_end_drawing": 3}) + monitor._hitch_max_s["gui_app.before_end_drawing"] = 0.75 + monitor._hitch_report_interval_s = 10.0 + monitor._hitch_report_min_count = 3 + monitor._last_hitch_report = 5.0 + monitor._hitch_window_started = 5.0 + monitor.set_context({"ui_mode": "small", "started": False}) + + assert monitor._take_hitch_report(14.9) is None + report = monitor._take_hitch_report(15.0) + assert report is not None + monitor._report_hitches(report) + + message, kwargs = reports[0] + assert message == "raylib UI frame hitches" + assert kwargs["level"] == "warning" + assert kwargs["tags"] == { + "ui_stall_name": "raylib_ui", + "ui_hitch_worst_phase": "gui_app.before_end_drawing", + "ui_mode": "small", + "ui_onroad": "false", + } + assert kwargs["extras"]["total_hitches"] == 3 + assert monitor._take_hitch_report(30.0) is None + + +def test_stall_report_uses_captured_stack_preview(monkeypatch, tmp_path): + report = {} + monkeypatch.setattr(stall_monitor, "_capture_message", lambda _message, **kwargs: report.update(kwargs)) + monitor = stall_monitor.UIStallMonitor("raylib_ui") + dump_path = monitor._write_dump("thread dump") + + monitor._report_stall("thread dump", dump_path, "gui_app.before_end_drawing", 5.0, 5.0, + preview="main_thread_stack:\ncaptured before recovery") + + assert report["extras"]["main_thread_stack"] == "main_thread_stack:\ncaptured before recovery" diff --git a/selfdrive/ui/tests/test_ui_state_performance.py b/selfdrive/ui/tests/test_ui_state_performance.py new file mode 100644 index 000000000..c36ba31bc --- /dev/null +++ b/selfdrive/ui/tests/test_ui_state_performance.py @@ -0,0 +1,35 @@ +import threading + +from openpilot.selfdrive.ui.lib.ui_param_cache import UIParamCache +from openpilot.selfdrive.ui import ui_state as ui_state_module + + +def test_raylib_ui_uses_read_through_param_cache(): + assert isinstance(ui_state_module.ui_state.ui_params, UIParamCache) + assert ui_state_module.ui_state.ui_params is not ui_state_module.ui_state.params + + +def test_usbgpu_poll_does_not_block_ui_thread(monkeypatch): + started = threading.Event() + release = threading.Event() + + def poll(): + started.set() + release.wait(timeout=1.0) + return True + + monkeypatch.setattr(ui_state_module, "chestnut_present", poll) + state = object.__new__(ui_state_module.UIState) + state.usbgpu = False + state._usbgpu_update_time = 0.0 + state._usbgpu_poll_thread = None + + state._schedule_usbgpu_poll(now=1.0, force=True) + assert started.wait(timeout=0.2) + polling_thread = state._usbgpu_poll_thread + state._schedule_usbgpu_poll(now=2.0, force=True) + assert state._usbgpu_poll_thread is polling_thread + + release.set() + polling_thread.join(timeout=1.0) + assert state.usbgpu is True diff --git a/selfdrive/ui/ui.py b/selfdrive/ui/ui.py index 1251f45ad..0d2444bcb 100644 --- a/selfdrive/ui/ui.py +++ b/selfdrive/ui/ui.py @@ -1,5 +1,6 @@ #!/usr/bin/env python3 import os +import time from openpilot.system.hardware import TICI from openpilot.common.realtime import config_realtime_process, set_core_affinity @@ -11,6 +12,36 @@ from openpilot.selfdrive.ui.ui_state import ui_state BIG_UI = gui_app.big_ui() +def _stall_context() -> dict[str, object]: + active_widget = gui_app.get_active_widget() + context = { + "ui_mode": "big" if BIG_UI else "small", + "started": ui_state.started, + "ignition": ui_state.ignition, + "engaged": ui_state.engaged, + "render_frame": gui_app.frame, + "ui_state_frame": ui_state.sm.frame, + "target_fps": gui_app.target_fps, + "active_widget": type(active_widget).__name__ if active_widget is not None else "none", + } + + try: + device_state = ui_state.sm["deviceState"] + context.update({ + "device_state_valid": bool(ui_state.sm.valid["deviceState"]), + "memory_usage_percent": int(device_state.memoryUsagePercent), + "gpu_usage_percent": int(device_state.gpuUsagePercent), + "max_cpu_usage_percent": max((int(value) for value in device_state.cpuUsagePercent), default=0), + "max_cpu_temp_c": round(max((float(value) for value in device_state.cpuTempC), default=0.0), 1), + "max_gpu_temp_c": round(max((float(value) for value in device_state.gpuTempC), default=0.0), 1), + "thermal_status": str(device_state.thermalStatus), + }) + except Exception: + pass + + return context + + def main(): cores = {5, } config_realtime_process(0, 51) @@ -32,8 +63,10 @@ def main(): from openpilot.selfdrive.ui.mici.layouts.main import MiciMainLayout MiciMainLayout() stall_monitor.progress("ui.after_layout_init") + stall_monitor.set_context(_stall_context()) kick_watchdog() stall_monitor.progress("ui.loop_ready") + context_update_time = 0.0 for should_render in gui_app.render(): stall_monitor.progress("ui.loop_iteration") @@ -41,6 +74,10 @@ def main(): stall_monitor.progress("ui.after_watchdog") ui_state.update() stall_monitor.progress("ui.after_state_update") + now = time.monotonic() + if now - context_update_time >= 1.0: + stall_monitor.set_context(_stall_context()) + context_update_time = now if should_render: # reaffine after power save offlines our core if TICI and os.sched_getaffinity(0) != cores: diff --git a/selfdrive/ui/ui_state.py b/selfdrive/ui/ui_state.py index 74b170914..e72dd204d 100644 --- a/selfdrive/ui/ui_state.py +++ b/selfdrive/ui/ui_state.py @@ -36,9 +36,7 @@ class UIState: def _initialize(self): self.params = Params() - # BIG UI views use this read-through cache; keep ``params`` untouched for - # MICI and for non-rendering callers that rely on its exact semantics. - self.ui_params = shared_ui_params() if gui_app.big_ui() else self.params + self.ui_params = shared_ui_params() self.params_memory = Params(memory=True) self.sm = messaging.SubMaster( [ @@ -87,10 +85,11 @@ class UIState: self.is_metric: bool = self.params.get_bool("IsMetric") self.is_release = self.params.get_bool("IsReleaseBranch") self.always_on_dm: bool = self.params.get_bool("AlwaysOnDM") - self.usbgpu: bool = chestnut_present() + self.usbgpu: bool = False self.usbgpu_compiled: bool = self.params.get_bool("UsbGpuCompiled") self.usbgpu_active: bool = self.params.get_bool("UsbGpuActive") self._usbgpu_update_time: float = 0.0 + self._usbgpu_poll_thread: threading.Thread | None = None self.started: bool = False self.ignition: bool = False self.recording_audio: bool = False @@ -125,8 +124,26 @@ class UIState: self._offroad_transition_callbacks: list[Callable[[], None]] = [] self._engaged_transition_callbacks: list[Callable[[], None]] = [] + self._schedule_usbgpu_poll(force=True) self.update_params() + def _poll_usbgpu_presence(self) -> None: + try: + self.usbgpu = chestnut_present() + except Exception: + cloudlog.exception("USB GPU presence poll failed") + + def _schedule_usbgpu_poll(self, now: float | None = None, force: bool = False) -> None: + now = time.monotonic() if now is None else now + if not force and now - self._usbgpu_update_time < USBGPU_POLL_INTERVAL: + return + if self._usbgpu_poll_thread is not None and self._usbgpu_poll_thread.is_alive(): + return + + self._usbgpu_update_time = now + self._usbgpu_poll_thread = threading.Thread(target=self._poll_usbgpu_presence, name="ui_usbgpu_poll", daemon=True) + self._usbgpu_poll_thread.start() + def add_offroad_transition_callback(self, callback: Callable[[], None]): self._offroad_transition_callbacks.append(callback) @@ -180,7 +197,6 @@ class UIState: self.light_sensor = -1 # Trust hardwared's filtered started state; raw ignition can flap on Toyota. - # Use the BIG-UI cache here as this path runs once per render iteration. params = self.ui_params force_onroad = params.get_bool("ForceOnroad") force_offroad = params.get_bool("ForceOffroad") @@ -195,9 +211,7 @@ class UIState: self.is_metric = params.get_bool("IsMetric") self.always_on_dm = params.get_bool("AlwaysOnDM") now = time.monotonic() - if now - self._usbgpu_update_time >= USBGPU_POLL_INTERVAL: - self.usbgpu = chestnut_present() - self._usbgpu_update_time = now + self._schedule_usbgpu_poll(now) self.usbgpu_compiled = params.get_bool("UsbGpuCompiled") self.usbgpu_active = params.get_bool("UsbGpuActive") self.switchback_mode_enabled = self.params_memory.get_bool("SwitchbackModeEnabled") if self.started else False diff --git a/starpilot/system/the_galaxy/assets/components/router.js b/starpilot/system/the_galaxy/assets/components/router.js index d09ef9849..e6e42c548 100644 --- a/starpilot/system/the_galaxy/assets/components/router.js +++ b/starpilot/system/the_galaxy/assets/components/router.js @@ -19,7 +19,7 @@ import { ModelManager } from "/assets/components/tools/model_manager.js?v=202603 import { LivePlots } from "/assets/components/tools/plots.js" import { ThemeMaker } from "/assets/components/tools/theme_maker.js" import { TestingGround } from "/assets/components/tools/testing_ground.js" -import { Tuning } from "/assets/components/tools/tuning.js?v=flm-workspace-9" +import { Tuning } from "/assets/components/tools/tuning.js?v=flm-saved-tunes-1" import { Troubleshoot } from "/assets/components/tools/troubleshoot.js" import { TmuxLog } from "/assets/components/tools/tmux.js" import { ToggleControl } from "/assets/components/tools/toggles.js" diff --git a/starpilot/system/the_galaxy/assets/components/tools/tuning.css b/starpilot/system/the_galaxy/assets/components/tools/tuning.css index 7bba68d77..681359f02 100644 --- a/starpilot/system/the_galaxy/assets/components/tools/tuning.css +++ b/starpilot/system/the_galaxy/assets/components/tools/tuning.css @@ -93,7 +93,6 @@ border: 1px solid rgba(255, 255, 255, 0.06); border-radius: var(--border-radius-sm); color: var(--text-color); - cursor: pointer; display: flex; gap: var(--gap-sm); padding: var(--padding-sm); @@ -105,6 +104,7 @@ } .flmRouteItem { + cursor: pointer; flex: 1 1 auto; min-width: 0; } @@ -141,13 +141,20 @@ flex: 0 0 auto; } +.flmSavedTuneActions { + align-items: stretch; + display: flex; + flex: 0 0 auto; + flex-direction: column; + gap: var(--gap-xs); +} + .flmRouteItem small, .flmWorkspaceItem small, .flmWorkspaceItem span { color: var(--text-muted); } -.flmWorkspaceItem:hover, .flmRouteItem:hover, .flmCard button.selected { border-color: var(--main-fg); @@ -387,4 +394,13 @@ font-size: var(--font-size-sm); grid-template-columns: minmax(8rem, 1.3fr) minmax(5rem, 1fr) auto minmax(5rem, 1fr); } + + .flmWorkspaceRow { + flex-direction: column; + } + + .flmSavedTuneActions { + flex-direction: row; + flex-wrap: wrap; + } } diff --git a/starpilot/system/the_galaxy/assets/components/tools/tuning.js b/starpilot/system/the_galaxy/assets/components/tools/tuning.js index 3f18ba332..8e57f3de2 100644 --- a/starpilot/system/the_galaxy/assets/components/tools/tuning.js +++ b/starpilot/system/the_galaxy/assets/components/tools/tuning.js @@ -16,7 +16,7 @@ const state = reactive({ routeProgress: 0, routeTotal: 0, connectDongleId: "", - workspace: { reports: [], activeTrial: null, status: {} }, + workspace: { reports: [], savedTunes: [], activeTrial: null, status: {} }, status: {}, report: null, feedbackAccepted: [], @@ -258,29 +258,6 @@ async function deleteReport(reportId) { } } -async function clearWorkspace() { - if (state.runningAction) return - if (!window.confirm("Clear every saved tuning report, feedback entry, generated profile, and snapshot from the device?")) return - - state.runningAction = true - try { - const response = await fetch("/api/flm/workspace/clear", { method: "POST" }) - const payload = await response.json() - if (!response.ok) throw new Error(payload.error || "Failed to clear tuning workspace.") - - state.report = null - syncFeedbackState(null) - state.workspace = payload.workspace || { reports: [], activeTrial: null, status: {} } - state.status = { ...state.status, ...(payload.workspace?.status || {}) } - showSnackbar(payload.message || "Cleared tuning workspace.") - } catch (error) { - state.error = error?.message || "Failed to clear tuning workspace." - showSnackbar(state.error, "error") - } finally { - state.runningAction = false - } -} - async function fetchStatus() { try { const response = await fetch("/api/flm/status") @@ -291,7 +268,12 @@ async function fetchStatus() { isOnroad: !!payload.isOnroad, } if (payload.activeTrial !== undefined) { - state.workspace = { ...state.workspace, activeTrial: payload.activeTrial, reports: payload.reports || state.workspace.reports } + state.workspace = { + ...state.workspace, + activeTrial: payload.activeTrial, + reports: payload.reports || state.workspace.reports, + savedTunes: payload.savedTunes || state.workspace.savedTunes, + } } const reportId = state.status.reportId if (reportId && state.report?.reportId !== reportId) { @@ -431,6 +413,95 @@ async function applyProfile(profileId) { } } +async function saveCurrentTune() { + if (state.runningAction || !state.workspace?.activeTrial) return + const defaultName = state.workspace.activeTrial.profileLabel || state.workspace.currentCarFingerprint || "Saved Tune" + const name = window.prompt("Name this tune", defaultName) + if (name === null) return + + state.runningAction = true + try { + const response = await fetch("/api/flm/saved-tunes", { + method: "POST", + headers: { "Content-Type": "application/json" }, + body: JSON.stringify({ name }), + }) + const payload = await response.json() + if (!response.ok) throw new Error(payload.error || "Failed to save the active tune.") + state.error = "" + state.workspace = payload.workspace || state.workspace + showSnackbar(payload.message || "Saved the active tune.") + } catch (error) { + state.error = error?.message || "Failed to save the active tune." + showSnackbar(state.error, "error") + } finally { + state.runningAction = false + } +} + +async function applySavedTune(tuneId) { + if (!tuneId || state.runningAction) return + state.runningAction = true + try { + const response = await fetch(`/api/flm/saved-tunes/${encodeURIComponent(tuneId)}/apply`, { method: "POST" }) + const payload = await response.json() + if (!response.ok) throw new Error(payload.error || "Failed to apply saved tune.") + state.error = "" + state.workspace = payload.workspace || state.workspace + showSnackbar(payload.message || "Saved tune applied.") + } catch (error) { + state.error = error?.message || "Failed to apply saved tune." + showSnackbar(state.error, "error") + } finally { + state.runningAction = false + } +} + +async function renameSavedTune(tune) { + if (!tune?.tuneId || state.runningAction) return + const name = window.prompt("Rename saved tune", tune.name || "Saved Tune") + if (name === null) return + + state.runningAction = true + try { + const response = await fetch(`/api/flm/saved-tunes/${encodeURIComponent(tune.tuneId)}`, { + method: "PATCH", + headers: { "Content-Type": "application/json" }, + body: JSON.stringify({ name }), + }) + const payload = await response.json() + if (!response.ok) throw new Error(payload.error || "Failed to rename saved tune.") + state.error = "" + state.workspace = payload.workspace || state.workspace + showSnackbar(payload.message || "Saved tune renamed.") + } catch (error) { + state.error = error?.message || "Failed to rename saved tune." + showSnackbar(state.error, "error") + } finally { + state.runningAction = false + } +} + +async function deleteSavedTune(tune) { + if (!tune?.tuneId || state.runningAction) return + if (!window.confirm(`Delete saved tune "${tune.name || "Saved Tune"}"?`)) return + + state.runningAction = true + try { + const response = await fetch(`/api/flm/saved-tunes/${encodeURIComponent(tune.tuneId)}`, { method: "DELETE" }) + const payload = await response.json() + if (!response.ok) throw new Error(payload.error || "Failed to delete saved tune.") + state.error = "" + state.workspace = payload.workspace || state.workspace + showSnackbar(payload.message || "Saved tune deleted.") + } catch (error) { + state.error = error?.message || "Failed to delete saved tune." + showSnackbar(state.error, "error") + } finally { + state.runningAction = false + } +} + async function selectPath(pathKey) { if (!state.report?.reportId || !pathKey || state.runningAction) return if (pathKey === (state.report.selectedPathKey || state.report.primaryPathKey)) return @@ -596,8 +667,19 @@ function allReportProfiles() { function activeTrialProfile() { const activeTrial = state.workspace?.activeTrial - if (!activeTrial || activeTrial.reportId !== state.report?.reportId) return null - return allReportProfiles().find((profile) => profile.id === activeTrial.profileId) || null + if (!activeTrial) return null + if (activeTrial.reportId === state.report?.reportId) { + const reportProfile = allReportProfiles().find((profile) => profile.id === activeTrial.profileId) + if (reportProfile) return reportProfile + } + return { + id: activeTrial.profileId, + genericParams: activeTrial.appliedGenericParams || {}, + flmOverrides: { + baseFrictionThresholds: activeTrial.appliedFrictionThresholds || {}, + vehicleKnobs: activeTrial.appliedVehicleKnobs || {}, + }, + } } function mergedFlmOverrides() { @@ -1020,6 +1102,12 @@ export function Tuning() { @click="${revertProfile}"> Revert Trial + ${() => state.workspace?.activeTrial?.rollbackAvailable === false ? html` - ${() => state.loadingWorkspace ? html`

Loading workspace...

` : ""} + ${() => state.loadingWorkspace ? html`

Loading saved tunes...

` : ""}

- Recent reports stay on-device under /data/galaxy/flm. Loading a report refreshes the suggestion and trial view below. + Save a working FLM trial, switch between vehicle or trailer setups, then use Revert Trial to return to the exact manual settings from before FLM.

- ${() => (state.workspace?.reports || []).length - ? state.workspace.reports.map((report) => html` + ${() => (state.workspace?.savedTunes || []).length + ? state.workspace.savedTunes.map((tune) => html`
- - +
+ ${tune.name || "Saved Tune"}${tune.active ? " (Active)" : ""} + ${tune.carFingerprint || "Unknown car"}${tune.pathLabel ? ` / ${tune.pathLabel}` : ""} + + ${tune.genericParamCount} generic, ${tune.frictionCurveCount} friction curve, ${tune.vehicleKnobCount} vehicle knobs + + ${formatTimestamp(tune.updatedAt ? new Date(tune.updatedAt * 1000).toISOString() : "")} +
+
+ + + +
`) - : html`

No tuning reports yet.

`} + : html`

No saved tunes yet. Apply a trial, then save it here.

`}
diff --git a/starpilot/system/the_galaxy/flm_workspace.py b/starpilot/system/the_galaxy/flm_workspace.py index 073c5b177..ab7032936 100644 --- a/starpilot/system/the_galaxy/flm_workspace.py +++ b/starpilot/system/the_galaxy/flm_workspace.py @@ -259,6 +259,7 @@ def _workspace_paths() -> dict[str, Path]: "profiles": root / "profiles", "feedback": root / "feedback", "snapshots": root / "snapshots", + "savedTunes": root / "saved_tunes", "reference": root / "reference", } @@ -1136,6 +1137,88 @@ def _current_family_curve(family: str, current: dict[str, Any]) -> list[float]: return _baseline_family_curve(family) +FLM_CHATTER_FRICTION_DELTAS = { + "low": [0.012, 0.020, 0.008, 0.0, 0.0], + "mid": [0.0, 0.012, 0.020, 0.008, 0.0], + "fast": [0.0, 0.0, 0.010, 0.020, 0.010], + "highway": [0.0, 0.0, 0.0, 0.012, 0.025], + "mixed": [0.0, 0.010, 0.018, 0.022, 0.025], +} +FLM_CHATTER_DEADBAND_SUFFIX = { + "low": "center_deadband_low_deg", + "mid": "center_deadband_mid_deg", + "fast": "center_deadband_fast_deg", + "highway": "center_deadband_highway_deg", + "mixed": "center_deadband_mid_deg", +} +FLM_CHATTER_DEADBAND_DELTA = { + "low": 0.035, + "mid": 0.025, + "fast": 0.018, + "highway": 0.012, + "mixed": 0.020, +} +FLM_CHATTER_THRESHOLD_PASS_MIN_DELTA = 0.012 + + +def _center_chatter_friction_adjustment(family: str, speed_band: str, severity: float, + current: dict[str, Any]) -> dict[str, Any]: + current_curve = _current_family_curve(family, current) + deltas = FLM_CHATTER_FRICTION_DELTAS.get(speed_band, FLM_CHATTER_FRICTION_DELTAS["mixed"]) + scale = min(max(severity, 0.45), 1.2) + suggested = [round(current_curve[idx] + (delta * scale), 4) for idx, delta in enumerate(deltas)] + return { + "type": "friction_curve", + "symbol": f"base_friction_threshold.{family}", + "family": family, + "current": current_curve, + "suggested": suggested, + "delta": [round(suggested[idx] - current_curve[idx], 4) for idx in range(len(current_curve))], + "stage": "friction_threshold", + "speedBand": speed_band, + } + + +def _center_chatter_threshold_pass_applied(family: str, speed_band: str, current: dict[str, Any]) -> bool: + baseline = _baseline_family_curve(family) + active = _current_family_curve(family, current) + target_indexes = { + "low": (0, 1), + "mid": (1, 2), + "fast": (2, 3), + "highway": (3, 4), + "mixed": tuple(range(len(FLM_FRICTION_SPEED_KNOTS))), + }.get(speed_band, tuple(range(len(FLM_FRICTION_SPEED_KNOTS)))) + return max((active[idx] - baseline[idx] for idx in target_indexes), default=0.0) >= FLM_CHATTER_THRESHOLD_PASS_MIN_DELTA + + +def _center_chatter_deadband_adjustment(capabilities: dict[str, Any], speed_band: str, severity: float, + current: dict[str, Any]) -> dict[str, Any] | None: + rich_profile = capabilities.get("richProfileKey") + suffix = FLM_CHATTER_DEADBAND_SUFFIX.get(speed_band, FLM_CHATTER_DEADBAND_SUFFIX["mixed"]) + if not rich_profile or not _rich_profile_supports_knob(capabilities, suffix): + return None + adjustment = _vehicle_knob_adjustment( + f"{rich_profile}.{suffix}", + FLM_CHATTER_DEADBAND_DELTA.get(speed_band, FLM_CHATTER_DEADBAND_DELTA["mixed"]) * min(max(severity, 0.5), 1.2), + current, + ) + if adjustment is not None: + adjustment["stage"] = "center_deadband" + adjustment["speedBand"] = speed_band + return adjustment + + +def _direction_reversal_count(values: np.ndarray, min_step: float) -> int: + if len(values) < 3: + return 0 + deltas = np.diff(values) + significant = deltas[np.abs(deltas) >= min_step] + if len(significant) < 2: + return 0 + return int(np.sum(np.sign(significant[1:]) != np.sign(significant[:-1]))) + + def _clamp(value: float, lower: float, upper: float) -> float: return min(max(float(value), lower), upper) @@ -1231,23 +1314,52 @@ def _build_event_summaries(samples: list[FLMSample]) -> tuple[list[dict[str, Any "saturation_limited": [1.0 if sample.saturated else 0.0 for sample in samples], } - # Straight-road chatter detection uses a simple 4-second window. + # Detect controller-driven center chatter independently in each speed band. + # The desired path must remain calm while steering angle and either output or + # tracking error repeatedly reverse direction. straight_windows = [] + angle_thresholds = {"low": 0.80, "mid": 0.55, "fast": 0.38, "highway": 0.28} + error_thresholds = {"low": 0.16, "mid": 0.12, "fast": 0.09, "highway": 0.07} + output_thresholds = {"low": 0.055, "mid": 0.045, "fast": 0.035, "highway": 0.025} for start_idx in range(0, max(len(samples) - 20, 1), 10): window = samples[start_idx:start_idx + 40] if len(window) < 20: continue if not all(eligibility[start_idx:start_idx + len(window)]): continue - if float(np.mean([sample.v_ego for sample in window])) < 20.0: + mean_speed = float(np.mean([sample.v_ego for sample in window])) + if mean_speed < 2.0: continue - if float(np.mean([abs(sample.desired_la) for sample in window])) > 0.12: + speed_band = _speed_band_label(mean_speed) + desired_series = np.array([sample.desired_la for sample in window]) + if float(np.mean(np.abs(desired_series))) > (0.14 if speed_band == "low" else 0.18): continue - centered_angles = np.array([sample.steering_angle_deg for sample in window]) - float(np.mean([sample.steering_angle_deg for sample in window])) - sign_changes = int(np.sum(np.sign(centered_angles[1:]) != np.sign(centered_angles[:-1]))) - amplitude = float(np.max(centered_angles) - np.min(centered_angles)) - chatter_score = (amplitude * 0.25) + (sign_changes * 0.04) - if amplitude > 0.45 and sign_changes >= 6: + desired_span = float(np.ptp(desired_series)) + desired_reversals = _direction_reversal_count(desired_series, 0.008) + if desired_span > 0.18 or desired_reversals > 3: + continue + + angle_series = np.array([sample.steering_angle_deg for sample in window]) + angle_trend = np.linspace(angle_series[0], angle_series[-1], len(angle_series)) + centered_angles = angle_series - angle_trend + error_series = np.array([sample.actual_la - sample.desired_la for sample in window]) + output_series = np.array([sample.output for sample in window]) + angle_p2p = float(np.ptp(centered_angles)) + error_p2p = float(np.ptp(error_series)) + output_p2p = float(np.ptp(output_series)) + angle_reversals = _direction_reversal_count(centered_angles, max(angle_thresholds[speed_band] * 0.08, 0.025)) + error_reversals = _direction_reversal_count(error_series, max(error_thresholds[speed_band] * 0.08, 0.006)) + output_reversals = _direction_reversal_count(output_series, max(output_thresholds[speed_band] * 0.08, 0.002)) + angle_evidence = angle_p2p >= angle_thresholds[speed_band] and angle_reversals >= 3 + error_evidence = error_p2p >= error_thresholds[speed_band] and error_reversals >= 3 + output_evidence = output_p2p >= output_thresholds[speed_band] and output_reversals >= 3 + if angle_evidence and (error_evidence or output_evidence): + chatter_score = min(1.5, ( + 0.30 * (angle_p2p / angle_thresholds[speed_band]) + + 0.18 * (error_p2p / error_thresholds[speed_band]) + + 0.18 * (output_p2p / output_thresholds[speed_band]) + + 0.025 * min(angle_reversals + error_reversals + output_reversals, 14) + )) straight_windows.append({ "startIdx": start_idx, "endIdx": start_idx + len(window) - 1, @@ -1255,9 +1367,20 @@ def _build_event_summaries(samples: list[FLMSample]) -> tuple[list[dict[str, Any "peakScore": chatter_score, "route": window[0].route, "segment": window[0].segment, - "speedBand": "highway", + "speedBand": speed_band, "direction": "center", "supportCount": len(window), + "metrics": { + "meanSpeedMps": round(mean_speed, 3), + "steeringAngleP2P": round(angle_p2p, 4), + "trackingErrorP2P": round(error_p2p, 4), + "outputP2P": round(output_p2p, 4), + "steeringReversals": angle_reversals, + "trackingErrorReversals": error_reversals, + "outputReversals": output_reversals, + "desiredP2P": round(desired_span, 4), + "desiredReversals": desired_reversals, + }, }) curve_windows = [] @@ -1338,13 +1461,14 @@ def _build_event_summaries(samples: list[FLMSample]) -> tuple[list[dict[str, Any def _summaries_from_events(bucket: str, samples: list[FLMSample], events: list[dict[str, Any]], eligibility: list[bool] | None = None) -> list[dict[str, Any]]: - grouped: dict[tuple[str, str], list[dict[str, Any]]] = {} + grouped: dict[tuple[str, str, str], list[dict[str, Any]]] = {} for event in events: - key = (bucket, event["direction"]) + event_speed_band = event["speedBand"] if bucket == "center_chatter" else "mixed" + key = (bucket, event["direction"], event_speed_band) grouped.setdefault(key, []).append(event) summaries = [] - for (bucket_name, direction), grouped_events in grouped.items(): + for (bucket_name, direction, _group_speed_band), grouped_events in grouped.items(): grouped_events.sort(key=lambda item: item["peakScore"], reverse=True) strongest = grouped_events[:3] strongest_labels = [ @@ -1371,6 +1495,7 @@ def _summaries_from_events(bucket: str, samples: list[FLMSample], events: list[d "directionBias": direction, "eventCount": len(grouped_events), "segments": strongest_labels, + "chatterMetrics": top_event.get("metrics", {}), }, "events": grouped_events, "plotSvg": _build_plot_svg(plot_data), @@ -1410,9 +1535,12 @@ def _primary_delta_from_summary(summary: dict[str, Any], capabilities: dict[str, return None if strategy == "baseline": - if bucket in ("center_chatter", "notchy_mid_curve"): + if bucket == "center_chatter": + return _center_chatter_friction_adjustment(family, speed_band, severity, current) + + if bucket == "notchy_mid_curve": current_curve = _current_family_curve(family, current) - deltas = [0.0, 0.01, 0.02, 0.025, 0.03] if bucket == "center_chatter" else [0.0, 0.0, 0.015, 0.02, 0.02] + deltas = [0.0, 0.0, 0.015, 0.02, 0.02] scale = min(max(severity, 0.4), 1.2) suggested = [round(current_curve[idx] + (delta * scale), 4) for idx, delta in enumerate(deltas)] return { @@ -1463,12 +1591,16 @@ def _primary_delta_from_summary(summary: dict[str, Any], capabilities: dict[str, suggested_value = round(_clamp(current_value + (0.015 * severity * direction_mult), 0.0, 1.0), 4) return {"type": "generic_param", "paramKey": "SteerFriction", "current": current_value, "suggested": suggested_value, "delta": round(suggested_value - current_value, 4)} - if bucket in ("center_chatter", "notchy_mid_curve"): + if bucket == "center_chatter": + if _center_chatter_threshold_pass_applied(family, speed_band, current): + deadband_adjustment = _center_chatter_deadband_adjustment(capabilities, speed_band, severity, current) + if deadband_adjustment is not None: + return deadband_adjustment + return _center_chatter_friction_adjustment(family, speed_band, severity, current) + + if bucket == "notchy_mid_curve": current_curve = _current_family_curve(family, current) - if bucket == "center_chatter": - deltas = [0.0, 0.01, 0.02, 0.025, 0.03] - else: - deltas = [0.0, 0.0, 0.015, 0.02, 0.02] + deltas = [0.0, 0.0, 0.015, 0.02, 0.02] scale = min(max(severity, 0.4), 1.2) suggested = [round(current_curve[idx] + (delta * scale), 4) for idx, delta in enumerate(deltas)] return { @@ -1588,7 +1720,7 @@ def _observed_behavior(summary: dict[str, Any]) -> str: "early_turn_in": f"Turn-in is too eager{direction_text}; actual response jumps ahead of the plan during entry.", "unwind_too_slow": f"Unwind is hanging on too long{direction_text}; the car keeps steering after the plan starts releasing.", "unwind_too_fast": f"Unwind is releasing too quickly{direction_text}; the wheel gives back steering sooner than the plan wants.", - "center_chatter": "The car is doing repeated micro-corrections on straights or very light highway arcs.", + "center_chatter": f"The car is doing repeated micro-corrections around center in the {speed_band} speed band while the requested path stays calm.", "notchy_mid_curve": "Mid-curve tracking is correcting in steps instead of flowing through the same steering band cleanly.", "low_speed_unwillingness": "At low speed the controller is slow to wake up even though the turn request is already there.", "saturation_limited": "The controller is spending meaningful time at or near its steering authority ceiling.", @@ -1605,6 +1737,11 @@ def _likely_interpretation(summary: dict[str, Any], adjustment: dict[str, Any]) return "This looks more like a friction-threshold problem than a whole-tune problem; the controller is busy around center and needs a calmer deadzone slope." if adjustment["type"] == "vehicle_knob": symbol = adjustment["symbol"] + if "center_deadband_" in symbol: + return ( + "A friction-threshold pass is already active in this speed band, but controller-driven reversals remain. " + "The residual motion is narrow enough for a small deadband cleanup instead of another broad friction increase." + ) if "ff_gain_" in symbol: return "This car has a directional nonlinear torque map, and the mismatch is concentrated on one side. Correct that side's feedforward layer before moving global authority." if "low_speed_angle_assist_max_torque" in symbol: @@ -1635,6 +1772,11 @@ def _why_this_knob(adjustment: dict[str, Any]) -> str: return "This changes the threshold that maps small lateral-accel error into friction compensation without pretending the whole torque slope is wrong." if adjustment["type"] == "vehicle_knob": symbol = adjustment["symbol"] + if "center_deadband_" in symbol: + return ( + "This adds a small steering-angle deadband only around the affected speed knot, interpolated into neighboring speeds, " + "without reducing normal curve authority." + ) if "ff_gain_" in symbol: return "This compensates the affected side without flattening the car's separate left/right nonlinear torque response into one global value." if "low_speed_angle_assist_max_torque" in symbol: @@ -1664,11 +1806,20 @@ def _render_adjustment_line(adjustment: dict[str, Any]) -> str: curve = ", ".join(f"{value:.3f}" for value in adjustment["suggested"]) return f"Adjust {adjustment['family']} friction threshold curve at {FLM_FRICTION_SPEED_KNOTS} m/s to [{curve}]." if adjustment["type"] == "vehicle_knob": - return f"Move `{adjustment['symbol']}` from {adjustment['current']:.3f} to {adjustment['suggested']:.3f}." + suffix = " as the second-stage center-chatter cleanup." if adjustment.get("stage") == "center_deadband" else "." + return f"Move `{adjustment['symbol']}` from {adjustment['current']:.3f} to {adjustment['suggested']:.3f}{suffix}" return f"Move `{adjustment['paramKey']}` from {adjustment['current']:.3f} to {adjustment['suggested']:.3f}." def _what_not_to_touch_yet(summary: dict[str, Any], adjustment: dict[str, Any] | None, strategy: str) -> str: + if summary.get("bucket") == "center_chatter": + if adjustment and adjustment.get("stage") == "friction_threshold": + return "Do not add deadband or center taper yet. First verify whether the speed-localized friction threshold removes the repeated reversals." + if adjustment and adjustment.get("stage") == "center_deadband": + return ( + "Do not raise the whole friction curve again or reduce global feedforward. " + "This pass is only for the residual near-center motion in the affected speed band." + ) if strategy == "baseline": if adjustment and adjustment.get("type") in ("generic_param", "friction_curve"): return "Do not jump straight into phase-specific cleanup knobs yet. Get the broad authority and friction behavior into the right zip code first." @@ -1679,11 +1830,37 @@ def _what_not_to_touch_yet(summary: dict[str, Any], adjustment: dict[str, Any] | def _if_that_was_wrong(summary: dict[str, Any], adjustment: dict[str, Any], strategy: str) -> str: + if summary.get("bucket") == "center_chatter": + if adjustment.get("stage") == "friction_threshold": + return ( + "If chatter remains after this threshold pass, re-analyze the next drive. FLM will move to a bounded deadband cleanup " + "for the same speed band rather than repeatedly raising the whole threshold curve." + ) + if adjustment.get("stage") == "center_deadband": + return ( + "If steering becomes reluctant around center, use the conservative profile or halve this deadband step; " + "leave the completed friction-threshold pass in place." + ) if strategy == "baseline": return f"If this gets the car broadly closer but leaves one specific phase ugly, stop here and switch to Cleanup Pass for that band. {_why_this_knob(adjustment)}" return f"If this cleans up the main symptom but introduces the opposite behavior, keep half the change and move to the next phase-specific knob. {_why_this_knob(adjustment)}" +def _log_support(summary: dict[str, Any]) -> str: + evidence = summary.get("evidence", {}) + segment_labels = ", ".join(item["label"] for item in evidence.get("segments", [])[:3]) or "none" + base = f"Matched in {evidence.get('eventCount', 0)} event(s); strongest samples: {segment_labels}" + metrics = evidence.get("chatterMetrics", {}) + if summary.get("bucket") != "center_chatter" or not metrics: + return base + + return ( + f"{base}. Strongest window: steering moved {metrics.get('steeringAngleP2P', 0.0):.2f} deg peak-to-peak " + f"with {metrics.get('steeringReversals', 0)} steering reversal(s) and {metrics.get('outputReversals', 0)} output reversal(s), " + f"while the desired path moved only {metrics.get('desiredP2P', 0.0):.3f} m/s^2 peak-to-peak" + ) + + def build_suggestions(summaries: list[dict[str, Any]], capabilities: dict[str, Any], current: dict[str, Any], strategy: str = "cleanup") -> list[dict[str, Any]]: suggestions = [] @@ -1744,7 +1921,7 @@ def build_suggestions(summaries: list[dict[str, Any]], capabilities: dict[str, A "whatNotToTouchYet": _what_not_to_touch_yet(summary, adjustment, strategy), "ifThatWasWrong": _if_that_was_wrong(summary, adjustment, strategy), "driverFeel": _observed_behavior(summary), - "logSupport": f"Matched in {evidence.get('eventCount', 0)} event(s); strongest samples: {', '.join(item['label'] for item in evidence.get('segments', [])[:3]) or 'none'}", + "logSupport": _log_support(summary), "whyThisKnob": _why_this_knob(adjustment), "plotSvg": summary.get("plotSvg", ""), "plotData": summary.get("plotData", {}), @@ -1796,11 +1973,13 @@ def _merge_primary_adjustments(suggestions: list[dict[str, Any]], multiplier: fl bucket = friction_targets.setdefault(family, { "current": [float(value) for value in adjustment["current"]], "weightedDelta": [0.0] * len(delta_curve), - "weight": 0.0, + "weights": [0.0] * len(delta_curve), }) for idx, value in enumerate(delta_curve): + if math.isclose(value, 0.0, abs_tol=1e-9): + continue bucket["weightedDelta"][idx] += value * weight - bucket["weight"] += weight + bucket["weights"][idx] += weight requires_force_auto_tune_off = True overrides: dict[str, Any] = {"schemaVersion": 1, "baseFrictionThresholds": {}, "vehicleKnobs": {}} @@ -1825,9 +2004,12 @@ def _merge_primary_adjustments(suggestions: list[dict[str, Any]], multiplier: fl overrides["vehicleKnobs"][symbol] = next_value for family, bucket in friction_targets.items(): - if bucket["weight"] <= 0: + if not any(weight > 0.0 for weight in bucket["weights"]): continue - avg_delta_curve = [value / bucket["weight"] for value in bucket["weightedDelta"]] + avg_delta_curve = [ + value / bucket["weights"][idx] if bucket["weights"][idx] > 0.0 else 0.0 + for idx, value in enumerate(bucket["weightedDelta"]) + ] values = [ round(max(0.05, float(bucket["current"][idx]) + (avg_delta_curve[idx] * multiplier)), 4) for idx in range(len(bucket["current"])) @@ -2546,6 +2728,62 @@ def _active_trial_display_state(paths: dict[str, Path], snapshot: Any) -> dict[s } +def _current_car_identity(params: Params) -> dict[str, str]: + cp_bytes = params.get("CarParamsPersistent") + if not cp_bytes: + return {"carFingerprint": "", "brand": ""} + try: + with car.CarParams.from_bytes(cp_bytes) as car_params: + return { + "carFingerprint": str(getattr(car_params, "carFingerprint", "") or "").strip(), + "brand": str(getattr(car_params, "brand", "") or "").strip(), + } + except Exception: + return {"carFingerprint": "", "brand": ""} + + +def _normalize_saved_tune_name(name: str) -> str: + normalized = " ".join(str(name or "").split()) + if not normalized: + raise ValueError("A saved tune name is required.") + if len(normalized) > 64: + raise ValueError("Saved tune names must be 64 characters or fewer.") + return normalized + + +def _load_saved_tune(tune_id: str, paths: dict[str, Path] | None = None) -> dict[str, Any]: + paths = paths or ensure_flm_workspace() + tune = _read_json(paths["savedTunes"] / f"{tune_id}.json", {}) + if not isinstance(tune, dict) or not tune: + raise FileNotFoundError(tune_id) + return tune + + +def list_saved_tunes(paths: dict[str, Path] | None = None, active_tune_id: str = "") -> list[dict[str, Any]]: + paths = paths or ensure_flm_workspace() + saved_tunes = [] + for path in paths["savedTunes"].glob("*.json"): + payload = _read_json(path, {}) + if not isinstance(payload, dict) or not payload: + continue + flm_overrides = normalize_flm_overrides(payload.get("flmOverrides", {})) + saved_tunes.append({ + "tuneId": str(payload.get("tuneId", path.stem) or path.stem), + "name": str(payload.get("name", "Saved Tune") or "Saved Tune"), + "createdAt": float(payload.get("createdAt", path.stat().st_mtime) or path.stat().st_mtime), + "updatedAt": float(payload.get("updatedAt", path.stat().st_mtime) or path.stat().st_mtime), + "carFingerprint": str(payload.get("carFingerprint", "") or ""), + "brand": str(payload.get("brand", "") or ""), + "sourceReportId": str(payload.get("sourceReportId", "") or ""), + "pathLabel": str(payload.get("pathLabel", "") or ""), + "genericParamCount": len(payload.get("genericParams", {})) if isinstance(payload.get("genericParams"), dict) else 0, + "frictionCurveCount": len(flm_overrides.get("baseFrictionThresholds", {})), + "vehicleKnobCount": len(flm_overrides.get("vehicleKnobs", {})), + "active": str(payload.get("tuneId", path.stem) or path.stem) == active_tune_id, + }) + return sorted(saved_tunes, key=lambda tune: (tune["updatedAt"], tune["createdAt"]), reverse=True) + + def list_workspace() -> dict[str, Any]: paths = ensure_flm_workspace() reports = [] @@ -2582,9 +2820,27 @@ def list_workspace() -> dict[str, Any]: "recoveryNeeded": True, "rollbackAvailable": False, } + if current_profile_id.startswith("saved:"): + saved_tune_id = current_profile_id.split(":", 1)[1] + saved_tune = _read_json(paths["savedTunes"] / f"{saved_tune_id}.json", {}) + if isinstance(saved_tune, dict) and saved_tune: + saved_overrides = normalize_flm_overrides(saved_tune.get("flmOverrides", {})) + raw_active_snapshot = { + **raw_active_snapshot, + "savedTuneId": saved_tune_id, + "profileLabel": str(saved_tune.get("name", "Saved Tune") or "Saved Tune"), + "carFingerprint": str(saved_tune.get("carFingerprint", "") or ""), + "appliedGenericParams": dict(saved_tune.get("genericParams", {})), + "appliedFrictionThresholds": saved_overrides.get("baseFrictionThresholds", {}), + "appliedVehicleKnobs": saved_overrides.get("vehicleKnobs", {}), + } active_snapshot = _active_trial_display_state(paths, raw_active_snapshot) + active_tune_id = str(active_snapshot.get("savedTuneId", "") or "") if isinstance(active_snapshot, dict) else "" + current_car = _current_car_identity(params) return { "reports": reports[:20], + "savedTunes": list_saved_tunes(paths, active_tune_id), + "currentCarFingerprint": current_car["carFingerprint"], "feedbackCount": len(feedback_files), "activeTrial": active_snapshot, "status": read_flm_status(), @@ -2621,7 +2877,7 @@ def delete_report(report_id: str) -> dict[str, Any]: status = read_flm_status() if not status.get("running") and status.get("reportId") == report_id: - _clear_flm_status() + clear_flm_status() return { "message": f"Deleted tuning report {report_id}.", @@ -2654,7 +2910,7 @@ def clear_workspace() -> dict[str, Any]: removed.append(str(progress_path)) _clear_persistent_trial_baseline(params) - _clear_flm_status() + clear_flm_status() return { "message": "Cleared saved tuning reports, feedback, profiles, and snapshots.", @@ -2809,6 +3065,250 @@ def _find_revert_snapshot(paths: dict[str, Path], active_snapshot: dict[str, Any return _recover_report_baseline(paths, current_profile_id) +def _active_trial_adjustments(paths: dict[str, Path], params: Params, + active_snapshot: dict[str, Any]) -> tuple[dict[str, Any], dict[str, Any]]: + current_state = _snapshot_current_trial_state(params) + display_state = _active_trial_display_state(paths, active_snapshot) or {} + baseline_snapshot = _find_revert_snapshot( + paths, + active_snapshot, + str(current_state.get("FLMActiveProfileId", "") or ""), + params, + ) + baseline_params = baseline_snapshot.get("params", {}) if isinstance(baseline_snapshot, dict) else {} + + generic_params = {} + display_generic = display_state.get("appliedGenericParams", {}) + if not isinstance(display_generic, dict): + display_generic = {} + for key in FLM_ADVANCED_LATERAL_PARAM_KEYS: + if key not in current_state: + continue + if key in baseline_params: + if current_state[key] != baseline_params[key]: + generic_params[key] = current_state[key] + elif key in display_generic: + generic_params[key] = current_state[key] + + current_overrides = normalize_flm_overrides(current_state.get("FLMActiveOverrides", {})) + baseline_overrides = normalize_flm_overrides(baseline_params.get("FLMActiveOverrides", {})) + display_friction = display_state.get("appliedFrictionThresholds", {}) + display_knobs = display_state.get("appliedVehicleKnobs", {}) + if not isinstance(display_friction, dict): + display_friction = {} + if not isinstance(display_knobs, dict): + display_knobs = {} + + friction_thresholds = {} + for family, payload in current_overrides.get("baseFrictionThresholds", {}).items(): + if family in display_friction or payload != baseline_overrides.get("baseFrictionThresholds", {}).get(family): + friction_thresholds[family] = payload + vehicle_knobs = {} + for symbol, value in current_overrides.get("vehicleKnobs", {}).items(): + if symbol in display_knobs or value != baseline_overrides.get("vehicleKnobs", {}).get(symbol): + vehicle_knobs[symbol] = value + + return generic_params, normalize_flm_overrides({ + "schemaVersion": 1, + "baseFrictionThresholds": friction_thresholds, + "vehicleKnobs": vehicle_knobs, + }) + + +def _active_trial_car_fingerprint(paths: dict[str, Path], active_snapshot: dict[str, Any]) -> str: + fingerprint = str(active_snapshot.get("carFingerprint", "") or "") + if fingerprint: + return fingerprint + report_id = str(active_snapshot.get("reportId", "") or "") + report = _read_json(paths["reports"] / f"{report_id}.json", {}) if report_id else {} + return str(report.get("car", {}).get("carFingerprint", "") or "") if isinstance(report, dict) else "" + + +def save_active_trial_as_tune(name: str) -> dict[str, Any]: + paths = ensure_flm_workspace() + params = Params(return_defaults=True) + if not params.get_bool("FLMTrialApplied"): + raise RuntimeError("Apply an FLM trial before saving it as a tune.") + + active_snapshot = _read_json(paths["snapshots"] / "active.json", {}) + if not isinstance(active_snapshot, dict): + active_snapshot = {} + display_state = _active_trial_display_state(paths, active_snapshot) or {} + generic_params, flm_overrides = _active_trial_adjustments(paths, params, active_snapshot) + current_state = _snapshot_current_trial_state(params) + baseline_snapshot = _find_revert_snapshot( + paths, + active_snapshot, + str(current_state.get("FLMActiveProfileId", "") or ""), + params, + ) + baseline_params = dict(baseline_snapshot.get("params", {})) if isinstance(baseline_snapshot, dict) else {} + report_id = str(display_state.get("reportId", "") or "") + report = _read_json(paths["reports"] / f"{report_id}.json", {}) if report_id else {} + report_car = report.get("car", {}) if isinstance(report, dict) else {} + current_car = _current_car_identity(params) + car_fingerprint = current_car["carFingerprint"] or str(report_car.get("carFingerprint", "") or "") + brand = current_car["brand"] or str(report_car.get("brand", "") or "") + now = time.time() + tune_id = f"tune-{time.time_ns()}" + tune = { + "schemaVersion": 1, + "tuneId": tune_id, + "name": _normalize_saved_tune_name(name), + "createdAt": now, + "updatedAt": now, + "carFingerprint": car_fingerprint, + "brand": brand, + "sourceReportId": report_id, + "sourceProfileId": str(display_state.get("profileId", "") or ""), + "pathKey": str(display_state.get("pathKey", "") or ""), + "pathLabel": str(display_state.get("pathLabel", "") or ""), + "baselineParams": baseline_params, + "genericParams": generic_params, + "flmOverrides": flm_overrides, + } + _write_json(paths["savedTunes"] / f"{tune_id}.json", tune) + active_snapshot.update({ + "profileId": f"saved:{tune_id}", + "savedTuneId": tune_id, + "profileLabel": tune["name"], + "carFingerprint": car_fingerprint, + "updatedAt": now, + }) + _write_json(paths["snapshots"] / "active.json", active_snapshot) + _apply_param_bundle(params, {"FLMActiveProfileId": f"saved:{tune_id}"}) + return { + "message": f"Saved {tune['name']}.", + "tune": tune, + "workspace": list_workspace(), + } + + +def apply_saved_tune(tune_id: str) -> dict[str, Any]: + paths = ensure_flm_workspace() + tune = _load_saved_tune(tune_id, paths) + params = Params(return_defaults=True) + current_car = _current_car_identity(params) + tune_fingerprint = str(tune.get("carFingerprint", "") or "") + if current_car["carFingerprint"] and tune_fingerprint and current_car["carFingerprint"] != tune_fingerprint: + raise RuntimeError( + f"This tune is for {tune_fingerprint}, but the connected car is {current_car['carFingerprint']}." + ) + + current_state = _snapshot_current_trial_state(params) + raw_active_snapshot = _read_json(paths["snapshots"] / "active.json", {}) + if not isinstance(raw_active_snapshot, dict): + raw_active_snapshot = {} + previous_display_state = _active_trial_display_state(paths, raw_active_snapshot) or {} + if current_state.get("FLMTrialApplied", False): + active_fingerprint = _active_trial_car_fingerprint(paths, raw_active_snapshot) + changing_cars = bool(current_car["carFingerprint"] and active_fingerprint and current_car["carFingerprint"] != active_fingerprint) + if changing_cars: + saved_baseline = tune.get("baselineParams", {}) + if not isinstance(saved_baseline, dict) or not saved_baseline or saved_baseline.get("FLMTrialApplied", False): + raise RuntimeError("This saved tune does not contain a clean baseline for the connected car. Revert before changing cars, then save the tune again.") + baseline_params = saved_baseline + session_started_at = time.time() + else: + baseline_snapshot = _find_revert_snapshot( + paths, + raw_active_snapshot, + str(current_state.get("FLMActiveProfileId", "") or ""), + params, + ) + if baseline_snapshot is None: + raise RuntimeError("The active FLM trial has no recoverable rollback baseline. Keep the current tune as the new baseline before switching tunes.") + baseline_params = baseline_snapshot["params"] + session_started_at = float(baseline_snapshot.get("sessionStartedAt", baseline_snapshot.get("capturedAt", time.time())) or time.time()) + else: + baseline_params = current_state + session_started_at = time.time() + + generic_params = { + key: value for key, value in tune.get("genericParams", {}).items() + if key in FLM_ADVANCED_LATERAL_PARAM_KEYS + } if isinstance(tune.get("genericParams"), dict) else {} + flm_overrides = normalize_flm_overrides(tune.get("flmOverrides", {})) + profile_id = f"saved:{tune_id}" + now = time.time() + snapshot = { + "reportId": str(tune.get("sourceReportId", "") or ""), + "profileId": profile_id, + "profileLabel": str(tune.get("name", "Saved Tune") or "Saved Tune"), + "savedTuneId": tune_id, + "carFingerprint": tune_fingerprint, + "pathKey": str(tune.get("pathKey", "") or ""), + "pathLabel": str(tune.get("pathLabel", "") or ""), + "capturedAt": session_started_at, + "updatedAt": now, + "sessionStartedAt": session_started_at, + "revisionCount": int(previous_display_state.get("revisionCount", 0) or 0) + 1, + "params": baseline_params, + "appliedGenericParams": generic_params, + "appliedFrictionThresholds": flm_overrides.get("baseFrictionThresholds", {}), + "appliedVehicleKnobs": flm_overrides.get("vehicleKnobs", {}), + } + _write_json(paths["snapshots"] / "active.json", snapshot) + _write_json(paths["snapshots"] / f"saved-{tune_id}-{time.time_ns()}.json", snapshot) + _persist_trial_baseline(params, snapshot) + + # Start from the original manual baseline on every switch so values from the + # previously active saved tune cannot leak into this one. + bundle = { + key: baseline_params[key] for key in FLM_ADVANCED_LATERAL_PARAM_KEYS + if key in baseline_params + } + bundle.update(generic_params) + bundle["FLMActiveProfileId"] = profile_id + bundle["FLMActiveOverrides"] = flm_overrides + bundle["FLMTrialApplied"] = True + _apply_param_bundle(params, bundle) + if tune.get("pathKey") == "cleanup_pass" and tune_fingerprint: + _record_cleanup_progress(tune_fingerprint, str(tune.get("sourceReportId", "") or "")) + return { + "message": f"Applied saved tune {tune.get('name', 'Saved Tune')}.", + "tune": tune, + "workspace": list_workspace(), + } + + +def rename_saved_tune(tune_id: str, name: str) -> dict[str, Any]: + paths = ensure_flm_workspace() + tune = _load_saved_tune(tune_id, paths) + tune["name"] = _normalize_saved_tune_name(name) + tune["updatedAt"] = time.time() + _write_json(paths["savedTunes"] / f"{tune_id}.json", tune) + active_snapshot_path = paths["snapshots"] / "active.json" + active_snapshot = _read_json(active_snapshot_path, {}) + if isinstance(active_snapshot, dict) and active_snapshot.get("savedTuneId") == tune_id: + active_snapshot["profileLabel"] = tune["name"] + active_snapshot["updatedAt"] = time.time() + _write_json(active_snapshot_path, active_snapshot) + return { + "message": f"Renamed saved tune to {tune['name']}.", + "tune": tune, + "workspace": list_workspace(), + } + + +def delete_saved_tune(tune_id: str) -> dict[str, Any]: + paths = ensure_flm_workspace() + tune = _load_saved_tune(tune_id, paths) + active_snapshot = _read_json(paths["snapshots"] / "active.json", {}) + params = Params(return_defaults=True) + current_profile_id = params.get("FLMActiveProfileId", encoding="utf-8") or "" + if ( + (isinstance(active_snapshot, dict) and active_snapshot.get("savedTuneId") == tune_id) + or (params.get_bool("FLMTrialApplied") and current_profile_id == f"saved:{tune_id}") + ): + raise RuntimeError("Revert or switch away from this saved tune before deleting it.") + (paths["savedTunes"] / f"{tune_id}.json").unlink() + return { + "message": f"Deleted saved tune {tune.get('name', 'Saved Tune')}.", + "workspace": list_workspace(), + } + + def apply_trial_profile(report_id: str, profile_id: str) -> dict[str, Any]: paths = ensure_flm_workspace() params = Params(return_defaults=True) diff --git a/starpilot/system/the_galaxy/templates/index.html b/starpilot/system/the_galaxy/templates/index.html index 54eeae0c9..846f84e0b 100644 --- a/starpilot/system/the_galaxy/templates/index.html +++ b/starpilot/system/the_galaxy/templates/index.html @@ -34,7 +34,7 @@ - + diff --git a/starpilot/system/the_galaxy/tests/test_flm_workspace.py b/starpilot/system/the_galaxy/tests/test_flm_workspace.py index 31283f6c1..fe288bc52 100644 --- a/starpilot/system/the_galaxy/tests/test_flm_workspace.py +++ b/starpilot/system/the_galaxy/tests/test_flm_workspace.py @@ -125,8 +125,16 @@ def _install_flm_import_stubs(tmp_path): "hyundai_ioniq_6.crawl_turn_in_ff_boost_left": {"min": 0.0, "max": 0.5, "precision": 0.001, "defaultValue": 0.18, "profile": "hyundai_ioniq_6"}, "hyundai_ioniq_6.curvy_turn_in_trim_left": {"min": 0.0, "max": 0.2, "precision": 0.001, "defaultValue": 0.06, "profile": "hyundai_ioniq_6"}, "hyundai_ioniq_6.curvy_unwind_extra_reduction_left": {"min": 0.0, "max": 0.45, "precision": 0.001, "defaultValue": 0.18, "profile": "hyundai_ioniq_6"}, + "hyundai_ioniq_6.center_deadband_low_deg": {"min": 0.0, "max": 0.3, "precision": 0.005, "defaultValue": 0.0, "profile": "hyundai_ioniq_6"}, + "hyundai_ioniq_6.center_deadband_mid_deg": {"min": 0.0, "max": 0.3, "precision": 0.005, "defaultValue": 0.0, "profile": "hyundai_ioniq_6"}, + "hyundai_ioniq_6.center_deadband_fast_deg": {"min": 0.0, "max": 0.3, "precision": 0.005, "defaultValue": 0.0, "profile": "hyundai_ioniq_6"}, + "hyundai_ioniq_6.center_deadband_highway_deg": {"min": 0.0, "max": 0.3, "precision": 0.005, "defaultValue": 0.0, "profile": "hyundai_ioniq_6"}, "torque_universal.ff_gain_left": {"min": -0.4, "max": 0.6, "precision": 0.001, "defaultValue": 0.0, "profile": "torque_universal"}, "torque_universal.ff_gain_right": {"min": -0.4, "max": 0.6, "precision": 0.001, "defaultValue": 0.0, "profile": "torque_universal"}, + "torque_universal.center_deadband_low_deg": {"min": 0.0, "max": 0.3, "precision": 0.005, "defaultValue": 0.0, "profile": "torque_universal"}, + "torque_universal.center_deadband_mid_deg": {"min": 0.0, "max": 0.3, "precision": 0.005, "defaultValue": 0.0, "profile": "torque_universal"}, + "torque_universal.center_deadband_fast_deg": {"min": 0.0, "max": 0.3, "precision": 0.005, "defaultValue": 0.0, "profile": "torque_universal"}, + "torque_universal.center_deadband_highway_deg": {"min": 0.0, "max": 0.3, "precision": 0.005, "defaultValue": 0.0, "profile": "torque_universal"}, }, get_gm_base_friction_threshold=lambda v_ego: 0.20 + (0.001 * float(v_ego)), get_hkg_canfd_base_friction_threshold=lambda v_ego: 0.39 + (0.001 * float(v_ego)), @@ -375,6 +383,65 @@ def test_classify_torque_samples_detects_center_chatter(tmp_path): assert len(chatter["plotData"]["times"]) == len(chatter["plotData"]["actual"]) +def test_classify_torque_samples_detects_mid_speed_center_chatter(tmp_path): + module, _ = _load_flm_workspace_module(tmp_path) + samples = [] + for idx in range(80): + samples.append(_sample( + module, + t=idx * 0.1, + v_ego=10.0, + desired_la=0.025 * math.sin(idx * 0.08), + actual_la=0.09 * math.sin(idx * 0.85), + steering_angle_deg=0.65 * math.sin(idx * 0.85), + output=0.035 * math.sin(idx * 0.85), + )) + + summaries, _ = module.classify_torque_samples(samples) + chatter = next(summary for summary in summaries if summary["bucket"] == "center_chatter") + assert chatter["speedBand"] == "mid" + assert chatter["evidence"]["chatterMetrics"]["steeringReversals"] >= 3 + + +def test_classify_torque_samples_detects_low_speed_center_chatter(tmp_path): + module, _ = _load_flm_workspace_module(tmp_path) + samples = [] + for idx in range(80): + samples.append(_sample( + module, + t=idx * 0.1, + v_ego=4.0, + desired_la=0.018 * math.sin(idx * 0.07), + actual_la=0.14 * math.sin(idx * 0.78), + steering_angle_deg=1.05 * math.sin(idx * 0.78), + output=0.065 * math.sin(idx * 0.78), + )) + + summaries, _ = module.classify_torque_samples(samples) + chatter = next(summary for summary in summaries if summary["bucket"] == "center_chatter") + assert chatter["speedBand"] == "low" + assert chatter["evidence"]["chatterMetrics"]["outputReversals"] >= 3 + + +def test_classify_torque_samples_rejects_model_driven_center_motion(tmp_path): + module, _ = _load_flm_workspace_module(tmp_path) + samples = [] + for idx in range(80): + desired = 0.14 * math.sin(idx * 0.85) + samples.append(_sample( + module, + t=idx * 0.1, + v_ego=24.0, + desired_la=desired, + actual_la=desired * 0.95, + steering_angle_deg=0.55 * math.sin(idx * 0.85), + output=0.04 * math.sin(idx * 0.85), + )) + + summaries, _ = module.classify_torque_samples(samples) + assert not any(summary["bucket"] == "center_chatter" for summary in summaries) + + def test_plot_context_stops_at_ineligible_samples(tmp_path): module, _ = _load_flm_workspace_module(tmp_path) samples = [_sample(module, t=idx * 0.1, desired_la=idx * 0.01, actual_la=idx * 0.009) for idx in range(20)] @@ -628,7 +695,7 @@ def test_build_suggestions_rebases_friction_curve_against_active_override(tmp_pa "plotSvg": "", } capabilities = {"richProfileKey": "torque_universal", "frictionFamily": "standard"} - current_curve = [0.34, 0.35, 0.36, 0.37, 0.38] + current_curve = [0.34, 0.35, 0.36, 0.32, 0.33] current = { "SteerLatAccel": 1.8, "SteerFriction": 0.2, @@ -649,7 +716,64 @@ def test_build_suggestions_rebases_friction_curve_against_active_override(tmp_pa assert adjustment["type"] == "friction_curve" assert adjustment["family"] == "standard" assert adjustment["current"] == current_curve - assert adjustment["suggested"][2] > current_curve[2] + assert adjustment["suggested"][4] > current_curve[4] + + +def test_center_chatter_cleanup_moves_to_deadband_after_threshold_pass(tmp_path): + module, _ = _load_flm_workspace_module(tmp_path) + summary = { + "bucket": "center_chatter", + "dimensionId": "center_chatter:center:mid", + "direction": "center", + "speedBand": "mid", + "severity": 0.9, + "evidence": {"speedBand": "mid", "directionBias": "center", "eventCount": 3, "segments": [{"label": "route/2"}]}, + "plotSvg": "", + } + capabilities = {"richProfileKey": "torque_universal", "frictionFamily": "standard"} + current = { + "SteerLatAccel": 1.8, + "SteerFriction": 0.2, + "FLMActiveOverrides": { + "schemaVersion": 1, + "baseFrictionThresholds": { + "standard": { + "speedKnots": [0.0, 5.0, 10.0, 15.0, 25.0], + "values": [0.30, 0.32, 0.34, 0.33, 0.34], + }, + }, + "vehicleKnobs": {}, + }, + } + + suggestions = module.build_suggestions([summary], capabilities, current, strategy="cleanup") + adjustment = suggestions[0]["primaryAdjustmentRaw"] + assert adjustment["type"] == "vehicle_knob" + assert adjustment["symbol"] == "torque_universal.center_deadband_mid_deg" + assert adjustment["stage"] == "center_deadband" + assert adjustment["suggested"] > adjustment["current"] + + +def test_center_chatter_friction_merge_preserves_each_speed_band(tmp_path): + module, _ = _load_flm_workspace_module(tmp_path) + current_curve = [0.30, 0.30, 0.30, 0.30, 0.30] + suggestions = [] + for speed_band in ("low", "highway"): + adjustment = module._center_chatter_friction_adjustment("standard", speed_band, 1.0, { + "FLMActiveOverrides": { + "baseFrictionThresholds": { + "standard": {"speedKnots": [0.0, 5.0, 10.0, 15.0, 25.0], "values": current_curve}, + }, + }, + }) + suggestions.append({"severity": 1.0, "primaryAdjustmentRaw": adjustment}) + + _, overrides, _ = module._merge_primary_adjustments(suggestions, 1.0) + merged = overrides["baseFrictionThresholds"]["standard"]["values"] + assert merged[0] == pytest.approx(0.312) + assert merged[1] == pytest.approx(0.320) + assert merged[3] == pytest.approx(0.312) + assert merged[4] == pytest.approx(0.325) def test_select_primary_tuning_path_prefers_baseline_for_broad_mismatch(tmp_path): @@ -976,6 +1100,198 @@ def test_repeated_trial_revisions_revert_to_original_baseline(tmp_path): assert fake_params_cls._store["FLMActiveOverrides"] == {} +def test_saved_tunes_switch_cleanly_and_revert_to_original_baseline(tmp_path, monkeypatch): + module, fake_params_cls = _load_flm_workspace_module(tmp_path) + workspace = module.ensure_flm_workspace() + monkeypatch.setattr(module, "_current_car_identity", lambda _params: {"carFingerprint": "TEST_CAR", "brand": "test"}) + + first_report_id = "report-save-first" + first_profile_id = f"{first_report_id}:cleanup_pass:recommended" + second_report_id = "report-save-second" + second_profile_id = f"{second_report_id}:cleanup_pass:recommended" + first_profile = { + "id": first_profile_id, + "label": "First Trial", + "pathKey": "cleanup_pass", + "pathLabel": "Cleanup Pass", + "genericParams": { + "AdvancedLateralTune": True, + "SteerFriction": 0.2, + "SteerLatAccel": 1.9, + }, + "flmOverrides": { + "baseFrictionThresholds": {}, + "vehicleKnobs": {"hyundai_ioniq_6.turn_in_boost_left": 0.08}, + }, + } + second_profile = { + "id": second_profile_id, + "label": "Second Trial", + "pathKey": "cleanup_pass", + "pathLabel": "Cleanup Pass", + "genericParams": { + "AdvancedLateralTune": True, + "SteerLatAccel": 2.0, + }, + "flmOverrides": { + "baseFrictionThresholds": {}, + "vehicleKnobs": {"hyundai_ioniq_6.unwind_taper_left": 0.62}, + }, + } + for report_id, profile in ((first_report_id, first_profile), (second_report_id, second_profile)): + (workspace["reports"] / f"{report_id}.json").write_text(json.dumps({ + "reportId": report_id, + "car": {"carFingerprint": "TEST_CAR", "brand": "test"}, + }), encoding="utf-8") + (workspace["profiles"] / f"{report_id}.json").write_text(json.dumps([profile]), encoding="utf-8") + + fake_params_cls._store = { + "AdvancedLateralTune": False, + "ForceAutoTune": False, + "ForceAutoTuneOff": True, + "UseAutoSteerDelay": False, + "SteerDelay": 0.35, + "SteerFriction": 0.1, + "SteerKP": 1.0, + "SteerLatAccel": 1.5, + "SteerRatio": 15.0, + "FLMActiveProfileId": "", + "FLMActiveOverrides": {}, + "FLMTrialApplied": False, + } + + module.apply_trial_profile(first_report_id, first_profile_id) + first_tune = module.save_active_trial_as_tune("No Trailer")["tune"] + assert fake_params_cls._store["FLMActiveProfileId"] == f"saved:{first_tune['tuneId']}" + assert next(tune for tune in module.list_workspace()["savedTunes"] if tune["tuneId"] == first_tune["tuneId"])["active"] is True + module.revert_trial_profile() + module.apply_trial_profile(second_report_id, second_profile_id) + second_tune = module.save_active_trial_as_tune("With Trailer")["tune"] + + module.apply_saved_tune(first_tune["tuneId"]) + assert fake_params_cls._store["SteerFriction"] == pytest.approx(0.2) + assert fake_params_cls._store["SteerLatAccel"] == pytest.approx(1.9) + assert fake_params_cls._store["FLMActiveOverrides"]["vehicleKnobs"] == { + "hyundai_ioniq_6.turn_in_boost_left": pytest.approx(0.08), + } + + module.apply_saved_tune(second_tune["tuneId"]) + assert fake_params_cls._store["SteerFriction"] == pytest.approx(0.1) + assert fake_params_cls._store["SteerLatAccel"] == pytest.approx(2.0) + assert fake_params_cls._store["FLMActiveOverrides"]["vehicleKnobs"] == { + "hyundai_ioniq_6.unwind_taper_left": pytest.approx(0.62), + } + workspace_state = module.list_workspace() + assert next(tune for tune in workspace_state["savedTunes"] if tune["tuneId"] == second_tune["tuneId"])["active"] is True + + module.revert_trial_profile() + assert fake_params_cls._store["AdvancedLateralTune"] is False + assert fake_params_cls._store["SteerFriction"] == pytest.approx(0.1) + assert fake_params_cls._store["SteerLatAccel"] == pytest.approx(1.5) + assert fake_params_cls._store["FLMActiveOverrides"] == {} + assert fake_params_cls._store["FLMTrialApplied"] is False + + +def test_saved_tune_rename_delete_and_vehicle_guard(tmp_path, monkeypatch): + module, fake_params_cls = _load_flm_workspace_module(tmp_path) + workspace = module.ensure_flm_workspace() + tune_id = "tune-test" + tune_path = workspace["savedTunes"] / f"{tune_id}.json" + tune_path.write_text(json.dumps({ + "schemaVersion": 1, + "tuneId": tune_id, + "name": "Original", + "createdAt": 1.0, + "updatedAt": 1.0, + "carFingerprint": "CAR_A", + "genericParams": {"SteerLatAccel": 1.9}, + "flmOverrides": {}, + }), encoding="utf-8") + fake_params_cls._store = { + "SteerLatAccel": 1.5, + "FLMActiveProfileId": "", + "FLMActiveOverrides": {}, + "FLMTrialApplied": False, + } + + monkeypatch.setattr(module, "_current_car_identity", lambda _params: {"carFingerprint": "CAR_B", "brand": "test"}) + with pytest.raises(RuntimeError, match="connected car is CAR_B"): + module.apply_saved_tune(tune_id) + + monkeypatch.setattr(module, "_current_car_identity", lambda _params: {"carFingerprint": "CAR_A", "brand": "test"}) + rename_result = module.rename_saved_tune(tune_id, " Tow Setup ") + assert rename_result["tune"]["name"] == "Tow Setup" + module.apply_saved_tune(tune_id) + with pytest.raises(RuntimeError, match="Revert or switch"): + module.delete_saved_tune(tune_id) + module.revert_trial_profile() + delete_result = module.delete_saved_tune(tune_id) + assert "Deleted saved tune Tow Setup" in delete_result["message"] + assert not tune_path.exists() + + +def test_saved_tune_car_switch_uses_the_destination_car_baseline(tmp_path, monkeypatch): + module, fake_params_cls = _load_flm_workspace_module(tmp_path) + workspace = module.ensure_flm_workspace() + tune_id = "tune-car-b" + (workspace["savedTunes"] / f"{tune_id}.json").write_text(json.dumps({ + "schemaVersion": 1, + "tuneId": tune_id, + "name": "Car B", + "createdAt": 1.0, + "updatedAt": 1.0, + "carFingerprint": "CAR_B", + "baselineParams": { + "AdvancedLateralTune": False, + "SteerFriction": 0.08, + "SteerLatAccel": 1.3, + "FLMActiveProfileId": "", + "FLMActiveOverrides": {}, + "FLMTrialApplied": False, + }, + "genericParams": {"AdvancedLateralTune": True, "SteerLatAccel": 2.1}, + "flmOverrides": {}, + }), encoding="utf-8") + car_a_baseline = { + "AdvancedLateralTune": False, + "SteerFriction": 0.12, + "SteerLatAccel": 1.6, + "FLMActiveProfileId": "", + "FLMActiveOverrides": {}, + "FLMTrialApplied": False, + } + (workspace["snapshots"] / "active.json").write_text(json.dumps({ + "reportId": "", + "profileId": "saved:tune-car-a", + "profileLabel": "Car A", + "savedTuneId": "tune-car-a", + "carFingerprint": "CAR_A", + "capturedAt": 1.0, + "params": car_a_baseline, + "appliedGenericParams": {"AdvancedLateralTune": True, "SteerLatAccel": 1.9}, + "appliedFrictionThresholds": {}, + "appliedVehicleKnobs": {}, + }), encoding="utf-8") + fake_params_cls._store = { + "AdvancedLateralTune": True, + "SteerFriction": 0.12, + "SteerLatAccel": 1.9, + "FLMActiveProfileId": "saved:tune-car-a", + "FLMActiveOverrides": {}, + "FLMTrialApplied": True, + "FLMTrialBaseline": {"params": car_a_baseline}, + } + monkeypatch.setattr(module, "_current_car_identity", lambda _params: {"carFingerprint": "CAR_B", "brand": "test"}) + + module.apply_saved_tune(tune_id) + assert fake_params_cls._store["SteerFriction"] == pytest.approx(0.08) + assert fake_params_cls._store["SteerLatAccel"] == pytest.approx(2.1) + module.revert_trial_profile() + assert fake_params_cls._store["AdvancedLateralTune"] is False + assert fake_params_cls._store["SteerFriction"] == pytest.approx(0.08) + assert fake_params_cls._store["SteerLatAccel"] == pytest.approx(1.3) + + def test_orphaned_previous_revision_can_recover_its_baseline(tmp_path): module, fake_params_cls = _load_flm_workspace_module(tmp_path) workspace = module.ensure_flm_workspace() diff --git a/starpilot/system/the_galaxy/the_galaxy.py b/starpilot/system/the_galaxy/the_galaxy.py index 110524c96..9d8a31fde 100644 --- a/starpilot/system/the_galaxy/the_galaxy.py +++ b/starpilot/system/the_galaxy/the_galaxy.py @@ -5935,6 +5935,7 @@ def setup(app): "status": flm_workspace.read_flm_status(), "activeTrial": workspace.get("activeTrial"), "reports": workspace.get("reports", [])[:10], + "savedTunes": workspace.get("savedTunes", []), }), 200 @app.route(f"{LEGACY_LATERAL_METHOD_API_PREFIX}/analyze", methods=["POST"]) @@ -6018,6 +6019,48 @@ def setup(app): except RuntimeError as error: return jsonify({"error": str(error)}), 409 + @app.route(f"{LEGACY_LATERAL_METHOD_API_PREFIX}/saved-tunes", methods=["POST"]) + @app.route("/api/flm/saved-tunes", methods=["POST"]) + def save_flm_tune(): + data = request.get_json(silent=True) or {} + try: + return jsonify(flm_workspace.save_active_trial_as_tune(str(data.get("name") or ""))), 200 + except ValueError as error: + return jsonify({"error": str(error)}), 400 + except RuntimeError as error: + return jsonify({"error": str(error)}), 409 + + @app.route(f"{LEGACY_LATERAL_METHOD_API_PREFIX}/saved-tunes//apply", methods=["POST"]) + @app.route("/api/flm/saved-tunes//apply", methods=["POST"]) + def apply_flm_saved_tune(tune_id): + try: + return jsonify(flm_workspace.apply_saved_tune(tune_id)), 200 + except FileNotFoundError: + return jsonify({"error": "Saved FLM tune not found."}), 404 + except RuntimeError as error: + return jsonify({"error": str(error)}), 409 + + @app.route(f"{LEGACY_LATERAL_METHOD_API_PREFIX}/saved-tunes/", methods=["PATCH"]) + @app.route("/api/flm/saved-tunes/", methods=["PATCH"]) + def rename_flm_saved_tune(tune_id): + data = request.get_json(silent=True) or {} + try: + return jsonify(flm_workspace.rename_saved_tune(tune_id, str(data.get("name") or ""))), 200 + except FileNotFoundError: + return jsonify({"error": "Saved FLM tune not found."}), 404 + except ValueError as error: + return jsonify({"error": str(error)}), 400 + + @app.route(f"{LEGACY_LATERAL_METHOD_API_PREFIX}/saved-tunes/", methods=["DELETE"]) + @app.route("/api/flm/saved-tunes/", methods=["DELETE"]) + def delete_flm_saved_tune(tune_id): + try: + return jsonify(flm_workspace.delete_saved_tune(tune_id)), 200 + except FileNotFoundError: + return jsonify({"error": "Saved FLM tune not found."}), 404 + except RuntimeError as error: + return jsonify({"error": str(error)}), 409 + @app.route(f"{LEGACY_LATERAL_METHOD_API_PREFIX}/trials/apply", methods=["POST"]) @app.route("/api/flm/trials/apply", methods=["POST"]) def apply_flm_trial(): diff --git a/system/ui/lib/egl.py b/system/ui/lib/egl.py index 6676ca314..66d9d7ed3 100644 --- a/system/ui/lib/egl.py +++ b/system/ui/lib/egl.py @@ -60,6 +60,7 @@ class EGLState: active_texture: Any = None gen_textures: Any = None delete_textures: Any = None + gl_finish: Any = None # Create a single instance of the state @@ -99,6 +100,7 @@ def init_egl() -> bool: void glGenTextures(int n, unsigned int *textures); void glDeleteTextures(int n, const unsigned int *textures); GLenum glGetError(void); + void glFinish(void); """) # Load libraries @@ -121,6 +123,7 @@ def init_egl() -> bool: _egl.active_texture = _egl.gles_lib.glActiveTexture _egl.gen_textures = _egl.gles_lib.glGenTextures _egl.delete_textures = _egl.gles_lib.glDeleteTextures + _egl.gl_finish = _egl.gles_lib.glFinish # Initialize EGL display once here _egl.display = _egl.get_current_display() @@ -135,6 +138,15 @@ def init_egl() -> bool: return False +def is_egl_initialized() -> bool: + return _egl.initialized + + +def finish_gl() -> None: + if _egl.initialized: + _egl.gl_finish() + + def create_egl_image(width: int, height: int, stride: int, fd: int, uv_offset: int) -> EGLImage | None: assert _egl.initialized, "EGL not initialized" @@ -170,10 +182,12 @@ def create_egl_image(width: int, height: int, stride: int, fd: int, uv_offset: i return EGLImage(egl_image=egl_image, fd=dup_fd) -def destroy_egl_image(egl_image: EGLImage) -> None: +def destroy_egl_image(egl_image: EGLImage) -> bool: assert _egl.initialized, "EGL not initialized" - _egl.destroy_image_khr(_egl.display, egl_image.egl_image) + destroyed = bool(_egl.destroy_image_khr(_egl.display, egl_image.egl_image)) + if not destroyed: + cloudlog.error(f"Failed to destroy EGL image: {_egl.get_error()}") # Close the duplicated fd we created in create_egl_image() # We need to handle OSError since the fd might already be closed @@ -182,6 +196,8 @@ def destroy_egl_image(egl_image: EGLImage) -> None: except OSError: pass + return destroyed + def create_external_texture() -> int: """Create a texture name whose target is exclusively GL_TEXTURE_EXTERNAL_OES."""