IQ.Pilot Release Commit @ 661a2de

selfdrive/ui/layouts/settings/toggles.py

@@ -41,6 +41,10 @@ DESCRIPTIONS = {
   "DashcamEnabled": tr_noop("Record and upload driving data and video. Disabling this stops all recording! No logs, no video, no audio."),
   'RecordFront': tr_noop("Upload data from the driver facing camera and help improve the driver monitoring algorithm."),
   "IsMetric": tr_noop("Display speed in km/h instead of mph."),
+  "IQAutoUnits": tr_noop(
+    "Set the units from the device location. Speeds switch to km/h everywhere except the United States, " +
+    "the United Kingdom and Liberia, and are re-checked when you cross a border."
+  ),
   "RecordAudio": tr_noop("Record and store microphone audio while driving. The audio will be included in the dashcam video in Konn3kt."),
   "LongitudinalControlMode": tr_noop(
     "Choose longitudinal behavior: IQ.Pilot (IQ longitudinal + end-to-end), "
@@ -95,6 +99,12 @@ class TogglesLayout(Widget):
         "metric.png",
         False,
       ),
+      "IQAutoUnits": (
+        lambda: tr("Set Units From Location"),
+        DESCRIPTIONS["IQAutoUnits"],
+        "metric.png",
+        False,
+      ),
     }
 
     self._long_personality_setting = multiple_button_item(
@@ -296,6 +306,8 @@ class TogglesLayout(Widget):
 
   def _toggle_callback(self, state: bool, param: str):
     self._params.put_bool(param, state)
+    if param == "IQAutoUnits" and state:
+      self._params.remove("IQAutoUnitsRegion")
     if self._toggle_defs[param][3]:
       self._params.put_bool("OnroadCycleRequested", True)
 

selfdrive/ui/mici/layouts/settings/toggles.py

@@ -18,13 +18,15 @@ class TogglesLayoutMici(NavScroller):
     disengage = BigParamControl("disengage on accelerator", "DisengageOnAccelerator")
     ldw = BigParamControl("lane departure warnings", "IsLdwEnabled")
     is_metric = BigParamControl("use metric units", "IsMetric")
+    auto_units = BigParamControl("set units from location", "IQAutoUnits", toggle_callback=self._auto_units_callback)
 
-    self._scroller.add_widgets([disengage, ldw, is_metric])
+    self._scroller.add_widgets([disengage, ldw, is_metric, auto_units])
 
     self._refresh_toggles = (
       ("DisengageOnAccelerator", disengage),
       ("IsLdwEnabled", ldw),
       ("IsMetric", is_metric),
+      ("IQAutoUnits", auto_units),
     )
 
     if ui_state.params.get_bool("ShowDebugInfo"):
@@ -35,6 +37,10 @@ class TogglesLayoutMici(NavScroller):
     super().show_event()
     self._update_toggles()
 
+  def _auto_units_callback(self, state: bool):
+    if state:
+      ui_state.params.remove("IQAutoUnitsRegion")
+
   def _update_toggles(self):
     ui_state.update_params()
     for key, item in self._refresh_toggles:

selfdrive/ui/mici/onroad/cameraview.py

@@ -4,7 +4,7 @@ import pyray as rl
 
 from msgq.visionipc import VisionIpcClient, VisionStreamType, VisionBuf
 from openpilot.common.swaglog import cloudlog
-from openpilot.system.hardware import TICI
+from openpilot.system.hardware import EGL_DMA_BUF_SUPPORTED
 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, EGLImage
 from openpilot.system.ui.widgets import Widget
@@ -38,7 +38,7 @@ void main() {
 """
 
 # Choose fragment shader based on platform capabilities
-if TICI:
+if EGL_DMA_BUF_SUPPORTED:
   FRAME_FRAGMENT_SHADER = """
     #version 300 es
     #extension GL_OES_EGL_image_external_essl3 : enable
@@ -121,7 +121,7 @@ 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._texture1_loc: int = rl.get_shader_location(self.shader, "texture1") if not EGL_DMA_BUF_SUPPORTED 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")
@@ -137,8 +137,8 @@ class CameraView(Widget):
 
     self._placeholder_color: rl.Color | None = None
 
-    # Initialize EGL for zero-copy rendering on TICI
-    if TICI:
+    # Initialize EGL for zero-copy rendering on comma 3/3X.
+    if EGL_DMA_BUF_SUPPORTED:
       if not init_egl():
         raise RuntimeError("Failed to initialize EGL")
 
@@ -189,7 +189,7 @@ class CameraView(Widget):
     self._clear_textures()
 
     # Clean up EGL texture
-    if TICI and self.egl_texture:
+    if EGL_DMA_BUF_SUPPORTED and self.egl_texture:
       rl.unload_texture(self.egl_texture)
       self.egl_texture = None
 
@@ -264,7 +264,7 @@ class CameraView(Widget):
     dst_rect = rl.Rectangle(x_offset, y_offset, scale_x, scale_y)
 
     # Render with appropriate method
-    if TICI:
+    if EGL_DMA_BUF_SUPPORTED:
       self._render_egl(src_rect, dst_rect)
     else:
       self._render_textures(src_rect, dst_rect)
@@ -387,12 +387,12 @@ class CameraView(Widget):
     self._initialize_textures()
 
   def _initialize_textures(self):
-      self._clear_textures()
-      if not TICI:
-        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))
+    self._clear_textures()
+    if not EGL_DMA_BUF_SUPPORTED:
+      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 _clear_textures(self):
     if self.texture_y and self.texture_y.id:
@@ -404,7 +404,7 @@ class CameraView(Widget):
       self.texture_uv = None
 
     # Clean up EGL resources
-    if TICI:
+    if EGL_DMA_BUF_SUPPORTED:
       for data in self.egl_images.values():
         destroy_egl_image(data)
       self.egl_images = {}

selfdrive/ui/onroad/augmented_road_view.py

@@ -9,6 +9,7 @@ from openpilot.selfdrive.ui.onroad.alert_renderer import AlertRenderer
 from openpilot.selfdrive.ui.onroad.driver_state import DriverStateRenderer as BaseDriverStateRenderer, BTN_SIZE
 from openpilot.selfdrive.ui.onroad.hud_renderer import HudRenderer as BaseHudRenderer
 from openpilot.selfdrive.ui.onroad.model_renderer import ModelRenderer
+from openpilot.selfdrive.ui.onroad.environment_renderer import EnvironmentRenderer
 from openpilot.selfdrive.ui.onroad.cameraview import CameraView
 from openpilot.system.ui.lib.application import gui_app
 from openpilot.common.issue_debug import log_issue_limited
@@ -55,6 +56,7 @@ class AugmentedRoadView(CameraView, AugmentedRoadViewIQ):
     self._split_nav_available = False
 
     self.model_renderer = ModelRenderer()
+    self.environment_renderer = EnvironmentRenderer()
     self.alert_renderer = AlertRenderer()
     self._hud_renderer = IQHudRenderer()
     self.driver_state_renderer = DriverStateRendererIQ()
@@ -114,6 +116,7 @@ class AugmentedRoadView(CameraView, AugmentedRoadViewIQ):
 
     # Draw all UI overlays
     self.model_renderer.render(camera_rect)
+    self.environment_renderer.render(camera_rect)
     AugmentedRoadViewIQ.update_fade_out_bottom_overlay(self, camera_rect)
     self._hud_renderer.render(camera_rect)
 
@@ -280,6 +283,7 @@ class AugmentedRoadView(CameraView, AugmentedRoadViewIQ):
     ])
     self.model_renderer.set_transform(video_transform @ calib_transform)
     self.model_renderer.set_frame_transform(video_transform, is_wide_camera)
+    self.environment_renderer.set_transform(video_transform @ calib_transform)
 
     return self._cached_matrix
 

selfdrive/ui/onroad/cameraview.py

@@ -4,7 +4,7 @@ import pyray as rl
 
 from msgq.visionipc import VisionIpcClient, VisionStreamType, VisionBuf
 from openpilot.common.swaglog import cloudlog
-from openpilot.system.hardware import TICI
+from openpilot.system.hardware import EGL_DMA_BUF_SUPPORTED
 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, EGLImage
 from openpilot.system.ui.widgets import Widget
@@ -38,7 +38,7 @@ void main() {
 """
 
 # Choose fragment shader based on platform capabilities
-if TICI:
+if EGL_DMA_BUF_SUPPORTED:
   FRAME_FRAGMENT_SHADER = """
     #version 300 es
     #extension GL_OES_EGL_image_external_essl3 : enable
@@ -82,7 +82,7 @@ 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._texture1_loc: int = rl.get_shader_location(self.shader, "texture1") if not EGL_DMA_BUF_SUPPORTED else -1
 
     self.frame: VisionBuf | None = None
     self.texture_y: rl.Texture | None = None
@@ -94,8 +94,8 @@ class CameraView(Widget):
 
     self._placeholder_color: rl.Color | None = None
 
-    # Initialize EGL for zero-copy rendering on TICI
-    if TICI:
+    # Initialize EGL for zero-copy rendering on comma 3/3X.
+    if EGL_DMA_BUF_SUPPORTED:
       if not init_egl():
         raise RuntimeError("Failed to initialize EGL")
 
@@ -146,7 +146,7 @@ class CameraView(Widget):
     self._clear_textures()
 
     # Clean up EGL texture
-    if TICI and self.egl_texture:
+    if EGL_DMA_BUF_SUPPORTED and self.egl_texture:
       rl.unload_texture(self.egl_texture)
       self.egl_texture = None
 
@@ -220,7 +220,7 @@ class CameraView(Widget):
     dst_rect = rl.Rectangle(x_offset, y_offset, scale_x, scale_y)
 
     # Render with appropriate method
-    if TICI:
+    if EGL_DMA_BUF_SUPPORTED:
       self._render_egl(src_rect, dst_rect)
     else:
       self._render_textures(src_rect, dst_rect)
@@ -337,7 +337,7 @@ class CameraView(Widget):
 
   def _initialize_textures(self):
     self._clear_textures()
-    if not TICI:
+    if not EGL_DMA_BUF_SUPPORTED:
       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),
@@ -353,7 +353,7 @@ class CameraView(Widget):
       self.texture_uv = None
 
     # Clean up EGL resources
-    if TICI:
+    if EGL_DMA_BUF_SUPPORTED:
       for data in self.egl_images.values():
         destroy_egl_image(data)
       self.egl_images = {}

selfdrive/ui/onroad/environment_renderer.py

@@ -0,0 +1,141 @@
+import numpy as np
+import pyray as rl
+from cereal import custom
+
+from openpilot.selfdrive.ui.ui_state import ui_state
+from openpilot.system.ui.lib.application import gui_app
+from openpilot.system.ui.widgets import Widget
+
+MODE_OFF = 0
+MODE_OVERLAY = 1
+MODE_REPLACE = 2
+
+_ENV_LABEL = custom.IQEnvironment.Object.Label
+_OBJECT_COLORS = {
+  _ENV_LABEL.car: (40, 210, 200),
+  _ENV_LABEL.truck: (40, 210, 200),
+  _ENV_LABEL.bus: (40, 210, 200),
+  _ENV_LABEL.motorcycle: (90, 220, 255),
+  _ENV_LABEL.bicycle: (90, 220, 255),
+  _ENV_LABEL.person: (255, 210, 90),
+  _ENV_LABEL.stopSign: (255, 60, 45),
+  _ENV_LABEL.trafficLight: (255, 190, 0),
+}
+
+_BOX_EDGES = (
+  (0, 1), (1, 3), (3, 2), (2, 0),
+  (4, 5), (5, 7), (7, 6), (6, 4),
+  (0, 4), (1, 5), (2, 6), (3, 7),
+)
+
+GRID_HALF_WIDTH = 12.0
+GRID_MAX_DISTANCE = 90.0
+GRID_STEP = 6.0
+
+
+class EnvironmentRenderer(Widget):
+  def __init__(self):
+    Widget.__init__(self)
+    self._car_space_transform = np.zeros((3, 3), dtype=np.float32)
+    self._mode = MODE_OFF
+    self._counter = 0
+
+  def set_transform(self, transform: np.ndarray):
+    self._car_space_transform = transform.astype(np.float32)
+
+  def _project(self, pt: np.ndarray):
+    p = self._car_space_transform @ pt
+    if abs(p[2]) < 1e-6:
+      return None
+    return p[0] / p[2], p[1] / p[2]
+
+  def _in_rect(self, x: float, y: float) -> bool:
+    r = self._rect
+    return r.x - 400 <= x <= r.x + r.width + 400 and r.y - 400 <= y <= r.y + r.height + 400
+
+  def _render(self, rect: rl.Rectangle):
+    sm = ui_state.sm
+    if self._counter % 30 == 0:
+      self._mode = int(ui_state.params.get("EnvironmentView", return_default=True) or 0) if ui_state.active_bundle else 0
+    self._counter += 1
+
+    if self._mode == MODE_OFF:
+      return
+    if sm.recv_frame["liveCalibration"] < ui_state.started_frame:
+      return
+
+    if self._mode == MODE_REPLACE:
+      self._draw_backdrop(rect)
+      self._draw_ground_grid()
+      if sm.valid["modelV2"]:
+        self._draw_model_scene(sm["modelV2"])
+
+    if sm.alive["iqEnvironment"] and sm.valid["iqEnvironment"]:
+      self._draw_objects(sm["iqEnvironment"])
+
+  def _draw_backdrop(self, rect: rl.Rectangle):
+    rl.draw_rectangle_gradient_v(int(rect.x), int(rect.y), int(rect.width), int(rect.height),
+                                 rl.Color(14, 17, 22, 255), rl.Color(6, 8, 11, 255))
+
+  def _draw_ground_grid(self):
+    col = rl.Color(60, 70, 82, 90)
+    dist = GRID_STEP
+    while dist <= GRID_MAX_DISTANCE:
+      a = self._project(np.array([dist, -GRID_HALF_WIDTH, 0.0]))
+      b = self._project(np.array([dist, GRID_HALF_WIDTH, 0.0]))
+      if a and b and self._in_rect(*a) and self._in_rect(*b):
+        rl.draw_line_ex(rl.Vector2(*a), rl.Vector2(*b), 1.5, col)
+      dist += GRID_STEP
+    for off in np.arange(-GRID_HALF_WIDTH, GRID_HALF_WIDTH + 0.1, 3.0):
+      a = self._project(np.array([GRID_STEP, float(off), 0.0]))
+      b = self._project(np.array([GRID_MAX_DISTANCE, float(off), 0.0]))
+      if a and b and self._in_rect(*a) and self._in_rect(*b):
+        rl.draw_line_ex(rl.Vector2(*a), rl.Vector2(*b), 1.5, col)
+
+  def _draw_polyline(self, xs, ys, zs, color, thick):
+    pts = []
+    for x, y, z in zip(xs, ys, zs, strict=False):
+      if x < 0:
+        continue
+      s = self._project(np.array([x, y, z], dtype=np.float32))
+      if s and self._in_rect(*s):
+        pts.append(rl.Vector2(*s))
+    for i in range(len(pts) - 1):
+      rl.draw_line_ex(pts[i], pts[i + 1], thick, color)
+
+  def _draw_model_scene(self, model):
+    for i, lane in enumerate(model.laneLines):
+      a = int(np.clip(model.laneLineProbs[i], 0.0, 0.9) * 255)
+      self._draw_polyline(lane.x, lane.y, lane.z, rl.Color(235, 235, 235, a), 3.0)
+    for edge in model.roadEdges:
+      self._draw_polyline(edge.x, edge.y, edge.z, rl.Color(230, 70, 70, 180), 3.0)
+    pos = model.position
+    self._draw_polyline(pos.x, pos.y, pos.z, rl.Color(40, 210, 200, 220), 6.0)
+
+  def _draw_objects(self, env):
+    for obj in env.objects:
+      self._draw_box(obj)
+
+  def _draw_box(self, obj):
+    hx, hy = obj.length / 2.0, obj.width / 2.0
+    base = np.array([
+      [obj.x - hx, obj.y - hy, obj.z], [obj.x - hx, obj.y + hy, obj.z],
+      [obj.x + hx, obj.y - hy, obj.z], [obj.x + hx, obj.y + hy, obj.z],
+      [obj.x - hx, obj.y - hy, obj.z + obj.height], [obj.x - hx, obj.y + hy, obj.z + obj.height],
+      [obj.x + hx, obj.y - hy, obj.z + obj.height], [obj.x + hx, obj.y + hy, obj.z + obj.height],
+    ], dtype=np.float32)
+
+    screen = []
+    for corner in base:
+      s = self._project(corner)
+      if s is None or not self._in_rect(*s):
+        return
+      screen.append(s)
+
+    r, g, b = _OBJECT_COLORS.get(obj.label, (40, 210, 200))
+    a = int(np.clip(obj.prob, 0.3, 1.0) * 210)
+    floor = [rl.Vector2(*screen[i]) for i in (0, 1, 3, 2)]
+    rl.draw_triangle(floor[0], floor[1], floor[2], rl.Color(r, g, b, a // 5))
+    rl.draw_triangle(floor[0], floor[2], floor[3], rl.Color(r, g, b, a // 5))
+    for i, j in _BOX_EDGES:
+      rl.draw_line_ex(rl.Vector2(*screen[i]), rl.Vector2(*screen[j]), 2.0, rl.Color(r, g, b, a))

selfdrive/ui/soundd.py

@@ -224,6 +224,17 @@ class Soundd(AlertSoundFilter):
     sm = messaging.SubMaster(['selfdriveState', 'soundPressure'])
     threading.Thread(target=self.webrtc_audio_thread, daemon=True).start()
 
+    while True:
+      try:
+        self._stream_loop(sd, sm)
+      except Exception:
+        # Some A1s wedge the audio DSP (ALSA EINVAL / ADSP_EFAILED until reboot). Dying here
+        # crash-loops the process and selfdrived raises a takeover alert mid-drive over alert
+        # sounds - stay alive and keep retrying instead; recovers if the DSP comes back.
+        cloudlog.exception("soundd: audio stream unavailable, retrying")
+        time.sleep(10)
+
+  def _stream_loop(self, sd, sm):
     with self.get_stream(sd) as stream:
       rk = Ratekeeper(20)
 

selfdrive/ui/ui_state.py

@@ -8,6 +8,7 @@ from cereal import messaging, car, log, custom
 from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.common.params import Params
 from openpilot.common.swaglog import cloudlog
+from openpilot.iqpilot.common.auto_units import AutoUnits
 from openpilot.selfdrive.ui.lib.prime_state import PrimeState
 from openpilot.system.ui.lib.application import gui_app
 from openpilot.system.hardware import HARDWARE, PC
@@ -42,12 +43,13 @@ class IQUIState:
 
     self.update_params()
 
+    self.auto_units = AutoUnits(self.params)
     self.onroad_brightness_timer: int = 0
     self.custom_interactive_timeout: int = self.params.get("InteractivityTimeout", return_default=True)
     self.reset_onroad_sleep_timer()
 
   def update(self) -> None:
-    pass
+    self.auto_units.update()
 
   def onroad_brightness_handle_alerts(self, started: bool, alert):
     # while an alert is on screen the dim countdown is frozen and re-armed; otherwise it ticks down
@@ -254,6 +256,7 @@ class UIState(IQUIState):
         "radarState",
         "liveTracks",
         "iqVehicleTracks",
+        "iqEnvironment",
         "deviceState",
         "pandaStates",
         "carParams",