From eabee8c73f3c8fc4dcadb47f04ee527b99c44a03 Mon Sep 17 00:00:00 2001 From: Trey Moen <50057480+greatgitsby@users.noreply.github.com> Date: Sun, 25 May 2025 09:33:37 -0700 Subject: [PATCH 01/60] bump msgq (#35345) --- msgq_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/msgq_repo b/msgq_repo index 94e738992c..5483a02de3 160000 --- a/msgq_repo +++ b/msgq_repo @@ -1 +1 @@ -Subproject commit 94e738992ca8a12c2d9bb7bdeeaab4713543773e +Subproject commit 5483a02de303d40cb2632d59f3f3a54dabfb5965 From 22715464b97365b60ca812b23bc3c6f6e9001d3e Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Mon, 26 May 2025 00:47:41 +0800 Subject: [PATCH 02/60] system/ui: throttle camera connection attempts (#35343) throttle connection attempts --- system/ui/widgets/cameraview.py | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/system/ui/widgets/cameraview.py b/system/ui/widgets/cameraview.py index 01aac0370b..94625ab19d 100644 --- a/system/ui/widgets/cameraview.py +++ b/system/ui/widgets/cameraview.py @@ -5,6 +5,8 @@ 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 +CONNECTION_RETRY_INTERVAL = 0.2 # seconds between connection attempts + VERTEX_SHADER = """ #version 300 es in vec3 vertexPosition; @@ -53,6 +55,7 @@ else: class CameraView: def __init__(self, name: str, stream_type: VisionStreamType): self.client = VisionIpcClient(name, stream_type, False) + self.last_connection_attempt: float = 0.0 self.shader = rl.load_shader_from_memory(VERTEX_SHADER, FRAME_FRAGMENT_SHADER) self.frame: VisionBuf | None = None @@ -159,6 +162,14 @@ class CameraView: def _ensure_connection(self) -> bool: if not self.client.is_connected(): self.frame = None + + # 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 + if not self.client.connect(False) or not self.client.num_buffers: return False From 193df11a1c22b85fe914b772de66c0ce49a36354 Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Tue, 27 May 2025 00:29:29 +0800 Subject: [PATCH 03/60] system/ui: avoid redundant texture updates (#35347) avoid redundant texture updates --- system/ui/widgets/cameraview.py | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/system/ui/widgets/cameraview.py b/system/ui/widgets/cameraview.py index 94625ab19d..fe53507cef 100644 --- a/system/ui/widgets/cameraview.py +++ b/system/ui/widgets/cameraview.py @@ -55,6 +55,7 @@ else: class CameraView: def __init__(self, name: str, stream_type: VisionStreamType): self.client = VisionIpcClient(name, stream_type, False) + self._texture_needs_update = True self.last_connection_attempt: float = 0.0 self.shader = rl.load_shader_from_memory(VERTEX_SHADER, FRAME_FRAGMENT_SHADER) @@ -95,6 +96,7 @@ class CameraView: # Try to get a new buffer without blocking buffer = self.client.recv(timeout_ms=0) if buffer: + self._texture_needs_update = True self.frame = buffer if not self.frame: @@ -147,11 +149,13 @@ class CameraView: return # Update textures with new frame data - y_data = self.frame.data[: self.frame.uv_offset] - uv_data = self.frame.data[self.frame.uv_offset :] + 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)) + 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) From 3d3e9599d804149e0f3036cb46ec1b74fbc0c04c Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Tue, 27 May 2025 01:42:50 +0800 Subject: [PATCH 04/60] system/ui: add specialized camera views with custom transformations (#35346) * add specialized camera views with custom transformations for driver and road * improve * return np array * cached matrix --- system/ui/onroad/augmented_road_view.py | 127 ++++++++++++++++++++++++ system/ui/widgets/cameraview.py | 39 +++++++- system/ui/widgets/driver_camera_view.py | 46 +++++++++ 3 files changed, 207 insertions(+), 5 deletions(-) create mode 100644 system/ui/onroad/augmented_road_view.py create mode 100644 system/ui/widgets/driver_camera_view.py diff --git a/system/ui/onroad/augmented_road_view.py b/system/ui/onroad/augmented_road_view.py new file mode 100644 index 0000000000..58a37460d5 --- /dev/null +++ b/system/ui/onroad/augmented_road_view.py @@ -0,0 +1,127 @@ +import numpy as np +import pyray as rl + +from cereal import messaging, log +from msgq.visionipc import VisionStreamType +from openpilot.system.ui.widgets.cameraview import CameraView +from openpilot.system.ui.lib.application import gui_app +from openpilot.common.transformations.camera import DEVICE_CAMERAS, DeviceCameraConfig, view_frame_from_device_frame +from openpilot.common.transformations.orientation import rot_from_euler + + +CALIBRATED = log.LiveCalibrationData.Status.calibrated +DEFAULT_DEVICE_CAMERA = DEVICE_CAMERAS["tici", "ar0231"] + + +class AugmentedRoadView(CameraView): + def __init__(self, sm: messaging.SubMaster, stream_type: VisionStreamType): + super().__init__("camerad", stream_type) + + self.sm = sm + self.stream_type = stream_type + self.is_wide_camera = stream_type == VisionStreamType.VISION_STREAM_WIDE_ROAD + + self.device_camera: DeviceCameraConfig | None = None + self.view_from_calib = view_frame_from_device_frame.copy() + self.view_from_wide_calib = view_frame_from_device_frame.copy() + + self._last_calib_time: float = 0 + self._last_recect_dims = (0.0, 0.0) + self._cacheed_matrix: np.ndarray | None = None + + def render(self, rect): + # Update calibration before rendering + self._update_calibration() + + # Render the base camera view + super().render(rect) + + # TODO: Add road visualization overlays like: + # - Lane lines and road edges + # - Path prediction + # - Lead vehicle indicators + # - Additional features + + def _update_calibration(self): + # Update device camera if not already set + if not self.device_camera and sm.seen['roadCameraState'] and sm.seen['deviceState']: + self.device_camera = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['roadCameraState'].sensor))] + + # Check if live calibration data is available and valid + if not (sm.updated["liveCalibration"] and sm.valid['liveCalibration']): + return + + calib = self.sm['liveCalibration'] + if len(calib.rpyCalib) != 3 or calib.calStatus != CALIBRATED: + return + + # Update view_from_calib matrix + device_from_calib = rot_from_euler(calib.rpyCalib) + self.view_from_calib = view_frame_from_device_frame @ device_from_calib + + # Update wide calibration if available + if hasattr(calib, 'wideFromDeviceEuler') and len(calib.wideFromDeviceEuler) == 3: + wide_from_device = rot_from_euler(calib.wideFromDeviceEuler) + self.view_from_wide_calib = view_frame_from_device_frame @ wide_from_device @ device_from_calib + + def _calc_frame_matrix(self, rect: rl.Rectangle) -> np.ndarray: + # Check if we can use cached matrix + calib_time = self.sm.recv_frame.get('liveCalibration', 0) + current_dims = (rect.width, rect.height) + if (self._last_calib_time == calib_time and + self._last_recect_dims == current_dims and + self._cacheed_matrix is not None): + return self._cacheed_matrix + + # Get camera configuration + device_camera = self.device_camera or DEFAULT_DEVICE_CAMERA + intrinsic = device_camera.ecam.intrinsics if self.is_wide_camera else device_camera.fcam.intrinsics + calibration = self.view_from_wide_calib if self.is_wide_camera else self.view_from_calib + zoom = 2.0 if self.is_wide_camera else 1.1 + + # Calculate transforms for vanishing point + inf_point = np.array([1000.0, 0.0, 0.0]) + calib_transform = intrinsic @ calibration + kep = calib_transform @ inf_point + + # Calculate center points and dimensions + w, h = current_dims + cx, cy = intrinsic[0, 2], intrinsic[1, 2] + + # Calculate max allowed offsets with margins + margin = 5 + max_x_offset = cx * zoom - w / 2 - margin + max_y_offset = cy * zoom - h / 2 - margin + + # Calculate and clamp offsets to prevent out-of-bounds issues + try: + if abs(kep[2]) > 1e-6: + x_offset = np.clip((kep[0] / kep[2] - cx) * zoom, -max_x_offset, max_x_offset) + y_offset = np.clip((kep[1] / kep[2] - cy) * zoom, -max_y_offset, max_y_offset) + else: + x_offset, y_offset = 0, 0 + except (ZeroDivisionError, OverflowError): + x_offset, y_offset = 0, 0 + + # Update cache values + self._last_calib_time = calib_time + self._last_recect_dims = current_dims + self._cacheed_matrix = np.array([ + [zoom * 2 * cx / w, 0, -x_offset / w * 2], + [0, zoom * 2 * cy / h, -y_offset / h * 2], + [0, 0, 1.0] + ]) + + return self._cacheed_matrix + + +if __name__ == "__main__": + gui_app.init_window("OnRoad Camera View") + sm = messaging.SubMaster(['deviceState', 'liveCalibration', 'roadCameraState']) + road_camera_view = AugmentedRoadView(sm, VisionStreamType.VISION_STREAM_ROAD) + try: + for _ in gui_app.render(): + sm.update(0) + road_camera_view.render(rl.Rectangle(0, 0, gui_app.width, gui_app.height)) + finally: + road_camera_view.close() diff --git a/system/ui/widgets/cameraview.py b/system/ui/widgets/cameraview.py index fe53507cef..bff13a7910 100644 --- a/system/ui/widgets/cameraview.py +++ b/system/ui/widgets/cameraview.py @@ -1,4 +1,6 @@ +import numpy as np import pyray as rl + from openpilot.system.hardware import TICI from msgq.visionipc import VisionIpcClient, VisionStreamType, VisionBuf from openpilot.system.ui.lib.application import gui_app @@ -89,6 +91,24 @@ class CameraView: if self.shader and self.shader.id: rl.unload_shader(self.shader) + 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 not self._ensure_connection(): return @@ -102,12 +122,21 @@ class CameraView: if not self.frame: return - # Calculate scaling to maintain aspect ratio - scale = min(rect.width / self.frame.width, rect.height / self.frame.height) - x_offset = rect.x + (rect.width - (self.frame.width * scale)) / 2 - y_offset = rect.y + (rect.height - (self.frame.height * scale)) / 2 + transform = self._calc_frame_matrix(rect) src_rect = rl.Rectangle(0, 0, float(self.frame.width), float(self.frame.height)) - dst_rect = rl.Rectangle(x_offset, y_offset, self.frame.width * scale, self.frame.height * scale) + + # 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 TICI: diff --git a/system/ui/widgets/driver_camera_view.py b/system/ui/widgets/driver_camera_view.py new file mode 100644 index 0000000000..174fac378d --- /dev/null +++ b/system/ui/widgets/driver_camera_view.py @@ -0,0 +1,46 @@ +import numpy as np +import pyray as rl +from openpilot.system.ui.widgets.cameraview import CameraView +from msgq.visionipc import VisionStreamType +from openpilot.system.ui.lib.application import gui_app + + +class DriverCameraView(CameraView): + def __init__(self, stream_type: VisionStreamType): + super().__init__("camerad", stream_type) + + def render(self, rect): + super().render(rect) + + # TODO: Add additional rendering logic + + def _calc_frame_matrix(self, rect: rl.Rectangle) -> np.ndarray: + driver_view_ratio = 2.0 + + # Get stream dimensions + if self.frame: + stream_width = self.frame.width + stream_height = self.frame.height + else: + # Default values if frame not available + stream_width = 1928 + stream_height = 1208 + + yscale = stream_height * driver_view_ratio / stream_width + xscale = yscale * rect.height / rect.width * stream_width / stream_height + + return np.array([ + [xscale, 0.0, 0.0], + [0.0, yscale, 0.0], + [0.0, 0.0, 1.0] + ]) + + +if __name__ == "__main__": + gui_app.init_window("Driver Camera View") + driver_camera_view = DriverCameraView(VisionStreamType.VISION_STREAM_DRIVER) + try: + for _ in gui_app.render(): + driver_camera_view.render(rl.Rectangle(0, 0, gui_app.width, gui_app.height)) + finally: + driver_camera_view.close() From 8ffcddce30b07dd7871189d01ec9bbc1a6fbedeb Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Mon, 26 May 2025 11:02:14 -0700 Subject: [PATCH 05/60] [bot] Update translations (#35348) Update translations Co-authored-by: Vehicle Researcher --- selfdrive/ui/translations/main_ar.ts | 28 ++++++++++++++++++++---- selfdrive/ui/translations/main_de.ts | 28 ++++++++++++++++++++---- selfdrive/ui/translations/main_es.ts | 28 ++++++++++++++++++++---- selfdrive/ui/translations/main_fr.ts | 28 ++++++++++++++++++++---- selfdrive/ui/translations/main_ja.ts | 28 ++++++++++++++++++++---- selfdrive/ui/translations/main_ko.ts | 28 ++++++++++++++++++++---- selfdrive/ui/translations/main_pt-BR.ts | 28 ++++++++++++++++++++---- selfdrive/ui/translations/main_th.ts | 28 ++++++++++++++++++++---- selfdrive/ui/translations/main_tr.ts | 28 ++++++++++++++++++++---- selfdrive/ui/translations/main_zh-CHS.ts | 28 ++++++++++++++++++++---- selfdrive/ui/translations/main_zh-CHT.ts | 28 ++++++++++++++++++++---- 11 files changed, 264 insertions(+), 44 deletions(-) diff --git a/selfdrive/ui/translations/main_ar.ts b/selfdrive/ui/translations/main_ar.ts index fa3b2d69e7..603d6a01cd 100644 --- a/selfdrive/ui/translations/main_ar.ts +++ b/selfdrive/ui/translations/main_ar.ts @@ -62,10 +62,6 @@ Cellular Metered محدود بالاتصال الخلوي - - Prevent large data uploads when on a metered connection - منع تحميل البيانات الكبيرة عندما يكون الاتصال محدوداً - Hidden Network شبكة مخفية @@ -86,6 +82,30 @@ for "%1" من أجل "%1" + + Prevent large data uploads when on a metered cellular connection + + + + default + + + + metered + + + + unmetered + + + + Wi-Fi Network Metered + + + + Prevent large data uploads when on a metered Wi-FI connection + + ConfirmationDialog diff --git a/selfdrive/ui/translations/main_de.ts b/selfdrive/ui/translations/main_de.ts index 3123e52578..aa6cf75c30 100644 --- a/selfdrive/ui/translations/main_de.ts +++ b/selfdrive/ui/translations/main_de.ts @@ -62,10 +62,6 @@ Cellular Metered Getaktete Verbindung - - Prevent large data uploads when on a metered connection - Hochladen großer Dateien über getaktete Verbindungen unterbinden - Hidden Network Verborgenes Netzwerk @@ -86,6 +82,30 @@ for "%1" für "%1" + + Prevent large data uploads when on a metered cellular connection + + + + default + + + + metered + + + + unmetered + + + + Wi-Fi Network Metered + + + + Prevent large data uploads when on a metered Wi-FI connection + + ConfirmationDialog diff --git a/selfdrive/ui/translations/main_es.ts b/selfdrive/ui/translations/main_es.ts index 3d9be63caf..64272244ea 100644 --- a/selfdrive/ui/translations/main_es.ts +++ b/selfdrive/ui/translations/main_es.ts @@ -62,10 +62,6 @@ Cellular Metered Plano de datos limitado - - Prevent large data uploads when on a metered connection - Evitar grandes descargas de datos cuando tenga una conexión limitada - Hidden Network Red Oculta @@ -86,6 +82,30 @@ for "%1" para "%1" + + Prevent large data uploads when on a metered cellular connection + + + + default + + + + metered + + + + unmetered + + + + Wi-Fi Network Metered + + + + Prevent large data uploads when on a metered Wi-FI connection + + ConfirmationDialog diff --git a/selfdrive/ui/translations/main_fr.ts b/selfdrive/ui/translations/main_fr.ts index 69652510df..e7c4fa506a 100644 --- a/selfdrive/ui/translations/main_fr.ts +++ b/selfdrive/ui/translations/main_fr.ts @@ -62,10 +62,6 @@ Cellular Metered Connexion cellulaire limitée - - Prevent large data uploads when on a metered connection - Éviter les transferts de données importants sur une connexion limitée - Hidden Network Réseau Caché @@ -86,6 +82,30 @@ for "%1" pour "%1" + + Prevent large data uploads when on a metered cellular connection + + + + default + + + + metered + + + + unmetered + + + + Wi-Fi Network Metered + + + + Prevent large data uploads when on a metered Wi-FI connection + + ConfirmationDialog diff --git a/selfdrive/ui/translations/main_ja.ts b/selfdrive/ui/translations/main_ja.ts index 4cce79757f..8fff080283 100644 --- a/selfdrive/ui/translations/main_ja.ts +++ b/selfdrive/ui/translations/main_ja.ts @@ -62,10 +62,6 @@ Cellular Metered 従量制通信設定 - - Prevent large data uploads when on a metered connection - 大量のデータのアップロードを防止する - Hidden Network ネットワーク非表示 @@ -86,6 +82,30 @@ for "%1" [%1] + + Prevent large data uploads when on a metered cellular connection + + + + default + + + + metered + + + + unmetered + + + + Wi-Fi Network Metered + + + + Prevent large data uploads when on a metered Wi-FI connection + + ConfirmationDialog diff --git a/selfdrive/ui/translations/main_ko.ts b/selfdrive/ui/translations/main_ko.ts index 3be96846a7..268475f917 100644 --- a/selfdrive/ui/translations/main_ko.ts +++ b/selfdrive/ui/translations/main_ko.ts @@ -62,10 +62,6 @@ Cellular Metered 데이터 요금제 - - Prevent large data uploads when on a metered connection - 데이터 요금제 연결 시 대용량 데이터 업로드를 방지합니다 - Hidden Network 숨겨진 네트워크 @@ -86,6 +82,30 @@ for "%1" "%1"에 접속하려면 비밀번호가 필요합니다 + + Prevent large data uploads when on a metered cellular connection + + + + default + + + + metered + + + + unmetered + + + + Wi-Fi Network Metered + + + + Prevent large data uploads when on a metered Wi-FI connection + + ConfirmationDialog diff --git a/selfdrive/ui/translations/main_pt-BR.ts b/selfdrive/ui/translations/main_pt-BR.ts index a6a473bc92..c5e4926724 100644 --- a/selfdrive/ui/translations/main_pt-BR.ts +++ b/selfdrive/ui/translations/main_pt-BR.ts @@ -62,10 +62,6 @@ Cellular Metered Plano de Dados Limitado - - Prevent large data uploads when on a metered connection - Evite grandes uploads de dados quando estiver em uma conexão limitada - Hidden Network Rede Oculta @@ -86,6 +82,30 @@ for "%1" para "%1" + + Prevent large data uploads when on a metered cellular connection + + + + default + + + + metered + + + + unmetered + + + + Wi-Fi Network Metered + + + + Prevent large data uploads when on a metered Wi-FI connection + + ConfirmationDialog diff --git a/selfdrive/ui/translations/main_th.ts b/selfdrive/ui/translations/main_th.ts index c7521082c3..e04cdac3e3 100644 --- a/selfdrive/ui/translations/main_th.ts +++ b/selfdrive/ui/translations/main_th.ts @@ -62,10 +62,6 @@ Cellular Metered ลดการส่งข้อมูลผ่านเซลลูล่าร์ - - Prevent large data uploads when on a metered connection - ปิดการอัพโหลดข้อมูลขนาดใหญ่เมื่อเชื่อมต่อผ่านเซลลูล่าร์ - Hidden Network เครือข่ายที่ซ่อนอยู่ @@ -86,6 +82,30 @@ for "%1" สำหรับ "%1" + + Prevent large data uploads when on a metered cellular connection + + + + default + + + + metered + + + + unmetered + + + + Wi-Fi Network Metered + + + + Prevent large data uploads when on a metered Wi-FI connection + + ConfirmationDialog diff --git a/selfdrive/ui/translations/main_tr.ts b/selfdrive/ui/translations/main_tr.ts index b8b7202898..181c9c44f3 100644 --- a/selfdrive/ui/translations/main_tr.ts +++ b/selfdrive/ui/translations/main_tr.ts @@ -62,10 +62,6 @@ Cellular Metered - - Prevent large data uploads when on a metered connection - - Hidden Network @@ -86,6 +82,30 @@ for "%1" için "%1" + + Prevent large data uploads when on a metered cellular connection + + + + default + + + + metered + + + + unmetered + + + + Wi-Fi Network Metered + + + + Prevent large data uploads when on a metered Wi-FI connection + + ConfirmationDialog diff --git a/selfdrive/ui/translations/main_zh-CHS.ts b/selfdrive/ui/translations/main_zh-CHS.ts index c310265a1c..03e7a960be 100644 --- a/selfdrive/ui/translations/main_zh-CHS.ts +++ b/selfdrive/ui/translations/main_zh-CHS.ts @@ -62,10 +62,6 @@ Cellular Metered 按流量计费的手机移动网络 - - Prevent large data uploads when on a metered connection - 当使用按流量计费的连接时，避免上传大流量数据 - Hidden Network 隐藏的网络 @@ -86,6 +82,30 @@ for "%1" 网络名称："%1" + + Prevent large data uploads when on a metered cellular connection + + + + default + + + + metered + + + + unmetered + + + + Wi-Fi Network Metered + + + + Prevent large data uploads when on a metered Wi-FI connection + + ConfirmationDialog diff --git a/selfdrive/ui/translations/main_zh-CHT.ts b/selfdrive/ui/translations/main_zh-CHT.ts index 5f81cc2c53..e28ac44b98 100644 --- a/selfdrive/ui/translations/main_zh-CHT.ts +++ b/selfdrive/ui/translations/main_zh-CHT.ts @@ -62,10 +62,6 @@ Cellular Metered 行動網路 - - Prevent large data uploads when on a metered connection - 防止使用行動網路上傳大量的數據 - Hidden Network 隱藏的網路 @@ -86,6 +82,30 @@ for "%1" 給 "%1" + + Prevent large data uploads when on a metered cellular connection + + + + default + + + + metered + + + + unmetered + + + + Wi-Fi Network Metered + + + + Prevent large data uploads when on a metered Wi-FI connection + + ConfirmationDialog From 927ce0bc065a3419cc6be6c50ce24e4fd8bbb154 Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Tue, 27 May 2025 02:10:32 +0800 Subject: [PATCH 06/60] system/ui: fix typos (#35349) fix typos --- system/ui/onroad/augmented_road_view.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/system/ui/onroad/augmented_road_view.py b/system/ui/onroad/augmented_road_view.py index 58a37460d5..7ae8f37c13 100644 --- a/system/ui/onroad/augmented_road_view.py +++ b/system/ui/onroad/augmented_road_view.py @@ -26,8 +26,8 @@ class AugmentedRoadView(CameraView): self.view_from_wide_calib = view_frame_from_device_frame.copy() self._last_calib_time: float = 0 - self._last_recect_dims = (0.0, 0.0) - self._cacheed_matrix: np.ndarray | None = None + self._last_rect_dims = (0.0, 0.0) + self._cached_matrix: np.ndarray | None = None def render(self, rect): # Update calibration before rendering @@ -69,9 +69,9 @@ class AugmentedRoadView(CameraView): calib_time = self.sm.recv_frame.get('liveCalibration', 0) current_dims = (rect.width, rect.height) if (self._last_calib_time == calib_time and - self._last_recect_dims == current_dims and - self._cacheed_matrix is not None): - return self._cacheed_matrix + self._last_rect_dims == current_dims and + self._cached_matrix is not None): + return self._cached_matrix # Get camera configuration device_camera = self.device_camera or DEFAULT_DEVICE_CAMERA @@ -105,14 +105,14 @@ class AugmentedRoadView(CameraView): # Update cache values self._last_calib_time = calib_time - self._last_recect_dims = current_dims - self._cacheed_matrix = np.array([ + self._last_rect_dims = current_dims + self._cached_matrix = np.array([ [zoom * 2 * cx / w, 0, -x_offset / w * 2], [0, zoom * 2 * cy / h, -y_offset / h * 2], [0, 0, 1.0] ]) - return self._cacheed_matrix + return self._cached_matrix if __name__ == "__main__": From 427a61ab121ff270b58095155d084b6aa3b407e3 Mon Sep 17 00:00:00 2001 From: Alexandre Nobuharu Sato <66435071+AlexandreSato@users.noreply.github.com> Date: Mon, 26 May 2025 17:07:56 -0300 Subject: [PATCH 07/60] fix Wi-FI typo on networking.cc (#35350) --- selfdrive/ui/qt/network/networking.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/ui/qt/network/networking.cc b/selfdrive/ui/qt/network/networking.cc index bb914b6449..f19786a46f 100644 --- a/selfdrive/ui/qt/network/networking.cc +++ b/selfdrive/ui/qt/network/networking.cc @@ -184,7 +184,7 @@ AdvancedNetworking::AdvancedNetworking(QWidget* parent, WifiManager* wifi): QWid // Wi-Fi metered toggle std::vector metered_button_texts{tr("default"), tr("metered"), tr("unmetered")}; - wifiMeteredToggle = new MultiButtonControl(tr("Wi-Fi Network Metered"), tr("Prevent large data uploads when on a metered Wi-FI connection"), "", metered_button_texts); + wifiMeteredToggle = new MultiButtonControl(tr("Wi-Fi Network Metered"), tr("Prevent large data uploads when on a metered Wi-Fi connection"), "", metered_button_texts); QObject::connect(wifiMeteredToggle, &MultiButtonControl::buttonClicked, [=](int id) { wifiMeteredToggle->setEnabled(false); MeteredType metered = MeteredType::UNKNOWN; From 44d233337d9060b236a69b0536d725e4526e975d Mon Sep 17 00:00:00 2001 From: Trey Moen <50057480+greatgitsby@users.noreply.github.com> Date: Mon, 26 May 2025 13:08:28 -0700 Subject: [PATCH 08/60] esim: faster switching (#35344) * esim: lpac doesnt need disable here * more red diff --- system/hardware/tici/esim.py | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/system/hardware/tici/esim.py b/system/hardware/tici/esim.py index f657966ddf..b489286f50 100644 --- a/system/hardware/tici/esim.py +++ b/system/hardware/tici/esim.py @@ -54,15 +54,10 @@ class TiciLPA(LPABase): self._validate_successful(self._invoke('profile', 'nickname', iccid, nickname)) def switch_profile(self, iccid: str) -> None: - self._enable_profile(iccid) - - def _enable_profile(self, iccid: str) -> None: self._validate_profile_exists(iccid) latest = self.get_active_profile() - if latest: - if latest.iccid == iccid: - return - self._validate_successful(self._invoke('profile', 'disable', latest.iccid)) + if latest and latest.iccid == iccid: + return self._validate_successful(self._invoke('profile', 'enable', iccid)) self._process_notifications() From 1fb5744685b323b71d8ae43be8bf37172d1e9c51 Mon Sep 17 00:00:00 2001 From: Alexandre Nobuharu Sato <66435071+AlexandreSato@users.noreply.github.com> Date: Mon, 26 May 2025 17:08:54 -0300 Subject: [PATCH 09/60] Multilang: update pt-BR translations (#35351) --- selfdrive/ui/translations/main_pt-BR.ts | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/selfdrive/ui/translations/main_pt-BR.ts b/selfdrive/ui/translations/main_pt-BR.ts index c5e4926724..2a7d270713 100644 --- a/selfdrive/ui/translations/main_pt-BR.ts +++ b/selfdrive/ui/translations/main_pt-BR.ts @@ -88,23 +88,23 @@ default - + padrão metered - + limitada unmetered - + ilimitada Wi-Fi Network Metered - + Rede Wi-Fi com Franquia Prevent large data uploads when on a metered Wi-FI connection - + Previna o envio de grandes volumes de dados em conexões Wi-Fi com franquia de limite de dados From 742e30495a7368ac1143b3ae8ed5f909d07b4de3 Mon Sep 17 00:00:00 2001 From: Cameron Clough Date: Tue, 27 May 2025 01:25:56 +0100 Subject: [PATCH 10/60] Reapply "ui: rewrite installer using raylib, remove qt (#33756)" (#35308) This reverts commit 472feefcfdf20a8f4823e28baf596868fd70391b. --- selfdrive/ui/SConscript | 26 ++-- selfdrive/ui/installer/installer.cc | 182 +++++++++++-------------- selfdrive/ui/installer/installer.h | 28 ---- selfdrive/ui/installer/inter-ascii.ttf | 3 + 4 files changed, 97 insertions(+), 142 deletions(-) delete mode 100644 selfdrive/ui/installer/installer.h create mode 100644 selfdrive/ui/installer/inter-ascii.ttf diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript index e63359da05..7199399d41 100644 --- a/selfdrive/ui/SConscript +++ b/selfdrive/ui/SConscript @@ -1,6 +1,5 @@ -import os import json -Import('qt_env', 'arch', 'common', 'messaging', 'visionipc', 'transformations') +Import('env', 'qt_env', 'arch', 'common', 'messaging', 'visionipc', 'transformations') base_libs = [common, messaging, visionipc, transformations, 'm', 'OpenCL', 'ssl', 'crypto', 'pthread'] + qt_env["LIBS"] @@ -76,8 +75,15 @@ if GetOption('extras'): if arch != "Darwin": # build installers - senv = qt_env.Clone() - senv['LINKFLAGS'].append('-Wl,-strip-debug') + raylib_env = env.Clone() + raylib_env['LIBPATH'] += [f'#third_party/raylib/{arch}/'] + raylib_env['LINKFLAGS'].append('-Wl,-strip-debug') + + raylib_libs = common + ["raylib"] + if arch == "larch64": + raylib_libs += ["GLESv2", "wayland-client", "wayland-egl", "EGL"] + else: + raylib_libs += ["GL"] release = "release3" installers = [ @@ -87,17 +93,19 @@ if GetOption('extras'): ("openpilot_internal", "nightly-dev"), ] - cont = senv.Command(f"installer/continue_openpilot.o", f"installer/continue_openpilot.sh", - "ld -r -b binary -o $TARGET $SOURCE") + cont = raylib_env.Command("installer/continue_openpilot.o", "installer/continue_openpilot.sh", + "ld -r -b binary -o $TARGET $SOURCE") + inter = raylib_env.Command("installer/inter_ttf.o", "installer/inter-ascii.ttf", + "ld -r -b binary -o $TARGET $SOURCE") for name, branch in installers: d = {'BRANCH': f"'\"{branch}\"'"} if "internal" in name: d['INTERNAL'] = "1" - obj = senv.Object(f"installer/installers/installer_{name}.o", ["installer/installer.cc"], CPPDEFINES=d) - f = senv.Program(f"installer/installers/installer_{name}", [obj, cont], LIBS=qt_libs) + obj = raylib_env.Object(f"installer/installers/installer_{name}.o", ["installer/installer.cc"], CPPDEFINES=d) + f = raylib_env.Program(f"installer/installers/installer_{name}", [obj, cont, inter], LIBS=raylib_libs) # keep installers small - assert f[0].get_size() < 370*1e3 + assert f[0].get_size() < 1300*1e3, f[0].get_size() # build watch3 if arch in ['x86_64', 'aarch64', 'Darwin'] or GetOption('extras'): diff --git a/selfdrive/ui/installer/installer.cc b/selfdrive/ui/installer/installer.cc index 84486e61be..7326e089ab 100644 --- a/selfdrive/ui/installer/installer.cc +++ b/selfdrive/ui/installer/installer.cc @@ -1,19 +1,15 @@ -#include - -#include +#include +#include #include #include -#include - -#include -#include -#include -#include +#include "common/swaglog.h" #include "common/util.h" -#include "selfdrive/ui/installer/installer.h" -#include "selfdrive/ui/qt/util.h" -#include "selfdrive/ui/qt/qt_window.h" +#include "third_party/raylib/include/raylib.h" + +int freshClone(); +int cachedFetch(const std::string &cache); +int executeGitCommand(const std::string &cmd); std::string get_str(std::string const s) { std::string::size_type pos = s.find('?'); @@ -28,136 +24,108 @@ const std::string BRANCH_STR = get_str(BRANCH "? #define GIT_SSH_URL "git@github.com:commaai/openpilot.git" #define CONTINUE_PATH "/data/continue.sh" -const QString CACHE_PATH = "/data/openpilot.cache"; +const std::string CACHE_PATH = "/data/openpilot.cache"; #define INSTALL_PATH "/data/openpilot" #define TMP_INSTALL_PATH "/data/tmppilot" extern const uint8_t str_continue[] asm("_binary_selfdrive_ui_installer_continue_openpilot_sh_start"); extern const uint8_t str_continue_end[] asm("_binary_selfdrive_ui_installer_continue_openpilot_sh_end"); +extern const uint8_t inter_ttf[] asm("_binary_selfdrive_ui_installer_inter_ascii_ttf_start"); +extern const uint8_t inter_ttf_end[] asm("_binary_selfdrive_ui_installer_inter_ascii_ttf_end"); + +Font font; void run(const char* cmd) { int err = std::system(cmd); assert(err == 0); } -Installer::Installer(QWidget *parent) : QWidget(parent) { - QVBoxLayout *layout = new QVBoxLayout(this); - layout->setContentsMargins(150, 290, 150, 150); - layout->setSpacing(0); - - QLabel *title = new QLabel(tr("Installing...")); - title->setStyleSheet("font-size: 90px; font-weight: 600;"); - layout->addWidget(title, 0, Qt::AlignTop); - - layout->addSpacing(170); - - bar = new QProgressBar(); - bar->setRange(0, 100); - bar->setTextVisible(false); - bar->setFixedHeight(72); - layout->addWidget(bar, 0, Qt::AlignTop); - - layout->addSpacing(30); - - val = new QLabel("0%"); - val->setStyleSheet("font-size: 70px; font-weight: 300;"); - layout->addWidget(val, 0, Qt::AlignTop); - - layout->addStretch(); - - QObject::connect(&proc, QOverload::of(&QProcess::finished), this, &Installer::cloneFinished); - QObject::connect(&proc, &QProcess::readyReadStandardError, this, &Installer::readProgress); - - QTimer::singleShot(100, this, &Installer::doInstall); - - setStyleSheet(R"( - * { - font-family: Inter; - color: white; - background-color: black; - } - QProgressBar { - border: none; - background-color: #292929; - } - QProgressBar::chunk { - background-color: #364DEF; - } - )"); +void renderProgress(int progress) { + BeginDrawing(); + ClearBackground(BLACK); + DrawTextEx(font, "Installing...", (Vector2){150, 290}, 110, 0, WHITE); + Rectangle bar = {150, 570, (float)GetScreenWidth() - 300, 72}; + DrawRectangleRec(bar, (Color){41, 41, 41, 255}); + progress = std::clamp(progress, 0, 100); + bar.width *= progress / 100.0f; + DrawRectangleRec(bar, (Color){70, 91, 234, 255}); + DrawTextEx(font, (std::to_string(progress) + "%").c_str(), (Vector2){150, 670}, 85, 0, WHITE); + EndDrawing(); } -void Installer::updateProgress(int percent) { - bar->setValue(percent); - val->setText(QString("%1%").arg(percent)); - update(); -} - -void Installer::doInstall() { +int doInstall() { // wait for valid time while (!util::system_time_valid()) { - usleep(500 * 1000); - qDebug() << "Waiting for valid time"; + util::sleep_for(500); + LOGD("Waiting for valid time"); } // cleanup previous install attempts run("rm -rf " TMP_INSTALL_PATH " " INSTALL_PATH); // do the install - if (QDir(CACHE_PATH).exists()) { - cachedFetch(CACHE_PATH); + if (util::file_exists(CACHE_PATH)) { + return cachedFetch(CACHE_PATH); } else { - freshClone(); + return freshClone(); } } -void Installer::freshClone() { - qDebug() << "Doing fresh clone"; - proc.start("git", {"clone", "--progress", GIT_URL.c_str(), "-b", BRANCH_STR.c_str(), - "--depth=1", "--recurse-submodules", TMP_INSTALL_PATH}); +int freshClone() { + LOGD("Doing fresh clone"); + std::string cmd = util::string_format("git clone --progress %s -b %s --depth=1 --recurse-submodules %s 2>&1", + GIT_URL.c_str(), BRANCH_STR.c_str(), TMP_INSTALL_PATH); + return executeGitCommand(cmd); } -void Installer::cachedFetch(const QString &cache) { - qDebug() << "Fetching with cache: " << cache; +int cachedFetch(const std::string &cache) { + LOGD("Fetching with cache: %s", cache.c_str()); - run(QString("cp -rp %1 %2").arg(cache, TMP_INSTALL_PATH).toStdString().c_str()); - int err = chdir(TMP_INSTALL_PATH); - assert(err == 0); - run(("git remote set-branches --add origin " + BRANCH_STR).c_str()); + run(util::string_format("cp -rp %s %s", cache.c_str(), TMP_INSTALL_PATH).c_str()); + run(util::string_format("cd %s && git remote set-branches --add origin %s", TMP_INSTALL_PATH, BRANCH_STR.c_str()).c_str()); - updateProgress(10); + renderProgress(10); - proc.setWorkingDirectory(TMP_INSTALL_PATH); - proc.start("git", {"fetch", "--progress", "origin", BRANCH_STR.c_str()}); + return executeGitCommand(util::string_format("cd %s && git fetch --progress origin %s 2>&1", TMP_INSTALL_PATH, BRANCH_STR.c_str())); } -void Installer::readProgress() { - const QVector> stages = { +int executeGitCommand(const std::string &cmd) { + static const std::array stages = { // prefix, weight in percentage - {"Receiving objects: ", 91}, - {"Resolving deltas: ", 2}, - {"Updating files: ", 7}, + std::pair{"Receiving objects: ", 91}, + std::pair{"Resolving deltas: ", 2}, + std::pair{"Updating files: ", 7}, }; - auto line = QString(proc.readAllStandardError()); + FILE *pipe = popen(cmd.c_str(), "r"); + if (!pipe) return -1; - int base = 0; - for (const QPair kv : stages) { - if (line.startsWith(kv.first)) { - auto perc = line.split(kv.first)[1].split("%")[0]; - int p = base + int(perc.toFloat() / 100. * kv.second); - updateProgress(p); - break; + char buffer[512]; + while (fgets(buffer, sizeof(buffer), pipe) != nullptr) { + std::string line(buffer); + int base = 0; + for (const auto &[text, weight] : stages) { + if (line.find(text) != std::string::npos) { + size_t percentPos = line.find("%"); + if (percentPos != std::string::npos && percentPos >= 3) { + int percent = std::stoi(line.substr(percentPos - 3, 3)); + int progress = base + int(percent / 100. * weight); + renderProgress(progress); + } + break; + } + base += weight; } - base += kv.second; } + return pclose(pipe); } -void Installer::cloneFinished(int exitCode, QProcess::ExitStatus exitStatus) { - qDebug() << "git finished with " << exitCode; +void cloneFinished(int exitCode) { + LOGD("git finished with %d", exitCode); assert(exitCode == 0); - updateProgress(100); + renderProgress(100); // ensure correct branch is checked out int err = chdir(TMP_INSTALL_PATH); @@ -203,13 +171,17 @@ void Installer::cloneFinished(int exitCode, QProcess::ExitStatus exitStatus) { run("mv /data/continue.sh.new " CONTINUE_PATH); // wait for the installed software's UI to take over - QTimer::singleShot(60 * 1000, &QCoreApplication::quit); + util::sleep_for(60 * 1000); } int main(int argc, char *argv[]) { - initApp(argc, argv); - QApplication a(argc, argv); - Installer installer; - setMainWindow(&installer); - return a.exec(); + InitWindow(2160, 1080, "Installer"); + font = LoadFontFromMemory(".ttf", inter_ttf, inter_ttf_end - inter_ttf, 120, NULL, 0); + SetTextureFilter(font.texture, TEXTURE_FILTER_BILINEAR); + renderProgress(0); + int result = doInstall(); + cloneFinished(result); + CloseWindow(); + UnloadFont(font); + return 0; } diff --git a/selfdrive/ui/installer/installer.h b/selfdrive/ui/installer/installer.h deleted file mode 100644 index de3af0ff39..0000000000 --- a/selfdrive/ui/installer/installer.h +++ /dev/null @@ -1,28 +0,0 @@ -#pragma once - -#include -#include -#include -#include - -class Installer : public QWidget { - Q_OBJECT - -public: - explicit Installer(QWidget *parent = 0); - -private slots: - void updateProgress(int percent); - - void readProgress(); - void cloneFinished(int exitCode, QProcess::ExitStatus exitStatus); - -private: - QLabel *val; - QProgressBar *bar; - QProcess proc; - - void doInstall(); - void freshClone(); - void cachedFetch(const QString &cache); -}; diff --git a/selfdrive/ui/installer/inter-ascii.ttf b/selfdrive/ui/installer/inter-ascii.ttf new file mode 100644 index 0000000000..5d480c515a --- /dev/null +++ b/selfdrive/ui/installer/inter-ascii.ttf @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1ef26a4099ef867f3493389379d882381a2491cdbfa41a086be8899a2154dcb3 +size 26160 From 50aaa691373222dcc71c506c1d055e21c5bc97ea Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Mon, 26 May 2025 17:49:44 -0700 Subject: [PATCH 11/60] sensord: cleanup, prep for rewrite (#35352) * rm bmx * thanks claude * fix * fix mypy --- system/sensord/SConscript | 4 - system/sensord/sensors/bmx055_accel.cc | 85 -------- system/sensord/sensors/bmx055_accel.h | 41 ---- system/sensord/sensors/bmx055_gyro.cc | 92 --------- system/sensord/sensors/bmx055_gyro.h | 41 ---- system/sensord/sensors/bmx055_magn.cc | 258 ------------------------- system/sensord/sensors/bmx055_magn.h | 64 ------ system/sensord/sensors/bmx055_temp.cc | 31 --- system/sensord/sensors/bmx055_temp.h | 13 -- system/sensord/sensors_qcom2.cc | 9 - system/sensord/tests/test_sensord.py | 30 +-- system/sensord/tests/ttff_test.py | 48 ----- 12 files changed, 5 insertions(+), 711 deletions(-) delete mode 100644 system/sensord/sensors/bmx055_accel.cc delete mode 100644 system/sensord/sensors/bmx055_accel.h delete mode 100644 system/sensord/sensors/bmx055_gyro.cc delete mode 100644 system/sensord/sensors/bmx055_gyro.h delete mode 100644 system/sensord/sensors/bmx055_magn.cc delete mode 100644 system/sensord/sensors/bmx055_magn.h delete mode 100644 system/sensord/sensors/bmx055_temp.cc delete mode 100644 system/sensord/sensors/bmx055_temp.h delete mode 100755 system/sensord/tests/ttff_test.py diff --git a/system/sensord/SConscript b/system/sensord/SConscript index e2dfb522c6..1222f0bfcd 100644 --- a/system/sensord/SConscript +++ b/system/sensord/SConscript @@ -2,10 +2,6 @@ Import('env', 'arch', 'common', 'messaging') sensors = [ 'sensors/i2c_sensor.cc', - 'sensors/bmx055_accel.cc', - 'sensors/bmx055_gyro.cc', - 'sensors/bmx055_magn.cc', - 'sensors/bmx055_temp.cc', 'sensors/lsm6ds3_accel.cc', 'sensors/lsm6ds3_gyro.cc', 'sensors/lsm6ds3_temp.cc', diff --git a/system/sensord/sensors/bmx055_accel.cc b/system/sensord/sensors/bmx055_accel.cc deleted file mode 100644 index bcc31e1d6c..0000000000 --- a/system/sensord/sensors/bmx055_accel.cc +++ /dev/null @@ -1,85 +0,0 @@ -#include "system/sensord/sensors/bmx055_accel.h" - -#include - -#include "common/swaglog.h" -#include "common/timing.h" -#include "common/util.h" - -BMX055_Accel::BMX055_Accel(I2CBus *bus) : I2CSensor(bus) {} - -int BMX055_Accel::init() { - int ret = verify_chip_id(BMX055_ACCEL_I2C_REG_ID, {BMX055_ACCEL_CHIP_ID}); - if (ret == -1) { - goto fail; - } - - ret = set_register(BMX055_ACCEL_I2C_REG_PMU, BMX055_ACCEL_NORMAL_MODE); - if (ret < 0) { - goto fail; - } - - // bmx055 accel has a 1.3ms wakeup time from deep suspend mode - util::sleep_for(10); - - // High bandwidth - // ret = set_register(BMX055_ACCEL_I2C_REG_HBW, BMX055_ACCEL_HBW_ENABLE); - // if (ret < 0) { - // goto fail; - // } - - // Low bandwidth - ret = set_register(BMX055_ACCEL_I2C_REG_HBW, BMX055_ACCEL_HBW_DISABLE); - if (ret < 0) { - goto fail; - } - - ret = set_register(BMX055_ACCEL_I2C_REG_BW, BMX055_ACCEL_BW_125HZ); - if (ret < 0) { - goto fail; - } - - enabled = true; - -fail: - return ret; -} - -int BMX055_Accel::shutdown() { - if (!enabled) return 0; - - // enter deep suspend mode (lowest power mode) - int ret = set_register(BMX055_ACCEL_I2C_REG_PMU, BMX055_ACCEL_DEEP_SUSPEND); - if (ret < 0) { - LOGE("Could not move BMX055 ACCEL in deep suspend mode!"); - } - - return ret; -} - -bool BMX055_Accel::get_event(MessageBuilder &msg, uint64_t ts) { - uint64_t start_time = nanos_since_boot(); - uint8_t buffer[6]; - int len = read_register(BMX055_ACCEL_I2C_REG_X_LSB, buffer, sizeof(buffer)); - assert(len == 6); - - // 12 bit = +-2g - float scale = 9.81 * 2.0f / (1 << 11); - float x = -read_12_bit(buffer[0], buffer[1]) * scale; - float y = -read_12_bit(buffer[2], buffer[3]) * scale; - float z = read_12_bit(buffer[4], buffer[5]) * scale; - - auto event = msg.initEvent().initAccelerometer2(); - event.setSource(cereal::SensorEventData::SensorSource::BMX055); - event.setVersion(1); - event.setSensor(SENSOR_ACCELEROMETER); - event.setType(SENSOR_TYPE_ACCELEROMETER); - event.setTimestamp(start_time); - - float xyz[] = {x, y, z}; - auto svec = event.initAcceleration(); - svec.setV(xyz); - svec.setStatus(true); - - return true; -} diff --git a/system/sensord/sensors/bmx055_accel.h b/system/sensord/sensors/bmx055_accel.h deleted file mode 100644 index 2cc316e992..0000000000 --- a/system/sensord/sensors/bmx055_accel.h +++ /dev/null @@ -1,41 +0,0 @@ -#pragma once - -#include "system/sensord/sensors/i2c_sensor.h" - -// Address of the chip on the bus -#define BMX055_ACCEL_I2C_ADDR 0x18 - -// Registers of the chip -#define BMX055_ACCEL_I2C_REG_ID 0x00 -#define BMX055_ACCEL_I2C_REG_X_LSB 0x02 -#define BMX055_ACCEL_I2C_REG_TEMP 0x08 -#define BMX055_ACCEL_I2C_REG_BW 0x10 -#define BMX055_ACCEL_I2C_REG_PMU 0x11 -#define BMX055_ACCEL_I2C_REG_HBW 0x13 -#define BMX055_ACCEL_I2C_REG_FIFO 0x3F - -// Constants -#define BMX055_ACCEL_CHIP_ID 0xFA - -#define BMX055_ACCEL_HBW_ENABLE 0b10000000 -#define BMX055_ACCEL_HBW_DISABLE 0b00000000 -#define BMX055_ACCEL_DEEP_SUSPEND 0b00100000 -#define BMX055_ACCEL_NORMAL_MODE 0b00000000 - -#define BMX055_ACCEL_BW_7_81HZ 0b01000 -#define BMX055_ACCEL_BW_15_63HZ 0b01001 -#define BMX055_ACCEL_BW_31_25HZ 0b01010 -#define BMX055_ACCEL_BW_62_5HZ 0b01011 -#define BMX055_ACCEL_BW_125HZ 0b01100 -#define BMX055_ACCEL_BW_250HZ 0b01101 -#define BMX055_ACCEL_BW_500HZ 0b01110 -#define BMX055_ACCEL_BW_1000HZ 0b01111 - -class BMX055_Accel : public I2CSensor { - uint8_t get_device_address() {return BMX055_ACCEL_I2C_ADDR;} -public: - BMX055_Accel(I2CBus *bus); - int init(); - bool get_event(MessageBuilder &msg, uint64_t ts = 0); - int shutdown(); -}; diff --git a/system/sensord/sensors/bmx055_gyro.cc b/system/sensord/sensors/bmx055_gyro.cc deleted file mode 100644 index 0cc405f654..0000000000 --- a/system/sensord/sensors/bmx055_gyro.cc +++ /dev/null @@ -1,92 +0,0 @@ -#include "system/sensord/sensors/bmx055_gyro.h" - -#include -#include - -#include "common/swaglog.h" -#include "common/util.h" - -#define DEG2RAD(x) ((x) * M_PI / 180.0) - - -BMX055_Gyro::BMX055_Gyro(I2CBus *bus) : I2CSensor(bus) {} - -int BMX055_Gyro::init() { - int ret = verify_chip_id(BMX055_GYRO_I2C_REG_ID, {BMX055_GYRO_CHIP_ID}); - if (ret == -1) return -1; - - ret = set_register(BMX055_GYRO_I2C_REG_LPM1, BMX055_GYRO_NORMAL_MODE); - if (ret < 0) { - goto fail; - } - // bmx055 gyro has a 30ms wakeup time from deep suspend mode - util::sleep_for(50); - - // High bandwidth - // ret = set_register(BMX055_GYRO_I2C_REG_HBW, BMX055_GYRO_HBW_ENABLE); - // if (ret < 0) { - // goto fail; - // } - - // Low bandwidth - ret = set_register(BMX055_GYRO_I2C_REG_HBW, BMX055_GYRO_HBW_DISABLE); - if (ret < 0) { - goto fail; - } - - // 116 Hz filter - ret = set_register(BMX055_GYRO_I2C_REG_BW, BMX055_GYRO_BW_116HZ); - if (ret < 0) { - goto fail; - } - - // +- 125 deg/s range - ret = set_register(BMX055_GYRO_I2C_REG_RANGE, BMX055_GYRO_RANGE_125); - if (ret < 0) { - goto fail; - } - - enabled = true; - -fail: - return ret; -} - -int BMX055_Gyro::shutdown() { - if (!enabled) return 0; - - // enter deep suspend mode (lowest power mode) - int ret = set_register(BMX055_GYRO_I2C_REG_LPM1, BMX055_GYRO_DEEP_SUSPEND); - if (ret < 0) { - LOGE("Could not move BMX055 GYRO in deep suspend mode!"); - } - - return ret; -} - -bool BMX055_Gyro::get_event(MessageBuilder &msg, uint64_t ts) { - uint64_t start_time = nanos_since_boot(); - uint8_t buffer[6]; - int len = read_register(BMX055_GYRO_I2C_REG_RATE_X_LSB, buffer, sizeof(buffer)); - assert(len == 6); - - // 16 bit = +- 125 deg/s - float scale = 125.0f / (1 << 15); - float x = -DEG2RAD(read_16_bit(buffer[0], buffer[1]) * scale); - float y = -DEG2RAD(read_16_bit(buffer[2], buffer[3]) * scale); - float z = DEG2RAD(read_16_bit(buffer[4], buffer[5]) * scale); - - auto event = msg.initEvent().initGyroscope2(); - event.setSource(cereal::SensorEventData::SensorSource::BMX055); - event.setVersion(1); - event.setSensor(SENSOR_GYRO_UNCALIBRATED); - event.setType(SENSOR_TYPE_GYROSCOPE_UNCALIBRATED); - event.setTimestamp(start_time); - - float xyz[] = {x, y, z}; - auto svec = event.initGyroUncalibrated(); - svec.setV(xyz); - svec.setStatus(true); - - return true; -} diff --git a/system/sensord/sensors/bmx055_gyro.h b/system/sensord/sensors/bmx055_gyro.h deleted file mode 100644 index 7be3e56563..0000000000 --- a/system/sensord/sensors/bmx055_gyro.h +++ /dev/null @@ -1,41 +0,0 @@ -#pragma once - -#include "system/sensord/sensors/i2c_sensor.h" - -// Address of the chip on the bus -#define BMX055_GYRO_I2C_ADDR 0x68 - -// Registers of the chip -#define BMX055_GYRO_I2C_REG_ID 0x00 -#define BMX055_GYRO_I2C_REG_RATE_X_LSB 0x02 -#define BMX055_GYRO_I2C_REG_RANGE 0x0F -#define BMX055_GYRO_I2C_REG_BW 0x10 -#define BMX055_GYRO_I2C_REG_LPM1 0x11 -#define BMX055_GYRO_I2C_REG_HBW 0x13 -#define BMX055_GYRO_I2C_REG_FIFO 0x3F - -// Constants -#define BMX055_GYRO_CHIP_ID 0x0F - -#define BMX055_GYRO_HBW_ENABLE 0b10000000 -#define BMX055_GYRO_HBW_DISABLE 0b00000000 -#define BMX055_GYRO_DEEP_SUSPEND 0b00100000 -#define BMX055_GYRO_NORMAL_MODE 0b00000000 - -#define BMX055_GYRO_RANGE_2000 0b000 -#define BMX055_GYRO_RANGE_1000 0b001 -#define BMX055_GYRO_RANGE_500 0b010 -#define BMX055_GYRO_RANGE_250 0b011 -#define BMX055_GYRO_RANGE_125 0b100 - -#define BMX055_GYRO_BW_116HZ 0b0010 - - -class BMX055_Gyro : public I2CSensor { - uint8_t get_device_address() {return BMX055_GYRO_I2C_ADDR;} -public: - BMX055_Gyro(I2CBus *bus); - int init(); - bool get_event(MessageBuilder &msg, uint64_t ts = 0); - int shutdown(); -}; diff --git a/system/sensord/sensors/bmx055_magn.cc b/system/sensord/sensors/bmx055_magn.cc deleted file mode 100644 index b498c5fe3d..0000000000 --- a/system/sensord/sensors/bmx055_magn.cc +++ /dev/null @@ -1,258 +0,0 @@ -#include "system/sensord/sensors/bmx055_magn.h" - -#include - -#include -#include -#include - -#include "common/swaglog.h" -#include "common/util.h" - -static int16_t compensate_x(trim_data_t trim_data, int16_t mag_data_x, uint16_t data_rhall) { - uint16_t process_comp_x0 = data_rhall; - int32_t process_comp_x1 = ((int32_t)trim_data.dig_xyz1) * 16384; - uint16_t process_comp_x2 = ((uint16_t)(process_comp_x1 / process_comp_x0)) - ((uint16_t)0x4000); - int16_t retval = ((int16_t)process_comp_x2); - int32_t process_comp_x3 = (((int32_t)retval) * ((int32_t)retval)); - int32_t process_comp_x4 = (((int32_t)trim_data.dig_xy2) * (process_comp_x3 / 128)); - int32_t process_comp_x5 = (int32_t)(((int16_t)trim_data.dig_xy1) * 128); - int32_t process_comp_x6 = ((int32_t)retval) * process_comp_x5; - int32_t process_comp_x7 = (((process_comp_x4 + process_comp_x6) / 512) + ((int32_t)0x100000)); - int32_t process_comp_x8 = ((int32_t)(((int16_t)trim_data.dig_x2) + ((int16_t)0xA0))); - int32_t process_comp_x9 = ((process_comp_x7 * process_comp_x8) / 4096); - int32_t process_comp_x10 = ((int32_t)mag_data_x) * process_comp_x9; - retval = ((int16_t)(process_comp_x10 / 8192)); - retval = (retval + (((int16_t)trim_data.dig_x1) * 8)) / 16; - - return retval; -} - -static int16_t compensate_y(trim_data_t trim_data, int16_t mag_data_y, uint16_t data_rhall) { - uint16_t process_comp_y0 = trim_data.dig_xyz1; - int32_t process_comp_y1 = (((int32_t)trim_data.dig_xyz1) * 16384) / process_comp_y0; - uint16_t process_comp_y2 = ((uint16_t)process_comp_y1) - ((uint16_t)0x4000); - int16_t retval = ((int16_t)process_comp_y2); - int32_t process_comp_y3 = ((int32_t) retval) * ((int32_t)retval); - int32_t process_comp_y4 = ((int32_t)trim_data.dig_xy2) * (process_comp_y3 / 128); - int32_t process_comp_y5 = ((int32_t)(((int16_t)trim_data.dig_xy1) * 128)); - int32_t process_comp_y6 = ((process_comp_y4 + (((int32_t)retval) * process_comp_y5)) / 512); - int32_t process_comp_y7 = ((int32_t)(((int16_t)trim_data.dig_y2) + ((int16_t)0xA0))); - int32_t process_comp_y8 = (((process_comp_y6 + ((int32_t)0x100000)) * process_comp_y7) / 4096); - int32_t process_comp_y9 = (((int32_t)mag_data_y) * process_comp_y8); - retval = (int16_t)(process_comp_y9 / 8192); - retval = (retval + (((int16_t)trim_data.dig_y1) * 8)) / 16; - - return retval; -} - -static int16_t compensate_z(trim_data_t trim_data, int16_t mag_data_z, uint16_t data_rhall) { - int16_t process_comp_z0 = ((int16_t)data_rhall) - ((int16_t) trim_data.dig_xyz1); - int32_t process_comp_z1 = (((int32_t)trim_data.dig_z3) * ((int32_t)(process_comp_z0))) / 4; - int32_t process_comp_z2 = (((int32_t)(mag_data_z - trim_data.dig_z4)) * 32768); - int32_t process_comp_z3 = ((int32_t)trim_data.dig_z1) * (((int16_t)data_rhall) * 2); - int16_t process_comp_z4 = (int16_t)((process_comp_z3 + (32768)) / 65536); - int32_t retval = ((process_comp_z2 - process_comp_z1) / (trim_data.dig_z2 + process_comp_z4)); - - /* saturate result to +/- 2 micro-tesla */ - retval = std::clamp(retval, -32767, 32767); - - /* Conversion of LSB to micro-tesla*/ - retval = retval / 16; - - return (int16_t)retval; -} - -BMX055_Magn::BMX055_Magn(I2CBus *bus) : I2CSensor(bus) {} - -int BMX055_Magn::init() { - uint8_t trim_x1y1[2] = {0}; - uint8_t trim_x2y2[2] = {0}; - uint8_t trim_xy1xy2[2] = {0}; - uint8_t trim_z1[2] = {0}; - uint8_t trim_z2[2] = {0}; - uint8_t trim_z3[2] = {0}; - uint8_t trim_z4[2] = {0}; - uint8_t trim_xyz1[2] = {0}; - - // suspend -> sleep - int ret = set_register(BMX055_MAGN_I2C_REG_PWR_0, 0x01); - if (ret < 0) { - LOGD("Enabling power failed: %d", ret); - goto fail; - } - util::sleep_for(5); // wait until the chip is powered on - - ret = verify_chip_id(BMX055_MAGN_I2C_REG_ID, {BMX055_MAGN_CHIP_ID}); - if (ret == -1) { - goto fail; - } - - // Load magnetometer trim - ret = read_register(BMX055_MAGN_I2C_REG_DIG_X1, trim_x1y1, 2); - if (ret < 0) goto fail; - ret = read_register(BMX055_MAGN_I2C_REG_DIG_X2, trim_x2y2, 2); - if (ret < 0) goto fail; - ret = read_register(BMX055_MAGN_I2C_REG_DIG_XY2, trim_xy1xy2, 2); - if (ret < 0) goto fail; - ret = read_register(BMX055_MAGN_I2C_REG_DIG_Z1_LSB, trim_z1, 2); - if (ret < 0) goto fail; - ret = read_register(BMX055_MAGN_I2C_REG_DIG_Z2_LSB, trim_z2, 2); - if (ret < 0) goto fail; - ret = read_register(BMX055_MAGN_I2C_REG_DIG_Z3_LSB, trim_z3, 2); - if (ret < 0) goto fail; - ret = read_register(BMX055_MAGN_I2C_REG_DIG_Z4_LSB, trim_z4, 2); - if (ret < 0) goto fail; - ret = read_register(BMX055_MAGN_I2C_REG_DIG_XYZ1_LSB, trim_xyz1, 2); - if (ret < 0) goto fail; - - // Read trim data - trim_data.dig_x1 = trim_x1y1[0]; - trim_data.dig_y1 = trim_x1y1[1]; - - trim_data.dig_x2 = trim_x2y2[0]; - trim_data.dig_y2 = trim_x2y2[1]; - - trim_data.dig_xy1 = trim_xy1xy2[1]; // NB: MSB/LSB swapped - trim_data.dig_xy2 = trim_xy1xy2[0]; - - trim_data.dig_z1 = read_16_bit(trim_z1[0], trim_z1[1]); - trim_data.dig_z2 = read_16_bit(trim_z2[0], trim_z2[1]); - trim_data.dig_z3 = read_16_bit(trim_z3[0], trim_z3[1]); - trim_data.dig_z4 = read_16_bit(trim_z4[0], trim_z4[1]); - - trim_data.dig_xyz1 = read_16_bit(trim_xyz1[0], trim_xyz1[1] & 0x7f); - assert(trim_data.dig_xyz1 != 0); - - perform_self_test(); - - // f_max = 1 / (145us * nXY + 500us * NZ + 980us) - // Chose NXY = 7, NZ = 12, which gives 125 Hz, - // and has the same ratio as the high accuracy preset - ret = set_register(BMX055_MAGN_I2C_REG_REPXY, (7 - 1) / 2); - if (ret < 0) { - goto fail; - } - - ret = set_register(BMX055_MAGN_I2C_REG_REPZ, 12 - 1); - if (ret < 0) { - goto fail; - } - - enabled = true; - return 0; - - fail: - return ret; -} - -int BMX055_Magn::shutdown() { - if (!enabled) return 0; - - // move to suspend mode - int ret = set_register(BMX055_MAGN_I2C_REG_PWR_0, 0); - if (ret < 0) { - LOGE("Could not move BMX055 MAGN in suspend mode!"); - } - - return ret; -} - -bool BMX055_Magn::perform_self_test() { - uint8_t buffer[8]; - int16_t x, y; - int16_t neg_z, pos_z; - - // Increase z reps for less false positives (~30 Hz ODR) - set_register(BMX055_MAGN_I2C_REG_REPXY, 1); - set_register(BMX055_MAGN_I2C_REG_REPZ, 64 - 1); - - // Clean existing measurement - read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer)); - - uint8_t forced = BMX055_MAGN_FORCED; - - // Negative current - set_register(BMX055_MAGN_I2C_REG_MAG, forced | (uint8_t(0b10) << 6)); - util::sleep_for(100); - - read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer)); - parse_xyz(buffer, &x, &y, &neg_z); - - // Positive current - set_register(BMX055_MAGN_I2C_REG_MAG, forced | (uint8_t(0b11) << 6)); - util::sleep_for(100); - - read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer)); - parse_xyz(buffer, &x, &y, &pos_z); - - // Put back in normal mode - set_register(BMX055_MAGN_I2C_REG_MAG, 0); - - int16_t diff = pos_z - neg_z; - bool passed = (diff > 180) && (diff < 240); - - if (!passed) { - LOGE("self test failed: neg %d pos %d diff %d", neg_z, pos_z, diff); - } - - return passed; -} - -bool BMX055_Magn::parse_xyz(uint8_t buffer[8], int16_t *x, int16_t *y, int16_t *z) { - bool ready = buffer[6] & 0x1; - if (ready) { - int16_t mdata_x = (int16_t) (((int16_t)buffer[1] << 8) | buffer[0]) >> 3; - int16_t mdata_y = (int16_t) (((int16_t)buffer[3] << 8) | buffer[2]) >> 3; - int16_t mdata_z = (int16_t) (((int16_t)buffer[5] << 8) | buffer[4]) >> 1; - uint16_t data_r = (uint16_t) (((uint16_t)buffer[7] << 8) | buffer[6]) >> 2; - assert(data_r != 0); - - *x = compensate_x(trim_data, mdata_x, data_r); - *y = compensate_y(trim_data, mdata_y, data_r); - *z = compensate_z(trim_data, mdata_z, data_r); - } - return ready; -} - - -bool BMX055_Magn::get_event(MessageBuilder &msg, uint64_t ts) { - uint64_t start_time = nanos_since_boot(); - uint8_t buffer[8]; - int16_t _x, _y, x, y, z; - - int len = read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer)); - assert(len == sizeof(buffer)); - - bool parsed = parse_xyz(buffer, &_x, &_y, &z); - if (parsed) { - - auto event = msg.initEvent().initMagnetometer(); - event.setSource(cereal::SensorEventData::SensorSource::BMX055); - event.setVersion(2); - event.setSensor(SENSOR_MAGNETOMETER_UNCALIBRATED); - event.setType(SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED); - event.setTimestamp(start_time); - - // Move magnetometer into same reference frame as accel/gryo - x = -_y; - y = _x; - - // Axis convention - x = -x; - y = -y; - - float xyz[] = {(float)x, (float)y, (float)z}; - auto svec = event.initMagneticUncalibrated(); - svec.setV(xyz); - svec.setStatus(true); - } - - // The BMX055 Magnetometer has no FIFO mode. Self running mode only goes - // up to 30 Hz. Therefore we put in forced mode, and request measurements - // at a 100 Hz. When reading the registers we have to check the ready bit - // To verify the measurement was completed this cycle. - set_register(BMX055_MAGN_I2C_REG_MAG, BMX055_MAGN_FORCED); - - return parsed; -} diff --git a/system/sensord/sensors/bmx055_magn.h b/system/sensord/sensors/bmx055_magn.h deleted file mode 100644 index 15c4e734b9..0000000000 --- a/system/sensord/sensors/bmx055_magn.h +++ /dev/null @@ -1,64 +0,0 @@ -#pragma once -#include - -#include "system/sensord/sensors/i2c_sensor.h" - -// Address of the chip on the bus -#define BMX055_MAGN_I2C_ADDR 0x10 - -// Registers of the chip -#define BMX055_MAGN_I2C_REG_ID 0x40 -#define BMX055_MAGN_I2C_REG_PWR_0 0x4B -#define BMX055_MAGN_I2C_REG_MAG 0x4C -#define BMX055_MAGN_I2C_REG_DATAX_LSB 0x42 -#define BMX055_MAGN_I2C_REG_RHALL_LSB 0x48 -#define BMX055_MAGN_I2C_REG_REPXY 0x51 -#define BMX055_MAGN_I2C_REG_REPZ 0x52 - -#define BMX055_MAGN_I2C_REG_DIG_X1 0x5D -#define BMX055_MAGN_I2C_REG_DIG_Y1 0x5E -#define BMX055_MAGN_I2C_REG_DIG_Z4_LSB 0x62 -#define BMX055_MAGN_I2C_REG_DIG_Z4_MSB 0x63 -#define BMX055_MAGN_I2C_REG_DIG_X2 0x64 -#define BMX055_MAGN_I2C_REG_DIG_Y2 0x65 -#define BMX055_MAGN_I2C_REG_DIG_Z2_LSB 0x68 -#define BMX055_MAGN_I2C_REG_DIG_Z2_MSB 0x69 -#define BMX055_MAGN_I2C_REG_DIG_Z1_LSB 0x6A -#define BMX055_MAGN_I2C_REG_DIG_Z1_MSB 0x6B -#define BMX055_MAGN_I2C_REG_DIG_XYZ1_LSB 0x6C -#define BMX055_MAGN_I2C_REG_DIG_XYZ1_MSB 0x6D -#define BMX055_MAGN_I2C_REG_DIG_Z3_LSB 0x6E -#define BMX055_MAGN_I2C_REG_DIG_Z3_MSB 0x6F -#define BMX055_MAGN_I2C_REG_DIG_XY2 0x70 -#define BMX055_MAGN_I2C_REG_DIG_XY1 0x71 - -// Constants -#define BMX055_MAGN_CHIP_ID 0x32 -#define BMX055_MAGN_FORCED (0b01 << 1) - -struct trim_data_t { - int8_t dig_x1; - int8_t dig_y1; - int8_t dig_x2; - int8_t dig_y2; - uint16_t dig_z1; - int16_t dig_z2; - int16_t dig_z3; - int16_t dig_z4; - uint8_t dig_xy1; - int8_t dig_xy2; - uint16_t dig_xyz1; -}; - - -class BMX055_Magn : public I2CSensor{ - uint8_t get_device_address() {return BMX055_MAGN_I2C_ADDR;} - trim_data_t trim_data = {0}; - bool perform_self_test(); - bool parse_xyz(uint8_t buffer[8], int16_t *x, int16_t *y, int16_t *z); -public: - BMX055_Magn(I2CBus *bus); - int init(); - bool get_event(MessageBuilder &msg, uint64_t ts = 0); - int shutdown(); -}; diff --git a/system/sensord/sensors/bmx055_temp.cc b/system/sensord/sensors/bmx055_temp.cc deleted file mode 100644 index da7b86476c..0000000000 --- a/system/sensord/sensors/bmx055_temp.cc +++ /dev/null @@ -1,31 +0,0 @@ -#include "system/sensord/sensors/bmx055_temp.h" - -#include - -#include "system/sensord/sensors/bmx055_accel.h" -#include "common/swaglog.h" -#include "common/timing.h" - -BMX055_Temp::BMX055_Temp(I2CBus *bus) : I2CSensor(bus) {} - -int BMX055_Temp::init() { - return verify_chip_id(BMX055_ACCEL_I2C_REG_ID, {BMX055_ACCEL_CHIP_ID}) == -1 ? -1 : 0; -} - -bool BMX055_Temp::get_event(MessageBuilder &msg, uint64_t ts) { - uint64_t start_time = nanos_since_boot(); - uint8_t buffer[1]; - int len = read_register(BMX055_ACCEL_I2C_REG_TEMP, buffer, sizeof(buffer)); - assert(len == sizeof(buffer)); - - float temp = 23.0f + int8_t(buffer[0]) / 2.0f; - - auto event = msg.initEvent().initTemperatureSensor(); - event.setSource(cereal::SensorEventData::SensorSource::BMX055); - event.setVersion(1); - event.setType(SENSOR_TYPE_AMBIENT_TEMPERATURE); - event.setTimestamp(start_time); - event.setTemperature(temp); - - return true; -} diff --git a/system/sensord/sensors/bmx055_temp.h b/system/sensord/sensors/bmx055_temp.h deleted file mode 100644 index a2eabae395..0000000000 --- a/system/sensord/sensors/bmx055_temp.h +++ /dev/null @@ -1,13 +0,0 @@ -#pragma once - -#include "system/sensord/sensors/bmx055_accel.h" -#include "system/sensord/sensors/i2c_sensor.h" - -class BMX055_Temp : public I2CSensor { - uint8_t get_device_address() {return BMX055_ACCEL_I2C_ADDR;} -public: - BMX055_Temp(I2CBus *bus); - int init(); - bool get_event(MessageBuilder &msg, uint64_t ts = 0); - int shutdown() { return 0; } -}; diff --git a/system/sensord/sensors_qcom2.cc b/system/sensord/sensors_qcom2.cc index f9f51539c9..8ff9331b39 100644 --- a/system/sensord/sensors_qcom2.cc +++ b/system/sensord/sensors_qcom2.cc @@ -14,10 +14,6 @@ #include "common/swaglog.h" #include "common/timing.h" #include "common/util.h" -#include "system/sensord/sensors/bmx055_accel.h" -#include "system/sensord/sensors/bmx055_gyro.h" -#include "system/sensord/sensors/bmx055_magn.h" -#include "system/sensord/sensors/bmx055_temp.h" #include "system/sensord/sensors/constants.h" #include "system/sensord/sensors/lsm6ds3_accel.h" #include "system/sensord/sensors/lsm6ds3_gyro.h" @@ -117,11 +113,6 @@ void polling_loop(Sensor *sensor, std::string msg_name) { int sensor_loop(I2CBus *i2c_bus_imu) { // Sensor init std::vector> sensors_init = { - {new BMX055_Accel(i2c_bus_imu), "accelerometer2"}, - {new BMX055_Gyro(i2c_bus_imu), "gyroscope2"}, - {new BMX055_Magn(i2c_bus_imu), "magnetometer"}, - {new BMX055_Temp(i2c_bus_imu), "temperatureSensor2"}, - {new LSM6DS3_Accel(i2c_bus_imu, GPIO_LSM_INT), "accelerometer"}, {new LSM6DS3_Gyro(i2c_bus_imu, GPIO_LSM_INT, true), "gyroscope"}, {new LSM6DS3_Temp(i2c_bus_imu), "temperatureSensor"}, diff --git a/system/sensord/tests/test_sensord.py b/system/sensord/tests/test_sensord.py index 15f1f2dc35..1dab652386 100644 --- a/system/sensord/tests/test_sensord.py +++ b/system/sensord/tests/test_sensord.py @@ -12,13 +12,6 @@ from openpilot.common.timeout import Timeout from openpilot.system.hardware import HARDWARE from openpilot.system.manager.process_config import managed_processes -BMX = { - ('bmx055', 'acceleration'), - ('bmx055', 'gyroUncalibrated'), - ('bmx055', 'magneticUncalibrated'), - ('bmx055', 'temperature'), -} - LSM = { ('lsm6ds3', 'acceleration'), ('lsm6ds3', 'gyroUncalibrated'), @@ -30,17 +23,11 @@ MMC = { ('mmc5603nj', 'magneticUncalibrated'), } -SENSOR_CONFIGURATIONS: list[set] = [ - BMX | LSM, - MMC | LSM, - BMX | LSM_C, - MMC| LSM_C, -] -if HARDWARE.get_device_type() == "mici": - SENSOR_CONFIGURATIONS = [ - LSM, - LSM_C, - ] +SENSOR_CONFIGURATIONS: list[set] = { + "mici": [LSM, LSM_C], + "tizi": [MMC | LSM, MMC | LSM_C], + "tici": [LSM, LSM_C, MMC | LSM, MMC | LSM_C], +}.get(HARDWARE.get_device_type(), []) Sensor = log.SensorEventData.SensorSource SensorConfig = namedtuple('SensorConfig', ['type', 'sanity_min', 'sanity_max']) @@ -57,13 +44,6 @@ ALL_SENSORS = { SensorConfig("temperature", 0, 60), }, - Sensor.bmx055: { - SensorConfig("acceleration", 5, 15), - SensorConfig("gyroUncalibrated", 0, .2), - SensorConfig("magneticUncalibrated", 0, 300), - SensorConfig("temperature", 0, 60), - }, - Sensor.mmc5603nj: { SensorConfig("magneticUncalibrated", 0, 300), } diff --git a/system/sensord/tests/ttff_test.py b/system/sensord/tests/ttff_test.py deleted file mode 100755 index a9cc16d707..0000000000 --- a/system/sensord/tests/ttff_test.py +++ /dev/null @@ -1,48 +0,0 @@ -#!/usr/bin/env python3 - -import time -import atexit - -from cereal import messaging -from openpilot.system.manager.process_config import managed_processes - -TIMEOUT = 10*60 - -def kill(): - for proc in ['ubloxd', 'pigeond']: - managed_processes[proc].stop(retry=True, block=True) - -if __name__ == "__main__": - # start ubloxd - managed_processes['ubloxd'].start() - atexit.register(kill) - - sm = messaging.SubMaster(['ubloxGnss']) - - times = [] - for i in range(20): - # start pigeond - st = time.monotonic() - managed_processes['pigeond'].start() - - # wait for a >4 satellite fix - while True: - sm.update(0) - msg = sm['ubloxGnss'] - if msg.which() == 'measurementReport' and sm.updated["ubloxGnss"]: - report = msg.measurementReport - if report.numMeas > 4: - times.append(time.monotonic() - st) - print(f"\033[94m{i}: Got a fix in {round(times[-1], 2)} seconds\033[0m") - break - - if time.monotonic() - st > TIMEOUT: - raise TimeoutError("\033[91mFailed to get a fix in {TIMEOUT} seconds!\033[0m") - - time.sleep(0.1) - - # stop pigeond - managed_processes['pigeond'].stop(retry=True, block=True) - time.sleep(20) - - print(f"\033[92mAverage TTFF: {round(sum(times) / len(times), 2)}s\033[0m") From c48f161b046ccb7bd1d982fd998205cd04c32438 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Mon, 26 May 2025 18:54:34 -0700 Subject: [PATCH 12/60] bump tinygrad (#35355) --- tinygrad_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tinygrad_repo b/tinygrad_repo index 519dec6677..76eb130d8c 160000 --- a/tinygrad_repo +++ b/tinygrad_repo @@ -1 +1 @@ -Subproject commit 519dec6677f98718ee4f2d07be1936eb91dde73b +Subproject commit 76eb130d8c6567679641ff53a7215d1f6f88eb9a From 6a38dd13154542dcb337bb876086efed57e547e1 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Mon, 26 May 2025 19:44:21 -0700 Subject: [PATCH 13/60] [bot] Update translations (#35354) Update translations Co-authored-by: Vehicle Researcher --- selfdrive/ui/translations/main_ar.ts | 9 +-------- selfdrive/ui/translations/main_de.ts | 9 +-------- selfdrive/ui/translations/main_es.ts | 9 +-------- selfdrive/ui/translations/main_fr.ts | 9 +-------- selfdrive/ui/translations/main_ja.ts | 9 +-------- selfdrive/ui/translations/main_ko.ts | 9 +-------- selfdrive/ui/translations/main_pt-BR.ts | 11 ++--------- selfdrive/ui/translations/main_th.ts | 9 +-------- selfdrive/ui/translations/main_tr.ts | 9 +-------- selfdrive/ui/translations/main_zh-CHS.ts | 9 +-------- selfdrive/ui/translations/main_zh-CHT.ts | 9 +-------- 11 files changed, 12 insertions(+), 89 deletions(-) diff --git a/selfdrive/ui/translations/main_ar.ts b/selfdrive/ui/translations/main_ar.ts index 603d6a01cd..0c3b934dde 100644 --- a/selfdrive/ui/translations/main_ar.ts +++ b/selfdrive/ui/translations/main_ar.ts @@ -103,7 +103,7 @@ - Prevent large data uploads when on a metered Wi-FI connection + Prevent large data uploads when on a metered Wi-Fi connection @@ -400,13 +400,6 @@ Firehose Mode allows you to maximize your training data uploads to improve openp - - Installer - - Installing... - جارٍ التثبيت... - - MultiOptionDialog diff --git a/selfdrive/ui/translations/main_de.ts b/selfdrive/ui/translations/main_de.ts index aa6cf75c30..db0fe97036 100644 --- a/selfdrive/ui/translations/main_de.ts +++ b/selfdrive/ui/translations/main_de.ts @@ -103,7 +103,7 @@ - Prevent large data uploads when on a metered Wi-FI connection + Prevent large data uploads when on a metered Wi-Fi connection @@ -392,13 +392,6 @@ Der Firehose-Modus ermöglicht es dir, deine Trainingsdaten-Uploads zu maximiere - - Installer - - Installing... - Installiere... - - MultiOptionDialog diff --git a/selfdrive/ui/translations/main_es.ts b/selfdrive/ui/translations/main_es.ts index 64272244ea..c4ead7ca9d 100644 --- a/selfdrive/ui/translations/main_es.ts +++ b/selfdrive/ui/translations/main_es.ts @@ -103,7 +103,7 @@ - Prevent large data uploads when on a metered Wi-FI connection + Prevent large data uploads when on a metered Wi-Fi connection @@ -392,13 +392,6 @@ El Modo Firehose te permite maximizar las subidas de datos de entrenamiento para - - Installer - - Installing... - Instalando... - - MultiOptionDialog diff --git a/selfdrive/ui/translations/main_fr.ts b/selfdrive/ui/translations/main_fr.ts index e7c4fa506a..74849bd6fc 100644 --- a/selfdrive/ui/translations/main_fr.ts +++ b/selfdrive/ui/translations/main_fr.ts @@ -103,7 +103,7 @@ - Prevent large data uploads when on a metered Wi-FI connection + Prevent large data uploads when on a metered Wi-Fi connection @@ -390,13 +390,6 @@ Firehose Mode allows you to maximize your training data uploads to improve openp - - Installer - - Installing... - Installation... - - MultiOptionDialog diff --git a/selfdrive/ui/translations/main_ja.ts b/selfdrive/ui/translations/main_ja.ts index 8fff080283..85d2494dfe 100644 --- a/selfdrive/ui/translations/main_ja.ts +++ b/selfdrive/ui/translations/main_ja.ts @@ -103,7 +103,7 @@ - Prevent large data uploads when on a metered Wi-FI connection + Prevent large data uploads when on a metered Wi-Fi connection @@ -390,13 +390,6 @@ Firehoseモードを有効にすると、学習データを最大限アップロ - - Installer - - Installing... - インストール中... - - MultiOptionDialog diff --git a/selfdrive/ui/translations/main_ko.ts b/selfdrive/ui/translations/main_ko.ts index 268475f917..dea01b44ee 100644 --- a/selfdrive/ui/translations/main_ko.ts +++ b/selfdrive/ui/translations/main_ko.ts @@ -103,7 +103,7 @@ - Prevent large data uploads when on a metered Wi-FI connection + Prevent large data uploads when on a metered Wi-Fi connection @@ -390,13 +390,6 @@ Firehose Mode allows you to maximize your training data uploads to improve openp - - Installer - - Installing... - 설치 중... - - MultiOptionDialog diff --git a/selfdrive/ui/translations/main_pt-BR.ts b/selfdrive/ui/translations/main_pt-BR.ts index 2a7d270713..f98f32b9a8 100644 --- a/selfdrive/ui/translations/main_pt-BR.ts +++ b/selfdrive/ui/translations/main_pt-BR.ts @@ -103,8 +103,8 @@ Rede Wi-Fi com Franquia - Prevent large data uploads when on a metered Wi-FI connection - Previna o envio de grandes volumes de dados em conexões Wi-Fi com franquia de limite de dados + Prevent large data uploads when on a metered Wi-Fi connection + @@ -392,13 +392,6 @@ O Modo Firehose permite maximizar o envio de dados de treinamento para melhorar - - Installer - - Installing... - Instalando... - - MultiOptionDialog diff --git a/selfdrive/ui/translations/main_th.ts b/selfdrive/ui/translations/main_th.ts index e04cdac3e3..c587cfbd12 100644 --- a/selfdrive/ui/translations/main_th.ts +++ b/selfdrive/ui/translations/main_th.ts @@ -103,7 +103,7 @@ - Prevent large data uploads when on a metered Wi-FI connection + Prevent large data uploads when on a metered Wi-Fi connection @@ -390,13 +390,6 @@ Firehose Mode allows you to maximize your training data uploads to improve openp - - Installer - - Installing... - กำลังติดตั้ง... - - MultiOptionDialog diff --git a/selfdrive/ui/translations/main_tr.ts b/selfdrive/ui/translations/main_tr.ts index 181c9c44f3..62de11145a 100644 --- a/selfdrive/ui/translations/main_tr.ts +++ b/selfdrive/ui/translations/main_tr.ts @@ -103,7 +103,7 @@ - Prevent large data uploads when on a metered Wi-FI connection + Prevent large data uploads when on a metered Wi-Fi connection @@ -388,13 +388,6 @@ Firehose Mode allows you to maximize your training data uploads to improve openp - - Installer - - Installing... - Yükleniyor... - - MultiOptionDialog diff --git a/selfdrive/ui/translations/main_zh-CHS.ts b/selfdrive/ui/translations/main_zh-CHS.ts index 03e7a960be..26537d3f64 100644 --- a/selfdrive/ui/translations/main_zh-CHS.ts +++ b/selfdrive/ui/translations/main_zh-CHS.ts @@ -103,7 +103,7 @@ - Prevent large data uploads when on a metered Wi-FI connection + Prevent large data uploads when on a metered Wi-Fi connection @@ -390,13 +390,6 @@ Firehose Mode allows you to maximize your training data uploads to improve openp - - Installer - - Installing... - 正在安装…… - - MultiOptionDialog diff --git a/selfdrive/ui/translations/main_zh-CHT.ts b/selfdrive/ui/translations/main_zh-CHT.ts index e28ac44b98..160c293072 100644 --- a/selfdrive/ui/translations/main_zh-CHT.ts +++ b/selfdrive/ui/translations/main_zh-CHT.ts @@ -103,7 +103,7 @@ - Prevent large data uploads when on a metered Wi-FI connection + Prevent large data uploads when on a metered Wi-Fi connection @@ -390,13 +390,6 @@ Firehose Mode allows you to maximize your training data uploads to improve openp - - Installer - - Installing... - 安裝中… - - MultiOptionDialog From 7511983ccb2746afe5c35d00cd2e7ead8aa55cf0 Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Wed, 28 May 2025 03:47:58 +0800 Subject: [PATCH 14/60] system/ui: cache shader location (#35360) cache shader location --- system/ui/widgets/cameraview.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/system/ui/widgets/cameraview.py b/system/ui/widgets/cameraview.py index bff13a7910..49832444f8 100644 --- a/system/ui/widgets/cameraview.py +++ b/system/ui/widgets/cameraview.py @@ -60,6 +60,7 @@ class CameraView: 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.frame: VisionBuf | None = None self.texture_y: rl.Texture | None = None @@ -188,7 +189,7 @@ class CameraView: # Render with shader rl.begin_shader_mode(self.shader) - rl.set_shader_value_texture(self.shader, rl.get_shader_location(self.shader, "texture1"), self.texture_uv) + 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() From 28da563386c3bc4a480d441c95ca4982a70815ee Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Wed, 28 May 2025 03:48:33 +0800 Subject: [PATCH 15/60] system/ui: render model output with new ModelRenderer class (#35356) render model output with new ModelRenderer class --- system/ui/onroad/augmented_road_view.py | 15 +- system/ui/onroad/model_renderer.py | 360 ++++++++++++++++++++++++ 2 files changed, 374 insertions(+), 1 deletion(-) create mode 100644 system/ui/onroad/model_renderer.py diff --git a/system/ui/onroad/augmented_road_view.py b/system/ui/onroad/augmented_road_view.py index 7ae8f37c13..316167a828 100644 --- a/system/ui/onroad/augmented_road_view.py +++ b/system/ui/onroad/augmented_road_view.py @@ -3,6 +3,7 @@ import pyray as rl from cereal import messaging, log from msgq.visionipc import VisionStreamType +from openpilot.system.ui.onroad.model_renderer import ModelRenderer from openpilot.system.ui.widgets.cameraview import CameraView from openpilot.system.ui.lib.application import gui_app from openpilot.common.transformations.camera import DEVICE_CAMERAS, DeviceCameraConfig, view_frame_from_device_frame @@ -29,6 +30,8 @@ class AugmentedRoadView(CameraView): self._last_rect_dims = (0.0, 0.0) self._cached_matrix: np.ndarray | None = None + self.model_renderer = ModelRenderer() + def render(self, rect): # Update calibration before rendering self._update_calibration() @@ -41,6 +44,7 @@ class AugmentedRoadView(CameraView): # - Path prediction # - Lead vehicle indicators # - Additional features + self.model_renderer.draw(rect, self.sm) def _update_calibration(self): # Update device camera if not already set @@ -112,12 +116,21 @@ class AugmentedRoadView(CameraView): [0, 0, 1.0] ]) + video_transform = np.array([ + [zoom, 0.0, (w / 2 - x_offset) - (cx * zoom)], + [0.0, zoom, (h / 2 - y_offset) - (cy * zoom)], + [0.0, 0.0, 1.0] + ]) + self.model_renderer.set_transform(video_transform @ calib_transform) + return self._cached_matrix if __name__ == "__main__": gui_app.init_window("OnRoad Camera View") - sm = messaging.SubMaster(['deviceState', 'liveCalibration', 'roadCameraState']) + sm = messaging.SubMaster(["modelV2", "controlsState", "liveCalibration", "radarState", "deviceState", + "pandaStates", "carParams", "driverMonitoringState", "carState", "driverStateV2", + "roadCameraState", "wideRoadCameraState", "managerState", "selfdriveState", "longitudinalPlan"]) road_camera_view = AugmentedRoadView(sm, VisionStreamType.VISION_STREAM_ROAD) try: for _ in gui_app.render(): diff --git a/system/ui/onroad/model_renderer.py b/system/ui/onroad/model_renderer.py new file mode 100644 index 0000000000..d0b66666fa --- /dev/null +++ b/system/ui/onroad/model_renderer.py @@ -0,0 +1,360 @@ +import colorsys +import bisect +import numpy as np +import pyray as rl +from cereal import messaging, car +from openpilot.common.params import Params + + +CLIP_MARGIN = 500 +MIN_DRAW_DISTANCE = 10.0 +MAX_DRAW_DISTANCE = 100.0 + + +THROTTLE_COLORS = [ + rl.Color(0, 231, 130, 102), # Green with alpha 0.4 + rl.Color(112, 247, 35, 89), # Lime with alpha 0.35 + rl.Color(112, 247, 35, 0), # Transparent lime +] + +NO_THROTTLE_COLORS = [ + rl.Color(242, 242, 242, 102), # Light gray with alpha 0.4 + rl.Color(242, 242, 242, 89), # Light gray with alpha 0.35 + rl.Color(242, 242, 242, 0), # Transparent light gray +] + + +class ModelRenderer: + def __init__(self): + self._longitudinal_control = False + self._experimental_mode = False + self._blend_factor = 1.0 + self._prev_allow_throttle = True + self._lane_line_probs = [0.0] * 4 + self._road_edge_stds = [0.0] * 2 + self._path_offset_z = 1.22 + + # Initialize empty polygon vertices + self._track_vertices = [] + self._lane_line_vertices = [[] for _ in range(4)] + self._road_edge_vertices = [[] for _ in range(2)] + self._lead_vertices = [None, None] + + # Transform matrix (3x3 for car space to screen space) + self._car_space_transform = np.zeros((3, 3)) + self._transform_dirty = True + self._clip_region = None + + # Get longitudinal control setting from car parameters + car_params = Params().get("CarParams") + if car_params: + cp = messaging.log_from_bytes(car_params, car.CarParams) + self._longitudinal_control = cp.openpilotLongitudinalControl + + def set_transform(self, transform: np.ndarray): + self._car_space_transform = transform + self._transform_dirty = True + + def draw(self, rect: rl.Rectangle, sm: messaging.SubMaster): + # Check if data is up-to-date + if not sm.valid['modelV2'] or not sm.valid['liveCalibration']: + return + + # Set up clipping region + self._clip_region = rl.Rectangle( + rect.x - CLIP_MARGIN, rect.y - CLIP_MARGIN, rect.width + 2 * CLIP_MARGIN, rect.height + 2 * CLIP_MARGIN + ) + + # Update flags based on car state + self._experimental_mode = sm['selfdriveState'].experimentalMode + self._path_offset_z = sm['liveCalibration'].height[0] + if sm.updated['carParams']: + self._longitudinal_control = sm['carParams'].openpilotLongitudinalControl + + # Get model and radar data + model = sm['modelV2'] + radar_state = sm['radarState'] if sm.valid['radarState'] else None + lead_one = radar_state.leadOne if radar_state else None + render_lead_indicator = self._longitudinal_control and radar_state is not None + + # Update model data when needed + if self._transform_dirty or sm.updated['modelV2'] or sm.updated['radarState']: + self._update_model(model, lead_one) + if render_lead_indicator: + self._update_leads(radar_state, model.position) + self._transform_dirty = False + + # Draw elements + self._draw_lane_lines() + self._draw_path(sm, model, rect.height) + + # Draw lead vehicles if available + if render_lead_indicator and radar_state: + lead_two = radar_state.leadTwo + + if lead_one and lead_one.status: + self._draw_lead(lead_one, self._lead_vertices[0], rect) + + if lead_two and lead_two.status and lead_one and (abs(lead_one.dRel - lead_two.dRel) > 3.0): + self._draw_lead(lead_two, self._lead_vertices[1], rect) + + def _update_leads(self, radar_state, line): + """Update positions of lead vehicles""" + leads = [radar_state.leadOne, radar_state.leadTwo] + for i, lead_data in enumerate(leads): + if lead_data and lead_data.status: + d_rel = lead_data.dRel + y_rel = lead_data.yRel + idx = self._get_path_length_idx(line, d_rel) + z = line.z[idx] + self._lead_vertices[i] = self._map_to_screen(d_rel, -y_rel, z + self._path_offset_z) + + def _update_model(self, model, lead): + """Update model visualization data based on model message""" + model_position = model.position + + # Determine max distance to render + max_distance = np.clip(model_position.x[-1], MIN_DRAW_DISTANCE, MAX_DRAW_DISTANCE) + + # Update lane lines + lane_lines = model.laneLines + line_probs = model.laneLineProbs + max_idx = self._get_path_length_idx(lane_lines[0], max_distance) + + for i in range(4): + self._lane_line_probs[i] = line_probs[i] + self._lane_line_vertices[i] = self._map_line_to_polygon( + lane_lines[i], 0.025 * self._lane_line_probs[i], 0, max_idx + ) + + # Update road edges + road_edges = model.roadEdges + edge_stds = model.roadEdgeStds + + for i in range(2): + self._road_edge_stds[i] = edge_stds[i] + self._road_edge_vertices[i] = self._map_line_to_polygon(road_edges[i], 0.025, 0, max_idx) + + # Update path + if lead and lead.status: + lead_d = lead.dRel * 2.0 + max_distance = np.clip(lead_d - min(lead_d * 0.35, 10.0), 0.0, max_distance) + max_idx = self._get_path_length_idx(model_position, max_distance) + + self._track_vertices = self._map_line_to_polygon(model_position, 0.9, self._path_offset_z, max_idx, False) + + def _draw_lane_lines(self): + """Draw lane lines and road edges""" + for i in range(4): + # Skip if no vertices + if not self._lane_line_vertices[i]: + continue + + # Draw lane line + alpha = np.clip(self._lane_line_probs[i], 0.0, 0.7) + color = rl.Color(255, 255, 255, int(alpha * 255)) + self._draw_polygon(self._lane_line_vertices[i], color) + + for i in range(2): + # Skip if no vertices + if not self._road_edge_vertices[i]: + continue + + # Draw road edge + alpha = np.clip(1.0 - self._road_edge_stds[i], 0.0, 1.0) + color = rl.Color(255, 0, 0, int(alpha * 255)) + self._draw_polygon(self._road_edge_vertices[i], color) + + def _draw_path(self, sm, model, height): + """Draw the path polygon with gradient based on acceleration""" + if not self._track_vertices: + return + + if self._experimental_mode: + # Draw with acceleration coloring + acceleration = model.acceleration.x + max_len = min(len(self._track_vertices) // 2, len(acceleration)) + + # Create gradient colors for path sections + for i in range(max_len): + track_idx = max_len - i - 1 # flip idx to start from bottom right + track_y = self._track_vertices[track_idx][1] + # Skip points out of frame + if track_y < 0 or track_y > height: + continue + + # Calculate color based on acceleration + lin_grad_point = (height - track_y) / height + + # speed up: 120, slow down: 0 + path_hue = max(min(60 + acceleration[i] * 35, 120), 0) + path_hue = int(path_hue * 100 + 0.5) / 100 + + saturation = min(abs(acceleration[i] * 1.5), 1) + lightness = self._map_val(saturation, 0.0, 1.0, 0.95, 0.62) + alpha = self._map_val(lin_grad_point, 0.75 / 2.0, 0.75, 0.4, 0.0) + + # Use HSL to RGB conversion + color = self._hsla_to_color(path_hue / 360.0, saturation, lightness, alpha) + + # TODO: This is simplified - a full implementation would create a gradient fill + segment = self._track_vertices[track_idx : track_idx + 2] + self._track_vertices[-track_idx - 2 : -track_idx] + self._draw_polygon(segment, color) + + # Skip a point, unless next is last + i += 1 if i + 2 < max_len else 0 + else: + # Draw with throttle/no throttle gradient + allow_throttle = sm['longitudinalPlan'].allowThrottle or not self._longitudinal_control + + # Start transition if throttle state changes + if allow_throttle != self._prev_allow_throttle: + self._prev_allow_throttle = allow_throttle + self._blend_factor = max(1.0 - self._blend_factor, 0.0) + + # Update blend factor + if self._blend_factor < 1.0: + self._blend_factor = min(self._blend_factor + 0.1, 1.0) + + begin_colors = NO_THROTTLE_COLORS if allow_throttle else THROTTLE_COLORS + end_colors = THROTTLE_COLORS if allow_throttle else NO_THROTTLE_COLORS + + # Blend colors based on transition + colors = [ + self._blend_colors(begin_colors[0], end_colors[0], self._blend_factor), + self._blend_colors(begin_colors[1], end_colors[1], self._blend_factor), + self._blend_colors(begin_colors[2], end_colors[2], self._blend_factor), + ] + + self._draw_polygon(self._track_vertices, colors[0]) + + def _draw_lead(self, lead_data, vd, rect): + """Draw lead vehicle indicator""" + if not vd: + return + + speed_buff = 10.0 + lead_buff = 40.0 + d_rel = lead_data.dRel + v_rel = lead_data.vRel + + # Calculate fill alpha + fill_alpha = 0 + if d_rel < lead_buff: + fill_alpha = 255 * (1.0 - (d_rel / lead_buff)) + if v_rel < 0: + fill_alpha += 255 * (-1 * (v_rel / speed_buff)) + fill_alpha = min(fill_alpha, 255) + + # Calculate size and position + sz = np.clip((25 * 30) / (d_rel / 3 + 30), 15.0, 30.0) * 2.35 + x = np.clip(vd[0], 0.0, rect.width - sz / 2) + y = min(vd[1], rect.height - sz * 0.6) + + g_xo = sz / 5 + g_yo = sz / 10 + + # Draw glow + glow = [(x + (sz * 1.35) + g_xo, y + sz + g_yo), (x, y - g_yo), (x - (sz * 1.35) - g_xo, y + sz + g_yo)] + rl.draw_triangle_fan(glow, len(glow), rl.Color(218, 202, 37, 255)) + + # Draw chevron + chevron = [(x + (sz * 1.25), y + sz), (x, y), (x - (sz * 1.25), y + sz)] + rl.draw_triangle_fan(chevron, len(chevron), rl.Color(201, 34, 49, int(fill_alpha))) + + @staticmethod + def _get_path_length_idx(line, path_height): + """Get the index corresponding to the given path height""" + return bisect.bisect_right(line.x, path_height) - 1 + + def _map_to_screen(self, in_x, in_y, in_z): + """Project a point in car space to screen space""" + input_pt = np.array([in_x, in_y, in_z]) + pt = self._car_space_transform @ input_pt + + if abs(pt[2]) < 1e-6: + return None + + x = pt[0] / pt[2] + y = pt[1] / pt[2] + + clip = self._clip_region + if x < clip.x or x > clip.x + clip.width or y < clip.y or y > clip.y + clip.height: + return None + + return (x, y) + + def _map_line_to_polygon(self, line, y_off, z_off, max_idx, allow_invert=True): + """Convert a 3D line to a 2D polygon for drawing""" + line_x = line.x + line_y = line.y + line_z = line.z + + left_points = [] + right_points = [] + + for i in range(max_idx + 1): + # Skip points with negative x (behind camera) + if line_x[i] < 0: + continue + + left = self._map_to_screen(line_x[i], line_y[i] - y_off, line_z[i] + z_off) + right = self._map_to_screen(line_x[i], line_y[i] + y_off, line_z[i] + z_off) + + if left and right: + # Check for inversion when going over hills + if not allow_invert and left_points and left[1] > left_points[-1][1]: + continue + + left_points.append(left) + right_points.append(right) + + if not left_points: + return [] + + return left_points + right_points[::-1] + + def _draw_polygon(self, points, color): + # TODO: Enhance polygon drawing to support even-odd fill rule efficiently, as Raylib lacks native support. + # Use a faster triangulation algorithm (e.g., ear clipping) or GPU shader for + # efficient rendering of lane lines, road edges, and path polygons. + if len(points) <= 8: + rl.draw_triangle_fan(points, len(points), color) + else: + for i in range(1, len(points) - 1): + rl.draw_triangle(points[0], points[i], points[i + 1], color) + + for i in range(len(points)): + rl.draw_line_ex(points[i], points[(i + 1) % len(points)], 1.5, color) + + @staticmethod + def _map_val(x, x0, x1, y0, y1): + """Map value x from range [x0, x1] to range [y0, y1]""" + return y0 + (y1 - y0) * ((x - x0) / (x1 - x0)) if x1 != x0 else y0 + + @staticmethod + def _hsla_to_color(h, s, l, a): + """Convert HSLA color to Raylib Color using colorsys module""" + # colorsys uses HLS format (Hue, Lightness, Saturation) + r, g, b = colorsys.hls_to_rgb(h, l, s) + + # Ensure values are in valid range + r_val = max(0, min(255, int(r * 255))) + g_val = max(0, min(255, int(g * 255))) + b_val = max(0, min(255, int(b * 255))) + a_val = max(0, min(255, int(a * 255))) + + return rl.Color(r_val, g_val, b_val, a_val) + + @staticmethod + def _blend_colors(start, end, t): + """Blend between two colors with factor t""" + if t >= 1.0: + return end + + return rl.Color( + int((1 - t) * start.r + t * end.r), + int((1 - t) * start.g + t * end.g), + int((1 - t) * start.b + t * end.b), + int((1 - t) * start.a + t * end.a), + ) From 3a7f0b66aa502055ad035cbcd18d503d0198bb22 Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Wed, 28 May 2025 06:01:53 +0800 Subject: [PATCH 16/60] system/ui: fix remaining issues in WiFi Manager (#35301) * WIP * fix callback * fix connecting network displayed as Connected * thread safe states * fix state sync issues * fix callback --- system/ui/lib/wifi_manager.py | 69 +++++++++++++------------ system/ui/widgets/network.py | 95 +++++++++++++++++++++-------------- 2 files changed, 93 insertions(+), 71 deletions(-) diff --git a/system/ui/lib/wifi_manager.py b/system/ui/lib/wifi_manager.py index 96726bd999..d8a8144fb0 100644 --- a/system/ui/lib/wifi_manager.py +++ b/system/ui/lib/wifi_manager.py @@ -69,8 +69,9 @@ class NetworkInfo: class WifiManagerCallbacks: need_auth: Callable[[str], None] | None = None activated: Callable[[], None] | None = None - forgotten: Callable[[], None] | None = None + forgotten: Callable[[str], None] | None = None networks_updated: Callable[[list[NetworkInfo]], None] | None = None + connection_failed: Callable[[str, str], None] | None = None # Added for error feedback class WifiManager: @@ -98,8 +99,8 @@ class WifiManager: self.bus = await MessageBus(bus_type=BusType.SYSTEM).connect() if not await self._find_wifi_device(): raise ValueError("No Wi-Fi device found") - await self._setup_signals(self.device_path) + await self._setup_signals(self.device_path) self.active_ap_path = await self.get_active_access_point() await self.add_tethering_connection(self._tethering_ssid, DEFAULT_TETHERING_PASSWORD) self.saved_connections = await self._get_saved_connections() @@ -122,7 +123,7 @@ class WifiManager: if self.bus: self.bus.disconnect() - async def request_scan(self) -> None: + async def _request_scan(self) -> None: try: interface = self.device_proxy.get_interface(NM_WIRELESS_IFACE) await interface.call_request_scan({}) @@ -146,12 +147,23 @@ class WifiManager: try: nm_iface = await self._get_interface(NM, path, NM_CONNECTION_IFACE) await nm_iface.call_delete() + if self._current_connection_ssid == ssid: self._current_connection_ssid = None if ssid in self.saved_connections: del self.saved_connections[ssid] + for network in self.networks: + if network.ssid == ssid: + network.is_saved = False + network.is_connected = False + break + + # Notify UI of forgotten connection + if self.callbacks.networks_updated: + self.callbacks.networks_updated(copy.deepcopy(self.networks)) + return True except DBusError as e: cloudlog.error(f"Failed to delete connection for SSID: {ssid}. Error: {e}") @@ -212,10 +224,12 @@ class WifiManager: nm_iface = await self._get_interface(NM, NM_PATH, NM_IFACE) await nm_iface.call_add_and_activate_connection(connection, self.device_path, "/") - await self._update_connection_status() - except DBusError as e: + except Exception as e: self._current_connection_ssid = None cloudlog.error(f"Error connecting to network: {e}") + # Notify UI of failure + if self.callbacks.connection_failed: + self.callbacks.connection_failed(ssid, str(e)) def is_saved(self, ssid: str) -> bool: return ssid in self.saved_connections @@ -393,8 +407,7 @@ class WifiManager: async def _periodic_scan(self): while self.running: try: - await self.request_scan() - await self._get_available_networks() + await self._request_scan() await asyncio.sleep(30) except asyncio.CancelledError: break @@ -423,21 +436,24 @@ class WifiManager: def _on_properties_changed(self, interface: str, changed: dict, invalidated: list): # print("property changed", interface, changed, invalidated) if 'LastScan' in changed: - asyncio.create_task(self._get_available_networks()) + asyncio.create_task(self._refresh_networks()) elif interface == NM_WIRELESS_IFACE and "ActiveAccessPoint" in changed: - self.active_ap_path = changed["ActiveAccessPoint"].value - asyncio.create_task(self._get_available_networks()) + new_ap_path = changed["ActiveAccessPoint"].value + if self.active_ap_path != new_ap_path: + self.active_ap_path = new_ap_path + asyncio.create_task(self._refresh_networks()) def _on_state_changed(self, new_state: int, old_state: int, reason: int): - print(f"State changed: {old_state} -> {new_state}, reason: {reason}") + print("State changed", new_state, old_state, reason) if new_state == NMDeviceState.ACTIVATED: if self.callbacks.activated: self.callbacks.activated() - asyncio.create_task(self._update_connection_status()) + asyncio.create_task(self._refresh_networks()) self._current_connection_ssid = None elif new_state in (NMDeviceState.DISCONNECTED, NMDeviceState.NEED_AUTH): for network in self.networks: network.is_connected = False + if new_state == NMDeviceState.NEED_AUTH and reason == NM_DEVICE_STATE_REASON_SUPPLICANT_DISCONNECT and self.callbacks.need_auth: if self._current_connection_ssid: self.callbacks.need_auth(self._current_connection_ssid) @@ -453,19 +469,19 @@ class WifiManager: def _on_new_connection(self, path: str) -> None: """Callback for NewConnection signal.""" - print(f"New connection added: {path}") asyncio.create_task(self._add_saved_connection(path)) def _on_connection_removed(self, path: str) -> None: """Callback for ConnectionRemoved signal.""" - print(f"Connection removed: {path}") for ssid, p in list(self.saved_connections.items()): if path == p: del self.saved_connections[ssid] + if self.callbacks.forgotten: - self.callbacks.forgotten() + self.callbacks.forgotten(ssid) + # Update network list to reflect the removed saved connection - asyncio.create_task(self._update_connection_status()) + asyncio.create_task(self._refresh_networks()) break async def _add_saved_connection(self, path: str) -> None: @@ -474,7 +490,7 @@ class WifiManager: settings = await self._get_connection_settings(path) if ssid := self._extract_ssid(settings): self.saved_connections[ssid] = path - await self._update_connection_status() + await self._refresh_networks() except DBusError as e: cloudlog.error(f"Failed to add connection {path}: {e}") @@ -483,10 +499,6 @@ class WifiManager: ssid_variant = settings.get('802-11-wireless', {}).get('ssid', Variant('ay', b'')).value return ''.join(chr(b) for b in ssid_variant) if ssid_variant else None - async def _update_connection_status(self): - self.active_ap_path = await self.get_active_access_point() - await self._get_available_networks() - async def _add_match_rule(self, rule): """Add a match rule on the bus.""" reply = await self.bus.call( @@ -504,10 +516,11 @@ class WifiManager: assert reply.message_type == MessageType.METHOD_RETURN return reply - async def _get_available_networks(self): + async def _refresh_networks(self): """Get a list of available networks via NetworkManager.""" wifi_iface = self.device_proxy.get_interface(NM_WIRELESS_IFACE) access_points = await wifi_iface.get_access_points() + self.active_ap_path = await self.get_active_access_point() network_dict = {} for ap_path in access_points: try: @@ -531,7 +544,7 @@ class WifiManager: security_type=self._get_security_type(flags, wpa_flags, rsn_flags), path=ap_path, bssid=bssid, - is_connected=self.active_ap_path == ap_path, + is_connected=self.active_ap_path == ap_path and self._current_connection_ssid != ssid, is_saved=ssid in self.saved_connections ) @@ -555,9 +568,7 @@ class WifiManager: async def _get_connection_settings(self, path): """Fetch connection settings for a specific connection path.""" try: - connection_proxy = await self.bus.introspect(NM, path) - connection = self.bus.get_proxy_object(NM, path, connection_proxy) - settings = connection.get_interface(NM_CONNECTION_IFACE) + settings = await self._get_interface(NM, path, NM_CONNECTION_IFACE) return await settings.call_get_settings() except DBusError as e: cloudlog.error(f"Failed to get settings for {path}: {str(e)}") @@ -660,12 +671,6 @@ class WifiManagerWrapper: return self._run_coroutine(self._manager.connect()) - def request_scan(self): - """Request a scan for Wi-Fi networks.""" - if not self._manager: - return - self._run_coroutine(self._manager.request_scan()) - def forget_connection(self, ssid: str): """Forget a saved Wi-Fi connection.""" if not self._manager: diff --git a/system/ui/widgets/network.py b/system/ui/widgets/network.py index 5877b3ff7a..46274dbf7e 100644 --- a/system/ui/widgets/network.py +++ b/system/ui/widgets/network.py @@ -1,4 +1,5 @@ from dataclasses import dataclass +from threading import Lock from typing import Literal import pyray as rl @@ -53,48 +54,59 @@ UIState = StateIdle | StateConnecting | StateNeedsAuth | StateShowForgetConfirm class WifiManagerUI: def __init__(self, wifi_manager: WifiManagerWrapper): self.state: UIState = StateIdle() - self.btn_width = 200 + self.btn_width: int = 200 self.scroll_panel = GuiScrollPanel() self.keyboard = Keyboard(max_text_size=MAX_PASSWORD_LENGTH, min_text_size=MIN_PASSWORD_LENGTH, show_password_toggle=True) self._networks: list[NetworkInfo] = [] - + self._lock = Lock() self.wifi_manager = wifi_manager - self.wifi_manager.set_callbacks(WifiManagerCallbacks(self._on_need_auth, self._on_activated, self._on_forgotten, self._on_network_updated)) + + self.wifi_manager.set_callbacks( + WifiManagerCallbacks( + need_auth = self._on_need_auth, + activated = self._on_activated, + forgotten = self._on_forgotten, + networks_updated = self._on_network_updated, + connection_failed = self._on_connection_failed + ) + ) self.wifi_manager.start() self.wifi_manager.connect() def render(self, rect: rl.Rectangle): - if not self._networks: - gui_label(rect, "Scanning Wi-Fi networks...", 72, alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER) - return + with self._lock: + if not self._networks: + gui_label(rect, "Scanning Wi-Fi networks...", 72, alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER) + return - match self.state: - case StateNeedsAuth(network): - result = self.keyboard.render("Enter password", f"for {network.ssid}") - if result == 1: - password = self.keyboard.text - self.keyboard.clear() + match self.state: + case StateNeedsAuth(network): + result = self.keyboard.render("Enter password", f"for {network.ssid}") + if result == 1: + password = self.keyboard.text + self.keyboard.clear() - if len(password) >= MIN_PASSWORD_LENGTH: - self.connect_to_network(network, password) - elif result == 0: - self.state = StateIdle() + if len(password) >= MIN_PASSWORD_LENGTH: + self.connect_to_network(network, password) + elif result == 0: + self.state = StateIdle() - case StateShowForgetConfirm(network): - result = confirm_dialog(f'Forget Wi-Fi Network "{network.ssid}"?', "Forget") - if result == 1: - self.forget_network(network) - elif result == 0: - self.state = StateIdle() + case StateShowForgetConfirm(network): + result = confirm_dialog(f'Forget Wi-Fi Network "{network.ssid}"?', "Forget") + if result == 1: + self.forget_network(network) + elif result == 0: + self.state = StateIdle() - case _: - self._draw_network_list(rect) + case _: + self._draw_network_list(rect) @property def require_full_screen(self) -> bool: """Check if the WiFi UI requires exclusive full-screen rendering.""" - return isinstance(self.state, (StateNeedsAuth, StateShowForgetConfirm)) + with self._lock: + return isinstance(self.state, (StateNeedsAuth, StateShowForgetConfirm)) def _draw_network_list(self, rect: rl.Rectangle): content_rect = rl.Rectangle(rect.x, rect.y, rect.width, len(self._networks) * ITEM_HEIGHT) @@ -190,25 +202,30 @@ class WifiManagerUI: self.wifi_manager.forget_connection(network.ssid) def _on_network_updated(self, networks: list[NetworkInfo]): - self._networks = networks + with self._lock: + self._networks = networks def _on_need_auth(self, ssid): - match self.state: - case StateConnecting(ssid): - self.state = StateNeedsAuth(ssid) - case _: - # Find network by SSID - network = next((n for n in self.wifi_manager.networks if n.ssid == ssid), None) - if network: - self.state = StateNeedsAuth(network) + with self._lock: + network = next((n for n in self._networks if n.ssid == ssid), None) + if network: + self.state = StateNeedsAuth(network) def _on_activated(self): - if isinstance(self.state, StateConnecting): - self.state = StateIdle() + with self._lock: + if isinstance(self.state, StateConnecting): + self.state = StateIdle() + + def _on_forgotten(self, ssid): + with self._lock: + if isinstance(self.state, StateForgetting): + self.state = StateIdle() + + def _on_connection_failed(self, ssid: str, error: str): + with self._lock: + if isinstance(self.state, StateConnecting): + self.state = StateIdle() - def _on_forgotten(self): - if isinstance(self.state, StateForgetting): - self.state = StateIdle() def main(): From 83ba27d7c01d1511a1def1f9e4d0214dca33b35f Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 27 May 2025 19:42:02 -0700 Subject: [PATCH 17/60] Update lane departure warning toggle live (#35363) update LDWS live --- selfdrive/selfdrived/selfdrived.py | 1 + 1 file changed, 1 insertion(+) diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index 98c03acf1e..f3dabf3791 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -485,6 +485,7 @@ class SelfdriveD: def params_thread(self, evt): while not evt.is_set(): self.is_metric = self.params.get_bool("IsMetric") + self.is_ldw_enabled = self.params.get_bool("IsLdwEnabled") self.disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator") self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl self.personality = self.read_personality_param() From b119006f6a67ba874ef6c718ac49c25deef06e29 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 27 May 2025 20:20:38 -0700 Subject: [PATCH 18/60] Cycle onroad when changing offroad-only toggles (#35361) * bad * Revert "bad" This reverts commit 6b5475dd90c3a29c00d946c94d726563cbec6179. * notify param * doesn't need to live in low level paramcontrol, rename param * should work * fix * disable while engaged * note * fix * just in case * param is cleared by manager -- this was all to ensure manager got our `started` transition * clean up * and that * rm * negative better than generic thing * timer is needed as it's not clean to fix case where you toggle while no ignition -- you can't go onroad + this allows some nice tolerance time for user to switch 2 toggles * these aren't required or useful * add to description * no longer unlive * allow reset button too * another pr -- Revert "allow reset button too" This reverts commit 5d03edddc80d8625ceba5d5178b2781e9d10d9c9. * rm space from i18n string * car is powered on --- common/params_keys.h | 1 + selfdrive/ui/qt/offroad/settings.cc | 27 +++++++++++++++++++++++---- selfdrive/ui/ui.cc | 5 +++++ selfdrive/ui/ui.h | 2 ++ system/hardware/hardwared.py | 11 ++++++++++- 5 files changed, 41 insertions(+), 5 deletions(-) diff --git a/common/params_keys.h b/common/params_keys.h index ca779a5b5c..3fd4e1b6ab 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -93,6 +93,7 @@ inline static std::unordered_map keys = { {"Offroad_TemperatureTooHigh", CLEAR_ON_MANAGER_START}, {"Offroad_UnofficialHardware", CLEAR_ON_MANAGER_START}, {"Offroad_UpdateFailed", CLEAR_ON_MANAGER_START}, + {"OnroadCycleRequested", CLEAR_ON_MANAGER_START}, {"OpenpilotEnabledToggle", PERSISTENT}, {"PandaHeartbeatLost", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION}, {"PandaSomResetTriggered", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION}, diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index ed06734f0f..baac582645 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -17,49 +17,56 @@ #include "selfdrive/ui/qt/offroad/firehose.h" TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) { - // param, title, desc, icon - std::vector> toggle_defs{ + // param, title, desc, icon, restart needed + std::vector> toggle_defs{ { "OpenpilotEnabledToggle", tr("Enable openpilot"), - tr("Use the openpilot system for adaptive cruise control and lane keep driver assistance. Your attention is required at all times to use this feature. Changing this setting takes effect when the car is powered off."), + tr("Use the openpilot system for adaptive cruise control and lane keep driver assistance. Your attention is required at all times to use this feature."), "../assets/icons/chffr_wheel.png", + true, }, { "ExperimentalMode", tr("Experimental Mode"), "", "../assets/icons/experimental_white.svg", + false, }, { "DisengageOnAccelerator", tr("Disengage on Accelerator Pedal"), tr("When enabled, pressing the accelerator pedal will disengage openpilot."), "../assets/icons/disengage_on_accelerator.svg", + false, }, { "IsLdwEnabled", tr("Enable Lane Departure Warnings"), tr("Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line without a turn signal activated while driving over 31 mph (50 km/h)."), "../assets/icons/warning.png", + false, }, { "AlwaysOnDM", tr("Always-On Driver Monitoring"), tr("Enable driver monitoring even when openpilot is not engaged."), "../assets/icons/monitoring.png", + false, }, { "RecordFront", tr("Record and Upload Driver Camera"), tr("Upload data from the driver facing camera and help improve the driver monitoring algorithm."), "../assets/icons/monitoring.png", + true, }, { "IsMetric", tr("Use Metric System"), tr("Display speed in km/h instead of mph."), "../assets/icons/metric.png", + false, }, }; @@ -75,12 +82,24 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) { // set up uiState update for personality setting QObject::connect(uiState(), &UIState::uiUpdate, this, &TogglesPanel::updateState); - for (auto &[param, title, desc, icon] : toggle_defs) { + for (auto &[param, title, desc, icon, needs_restart] : toggle_defs) { auto toggle = new ParamControl(param, title, desc, icon, this); bool locked = params.getBool((param + "Lock").toStdString()); toggle->setEnabled(!locked); + if (needs_restart && !locked) { + toggle->setDescription(toggle->getDescription() + " " + tr("Changing this setting will restart openpilot if the car is powered on.")); + + QObject::connect(uiState(), &UIState::engagedChanged, [toggle](bool engaged) { + toggle->setEnabled(!engaged); + }); + + QObject::connect(toggle, &ParamControl::toggleFlipped, [=](bool state) { + params.putBool("OnroadCycleRequested", true); + }); + } + addItem(toggle); toggles[param.toStdString()] = toggle; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index ec3d40961d..79a245a0e7 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -78,6 +78,11 @@ void UIState::updateStatus() { } } + if (engaged() != engaged_prev) { + engaged_prev = engaged(); + emit engagedChanged(engaged()); + } + // Handle onroad/offroad transition if (scene.started != started_prev || sm->frame == 1) { if (scene.started) { diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 6ff67cacde..fd2aee771e 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -81,6 +81,7 @@ public: signals: void uiUpdate(const UIState &s); void offroadTransition(bool offroad); + void engagedChanged(bool engaged); private slots: void update(); @@ -88,6 +89,7 @@ private slots: private: QTimer *timer; bool started_prev = false; + bool engaged_prev = false; }; UIState *uiState(); diff --git a/system/hardware/hardwared.py b/system/hardware/hardwared.py index b6de91818e..15a191adc4 100755 --- a/system/hardware/hardwared.py +++ b/system/hardware/hardwared.py @@ -34,6 +34,7 @@ CURRENT_TAU = 15. # 15s time constant TEMP_TAU = 5. # 5s time constant DISCONNECT_TIMEOUT = 5. # wait 5 seconds before going offroad after disconnect so you get an alert PANDA_STATES_TIMEOUT = round(1000 / SERVICE_LIST['pandaStates'].frequency * 1.5) # 1.5x the expected pandaState frequency +ONROAD_CYCLE_TIME = 1 # seconds to wait offroad after requesting an onroad cycle ThermalBand = namedtuple("ThermalBand", ['min_temp', 'max_temp']) HardwareState = namedtuple("HardwareState", ['network_type', 'network_info', 'network_strength', 'network_stats', @@ -170,6 +171,7 @@ def hardware_thread(end_event, hw_queue) -> None: onroad_conditions: dict[str, bool] = { "ignition": False, + "not_onroad_cycle": True, } startup_conditions: dict[str, bool] = {} startup_conditions_prev: dict[str, bool] = {} @@ -195,6 +197,7 @@ def hardware_thread(end_event, hw_queue) -> None: should_start_prev = False in_car = False engaged_prev = False + offroad_cycle_count = 0 params = Params() power_monitor = PowerMonitoring() @@ -211,6 +214,12 @@ def hardware_thread(end_event, hw_queue) -> None: peripheralState = sm['peripheralState'] peripheral_panda_present = peripheralState.pandaType != log.PandaState.PandaType.unknown + # handle requests to cycle system started state + if params.get_bool("OnroadCycleRequested"): + params.put_bool("OnroadCycleRequested", False) + offroad_cycle_count = sm.frame + onroad_conditions["not_onroad_cycle"] = (sm.frame - offroad_cycle_count) >= ONROAD_CYCLE_TIME * SERVICE_LIST['pandaStates'].frequency + if sm.updated['pandaStates'] and len(pandaStates) > 0: # Set ignition based on any panda connected @@ -231,7 +240,7 @@ def hardware_thread(end_event, hw_queue) -> None: cloudlog.error("panda timed out onroad") # Run at 2Hz, plus either edge of ignition - ign_edge = (started_ts is not None) != onroad_conditions["ignition"] + ign_edge = (started_ts is not None) != all(onroad_conditions.values()) if (sm.frame % round(SERVICE_LIST['pandaStates'].frequency * DT_HW) != 0) and not ign_edge: continue From 3d53df644ea7bd3c1fedc3610792eb640a161ffc Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 27 May 2025 20:27:54 -0700 Subject: [PATCH 19/60] Live calibration reset button (#35364) * allow reset button too * better lang * copy * fix * meh we don't do this anywhere else --- selfdrive/ui/qt/offroad/settings.cc | 25 +++++++++++++++++-------- 1 file changed, 17 insertions(+), 8 deletions(-) diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index baac582645..e81b58b919 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -89,7 +89,7 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) { toggle->setEnabled(!locked); if (needs_restart && !locked) { - toggle->setDescription(toggle->getDescription() + " " + tr("Changing this setting will restart openpilot if the car is powered on.")); + toggle->setDescription(toggle->getDescription() + tr(" Changing this setting will restart openpilot if the car is powered on.")); QObject::connect(uiState(), &UIState::engagedChanged, [toggle](bool engaged) { toggle->setEnabled(!engaged); @@ -210,12 +210,20 @@ DevicePanel::DevicePanel(SettingsWindow *parent) : ListWidget(parent) { auto resetCalibBtn = new ButtonControl(tr("Reset Calibration"), tr("RESET"), ""); connect(resetCalibBtn, &ButtonControl::showDescriptionEvent, this, &DevicePanel::updateCalibDescription); connect(resetCalibBtn, &ButtonControl::clicked, [&]() { - if (ConfirmationDialog::confirm(tr("Are you sure you want to reset calibration?"), tr("Reset"), this)) { - params.remove("CalibrationParams"); - params.remove("LiveTorqueParameters"); - params.remove("LiveParameters"); - params.remove("LiveParametersV2"); - params.remove("LiveDelay"); + if (!uiState()->engaged()) { + if (ConfirmationDialog::confirm(tr("Are you sure you want to reset calibration?"), tr("Reset"), this)) { + // Check engaged again in case it changed while the dialog was open + if (!uiState()->engaged()) { + params.remove("CalibrationParams"); + params.remove("LiveTorqueParameters"); + params.remove("LiveParameters"); + params.remove("LiveParametersV2"); + params.remove("LiveDelay"); + params.putBool("OnroadCycleRequested", true); + } + } + } else { + ConfirmationDialog::alert(tr("Disengage to Reset Calibration"), this); } }); addItem(resetCalibBtn); @@ -255,7 +263,7 @@ DevicePanel::DevicePanel(SettingsWindow *parent) : ListWidget(parent) { }); QObject::connect(uiState(), &UIState::offroadTransition, [=](bool offroad) { for (auto btn : findChildren()) { - if (btn != pair_device) { + if (btn != pair_device && btn != resetCalibBtn) { btn->setEnabled(offroad); } } @@ -309,6 +317,7 @@ void DevicePanel::updateCalibDescription() { qInfo() << "invalid CalibrationParams"; } } + desc += tr(" Resetting calibration will restart openpilot if the car is powered on."); qobject_cast(sender())->setDescription(desc); } From feaef58188fc38a7b850541197548c22419de058 Mon Sep 17 00:00:00 2001 From: Maxime Desroches Date: Tue, 27 May 2025 21:20:30 -0700 Subject: [PATCH 20/60] AGNOS 12.3 (#35362) 12.3 --- launch_env.sh | 2 +- system/hardware/tici/agnos.json | 20 +++++------ system/hardware/tici/all-partitions.json | 44 ++++++++++++------------ 3 files changed, 33 insertions(+), 33 deletions(-) diff --git a/launch_env.sh b/launch_env.sh index 6f31fcf776..29e3e38905 100755 --- a/launch_env.sh +++ b/launch_env.sh @@ -7,7 +7,7 @@ export OPENBLAS_NUM_THREADS=1 export VECLIB_MAXIMUM_THREADS=1 if [ -z "$AGNOS_VERSION" ]; then - export AGNOS_VERSION="12.2" + export AGNOS_VERSION="12.3" fi export STAGING_ROOT="/data/safe_staging" diff --git a/system/hardware/tici/agnos.json b/system/hardware/tici/agnos.json index e209613f42..7e5b9f187d 100644 --- a/system/hardware/tici/agnos.json +++ b/system/hardware/tici/agnos.json @@ -56,28 +56,28 @@ }, { "name": "boot", - "url": "https://commadist.azureedge.net/agnosupdate/boot-eb0dd0a8ffdc1c03aa28d055788d70be885a00f523b552d7acf79b954838e968.img.xz", - "hash": "eb0dd0a8ffdc1c03aa28d055788d70be885a00f523b552d7acf79b954838e968", - "hash_raw": "eb0dd0a8ffdc1c03aa28d055788d70be885a00f523b552d7acf79b954838e968", + "url": "https://commadist.azureedge.net/agnosupdate/boot-4143170bad94968fd9be870b1498b4100bf273ed0aec2a2601c9017991d4bd42.img.xz", + "hash": "4143170bad94968fd9be870b1498b4100bf273ed0aec2a2601c9017991d4bd42", + "hash_raw": "4143170bad94968fd9be870b1498b4100bf273ed0aec2a2601c9017991d4bd42", "size": 18479104, "sparse": false, "full_check": true, "has_ab": true, - "ondevice_hash": "800868bd9d340f1fdf8340924caca374409624658324607621663fc5c7d10d4f" + "ondevice_hash": "6b7b3371100ad36d8a5a9ff19a1663b9b9e2d5e99cbe3cf9255e9c3017291ce3" }, { "name": "system", - "url": "https://commadist.azureedge.net/agnosupdate/system-4c3fa88833a8a3876653fa53a658769ecc64be8fc6f6a28ce520ef79bf3061c1.img.xz", - "hash": "226794914e9d157b34cc86f1fbe1f2827a9a9919dbe560021b61573a7e5d3101", - "hash_raw": "4c3fa88833a8a3876653fa53a658769ecc64be8fc6f6a28ce520ef79bf3061c1", + "url": "https://commadist.azureedge.net/agnosupdate/system-c51bb5841011728f7cf108a9138ba68228ffb4232dfd91d6e082a6d8a6a8deaa.img.xz", + "hash": "993d6a1cd2b684e2b1cf6ff840f8996f02a529011372d9c1471e4c80719e7da9", + "hash_raw": "c51bb5841011728f7cf108a9138ba68228ffb4232dfd91d6e082a6d8a6a8deaa", "size": 5368709120, "sparse": true, "full_check": false, "has_ab": true, - "ondevice_hash": "236890f04ee21b7eb92a59bc47971bd96cad5a4381ed8935eb4be0cb5a4cf48b", + "ondevice_hash": "59db25651da977eeb16a1af741fd01fc3d6b50d21544b1a7428b7c86b2cdef2d", "alt": { - "hash": "4c3fa88833a8a3876653fa53a658769ecc64be8fc6f6a28ce520ef79bf3061c1", - "url": "https://commadist.azureedge.net/agnosupdate/system-4c3fa88833a8a3876653fa53a658769ecc64be8fc6f6a28ce520ef79bf3061c1.img", + "hash": "c51bb5841011728f7cf108a9138ba68228ffb4232dfd91d6e082a6d8a6a8deaa", + "url": "https://commadist.azureedge.net/agnosupdate/system-c51bb5841011728f7cf108a9138ba68228ffb4232dfd91d6e082a6d8a6a8deaa.img", "size": 5368709120 } } diff --git a/system/hardware/tici/all-partitions.json b/system/hardware/tici/all-partitions.json index 7e39d275cc..bc1141f763 100644 --- a/system/hardware/tici/all-partitions.json +++ b/system/hardware/tici/all-partitions.json @@ -339,62 +339,62 @@ }, { "name": "boot", - "url": "https://commadist.azureedge.net/agnosupdate/boot-eb0dd0a8ffdc1c03aa28d055788d70be885a00f523b552d7acf79b954838e968.img.xz", - "hash": "eb0dd0a8ffdc1c03aa28d055788d70be885a00f523b552d7acf79b954838e968", - "hash_raw": "eb0dd0a8ffdc1c03aa28d055788d70be885a00f523b552d7acf79b954838e968", + "url": "https://commadist.azureedge.net/agnosupdate/boot-4143170bad94968fd9be870b1498b4100bf273ed0aec2a2601c9017991d4bd42.img.xz", + "hash": "4143170bad94968fd9be870b1498b4100bf273ed0aec2a2601c9017991d4bd42", + "hash_raw": "4143170bad94968fd9be870b1498b4100bf273ed0aec2a2601c9017991d4bd42", "size": 18479104, "sparse": false, "full_check": true, "has_ab": true, - "ondevice_hash": "800868bd9d340f1fdf8340924caca374409624658324607621663fc5c7d10d4f" + "ondevice_hash": "6b7b3371100ad36d8a5a9ff19a1663b9b9e2d5e99cbe3cf9255e9c3017291ce3" }, { "name": "system", - "url": "https://commadist.azureedge.net/agnosupdate/system-4c3fa88833a8a3876653fa53a658769ecc64be8fc6f6a28ce520ef79bf3061c1.img.xz", - "hash": "226794914e9d157b34cc86f1fbe1f2827a9a9919dbe560021b61573a7e5d3101", - "hash_raw": "4c3fa88833a8a3876653fa53a658769ecc64be8fc6f6a28ce520ef79bf3061c1", + "url": "https://commadist.azureedge.net/agnosupdate/system-c51bb5841011728f7cf108a9138ba68228ffb4232dfd91d6e082a6d8a6a8deaa.img.xz", + "hash": "993d6a1cd2b684e2b1cf6ff840f8996f02a529011372d9c1471e4c80719e7da9", + "hash_raw": "c51bb5841011728f7cf108a9138ba68228ffb4232dfd91d6e082a6d8a6a8deaa", "size": 5368709120, "sparse": true, "full_check": false, "has_ab": true, - "ondevice_hash": "236890f04ee21b7eb92a59bc47971bd96cad5a4381ed8935eb4be0cb5a4cf48b", + "ondevice_hash": "59db25651da977eeb16a1af741fd01fc3d6b50d21544b1a7428b7c86b2cdef2d", "alt": { - "hash": "4c3fa88833a8a3876653fa53a658769ecc64be8fc6f6a28ce520ef79bf3061c1", - "url": "https://commadist.azureedge.net/agnosupdate/system-4c3fa88833a8a3876653fa53a658769ecc64be8fc6f6a28ce520ef79bf3061c1.img", + "hash": "c51bb5841011728f7cf108a9138ba68228ffb4232dfd91d6e082a6d8a6a8deaa", + "url": "https://commadist.azureedge.net/agnosupdate/system-c51bb5841011728f7cf108a9138ba68228ffb4232dfd91d6e082a6d8a6a8deaa.img", "size": 5368709120 } }, { "name": "userdata_90", - "url": "https://commadist.azureedge.net/agnosupdate/userdata_90-f6d24876234f6bea9cc753892eea99ac4b0c8646e93b93d76fc5dddce67347f1.img.xz", - "hash": "2485459e9fbfc0d88a59a017bfa193d97b80afed30d6f59db711e53f62a21c72", - "hash_raw": "f6d24876234f6bea9cc753892eea99ac4b0c8646e93b93d76fc5dddce67347f1", + "url": "https://commadist.azureedge.net/agnosupdate/userdata_90-89a161f17b86637413fe10a641550110b626b699382f5138c02267b7866a8494.img.xz", + "hash": "99d9e6cf6755581c6879bbf442bd62212beb8a04116e965ab987135b8842188b", + "hash_raw": "89a161f17b86637413fe10a641550110b626b699382f5138c02267b7866a8494", "size": 96636764160, "sparse": true, "full_check": true, "has_ab": false, - "ondevice_hash": "893cfb3e95d6025b9fa6a5c8c3b97bf2abc4ff036c3da846a07536653dbb3a1d" + "ondevice_hash": "24ea29ab9c4ecec0568a4aa83e38790fedfce694060e90f4bde725931386ff41" }, { "name": "userdata_89", - "url": "https://commadist.azureedge.net/agnosupdate/userdata_89-14f0f02e53ce62a79b337d7b3174dcaffebaa4130264bb5ed9105d942338230b.img.xz", - "hash": "9222aebb280531b7015ee9bab3848dd195567d075b6f37f74d4cd0186685a11f", - "hash_raw": "14f0f02e53ce62a79b337d7b3174dcaffebaa4130264bb5ed9105d942338230b", + "url": "https://commadist.azureedge.net/agnosupdate/userdata_89-cdd3401168819987c4840765bba1aa2217641b1a6a4165c412f44cac14ccfcbf.img.xz", + "hash": "5fbfa008a7f6b58ab01d4d171f3185924d4c9db69b54f4bfc0f214c6f17c2435", + "hash_raw": "cdd3401168819987c4840765bba1aa2217641b1a6a4165c412f44cac14ccfcbf", "size": 95563022336, "sparse": true, "full_check": true, "has_ab": false, - "ondevice_hash": "a5459ec858d39e3d0709a09af430cb644fe640f92e0cd216b138f01401e4e17e" + "ondevice_hash": "c07dc2e883a23d4a24d976cdf53a767a2fd699c8eeb476d60cdf18e84b417a52" }, { "name": "userdata_30", - "url": "https://commadist.azureedge.net/agnosupdate/userdata_30-180ad331538f20c00218d41591afed3a25bb0f76fa30b1a665cad102cf6c9f7d.img.xz", - "hash": "7fc09317f005676150bfcced43ebb1aa919d854c2511d547a588a35c1122bfe3", - "hash_raw": "180ad331538f20c00218d41591afed3a25bb0f76fa30b1a665cad102cf6c9f7d", + "url": "https://commadist.azureedge.net/agnosupdate/userdata_30-2a8e8278b3bb545e6d7292c2417ccebdca9b47507eb5924f7c1e068737a7edfd.img.xz", + "hash": "b3bc293c9c5e0480ef663e980c8ccb2fb83ffd230c85f8797830fb61b8f59360", + "hash_raw": "2a8e8278b3bb545e6d7292c2417ccebdca9b47507eb5924f7c1e068737a7edfd", "size": 32212254720, "sparse": true, "full_check": true, "has_ab": false, - "ondevice_hash": "1a2f2d14c922bd2895073446460a0bd680711a7b14318a4402631635e6d133b3" + "ondevice_hash": "8dae1cda089828c750d1d646337774ccd9432f567ecefde19a06dc7feeda9cd3" } ] \ No newline at end of file From 6c28575573b263b50469de05abf4e078dd5d06a5 Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Wed, 28 May 2025 12:47:55 +0800 Subject: [PATCH 21/60] system/ui: GPU-accelerated polygon rendering with anti-aliasing and gradients (#35357) * Add GPU-accelerated polygon rendering with anti-aliased edges and gradient support * use np array * update ModelRenderer * ndarray * cleanup * improve shader * Revert "improve shader" This reverts commit 992247617a9947bceb365f7b056fed6ebed3793d. * improve shader for smoother edges --- system/ui/lib/shader_polygon.py | 338 +++++++++++++++++++++++++++++ system/ui/onroad/model_renderer.py | 101 +++++---- 2 files changed, 397 insertions(+), 42 deletions(-) create mode 100644 system/ui/lib/shader_polygon.py diff --git a/system/ui/lib/shader_polygon.py b/system/ui/lib/shader_polygon.py new file mode 100644 index 0000000000..dfbfa969d5 --- /dev/null +++ b/system/ui/lib/shader_polygon.py @@ -0,0 +1,338 @@ +import pyray as rl +import numpy as np +from typing import Any + + +FRAGMENT_SHADER = """ +#version 300 es +precision mediump float; + +in vec2 fragTexCoord; +out vec4 finalColor; + +uniform vec2 points[100]; +uniform int pointCount; +uniform vec4 fillColor; +uniform vec2 resolution; + +uniform bool useGradient; +uniform vec2 gradientStart; +uniform vec2 gradientEnd; +uniform vec4 gradientColors[8]; +uniform float gradientStops[8]; +uniform int gradientColorCount; + +vec4 getGradientColor(vec2 pos) { + vec2 gradientDir = gradientEnd - gradientStart; + float gradientLength = length(gradientDir); + + if (gradientLength < 0.001) return gradientColors[0]; + + vec2 normalizedDir = gradientDir / gradientLength; + vec2 pointVec = pos - gradientStart; + float projection = dot(pointVec, normalizedDir); + float t = clamp(projection / gradientLength, 0.0, 1.0); + + for (int i = 0; i < gradientColorCount - 1; i++) { + if (t >= gradientStops[i] && t <= gradientStops[i+1]) { + float segmentT = (t - gradientStops[i]) / (gradientStops[i+1] - gradientStops[i]); + return mix(gradientColors[i], gradientColors[i+1], segmentT); + } + } + + return gradientColors[gradientColorCount-1]; +} + +bool isPointInsidePolygon(vec2 p) { + if (pointCount < 3) return false; + + if (pointCount == 3) { + vec2 v0 = points[0]; + vec2 v1 = points[1]; + vec2 v2 = points[2]; + + float d = (v1.y - v2.y) * (v0.x - v2.x) + (v2.x - v1.x) * (v0.y - v2.y); + if (abs(d) < 0.0001) return false; + + float a = ((v1.y - v2.y) * (p.x - v2.x) + (v2.x - v1.x) * (p.y - v2.y)) / d; + float b = ((v2.y - v0.y) * (p.x - v2.x) + (v0.x - v2.x) * (p.y - v2.y)) / d; + float c = 1.0 - a - b; + + return (a >= 0.0 && b >= 0.0 && c >= 0.0); + } + + bool inside = false; + for (int i = 0, j = pointCount - 1; i < pointCount; j = i++) { + if (distance(points[i], points[j]) < 0.0001) continue; + + float dy = points[j].y - points[i].y; + if (abs(dy) < 0.0001) continue; + + if (((points[i].y > p.y) != (points[j].y > p.y))) { + float x_intersect = points[i].x + (points[j].x - points[i].x) * (p.y - points[i].y) / dy; + if (p.x < x_intersect) { + inside = !inside; + } + } + } + return inside; +} + +float distanceToEdge(vec2 p) { + float minDist = 1000.0; + + for (int i = 0, j = pointCount - 1; i < pointCount; j = i++) { + vec2 edge0 = points[j]; + vec2 edge1 = points[i]; + + if (distance(edge0, edge1) < 0.0001) continue; + + vec2 v1 = p - edge0; + vec2 v2 = edge1 - edge0; + float l2 = dot(v2, v2); + + if (l2 < 0.0001) { + float dist = length(v1); + minDist = min(minDist, dist); + continue; + } + + float t = clamp(dot(v1, v2) / l2, 0.0, 1.0); + vec2 projection = edge0 + t * v2; + float dist = length(p - projection); + minDist = min(minDist, dist); + } + + return minDist; +} + +float signedDistanceToPolygon(vec2 p) { + float dist = distanceToEdge(p); + bool inside = isPointInsidePolygon(p); + return inside ? dist : -dist; +} + +void main() { + vec2 pixel = fragTexCoord * resolution; + + float signedDist = signedDistanceToPolygon(pixel); + + vec2 pixelGrad = vec2(dFdx(pixel.x), dFdy(pixel.y)); + float pixelSize = length(pixelGrad); + float aaWidth = max(0.5, pixelSize * 1.0); + + float alpha = smoothstep(-aaWidth, aaWidth, signedDist); + + if (alpha > 0.0) { + vec4 color; + if (useGradient) { + color = getGradientColor(fragTexCoord); + } else { + color = fillColor; + } + finalColor = vec4(color.rgb, color.a * alpha); + } else { + finalColor = vec4(0.0, 0.0, 0.0, 0.0); + } +} +""" + +# Default vertex shader +VERTEX_SHADER = """ +#version 300 es +in vec3 vertexPosition; +in vec2 vertexTexCoord; +out vec2 fragTexCoord; +uniform mat4 mvp; + +void main() { + fragTexCoord = vertexTexCoord; + gl_Position = mvp * vec4(vertexPosition, 1.0); +} +""" + + +class ShaderState: + _instance: Any = None + + @classmethod + def get_instance(cls): + if cls._instance is None: + cls._instance = cls() + return cls._instance + + def __init__(self): + if ShaderState._instance is not None: + raise Exception("This class is a singleton. Use get_instance() instead.") + + self.initialized = False + self.shader = None + self.white_texture = None + + # Shader uniform locations + self.locations = { + 'pointCount': None, + 'fillColor': None, + 'resolution': None, + 'points': None, + 'useGradient': None, + 'gradientStart': None, + 'gradientEnd': None, + 'gradientColors': None, + 'gradientStops': None, + 'gradientColorCount': None, + 'mvp': None, + } + + def initialize(self): + if self.initialized: + return + + vertex_shader = rl.load_shader_from_memory(VERTEX_SHADER, FRAGMENT_SHADER) + self.shader = vertex_shader + + # Create and cache white texture + white_img = rl.gen_image_color(2, 2, rl.WHITE) + self.white_texture = rl.load_texture_from_image(white_img) + rl.set_texture_filter(self.white_texture, rl.TEXTURE_FILTER_BILINEAR) + rl.unload_image(white_img) + + # Cache all uniform locations + for uniform in self.locations.keys(): + self.locations[uniform] = rl.get_shader_location(self.shader, uniform) + + # Setup default MVP matrix + mvp_ptr = rl.ffi.new("float[16]", [1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0]) + rl.set_shader_value_matrix(self.shader, self.locations['mvp'], rl.Matrix(*mvp_ptr)) + + self.initialized = True + + def cleanup(self): + if not self.initialized: + return + + if self.white_texture: + rl.unload_texture(self.white_texture) + self.white_texture = None + + if self.shader: + rl.unload_shader(self.shader) + self.shader = None + + self.initialized = False + + +def draw_polygon(points: np.ndarray, color=None, gradient=None): + """ + Draw a complex polygon using shader-based even-odd fill rule + + Args: + points: List of (x,y) points defining the polygon + color: Solid fill color (rl.Color) + gradient: Dict with gradient parameters: + { + 'start': (x1, y1), # Start point (normalized 0-1) + 'end': (x2, y2), # End point (normalized 0-1) + 'colors': [rl.Color], # List of colors at stops + 'stops': [float] # List of positions (0-1) + } + """ + if len(points) < 3: + return + + # Get shader state singleton + state = ShaderState.get_instance() + + # Initialize shader if not already done + if not state.initialized: + state.initialize() + + # Find bounding box + min_xy = np.min(points, axis=0) + min_x, min_y = min_xy + max_x, max_y = np.max(points, axis=0) + + width = max(1, max_x - min_x) + height = max(1, max_y - min_y) + + # Transform points to shader space + transformed_points = points - min_xy + + # Set basic shader uniforms using cached locations + point_count_ptr = rl.ffi.new("int[]", [len(transformed_points)]) + rl.set_shader_value(state.shader, state.locations['pointCount'], point_count_ptr, rl.ShaderUniformDataType.SHADER_UNIFORM_INT) + + resolution_ptr = rl.ffi.new("float[]", [width, height]) + rl.set_shader_value(state.shader, state.locations['resolution'], resolution_ptr, rl.ShaderUniformDataType.SHADER_UNIFORM_VEC2) + + # Set points + flat_points = np.ascontiguousarray(transformed_points.flatten().astype(np.float32)) + points_ptr = rl.ffi.cast("float *", flat_points.ctypes.data) + rl.set_shader_value_v( + state.shader, state.locations['points'], points_ptr, rl.ShaderUniformDataType.SHADER_UNIFORM_VEC2, len(transformed_points) + ) + + # Set gradient or solid color based on what was provided + if gradient: + # Enable gradient + use_gradient_ptr = rl.ffi.new("int[]", [1]) + rl.set_shader_value(state.shader, state.locations['useGradient'], use_gradient_ptr, rl.ShaderUniformDataType.SHADER_UNIFORM_INT) + + # Set gradient start/end + start_ptr = rl.ffi.new("float[]", [gradient['start'][0], gradient['start'][1]]) + end_ptr = rl.ffi.new("float[]", [gradient['end'][0], gradient['end'][1]]) + rl.set_shader_value(state.shader, state.locations['gradientStart'], start_ptr, rl.ShaderUniformDataType.SHADER_UNIFORM_VEC2) + rl.set_shader_value(state.shader, state.locations['gradientEnd'], end_ptr, rl.ShaderUniformDataType.SHADER_UNIFORM_VEC2) + + # Set gradient colors + colors = gradient['colors'] + color_count = min(len(colors), 8) # Max 8 colors + colors_ptr = rl.ffi.new("float[]", color_count * 4) + for i, c in enumerate(colors[:color_count]): + colors_ptr[i * 4] = c.r / 255.0 + colors_ptr[i * 4 + 1] = c.g / 255.0 + colors_ptr[i * 4 + 2] = c.b / 255.0 + colors_ptr[i * 4 + 3] = c.a / 255.0 + rl.set_shader_value_v( + state.shader, state.locations['gradientColors'], colors_ptr, rl.ShaderUniformDataType.SHADER_UNIFORM_VEC4, color_count + ) + + # Set gradient stops + stops = gradient.get('stops', [i / (color_count - 1) for i in range(color_count)]) + stops_ptr = rl.ffi.new("float[]", color_count) + for i, s in enumerate(stops[:color_count]): + stops_ptr[i] = s + rl.set_shader_value_v( + state.shader, state.locations['gradientStops'], stops_ptr, rl.ShaderUniformDataType.SHADER_UNIFORM_FLOAT, color_count + ) + + # Set color count + color_count_ptr = rl.ffi.new("int[]", [color_count]) + rl.set_shader_value(state.shader, state.locations['gradientColorCount'], color_count_ptr, rl.ShaderUniformDataType.SHADER_UNIFORM_INT) + else: + # Disable gradient + use_gradient_ptr = rl.ffi.new("int[]", [0]) + rl.set_shader_value(state.shader, state.locations['useGradient'], use_gradient_ptr, rl.ShaderUniformDataType.SHADER_UNIFORM_INT) + + # Set solid color + if color is None: + color = rl.WHITE + fill_color_ptr = rl.ffi.new("float[]", [color.r / 255.0, color.g / 255.0, color.b / 255.0, color.a / 255.0]) + rl.set_shader_value(state.shader, state.locations['fillColor'], fill_color_ptr, rl.ShaderUniformDataType.SHADER_UNIFORM_VEC4) + + # Draw with shader + rl.begin_shader_mode(state.shader) + rl.draw_texture_pro( + state.white_texture, + rl.Rectangle(0, 0, 2, 2), + rl.Rectangle(int(min_x), int(min_y), int(width), int(height)), + rl.Vector2(0, 0), + 0.0, + rl.WHITE, + ) + rl.end_shader_mode() + + +def cleanup_shader_resources(): + state = ShaderState.get_instance() + state.cleanup() diff --git a/system/ui/onroad/model_renderer.py b/system/ui/onroad/model_renderer.py index d0b66666fa..9060dd3f74 100644 --- a/system/ui/onroad/model_renderer.py +++ b/system/ui/onroad/model_renderer.py @@ -4,6 +4,7 @@ import numpy as np import pyray as rl from cereal import messaging, car from openpilot.common.params import Params +from openpilot.system.ui.lib.shader_polygon import draw_polygon CLIP_MARGIN = 500 @@ -30,14 +31,14 @@ class ModelRenderer: self._experimental_mode = False self._blend_factor = 1.0 self._prev_allow_throttle = True - self._lane_line_probs = [0.0] * 4 - self._road_edge_stds = [0.0] * 2 + self._lane_line_probs = np.zeros(4, dtype=np.float32) + self._road_edge_stds = np.zeros(2, dtype=np.float32) self._path_offset_z = 1.22 # Initialize empty polygon vertices - self._track_vertices = [] - self._lane_line_vertices = [[] for _ in range(4)] - self._road_edge_vertices = [[] for _ in range(2)] + self._track_vertices = np.empty((0, 2), dtype=np.float32) + self._lane_line_vertices = [np.empty((0, 2), dtype=np.float32) for _ in range(4)] + self._road_edge_vertices = [np.empty((0, 2), dtype=np.float32) for _ in range(2)] self._lead_vertices = [None, None] # Transform matrix (3x3 for car space to screen space) @@ -145,29 +146,29 @@ class ModelRenderer: def _draw_lane_lines(self): """Draw lane lines and road edges""" - for i in range(4): + for i, vertices in enumerate(self._lane_line_vertices): # Skip if no vertices - if not self._lane_line_vertices[i]: + if vertices.size == 0: continue # Draw lane line alpha = np.clip(self._lane_line_probs[i], 0.0, 0.7) color = rl.Color(255, 255, 255, int(alpha * 255)) - self._draw_polygon(self._lane_line_vertices[i], color) + draw_polygon(vertices, color) - for i in range(2): + for i, vertices in enumerate(self._road_edge_vertices): # Skip if no vertices - if not self._road_edge_vertices[i]: + if vertices.size == 0: continue # Draw road edge alpha = np.clip(1.0 - self._road_edge_stds[i], 0.0, 1.0) color = rl.Color(255, 0, 0, int(alpha * 255)) - self._draw_polygon(self._road_edge_vertices[i], color) + draw_polygon(vertices, color) def _draw_path(self, sm, model, height): """Draw the path polygon with gradient based on acceleration""" - if not self._track_vertices: + if self._track_vertices.size == 0: return if self._experimental_mode: @@ -175,16 +176,29 @@ class ModelRenderer: acceleration = model.acceleration.x max_len = min(len(self._track_vertices) // 2, len(acceleration)) - # Create gradient colors for path sections - for i in range(max_len): + # Find midpoint index for polygon + mid_point = len(self._track_vertices) // 2 + + # For acceleration-based coloring, process segments separately + left_side = self._track_vertices[:mid_point] + right_side = self._track_vertices[mid_point:][::-1] # Reverse for proper winding + + # Create segments for gradient coloring + segment_colors = [] + gradient_stops = [] + + for i in range(max_len - 1): + if i >= len(left_side) - 1 or i >= len(right_side) - 1: + break + track_idx = max_len - i - 1 # flip idx to start from bottom right - track_y = self._track_vertices[track_idx][1] + # Skip points out of frame - if track_y < 0 or track_y > height: + if left_side[track_idx][1] < 0 or left_side[track_idx][1] > height: continue # Calculate color based on acceleration - lin_grad_point = (height - track_y) / height + lin_grad_point = (height - left_side[track_idx][1]) / height # speed up: 120, slow down: 0 path_hue = max(min(60 + acceleration[i] * 35, 120), 0) @@ -197,12 +211,22 @@ class ModelRenderer: # Use HSL to RGB conversion color = self._hsla_to_color(path_hue / 360.0, saturation, lightness, alpha) - # TODO: This is simplified - a full implementation would create a gradient fill - segment = self._track_vertices[track_idx : track_idx + 2] + self._track_vertices[-track_idx - 2 : -track_idx] - self._draw_polygon(segment, color) + # Create quad segment + gradient_stops.append(lin_grad_point) + segment_colors.append(color) - # Skip a point, unless next is last - i += 1 if i + 2 < max_len else 0 + if len(segment_colors) < 2: + draw_polygon(self._track_vertices, rl.Color(255, 255, 255, 30)) + return + + # Create gradient specification + gradient = { + 'start': (0.0, 1.0), # Bottom of path + 'end': (0.0, 0.0), # Top of path + 'colors': segment_colors, + 'stops': gradient_stops, + } + draw_polygon(self._track_vertices, gradient=gradient) else: # Draw with throttle/no throttle gradient allow_throttle = sm['longitudinalPlan'].allowThrottle or not self._longitudinal_control @@ -226,7 +250,13 @@ class ModelRenderer: self._blend_colors(begin_colors[2], end_colors[2], self._blend_factor), ] - self._draw_polygon(self._track_vertices, colors[0]) + gradient = { + 'start': (0.0, 1.0), # Bottom of path + 'end': (0.0, 0.0), # Top of path + 'colors': colors, + 'stops': [0.0, 1.0], + } + draw_polygon(self._track_vertices, gradient=gradient) def _draw_lead(self, lead_data, vd, rect): """Draw lead vehicle indicator""" @@ -284,14 +314,14 @@ class ModelRenderer: return (x, y) - def _map_line_to_polygon(self, line, y_off, z_off, max_idx, allow_invert=True): + def _map_line_to_polygon(self, line, y_off, z_off, max_idx, allow_invert=True)-> np.ndarray: """Convert a 3D line to a 2D polygon for drawing""" line_x = line.x line_y = line.y line_z = line.z - left_points = [] - right_points = [] + left_points: list[tuple[float, float]] = [] + right_points: list[tuple[float, float]] = [] for i in range(max_idx + 1): # Skip points with negative x (behind camera) @@ -309,23 +339,10 @@ class ModelRenderer: left_points.append(left) right_points.append(right) - if not left_points: - return [] + if not left_points or not right_points: + return np.empty((0, 2), dtype=np.float32) - return left_points + right_points[::-1] - - def _draw_polygon(self, points, color): - # TODO: Enhance polygon drawing to support even-odd fill rule efficiently, as Raylib lacks native support. - # Use a faster triangulation algorithm (e.g., ear clipping) or GPU shader for - # efficient rendering of lane lines, road edges, and path polygons. - if len(points) <= 8: - rl.draw_triangle_fan(points, len(points), color) - else: - for i in range(1, len(points) - 1): - rl.draw_triangle(points[0], points[i], points[i + 1], color) - - for i in range(len(points)): - rl.draw_line_ex(points[i], points[(i + 1) % len(points)], 1.5, color) + return np.array(left_points + right_points[::-1], dtype=np.float32) @staticmethod def _map_val(x, x0, x1, y0, y1): From 9460ff8f305045d7cf0aa8b40718b47824e633da Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Thu, 29 May 2025 00:26:38 +0800 Subject: [PATCH 22/60] system/ui: fix gradient colors and path stops in path rendering (#35367) Correct gradient colors and direction in path rendering --- system/ui/onroad/model_renderer.py | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/system/ui/onroad/model_renderer.py b/system/ui/onroad/model_renderer.py index 9060dd3f74..649360a7e3 100644 --- a/system/ui/onroad/model_renderer.py +++ b/system/ui/onroad/model_renderer.py @@ -13,15 +13,15 @@ MAX_DRAW_DISTANCE = 100.0 THROTTLE_COLORS = [ - rl.Color(0, 231, 130, 102), # Green with alpha 0.4 - rl.Color(112, 247, 35, 89), # Lime with alpha 0.35 - rl.Color(112, 247, 35, 0), # Transparent lime + rl.Color(25, 235, 99, 102), # HSLF(148/360, 0.94, 0.51, 0.4) + rl.Color(92, 255, 32, 89), # HSLF(112/360, 1.0, 0.68, 0.35) + rl.Color(92, 255, 32, 0), # HSLF(112/360, 1.0, 0.68, 0.0) ] NO_THROTTLE_COLORS = [ - rl.Color(242, 242, 242, 102), # Light gray with alpha 0.4 - rl.Color(242, 242, 242, 89), # Light gray with alpha 0.35 - rl.Color(242, 242, 242, 0), # Transparent light gray + rl.Color(242, 242, 242, 102), # HSLF(148/360, 0.0, 0.95, 0.4) + rl.Color(242, 242, 242, 89), # HSLF(112/360, 0.0, 0.95, 0.35) + rl.Color(242, 242, 242, 0), # HSLF(112/360, 0.0, 0.95, 0.0) ] @@ -254,7 +254,7 @@ class ModelRenderer: 'start': (0.0, 1.0), # Bottom of path 'end': (0.0, 0.0), # Top of path 'colors': colors, - 'stops': [0.0, 1.0], + 'stops': [0.0, 0.5, 1.0], } draw_polygon(self._track_vertices, gradient=gradient) @@ -364,14 +364,14 @@ class ModelRenderer: return rl.Color(r_val, g_val, b_val, a_val) @staticmethod - def _blend_colors(start, end, t): + def _blend_colors(start: rl.Color, end: rl.Color, t: float) -> rl.Color: """Blend between two colors with factor t""" if t >= 1.0: return end return rl.Color( - int((1 - t) * start.r + t * end.r), - int((1 - t) * start.g + t * end.g), - int((1 - t) * start.b + t * end.b), - int((1 - t) * start.a + t * end.a), + int((1.0 - t) * start.r + t * end.r), + int((1.0 - t) * start.g + t * end.g), + int((1.0 - t) * start.b + t * end.b), + int((1.0 - t) * start.a + t * end.a), ) From 3682fac7b6f43b1417236d4e7d69035feb6e8e31 Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Thu, 29 May 2025 00:29:33 +0800 Subject: [PATCH 23/60] system/ui: optimizes the draw_polygon() and improving code maintainability (#35366) improve draw_polygon --- system/ui/lib/shader_polygon.py | 132 ++++++++++++++++---------------- 1 file changed, 67 insertions(+), 65 deletions(-) diff --git a/system/ui/lib/shader_polygon.py b/system/ui/lib/shader_polygon.py index dfbfa969d5..23f54efcd7 100644 --- a/system/ui/lib/shader_polygon.py +++ b/system/ui/lib/shader_polygon.py @@ -152,8 +152,14 @@ void main() { """ +UNIFORM_INT = rl.ShaderUniformDataType.SHADER_UNIFORM_INT +UNIFORM_FLOAT = rl.ShaderUniformDataType.SHADER_UNIFORM_FLOAT +UNIFORM_VEC2 = rl.ShaderUniformDataType.SHADER_UNIFORM_VEC2 +UNIFORM_VEC4 = rl.ShaderUniformDataType.SHADER_UNIFORM_VEC4 + + class ShaderState: - _instance: Any = None + _instance: Any = None @classmethod def get_instance(cls): @@ -184,6 +190,19 @@ class ShaderState: 'mvp': None, } + # Pre-allocated FFI objects + self.point_count_ptr = rl.ffi.new("int[]", [0]) + self.resolution_ptr = rl.ffi.new("float[]", [0.0, 0.0]) + self.fill_color_ptr = rl.ffi.new("float[]", [0.0, 0.0, 0.0, 0.0]) + self.use_gradient_ptr = rl.ffi.new("int[]", [0]) + self.gradient_start_ptr = rl.ffi.new("float[]", [0.0, 0.0]) + self.gradient_end_ptr = rl.ffi.new("float[]", [0.0, 0.0]) + self.color_count_ptr = rl.ffi.new("int[]", [0]) + + # Pre-allocate gradient arrays (max 8 colors) + self.gradient_colors_ptr = rl.ffi.new("float[]", 32) # 8 colors * 4 components + self.gradient_stops_ptr = rl.ffi.new("float[]", 8) + def initialize(self): if self.initialized: return @@ -222,12 +241,46 @@ class ShaderState: self.initialized = False +def _configure_shader_color(state, color, gradient): + """Configure shader uniforms for solid color or gradient rendering""" + state.use_gradient_ptr[0] = 1 if gradient else 0 + rl.set_shader_value(state.shader, state.locations['useGradient'], state.use_gradient_ptr, UNIFORM_INT) + + if gradient: + # Set gradient start/end + state.gradient_start_ptr[0:2] = gradient['start'] + state.gradient_end_ptr[0:2] = gradient['end'] + rl.set_shader_value(state.shader, state.locations['gradientStart'], state.gradient_start_ptr, UNIFORM_VEC2) + rl.set_shader_value(state.shader, state.locations['gradientEnd'], state.gradient_end_ptr, UNIFORM_VEC2) + + # Set gradient colors + colors = gradient['colors'] + color_count = min(len(colors), 8) # Max 8 colors + for i, c in enumerate(colors[:color_count]): + base_idx = i * 4 + state.gradient_colors_ptr[base_idx:base_idx+4] = [c.r / 255.0, c.g / 255.0, c.b / 255.0, c.a / 255.0] + rl.set_shader_value_v(state.shader, state.locations['gradientColors'], state.gradient_colors_ptr, UNIFORM_VEC4, color_count) + + # Set gradient stops + stops = gradient.get('stops', [i / (color_count - 1) for i in range(color_count)]) + state.gradient_stops_ptr[0:color_count] = stops[:color_count] + rl.set_shader_value_v(state.shader, state.locations['gradientStops'], state.gradient_stops_ptr, UNIFORM_FLOAT, color_count) + + # Set color count + state.color_count_ptr[0] = color_count + rl.set_shader_value(state.shader, state.locations['gradientColorCount'], state.color_count_ptr, UNIFORM_INT) + else: + color = color or rl.WHITE # Default to white if no color provided + state.fill_color_ptr[0:4] = [color.r / 255.0, color.g / 255.0, color.b / 255.0, color.a / 255.0] + rl.set_shader_value(state.shader, state.locations['fillColor'], state.fill_color_ptr, UNIFORM_VEC4) + + def draw_polygon(points: np.ndarray, color=None, gradient=None): """ Draw a complex polygon using shader-based even-odd fill rule Args: - points: List of (x,y) points defining the polygon + points: numpy array of (x,y) points defining the polygon color: Solid fill color (rl.Color) gradient: Dict with gradient parameters: { @@ -240,92 +293,41 @@ def draw_polygon(points: np.ndarray, color=None, gradient=None): if len(points) < 3: return - # Get shader state singleton state = ShaderState.get_instance() - - # Initialize shader if not already done if not state.initialized: state.initialize() # Find bounding box min_xy = np.min(points, axis=0) - min_x, min_y = min_xy - max_x, max_y = np.max(points, axis=0) + max_xy = np.max(points, axis=0) - width = max(1, max_x - min_x) - height = max(1, max_y - min_y) + width = max(1, max_xy[0] - min_xy[0]) + height = max(1, max_xy[1] - min_xy[1]) # Transform points to shader space transformed_points = points - min_xy - # Set basic shader uniforms using cached locations - point_count_ptr = rl.ffi.new("int[]", [len(transformed_points)]) - rl.set_shader_value(state.shader, state.locations['pointCount'], point_count_ptr, rl.ShaderUniformDataType.SHADER_UNIFORM_INT) + state.point_count_ptr[0] = len(transformed_points) + rl.set_shader_value(state.shader, state.locations['pointCount'], state.point_count_ptr, UNIFORM_INT) + + state.resolution_ptr[0:2] = [width, height] + rl.set_shader_value(state.shader, state.locations['resolution'], state.resolution_ptr, UNIFORM_VEC2) - resolution_ptr = rl.ffi.new("float[]", [width, height]) - rl.set_shader_value(state.shader, state.locations['resolution'], resolution_ptr, rl.ShaderUniformDataType.SHADER_UNIFORM_VEC2) # Set points flat_points = np.ascontiguousarray(transformed_points.flatten().astype(np.float32)) points_ptr = rl.ffi.cast("float *", flat_points.ctypes.data) - rl.set_shader_value_v( - state.shader, state.locations['points'], points_ptr, rl.ShaderUniformDataType.SHADER_UNIFORM_VEC2, len(transformed_points) - ) + rl.set_shader_value_v(state.shader, state.locations['points'], points_ptr, UNIFORM_VEC2, len(transformed_points)) - # Set gradient or solid color based on what was provided - if gradient: - # Enable gradient - use_gradient_ptr = rl.ffi.new("int[]", [1]) - rl.set_shader_value(state.shader, state.locations['useGradient'], use_gradient_ptr, rl.ShaderUniformDataType.SHADER_UNIFORM_INT) - - # Set gradient start/end - start_ptr = rl.ffi.new("float[]", [gradient['start'][0], gradient['start'][1]]) - end_ptr = rl.ffi.new("float[]", [gradient['end'][0], gradient['end'][1]]) - rl.set_shader_value(state.shader, state.locations['gradientStart'], start_ptr, rl.ShaderUniformDataType.SHADER_UNIFORM_VEC2) - rl.set_shader_value(state.shader, state.locations['gradientEnd'], end_ptr, rl.ShaderUniformDataType.SHADER_UNIFORM_VEC2) - - # Set gradient colors - colors = gradient['colors'] - color_count = min(len(colors), 8) # Max 8 colors - colors_ptr = rl.ffi.new("float[]", color_count * 4) - for i, c in enumerate(colors[:color_count]): - colors_ptr[i * 4] = c.r / 255.0 - colors_ptr[i * 4 + 1] = c.g / 255.0 - colors_ptr[i * 4 + 2] = c.b / 255.0 - colors_ptr[i * 4 + 3] = c.a / 255.0 - rl.set_shader_value_v( - state.shader, state.locations['gradientColors'], colors_ptr, rl.ShaderUniformDataType.SHADER_UNIFORM_VEC4, color_count - ) - - # Set gradient stops - stops = gradient.get('stops', [i / (color_count - 1) for i in range(color_count)]) - stops_ptr = rl.ffi.new("float[]", color_count) - for i, s in enumerate(stops[:color_count]): - stops_ptr[i] = s - rl.set_shader_value_v( - state.shader, state.locations['gradientStops'], stops_ptr, rl.ShaderUniformDataType.SHADER_UNIFORM_FLOAT, color_count - ) - - # Set color count - color_count_ptr = rl.ffi.new("int[]", [color_count]) - rl.set_shader_value(state.shader, state.locations['gradientColorCount'], color_count_ptr, rl.ShaderUniformDataType.SHADER_UNIFORM_INT) - else: - # Disable gradient - use_gradient_ptr = rl.ffi.new("int[]", [0]) - rl.set_shader_value(state.shader, state.locations['useGradient'], use_gradient_ptr, rl.ShaderUniformDataType.SHADER_UNIFORM_INT) - - # Set solid color - if color is None: - color = rl.WHITE - fill_color_ptr = rl.ffi.new("float[]", [color.r / 255.0, color.g / 255.0, color.b / 255.0, color.a / 255.0]) - rl.set_shader_value(state.shader, state.locations['fillColor'], fill_color_ptr, rl.ShaderUniformDataType.SHADER_UNIFORM_VEC4) + # Configure color/gradient uniforms + _configure_shader_color(state, color, gradient) # Draw with shader rl.begin_shader_mode(state.shader) rl.draw_texture_pro( state.white_texture, rl.Rectangle(0, 0, 2, 2), - rl.Rectangle(int(min_x), int(min_y), int(width), int(height)), + rl.Rectangle(int(min_xy[0]), int(min_xy[1]), int(width), int(height)), rl.Vector2(0, 0), 0.0, rl.WHITE, From db8ecf183f7e8d78807cfcfa6aeac8eed3a3d5e0 Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Thu, 29 May 2025 02:35:04 +0800 Subject: [PATCH 24/60] system/ui: fix rapid path color transition by correcting hardcoded increment (#35368) refactor color blending with configurable transition duration --- system/ui/onroad/model_renderer.py | 35 +++++++++++++++--------------- 1 file changed, 17 insertions(+), 18 deletions(-) diff --git a/system/ui/onroad/model_renderer.py b/system/ui/onroad/model_renderer.py index 649360a7e3..8a87f2f388 100644 --- a/system/ui/onroad/model_renderer.py +++ b/system/ui/onroad/model_renderer.py @@ -4,13 +4,15 @@ import numpy as np import pyray as rl from cereal import messaging, car from openpilot.common.params import Params +from openpilot.system.ui.lib.application import DEFAULT_FPS from openpilot.system.ui.lib.shader_polygon import draw_polygon CLIP_MARGIN = 500 MIN_DRAW_DISTANCE = 10.0 MAX_DRAW_DISTANCE = 100.0 - +PATH_COLOR_TRANSITION_DURATION = 0.5 # Seconds for color transition animation +PATH_BLEND_INCREMENT = 1.0 / (PATH_COLOR_TRANSITION_DURATION * DEFAULT_FPS) THROTTLE_COLORS = [ rl.Color(25, 235, 99, 102), # HSLF(148/360, 0.94, 0.51, 0.4) @@ -238,22 +240,17 @@ class ModelRenderer: # Update blend factor if self._blend_factor < 1.0: - self._blend_factor = min(self._blend_factor + 0.1, 1.0) + self._blend_factor = min(self._blend_factor + PATH_BLEND_INCREMENT, 1.0) begin_colors = NO_THROTTLE_COLORS if allow_throttle else THROTTLE_COLORS end_colors = THROTTLE_COLORS if allow_throttle else NO_THROTTLE_COLORS # Blend colors based on transition - colors = [ - self._blend_colors(begin_colors[0], end_colors[0], self._blend_factor), - self._blend_colors(begin_colors[1], end_colors[1], self._blend_factor), - self._blend_colors(begin_colors[2], end_colors[2], self._blend_factor), - ] - + blended_colors = self._blend_colors(begin_colors, end_colors, self._blend_factor) gradient = { 'start': (0.0, 1.0), # Bottom of path 'end': (0.0, 0.0), # Top of path - 'colors': colors, + 'colors': blended_colors, 'stops': [0.0, 0.5, 1.0], } draw_polygon(self._track_vertices, gradient=gradient) @@ -364,14 +361,16 @@ class ModelRenderer: return rl.Color(r_val, g_val, b_val, a_val) @staticmethod - def _blend_colors(start: rl.Color, end: rl.Color, t: float) -> rl.Color: - """Blend between two colors with factor t""" + def _blend_colors(begin_colors, end_colors, t): if t >= 1.0: - return end + return end_colors + if t <= 0.0: + return begin_colors - return rl.Color( - int((1.0 - t) * start.r + t * end.r), - int((1.0 - t) * start.g + t * end.g), - int((1.0 - t) * start.b + t * end.b), - int((1.0 - t) * start.a + t * end.a), - ) + inv_t = 1.0 - t + return [rl.Color( + int(inv_t * start.r + t * end.r), + int(inv_t * start.g + t * end.g), + int(inv_t * start.b + t * end.b), + int(inv_t * start.a + t * end.a) + ) for start, end in zip(begin_colors, end_colors, strict=True)] From 26c61d8674800e71ef8f78227a13e6eadfe9a34f Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Thu, 29 May 2025 04:07:19 +0800 Subject: [PATCH 25/60] system/ui: fix incorrect THROTTLE_COLOR (#35370) fix throttle colors --- system/ui/onroad/model_renderer.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/system/ui/onroad/model_renderer.py b/system/ui/onroad/model_renderer.py index 8a87f2f388..ac3ac74430 100644 --- a/system/ui/onroad/model_renderer.py +++ b/system/ui/onroad/model_renderer.py @@ -15,9 +15,9 @@ PATH_COLOR_TRANSITION_DURATION = 0.5 # Seconds for color transition animation PATH_BLEND_INCREMENT = 1.0 / (PATH_COLOR_TRANSITION_DURATION * DEFAULT_FPS) THROTTLE_COLORS = [ - rl.Color(25, 235, 99, 102), # HSLF(148/360, 0.94, 0.51, 0.4) - rl.Color(92, 255, 32, 89), # HSLF(112/360, 1.0, 0.68, 0.35) - rl.Color(92, 255, 32, 0), # HSLF(112/360, 1.0, 0.68, 0.0) + rl.Color(13, 248, 122, 102), # HSLF(148/360, 0.94, 0.51, 0.4) + rl.Color(114, 255, 92, 89), # HSLF(112/360, 1.0, 0.68, 0.35) + rl.Color(114, 255, 92, 0), # HSLF(112/360, 1.0, 0.68, 0.0) ] NO_THROTTLE_COLORS = [ From 5bf81048bbddd472e4bc37187ef5f4ea53ad3431 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 28 May 2025 14:58:48 -0700 Subject: [PATCH 26/60] bump opendbc (#35372) bump --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index 3ae4dc13cd..ffe9db9b7c 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 3ae4dc13cd9e628672a01651896021214b7c3c17 +Subproject commit ffe9db9b7cffde1a21475d912b5145cd401d0fd4 From 1db3644a2b3f156c9f5790b6cfe0d6a46c4b6b54 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 28 May 2025 17:41:38 -0700 Subject: [PATCH 27/60] car interfaces: pass in if release build (#35373) * dashcam for release * get car * fixes * more fixes * bump * bump --- opendbc_repo | 2 +- selfdrive/car/card.py | 6 ++++-- selfdrive/car/tests/test_car_interfaces.py | 2 +- selfdrive/car/tests/test_models.py | 2 +- selfdrive/test/process_replay/process_replay.py | 2 +- 5 files changed, 8 insertions(+), 6 deletions(-) diff --git a/opendbc_repo b/opendbc_repo index ffe9db9b7c..d33a033a7f 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit ffe9db9b7cffde1a21475d912b5145cd401d0fd4 +Subproject commit d33a033a7fceaf660a1ae1e6e7d00fb4737f6745 diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 502daaa015..3339096307 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -80,6 +80,8 @@ class Car: self.can_callbacks = can_comm_callbacks(self.can_sock, self.pm.sock['sendcan']) + is_release = self.params.get_bool("IsReleaseBranch") + if CI is None: # wait for one pandaState and one CAN packet print("Waiting for CAN messages...") @@ -97,7 +99,7 @@ class Car: with car.CarParams.from_bytes(cached_params_raw) as _cached_params: cached_params = _cached_params - self.CI = get_car(*self.can_callbacks, obd_callback(self.params), alpha_long_allowed, num_pandas, cached_params) + self.CI = get_car(*self.can_callbacks, obd_callback(self.params), alpha_long_allowed, is_release, num_pandas, cached_params) self.RI = interfaces[self.CI.CP.carFingerprint].RadarInterface(self.CI.CP) self.CP = self.CI.CP @@ -116,7 +118,7 @@ class Car: safety_config.safetyModel = structs.CarParams.SafetyModel.noOutput self.CP.safetyConfigs = [safety_config] - if self.CP.secOcRequired and not self.params.get_bool("IsReleaseBranch"): + if self.CP.secOcRequired and not is_release: # Copy user key if available try: with open("/cache/params/SecOCKey") as f: diff --git a/selfdrive/car/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py index 92c7ff8f1c..5c4729ee9a 100644 --- a/selfdrive/car/tests/test_car_interfaces.py +++ b/selfdrive/car/tests/test_car_interfaces.py @@ -39,7 +39,7 @@ class TestCarInterfaces: args = get_fuzzy_car_interface_args(data.draw) car_params = CarInterface.get_params(car_name, args['fingerprints'], args['car_fw'], - alpha_long=args['alpha_long'], docs=False) + alpha_long=args['alpha_long'], is_release=False, docs=False) car_params = car_params.as_reader() car_interface = CarInterface(car_params) assert car_params diff --git a/selfdrive/car/tests/test_models.py b/selfdrive/car/tests/test_models.py index 0a4784aca8..b918f4c29f 100644 --- a/selfdrive/car/tests/test_models.py +++ b/selfdrive/car/tests/test_models.py @@ -152,7 +152,7 @@ class TestCarModelBase(unittest.TestCase): cls.openpilot_enabled = cls.car_safety_mode_frame is not None cls.CarInterface = interfaces[cls.platform] - cls.CP = cls.CarInterface.get_params(cls.platform, cls.fingerprint, car_fw, alpha_long, docs=False) + cls.CP = cls.CarInterface.get_params(cls.platform, cls.fingerprint, car_fw, alpha_long, False, docs=False) assert cls.CP assert cls.CP.carFingerprint == cls.platform diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 8de2e60055..4f8aa9d99d 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -362,7 +362,7 @@ def get_car_params_callback(rc, pm, msgs, fingerprint): with car.CarParams.from_bytes(cached_params_raw) as _cached_params: cached_params = _cached_params - CP = get_car(*can_callbacks, lambda obd: None, Params().get_bool("AlphaLongitudinalEnabled"), cached_params=cached_params).CP + CP = get_car(*can_callbacks, lambda obd: None, Params().get_bool("AlphaLongitudinalEnabled"), False, cached_params=cached_params).CP params.put("CarParams", CP.to_bytes()) From fe713b867f5866b64b5e3484495081c97da864c4 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Wed, 28 May 2025 19:02:00 -0700 Subject: [PATCH 28/60] CI: fix nightly build (#35374) * CI: fix nightly build * schedule * simplify * cleanup --- .github/workflows/release.yaml | 18 +++--------------- 1 file changed, 3 insertions(+), 15 deletions(-) diff --git a/.github/workflows/release.yaml b/.github/workflows/release.yaml index 1dd06f77e9..4065b0ef3e 100644 --- a/.github/workflows/release.yaml +++ b/.github/workflows/release.yaml @@ -8,8 +8,7 @@ jobs: build_masterci: name: build master-ci env: - TARGET_DIR: /tmp/openpilot - ImageOS: ubuntu20 + ImageOS: ubuntu24 container: image: ghcr.io/commaai/openpilot-base:latest runs-on: ubuntu-latest @@ -23,7 +22,7 @@ jobs: sudo apt-get update sudo apt-get install -y libyaml-dev - name: Wait for green check mark - if: ${{ github.event_name != 'workflow_dispatch' }} + if: ${{ github.event_name == 'schedule' }} uses: lewagon/wait-on-check-action@ccfb013c15c8afb7bf2b7c028fb74dc5a068cccc with: ref: master @@ -39,16 +38,5 @@ jobs: run: | git config --global --add safe.directory '*' git lfs pull - - name: Build master-ci - run: | - release/build_devel.sh - - name: Run tests - run: | - export PYTHONPATH=$TARGET_DIR - cd $TARGET_DIR - scons -j$(nproc) - pytest -n logical selfdrive/car/tests/test_car_interfaces.py - name: Push master-ci - run: | - unset TARGET_DIR - BRANCH=__nightly release/build_devel.sh + run: BRANCH=__nightly release/build_devel.sh From deebbd3e7767d314d1f66faae63e333c0a11f749 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 28 May 2025 19:15:50 -0700 Subject: [PATCH 29/60] add missing Fords to release notes --- RELEASES.md | 1 + 1 file changed, 1 insertion(+) diff --git a/RELEASES.md b/RELEASES.md index acdb709c9b..ab21484a68 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -3,6 +3,7 @@ Version 0.9.9 (2025-05-23) * New driving model * New training architecture using parts from MLSIM * Steering actuation delay is now learned online +* Ford Escape 2023-24 and Ford Kuga 2024 support thanks to incognitojam! * Hyundai Nexo 2021 support thanks to sunnyhaibin! * Tesla Model 3 and Y support thanks to lukasloetkolben! * Lexus RC 2023 support thanks to nelsonjchen! From 51c437c61ed836a5b2321e28895dae0af52d1b5f Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 28 May 2025 19:18:03 -0700 Subject: [PATCH 30/60] split --- RELEASES.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/RELEASES.md b/RELEASES.md index ab21484a68..5d74689a2d 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -3,7 +3,8 @@ Version 0.9.9 (2025-05-23) * New driving model * New training architecture using parts from MLSIM * Steering actuation delay is now learned online -* Ford Escape 2023-24 and Ford Kuga 2024 support thanks to incognitojam! +* Ford Escape 2023-24 support thanks to incognitojam! +* Ford Kuga 2024 support thanks to incognitojam! * Hyundai Nexo 2021 support thanks to sunnyhaibin! * Tesla Model 3 and Y support thanks to lukasloetkolben! * Lexus RC 2023 support thanks to nelsonjchen! From 618645f1d7eaec635514e712bac4582691b38862 Mon Sep 17 00:00:00 2001 From: YassineYousfi Date: Wed, 28 May 2025 20:45:53 -0700 Subject: [PATCH 31/60] Revert "Vegan Filet o Fish model " (#35376) Revert "Vegan Filet o Fish model (#35240)" This reverts commit b4d6b52edd815364b9d3503aff6dd3ce1005fe45. --- selfdrive/modeld/models/driving_policy.onnx | 4 ++-- selfdrive/modeld/models/driving_vision.onnx | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/selfdrive/modeld/models/driving_policy.onnx b/selfdrive/modeld/models/driving_policy.onnx index 5da6d85300..3601bbb5da 100644 --- a/selfdrive/modeld/models/driving_policy.onnx +++ b/selfdrive/modeld/models/driving_policy.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:abbb1f5a74a6d047ed4b7f376b19451a9443986660f136afd0f8d76fc254a579 -size 15976894 +oid sha256:98f0121ccb6f850077b04cc91bd33d370fc6cbdc2bd35f1ab55628a15a813f36 +size 15966721 diff --git a/selfdrive/modeld/models/driving_vision.onnx b/selfdrive/modeld/models/driving_vision.onnx index 6c8cf592b0..aee6d8f1bf 100644 --- a/selfdrive/modeld/models/driving_vision.onnx +++ b/selfdrive/modeld/models/driving_vision.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:6dfffac033d7ae8e68aa5763bd9b5dd99ddc748f6a95523c84fc2523eefdec55 +oid sha256:897f80d0388250f99bba69b6a8434560cc0fd83157cbeb0bc134c67fe4e64624 size 34882971 From ad0e556236d1886955f4a5523ee296d6d8704fa8 Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Fri, 30 May 2025 02:32:08 +0800 Subject: [PATCH 32/60] system/ui: fix gradient rendering to match Qt linear gradients (#35378) Fix shader_polygon gradient rendering to match Qt linear gradients --- system/ui/lib/shader_polygon.py | 147 +++++++++++++++++------------ system/ui/onroad/model_renderer.py | 12 ++- 2 files changed, 94 insertions(+), 65 deletions(-) diff --git a/system/ui/lib/shader_polygon.py b/system/ui/lib/shader_polygon.py index 23f54efcd7..9ecaa926bc 100644 --- a/system/ui/lib/shader_polygon.py +++ b/system/ui/lib/shader_polygon.py @@ -2,6 +2,7 @@ import pyray as rl import numpy as np from typing import Any +MAX_GRADIENT_COLORS = 15 FRAGMENT_SHADER = """ #version 300 es @@ -18,21 +19,33 @@ uniform vec2 resolution; uniform bool useGradient; uniform vec2 gradientStart; uniform vec2 gradientEnd; -uniform vec4 gradientColors[8]; -uniform float gradientStops[8]; +uniform vec4 gradientColors[15]; +uniform float gradientStops[15]; uniform int gradientColorCount; +uniform vec2 visibleGradientRange; vec4 getGradientColor(vec2 pos) { vec2 gradientDir = gradientEnd - gradientStart; float gradientLength = length(gradientDir); - if (gradientLength < 0.001) return gradientColors[0]; vec2 normalizedDir = gradientDir / gradientLength; vec2 pointVec = pos - gradientStart; float projection = dot(pointVec, normalizedDir); - float t = clamp(projection / gradientLength, 0.0, 1.0); + float t = projection / gradientLength; + + // Gradient clipping: remap t to visible range + float visibleStart = visibleGradientRange.x; + float visibleEnd = visibleGradientRange.y; + float visibleRange = visibleEnd - visibleStart; + + // Remap t to visible range + if (visibleRange > 0.001) { + t = visibleStart + t * visibleRange; + } + + t = clamp(t, 0.0, 1.0); for (int i = 0; i < gradientColorCount - 1; i++) { if (t >= gradientStops[i] && t <= gradientStops[i+1]) { float segmentT = (t - gradientStops[i]) / (gradientStops[i+1] - gradientStops[i]); @@ -46,36 +59,21 @@ vec4 getGradientColor(vec2 pos) { bool isPointInsidePolygon(vec2 p) { if (pointCount < 3) return false; - if (pointCount == 3) { - vec2 v0 = points[0]; - vec2 v1 = points[1]; - vec2 v2 = points[2]; - - float d = (v1.y - v2.y) * (v0.x - v2.x) + (v2.x - v1.x) * (v0.y - v2.y); - if (abs(d) < 0.0001) return false; - - float a = ((v1.y - v2.y) * (p.x - v2.x) + (v2.x - v1.x) * (p.y - v2.y)) / d; - float b = ((v2.y - v0.y) * (p.x - v2.x) + (v0.x - v2.x) * (p.y - v2.y)) / d; - float c = 1.0 - a - b; - - return (a >= 0.0 && b >= 0.0 && c >= 0.0); - } - - bool inside = false; + int crossings = 0; for (int i = 0, j = pointCount - 1; i < pointCount; j = i++) { - if (distance(points[i], points[j]) < 0.0001) continue; + vec2 pi = points[i]; + vec2 pj = points[j]; - float dy = points[j].y - points[i].y; - if (abs(dy) < 0.0001) continue; + // Skip degenerate edges + if (distance(pi, pj) < 0.001) continue; - if (((points[i].y > p.y) != (points[j].y > p.y))) { - float x_intersect = points[i].x + (points[j].x - points[i].x) * (p.y - points[i].y) / dy; - if (p.x < x_intersect) { - inside = !inside; - } + // Ray-casting + if (((pi.y > p.y) != (pj.y > p.y)) && + (p.x < (pj.x - pi.x) * (p.y - pi.y) / (pj.y - pi.y + 0.001) + pi.x)) { + crossings++; } } - return inside; + return (crossings & 1) == 1; } float distanceToEdge(vec2 p) { @@ -119,20 +117,14 @@ void main() { vec2 pixelGrad = vec2(dFdx(pixel.x), dFdy(pixel.y)); float pixelSize = length(pixelGrad); - float aaWidth = max(0.5, pixelSize * 1.0); + float aaWidth = max(0.5, pixelSize * 0.5); // Sharper anti-aliasing float alpha = smoothstep(-aaWidth, aaWidth, signedDist); - if (alpha > 0.0) { - vec4 color; - if (useGradient) { - color = getGradientColor(fragTexCoord); - } else { - color = fillColor; - } + vec4 color = useGradient ? getGradientColor(fragTexCoord) : fillColor; finalColor = vec4(color.rgb, color.a * alpha); } else { - finalColor = vec4(0.0, 0.0, 0.0, 0.0); + finalColor = vec4(0.0); } } """ @@ -188,6 +180,7 @@ class ShaderState: 'gradientStops': None, 'gradientColorCount': None, 'mvp': None, + 'visibleGradientRange': None, } # Pre-allocated FFI objects @@ -198,17 +191,15 @@ class ShaderState: self.gradient_start_ptr = rl.ffi.new("float[]", [0.0, 0.0]) self.gradient_end_ptr = rl.ffi.new("float[]", [0.0, 0.0]) self.color_count_ptr = rl.ffi.new("int[]", [0]) - - # Pre-allocate gradient arrays (max 8 colors) - self.gradient_colors_ptr = rl.ffi.new("float[]", 32) # 8 colors * 4 components - self.gradient_stops_ptr = rl.ffi.new("float[]", 8) + self.visible_gradient_range_ptr = rl.ffi.new("float[]", [0.0, 0.0]) + self.gradient_colors_ptr = rl.ffi.new("float[]", MAX_GRADIENT_COLORS * 4) + self.gradient_stops_ptr = rl.ffi.new("float[]", MAX_GRADIENT_COLORS) def initialize(self): if self.initialized: return - vertex_shader = rl.load_shader_from_memory(VERTEX_SHADER, FRAGMENT_SHADER) - self.shader = vertex_shader + self.shader = rl.load_shader_from_memory(VERTEX_SHADER, FRAGMENT_SHADER) # Create and cache white texture white_img = rl.gen_image_color(2, 2, rl.WHITE) @@ -241,21 +232,46 @@ class ShaderState: self.initialized = False -def _configure_shader_color(state, color, gradient): +def _configure_shader_color(state, color, gradient, rect, min_xy, max_xy): """Configure shader uniforms for solid color or gradient rendering""" - state.use_gradient_ptr[0] = 1 if gradient else 0 + use_gradient = 1 if gradient else 0 + state.use_gradient_ptr[0] = use_gradient rl.set_shader_value(state.shader, state.locations['useGradient'], state.use_gradient_ptr, UNIFORM_INT) - if gradient: + if use_gradient: # Set gradient start/end state.gradient_start_ptr[0:2] = gradient['start'] state.gradient_end_ptr[0:2] = gradient['end'] rl.set_shader_value(state.shader, state.locations['gradientStart'], state.gradient_start_ptr, UNIFORM_VEC2) rl.set_shader_value(state.shader, state.locations['gradientEnd'], state.gradient_end_ptr, UNIFORM_VEC2) + # Calculate visible gradient range + width = max_xy[0] - min_xy[0] + height = max_xy[1] - min_xy[1] + + gradient_dir = (gradient['end'][0] - gradient['start'][0], gradient['end'][1] - gradient['start'][1]) + is_vertical = abs(gradient_dir[1]) > abs(gradient_dir[0]) + + visible_start = 0.0 + visible_end = 1.0 + + if is_vertical and height > 0: + visible_start = (rect.y - min_xy[1]) / height + visible_end = visible_start + rect.height / height + elif width > 0: + visible_start = (rect.x - min_xy[0]) / width + visible_end = visible_start + rect.width / width + + # Clamp visible range + visible_start = max(0.0, min(1.0, visible_start)) + visible_end = max(0.0, min(1.0, visible_end)) + + state.visible_gradient_range_ptr[0:2] = [visible_start, visible_end] + rl.set_shader_value(state.shader, state.locations['visibleGradientRange'], state.visible_gradient_range_ptr, UNIFORM_VEC2) + # Set gradient colors colors = gradient['colors'] - color_count = min(len(colors), 8) # Max 8 colors + color_count = min(len(colors), MAX_GRADIENT_COLORS) for i, c in enumerate(colors[:color_count]): base_idx = i * 4 state.gradient_colors_ptr[base_idx:base_idx+4] = [c.r / 255.0, c.g / 255.0, c.b / 255.0, c.a / 255.0] @@ -275,11 +291,12 @@ def _configure_shader_color(state, color, gradient): rl.set_shader_value(state.shader, state.locations['fillColor'], state.fill_color_ptr, UNIFORM_VEC4) -def draw_polygon(points: np.ndarray, color=None, gradient=None): +def draw_polygon(rect: rl.Rectangle, points: np.ndarray, color=None, gradient=None): """ Draw a complex polygon using shader-based even-odd fill rule Args: + rect: Rectangle defining the drawing area points: numpy array of (x,y) points defining the polygon color: Solid fill color (rl.Color) gradient: Dict with gradient parameters: @@ -301,33 +318,43 @@ def draw_polygon(points: np.ndarray, color=None, gradient=None): min_xy = np.min(points, axis=0) max_xy = np.max(points, axis=0) - width = max(1, max_xy[0] - min_xy[0]) - height = max(1, max_xy[1] - min_xy[1]) + # Clip coordinates to rectangle + clip_x = max(rect.x, min_xy[0]) + clip_y = max(rect.y, min_xy[1]) + clip_right = min(rect.x + rect.width, max_xy[0]) + clip_bottom = min(rect.y + rect.height, max_xy[1]) - # Transform points to shader space - transformed_points = points - min_xy + # Check if polygon is completely off-screen + if clip_x >= clip_right or clip_y >= clip_bottom: + return + clipped_width = clip_right - clip_x + clipped_height = clip_bottom - clip_y + + clip_rect = rl.Rectangle(clip_x, clip_y, clipped_width, clipped_height) + + # Transform points relative to the CLIPPED area + transformed_points = points - np.array([clip_x, clip_y]) + + # Set shader values state.point_count_ptr[0] = len(transformed_points) rl.set_shader_value(state.shader, state.locations['pointCount'], state.point_count_ptr, UNIFORM_INT) - state.resolution_ptr[0:2] = [width, height] + state.resolution_ptr[0:2] = [clipped_width, clipped_height] rl.set_shader_value(state.shader, state.locations['resolution'], state.resolution_ptr, UNIFORM_VEC2) - - # Set points flat_points = np.ascontiguousarray(transformed_points.flatten().astype(np.float32)) points_ptr = rl.ffi.cast("float *", flat_points.ctypes.data) rl.set_shader_value_v(state.shader, state.locations['points'], points_ptr, UNIFORM_VEC2, len(transformed_points)) - # Configure color/gradient uniforms - _configure_shader_color(state, color, gradient) + _configure_shader_color(state, color, gradient, clip_rect, min_xy, max_xy) - # Draw with shader + # Render rl.begin_shader_mode(state.shader) rl.draw_texture_pro( state.white_texture, rl.Rectangle(0, 0, 2, 2), - rl.Rectangle(int(min_xy[0]), int(min_xy[1]), int(width), int(height)), + clip_rect, rl.Vector2(0, 0), 0.0, rl.WHITE, diff --git a/system/ui/onroad/model_renderer.py b/system/ui/onroad/model_renderer.py index ac3ac74430..1f608a6ddb 100644 --- a/system/ui/onroad/model_renderer.py +++ b/system/ui/onroad/model_renderer.py @@ -47,6 +47,7 @@ class ModelRenderer: self._car_space_transform = np.zeros((3, 3)) self._transform_dirty = True self._clip_region = None + self._rect = None # Get longitudinal control setting from car parameters car_params = Params().get("CarParams") @@ -64,6 +65,7 @@ class ModelRenderer: return # Set up clipping region + self._rect = rect self._clip_region = rl.Rectangle( rect.x - CLIP_MARGIN, rect.y - CLIP_MARGIN, rect.width + 2 * CLIP_MARGIN, rect.height + 2 * CLIP_MARGIN ) @@ -156,7 +158,7 @@ class ModelRenderer: # Draw lane line alpha = np.clip(self._lane_line_probs[i], 0.0, 0.7) color = rl.Color(255, 255, 255, int(alpha * 255)) - draw_polygon(vertices, color) + draw_polygon(self._rect, vertices, color) for i, vertices in enumerate(self._road_edge_vertices): # Skip if no vertices @@ -166,7 +168,7 @@ class ModelRenderer: # Draw road edge alpha = np.clip(1.0 - self._road_edge_stds[i], 0.0, 1.0) color = rl.Color(255, 0, 0, int(alpha * 255)) - draw_polygon(vertices, color) + draw_polygon(self._rect, vertices, color) def _draw_path(self, sm, model, height): """Draw the path polygon with gradient based on acceleration""" @@ -218,7 +220,7 @@ class ModelRenderer: segment_colors.append(color) if len(segment_colors) < 2: - draw_polygon(self._track_vertices, rl.Color(255, 255, 255, 30)) + draw_polygon(self._rect, self._track_vertices, rl.Color(255, 255, 255, 30)) return # Create gradient specification @@ -228,7 +230,7 @@ class ModelRenderer: 'colors': segment_colors, 'stops': gradient_stops, } - draw_polygon(self._track_vertices, gradient=gradient) + draw_polygon(self._rect, self._track_vertices, gradient=gradient) else: # Draw with throttle/no throttle gradient allow_throttle = sm['longitudinalPlan'].allowThrottle or not self._longitudinal_control @@ -253,7 +255,7 @@ class ModelRenderer: 'colors': blended_colors, 'stops': [0.0, 0.5, 1.0], } - draw_polygon(self._track_vertices, gradient=gradient) + draw_polygon(self._rect, self._track_vertices, gradient=gradient) def _draw_lead(self, lead_data, vd, rect): """Draw lead vehicle indicator""" From 2d6662ae9f0af252dbe5d0ab1395810011150be1 Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Fri, 30 May 2025 04:07:43 +0800 Subject: [PATCH 33/60] system/ui: match experimental path rendering with C++ version (#35380) match draw_path with c++ implementation --- system/ui/onroad/model_renderer.py | 30 +++++++++++++----------------- 1 file changed, 13 insertions(+), 17 deletions(-) diff --git a/system/ui/onroad/model_renderer.py b/system/ui/onroad/model_renderer.py index 1f608a6ddb..afed67aab2 100644 --- a/system/ui/onroad/model_renderer.py +++ b/system/ui/onroad/model_renderer.py @@ -180,29 +180,20 @@ class ModelRenderer: acceleration = model.acceleration.x max_len = min(len(self._track_vertices) // 2, len(acceleration)) - # Find midpoint index for polygon - mid_point = len(self._track_vertices) // 2 - - # For acceleration-based coloring, process segments separately - left_side = self._track_vertices[:mid_point] - right_side = self._track_vertices[mid_point:][::-1] # Reverse for proper winding - # Create segments for gradient coloring segment_colors = [] gradient_stops = [] - for i in range(max_len - 1): - if i >= len(left_side) - 1 or i >= len(right_side) - 1: - break - + i = 0 + while i < max_len: track_idx = max_len - i - 1 # flip idx to start from bottom right - - # Skip points out of frame - if left_side[track_idx][1] < 0 or left_side[track_idx][1] > height: + track_y = self._track_vertices[track_idx][1] + if track_y < 0 or track_y > height: + i += 1 continue # Calculate color based on acceleration - lin_grad_point = (height - left_side[track_idx][1]) / height + lin_grad_point = (height - track_y) / height # speed up: 120, slow down: 0 path_hue = max(min(60 + acceleration[i] * 35, 120), 0) @@ -219,6 +210,9 @@ class ModelRenderer: gradient_stops.append(lin_grad_point) segment_colors.append(color) + # Skip a point, unless next is last + i += 1 + (1 if (i + 2) < max_len else 0) + if len(segment_colors) < 2: draw_polygon(self._rect, self._track_vertices, rl.Color(255, 255, 255, 30)) return @@ -345,8 +339,10 @@ class ModelRenderer: @staticmethod def _map_val(x, x0, x1, y0, y1): - """Map value x from range [x0, x1] to range [y0, y1]""" - return y0 + (y1 - y0) * ((x - x0) / (x1 - x0)) if x1 != x0 else y0 + x = max(x0, min(x, x1)) + ra = x1 - x0 + rb = y1 - y0 + return (x - x0) * rb / ra + y0 if ra != 0 else y0 @staticmethod def _hsla_to_color(h, s, l, a): From ae5e87e91586c7f92e5181cfd976cb71c92bf132 Mon Sep 17 00:00:00 2001 From: Maxime Desroches Date: Thu, 29 May 2025 13:43:38 -0700 Subject: [PATCH 34/60] bump panda --- panda | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panda b/panda index d75699e6ab..4f227f88c8 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit d75699e6abf41636701775ba0eea8e07858599ad +Subproject commit 4f227f88c827d02188763d676aab941401be8212 From a385ed59cf6ef65a8e97aef79bf53421e40013a2 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 29 May 2025 14:24:39 -0700 Subject: [PATCH 35/60] Invalid LKAS setting alert: custom text per brand (#35377) * draft * use it * friendly * clean up * CC * Apply suggestions from code review Co-authored-by: Adeeb Shihadeh * Update selfdrive/selfdrived/events.py --------- Co-authored-by: Adeeb Shihadeh --- selfdrive/selfdrived/events.py | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/selfdrive/selfdrived/events.py b/selfdrive/selfdrived/events.py index 6293deb2af..9e1892bd5f 100755 --- a/selfdrive/selfdrived/events.py +++ b/selfdrive/selfdrived/events.py @@ -359,6 +359,16 @@ def personality_changed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging return NormalPermanentAlert(f"Driving Personality: {personality}", duration=1.5) +def invalid_lkas_setting_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: + text = "Toggle stock LKAS on or off to engage" + if CP.brand == "tesla": + text = "Switch to Traffic-Aware Cruise Control to engage" + elif CP.brand == "mazda": + text = "Enable your car's LKAS to engage" + elif CP.brand == "nissan": + text = "Disable your car's stock LKAS to engage" + return NormalPermanentAlert("Invalid LKAS setting", text) + EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { # ********** events with no alerts ********** @@ -412,8 +422,7 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { }, EventName.invalidLkasSetting: { - ET.PERMANENT: NormalPermanentAlert("Invalid LKAS setting", - "Toggle stock LKAS on or off to engage"), + ET.PERMANENT: invalid_lkas_setting_alert, ET.NO_ENTRY: NoEntryAlert("Invalid LKAS setting"), }, From 0d527c2409b8db15a068b398de23a2ba52d5fc2a Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Fri, 30 May 2025 11:51:15 +0800 Subject: [PATCH 36/60] system/ui: fix the issue of missing path segments (#35383) match c++ version --- system/ui/onroad/model_renderer.py | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/system/ui/onroad/model_renderer.py b/system/ui/onroad/model_renderer.py index afed67aab2..141ffa3110 100644 --- a/system/ui/onroad/model_renderer.py +++ b/system/ui/onroad/model_renderer.py @@ -313,8 +313,7 @@ class ModelRenderer: line_y = line.y line_z = line.z - left_points: list[tuple[float, float]] = [] - right_points: list[tuple[float, float]] = [] + points = [] for i in range(max_idx + 1): # Skip points with negative x (behind camera) @@ -326,16 +325,16 @@ class ModelRenderer: if left and right: # Check for inversion when going over hills - if not allow_invert and left_points and left[1] > left_points[-1][1]: + if not allow_invert and points and left[1] > points[-1][1]: continue - left_points.append(left) - right_points.append(right) + points.append(left) + points.insert(0, right) - if not left_points or not right_points: + if not points: return np.empty((0, 2), dtype=np.float32) - return np.array(left_points + right_points[::-1], dtype=np.float32) + return np.array(points, dtype=np.float32) @staticmethod def _map_val(x, x0, x1, y0, y1): From 6e9e43d03bb9222555d54c5825f3aa9a8536ece7 Mon Sep 17 00:00:00 2001 From: Brett Sanderson Date: Fri, 30 May 2025 02:18:44 -0400 Subject: [PATCH 37/60] Fix CI for external repositories (#35382) --- .github/workflows/selfdrive_tests.yaml | 72 +++++++++++++++++--------- 1 file changed, 48 insertions(+), 24 deletions(-) diff --git a/.github/workflows/selfdrive_tests.yaml b/.github/workflows/selfdrive_tests.yaml index 3edb6c2981..8403bdd9cc 100644 --- a/.github/workflows/selfdrive_tests.yaml +++ b/.github/workflows/selfdrive_tests.yaml @@ -32,9 +32,12 @@ env: jobs: build_release: name: build release - runs-on: - - ${{ ((github.repository == 'commaai/openpilot') && ((github.event_name != 'pull_request') || (github.event.pull_request.head.repo.full_name == 'commaai/openpilot'))) && 'namespace-profile-amd64-8x16' || 'ubuntu-24.04' }} - - ${{ ((github.repository == 'commaai/openpilot') && ((github.event_name != 'pull_request') || (github.event.pull_request.head.repo.full_name == 'commaai/openpilot'))) && 'namespace-experiments:docker.builds.local-cache=separate' || 'ubuntu-24.04' }} + runs-on: ${{ + (github.repository == 'commaai/openpilot') && + ((github.event_name != 'pull_request') || + (github.event.pull_request.head.repo.full_name == 'commaai/openpilot')) + && fromJSON('["namespace-profile-amd64-8x16", "namespace-experiments:docker.builds.local-cache=separate"]') + || fromJSON('["ubuntu-24.04"]') }} env: STRIPPED_DIR: /tmp/releasepilot steps: @@ -67,9 +70,12 @@ jobs: run: release/check-submodules.sh build: - runs-on: - - ${{ ((github.repository == 'commaai/openpilot') && ((github.event_name != 'pull_request') || (github.event.pull_request.head.repo.full_name == 'commaai/openpilot'))) && 'namespace-profile-amd64-8x16' || 'ubuntu-24.04' }} - - ${{ ((github.repository == 'commaai/openpilot') && ((github.event_name != 'pull_request') || (github.event.pull_request.head.repo.full_name == 'commaai/openpilot'))) && 'namespace-experiments:docker.builds.local-cache=separate' || 'ubuntu-24.04' }} + runs-on: ${{ + (github.repository == 'commaai/openpilot') && + ((github.event_name != 'pull_request') || + (github.event.pull_request.head.repo.full_name == 'commaai/openpilot')) + && fromJSON('["namespace-profile-amd64-8x16", "namespace-experiments:docker.builds.local-cache=separate"]') + || fromJSON('["ubuntu-24.04"]') }} steps: - uses: actions/checkout@v4 with: @@ -118,9 +124,12 @@ jobs: static_analysis: name: static analysis - runs-on: - - ${{ ((github.repository == 'commaai/openpilot') && ((github.event_name != 'pull_request') || (github.event.pull_request.head.repo.full_name == 'commaai/openpilot'))) && 'namespace-profile-amd64-8x16' || 'ubuntu-24.04' }} - - ${{ ((github.repository == 'commaai/openpilot') && ((github.event_name != 'pull_request') || (github.event.pull_request.head.repo.full_name == 'commaai/openpilot'))) && 'namespace-experiments:docker.builds.local-cache=separate' || 'ubuntu-24.04' }} + runs-on: ${{ + (github.repository == 'commaai/openpilot') && + ((github.event_name != 'pull_request') || + (github.event.pull_request.head.repo.full_name == 'commaai/openpilot')) + && fromJSON('["namespace-profile-amd64-8x16", "namespace-experiments:docker.builds.local-cache=separate"]') + || fromJSON('["ubuntu-24.04"]') }} env: PYTHONWARNINGS: default steps: @@ -134,9 +143,12 @@ jobs: unit_tests: name: unit tests - runs-on: - - ${{ ((github.repository == 'commaai/openpilot') && ((github.event_name != 'pull_request') || (github.event.pull_request.head.repo.full_name == 'commaai/openpilot'))) && 'namespace-profile-amd64-8x16' || 'ubuntu-24.04' }} - - ${{ ((github.repository == 'commaai/openpilot') && ((github.event_name != 'pull_request') || (github.event.pull_request.head.repo.full_name == 'commaai/openpilot'))) && 'namespace-experiments:docker.builds.local-cache=separate' || 'ubuntu-24.04' }} + runs-on: ${{ + (github.repository == 'commaai/openpilot') && + ((github.event_name != 'pull_request') || + (github.event.pull_request.head.repo.full_name == 'commaai/openpilot')) + && fromJSON('["namespace-profile-amd64-8x16", "namespace-experiments:docker.builds.local-cache=separate"]') + || fromJSON('["ubuntu-24.04"]') }} steps: - uses: actions/checkout@v4 with: @@ -161,9 +173,12 @@ jobs: process_replay: name: process replay - runs-on: - - ${{ ((github.repository == 'commaai/openpilot') && ((github.event_name != 'pull_request') || (github.event.pull_request.head.repo.full_name == 'commaai/openpilot'))) && 'namespace-profile-amd64-8x16' || 'ubuntu-24.04' }} - - ${{ ((github.repository == 'commaai/openpilot') && ((github.event_name != 'pull_request') || (github.event.pull_request.head.repo.full_name == 'commaai/openpilot'))) && 'namespace-experiments:docker.builds.local-cache=separate' || 'ubuntu-24.04' }} + runs-on: ${{ + (github.repository == 'commaai/openpilot') && + ((github.event_name != 'pull_request') || + (github.event.pull_request.head.repo.full_name == 'commaai/openpilot')) + && fromJSON('["namespace-profile-amd64-8x16", "namespace-experiments:docker.builds.local-cache=separate"]') + || fromJSON('["ubuntu-24.04"]') }} steps: - uses: actions/checkout@v4 with: @@ -214,9 +229,12 @@ jobs: test_cars: name: cars - runs-on: - - ${{ ((github.repository == 'commaai/openpilot') && ((github.event_name != 'pull_request') || (github.event.pull_request.head.repo.full_name == 'commaai/openpilot'))) && 'namespace-profile-amd64-8x16' || 'ubuntu-24.04' }} - - ${{ ((github.repository == 'commaai/openpilot') && ((github.event_name != 'pull_request') || (github.event.pull_request.head.repo.full_name == 'commaai/openpilot'))) && 'namespace-experiments:docker.builds.local-cache=separate' || 'ubuntu-24.04' }} + runs-on: ${{ + (github.repository == 'commaai/openpilot') && + ((github.event_name != 'pull_request') || + (github.event.pull_request.head.repo.full_name == 'commaai/openpilot')) + && fromJSON('["namespace-profile-amd64-8x16", "namespace-experiments:docker.builds.local-cache=separate"]') + || fromJSON('["ubuntu-24.04"]') }} strategy: fail-fast: false matrix: @@ -306,9 +324,12 @@ jobs: simulator_driving: name: simulator driving - runs-on: - - ${{ ((github.repository == 'commaai/openpilot') && ((github.event_name != 'pull_request') || (github.event.pull_request.head.repo.full_name == 'commaai/openpilot'))) && 'namespace-profile-amd64-8x16' || 'ubuntu-24.04' }} - - ${{ ((github.repository == 'commaai/openpilot') && ((github.event_name != 'pull_request') || (github.event.pull_request.head.repo.full_name == 'commaai/openpilot'))) && 'namespace-experiments:docker.builds.local-cache=separate' || 'ubuntu-24.04' }} + runs-on: ${{ + (github.repository == 'commaai/openpilot') && + ((github.event_name != 'pull_request') || + (github.event.pull_request.head.repo.full_name == 'commaai/openpilot')) + && fromJSON('["namespace-profile-amd64-8x16", "namespace-experiments:docker.builds.local-cache=separate"]') + || fromJSON('["ubuntu-24.04"]') }} if: (github.repository == 'commaai/openpilot') && ((github.event_name != 'pull_request') || (github.event.pull_request.head.repo.full_name == 'commaai/openpilot')) steps: - uses: actions/checkout@v4 @@ -328,9 +349,12 @@ jobs: create_ui_report: # This job name needs to be the same as UI_JOB_NAME in ui_preview.yaml name: Create UI Report - runs-on: - - ${{ ((github.repository == 'commaai/openpilot') && ((github.event_name != 'pull_request') || (github.event.pull_request.head.repo.full_name == 'commaai/openpilot'))) && 'namespace-profile-amd64-8x16' || 'ubuntu-24.04' }} - - ${{ ((github.repository == 'commaai/openpilot') && ((github.event_name != 'pull_request') || (github.event.pull_request.head.repo.full_name == 'commaai/openpilot'))) && 'namespace-experiments:docker.builds.local-cache=separate' || 'ubuntu-24.04' }} + runs-on: ${{ + (github.repository == 'commaai/openpilot') && + ((github.event_name != 'pull_request') || + (github.event.pull_request.head.repo.full_name == 'commaai/openpilot')) + && fromJSON('["namespace-profile-amd64-8x16", "namespace-experiments:docker.builds.local-cache=separate"]') + || fromJSON('["ubuntu-24.04"]') }} if: false # FIXME: FrameReader is broken on CI runners steps: - uses: actions/checkout@v4 From 31e22b0a1d120eb1de4fc6ed38eb613431410aa1 Mon Sep 17 00:00:00 2001 From: Nayan Date: Fri, 30 May 2025 03:06:15 -0400 Subject: [PATCH 38/60] ui: add models panel (#956) * models panel * models panel * fix ui report * stupid scroll * cleanup whitespaces * Update selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.cc --------- Co-authored-by: DevTekVE --- selfdrive/ui/sunnypilot/SConscript | 1 + .../qt/offroad/settings/models_panel.cc | 12 ++++++++++++ .../qt/offroad/settings/models_panel.h | 17 +++++++++++++++++ .../sunnypilot/qt/offroad/settings/settings.cc | 2 ++ selfdrive/ui/tests/test_ui/run.py | 14 +++++++++++--- .../selfdrive/assets/offroad/icon_models.png | 3 +++ 6 files changed, 46 insertions(+), 3 deletions(-) create mode 100644 selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.cc create mode 100644 selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.h create mode 100644 sunnypilot/selfdrive/assets/offroad/icon_models.png diff --git a/selfdrive/ui/sunnypilot/SConscript b/selfdrive/ui/sunnypilot/SConscript index d189751843..b5d18c5b91 100644 --- a/selfdrive/ui/sunnypilot/SConscript +++ b/selfdrive/ui/sunnypilot/SConscript @@ -24,6 +24,7 @@ qt_src = [ "sunnypilot/qt/offroad/settings/lateral_panel.cc", "sunnypilot/qt/offroad/settings/longitudinal_panel.cc", "sunnypilot/qt/offroad/settings/max_time_offroad.cc", + "sunnypilot/qt/offroad/settings/models_panel.cc", "sunnypilot/qt/offroad/settings/settings.cc", "sunnypilot/qt/offroad/settings/software_panel.cc", "sunnypilot/qt/offroad/settings/sunnylink_panel.cc", diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.cc new file mode 100644 index 0000000000..8f8d38d2d8 --- /dev/null +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.cc @@ -0,0 +1,12 @@ +/** + * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + * + * This file is part of sunnypilot and is licensed under the MIT License. + * See the LICENSE.md file in the root directory for more details. + */ + +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.h" + +ModelsPanel::ModelsPanel(QWidget *parent) : QWidget(parent) { + +} diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.h new file mode 100644 index 0000000000..6c28b5bf35 --- /dev/null +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.h @@ -0,0 +1,17 @@ +/** + * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + * + * This file is part of sunnypilot and is licensed under the MIT License. + * See the LICENSE.md file in the root directory for more details. + */ + +#pragma once + +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h" + +class ModelsPanel : public QWidget { + Q_OBJECT + +public: + explicit ModelsPanel(QWidget *parent = nullptr); +}; diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/settings.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/settings.cc index 48a395dad7..ad057ac714 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/settings.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/settings.cc @@ -13,6 +13,7 @@ #include "selfdrive/ui/sunnypilot/qt/network/networking.h" #include "selfdrive/ui/sunnypilot/qt/offroad/settings/device_panel.h" +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.h" #include "selfdrive/ui/sunnypilot/qt/offroad/settings/software_panel.h" #include "selfdrive/ui/sunnypilot/qt/offroad/settings/sunnylink_panel.h" #include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.h" @@ -79,6 +80,7 @@ SettingsWindowSP::SettingsWindowSP(QWidget *parent) : SettingsWindow(parent) { PanelInfo(" " + tr("sunnylink"), new SunnylinkPanel(this), "../assets/icons/wifi_strength_full.svg"), PanelInfo(" " + tr("Toggles"), toggles, "../../sunnypilot/selfdrive/assets/offroad/icon_toggle.png"), PanelInfo(" " + tr("Software"), new SoftwarePanelSP(this), "../../sunnypilot/selfdrive/assets/offroad/icon_software.png"), + PanelInfo(" " + tr("Models"), new ModelsPanel(this), "../../sunnypilot/selfdrive/assets/offroad/icon_models.png"), PanelInfo(" " + tr("Steering"), new LateralPanel(this), "../../sunnypilot/selfdrive/assets/offroad/icon_lateral.png"), PanelInfo(" " + tr("Cruise"), new LongitudinalPanel(this), "../assets/icons/speed_limit.png"), PanelInfo(" " + tr("Trips"), new TripsPanel(this), "../../sunnypilot/selfdrive/assets/offroad/icon_trips.png"), diff --git a/selfdrive/ui/tests/test_ui/run.py b/selfdrive/ui/tests/test_ui/run.py index 9d078fdd9e..f8c64de769 100755 --- a/selfdrive/ui/tests/test_ui/run.py +++ b/selfdrive/ui/tests/test_ui/run.py @@ -213,6 +213,12 @@ def setup_settings_sunnylink_sponsor_button(click, pm: PubMaster, scroll=None): click(1967, 225) time.sleep(UI_DELAY) +def setup_settings_models(click, pm: PubMaster, scroll=None): + setup_settings_device(click, pm) + click(278, 852) + time.sleep(UI_DELAY) + + def setup_settings_steering(click, pm: PubMaster, scroll=None): CP = car.CarParams() CP.carFingerprint = "HONDA_CIVIC" @@ -223,25 +229,26 @@ def setup_settings_steering(click, pm: PubMaster, scroll=None): Params().put("CarParamsSPPersistent", CP_SP.to_bytes()) setup_settings_device(click, pm) - click(278, 852) + click(278, 962) time.sleep(UI_DELAY) def setup_settings_steering_mads(click, pm: PubMaster, scroll=None): Params().put_bool("Mads", True) setup_settings_device(click, pm) - click(278, 852) + click(278, 962) click(970, 250) time.sleep(UI_DELAY) def setup_settings_steering_alc(click, pm: PubMaster, scroll=None): setup_settings_device(click, pm) - click(278, 852) + click(278, 962) click(970, 534) time.sleep(UI_DELAY) def setup_settings_driving(click, pm: PubMaster, scroll=None): setup_settings_device(click, pm) + scroll(-1, 278, 962) click(278, 962) time.sleep(UI_DELAY) @@ -295,6 +302,7 @@ CASES = { CASES.update({ "settings_sunnylink": setup_settings_sunnylink, "settings_sunnylink_sponsor_button": setup_settings_sunnylink_sponsor_button, + "settings_models": setup_settings_models, "settings_steering": setup_settings_steering, "settings_steering_mads": setup_settings_steering_mads, "settings_steering_alc": setup_settings_steering_alc, diff --git a/sunnypilot/selfdrive/assets/offroad/icon_models.png b/sunnypilot/selfdrive/assets/offroad/icon_models.png new file mode 100644 index 0000000000..0131759703 --- /dev/null +++ b/sunnypilot/selfdrive/assets/offroad/icon_models.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d9b431c274a55437b5a65ada7c85503e137bc2ffa8917510ad9affb14a407d82 +size 14366 From d9d32ebea26955f3a535be33a44ded11026ae672 Mon Sep 17 00:00:00 2001 From: Nayan Date: Fri, 30 May 2025 06:06:50 -0400 Subject: [PATCH 39/60] UI: Add Search Capability to Branch Selector (#814) * Create sync-dev.yml * Update selfdrive_tests.yaml * Update sync-dev.yml * Update sync-dev.yml * Update sync-dev.yml * add search capability in branch selector * maybe i should remove test code * include * reduce needed includes * add newline for eof * no changes needed in upstream file * not sure why this was here * try to resolve merge conflicts for dev squash * try to resolve merge conflict * No need for this for the time being --------- Co-authored-by: Discountchubbs <159560811+Discountchubbs@users.noreply.github.com> Co-authored-by: Jason Wen Co-authored-by: DevTekVE --- .../qt/offroad/settings/software_panel.cc | 43 +++++++++++++++++++ .../qt/offroad/settings/software_panel.h | 4 ++ selfdrive/ui/sunnypilot/qt/util.cc | 32 ++++++++++++++ selfdrive/ui/sunnypilot/qt/util.h | 2 + 4 files changed, 81 insertions(+) diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/software_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/software_panel.cc index 20f3fdb598..57fb38c20b 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/software_panel.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/software_panel.cc @@ -17,6 +17,19 @@ * @param parent Parent widget */ SoftwarePanelSP::SoftwarePanelSP(QWidget *parent) : SoftwarePanel(parent) { + +// branch selector + QObject::disconnect(targetBranchBtn, nullptr, nullptr, nullptr); + connect(targetBranchBtn, &ButtonControlSP::clicked, [=]() { + InputDialog d(tr("Search Branch"), this, tr("Enter search keywords, or leave blank to list all branches."), false); + d.setMinLength(0); + const int ret = d.exec(); + if (ret) { + searchBranches(d.text()); + } + + }); + const auto current_model = GetActiveModelName(); currentModelLblBtn = new ButtonControlSP(tr("Current Model"), tr("SELECT"), current_model); currentModelLblBtn->setValue(current_model); @@ -205,3 +218,33 @@ void SoftwarePanelSP::showResetParamsDialog() { params.remove("LiveTorqueParameters"); } } + +/** + * @brief Searches for available branches based on a query string, presents the results in a dialog, + * and updates the target branch if a selection is made. + * + * This function filters the list of branches based on the provided query, and displays the filtered branches in a selection dialog. + * If a branch is selected, the "UpdaterTargetBranch" parameter is updated and a check for updates is triggered. + * If no branches are found matching the query, an alert dialog is displayed. + * + * @param query The search query string. + */ +void SoftwarePanelSP::searchBranches(const QString &query) { + + QStringList branches = QString::fromStdString(params.get("UpdaterAvailableBranches")).split(","); + QStringList results = searchFromList(query, branches); + results.sort(); + + if (results.isEmpty()) { + ConfirmationDialog::alert(tr("No branches found for keywords: %1").arg(query), this); + return; + } + + QString selected_branch = MultiOptionDialog::getSelection(tr("Select a branch"), results, "", this); + + if (!selected_branch.isEmpty()) { + params.put("UpdaterTargetBranch", selected_branch.toStdString()); + targetBranchBtn->setValue(selected_branch); + checkForUpdates(); + } +} diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/software_panel.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/software_panel.h index d0299d6efd..3679ade410 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/software_panel.h +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/software_panel.h @@ -9,6 +9,7 @@ #include #include "selfdrive/ui/sunnypilot/ui.h" +#include "selfdrive/ui/sunnypilot/qt/util.h" #include "selfdrive/ui/qt/offroad/settings.h" class SoftwarePanelSP final : public SoftwarePanel { @@ -17,6 +18,9 @@ class SoftwarePanelSP final : public SoftwarePanel { public: explicit SoftwarePanelSP(QWidget *parent = nullptr); +private: + void searchBranches(const QString &query); + private: QString GetActiveModelName(); void updateModelManagerState(); diff --git a/selfdrive/ui/sunnypilot/qt/util.cc b/selfdrive/ui/sunnypilot/qt/util.cc index c729eaafd5..ca85935d0b 100644 --- a/selfdrive/ui/sunnypilot/qt/util.cc +++ b/selfdrive/ui/sunnypilot/qt/util.cc @@ -78,3 +78,35 @@ QMap loadPlatformList() { return _platforms; } + +/** + * @brief Searches a list of strings for elements containing all search terms in a query. + * + * The search is case-insensitive and normalizes both the query and the list elements + * using Unicode KD normalization before comparison. Non-alphanumeric characters are + * removed from the search terms before comparison. + * + * @param query The search query string. Multiple words can be separated by spaces. + * @param list The source list of strings to search. + * @return A list of strings from the input list that contain all of the search terms. + */ +QStringList searchFromList(const QString &query, const QStringList &list) { + if (query.isEmpty()) { + return list; + } + + QStringList search_terms = query.simplified().toLower().split(" ", QString::SkipEmptyParts); + QStringList search_results; + + for (const QString &element : list) { + if (std::all_of(search_terms.begin(), search_terms.end(), [&](const QString &term) { + QString normalized_term = term.normalized(QString::NormalizationForm_KD).toLower(); + normalized_term.remove(QRegularExpression("[^a-zA-Z0-9\\s]")); + QString normalized_element = element.normalized(QString::NormalizationForm_KD).toLower(); + return normalized_element.contains(normalized_term, Qt::CaseInsensitive); + })) { + search_results << element; + } + } + return search_results; +} diff --git a/selfdrive/ui/sunnypilot/qt/util.h b/selfdrive/ui/sunnypilot/qt/util.h index 0a581579e2..089b5370cc 100644 --- a/selfdrive/ui/sunnypilot/qt/util.h +++ b/selfdrive/ui/sunnypilot/qt/util.h @@ -12,9 +12,11 @@ #include #include +#include #include QString getUserAgent(bool sunnylink = false); std::optional getSunnylinkDongleId(); std::optional getParamIgnoringDefault(const std::string ¶m_name, const std::string &default_value); QMap loadPlatformList(); +QStringList searchFromList(const QString &query, const QStringList &list); From be406d8f838f58de21c746eccba73e705bf11841 Mon Sep 17 00:00:00 2001 From: Nayan Date: Fri, 30 May 2025 07:06:09 -0400 Subject: [PATCH 40/60] ui: move model selector to models panel (#957) * models panel * models panel * fix ui report * stupid scroll * move model selector to models panel * cleanup whitespaces * cleanup bad merge --------- Co-authored-by: DevTekVE --- .../qt/offroad/settings/models_panel.cc | 201 ++++++++++++++++++ .../qt/offroad/settings/models_panel.h | 47 ++++ .../qt/offroad/settings/software_panel.cc | 201 +----------------- .../qt/offroad/settings/software_panel.h | 44 ---- 4 files changed, 249 insertions(+), 244 deletions(-) diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.cc index 8f8d38d2d8..d33cb550ce 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.cc @@ -5,8 +5,209 @@ * See the LICENSE.md file in the root directory for more details. */ +#include +#include + +#include "common/model.h" #include "selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.h" +#include "selfdrive/ui/sunnypilot/qt/widgets/scrollview.h" ModelsPanel::ModelsPanel(QWidget *parent) : QWidget(parent) { + QVBoxLayout *main_layout = new QVBoxLayout(this); + main_layout->setContentsMargins(50, 20, 50, 20); + ListWidgetSP *list = new ListWidgetSP(this); + ScrollViewSP *scroller = new ScrollViewSP(list, this); + main_layout->addWidget(scroller); + + const auto current_model = GetActiveModelName(); + currentModelLblBtn = new ButtonControlSP(tr("Current Model"), tr("SELECT"), current_model); + currentModelLblBtn->setValue(current_model); + + connect(currentModelLblBtn, &ButtonControlSP::clicked, this, &ModelsPanel::handleCurrentModelLblBtnClicked); + connect(uiState(), &UIState::offroadTransition, [=](bool offroad) { + is_onroad = !offroad; + updateLabels(); + }); + connect(uiStateSP(), &UIStateSP::uiUpdate, this, &ModelsPanel::updateLabels); + list->addItem(currentModelLblBtn); +} + + +/** + * @brief Updates the UI with bundle download progress information + * Reads status from modelManagerSP cereal message and displays status for all models + */ +void ModelsPanel::handleBundleDownloadProgress() { + using DS = cereal::ModelManagerSP::DownloadStatus; + if (!model_manager.hasSelectedBundle() && !model_manager.hasActiveBundle()) { + currentModelLblBtn->setDescription(tr("No custom model selected!")); + return; + } + + const bool showSelectedBundle = model_manager.hasSelectedBundle() && (isDownloading() || model_manager.getSelectedBundle().getStatus() == DS::FAILED); + const auto &bundle = showSelectedBundle ? model_manager.getSelectedBundle() : model_manager.getActiveBundle(); + const auto &models = bundle.getModels(); + download_status = bundle.getStatus(); + const auto download_status_changed = prev_download_status != download_status; + QStringList status; + + // Get status for each model type in order + for (const auto &model: models) { + QString typeName; + QString modelName = QString::fromStdString(bundle.getDisplayName()); + + switch (model.getType()) { + case cereal::ModelManagerSP::Model::Type::SUPERCOMBO: + typeName = tr("Driving"); + break; + case cereal::ModelManagerSP::Model::Type::NAVIGATION: + typeName = tr("Navigation"); + break; + case cereal::ModelManagerSP::Model::Type::VISION: + typeName = tr("Vision"); + break; + case cereal::ModelManagerSP::Model::Type::POLICY: + typeName = tr("Policy"); + break; + } + + const auto &progress = model.getArtifact().getDownloadProgress(); + QString line; + + if (progress.getStatus() == cereal::ModelManagerSP::DownloadStatus::DOWNLOADING) { + line = tr("Downloading %1 model [%2]... (%3%)").arg(typeName, modelName).arg(progress.getProgress(), 0, 'f', 2); + } else if (progress.getStatus() == cereal::ModelManagerSP::DownloadStatus::DOWNLOADED) { + line = tr("%1 model [%2] %3").arg(typeName, modelName, download_status_changed ? tr("downloaded") : tr("ready")); + } else if (progress.getStatus() == cereal::ModelManagerSP::DownloadStatus::CACHED) { + line = tr("%1 model [%2] %3").arg(typeName, modelName, download_status_changed ? tr("from cache") : tr("ready")); + } else if (progress.getStatus() == cereal::ModelManagerSP::DownloadStatus::FAILED) { + line = tr("%1 model [%2] download failed").arg(typeName, modelName); + } else { + line = tr("%1 model [%2] pending...").arg(typeName, modelName); + } + status.append(line); + } + + currentModelLblBtn->setDescription(status.join("\n")); + + if (prev_download_status != download_status) { + switch (bundle.getStatus()) { + case cereal::ModelManagerSP::DownloadStatus::DOWNLOADING: + case cereal::ModelManagerSP::DownloadStatus::CACHED: + case cereal::ModelManagerSP::DownloadStatus::DOWNLOADED: + currentModelLblBtn->showDescription(); + break; + case cereal::ModelManagerSP::DownloadStatus::FAILED: + default: + break; + } + } + prev_download_status = download_status; +} + +/** + * @brief Gets the name of the currently selected model bundle + * @return Display name of the selected bundle or default model name + */ +QString ModelsPanel::GetActiveModelName() { + if (model_manager.hasActiveBundle()) { + return QString::fromStdString(model_manager.getActiveBundle().getDisplayName()); + } + + return DEFAULT_MODEL; +} + +void ModelsPanel::updateModelManagerState() { + const SubMaster &sm = *(uiStateSP()->sm); + model_manager = sm["modelManagerSP"].getModelManagerSP(); +} + +/** + * @brief Handles the model bundle selection button click + * Displays available bundles, allows selection, and initiates download + */ +void ModelsPanel::handleCurrentModelLblBtnClicked() { + currentModelLblBtn->setEnabled(false); + currentModelLblBtn->setValue(tr("Fetching models...")); + + // Create mapping of bundle indices to display names + QMap index_to_bundle; + const auto bundles = model_manager.getAvailableBundles(); + for (const auto &bundle: bundles) { + index_to_bundle.insert(bundle.getIndex(), QString::fromStdString(bundle.getDisplayName())); + } + + // Sort bundles by index in descending order + QStringList bundleNames; + // Add "Default" as the first option + bundleNames.append(tr("Use Default")); + + auto indices = index_to_bundle.keys(); + std::sort(indices.begin(), indices.end(), std::greater()); + for (const auto &index: indices) { + bundleNames.append(index_to_bundle[index]); + } + + currentModelLblBtn->setValue(GetActiveModelName()); + + const QString selectedBundleName = MultiOptionDialog::getSelection( + tr("Select a Model"), bundleNames, GetActiveModelName(), this); + + if (selectedBundleName.isEmpty() || !canContinueOnMeteredDialog()) { + return; + } + + // Handle "Stock" selection differently + if (selectedBundleName == tr("Use Default")) { + params.remove("ModelManager_ActiveBundle"); + currentModelLblBtn->setValue(tr("Default")); + showResetParamsDialog(); + } else { + // Find selected bundle and initiate download + for (const auto &bundle: bundles) { + if (QString::fromStdString(bundle.getDisplayName()) == selectedBundleName) { + params.put("ModelManager_DownloadIndex", std::to_string(bundle.getIndex())); + if (bundle.getGeneration() != model_manager.getActiveBundle().getGeneration()) { + showResetParamsDialog(); + } + break; + } + } + } + + updateLabels(); +} + +/** + * @brief Updates the UI elements based on current state + */ +void ModelsPanel::updateLabels() { + if (!isVisible()) { + return; + } + + updateModelManagerState(); + handleBundleDownloadProgress(); + currentModelLblBtn->setEnabled(!is_onroad && !isDownloading()); + currentModelLblBtn->setValue(GetActiveModelName()); +} + +/** + * @brief Shows dialog prompting user to reset calibration after model download + */ +void ModelsPanel::showResetParamsDialog() { + const auto confirmMsg = QString("%1

%2

%3") + .arg(tr("Model download has started in the background.")) + .arg(tr("We STRONGLY suggest you to reset calibration.")) + .arg(tr("Would you like to do that now?")); + const auto button_text = tr("Reset Calibration"); + + QString content("

" + tr("Driving Model Selector") + "


" + "

" + confirmMsg + "

"); + + if (showConfirmationDialog(content, button_text, false)) { + params.remove("CalibrationParams"); + params.remove("LiveTorqueParameters"); + } } diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.h index 6c28b5bf35..4ba526651e 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.h +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.h @@ -14,4 +14,51 @@ class ModelsPanel : public QWidget { public: explicit ModelsPanel(QWidget *parent = nullptr); + +private: + QString GetActiveModelName(); + void updateModelManagerState(); + + bool isDownloading() const { + if (!model_manager.hasSelectedBundle()) { + return false; + } + + const auto &selected_bundle = model_manager.getSelectedBundle(); + return selected_bundle.getStatus() == cereal::ModelManagerSP::DownloadStatus::DOWNLOADING; + } + + // UI update related methods + void updateLabels(); + void handleCurrentModelLblBtnClicked(); + void handleBundleDownloadProgress(); + void showResetParamsDialog(); + cereal::ModelManagerSP::Reader model_manager; + cereal::ModelManagerSP::DownloadStatus download_status{}; + cereal::ModelManagerSP::DownloadStatus prev_download_status{}; + + bool canContinueOnMeteredDialog() { + if (!is_metered) return true; + return showConfirmationDialog(QString(), QString(), is_metered); + } + + inline bool showConfirmationDialog(const QString &message = QString(), const QString &confirmButtonText = QString(), const bool show_metered_warning = false) { + return showConfirmationDialog(this, message, confirmButtonText, show_metered_warning); + } + + static inline bool showConfirmationDialog(QWidget *parent, const QString &message = QString(), const QString &confirmButtonText = QString(), const bool show_metered_warning = false) { + const QString warning_message = show_metered_warning ? tr("Warning: You are on a metered connection!") : QString(); + const QString final_message = QString("%1%2").arg(!message.isEmpty() ? message + "\n" : QString(), warning_message); + const QString final_buttonText = !confirmButtonText.isEmpty() ? confirmButtonText : QString(tr("Continue") + " %1").arg(show_metered_warning ? tr("on Metered") : ""); + + return ConfirmationDialog(final_message, final_buttonText, tr("Cancel"), true, parent).exec(); + } + + bool is_metered{}; + bool is_wifi{}; + bool is_onroad = false; + + ButtonControlSP *currentModelLblBtn; + Params params; + }; diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/software_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/software_panel.cc index 57fb38c20b..9430d02ba8 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/software_panel.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/software_panel.cc @@ -7,18 +7,8 @@ #include "selfdrive/ui/sunnypilot/qt/offroad/settings/software_panel.h" -#include -#include - -#include "common/model.h" - -/** - * @brief Constructs the software panel with model bundle selection functionality - * @param parent Parent widget - */ SoftwarePanelSP::SoftwarePanelSP(QWidget *parent) : SoftwarePanel(parent) { - -// branch selector + // branch selector QObject::disconnect(targetBranchBtn, nullptr, nullptr, nullptr); connect(targetBranchBtn, &ButtonControlSP::clicked, [=]() { InputDialog d(tr("Search Branch"), this, tr("Enter search keywords, or leave blank to list all branches."), false); @@ -27,196 +17,7 @@ SoftwarePanelSP::SoftwarePanelSP(QWidget *parent) : SoftwarePanel(parent) { if (ret) { searchBranches(d.text()); } - }); - - const auto current_model = GetActiveModelName(); - currentModelLblBtn = new ButtonControlSP(tr("Current Model"), tr("SELECT"), current_model); - currentModelLblBtn->setValue(current_model); - - connect(currentModelLblBtn, &ButtonControlSP::clicked, this, &SoftwarePanelSP::handleCurrentModelLblBtnClicked); - QObject::connect(uiStateSP(), &UIStateSP::uiUpdate, this, &SoftwarePanelSP::updateLabels); - AddWidgetAt(0, currentModelLblBtn); -} - -/** - * @brief Updates the UI with bundle download progress information - * Reads status from modelManagerSP cereal message and displays status for all models - */ -void SoftwarePanelSP::handleBundleDownloadProgress() { - using DS = cereal::ModelManagerSP::DownloadStatus; - if (!model_manager.hasSelectedBundle() && !model_manager.hasActiveBundle()) { - currentModelLblBtn->setDescription(tr("No custom model selected!")); - return; - } - - const bool showSelectedBundle = model_manager.hasSelectedBundle() && (isDownloading() || model_manager.getSelectedBundle().getStatus() == DS::FAILED); - const auto &bundle = showSelectedBundle ? model_manager.getSelectedBundle() : model_manager.getActiveBundle(); - const auto &models = bundle.getModels(); - download_status = bundle.getStatus(); - const auto download_status_changed = prev_download_status != download_status; - QStringList status; - - // Get status for each model type in order - for (const auto &model: models) { - QString typeName; - QString modelName = QString::fromStdString(bundle.getDisplayName()); - - switch (model.getType()) { - case cereal::ModelManagerSP::Model::Type::SUPERCOMBO: - typeName = tr("Driving"); - break; - case cereal::ModelManagerSP::Model::Type::NAVIGATION: - typeName = tr("Navigation"); - break; - case cereal::ModelManagerSP::Model::Type::VISION: - typeName = tr("Vision"); - break; - case cereal::ModelManagerSP::Model::Type::POLICY: - typeName = tr("Policy"); - break; - } - - const auto &progress = model.getArtifact().getDownloadProgress(); - QString line; - - if (progress.getStatus() == cereal::ModelManagerSP::DownloadStatus::DOWNLOADING) { - line = tr("Downloading %1 model [%2]... (%3%)").arg(typeName, modelName).arg(progress.getProgress(), 0, 'f', 2); - } else if (progress.getStatus() == cereal::ModelManagerSP::DownloadStatus::DOWNLOADED) { - line = tr("%1 model [%2] %3").arg(typeName, modelName, download_status_changed ? tr("downloaded") : tr("ready")); - } else if (progress.getStatus() == cereal::ModelManagerSP::DownloadStatus::CACHED) { - line = tr("%1 model [%2] %3").arg(typeName, modelName, download_status_changed ? tr("from cache") : tr("ready")); - } else if (progress.getStatus() == cereal::ModelManagerSP::DownloadStatus::FAILED) { - line = tr("%1 model [%2] download failed").arg(typeName, modelName); - } else { - line = tr("%1 model [%2] pending...").arg(typeName, modelName); - } - status.append(line); - } - - currentModelLblBtn->setDescription(status.join("\n")); - - if (prev_download_status != download_status) { - switch (bundle.getStatus()) { - case cereal::ModelManagerSP::DownloadStatus::DOWNLOADING: - case cereal::ModelManagerSP::DownloadStatus::CACHED: - case cereal::ModelManagerSP::DownloadStatus::DOWNLOADED: - currentModelLblBtn->showDescription(); - break; - case cereal::ModelManagerSP::DownloadStatus::FAILED: - default: - break; - } - } - prev_download_status = download_status; -} - -/** - * @brief Gets the name of the currently selected model bundle - * @return Display name of the selected bundle or default model name - */ -QString SoftwarePanelSP::GetActiveModelName() { - if (model_manager.hasActiveBundle()) { - return QString::fromStdString(model_manager.getActiveBundle().getDisplayName()); - } - - return DEFAULT_MODEL; -} - -void SoftwarePanelSP::updateModelManagerState() { - const SubMaster &sm = *(uiStateSP()->sm); - model_manager = sm["modelManagerSP"].getModelManagerSP(); -} - -/** - * @brief Handles the model bundle selection button click - * Displays available bundles, allows selection, and initiates download - */ -void SoftwarePanelSP::handleCurrentModelLblBtnClicked() { - currentModelLblBtn->setEnabled(false); - currentModelLblBtn->setValue(tr("Fetching models...")); - - // Create mapping of bundle indices to display names - QMap index_to_bundle; - const auto bundles = model_manager.getAvailableBundles(); - for (const auto &bundle: bundles) { - index_to_bundle.insert(bundle.getIndex(), QString::fromStdString(bundle.getDisplayName())); - } - - // Sort bundles by index in descending order - QStringList bundleNames; - // Add "Default" as the first option - bundleNames.append(tr("Use Default")); - - auto indices = index_to_bundle.keys(); - std::sort(indices.begin(), indices.end(), std::greater()); - for (const auto &index: indices) { - bundleNames.append(index_to_bundle[index]); - } - - currentModelLblBtn->setValue(GetActiveModelName()); - - const QString selectedBundleName = MultiOptionDialog::getSelection( - tr("Select a Model"), bundleNames, GetActiveModelName(), this); - - if (selectedBundleName.isEmpty() || !canContinueOnMeteredDialog()) { - return; - } - - // Handle "Stock" selection differently - if (selectedBundleName == tr("Use Default")) { - params.remove("ModelManager_ActiveBundle"); - currentModelLblBtn->setValue(tr("Default")); - showResetParamsDialog(); - } else { - // Find selected bundle and initiate download - for (const auto &bundle: bundles) { - if (QString::fromStdString(bundle.getDisplayName()) == selectedBundleName) { - params.put("ModelManager_DownloadIndex", std::to_string(bundle.getIndex())); - if (bundle.getGeneration() != model_manager.getActiveBundle().getGeneration()) { - showResetParamsDialog(); - } - break; - } - } - } - - updateLabels(); -} - -/** - * @brief Updates the UI elements based on current state - */ -void SoftwarePanelSP::updateLabels() { - if (!isVisible()) { - return; - } - - updateModelManagerState(); - handleBundleDownloadProgress(); - currentModelLblBtn->setEnabled(!is_onroad && !isDownloading()); - currentModelLblBtn->setValue(GetActiveModelName()); - - SoftwarePanel::updateLabels(); -} - -/** - * @brief Shows dialog prompting user to reset calibration after model download - */ -void SoftwarePanelSP::showResetParamsDialog() { - const auto confirmMsg = QString("%1

%2

%3") - .arg(tr("Model download has started in the background.")) - .arg(tr("We STRONGLY suggest you to reset calibration.")) - .arg(tr("Would you like to do that now?")); - const auto button_text = tr("Reset Calibration"); - - QString content("

" + tr("Driving Model Selector") + "


" - "

" + confirmMsg + "

"); - - if (showConfirmationDialog(content, button_text, false)) { - params.remove("CalibrationParams"); - params.remove("LiveTorqueParameters"); - } } /** diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/software_panel.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/software_panel.h index 3679ade410..56d8b9be5b 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/software_panel.h +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/software_panel.h @@ -7,7 +7,6 @@ #pragma once -#include #include "selfdrive/ui/sunnypilot/ui.h" #include "selfdrive/ui/sunnypilot/qt/util.h" #include "selfdrive/ui/qt/offroad/settings.h" @@ -20,47 +19,4 @@ public: private: void searchBranches(const QString &query); - -private: - QString GetActiveModelName(); - void updateModelManagerState(); - - bool isDownloading() const { - if (!model_manager.hasSelectedBundle()) { - return false; - } - - const auto &selected_bundle = model_manager.getSelectedBundle(); - return selected_bundle.getStatus() == cereal::ModelManagerSP::DownloadStatus::DOWNLOADING; - } - - // UI update related methods - void updateLabels() override; - void handleCurrentModelLblBtnClicked(); - void handleBundleDownloadProgress(); - void showResetParamsDialog(); - cereal::ModelManagerSP::Reader model_manager; - cereal::ModelManagerSP::DownloadStatus download_status{}; - cereal::ModelManagerSP::DownloadStatus prev_download_status{}; - - bool canContinueOnMeteredDialog() { - if (!is_metered) return true; - return showConfirmationDialog(QString(), QString(), is_metered); - } - - inline bool showConfirmationDialog(const QString &message = QString(), const QString &confirmButtonText = QString(), const bool show_metered_warning = false) { - return showConfirmationDialog(this, message, confirmButtonText, show_metered_warning); - } - - static inline bool showConfirmationDialog(QWidget *parent, const QString &message = QString(), const QString &confirmButtonText = QString(), const bool show_metered_warning = false) { - const QString warning_message = show_metered_warning ? tr("Warning: You are on a metered connection!") : QString(); - const QString final_message = QString("%1%2").arg(!message.isEmpty() ? message + "\n" : QString(), warning_message); - const QString final_buttonText = !confirmButtonText.isEmpty() ? confirmButtonText : QString(tr("Continue") + " %1").arg(show_metered_warning ? tr("on Metered") : ""); - - return ConfirmationDialog(final_message, final_buttonText, tr("Cancel"), true, parent).exec(); - } - - bool is_metered{}; - bool is_wifi{}; - ButtonControlSP *currentModelLblBtn; }; From ff5b587fd17dee1070086af8a62da42bc33302b6 Mon Sep 17 00:00:00 2001 From: DevTekVE Date: Fri, 30 May 2025 13:14:30 +0200 Subject: [PATCH 41/60] ui: Bumping the total scons nodes for progress bar (#965) Bumping the total scons nodes --- system/manager/build.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/system/manager/build.py b/system/manager/build.py index 771024794f..49c8128660 100755 --- a/system/manager/build.py +++ b/system/manager/build.py @@ -14,7 +14,7 @@ from openpilot.system.version import get_build_metadata MAX_CACHE_SIZE = 4e9 if "CI" in os.environ else 2e9 CACHE_DIR = Path("/data/scons_cache" if AGNOS else "/tmp/scons_cache") -TOTAL_SCONS_NODES = 3130 +TOTAL_SCONS_NODES = 3765 MAX_BUILD_PROGRESS = 100 def build(spinner: Spinner, dirty: bool = False, minimal: bool = False) -> None: From e42044b8339c04bbe609a70e7b8a9a4f25a20597 Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Fri, 30 May 2025 22:36:05 +0800 Subject: [PATCH 42/60] system/ui: fix lint error (#35387) fix lint error --- system/ui/onroad/model_renderer.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/system/ui/onroad/model_renderer.py b/system/ui/onroad/model_renderer.py index 141ffa3110..ea5363db79 100644 --- a/system/ui/onroad/model_renderer.py +++ b/system/ui/onroad/model_renderer.py @@ -313,7 +313,7 @@ class ModelRenderer: line_y = line.y line_z = line.z - points = [] + points: list = [] for i in range(max_idx + 1): # Skip points with negative x (behind camera) From b8f3e7bcf089959f788f024d1772a5575dc97291 Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Fri, 30 May 2025 22:37:07 +0800 Subject: [PATCH 43/60] system/ui: improve road view with driving state border and clipping (#35385) improve road view with driving state border and clipping --- system/ui/onroad/augmented_road_view.py | 54 ++++++++++++++++++++++--- 1 file changed, 49 insertions(+), 5 deletions(-) diff --git a/system/ui/onroad/augmented_road_view.py b/system/ui/onroad/augmented_road_view.py index 316167a828..b4b6eef12c 100644 --- a/system/ui/onroad/augmented_road_view.py +++ b/system/ui/onroad/augmented_road_view.py @@ -1,5 +1,6 @@ import numpy as np import pyray as rl +from enum import Enum from cereal import messaging, log from msgq.visionipc import VisionStreamType @@ -10,8 +11,15 @@ from openpilot.common.transformations.camera import DEVICE_CAMERAS, DeviceCamera from openpilot.common.transformations.orientation import rot_from_euler +OpState = log.SelfdriveState.OpenpilotState CALIBRATED = log.LiveCalibrationData.Status.calibrated DEFAULT_DEVICE_CAMERA = DEVICE_CAMERAS["tici", "ar0231"] +UI_BORDER_SIZE = 30 + +class BorderStatus(Enum): + DISENGAGED = rl.Color(0x17, 0x33, 0x49, 0xc8) # Blue for disengaged state + OVERRIDE = rl.Color(0x91, 0x9b, 0x95, 0xf1) # Gray for override state + ENGAGED = rl.Color(0x17, 0x86, 0x44, 0xf1) # Green for engaged state class AugmentedRoadView(CameraView): @@ -29,6 +37,7 @@ class AugmentedRoadView(CameraView): self._last_calib_time: float = 0 self._last_rect_dims = (0.0, 0.0) self._cached_matrix: np.ndarray | None = None + self._content_rect = rl.Rectangle() self.model_renderer = ModelRenderer() @@ -36,6 +45,26 @@ class AugmentedRoadView(CameraView): # Update calibration before rendering self._update_calibration() + # Create inner content area with border padding + self._content_rect = rl.Rectangle( + rect.x + UI_BORDER_SIZE, + rect.y + UI_BORDER_SIZE, + rect.width - 2 * UI_BORDER_SIZE, + rect.height - 2 * UI_BORDER_SIZE, + ) + + # Draw colored border based on driving state + self._draw_border(rect) + + # Enable scissor mode to clip all rendering within content rectangle boundaries + # This creates a rendering viewport that prevents graphics from drawing outside the border + rl.begin_scissor_mode( + int(self._content_rect.x), + int(self._content_rect.y), + int(self._content_rect.width), + int(self._content_rect.height) + ) + # Render the base camera view super().render(rect) @@ -44,7 +73,21 @@ class AugmentedRoadView(CameraView): # - Path prediction # - Lead vehicle indicators # - Additional features - self.model_renderer.draw(rect, self.sm) + self.model_renderer.draw(self._content_rect, self.sm) + + # End clipping region + rl.end_scissor_mode() + + def _draw_border(self, rect: rl.Rectangle): + state = self.sm["selfdriveState"] + if state.state in (OpState.preEnabled, OpState.overriding): + status = BorderStatus.OVERRIDE + elif state.enabled: + status = BorderStatus.ENGAGED + else: + status = BorderStatus.DISENGAGED + + rl.draw_rectangle_lines_ex(rect, UI_BORDER_SIZE, status.value) def _update_calibration(self): # Update device camera if not already set @@ -71,7 +114,7 @@ class AugmentedRoadView(CameraView): def _calc_frame_matrix(self, rect: rl.Rectangle) -> np.ndarray: # Check if we can use cached matrix calib_time = self.sm.recv_frame.get('liveCalibration', 0) - current_dims = (rect.width, rect.height) + current_dims = (self._content_rect.width, self._content_rect.height) if (self._last_calib_time == calib_time and self._last_rect_dims == current_dims and self._cached_matrix is not None): @@ -89,7 +132,8 @@ class AugmentedRoadView(CameraView): kep = calib_transform @ inf_point # Calculate center points and dimensions - w, h = current_dims + x, y = self._content_rect.x, self._content_rect.y + w, h = self._content_rect.width, self._content_rect.height cx, cy = intrinsic[0, 2], intrinsic[1, 2] # Calculate max allowed offsets with margins @@ -117,8 +161,8 @@ class AugmentedRoadView(CameraView): ]) video_transform = np.array([ - [zoom, 0.0, (w / 2 - x_offset) - (cx * zoom)], - [0.0, zoom, (h / 2 - y_offset) - (cy * zoom)], + [zoom, 0.0, (w / 2 + x - x_offset) - (cx * zoom)], + [0.0, zoom, (h / 2 + y - y_offset) - (cy * zoom)], [0.0, 0.0, 1.0] ]) self.model_renderer.set_transform(video_transform @ calib_transform) From 29010cae23db80405a0ce4e3d773d4bb9569c198 Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Sat, 31 May 2025 00:34:56 +0800 Subject: [PATCH 44/60] system/ui: optimize ModelRenderer (#35369) * optimize ModelRenderer with vectorized operations * pre-calculate the exp mode colors * cleanup * improve batch map line to polygon * pre-calc leads --- system/ui/onroad/model_renderer.py | 407 ++++++++++++++++------------- 1 file changed, 224 insertions(+), 183 deletions(-) diff --git a/system/ui/onroad/model_renderer.py b/system/ui/onroad/model_renderer.py index ea5363db79..faf538c58d 100644 --- a/system/ui/onroad/model_renderer.py +++ b/system/ui/onroad/model_renderer.py @@ -1,8 +1,8 @@ import colorsys -import bisect import numpy as np import pyray as rl from cereal import messaging, car +from dataclasses import dataclass, field from openpilot.common.params import Params from openpilot.system.ui.lib.application import DEFAULT_FPS from openpilot.system.ui.lib.shader_polygon import draw_polygon @@ -27,6 +27,18 @@ NO_THROTTLE_COLORS = [ ] +@dataclass +class ModelPoints: + raw_points: np.ndarray = field(default_factory=lambda: np.empty((0, 3), dtype=np.float32)) + projected_points: np.ndarray = field(default_factory=lambda: np.empty((0, 2), dtype=np.float32)) + +@dataclass +class LeadVehicle: + glow: list[float] = field(default_factory=list) + chevron: list[float] = field(default_factory=list) + fill_alpha: int = 0 + + class ModelRenderer: def __init__(self): self._longitudinal_control = False @@ -35,28 +47,34 @@ class ModelRenderer: self._prev_allow_throttle = True self._lane_line_probs = np.zeros(4, dtype=np.float32) self._road_edge_stds = np.zeros(2, dtype=np.float32) + self._lead_vehicles = [LeadVehicle(), LeadVehicle()] self._path_offset_z = 1.22 - # Initialize empty polygon vertices - self._track_vertices = np.empty((0, 2), dtype=np.float32) - self._lane_line_vertices = [np.empty((0, 2), dtype=np.float32) for _ in range(4)] - self._road_edge_vertices = [np.empty((0, 2), dtype=np.float32) for _ in range(2)] - self._lead_vertices = [None, None] + # Initialize ModelPoints objects + self._path = ModelPoints() + self._lane_lines = [ModelPoints() for _ in range(4)] + self._road_edges = [ModelPoints() for _ in range(2)] + self._acceleration_x = np.empty((0,), dtype=np.float32) # Transform matrix (3x3 for car space to screen space) - self._car_space_transform = np.zeros((3, 3)) + self._car_space_transform = np.zeros((3, 3), dtype=np.float32) self._transform_dirty = True self._clip_region = None self._rect = None + self._exp_gradient = { + 'start': (0.0, 1.0), # Bottom of path + 'end': (0.0, 0.0), # Top of path + 'colors': [], + 'stops': [], + } # Get longitudinal control setting from car parameters - car_params = Params().get("CarParams") - if car_params: + if car_params := Params().get("CarParams"): cp = messaging.log_from_bytes(car_params, car.CarParams) self._longitudinal_control = cp.openpilotLongitudinalControl def set_transform(self, transform: np.ndarray): - self._car_space_transform = transform + self._car_space_transform = transform.astype(np.float32) self._transform_dirty = True def draw(self, rect: rl.Rectangle, sm: messaging.SubMaster): @@ -70,161 +88,189 @@ class ModelRenderer: rect.x - CLIP_MARGIN, rect.y - CLIP_MARGIN, rect.width + 2 * CLIP_MARGIN, rect.height + 2 * CLIP_MARGIN ) - # Update flags based on car state + # Update state self._experimental_mode = sm['selfdriveState'].experimentalMode self._path_offset_z = sm['liveCalibration'].height[0] if sm.updated['carParams']: self._longitudinal_control = sm['carParams'].openpilotLongitudinalControl - # Get model and radar data model = sm['modelV2'] radar_state = sm['radarState'] if sm.valid['radarState'] else None lead_one = radar_state.leadOne if radar_state else None render_lead_indicator = self._longitudinal_control and radar_state is not None # Update model data when needed - if self._transform_dirty or sm.updated['modelV2'] or sm.updated['radarState']: - self._update_model(model, lead_one) + model_updated = sm.updated['modelV2'] + if model_updated or sm.updated['radarState'] or self._transform_dirty: + if model_updated: + self._update_raw_points(model) + + pos_x_array = self._path.raw_points[:, 0] + if pos_x_array.size == 0: + return + + self._update_model(lead_one, pos_x_array) if render_lead_indicator: - self._update_leads(radar_state, model.position) + self._update_leads(radar_state, pos_x_array) self._transform_dirty = False + # Draw elements self._draw_lane_lines() - self._draw_path(sm, model, rect.height) + self._draw_path(sm) - # Draw lead vehicles if available if render_lead_indicator and radar_state: - lead_two = radar_state.leadTwo + self._draw_lead_indicator() - if lead_one and lead_one.status: - self._draw_lead(lead_one, self._lead_vertices[0], rect) + def _update_raw_points(self, model): + """Update raw 3D points from model data""" + self._path.raw_points = np.array([model.position.x, model.position.y, model.position.z], dtype=np.float32).T - if lead_two and lead_two.status and lead_one and (abs(lead_one.dRel - lead_two.dRel) > 3.0): - self._draw_lead(lead_two, self._lead_vertices[1], rect) + for i, lane_line in enumerate(model.laneLines): + self._lane_lines[i].raw_points = np.array([lane_line.x, lane_line.y, lane_line.z], dtype=np.float32).T - def _update_leads(self, radar_state, line): + for i, road_edge in enumerate(model.roadEdges): + self._road_edges[i].raw_points = np.array([road_edge.x, road_edge.y, road_edge.z], dtype=np.float32).T + + self._lane_line_probs = np.array(model.laneLineProbs, dtype=np.float32) + self._road_edge_stds = np.array(model.roadEdgeStds, dtype=np.float32) + self._acceleration_x = np.array(model.acceleration.x, dtype=np.float32) + + def _update_leads(self, radar_state, pos_x_array): """Update positions of lead vehicles""" + self._lead_vehicles = [LeadVehicle(), LeadVehicle()] leads = [radar_state.leadOne, radar_state.leadTwo] for i, lead_data in enumerate(leads): if lead_data and lead_data.status: - d_rel = lead_data.dRel - y_rel = lead_data.yRel - idx = self._get_path_length_idx(line, d_rel) - z = line.z[idx] - self._lead_vertices[i] = self._map_to_screen(d_rel, -y_rel, z + self._path_offset_z) + d_rel, y_rel, v_rel = lead_data.dRel, lead_data.yRel, lead_data.vRel + idx = self._get_path_length_idx(pos_x_array, d_rel) + z = self._path.raw_points[idx, 2] if idx < len(self._path.raw_points) else 0.0 + point = self._map_to_screen(d_rel, -y_rel, z + self._path_offset_z) + if point: + self._lead_vehicles[i] = self._update_lead_vehicle(d_rel, v_rel, point, self._rect) - def _update_model(self, model, lead): + def _update_model(self, lead, pos_x_array): """Update model visualization data based on model message""" - model_position = model.position + max_distance = np.clip(pos_x_array[-1], MIN_DRAW_DISTANCE, MAX_DRAW_DISTANCE) + max_idx = self._get_path_length_idx(pos_x_array, max_distance) - # Determine max distance to render - max_distance = np.clip(model_position.x[-1], MIN_DRAW_DISTANCE, MAX_DRAW_DISTANCE) - - # Update lane lines - lane_lines = model.laneLines - line_probs = model.laneLineProbs - max_idx = self._get_path_length_idx(lane_lines[0], max_distance) - - for i in range(4): - self._lane_line_probs[i] = line_probs[i] - self._lane_line_vertices[i] = self._map_line_to_polygon( - lane_lines[i], 0.025 * self._lane_line_probs[i], 0, max_idx + # Update lane lines using raw points + for i, lane_line in enumerate(self._lane_lines): + lane_line.projected_points = self._map_line_to_polygon( + lane_line.raw_points, 0.025 * self._lane_line_probs[i], 0.0, max_idx ) - # Update road edges - road_edges = model.roadEdges - edge_stds = model.roadEdgeStds + # Update road edges using raw points + for road_edge in self._road_edges: + road_edge.projected_points = self._map_line_to_polygon(road_edge.raw_points, 0.025, 0.0, max_idx) - for i in range(2): - self._road_edge_stds[i] = edge_stds[i] - self._road_edge_vertices[i] = self._map_line_to_polygon(road_edges[i], 0.025, 0, max_idx) - - # Update path + # Update path using raw points if lead and lead.status: lead_d = lead.dRel * 2.0 max_distance = np.clip(lead_d - min(lead_d * 0.35, 10.0), 0.0, max_distance) - max_idx = self._get_path_length_idx(model_position, max_distance) + max_idx = self._get_path_length_idx(pos_x_array, max_distance) - self._track_vertices = self._map_line_to_polygon(model_position, 0.9, self._path_offset_z, max_idx, False) + self._path.projected_points = self._map_line_to_polygon( + self._path.raw_points, 0.9, self._path_offset_z, max_idx, allow_invert=False + ) + + self._update_experimental_gradient(self._rect.height) + + def _update_experimental_gradient(self, height): + """Pre-calculate experimental mode gradient colors""" + if not self._experimental_mode: + return + + max_len = min(len(self._path.projected_points) // 2, len(self._acceleration_x)) + + segment_colors = [] + gradient_stops = [] + + i = 0 + while i < max_len: + track_idx = max_len - i - 1 # flip idx to start from bottom right + track_y = self._path.projected_points[track_idx][1] + if track_y < 0 or track_y > height: + i += 1 + continue + + # Calculate color based on acceleration + lin_grad_point = (height - track_y) / height + + # speed up: 120, slow down: 0 + path_hue = max(min(60 + self._acceleration_x[i] * 35, 120), 0) + path_hue = int(path_hue * 100 + 0.5) / 100 + + saturation = min(abs(self._acceleration_x[i] * 1.5), 1) + lightness = self._map_val(saturation, 0.0, 1.0, 0.95, 0.62) + alpha = self._map_val(lin_grad_point, 0.75 / 2.0, 0.75, 0.4, 0.0) + + # Use HSL to RGB conversion + color = self._hsla_to_color(path_hue / 360.0, saturation, lightness, alpha) + + gradient_stops.append(lin_grad_point) + segment_colors.append(color) + + # Skip a point, unless next is last + i += 1 + (1 if (i + 2) < max_len else 0) + + # Store the gradient in the path object + self._exp_gradient['colors'] = segment_colors + self._exp_gradient['stops'] = gradient_stops + + def _update_lead_vehicle(self, d_rel, v_rel, point, rect): + speed_buff, lead_buff = 10.0, 40.0 + + # Calculate fill alpha + fill_alpha = 0 + if d_rel < lead_buff: + fill_alpha = 255 * (1.0 - (d_rel / lead_buff)) + if v_rel < 0: + fill_alpha += 255 * (-1 * (v_rel / speed_buff)) + fill_alpha = min(fill_alpha, 255) + + # Calculate size and position + sz = np.clip((25 * 30) / (d_rel / 3 + 30), 15.0, 30.0) * 2.35 + x = np.clip(point[0], 0.0, rect.width - sz / 2) + y = min(point[1], rect.height - sz * 0.6) + + g_xo = sz / 5 + g_yo = sz / 10 + + glow = [(x + (sz * 1.35) + g_xo, y + sz + g_yo), (x, y - g_yo), (x - (sz * 1.35) - g_xo, y + sz + g_yo)] + chevron = [(x + (sz * 1.25), y + sz), (x, y), (x - (sz * 1.25), y + sz)] + + return LeadVehicle(glow=glow,chevron=chevron, fill_alpha=int(fill_alpha)) def _draw_lane_lines(self): """Draw lane lines and road edges""" - for i, vertices in enumerate(self._lane_line_vertices): - # Skip if no vertices - if vertices.size == 0: + for i, lane_line in enumerate(self._lane_lines): + if lane_line.projected_points.size == 0: continue - # Draw lane line alpha = np.clip(self._lane_line_probs[i], 0.0, 0.7) color = rl.Color(255, 255, 255, int(alpha * 255)) - draw_polygon(self._rect, vertices, color) + draw_polygon(self._rect, lane_line.projected_points, color) - for i, vertices in enumerate(self._road_edge_vertices): - # Skip if no vertices - if vertices.size == 0: + for i, road_edge in enumerate(self._road_edges): + if road_edge.projected_points.size == 0: continue - # Draw road edge alpha = np.clip(1.0 - self._road_edge_stds[i], 0.0, 1.0) color = rl.Color(255, 0, 0, int(alpha * 255)) - draw_polygon(self._rect, vertices, color) + draw_polygon(self._rect, road_edge.projected_points, color) - def _draw_path(self, sm, model, height): - """Draw the path polygon with gradient based on acceleration""" - if self._track_vertices.size == 0: + def _draw_path(self, sm): + """Draw path with dynamic coloring based on mode and throttle state.""" + if not self._path.projected_points.size: return if self._experimental_mode: # Draw with acceleration coloring - acceleration = model.acceleration.x - max_len = min(len(self._track_vertices) // 2, len(acceleration)) - - # Create segments for gradient coloring - segment_colors = [] - gradient_stops = [] - - i = 0 - while i < max_len: - track_idx = max_len - i - 1 # flip idx to start from bottom right - track_y = self._track_vertices[track_idx][1] - if track_y < 0 or track_y > height: - i += 1 - continue - - # Calculate color based on acceleration - lin_grad_point = (height - track_y) / height - - # speed up: 120, slow down: 0 - path_hue = max(min(60 + acceleration[i] * 35, 120), 0) - path_hue = int(path_hue * 100 + 0.5) / 100 - - saturation = min(abs(acceleration[i] * 1.5), 1) - lightness = self._map_val(saturation, 0.0, 1.0, 0.95, 0.62) - alpha = self._map_val(lin_grad_point, 0.75 / 2.0, 0.75, 0.4, 0.0) - - # Use HSL to RGB conversion - color = self._hsla_to_color(path_hue / 360.0, saturation, lightness, alpha) - - # Create quad segment - gradient_stops.append(lin_grad_point) - segment_colors.append(color) - - # Skip a point, unless next is last - i += 1 + (1 if (i + 2) < max_len else 0) - - if len(segment_colors) < 2: - draw_polygon(self._rect, self._track_vertices, rl.Color(255, 255, 255, 30)) - return - - # Create gradient specification - gradient = { - 'start': (0.0, 1.0), # Bottom of path - 'end': (0.0, 0.0), # Top of path - 'colors': segment_colors, - 'stops': gradient_stops, - } - draw_polygon(self._rect, self._track_vertices, gradient=gradient) + if len(self._exp_gradient['colors']) > 2: + draw_polygon(self._rect, self._path.projected_points, gradient=self._exp_gradient) + else: + draw_polygon(self._rect, self._path.projected_points, rl.Color(255, 255, 255, 30)) else: # Draw with throttle/no throttle gradient allow_throttle = sm['longitudinalPlan'].allowThrottle or not self._longitudinal_control @@ -249,46 +295,22 @@ class ModelRenderer: 'colors': blended_colors, 'stops': [0.0, 0.5, 1.0], } - draw_polygon(self._rect, self._track_vertices, gradient=gradient) + draw_polygon(self._rect, self._path.projected_points, gradient=gradient) - def _draw_lead(self, lead_data, vd, rect): - """Draw lead vehicle indicator""" - if not vd: - return + def _draw_lead_indicator(self): + # Draw lead vehicles if available + for lead in self._lead_vehicles: + if not lead.glow or not lead.chevron: + continue - speed_buff = 10.0 - lead_buff = 40.0 - d_rel = lead_data.dRel - v_rel = lead_data.vRel - - # Calculate fill alpha - fill_alpha = 0 - if d_rel < lead_buff: - fill_alpha = 255 * (1.0 - (d_rel / lead_buff)) - if v_rel < 0: - fill_alpha += 255 * (-1 * (v_rel / speed_buff)) - fill_alpha = min(fill_alpha, 255) - - # Calculate size and position - sz = np.clip((25 * 30) / (d_rel / 3 + 30), 15.0, 30.0) * 2.35 - x = np.clip(vd[0], 0.0, rect.width - sz / 2) - y = min(vd[1], rect.height - sz * 0.6) - - g_xo = sz / 5 - g_yo = sz / 10 - - # Draw glow - glow = [(x + (sz * 1.35) + g_xo, y + sz + g_yo), (x, y - g_yo), (x - (sz * 1.35) - g_xo, y + sz + g_yo)] - rl.draw_triangle_fan(glow, len(glow), rl.Color(218, 202, 37, 255)) - - # Draw chevron - chevron = [(x + (sz * 1.25), y + sz), (x, y), (x - (sz * 1.25), y + sz)] - rl.draw_triangle_fan(chevron, len(chevron), rl.Color(201, 34, 49, int(fill_alpha))) + rl.draw_triangle_fan(lead.glow, len(lead.glow), rl.Color(218, 202, 37, 255)) + rl.draw_triangle_fan(lead.chevron, len(lead.chevron), rl.Color(201, 34, 49, lead.fill_alpha)) @staticmethod - def _get_path_length_idx(line, path_height): + def _get_path_length_idx(pos_x_array: np.ndarray, path_height: float) -> int: """Get the index corresponding to the given path height""" - return bisect.bisect_right(line.x, path_height) - 1 + idx = np.searchsorted(pos_x_array, path_height, side='right') + return int(np.clip(idx - 1, 0, len(pos_x_array) - 1)) def _map_to_screen(self, in_x, in_y, in_z): """Project a point in car space to screen space""" @@ -298,43 +320,71 @@ class ModelRenderer: if abs(pt[2]) < 1e-6: return None - x = pt[0] / pt[2] - y = pt[1] / pt[2] + x, y = pt[0] / pt[2], pt[1] / pt[2] clip = self._clip_region - if x < clip.x or x > clip.x + clip.width or y < clip.y or y > clip.y + clip.height: + if not (clip.x <= x <= clip.x + clip.width and clip.y <= y <= clip.y + clip.height): return None return (x, y) - def _map_line_to_polygon(self, line, y_off, z_off, max_idx, allow_invert=True)-> np.ndarray: - """Convert a 3D line to a 2D polygon for drawing""" - line_x = line.x - line_y = line.y - line_z = line.z + def _map_line_to_polygon(self, line: np.ndarray, y_off: float, z_off: float, max_idx: int, allow_invert: bool = True) -> np.ndarray: + """Convert 3D line to 2D polygon for rendering.""" + if line.shape[0] == 0: + return np.empty((0, 2), dtype=np.float32) - points: list = [] + # Slice points and filter non-negative x-coordinates + points = line[:max_idx + 1][line[:max_idx + 1, 0] >= 0] + if points.shape[0] == 0: + return np.empty((0, 2), dtype=np.float32) - for i in range(max_idx + 1): - # Skip points with negative x (behind camera) - if line_x[i] < 0: - continue + # Create left and right 3D points in one array + n_points = points.shape[0] + points_3d = np.empty((n_points * 2, 3), dtype=np.float32) + points_3d[:n_points, 0] = points_3d[n_points:, 0] = points[:, 0] + points_3d[:n_points, 1] = points[:, 1] - y_off + points_3d[n_points:, 1] = points[:, 1] + y_off + points_3d[:n_points, 2] = points_3d[n_points:, 2] = points[:, 2] + z_off - left = self._map_to_screen(line_x[i], line_y[i] - y_off, line_z[i] + z_off) - right = self._map_to_screen(line_x[i], line_y[i] + y_off, line_z[i] + z_off) + # Single matrix multiplication for projections + proj = self._car_space_transform @ points_3d.T + valid_z = np.abs(proj[2]) > 1e-6 + if not np.any(valid_z): + return np.empty((0, 2), dtype=np.float32) - if left and right: - # Check for inversion when going over hills - if not allow_invert and points and left[1] > points[-1][1]: - continue + # Compute screen coordinates + screen = proj[:2, valid_z] / proj[2, valid_z][None, :] + left_screen = screen[:, :n_points].T + right_screen = screen[:, n_points:].T - points.append(left) - points.insert(0, right) + # Ensure consistent shapes by re-aligning valid points + valid_points = np.minimum(left_screen.shape[0], right_screen.shape[0]) + if valid_points == 0: + return np.empty((0, 2), dtype=np.float32) + left_screen = left_screen[:valid_points] + right_screen = right_screen[:valid_points] - if not points: - return np.empty((0, 2), dtype=np.float32) + if self._clip_region: + clip = self._clip_region + bounds_mask = ( + (left_screen[:, 0] >= clip.x) & (left_screen[:, 0] <= clip.x + clip.width) & + (left_screen[:, 1] >= clip.y) & (left_screen[:, 1] <= clip.y + clip.height) & + (right_screen[:, 0] >= clip.x) & (right_screen[:, 0] <= clip.x + clip.width) & + (right_screen[:, 1] >= clip.y) & (right_screen[:, 1] <= clip.y + clip.height) + ) + if not np.any(bounds_mask): + return np.empty((0, 2), dtype=np.float32) + left_screen = left_screen[bounds_mask] + right_screen = right_screen[bounds_mask] - return np.array(points, dtype=np.float32) + if not allow_invert and left_screen.shape[0] > 1: + keep = np.concatenate(([True], np.diff(left_screen[:, 1]) < 0)) + left_screen = left_screen[keep] + right_screen = right_screen[keep] + if left_screen.shape[0] == 0: + return np.empty((0, 2), dtype=np.float32) + + return np.vstack((left_screen, right_screen[::-1])).astype(np.float32) @staticmethod def _map_val(x, x0, x1, y0, y1): @@ -345,17 +395,8 @@ class ModelRenderer: @staticmethod def _hsla_to_color(h, s, l, a): - """Convert HSLA color to Raylib Color using colorsys module""" - # colorsys uses HLS format (Hue, Lightness, Saturation) - r, g, b = colorsys.hls_to_rgb(h, l, s) - - # Ensure values are in valid range - r_val = max(0, min(255, int(r * 255))) - g_val = max(0, min(255, int(g * 255))) - b_val = max(0, min(255, int(b * 255))) - a_val = max(0, min(255, int(a * 255))) - - return rl.Color(r_val, g_val, b_val, a_val) + r, g, b = [max(0, min(255, int(v * 255))) for v in colorsys.hls_to_rgb(h, l, s)] + return rl.Color(r, g, b, max(0, min(255, int(a * 255)))) @staticmethod def _blend_colors(begin_colors, end_colors, t): From e6eef5d9d01d7b75d56f58a964282d6f839371e5 Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Sat, 31 May 2025 00:35:10 +0800 Subject: [PATCH 45/60] system/ui: implement driver monitoring UI (#35358) * pyui_driver_state_reander * ddd * draw_spline_linear * pre-calculate the face keypoints transform * remove int convert * improve * use draw_spline_linear * pre-calc points * state updated * render to texture * Revert "render to texture" This reverts commit 27be710f4c7aca3bb05e94ad69635d292e799ff4. * cleanup * dd * new dataclass * cleanup * use content_rect --- system/ui/onroad/augmented_road_view.py | 3 + system/ui/onroad/driver_state.py | 238 ++++++++++++++++++++++++ 2 files changed, 241 insertions(+) create mode 100644 system/ui/onroad/driver_state.py diff --git a/system/ui/onroad/augmented_road_view.py b/system/ui/onroad/augmented_road_view.py index b4b6eef12c..67f6bb99a9 100644 --- a/system/ui/onroad/augmented_road_view.py +++ b/system/ui/onroad/augmented_road_view.py @@ -4,6 +4,7 @@ from enum import Enum from cereal import messaging, log from msgq.visionipc import VisionStreamType +from openpilot.system.ui.onroad.driver_state import DriverStateRenderer from openpilot.system.ui.onroad.model_renderer import ModelRenderer from openpilot.system.ui.widgets.cameraview import CameraView from openpilot.system.ui.lib.application import gui_app @@ -40,6 +41,7 @@ class AugmentedRoadView(CameraView): self._content_rect = rl.Rectangle() self.model_renderer = ModelRenderer() + self.driver_state_renderer = DriverStateRenderer() def render(self, rect): # Update calibration before rendering @@ -74,6 +76,7 @@ class AugmentedRoadView(CameraView): # - Lead vehicle indicators # - Additional features self.model_renderer.draw(self._content_rect, self.sm) + self.driver_state_renderer.draw(self._content_rect, self.sm) # End clipping region rl.end_scissor_mode() diff --git a/system/ui/onroad/driver_state.py b/system/ui/onroad/driver_state.py new file mode 100644 index 0000000000..4ad35bfaa5 --- /dev/null +++ b/system/ui/onroad/driver_state.py @@ -0,0 +1,238 @@ +import numpy as np +import pyray as rl +from dataclasses import dataclass +from openpilot.system.ui.lib.application import gui_app + +# Default 3D coordinates for face keypoints as a NumPy array +DEFAULT_FACE_KPTS_3D = np.array([ + [-5.98, -51.20, 8.00], [-17.64, -49.14, 8.00], [-23.81, -46.40, 8.00], [-29.98, -40.91, 8.00], + [-32.04, -37.49, 8.00], [-34.10, -32.00, 8.00], [-36.16, -21.03, 8.00], [-36.16, 6.40, 8.00], + [-35.47, 10.51, 8.00], [-32.73, 19.43, 8.00], [-29.30, 26.29, 8.00], [-24.50, 33.83, 8.00], + [-19.01, 41.37, 8.00], [-14.21, 46.17, 8.00], [-12.16, 47.54, 8.00], [-4.61, 49.60, 8.00], + [4.99, 49.60, 8.00], [12.53, 47.54, 8.00], [14.59, 46.17, 8.00], [19.39, 41.37, 8.00], + [24.87, 33.83, 8.00], [29.67, 26.29, 8.00], [33.10, 19.43, 8.00], [35.84, 10.51, 8.00], + [36.53, 6.40, 8.00], [36.53, -21.03, 8.00], [34.47, -32.00, 8.00], [32.42, -37.49, 8.00], + [30.36, -40.91, 8.00], [24.19, -46.40, 8.00], [18.02, -49.14, 8.00], [6.36, -51.20, 8.00], + [-5.98, -51.20, 8.00], +], dtype=np.float32) + +# UI constants +UI_BORDER_SIZE = 30 +BTN_SIZE = 192 +IMG_SIZE = 144 +ARC_LENGTH = 133 +ARC_THICKNESS_DEFAULT = 6.7 +ARC_THICKNESS_EXTEND = 12.0 + +SCALES_POS = np.array([0.9, 0.4, 0.4], dtype=np.float32) +SCALES_NEG = np.array([0.7, 0.4, 0.4], dtype=np.float32) + +@dataclass +class ArcData: + """Data structure for arc rendering parameters.""" + x: float + y: float + width: float + height: float + thickness: float + +class DriverStateRenderer: + def __init__(self): + # Initial state with NumPy arrays + self.face_kpts_draw = DEFAULT_FACE_KPTS_3D.copy() + self.is_active = False + self.is_rhd = False + self.dm_fade_state = 0.0 + self.state_updated = False + self.last_rect: rl.Rectangle = rl.Rectangle(0, 0, 0, 0) + self.driver_pose_vals = np.zeros(3, dtype=np.float32) + self.driver_pose_diff = np.zeros(3, dtype=np.float32) + self.driver_pose_sins = np.zeros(3, dtype=np.float32) + self.driver_pose_coss = np.zeros(3, dtype=np.float32) + self.face_keypoints_transformed = np.zeros((DEFAULT_FACE_KPTS_3D.shape[0], 2), dtype=np.float32) + self.position_x: float = 0.0 + self.position_y: float = 0.0 + self.h_arc_data = None + self.v_arc_data = None + + # Pre-allocate drawing arrays + self.face_lines = [rl.Vector2(0, 0) for _ in range(len(DEFAULT_FACE_KPTS_3D))] + self.h_arc_lines = [rl.Vector2(0, 0) for _ in range(37)] # 37 points for horizontal arc + self.v_arc_lines = [rl.Vector2(0, 0) for _ in range(37)] # 37 points for vertical arc + + # Load the driver face icon + self.dm_img = gui_app.texture("icons/driver_face.png", IMG_SIZE, IMG_SIZE) + + # Colors + self.white_color = rl.Color(255, 255, 255, 255) + self.arc_color = rl.Color(26, 242, 66, 255) + self.engaged_color = rl.Color(26, 242, 66, 255) + self.disengaged_color = rl.Color(139, 139, 139, 255) + + def draw(self, rect, sm): + if not self._is_visible(sm): + return + + self._update_state(sm, rect) + if not self.state_updated: + return + + # Set opacity based on active state + opacity = 0.65 if self.is_active else 0.2 + + # Draw background circle + rl.draw_circle(int(self.position_x), int(self.position_y), BTN_SIZE // 2, rl.Color(0, 0, 0, 70)) + + # Draw face icon + icon_pos = rl.Vector2(self.position_x - self.dm_img.width // 2, self.position_y - self.dm_img.height // 2) + rl.draw_texture_v(self.dm_img, icon_pos, rl.Color(255, 255, 255, int(255 * opacity))) + + # Draw face outline + self.white_color.a = int(255 * opacity) + rl.draw_spline_linear(self.face_lines, len(self.face_lines), 5.2, self.white_color) + + # Set arc color based on engaged state + engaged = True + self.arc_color = self.engaged_color if engaged else self.disengaged_color + self.arc_color.a = int(0.4 * 255 * (1.0 - self.dm_fade_state)) # Fade out when inactive + + # Draw arcs + if self.h_arc_data: + rl.draw_spline_linear(self.h_arc_lines, len(self.h_arc_lines), self.h_arc_data.thickness, self.arc_color) + if self.v_arc_data: + rl.draw_spline_linear(self.v_arc_lines, len(self.v_arc_lines), self.v_arc_data.thickness, self.arc_color) + + def _is_visible(self, sm): + """Check if the visualization should be rendered.""" + return (sm.seen['driverStateV2'] and + sm.seen['driverMonitoringState'] and + sm['selfdriveState'].alertSize == 0) + + def _update_state(self, sm, rect): + """Update the driver monitoring state based on model data""" + if not sm.updated["driverMonitoringState"]: + if self.state_updated and (rect.x != self.last_rect.x or rect.y != self.last_rect.y or \ + rect.width != self.last_rect.width or rect.height != self.last_rect.height): + self.pre_calculate_drawing_elements(rect) + return + + # Get monitoring state + dm_state = sm["driverMonitoringState"] + self.is_active = dm_state.isActiveMode + self.is_rhd = dm_state.isRHD + + # Update fade state (smoother transition between active/inactive) + fade_target = 0.0 if self.is_active else 0.5 + self.dm_fade_state = np.clip(self.dm_fade_state + 0.2 * (fade_target - self.dm_fade_state), 0.0, 1.0) + + # Get driver orientation data from appropriate camera + driverstate = sm["driverStateV2"] + driver_data = driverstate.rightDriverData if self.is_rhd else driverstate.leftDriverData + driver_orient = driver_data.faceOrientation + + # Update pose values with scaling and smoothing + driver_orient = np.array(driver_orient) + scales = np.where(driver_orient < 0, SCALES_NEG, SCALES_POS) + v_this = driver_orient * scales + self.driver_pose_diff = np.abs(self.driver_pose_vals - v_this) + self.driver_pose_vals = 0.8 * v_this + 0.2 * self.driver_pose_vals # Smooth changes + + # Apply fade to rotation and compute sin/cos + rotation_amount = self.driver_pose_vals * (1.0 - self.dm_fade_state) + self.driver_pose_sins = np.sin(rotation_amount) + self.driver_pose_coss = np.cos(rotation_amount) + + # Create rotation matrix for 3D face model + sin_y, sin_x, sin_z = self.driver_pose_sins + cos_y, cos_x, cos_z = self.driver_pose_coss + r_xyz = np.array( + [ + [cos_x * cos_z, cos_x * sin_z, -sin_x], + [-sin_y * sin_x * cos_z - cos_y * sin_z, -sin_y * sin_x * sin_z + cos_y * cos_z, -sin_y * cos_x], + [cos_y * sin_x * cos_z - sin_y * sin_z, cos_y * sin_x * sin_z + sin_y * cos_z, cos_y * cos_x], + ] + ) + + # Transform face keypoints using vectorized matrix multiplication + self.face_kpts_draw = DEFAULT_FACE_KPTS_3D @ r_xyz.T + self.face_kpts_draw[:, 2] = self.face_kpts_draw[:, 2] * (1.0 - self.dm_fade_state) + 8 * self.dm_fade_state + + # Pre-calculate the transformed keypoints + kp_depth = (self.face_kpts_draw[:, 2] - 8) / 120.0 + 1.0 + self.face_keypoints_transformed = self.face_kpts_draw[:, :2] * kp_depth[:, None] + + # Pre-calculate all drawing elements + self._pre_calculate_drawing_elements(rect) + self.state_updated = True + + def _pre_calculate_drawing_elements(self, rect): + """Pre-calculate all drawing elements based on the current rectangle""" + # Calculate icon position (bottom-left or bottom-right) + width, height = rect.width, rect.height + offset = UI_BORDER_SIZE + BTN_SIZE // 2 + self.position_x = rect.x + (width - offset if self.is_rhd else offset) + self.position_y = rect.y + height - offset + + # Pre-calculate the face lines positions + positioned_keypoints = self.face_keypoints_transformed + np.array([self.position_x, self.position_y]) + for i in range(len(positioned_keypoints)): + self.face_lines[i].x = positioned_keypoints[i][0] + self.face_lines[i].y = positioned_keypoints[i][1] + + # Calculate arc dimensions based on head rotation + delta_x = -self.driver_pose_sins[1] * ARC_LENGTH / 2.0 # Horizontal movement + delta_y = -self.driver_pose_sins[0] * ARC_LENGTH / 2.0 # Vertical movement + + # Horizontal arc + h_width = abs(delta_x) + self.h_arc_data = self._calculate_arc_data( + delta_x, h_width, self.position_x, self.position_y - ARC_LENGTH / 2, + self.driver_pose_sins[1], self.driver_pose_diff[1], is_horizontal=True + ) + + # Vertical arc + v_height = abs(delta_y) + self.v_arc_data = self._calculate_arc_data( + delta_y, v_height, self.position_x - ARC_LENGTH / 2, self.position_y, + self.driver_pose_sins[0], self.driver_pose_diff[0], is_horizontal=False + ) + + def _calculate_arc_data( + self, delta: float, size: float, x: float, y: float, sin_val: float, diff_val: float, is_horizontal: bool + ): + """Calculate arc data and pre-compute arc points.""" + if size <= 0: + return None + + thickness = ARC_THICKNESS_DEFAULT + ARC_THICKNESS_EXTEND * min(1.0, diff_val * 5.0) + start_angle = (90 if sin_val > 0 else -90) if is_horizontal else (0 if sin_val > 0 else 180) + x = min(x + delta, x) if is_horizontal else x + y = y if is_horizontal else min(y + delta, y) + + arc_data = ArcData( + x=x, + y=y, + width=size if is_horizontal else ARC_LENGTH, + height=ARC_LENGTH if is_horizontal else size, + thickness=thickness, + ) + + # Pre-calculate arc points + start_rad = np.deg2rad(start_angle) + end_rad = np.deg2rad(start_angle + 180) + angles = np.linspace(start_rad, end_rad, 37) + + center_x = x + arc_data.width / 2 + center_y = y + arc_data.height / 2 + radius_x = arc_data.width / 2 + radius_y = arc_data.height / 2 + + x_coords = center_x + np.cos(angles) * radius_x + y_coords = center_y + np.sin(angles) * radius_y + + arc_lines = self.h_arc_lines if is_horizontal else self.v_arc_lines + for i, (x_coord, y_coord) in enumerate(zip(x_coords, y_coords, strict=True)): + arc_lines[i].x = x_coord + arc_lines[i].y = y_coord + + return arc_data From a3fab434a47c0e308021984950d39e5e6f2d8a62 Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Sat, 31 May 2025 00:53:11 +0800 Subject: [PATCH 46/60] system/ui: migrate c++ HudRenderer to python (#35359) * port c++ HudRenderer to python * cache font metrics * cache fonts * improve * fix bg * refactor * rebase * fix --- system/ui/onroad/augmented_road_view.py | 3 + system/ui/onroad/driver_state.py | 2 +- system/ui/onroad/hud_renderer.py | 194 ++++++++++++++++++++++++ 3 files changed, 198 insertions(+), 1 deletion(-) create mode 100644 system/ui/onroad/hud_renderer.py diff --git a/system/ui/onroad/augmented_road_view.py b/system/ui/onroad/augmented_road_view.py index 67f6bb99a9..7128917adf 100644 --- a/system/ui/onroad/augmented_road_view.py +++ b/system/ui/onroad/augmented_road_view.py @@ -5,6 +5,7 @@ from enum import Enum from cereal import messaging, log from msgq.visionipc import VisionStreamType from openpilot.system.ui.onroad.driver_state import DriverStateRenderer +from openpilot.system.ui.onroad.hud_renderer import HudRenderer from openpilot.system.ui.onroad.model_renderer import ModelRenderer from openpilot.system.ui.widgets.cameraview import CameraView from openpilot.system.ui.lib.application import gui_app @@ -41,6 +42,7 @@ class AugmentedRoadView(CameraView): self._content_rect = rl.Rectangle() self.model_renderer = ModelRenderer() + self._hud_renderer = HudRenderer() self.driver_state_renderer = DriverStateRenderer() def render(self, rect): @@ -76,6 +78,7 @@ class AugmentedRoadView(CameraView): # - Lead vehicle indicators # - Additional features self.model_renderer.draw(self._content_rect, self.sm) + self._hud_renderer.draw(self._content_rect, self.sm) self.driver_state_renderer.draw(self._content_rect, self.sm) # End clipping region diff --git a/system/ui/onroad/driver_state.py b/system/ui/onroad/driver_state.py index 4ad35bfaa5..b998936903 100644 --- a/system/ui/onroad/driver_state.py +++ b/system/ui/onroad/driver_state.py @@ -113,7 +113,7 @@ class DriverStateRenderer: if not sm.updated["driverMonitoringState"]: if self.state_updated and (rect.x != self.last_rect.x or rect.y != self.last_rect.y or \ rect.width != self.last_rect.width or rect.height != self.last_rect.height): - self.pre_calculate_drawing_elements(rect) + self._pre_calculate_drawing_elements(rect) return # Get monitoring state diff --git a/system/ui/onroad/hud_renderer.py b/system/ui/onroad/hud_renderer.py new file mode 100644 index 0000000000..b63b50f26d --- /dev/null +++ b/system/ui/onroad/hud_renderer.py @@ -0,0 +1,194 @@ +import pyray as rl +from dataclasses import dataclass +from cereal.messaging import SubMaster +from openpilot.system.ui.lib.application import gui_app, FontWeight +from openpilot.common.conversions import Conversions as CV +from enum import IntEnum + +# Constants +SET_SPEED_NA = 255 +KM_TO_MILE = 0.621371 + + +@dataclass(frozen=True) +class UIConfig: + header_height: int = 300 + border_size: int = 30 + button_size: int = 192 + set_speed_width_metric: int = 200 + set_speed_width_imperial: int = 172 + set_speed_height: int = 204 + wheel_icon_size: int = 144 + + +@dataclass(frozen=True) +class FontSizes: + current_speed: int = 176 + speed_unit: int = 66 + max_speed: int = 40 + set_speed: int = 90 + + +@dataclass(frozen=True) +class Colors: + white: rl.Color = rl.Color(255, 255, 255, 255) + disengaged: rl.Color = rl.Color(145, 155, 149, 255) + override: rl.Color = rl.Color(145, 155, 149, 255) # Added + engaged: rl.Color = rl.Color(128, 216, 166, 255) + disengaged_bg: rl.Color = rl.Color(0, 0, 0, 153) + override_bg: rl.Color = rl.Color(145, 155, 149, 204) + engaged_bg: rl.Color = rl.Color(128, 216, 166, 204) + grey: rl.Color = rl.Color(166, 166, 166, 255) + dark_grey: rl.Color = rl.Color(114, 114, 114, 255) + black_translucent: rl.Color = rl.Color(0, 0, 0, 166) + white_translucent: rl.Color = rl.Color(255, 255, 255, 200) + border_translucent: rl.Color = rl.Color(255, 255, 255, 75) + header_gradient_start: rl.Color = rl.Color(0, 0, 0, 114) + header_gradient_end: rl.Color = rl.Color(0, 0, 0, 0) + + +UI_CONFIG = UIConfig() +FONT_SIZES = FontSizes() +COLORS = Colors() + + +class HudStatus(IntEnum): + DISENGAGED = 0 + OVERRIDE = 1 + ENGAGED = 2 + + +class HudRenderer: + def __init__(self): + """Initialize the HUD renderer.""" + self.is_metric: bool = False + self.status: HudStatus = HudStatus.DISENGAGED + self.is_cruise_set: bool = False + self.is_cruise_available: bool = False + self.set_speed: float = SET_SPEED_NA + self.speed: float = 0.0 + self.v_ego_cluster_seen: bool = False + self.font_metrics_cache: dict[[str, int, str], rl.Vector2] = {} + self._wheel_texture: rl.Texture = gui_app.texture('icons/chffr_wheel.png', UI_CONFIG.wheel_icon_size, UI_CONFIG.wheel_icon_size) + self._font_semi_bold: rl.Font = gui_app.font(FontWeight.SEMI_BOLD) + self._font_bold: rl.Font = gui_app.font(FontWeight.BOLD) + self._font_medium: rl.Font = gui_app.font(FontWeight.MEDIUM) + + def _update_state(self, sm: SubMaster) -> None: + """Update HUD state based on car state and controls state.""" + self.is_metric = True + self.status = HudStatus.DISENGAGED + + if not sm.valid['carState']: + self.is_cruise_set = False + self.set_speed = SET_SPEED_NA + self.speed = 0.0 + return + + controls_state = sm['controlsState'] + car_state = sm['carState'] + + v_cruise_cluster = car_state.vCruiseCluster + self.set_speed = ( + controls_state.vCruiseDEPRECATED if v_cruise_cluster == 0.0 else v_cruise_cluster + ) + self.is_cruise_set = 0 < self.set_speed < SET_SPEED_NA + self.is_cruise_available = self.set_speed != -1 + + if self.is_cruise_set and not self.is_metric: + self.set_speed *= KM_TO_MILE + + v_ego_cluster = car_state.vEgoCluster + self.v_ego_cluster_seen = self.v_ego_cluster_seen or v_ego_cluster != 0.0 + v_ego = v_ego_cluster if self.v_ego_cluster_seen else car_state.vEgo + speed_conversion = CV.MS_TO_KPH if self.is_metric else CV.MS_TO_MPH + self.speed = max(0.0, v_ego * speed_conversion) + + def draw(self, rect: rl.Rectangle, sm: SubMaster) -> None: + """Render HUD elements to the screen.""" + self._update_state(sm) + rl.draw_rectangle_gradient_v( + int(rect.x), + int(rect.y), + int(rect.width), + UI_CONFIG.header_height, + COLORS.header_gradient_start, + COLORS.header_gradient_end, + ) + + if self.is_cruise_available: + self._draw_set_speed(rect) + + self._draw_current_speed(rect) + self._draw_wheel_icon(rect) + + def _draw_set_speed(self, rect: rl.Rectangle) -> None: + """Draw the MAX speed indicator box.""" + set_speed_width = UI_CONFIG.set_speed_width_metric if self.is_metric else UI_CONFIG.set_speed_width_imperial + x = rect.x + 60 + (UI_CONFIG.set_speed_width_imperial - set_speed_width) // 2 + y = rect.y + 45 + + set_speed_rect = rl.Rectangle(x, y, set_speed_width, UI_CONFIG.set_speed_height) + rl.draw_rectangle_rounded(set_speed_rect, 0.2, 30, COLORS.black_translucent) + rl.draw_rectangle_rounded_lines_ex(set_speed_rect, 0.2, 30, 6, COLORS.border_translucent) + + max_color = COLORS.grey + set_speed_color = COLORS.dark_grey + if self.is_cruise_set: + set_speed_color = COLORS.white + max_color = { + HudStatus.DISENGAGED: COLORS.disengaged, + HudStatus.OVERRIDE: COLORS.override, + HudStatus.ENGAGED: COLORS.engaged, + }.get(self.status, COLORS.grey) + + max_text = "MAX" + max_text_width = self._measure_text(max_text, self._font_semi_bold, FONT_SIZES.max_speed, 'semi_bold').x + rl.draw_text_ex( + self._font_semi_bold, + max_text, + rl.Vector2(x + (set_speed_width - max_text_width) / 2, y + 27), + FONT_SIZES.max_speed, + 0, + max_color, + ) + + set_speed_text = "–" if not self.is_cruise_set else str(round(self.set_speed)) + speed_text_width = self._measure_text(set_speed_text, self._font_bold, FONT_SIZES.set_speed, 'bold').x + rl.draw_text_ex( + self._font_bold, + set_speed_text, + rl.Vector2(x + (set_speed_width - speed_text_width) / 2, y + 77), + FONT_SIZES.set_speed, + 0, + set_speed_color, + ) + + def _draw_current_speed(self, rect: rl.Rectangle) -> None: + """Draw the current vehicle speed and unit.""" + speed_text = str(round(self.speed)) + speed_text_size = self._measure_text(speed_text, self._font_bold, FONT_SIZES.current_speed, 'bold') + speed_pos = rl.Vector2(rect.x + rect.width / 2 - speed_text_size.x / 2, 180 - speed_text_size.y / 2) + rl.draw_text_ex(self._font_bold, speed_text, speed_pos, FONT_SIZES.current_speed, 0, COLORS.white) + + unit_text = "km/h" if self.is_metric else "mph" + unit_text_size = self._measure_text(unit_text, self._font_medium, FONT_SIZES.speed_unit, 'medium') + unit_pos = rl.Vector2(rect.x + rect.width / 2 - unit_text_size.x / 2, 290 - unit_text_size.y / 2) + rl.draw_text_ex(self._font_medium, unit_text, unit_pos, FONT_SIZES.speed_unit, 0, COLORS.white_translucent) + + def _draw_wheel_icon(self, rect: rl.Rectangle) -> None: + """Draw the steering wheel icon with status-based opacity.""" + center_x = int(rect.x + rect.width - UI_CONFIG.border_size - UI_CONFIG.button_size / 2) + center_y = int(rect.y + UI_CONFIG.border_size + UI_CONFIG.button_size / 2) + rl.draw_circle(center_x, center_y, UI_CONFIG.button_size / 2, COLORS.black_translucent) + + opacity = 0.7 if self.status == HudStatus.DISENGAGED else 1.0 + img_pos = rl.Vector2(center_x - self._wheel_texture.width / 2, center_y - self._wheel_texture.height / 2) + rl.draw_texture_v(self._wheel_texture, img_pos, rl.Color(255, 255, 255, int(255 * opacity))) + + def _measure_text(self, text: str, font: rl.Font, font_size: int, font_type: str) -> rl.Vector2: + """Measure text dimensions with caching.""" + key = (text, font_size, font_type) + if key not in self.font_metrics_cache: + self.font_metrics_cache[key] = rl.measure_text_ex(font, text, font_size, 0) + return self.font_metrics_cache[key] From e51243f2cd6c6e2d236b2295f336b763ff33f094 Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Sat, 31 May 2025 01:53:39 +0800 Subject: [PATCH 47/60] system/ui: remove todo and add comment (#35390) remove todo and add document --- system/ui/onroad/augmented_road_view.py | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/system/ui/onroad/augmented_road_view.py b/system/ui/onroad/augmented_road_view.py index 7128917adf..83d2271028 100644 --- a/system/ui/onroad/augmented_road_view.py +++ b/system/ui/onroad/augmented_road_view.py @@ -72,15 +72,14 @@ class AugmentedRoadView(CameraView): # Render the base camera view super().render(rect) - # TODO: Add road visualization overlays like: - # - Lane lines and road edges - # - Path prediction - # - Lead vehicle indicators - # - Additional features + # Draw all UI overlays self.model_renderer.draw(self._content_rect, self.sm) self._hud_renderer.draw(self._content_rect, self.sm) self.driver_state_renderer.draw(self._content_rect, self.sm) + # Custom UI extension point - add custom overlays here + # Use self._content_rect for positioning within camera bounds + # End clipping region rl.end_scissor_mode() From 255b606fe49fac4b3e5e83982628e67161d715b4 Mon Sep 17 00:00:00 2001 From: Trey Moen <50057480+greatgitsby@users.noreply.github.com> Date: Fri, 30 May 2025 10:54:07 -0700 Subject: [PATCH 48/60] feat: remove esim.nmconnection, use AGNOS lte conn (#35389) * feat: remove esim.nmconnection, use AGNOS lte conn * unused * remove old --- system/hardware/base.py | 3 +++ system/hardware/tici/esim.nmconnection | 30 -------------------------- system/hardware/tici/hardware.py | 22 +++++++++---------- 3 files changed, 14 insertions(+), 41 deletions(-) delete mode 100644 system/hardware/tici/esim.nmconnection diff --git a/system/hardware/base.py b/system/hardware/base.py index e429f0e9f2..3506be5145 100644 --- a/system/hardware/base.py +++ b/system/hardware/base.py @@ -210,6 +210,9 @@ class HardwareBase(ABC): def configure_modem(self): pass + def reboot_modem(self): + pass + @abstractmethod def get_networks(self): pass diff --git a/system/hardware/tici/esim.nmconnection b/system/hardware/tici/esim.nmconnection deleted file mode 100644 index 74f6f8e82c..0000000000 --- a/system/hardware/tici/esim.nmconnection +++ /dev/null @@ -1,30 +0,0 @@ -[connection] -id=esim -uuid=fff6553c-3284-4707-a6b1-acc021caaafb -type=gsm -permissions= -autoconnect=true -autoconnect-retries=100 -autoconnect-priority=2 -metered=1 - -[gsm] -apn= -home-only=false -auto-config=true -sim-id= - -[ipv4] -route-metric=1000 -dns-priority=1000 -dns-search= -method=auto - -[ipv6] -ddr-gen-mode=stable-privacy -dns-search= -route-metric=1000 -dns-priority=1000 -method=auto - -[proxy] diff --git a/system/hardware/tici/hardware.py b/system/hardware/tici/hardware.py index 3add52d21c..f5991ec7fa 100644 --- a/system/hardware/tici/hardware.py +++ b/system/hardware/tici/hardware.py @@ -3,7 +3,6 @@ import math import os import subprocess import time -import tempfile from enum import IntEnum from functools import cached_property, lru_cache from pathlib import Path @@ -518,18 +517,19 @@ class Tici(HardwareBase): except Exception: pass - # eSIM prime + # we use the lte connection built into AGNOS. cleanup esim connection if it exists dest = "/etc/NetworkManager/system-connections/esim.nmconnection" - if sim_id.startswith('8985235') and not os.path.exists(dest): - with open(Path(__file__).parent/'esim.nmconnection') as f, tempfile.NamedTemporaryFile(mode='w') as tf: - dat = f.read() - dat = dat.replace("sim-id=", f"sim-id={sim_id}") - tf.write(dat) - tf.flush() + if os.path.exists(dest): + os.system(f"sudo nmcli con delete {dest}") + self.reboot_modem() - # needs to be root - os.system(f"sudo cp {tf.name} {dest}") - os.system(f"sudo nmcli con load {dest}") + def reboot_modem(self): + modem = self.get_modem() + for state in (0, 1): + try: + modem.Command(f'AT+CFUN={state}', math.ceil(TIMEOUT), dbus_interface=MM_MODEM, timeout=TIMEOUT) + except Exception: + pass def get_networks(self): r = {} From 122182176161bff56e51b68afcfec07264925093 Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Sat, 31 May 2025 02:03:38 +0800 Subject: [PATCH 49/60] system/ui: migrate c++ alert renderer to python (#35386) * rebase * cache metrics * measure text * type hint * improve * fix roundness * rebase --- system/ui/onroad/alert_renderer.py | 234 ++++++++++++++++++++++++ system/ui/onroad/augmented_road_view.py | 5 +- 2 files changed, 238 insertions(+), 1 deletion(-) create mode 100644 system/ui/onroad/alert_renderer.py diff --git a/system/ui/onroad/alert_renderer.py b/system/ui/onroad/alert_renderer.py new file mode 100644 index 0000000000..600c721680 --- /dev/null +++ b/system/ui/onroad/alert_renderer.py @@ -0,0 +1,234 @@ +import numpy as np +import pyray as rl +from dataclasses import dataclass +from cereal import messaging, log +from openpilot.system.ui.lib.application import gui_app, FontWeight + +# Constants +ALERT_COLORS = { + log.SelfdriveState.AlertStatus.normal: rl.Color(0, 0, 0, 150), # Black + log.SelfdriveState.AlertStatus.userPrompt: rl.Color(0xFE, 0x8C, 0x34, 100), # Orange + log.SelfdriveState.AlertStatus.critical: rl.Color(0xC9, 0x22, 0x31, 150), # Red +} + +ALERT_HEIGHTS = { + log.SelfdriveState.AlertSize.small: 271, + log.SelfdriveState.AlertSize.mid: 420, +} + +SELFDRIVE_STATE_TIMEOUT = 5 # Seconds +SELFDRIVE_UNRESPONSIVE_TIMEOUT = 10 # Seconds + + +@dataclass +class Alert: + text1: str = "" + text2: str = "" + alert_type: str = "" + size: log.SelfdriveState.AlertSize = log.SelfdriveState.AlertSize.none + status: log.SelfdriveState.AlertStatus = log.SelfdriveState.AlertStatus.normal + + def is_equal(self, other: 'Alert') -> bool: + """Check if two alerts are equal.""" + return ( + self.text1 == other.text1 + and self.text2 == other.text2 + and self.alert_type == other.alert_type + and self.size == other.size + and self.status == other.status + ) + + +class AlertRenderer: + def __init__(self): + """Initialize the alert renderer.""" + self.alert: Alert = Alert() + self.started_frame: int = 0 + self.font_regular: rl.Font = gui_app.font(FontWeight.NORMAL) + self.font_bold: rl.Font = gui_app.font(FontWeight.BOLD) + self.font_metrics_cache: dict[tuple[str, int, str], rl.Vector2] = {} + + def clear(self) -> None: + """Reset the alert to its default state.""" + self.alert = Alert() + + def update_state(self, sm: messaging.SubMaster, started_frame: int) -> None: + """Update alert state based on SubMaster data.""" + self.started_frame = started_frame + new_alert = self.get_alert(sm) + if not self.alert.is_equal(new_alert): + self.alert = new_alert + + def get_alert(self, sm: messaging.SubMaster) -> Alert: + """Generate the current alert based on selfdrive state.""" + if not sm.valid['selfdriveState']: + return Alert() + + ss = sm['selfdriveState'] + selfdrive_frame = sm.recv_frame['selfdriveState'] + alert_status = self._get_enum_value(ss.alertStatus, log.SelfdriveState.AlertStatus) + + # Return current alert if selfdrive state is recent + if selfdrive_frame >= self.started_frame: + return Alert( + text1=ss.alertText1, + text2=ss.alertText2, + alert_type=ss.alertType, + size=self._get_enum_value(ss.alertSize, log.SelfdriveState.AlertSize), + status=alert_status, + ) + + # Handle selfdrive timeout + ss_missing = (np.uint64(rl.get_time() * 1e9) - sm.recv_time['selfdriveState']) / 1e9 + if selfdrive_frame < self.started_frame: + return Alert( + text1="openpilot Unavailable", + text2="Waiting to start", + alert_type="selfdriveWaiting", + size=log.SelfdriveState.AlertSize.mid, + status=log.SelfdriveState.AlertStatus.normal, + ) + elif ss_missing > SELFDRIVE_STATE_TIMEOUT: + if ss.enabled and (ss_missing - SELFDRIVE_STATE_TIMEOUT) < SELFDRIVE_UNRESPONSIVE_TIMEOUT: + return Alert( + text1="TAKE CONTROL IMMEDIATELY", + text2="System Unresponsive", + alert_type="selfdriveUnresponsive", + size=log.SelfdriveState.AlertSize.full, + status=log.SelfdriveState.AlertStatus.critical, + ) + return Alert( + text1="System Unresponsive", + text2="Reboot Device", + alert_type="selfdriveUnresponsivePermanent", + size=log.SelfdriveState.AlertSize.mid, + status=log.SelfdriveState.AlertStatus.normal, + ) + + return Alert() + + def draw(self, rect: rl.Rectangle, sm: messaging.SubMaster) -> None: + """Render the alert within the specified rectangle.""" + self.update_state(sm, sm.recv_frame['selfdriveState']) + alert_size = self._get_enum_value(self.alert.size, log.SelfdriveState.AlertSize) + if alert_size == log.SelfdriveState.AlertSize.none: + return + + # Calculate alert rectangle + margin = 0 if alert_size == log.SelfdriveState.AlertSize.full else 40 + radius = 0 if alert_size == log.SelfdriveState.AlertSize.full else 30 + height = ALERT_HEIGHTS.get(alert_size, rect.height) + alert_rect = rl.Rectangle( + rect.x + margin, + rect.y + rect.height - height + margin, + rect.width - margin * 2, + height - margin * 2, + ) + + # Draw background + alert_status = self._get_enum_value(self.alert.status, log.SelfdriveState.AlertStatus) + color = ALERT_COLORS.get(alert_status, ALERT_COLORS[log.SelfdriveState.AlertStatus.normal]) + if alert_size != log.SelfdriveState.AlertSize.full: + roundness = radius / (min(alert_rect.width, alert_rect.height) / 2) + rl.draw_rectangle_rounded(alert_rect, roundness, 10, color) + else: + rl.draw_rectangle_rec(alert_rect, color) + + # Draw text + center_x = rect.x + rect.width / 2 + center_y = alert_rect.y + alert_rect.height / 2 + self._draw_text(alert_size, alert_rect, center_x, center_y) + + def _draw_text( + self, alert_size: log.SelfdriveState.AlertSize, alert_rect: rl.Rectangle, center_x: float, center_y: float + ) -> None: + """Draw text based on alert size.""" + if alert_size == log.SelfdriveState.AlertSize.small: + font_size = 74 + text_width = self._measure_text(self.font_bold, self.alert.text1, font_size, 'bold').x + rl.draw_text_ex( + self.font_bold, + self.alert.text1, + rl.Vector2(center_x - text_width / 2, center_y - font_size / 2), + font_size, + 0, + rl.WHITE, + ) + elif alert_size == log.SelfdriveState.AlertSize.mid: + font_size1 = 88 + text1_width = self._measure_text(self.font_bold, self.alert.text1, font_size1, 'bold').x + rl.draw_text_ex( + self.font_bold, + self.alert.text1, + rl.Vector2(center_x - text1_width / 2, center_y - 125), + font_size1, + 0, + rl.WHITE, + ) + font_size2 = 66 + text2_width = self._measure_text(self.font_regular, self.alert.text2, font_size2, 'regular').x + rl.draw_text_ex( + self.font_regular, + self.alert.text2, + rl.Vector2(center_x - text2_width / 2, center_y + 21), + font_size2, + 0, + rl.WHITE, + ) + elif alert_size == log.SelfdriveState.AlertSize.full: + is_long = len(self.alert.text1) > 15 + font_size1 = 132 if is_long else 177 + text1_y = alert_rect.y + (240 if is_long else 270) + wrapped_text1 = self._wrap_text(self.alert.text1, alert_rect.width - 100, font_size1, self.font_bold) + for i, line in enumerate(wrapped_text1): + line_width = self._measure_text(self.font_bold, line, font_size1, 'bold').x + rl.draw_text_ex( + self.font_bold, + line, + rl.Vector2(center_x - line_width / 2, text1_y + i * font_size1), + font_size1, + 0, + rl.WHITE, + ) + font_size2 = 88 + text2_y = alert_rect.y + alert_rect.height - (361 if is_long else 420) + wrapped_text2 = self._wrap_text(self.alert.text2, alert_rect.width - 100, font_size2, self.font_regular) + for i, line in enumerate(wrapped_text2): + line_width = self._measure_text(self.font_regular, line, font_size2, 'regular').x + rl.draw_text_ex( + self.font_regular, + line, + rl.Vector2(center_x - line_width / 2, text2_y + i * font_size2), + font_size2, + 0, + rl.WHITE, + ) + + def _wrap_text(self, text: str, max_width: float, font_size: int, font: rl.Font) -> list[str]: + """Wrap text to fit within max width.""" + words = text.split() + lines = [] + current_line = "" + for word in words: + test_line = f"{current_line} {word}" if current_line else word + if self._measure_text(font, test_line, font_size, 'bold' if font == self.font_bold else 'regular').x <= max_width: + current_line = test_line + else: + if current_line: + lines.append(current_line) + current_line = word + if current_line: + lines.append(current_line) + return lines + + def _measure_text(self, font: rl.Font, text: str, font_size: int, font_type: str) -> rl.Vector2: + """Measure text dimensions with caching.""" + key = (text, font_size, font_type) + if key not in self.font_metrics_cache: + self.font_metrics_cache[key] = rl.measure_text_ex(font, text, font_size, 0) + return self.font_metrics_cache[key] + + @staticmethod + def _get_enum_value(enum_value, enum_type: type): + """Safely convert capnp enum to Python enum value.""" + return enum_value.raw if hasattr(enum_value, 'raw') else enum_value diff --git a/system/ui/onroad/augmented_road_view.py b/system/ui/onroad/augmented_road_view.py index 83d2271028..18f94325f4 100644 --- a/system/ui/onroad/augmented_road_view.py +++ b/system/ui/onroad/augmented_road_view.py @@ -4,6 +4,7 @@ from enum import Enum from cereal import messaging, log from msgq.visionipc import VisionStreamType +from openpilot.system.ui.onroad.alert_renderer import AlertRenderer from openpilot.system.ui.onroad.driver_state import DriverStateRenderer from openpilot.system.ui.onroad.hud_renderer import HudRenderer from openpilot.system.ui.onroad.model_renderer import ModelRenderer @@ -43,6 +44,7 @@ class AugmentedRoadView(CameraView): self.model_renderer = ModelRenderer() self._hud_renderer = HudRenderer() + self.alert_renderer = AlertRenderer() self.driver_state_renderer = DriverStateRenderer() def render(self, rect): @@ -75,6 +77,7 @@ class AugmentedRoadView(CameraView): # Draw all UI overlays self.model_renderer.draw(self._content_rect, self.sm) self._hud_renderer.draw(self._content_rect, self.sm) + self.alert_renderer.draw(self._content_rect, self.sm) self.driver_state_renderer.draw(self._content_rect, self.sm) # Custom UI extension point - add custom overlays here @@ -118,7 +121,7 @@ class AugmentedRoadView(CameraView): def _calc_frame_matrix(self, rect: rl.Rectangle) -> np.ndarray: # Check if we can use cached matrix - calib_time = self.sm.recv_frame.get('liveCalibration', 0) + calib_time = self.sm.recv_frame['liveCalibration'] current_dims = (self._content_rect.width, self._content_rect.height) if (self._last_calib_time == calib_time and self._last_rect_dims == current_dims and From c4f2cf52998384cc1a578cd121f2a475cbf6bb2e Mon Sep 17 00:00:00 2001 From: Trey Moen <50057480+greatgitsby@users.noreply.github.com> Date: Fri, 30 May 2025 11:29:17 -0700 Subject: [PATCH 50/60] feat(esim): enable eSIM profile hotswapping (#35324) * reboot * no sleep * test * back * wait for sim * simpler * retry * Revert "retry" This reverts commit f1297160f3c085f43fc0356abb51fb52fa93ea2c. --- system/hardware/esim.py | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/system/hardware/esim.py b/system/hardware/esim.py index 6668a1cdd3..58ead6593f 100755 --- a/system/hardware/esim.py +++ b/system/hardware/esim.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 import argparse - +import time from openpilot.system.hardware import HARDWARE @@ -14,14 +14,16 @@ if __name__ == '__main__': parser.add_argument('--nickname', nargs=2, metavar=('iccid', 'name'), help='update the nickname for a profile') args = parser.parse_args() + mutated = False lpa = HARDWARE.get_sim_lpa() if args.switch: lpa.switch_profile(args.switch) + mutated = True elif args.delete: confirm = input('are you sure you want to delete this profile? (y/N) ') if confirm == 'y': lpa.delete_profile(args.delete) - print('deleted profile, please restart device to apply changes') + mutated = True else: print('cancelled') exit(0) @@ -32,6 +34,11 @@ if __name__ == '__main__': else: parser.print_help() + if mutated: + HARDWARE.reboot_modem() + # eUICC needs a small delay post-reboot before querying profiles + time.sleep(.5) + profiles = lpa.list_profiles() print(f'\n{len(profiles)} profile{"s" if len(profiles) > 1 else ""}:') for p in profiles: From ea9ff45ccb0461c08b084bd999576cb9c990a678 Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Sat, 31 May 2025 02:31:03 +0800 Subject: [PATCH 51/60] system/ui: fix indentation (#35391) fix indentation --- system/ui/onroad/model_renderer.py | 40 +++++++++++++++--------------- 1 file changed, 20 insertions(+), 20 deletions(-) diff --git a/system/ui/onroad/model_renderer.py b/system/ui/onroad/model_renderer.py index faf538c58d..ef7e567cda 100644 --- a/system/ui/onroad/model_renderer.py +++ b/system/ui/onroad/model_renderer.py @@ -331,12 +331,12 @@ class ModelRenderer: def _map_line_to_polygon(self, line: np.ndarray, y_off: float, z_off: float, max_idx: int, allow_invert: bool = True) -> np.ndarray: """Convert 3D line to 2D polygon for rendering.""" if line.shape[0] == 0: - return np.empty((0, 2), dtype=np.float32) + return np.empty((0, 2), dtype=np.float32) # Slice points and filter non-negative x-coordinates points = line[:max_idx + 1][line[:max_idx + 1, 0] >= 0] if points.shape[0] == 0: - return np.empty((0, 2), dtype=np.float32) + return np.empty((0, 2), dtype=np.float32) # Create left and right 3D points in one array n_points = points.shape[0] @@ -350,7 +350,7 @@ class ModelRenderer: proj = self._car_space_transform @ points_3d.T valid_z = np.abs(proj[2]) > 1e-6 if not np.any(valid_z): - return np.empty((0, 2), dtype=np.float32) + return np.empty((0, 2), dtype=np.float32) # Compute screen coordinates screen = proj[:2, valid_z] / proj[2, valid_z][None, :] @@ -360,29 +360,29 @@ class ModelRenderer: # Ensure consistent shapes by re-aligning valid points valid_points = np.minimum(left_screen.shape[0], right_screen.shape[0]) if valid_points == 0: - return np.empty((0, 2), dtype=np.float32) + return np.empty((0, 2), dtype=np.float32) left_screen = left_screen[:valid_points] right_screen = right_screen[:valid_points] if self._clip_region: - clip = self._clip_region - bounds_mask = ( - (left_screen[:, 0] >= clip.x) & (left_screen[:, 0] <= clip.x + clip.width) & - (left_screen[:, 1] >= clip.y) & (left_screen[:, 1] <= clip.y + clip.height) & - (right_screen[:, 0] >= clip.x) & (right_screen[:, 0] <= clip.x + clip.width) & - (right_screen[:, 1] >= clip.y) & (right_screen[:, 1] <= clip.y + clip.height) - ) - if not np.any(bounds_mask): - return np.empty((0, 2), dtype=np.float32) - left_screen = left_screen[bounds_mask] - right_screen = right_screen[bounds_mask] + clip = self._clip_region + bounds_mask = ( + (left_screen[:, 0] >= clip.x) & (left_screen[:, 0] <= clip.x + clip.width) & + (left_screen[:, 1] >= clip.y) & (left_screen[:, 1] <= clip.y + clip.height) & + (right_screen[:, 0] >= clip.x) & (right_screen[:, 0] <= clip.x + clip.width) & + (right_screen[:, 1] >= clip.y) & (right_screen[:, 1] <= clip.y + clip.height) + ) + if not np.any(bounds_mask): + return np.empty((0, 2), dtype=np.float32) + left_screen = left_screen[bounds_mask] + right_screen = right_screen[bounds_mask] if not allow_invert and left_screen.shape[0] > 1: - keep = np.concatenate(([True], np.diff(left_screen[:, 1]) < 0)) - left_screen = left_screen[keep] - right_screen = right_screen[keep] - if left_screen.shape[0] == 0: - return np.empty((0, 2), dtype=np.float32) + keep = np.concatenate(([True], np.diff(left_screen[:, 1]) < 0)) + left_screen = left_screen[keep] + right_screen = right_screen[keep] + if left_screen.shape[0] == 0: + return np.empty((0, 2), dtype=np.float32) return np.vstack((left_screen, right_screen[::-1])).astype(np.float32) From 45f90b1a553e37f5f3f48c5a968d24357ce84c0d Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Sat, 31 May 2025 03:57:37 +0800 Subject: [PATCH 52/60] system/ui: add animation to toggle (#35392) add animation to toogle --- system/ui/lib/toggle.py | 47 ++++++++++++++++++++++------------------- 1 file changed, 25 insertions(+), 22 deletions(-) diff --git a/system/ui/lib/toggle.py b/system/ui/lib/toggle.py index e72faef11c..5ae2500406 100644 --- a/system/ui/lib/toggle.py +++ b/system/ui/lib/toggle.py @@ -1,45 +1,49 @@ import pyray as rl -ON_COLOR = rl.GREEN +ON_COLOR = rl.Color(0, 255, 0, 255) OFF_COLOR = rl.Color(0x39, 0x39, 0x39, 255) KNOB_COLOR = rl.WHITE +WIDTH, HEIGHT = 160, 80 BG_HEIGHT = 60 -KNOB_HEIGHT = 80 -WIDTH = 160 +ANIMATION_SPEED = 8.0 class Toggle: def __init__(self, x, y, initial_state=False): self._state = initial_state - self._rect = rl.Rectangle(x, y, WIDTH, KNOB_HEIGHT) + self._rect = rl.Rectangle(x, y, WIDTH, HEIGHT) + self._progress = 1.0 if initial_state else 0.0 + self._target = self._progress def handle_input(self): if rl.is_mouse_button_pressed(rl.MOUSE_LEFT_BUTTON): - mouse_pos = rl.get_mouse_position() - if rl.check_collision_point_rec(mouse_pos, self._rect): + if rl.check_collision_point_rec(rl.get_mouse_position(), self._rect): self._state = not self._state + self._target = 1.0 if self._state else 0.0 def get_state(self): return self._state + def update(self): + if abs(self._progress - self._target) > 0.01: + delta = rl.get_frame_time() * ANIMATION_SPEED + self._progress += delta if self._progress < self._target else -delta + self._progress = max(0.0, min(1.0, self._progress)) + def render(self): - self._draw_background() - self._draw_knob() + self. update() + # Draw background + bg_rect = rl.Rectangle(self._rect.x + 5, self._rect.y + 10, WIDTH - 10, BG_HEIGHT) + bg_color = self._blend_color(OFF_COLOR, ON_COLOR, self._progress) + rl.draw_rectangle_rounded(bg_rect, 1.0, 10, bg_color) - def _draw_background(self): - bg_rect = rl.Rectangle( - self._rect.x + 5, - self._rect.y + (KNOB_HEIGHT - BG_HEIGHT) / 2, - self._rect.width - 10, - BG_HEIGHT, - ) - rl.draw_rectangle_rounded(bg_rect, 1.0, 10, ON_COLOR if self._state else OFF_COLOR) + # Draw knob + knob_x = self._rect.x + HEIGHT / 2 + (WIDTH - HEIGHT) * self._progress + knob_y = self._rect.y + HEIGHT / 2 + rl.draw_circle(int(knob_x), int(knob_y), HEIGHT / 2, KNOB_COLOR) - def _draw_knob(self): - knob_radius = KNOB_HEIGHT / 2 - knob_x = self._rect.x + knob_radius if not self._state else self._rect.x + self._rect.width - knob_radius - knob_y = self._rect.y + knob_radius - rl.draw_circle(int(knob_x), int(knob_y), knob_radius, KNOB_COLOR) + def _blend_color(self, c1, c2, t): + return rl.Color(int(c1.r + (c2.r - c1.r) * t), int(c1.g + (c2.g - c1.g) * t), int(c1.b + (c2.b - c1.b) * t), 255) if __name__ == "__main__": @@ -50,4 +54,3 @@ if __name__ == "__main__": for _ in gui_app.render(): toggle.handle_input() toggle.render() - From 2f808546449ea1aa072a2ab7f0bf6f876fe40de8 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Fri, 30 May 2025 13:31:07 -0700 Subject: [PATCH 53/60] sensord: rewrite in Python (#35353) * py sensord * fix up mmc * temp * port over accel * lil more * kinda works * rm that * gpiochip * mostly there * lil more * lil more * irq timestamps * fix ts * fix double deg2rad * test passes * fix up mypy * rm one more * exception * lint: * read in all events * bump that * get under budget: * accel self test * gyro self-test * keep these readable * give it more cores * debug * valid * rewrite that --------- Co-authored-by: Comma Device --- SConstruct | 1 - common/SConscript | 4 - common/gpio.cc | 84 -------- common/gpio.h | 33 --- common/gpio.py | 35 ++++ common/i2c.cc | 92 --------- common/i2c.h | 19 -- common/util.py | 22 ++ selfdrive/test/test_onroad.py | 8 +- system/hardware/tici/hardware.py | 20 +- system/manager/process_config.py | 2 +- system/sensord/.gitignore | 1 - system/sensord/SConscript | 13 -- system/sensord/sensord.py | 139 +++++++++++++ system/sensord/sensors/__init__.py | 0 system/sensord/sensors/constants.h | 18 -- system/sensord/sensors/i2c_sensor.cc | 50 ----- system/sensord/sensors/i2c_sensor.h | 51 ----- system/sensord/sensors/i2c_sensor.py | 72 +++++++ system/sensord/sensors/lsm6ds3_accel.cc | 250 ----------------------- system/sensord/sensors/lsm6ds3_accel.h | 49 ----- system/sensord/sensors/lsm6ds3_accel.py | 157 ++++++++++++++ system/sensord/sensors/lsm6ds3_gyro.cc | 233 --------------------- system/sensord/sensors/lsm6ds3_gyro.h | 45 ---- system/sensord/sensors/lsm6ds3_gyro.py | 141 +++++++++++++ system/sensord/sensors/lsm6ds3_temp.cc | 37 ---- system/sensord/sensors/lsm6ds3_temp.h | 26 --- system/sensord/sensors/lsm6ds3_temp.py | 33 +++ system/sensord/sensors/mmc5603nj_magn.cc | 108 ---------- system/sensord/sensors/mmc5603nj_magn.h | 37 ---- system/sensord/sensors/mmc5603nj_magn.py | 76 +++++++ system/sensord/sensors/sensor.h | 24 --- system/sensord/sensors_qcom2.cc | 170 --------------- 33 files changed, 681 insertions(+), 1369 deletions(-) delete mode 100644 common/gpio.cc delete mode 100644 common/gpio.h delete mode 100644 common/i2c.cc delete mode 100644 common/i2c.h delete mode 100644 system/sensord/.gitignore delete mode 100644 system/sensord/SConscript create mode 100755 system/sensord/sensord.py create mode 100644 system/sensord/sensors/__init__.py delete mode 100644 system/sensord/sensors/constants.h delete mode 100644 system/sensord/sensors/i2c_sensor.cc delete mode 100644 system/sensord/sensors/i2c_sensor.h create mode 100644 system/sensord/sensors/i2c_sensor.py delete mode 100644 system/sensord/sensors/lsm6ds3_accel.cc delete mode 100644 system/sensord/sensors/lsm6ds3_accel.h create mode 100644 system/sensord/sensors/lsm6ds3_accel.py delete mode 100644 system/sensord/sensors/lsm6ds3_gyro.cc delete mode 100644 system/sensord/sensors/lsm6ds3_gyro.h create mode 100644 system/sensord/sensors/lsm6ds3_gyro.py delete mode 100644 system/sensord/sensors/lsm6ds3_temp.cc delete mode 100644 system/sensord/sensors/lsm6ds3_temp.h create mode 100644 system/sensord/sensors/lsm6ds3_temp.py delete mode 100644 system/sensord/sensors/mmc5603nj_magn.cc delete mode 100644 system/sensord/sensors/mmc5603nj_magn.h create mode 100644 system/sensord/sensors/mmc5603nj_magn.py delete mode 100644 system/sensord/sensors/sensor.h delete mode 100644 system/sensord/sensors_qcom2.cc diff --git a/SConstruct b/SConstruct index d5597a70fe..c534eedbe8 100644 --- a/SConstruct +++ b/SConstruct @@ -348,7 +348,6 @@ SConscript([ ]) if arch != "Darwin": SConscript([ - 'system/sensord/SConscript', 'system/logcatd/SConscript', ]) diff --git a/common/SConscript b/common/SConscript index 829db6eeec..3cdb6fc5a2 100644 --- a/common/SConscript +++ b/common/SConscript @@ -4,14 +4,10 @@ common_libs = [ 'params.cc', 'swaglog.cc', 'util.cc', - 'i2c.cc', 'watchdog.cc', 'ratekeeper.cc' ] -if arch != "Darwin": - common_libs.append('gpio.cc') - _common = env.Library('common', common_libs, LIBS="json11") files = [ diff --git a/common/gpio.cc b/common/gpio.cc deleted file mode 100644 index dd7ba34b6d..0000000000 --- a/common/gpio.cc +++ /dev/null @@ -1,84 +0,0 @@ -#include "common/gpio.h" - -#include - -#ifdef __APPLE__ -int gpio_init(int pin_nr, bool output) { - return 0; -} - -int gpio_set(int pin_nr, bool high) { - return 0; -} - -int gpiochip_get_ro_value_fd(const char* consumer_label, int gpiochiop_id, int pin_nr) { - return 0; -} - -#else - -#include -#include - -#include -#include -#include - -#include "common/util.h" -#include "common/swaglog.h" - -int gpio_init(int pin_nr, bool output) { - char pin_dir_path[50]; - int pin_dir_path_len = snprintf(pin_dir_path, sizeof(pin_dir_path), - "/sys/class/gpio/gpio%d/direction", pin_nr); - if (pin_dir_path_len <= 0) { - return -1; - } - const char *value = output ? "out" : "in"; - return util::write_file(pin_dir_path, (void*)value, strlen(value)); -} - -int gpio_set(int pin_nr, bool high) { - char pin_val_path[50]; - int pin_val_path_len = snprintf(pin_val_path, sizeof(pin_val_path), - "/sys/class/gpio/gpio%d/value", pin_nr); - if (pin_val_path_len <= 0) { - return -1; - } - return util::write_file(pin_val_path, (void*)(high ? "1" : "0"), 1); -} - -int gpiochip_get_ro_value_fd(const char* consumer_label, int gpiochiop_id, int pin_nr) { - - // Assumed that all interrupt pins are unexported and rights are given to - // read from gpiochip0. - std::string gpiochip_path = "/dev/gpiochip" + std::to_string(gpiochiop_id); - int fd = open(gpiochip_path.c_str(), O_RDONLY); - if (fd < 0) { - LOGE("Error opening gpiochip0 fd"); - return -1; - } - - // Setup event - struct gpioevent_request rq; - rq.lineoffset = pin_nr; - rq.handleflags = GPIOHANDLE_REQUEST_INPUT; - - /* Requesting both edges as the data ready pulse from the lsm6ds sensor is - very short(75us) and is mostly detected as falling edge instead of rising. - So if it is detected as rising the following falling edge is skipped. */ - rq.eventflags = GPIOEVENT_REQUEST_BOTH_EDGES; - - strncpy(rq.consumer_label, consumer_label, std::size(rq.consumer_label) - 1); - int ret = util::safe_ioctl(fd, GPIO_GET_LINEEVENT_IOCTL, &rq); - if (ret == -1) { - LOGE("Unable to get line event from ioctl : %s", strerror(errno)); - close(fd); - return -1; - } - - close(fd); - return rq.fd; -} - -#endif diff --git a/common/gpio.h b/common/gpio.h deleted file mode 100644 index 89cdedd66c..0000000000 --- a/common/gpio.h +++ /dev/null @@ -1,33 +0,0 @@ -#pragma once - -// Pin definitions -#ifdef QCOM2 - #define GPIO_HUB_RST_N 30 - #define GPIO_UBLOX_RST_N 32 - #define GPIO_UBLOX_SAFEBOOT_N 33 - #define GPIO_GNSS_PWR_EN 34 /* SCHEMATIC LABEL: GPIO_UBLOX_PWR_EN */ - #define GPIO_STM_RST_N 124 - #define GPIO_STM_BOOT0 134 - #define GPIO_BMX_ACCEL_INT 21 - #define GPIO_BMX_GYRO_INT 23 - #define GPIO_BMX_MAGN_INT 87 - #define GPIO_LSM_INT 84 - #define GPIOCHIP_INT 0 -#else - #define GPIO_HUB_RST_N 0 - #define GPIO_UBLOX_RST_N 0 - #define GPIO_UBLOX_SAFEBOOT_N 0 - #define GPIO_GNSS_PWR_EN 0 /* SCHEMATIC LABEL: GPIO_UBLOX_PWR_EN */ - #define GPIO_STM_RST_N 0 - #define GPIO_STM_BOOT0 0 - #define GPIO_BMX_ACCEL_INT 0 - #define GPIO_BMX_GYRO_INT 0 - #define GPIO_BMX_MAGN_INT 0 - #define GPIO_LSM_INT 0 - #define GPIOCHIP_INT 0 -#endif - -int gpio_init(int pin_nr, bool output); -int gpio_set(int pin_nr, bool high); - -int gpiochip_get_ro_value_fd(const char* consumer_label, int gpiochiop_id, int pin_nr); diff --git a/common/gpio.py b/common/gpio.py index 66b2adf438..8f025a2daf 100644 --- a/common/gpio.py +++ b/common/gpio.py @@ -1,4 +1,6 @@ import os +import fcntl +import ctypes from functools import cache def gpio_init(pin: int, output: bool) -> None: @@ -52,3 +54,36 @@ def get_irqs_for_action(action: str) -> list[str]: if irq.isdigit() and action in get_irq_action(irq): ret.append(irq) return ret + +# *** gpiochip *** + +class gpioevent_data(ctypes.Structure): + _fields_ = [ + ("timestamp", ctypes.c_uint64), + ("id", ctypes.c_uint32), + ] + +class gpioevent_request(ctypes.Structure): + _fields_ = [ + ("lineoffset", ctypes.c_uint32), + ("handleflags", ctypes.c_uint32), + ("eventflags", ctypes.c_uint32), + ("label", ctypes.c_char * 32), + ("fd", ctypes.c_int) + ] + +def gpiochip_get_ro_value_fd(label: str, gpiochip_id: int, pin: int) -> int: + GPIOEVENT_REQUEST_BOTH_EDGES = 0x3 + GPIOHANDLE_REQUEST_INPUT = 0x1 + GPIO_GET_LINEEVENT_IOCTL = 0xc030b404 + + rq = gpioevent_request() + rq.lineoffset = pin + rq.handleflags = GPIOHANDLE_REQUEST_INPUT + rq.eventflags = GPIOEVENT_REQUEST_BOTH_EDGES + rq.label = label.encode('utf-8')[:31] + b'\0' + + fd = os.open(f"/dev/gpiochip{gpiochip_id}", os.O_RDONLY) + fcntl.ioctl(fd, GPIO_GET_LINEEVENT_IOCTL, rq) + os.close(fd) + return int(rq.fd) diff --git a/common/i2c.cc b/common/i2c.cc deleted file mode 100644 index 3d6c79efc5..0000000000 --- a/common/i2c.cc +++ /dev/null @@ -1,92 +0,0 @@ -#include "common/i2c.h" - -#include -#include -#include - -#include -#include -#include - -#include "common/swaglog.h" -#include "common/util.h" - -#define UNUSED(x) (void)(x) - -#ifdef QCOM2 -// TODO: decide if we want to install libi2c-dev everywhere -extern "C" { - #include - #include -} - -I2CBus::I2CBus(uint8_t bus_id) { - char bus_name[20]; - snprintf(bus_name, 20, "/dev/i2c-%d", bus_id); - - i2c_fd = HANDLE_EINTR(open(bus_name, O_RDWR)); - if (i2c_fd < 0) { - throw std::runtime_error("Failed to open I2C bus"); - } -} - -I2CBus::~I2CBus() { - if (i2c_fd >= 0) { - close(i2c_fd); - } -} - -int I2CBus::read_register(uint8_t device_address, uint register_address, uint8_t *buffer, uint8_t len) { - std::lock_guard lk(m); - - int ret = 0; - - ret = HANDLE_EINTR(ioctl(i2c_fd, I2C_SLAVE, device_address)); - if (ret < 0) { goto fail; } - - ret = i2c_smbus_read_i2c_block_data(i2c_fd, register_address, len, buffer); - if ((ret < 0) || (ret != len)) { goto fail; } - -fail: - return ret; -} - -int I2CBus::set_register(uint8_t device_address, uint register_address, uint8_t data) { - std::lock_guard lk(m); - - int ret = 0; - - ret = HANDLE_EINTR(ioctl(i2c_fd, I2C_SLAVE, device_address)); - if (ret < 0) { goto fail; } - - ret = i2c_smbus_write_byte_data(i2c_fd, register_address, data); - if (ret < 0) { goto fail; } - -fail: - return ret; -} - -#else - -I2CBus::I2CBus(uint8_t bus_id) { - UNUSED(bus_id); - i2c_fd = -1; -} - -I2CBus::~I2CBus() {} - -int I2CBus::read_register(uint8_t device_address, uint register_address, uint8_t *buffer, uint8_t len) { - UNUSED(device_address); - UNUSED(register_address); - UNUSED(buffer); - UNUSED(len); - return -1; -} - -int I2CBus::set_register(uint8_t device_address, uint register_address, uint8_t data) { - UNUSED(device_address); - UNUSED(register_address); - UNUSED(data); - return -1; -} -#endif diff --git a/common/i2c.h b/common/i2c.h deleted file mode 100644 index ca0d4635b8..0000000000 --- a/common/i2c.h +++ /dev/null @@ -1,19 +0,0 @@ -#pragma once - -#include -#include - -#include - -class I2CBus { - private: - int i2c_fd; - std::mutex m; - - public: - I2CBus(uint8_t bus_id); - ~I2CBus(); - - int read_register(uint8_t device_address, uint register_address, uint8_t *buffer, uint8_t len); - int set_register(uint8_t device_address, uint register_address, uint8_t data); -}; diff --git a/common/util.py b/common/util.py index 33885a9024..e6ddb46e7b 100644 --- a/common/util.py +++ b/common/util.py @@ -1,3 +1,25 @@ +import os +import subprocess + +def sudo_write(val: str, path: str) -> None: + try: + with open(path, 'w') as f: + f.write(str(val)) + except PermissionError: + os.system(f"sudo chmod a+w {path}") + try: + with open(path, 'w') as f: + f.write(str(val)) + except PermissionError: + # fallback for debugfs files + os.system(f"sudo su -c 'echo {val} > {path}'") + +def sudo_read(path: str) -> str: + try: + return subprocess.check_output(f"sudo cat {path}", shell=True, encoding='utf8').strip() + except Exception: + return "" + class MovingAverage: def __init__(self, window_size: int): self.window_size: int = window_size diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index d0c725cacb..f98e3bd88c 100644 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -40,14 +40,14 @@ PROCS = { "selfdrive.selfdrived.selfdrived": 16.0, "selfdrive.car.card": 26.0, "./loggerd": 14.0, - "./encoderd": 17.0, + "./encoderd": 13.0, "./camerad": 10.0, - "selfdrive.controls.plannerd": 9.0, + "selfdrive.controls.plannerd": 8.0, "./ui": 18.0, - "./sensord": 7.0, + "system.sensord.sensord": 13.0, "selfdrive.controls.radard": 2.0, "selfdrive.modeld.modeld": 22.0, - "selfdrive.modeld.dmonitoringmodeld": 21.0, + "selfdrive.modeld.dmonitoringmodeld": 18.0, "system.hardware.hardwared": 4.0, "selfdrive.locationd.calibrationd": 2.0, "selfdrive.locationd.torqued": 5.0, diff --git a/system/hardware/tici/hardware.py b/system/hardware/tici/hardware.py index f5991ec7fa..f46c94c4f0 100644 --- a/system/hardware/tici/hardware.py +++ b/system/hardware/tici/hardware.py @@ -8,6 +8,7 @@ from functools import cached_property, lru_cache from pathlib import Path from cereal import log +from openpilot.common.util import sudo_read, sudo_write from openpilot.common.gpio import gpio_set, gpio_init, get_irqs_for_action from openpilot.system.hardware.base import HardwareBase, LPABase, ThermalConfig, ThermalZone from openpilot.system.hardware.tici import iwlist @@ -61,25 +62,6 @@ MM_MODEM_ACCESS_TECHNOLOGY_UMTS = 1 << 5 MM_MODEM_ACCESS_TECHNOLOGY_LTE = 1 << 14 -def sudo_write(val, path): - try: - with open(path, 'w') as f: - f.write(str(val)) - except PermissionError: - os.system(f"sudo chmod a+w {path}") - try: - with open(path, 'w') as f: - f.write(str(val)) - except PermissionError: - # fallback for debugfs files - os.system(f"sudo su -c 'echo {val} > {path}'") - -def sudo_read(path: str) -> str: - try: - return subprocess.check_output(f"sudo cat {path}", shell=True, encoding='utf8').strip() - except Exception: - return "" - def affine_irq(val, action): irqs = get_irqs_for_action(action) if len(irqs) == 0: diff --git a/system/manager/process_config.py b/system/manager/process_config.py index e25d556037..738c20a85b 100644 --- a/system/manager/process_config.py +++ b/system/manager/process_config.py @@ -78,7 +78,7 @@ procs = [ PythonProcess("modeld", "selfdrive.modeld.modeld", only_onroad), PythonProcess("dmonitoringmodeld", "selfdrive.modeld.dmonitoringmodeld", driverview, enabled=(WEBCAM or not PC)), - NativeProcess("sensord", "system/sensord", ["./sensord"], only_onroad, enabled=not PC), + PythonProcess("sensord", "system.sensord.sensord", only_onroad, enabled=not PC), NativeProcess("ui", "selfdrive/ui", ["./ui"], always_run, watchdog_max_dt=(5 if not PC else None)), PythonProcess("soundd", "selfdrive.ui.soundd", only_onroad), PythonProcess("locationd", "selfdrive.locationd.locationd", only_onroad), diff --git a/system/sensord/.gitignore b/system/sensord/.gitignore deleted file mode 100644 index e17675e254..0000000000 --- a/system/sensord/.gitignore +++ /dev/null @@ -1 +0,0 @@ -sensord diff --git a/system/sensord/SConscript b/system/sensord/SConscript deleted file mode 100644 index 1222f0bfcd..0000000000 --- a/system/sensord/SConscript +++ /dev/null @@ -1,13 +0,0 @@ -Import('env', 'arch', 'common', 'messaging') - -sensors = [ - 'sensors/i2c_sensor.cc', - 'sensors/lsm6ds3_accel.cc', - 'sensors/lsm6ds3_gyro.cc', - 'sensors/lsm6ds3_temp.cc', - 'sensors/mmc5603nj_magn.cc', -] -libs = [common, messaging, 'pthread'] -if arch == "larch64": - libs.append('i2c') -env.Program('sensord', ['sensors_qcom2.cc'] + sensors, LIBS=libs) diff --git a/system/sensord/sensord.py b/system/sensord/sensord.py new file mode 100755 index 0000000000..ed9d0ed415 --- /dev/null +++ b/system/sensord/sensord.py @@ -0,0 +1,139 @@ +#!/usr/bin/env python3 +import os +import time +import ctypes +import select +import threading + +import cereal.messaging as messaging +from cereal.services import SERVICE_LIST +from openpilot.common.util import sudo_write +from openpilot.common.realtime import config_realtime_process, Ratekeeper +from openpilot.common.swaglog import cloudlog +from openpilot.common.gpio import gpiochip_get_ro_value_fd, gpioevent_data + +from openpilot.system.sensord.sensors.i2c_sensor import Sensor +from openpilot.system.sensord.sensors.lsm6ds3_accel import LSM6DS3_Accel +from openpilot.system.sensord.sensors.lsm6ds3_gyro import LSM6DS3_Gyro +from openpilot.system.sensord.sensors.lsm6ds3_temp import LSM6DS3_Temp +from openpilot.system.sensord.sensors.mmc5603nj_magn import MMC5603NJ_Magn + +I2C_BUS_IMU = 1 + +def interrupt_loop(sensors: list[tuple[Sensor, str, bool]], event) -> None: + pm = messaging.PubMaster([service for sensor, service, interrupt in sensors if interrupt]) + + # Requesting both edges as the data ready pulse from the lsm6ds sensor is + # very short (75us) and is mostly detected as falling edge instead of rising. + # So if it is detected as rising the following falling edge is skipped. + fd = gpiochip_get_ro_value_fd("sensord", 0, 84) + + # Configure IRQ affinity + irq_path = "/proc/irq/336/smp_affinity_list" + if not os.path.exists(irq_path): + irq_path = "/proc/irq/335/smp_affinity_list" + if os.path.exists(irq_path): + sudo_write('1\n', irq_path) + + offset = time.time_ns() - time.monotonic_ns() + + poller = select.poll() + poller.register(fd, select.POLLIN | select.POLLPRI) + while not event.is_set(): + events = poller.poll(100) + if not events: + cloudlog.error("poll timed out") + continue + if not (events[0][1] & (select.POLLIN | select.POLLPRI)): + cloudlog.error("no poll events set") + continue + + dat = os.read(fd, ctypes.sizeof(gpioevent_data)*16) + evd = gpioevent_data.from_buffer_copy(dat) + + cur_offset = time.time_ns() - time.monotonic_ns() + if abs(cur_offset - offset) > 10 * 1e6: # ms + cloudlog.warning(f"time jumped: {cur_offset} {offset}") + offset = cur_offset + continue + + ts = evd.timestamp - cur_offset + for sensor, service, interrupt in sensors: + if interrupt: + try: + evt = sensor.get_event(ts) + if not sensor.is_data_valid(): + continue + msg = messaging.new_message(service, valid=True) + setattr(msg, service, evt) + pm.send(service, msg) + except Sensor.DataNotReady: + pass + except Exception: + cloudlog.exception(f"Error processing {service}") + + +def polling_loop(sensor: Sensor, service: str, event: threading.Event) -> None: + pm = messaging.PubMaster([service]) + rk = Ratekeeper(SERVICE_LIST[service].frequency, print_delay_threshold=None) + while not event.is_set(): + try: + evt = sensor.get_event() + if not sensor.is_data_valid(): + continue + msg = messaging.new_message(service, valid=True) + setattr(msg, service, evt) + pm.send(service, msg) + except Exception: + cloudlog.exception(f"Error in {service} polling loop") + rk.keep_time() + +def main() -> None: + config_realtime_process([1, ], 1) + + sensors_cfg = [ + (LSM6DS3_Accel(I2C_BUS_IMU), "accelerometer", True), + (LSM6DS3_Gyro(I2C_BUS_IMU), "gyroscope", True), + (LSM6DS3_Temp(I2C_BUS_IMU), "temperatureSensor", False), + (MMC5603NJ_Magn(I2C_BUS_IMU), "magnetometer", False), + ] + + # Initialize sensors + exit_event = threading.Event() + threads = [ + threading.Thread(target=interrupt_loop, args=(sensors_cfg, exit_event), daemon=True) + ] + for sensor, service, interrupt in sensors_cfg: + try: + sensor.init() + if not interrupt: + # Start polling thread for sensors without interrupts + threads.append(threading.Thread( + target=polling_loop, + args=(sensor, service, exit_event), + daemon=True + )) + except Exception: + cloudlog.exception(f"Error initializing {service} sensor") + + try: + for t in threads: + t.start() + while any(t.is_alive() for t in threads): + time.sleep(1) + except KeyboardInterrupt: + pass + finally: + exit_event.set() + for t in threads: + if t.is_alive(): + t.join() + + for sensor, _, _ in sensors_cfg: + try: + sensor.shutdown() + except Exception: + cloudlog.exception("Error shutting down sensor") + +if __name__ == "__main__": + main() diff --git a/system/sensord/sensors/__init__.py b/system/sensord/sensors/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/system/sensord/sensors/constants.h b/system/sensord/sensors/constants.h deleted file mode 100644 index c216f838a5..0000000000 --- a/system/sensord/sensors/constants.h +++ /dev/null @@ -1,18 +0,0 @@ -#pragma once - - -#define SENSOR_ACCELEROMETER 1 -#define SENSOR_MAGNETOMETER 2 -#define SENSOR_MAGNETOMETER_UNCALIBRATED 3 -#define SENSOR_GYRO 4 -#define SENSOR_GYRO_UNCALIBRATED 5 -#define SENSOR_LIGHT 7 - -#define SENSOR_TYPE_ACCELEROMETER 1 -#define SENSOR_TYPE_GEOMAGNETIC_FIELD 2 -#define SENSOR_TYPE_GYROSCOPE 4 -#define SENSOR_TYPE_LIGHT 5 -#define SENSOR_TYPE_AMBIENT_TEMPERATURE 13 -#define SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED 14 -#define SENSOR_TYPE_MAGNETIC_FIELD SENSOR_TYPE_GEOMAGNETIC_FIELD -#define SENSOR_TYPE_GYROSCOPE_UNCALIBRATED 16 diff --git a/system/sensord/sensors/i2c_sensor.cc b/system/sensord/sensors/i2c_sensor.cc deleted file mode 100644 index 90220f551d..0000000000 --- a/system/sensord/sensors/i2c_sensor.cc +++ /dev/null @@ -1,50 +0,0 @@ -#include "system/sensord/sensors/i2c_sensor.h" - -int16_t read_12_bit(uint8_t lsb, uint8_t msb) { - uint16_t combined = (uint16_t(msb) << 8) | uint16_t(lsb & 0xF0); - return int16_t(combined) / (1 << 4); -} - -int16_t read_16_bit(uint8_t lsb, uint8_t msb) { - uint16_t combined = (uint16_t(msb) << 8) | uint16_t(lsb); - return int16_t(combined); -} - -int32_t read_20_bit(uint8_t b2, uint8_t b1, uint8_t b0) { - uint32_t combined = (uint32_t(b0) << 16) | (uint32_t(b1) << 8) | uint32_t(b2); - return int32_t(combined) / (1 << 4); -} - -I2CSensor::I2CSensor(I2CBus *bus, int gpio_nr, bool shared_gpio) : - bus(bus), gpio_nr(gpio_nr), shared_gpio(shared_gpio) {} - -I2CSensor::~I2CSensor() { - if (gpio_fd != -1) { - close(gpio_fd); - } -} - -int I2CSensor::read_register(uint register_address, uint8_t *buffer, uint8_t len) { - return bus->read_register(get_device_address(), register_address, buffer, len); -} - -int I2CSensor::set_register(uint register_address, uint8_t data) { - return bus->set_register(get_device_address(), register_address, data); -} - -int I2CSensor::init_gpio() { - if (shared_gpio || gpio_nr == 0) { - return 0; - } - - gpio_fd = gpiochip_get_ro_value_fd("sensord", GPIOCHIP_INT, gpio_nr); - if (gpio_fd < 0) { - return -1; - } - - return 0; -} - -bool I2CSensor::has_interrupt_enabled() { - return gpio_nr != 0; -} diff --git a/system/sensord/sensors/i2c_sensor.h b/system/sensord/sensors/i2c_sensor.h deleted file mode 100644 index e6d328ce72..0000000000 --- a/system/sensord/sensors/i2c_sensor.h +++ /dev/null @@ -1,51 +0,0 @@ -#pragma once - -#include -#include -#include -#include "cereal/gen/cpp/log.capnp.h" - -#include "common/i2c.h" -#include "common/gpio.h" - -#include "common/swaglog.h" -#include "system/sensord/sensors/constants.h" -#include "system/sensord/sensors/sensor.h" - -int16_t read_12_bit(uint8_t lsb, uint8_t msb); -int16_t read_16_bit(uint8_t lsb, uint8_t msb); -int32_t read_20_bit(uint8_t b2, uint8_t b1, uint8_t b0); - - -class I2CSensor : public Sensor { -private: - I2CBus *bus; - int gpio_nr; - bool shared_gpio; - virtual uint8_t get_device_address() = 0; - -public: - I2CSensor(I2CBus *bus, int gpio_nr = 0, bool shared_gpio = false); - ~I2CSensor(); - int read_register(uint register_address, uint8_t *buffer, uint8_t len); - int set_register(uint register_address, uint8_t data); - int init_gpio(); - bool has_interrupt_enabled(); - virtual int init() = 0; - virtual bool get_event(MessageBuilder &msg, uint64_t ts = 0) = 0; - virtual int shutdown() = 0; - - int verify_chip_id(uint8_t address, const std::vector &expected_ids) { - uint8_t chip_id = 0; - int ret = read_register(address, &chip_id, 1); - if (ret < 0) { - LOGD("Reading chip ID failed: %d", ret); - return -1; - } - for (int i = 0; i < expected_ids.size(); ++i) { - if (chip_id == expected_ids[i]) return chip_id; - } - LOGE("Chip ID wrong. Got: %d, Expected %d", chip_id, expected_ids[0]); - return -1; - } -}; diff --git a/system/sensord/sensors/i2c_sensor.py b/system/sensord/sensors/i2c_sensor.py new file mode 100644 index 0000000000..0e15a6622b --- /dev/null +++ b/system/sensord/sensors/i2c_sensor.py @@ -0,0 +1,72 @@ +import time +import smbus2 +import ctypes +from collections.abc import Iterable + +from cereal import log + +class Sensor: + class SensorException(Exception): + pass + + class DataNotReady(SensorException): + pass + + def __init__(self, bus: int) -> None: + self.bus = smbus2.SMBus(bus) + self.source = log.SensorEventData.SensorSource.velodyne # unknown + self.start_ts = 0. + + def __del__(self): + self.bus.close() + + def read(self, addr: int, length: int) -> bytes: + return bytes(self.bus.read_i2c_block_data(self.device_address, addr, length)) + + def write(self, addr: int, data: int) -> None: + self.bus.write_byte_data(self.device_address, addr, data) + + def writes(self, writes: Iterable[tuple[int, int]]) -> None: + for addr, data in writes: + self.write(addr, data) + + def verify_chip_id(self, address: int, expected_ids: list[int]) -> int: + chip_id = self.read(address, 1)[0] + assert chip_id in expected_ids + return chip_id + + # Abstract methods that must be implemented by subclasses + @property + def device_address(self) -> int: + raise NotImplementedError + + def init(self) -> None: + raise NotImplementedError + + def get_event(self, ts: int | None = None) -> log.SensorEventData: + raise NotImplementedError + + def shutdown(self) -> None: + raise NotImplementedError + + def is_data_valid(self) -> bool: + if self.start_ts == 0: + self.start_ts = time.monotonic() + + # unclear whether we need this... + return (time.monotonic() - self.start_ts) > 0.5 + + # *** helpers *** + @staticmethod + def wait(): + # a standard small sleep + time.sleep(0.005) + + @staticmethod + def parse_16bit(lsb: int, msb: int) -> int: + return ctypes.c_int16((msb << 8) | lsb).value + + @staticmethod + def parse_20bit(b2: int, b1: int, b0: int) -> int: + combined = ctypes.c_uint32((b0 << 16) | (b1 << 8) | b2).value + return ctypes.c_int32(combined).value // (1 << 4) diff --git a/system/sensord/sensors/lsm6ds3_accel.cc b/system/sensord/sensors/lsm6ds3_accel.cc deleted file mode 100644 index 03533e0657..0000000000 --- a/system/sensord/sensors/lsm6ds3_accel.cc +++ /dev/null @@ -1,250 +0,0 @@ -#include "system/sensord/sensors/lsm6ds3_accel.h" - -#include -#include -#include - -#include "common/swaglog.h" -#include "common/timing.h" -#include "common/util.h" - -LSM6DS3_Accel::LSM6DS3_Accel(I2CBus *bus, int gpio_nr, bool shared_gpio) : - I2CSensor(bus, gpio_nr, shared_gpio) {} - -void LSM6DS3_Accel::wait_for_data_ready() { - uint8_t drdy = 0; - uint8_t buffer[6]; - - do { - read_register(LSM6DS3_ACCEL_I2C_REG_STAT_REG, &drdy, sizeof(drdy)); - drdy &= LSM6DS3_ACCEL_DRDY_XLDA; - } while (drdy == 0); - - read_register(LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL, buffer, sizeof(buffer)); -} - -void LSM6DS3_Accel::read_and_avg_data(float* out_buf) { - uint8_t drdy = 0; - uint8_t buffer[6]; - - float scaling = 0.061f; - if (source == cereal::SensorEventData::SensorSource::LSM6DS3TRC) { - scaling = 0.122f; - } - - for (int i = 0; i < 5; i++) { - do { - read_register(LSM6DS3_ACCEL_I2C_REG_STAT_REG, &drdy, sizeof(drdy)); - drdy &= LSM6DS3_ACCEL_DRDY_XLDA; - } while (drdy == 0); - - int len = read_register(LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL, buffer, sizeof(buffer)); - assert(len == sizeof(buffer)); - - for (int j = 0; j < 3; j++) { - out_buf[j] += (float)read_16_bit(buffer[j*2], buffer[j*2+1]) * scaling; - } - } - - for (int i = 0; i < 3; i++) { - out_buf[i] /= 5.0f; - } -} - -int LSM6DS3_Accel::self_test(int test_type) { - float val_st_off[3] = {0}; - float val_st_on[3] = {0}; - float test_val[3] = {0}; - uint8_t ODR_FS_MO = LSM6DS3_ACCEL_ODR_52HZ; // full scale: +-2g, ODR: 52Hz - - // prepare sensor for self-test - - // enable block data update and automatic increment - int ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL3_C, LSM6DS3_ACCEL_IF_INC_BDU); - if (ret < 0) { - return ret; - } - - if (source == cereal::SensorEventData::SensorSource::LSM6DS3TRC) { - ODR_FS_MO = LSM6DS3_ACCEL_FS_4G | LSM6DS3_ACCEL_ODR_52HZ; - } - ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, ODR_FS_MO); - if (ret < 0) { - return ret; - } - - // wait for stable output, and discard first values - util::sleep_for(100); - wait_for_data_ready(); - read_and_avg_data(val_st_off); - - // enable Self Test positive (or negative) - ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL5_C, test_type); - if (ret < 0) { - return ret; - } - - // wait for stable output, and discard first values - util::sleep_for(100); - wait_for_data_ready(); - read_and_avg_data(val_st_on); - - // disable sensor - ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, 0); - if (ret < 0) { - return ret; - } - - // disable self test - ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL5_C, 0); - if (ret < 0) { - return ret; - } - - // calculate the mg values for self test - for (int i = 0; i < 3; i++) { - test_val[i] = fabs(val_st_on[i] - val_st_off[i]); - } - - // verify test result - for (int i = 0; i < 3; i++) { - if ((LSM6DS3_ACCEL_MIN_ST_LIMIT_mg > test_val[i]) || - (test_val[i] > LSM6DS3_ACCEL_MAX_ST_LIMIT_mg)) { - return -1; - } - } - - return ret; -} - -int LSM6DS3_Accel::init() { - uint8_t value = 0; - bool do_self_test = false; - - const char* env_lsm_selftest = std::getenv("LSM_SELF_TEST"); - if (env_lsm_selftest != nullptr && strncmp(env_lsm_selftest, "1", 1) == 0) { - do_self_test = true; - } - - int ret = verify_chip_id(LSM6DS3_ACCEL_I2C_REG_ID, {LSM6DS3_ACCEL_CHIP_ID, LSM6DS3TRC_ACCEL_CHIP_ID}); - if (ret == -1) return -1; - - if (ret == LSM6DS3TRC_ACCEL_CHIP_ID) { - source = cereal::SensorEventData::SensorSource::LSM6DS3TRC; - } - - ret = self_test(LSM6DS3_ACCEL_POSITIVE_TEST); - if (ret < 0) { - LOGE("LSM6DS3 accel positive self-test failed!"); - if (do_self_test) goto fail; - } - - ret = self_test(LSM6DS3_ACCEL_NEGATIVE_TEST); - if (ret < 0) { - LOGE("LSM6DS3 accel negative self-test failed!"); - if (do_self_test) goto fail; - } - - ret = init_gpio(); - if (ret < 0) { - goto fail; - } - - // enable continuous update, and automatic increase - ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL3_C, LSM6DS3_ACCEL_IF_INC); - if (ret < 0) { - goto fail; - } - - // TODO: set scale and bandwidth. Default is +- 2G, 50 Hz - ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, LSM6DS3_ACCEL_ODR_104HZ); - if (ret < 0) { - goto fail; - } - - ret = set_register(LSM6DS3_ACCEL_I2C_REG_DRDY_CFG, LSM6DS3_ACCEL_DRDY_PULSE_MODE); - if (ret < 0) { - goto fail; - } - - // enable data ready interrupt for accel on INT1 - // (without resetting existing interrupts) - ret = read_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, &value, 1); - if (ret < 0) { - goto fail; - } - - value |= LSM6DS3_ACCEL_INT1_DRDY_XL; - ret = set_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, value); - -fail: - return ret; -} - -int LSM6DS3_Accel::shutdown() { - int ret = 0; - - // disable data ready interrupt for accel on INT1 - uint8_t value = 0; - ret = read_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, &value, 1); - if (ret < 0) { - goto fail; - } - - value &= ~(LSM6DS3_ACCEL_INT1_DRDY_XL); - ret = set_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, value); - if (ret < 0) { - LOGE("Could not disable lsm6ds3 acceleration interrupt!"); - goto fail; - } - - // enable power-down mode - value = 0; - ret = read_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, &value, 1); - if (ret < 0) { - goto fail; - } - - value &= 0x0F; - ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, value); - if (ret < 0) { - LOGE("Could not power-down lsm6ds3 accelerometer!"); - goto fail; - } - -fail: - return ret; -} - -bool LSM6DS3_Accel::get_event(MessageBuilder &msg, uint64_t ts) { - - // INT1 shared with gyro, check STATUS_REG who triggered - uint8_t status_reg = 0; - read_register(LSM6DS3_ACCEL_I2C_REG_STAT_REG, &status_reg, sizeof(status_reg)); - if ((status_reg & LSM6DS3_ACCEL_DRDY_XLDA) == 0) { - return false; - } - - uint8_t buffer[6]; - int len = read_register(LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL, buffer, sizeof(buffer)); - assert(len == sizeof(buffer)); - - float scale = 9.81 * 2.0f / (1 << 15); - float x = read_16_bit(buffer[0], buffer[1]) * scale; - float y = read_16_bit(buffer[2], buffer[3]) * scale; - float z = read_16_bit(buffer[4], buffer[5]) * scale; - - auto event = msg.initEvent().initAccelerometer(); - event.setSource(source); - event.setVersion(1); - event.setSensor(SENSOR_ACCELEROMETER); - event.setType(SENSOR_TYPE_ACCELEROMETER); - event.setTimestamp(ts); - - float xyz[] = {y, -x, z}; - auto svec = event.initAcceleration(); - svec.setV(xyz); - svec.setStatus(true); - - return true; -} diff --git a/system/sensord/sensors/lsm6ds3_accel.h b/system/sensord/sensors/lsm6ds3_accel.h deleted file mode 100644 index 69667cb759..0000000000 --- a/system/sensord/sensors/lsm6ds3_accel.h +++ /dev/null @@ -1,49 +0,0 @@ -#pragma once - -#include "system/sensord/sensors/i2c_sensor.h" - -// Address of the chip on the bus -#define LSM6DS3_ACCEL_I2C_ADDR 0x6A - -// Registers of the chip -#define LSM6DS3_ACCEL_I2C_REG_DRDY_CFG 0x0B -#define LSM6DS3_ACCEL_I2C_REG_ID 0x0F -#define LSM6DS3_ACCEL_I2C_REG_INT1_CTRL 0x0D -#define LSM6DS3_ACCEL_I2C_REG_CTRL1_XL 0x10 -#define LSM6DS3_ACCEL_I2C_REG_CTRL3_C 0x12 -#define LSM6DS3_ACCEL_I2C_REG_CTRL5_C 0x14 -#define LSM6DS3_ACCEL_I2C_REG_CTR9_XL 0x18 -#define LSM6DS3_ACCEL_I2C_REG_STAT_REG 0x1E -#define LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL 0x28 - -// Constants -#define LSM6DS3_ACCEL_CHIP_ID 0x69 -#define LSM6DS3TRC_ACCEL_CHIP_ID 0x6A -#define LSM6DS3_ACCEL_FS_4G (0b10 << 2) -#define LSM6DS3_ACCEL_ODR_52HZ (0b0011 << 4) -#define LSM6DS3_ACCEL_ODR_104HZ (0b0100 << 4) -#define LSM6DS3_ACCEL_INT1_DRDY_XL 0b1 -#define LSM6DS3_ACCEL_DRDY_XLDA 0b1 -#define LSM6DS3_ACCEL_DRDY_PULSE_MODE (1 << 7) -#define LSM6DS3_ACCEL_IF_INC 0b00000100 -#define LSM6DS3_ACCEL_IF_INC_BDU 0b01000100 -#define LSM6DS3_ACCEL_XYZ_DEN 0b11100000 -#define LSM6DS3_ACCEL_POSITIVE_TEST 0b01 -#define LSM6DS3_ACCEL_NEGATIVE_TEST 0b10 -#define LSM6DS3_ACCEL_MIN_ST_LIMIT_mg 90.0f -#define LSM6DS3_ACCEL_MAX_ST_LIMIT_mg 1700.0f - -class LSM6DS3_Accel : public I2CSensor { - uint8_t get_device_address() {return LSM6DS3_ACCEL_I2C_ADDR;} - cereal::SensorEventData::SensorSource source = cereal::SensorEventData::SensorSource::LSM6DS3; - - // self test functions - int self_test(int test_type); - void wait_for_data_ready(); - void read_and_avg_data(float* val_st_off); -public: - LSM6DS3_Accel(I2CBus *bus, int gpio_nr = 0, bool shared_gpio = false); - int init(); - bool get_event(MessageBuilder &msg, uint64_t ts = 0); - int shutdown(); -}; diff --git a/system/sensord/sensors/lsm6ds3_accel.py b/system/sensord/sensors/lsm6ds3_accel.py new file mode 100644 index 0000000000..2d788fcbe2 --- /dev/null +++ b/system/sensord/sensors/lsm6ds3_accel.py @@ -0,0 +1,157 @@ +import os +import time + +from cereal import log +from openpilot.system.sensord.sensors.i2c_sensor import Sensor + +class LSM6DS3_Accel(Sensor): + LSM6DS3_ACCEL_I2C_REG_DRDY_CFG = 0x0B + LSM6DS3_ACCEL_I2C_REG_INT1_CTRL = 0x0D + LSM6DS3_ACCEL_I2C_REG_CTRL1_XL = 0x10 + LSM6DS3_ACCEL_I2C_REG_CTRL3_C = 0x12 + LSM6DS3_ACCEL_I2C_REG_CTRL5_C = 0x14 + LSM6DS3_ACCEL_I2C_REG_STAT_REG = 0x1E + LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL = 0x28 + + LSM6DS3_ACCEL_ODR_104HZ = (0b0100 << 4) + LSM6DS3_ACCEL_INT1_DRDY_XL = 0b1 + LSM6DS3_ACCEL_DRDY_XLDA = 0b1 + LSM6DS3_ACCEL_DRDY_PULSE_MODE = (1 << 7) + LSM6DS3_ACCEL_IF_INC = 0b00000100 + + LSM6DS3_ACCEL_ODR_52HZ = (0b0011 << 4) + LSM6DS3_ACCEL_FS_4G = (0b10 << 2) + LSM6DS3_ACCEL_IF_INC_BDU = 0b01000100 + LSM6DS3_ACCEL_POSITIVE_TEST = 0b01 + LSM6DS3_ACCEL_NEGATIVE_TEST = 0b10 + LSM6DS3_ACCEL_MIN_ST_LIMIT_mg = 90.0 + LSM6DS3_ACCEL_MAX_ST_LIMIT_mg = 1700.0 + + @property + def device_address(self) -> int: + return 0x6A + + def init(self): + chip_id = self.verify_chip_id(0x0F, [0x69, 0x6A]) + if chip_id == 0x6A: + self.source = log.SensorEventData.SensorSource.lsm6ds3trc + else: + self.source = log.SensorEventData.SensorSource.lsm6ds3 + + # self-test + if os.getenv("LSM_SELF_TEST") == "1": + self.self_test(self.LSM6DS3_ACCEL_POSITIVE_TEST) + self.self_test(self.LSM6DS3_ACCEL_NEGATIVE_TEST) + + # actual init + int1 = self.read(self.LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, 1)[0] + int1 |= self.LSM6DS3_ACCEL_INT1_DRDY_XL + self.writes(( + # Enable continuous update and automatic address increment + (self.LSM6DS3_ACCEL_I2C_REG_CTRL3_C, self.LSM6DS3_ACCEL_IF_INC), + # Set ODR to 104 Hz, FS to ±2g (default) + (self.LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, self.LSM6DS3_ACCEL_ODR_104HZ), + # Configure data ready signal to pulse mode + (self.LSM6DS3_ACCEL_I2C_REG_DRDY_CFG, self.LSM6DS3_ACCEL_DRDY_PULSE_MODE), + # Enable data ready interrupt on INT1 without resetting existing interrupts + (self.LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, int1), + )) + + def get_event(self, ts: int | None = None) -> log.SensorEventData: + assert ts is not None # must come from the IRQ event + + # Check if data is ready since IRQ is shared with gyro + status_reg = self.read(self.LSM6DS3_ACCEL_I2C_REG_STAT_REG, 1)[0] + if (status_reg & self.LSM6DS3_ACCEL_DRDY_XLDA) == 0: + raise self.DataNotReady + + scale = 9.81 * 2.0 / (1 << 15) + b = self.read(self.LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL, 6) + x = self.parse_16bit(b[0], b[1]) * scale + y = self.parse_16bit(b[2], b[3]) * scale + z = self.parse_16bit(b[4], b[5]) * scale + + event = log.SensorEventData.new_message() + event.timestamp = ts + event.version = 1 + event.sensor = 1 # SENSOR_ACCELEROMETER + event.type = 1 # SENSOR_TYPE_ACCELEROMETER + event.source = self.source + a = event.init('acceleration') + a.v = [y, -x, z] + a.status = 1 + return event + + def shutdown(self) -> None: + # Disable data ready interrupt on INT1 + value = self.read(self.LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, 1)[0] + value &= ~self.LSM6DS3_ACCEL_INT1_DRDY_XL + self.write(self.LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, value) + + # Power down by clearing ODR bits + value = self.read(self.LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, 1)[0] + value &= 0x0F + self.write(self.LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, value) + + # *** self-test stuff *** + def _wait_for_data_ready(self): + while True: + drdy = self.read(self.LSM6DS3_ACCEL_I2C_REG_STAT_REG, 1)[0] + if drdy & self.LSM6DS3_ACCEL_DRDY_XLDA: + break + + def _read_and_avg_data(self, scaling: float) -> list[float]: + out_buf = [0.0, 0.0, 0.0] + for _ in range(5): + self._wait_for_data_ready() + b = self.read(self.LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL, 6) + for j in range(3): + val = self.parse_16bit(b[j*2], b[j*2+1]) * scaling + out_buf[j] += val + return [x / 5.0 for x in out_buf] + + def self_test(self, test_type: int) -> None: + # Prepare sensor for self-test + self.write(self.LSM6DS3_ACCEL_I2C_REG_CTRL3_C, self.LSM6DS3_ACCEL_IF_INC_BDU) + + # Configure ODR and full scale based on sensor type + if self.source == log.SensorEventData.SensorSource.lsm6ds3trc: + odr_fs = self.LSM6DS3_ACCEL_FS_4G | self.LSM6DS3_ACCEL_ODR_52HZ + scaling = 0.122 # mg/LSB for ±4g + else: + odr_fs = self.LSM6DS3_ACCEL_ODR_52HZ + scaling = 0.061 # mg/LSB for ±2g + self.write(self.LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, odr_fs) + + # Wait for stable output + time.sleep(0.1) + self._wait_for_data_ready() + val_st_off = self._read_and_avg_data(scaling) + + # Enable self-test + self.write(self.LSM6DS3_ACCEL_I2C_REG_CTRL5_C, test_type) + + # Wait for stable output + time.sleep(0.1) + self._wait_for_data_ready() + val_st_on = self._read_and_avg_data(scaling) + + # Disable sensor and self-test + self.write(self.LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, 0) + self.write(self.LSM6DS3_ACCEL_I2C_REG_CTRL5_C, 0) + + # Calculate differences and check limits + test_val = [abs(on - off) for on, off in zip(val_st_on, val_st_off, strict=False)] + for val in test_val: + if val < self.LSM6DS3_ACCEL_MIN_ST_LIMIT_mg or val > self.LSM6DS3_ACCEL_MAX_ST_LIMIT_mg: + raise self.SensorException(f"Accelerometer self-test failed for test type {test_type}") + +if __name__ == "__main__": + import numpy as np + s = LSM6DS3_Accel(1) + s.init() + time.sleep(0.2) + e = s.get_event(0) + print(e) + print(np.linalg.norm(e.acceleration.v)) + s.shutdown() diff --git a/system/sensord/sensors/lsm6ds3_gyro.cc b/system/sensord/sensors/lsm6ds3_gyro.cc deleted file mode 100644 index bb560edeab..0000000000 --- a/system/sensord/sensors/lsm6ds3_gyro.cc +++ /dev/null @@ -1,233 +0,0 @@ -#include "system/sensord/sensors/lsm6ds3_gyro.h" - -#include -#include -#include - -#include "common/swaglog.h" -#include "common/timing.h" -#include "common/util.h" - -#define DEG2RAD(x) ((x) * M_PI / 180.0) - -LSM6DS3_Gyro::LSM6DS3_Gyro(I2CBus *bus, int gpio_nr, bool shared_gpio) : - I2CSensor(bus, gpio_nr, shared_gpio) {} - -void LSM6DS3_Gyro::wait_for_data_ready() { - uint8_t drdy = 0; - uint8_t buffer[6]; - - do { - read_register(LSM6DS3_GYRO_I2C_REG_STAT_REG, &drdy, sizeof(drdy)); - drdy &= LSM6DS3_GYRO_DRDY_GDA; - } while (drdy == 0); - - read_register(LSM6DS3_GYRO_I2C_REG_OUTX_L_G, buffer, sizeof(buffer)); -} - -void LSM6DS3_Gyro::read_and_avg_data(float* out_buf) { - uint8_t drdy = 0; - uint8_t buffer[6]; - - for (int i = 0; i < 5; i++) { - do { - read_register(LSM6DS3_GYRO_I2C_REG_STAT_REG, &drdy, sizeof(drdy)); - drdy &= LSM6DS3_GYRO_DRDY_GDA; - } while (drdy == 0); - - int len = read_register(LSM6DS3_GYRO_I2C_REG_OUTX_L_G, buffer, sizeof(buffer)); - assert(len == sizeof(buffer)); - - for (int j = 0; j < 3; j++) { - out_buf[j] += (float)read_16_bit(buffer[j*2], buffer[j*2+1]) * 70.0f; - } - } - - // calculate the mg average values - for (int i = 0; i < 3; i++) { - out_buf[i] /= 5.0f; - } -} - -int LSM6DS3_Gyro::self_test(int test_type) { - float val_st_off[3] = {0}; - float val_st_on[3] = {0}; - float test_val[3] = {0}; - - // prepare sensor for self-test - - // full scale: 2000dps, ODR: 208Hz - int ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, LSM6DS3_GYRO_ODR_208HZ | LSM6DS3_GYRO_FS_2000dps); - if (ret < 0) { - return ret; - } - - // wait for stable output, and discard first values - util::sleep_for(150); - wait_for_data_ready(); - read_and_avg_data(val_st_off); - - // enable Self Test positive (or negative) - ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL5_C, test_type); - if (ret < 0) { - return ret; - } - - // wait for stable output, and discard first values - util::sleep_for(50); - wait_for_data_ready(); - read_and_avg_data(val_st_on); - - // disable sensor - ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, 0); - if (ret < 0) { - return ret; - } - - // disable self test - ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL5_C, 0); - if (ret < 0) { - return ret; - } - - // calculate the mg values for self test - for (int i = 0; i < 3; i++) { - test_val[i] = fabs(val_st_on[i] - val_st_off[i]); - } - - // verify test result - for (int i = 0; i < 3; i++) { - if ((LSM6DS3_GYRO_MIN_ST_LIMIT_mdps > test_val[i]) || - (test_val[i] > LSM6DS3_GYRO_MAX_ST_LIMIT_mdps)) { - return -1; - } - } - - return ret; -} - -int LSM6DS3_Gyro::init() { - uint8_t value = 0; - bool do_self_test = false; - - const char* env_lsm_selftest = std::getenv("LSM_SELF_TEST"); - if (env_lsm_selftest != nullptr && strncmp(env_lsm_selftest, "1", 1) == 0) { - do_self_test = true; - } - - int ret = verify_chip_id(LSM6DS3_GYRO_I2C_REG_ID, {LSM6DS3_GYRO_CHIP_ID, LSM6DS3TRC_GYRO_CHIP_ID}); - if (ret == -1) return -1; - - if (ret == LSM6DS3TRC_GYRO_CHIP_ID) { - source = cereal::SensorEventData::SensorSource::LSM6DS3TRC; - } - - ret = init_gpio(); - if (ret < 0) { - goto fail; - } - - ret = self_test(LSM6DS3_GYRO_POSITIVE_TEST); - if (ret < 0) { - LOGE("LSM6DS3 gyro positive self-test failed!"); - if (do_self_test) goto fail; - } - - ret = self_test(LSM6DS3_GYRO_NEGATIVE_TEST); - if (ret < 0) { - LOGE("LSM6DS3 gyro negative self-test failed!"); - if (do_self_test) goto fail; - } - - // TODO: set scale. Default is +- 250 deg/s - ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, LSM6DS3_GYRO_ODR_104HZ); - if (ret < 0) { - goto fail; - } - - ret = set_register(LSM6DS3_GYRO_I2C_REG_DRDY_CFG, LSM6DS3_GYRO_DRDY_PULSE_MODE); - if (ret < 0) { - goto fail; - } - - // enable data ready interrupt for gyro on INT1 - // (without resetting existing interrupts) - ret = read_register(LSM6DS3_GYRO_I2C_REG_INT1_CTRL, &value, 1); - if (ret < 0) { - goto fail; - } - - value |= LSM6DS3_GYRO_INT1_DRDY_G; - ret = set_register(LSM6DS3_GYRO_I2C_REG_INT1_CTRL, value); - -fail: - return ret; -} - -int LSM6DS3_Gyro::shutdown() { - int ret = 0; - - // disable data ready interrupt for gyro on INT1 - uint8_t value = 0; - ret = read_register(LSM6DS3_GYRO_I2C_REG_INT1_CTRL, &value, 1); - if (ret < 0) { - goto fail; - } - - value &= ~(LSM6DS3_GYRO_INT1_DRDY_G); - ret = set_register(LSM6DS3_GYRO_I2C_REG_INT1_CTRL, value); - if (ret < 0) { - LOGE("Could not disable lsm6ds3 gyroscope interrupt!"); - goto fail; - } - - // enable power-down mode - value = 0; - ret = read_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, &value, 1); - if (ret < 0) { - goto fail; - } - - value &= 0x0F; - ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, value); - if (ret < 0) { - LOGE("Could not power-down lsm6ds3 gyroscope!"); - goto fail; - } - -fail: - return ret; -} - -bool LSM6DS3_Gyro::get_event(MessageBuilder &msg, uint64_t ts) { - - // INT1 shared with accel, check STATUS_REG who triggered - uint8_t status_reg = 0; - read_register(LSM6DS3_GYRO_I2C_REG_STAT_REG, &status_reg, sizeof(status_reg)); - if ((status_reg & LSM6DS3_GYRO_DRDY_GDA) == 0) { - return false; - } - - uint8_t buffer[6]; - int len = read_register(LSM6DS3_GYRO_I2C_REG_OUTX_L_G, buffer, sizeof(buffer)); - assert(len == sizeof(buffer)); - - float scale = 8.75 / 1000.0; - float x = DEG2RAD(read_16_bit(buffer[0], buffer[1]) * scale); - float y = DEG2RAD(read_16_bit(buffer[2], buffer[3]) * scale); - float z = DEG2RAD(read_16_bit(buffer[4], buffer[5]) * scale); - - auto event = msg.initEvent().initGyroscope(); - event.setSource(source); - event.setVersion(2); - event.setSensor(SENSOR_GYRO_UNCALIBRATED); - event.setType(SENSOR_TYPE_GYROSCOPE_UNCALIBRATED); - event.setTimestamp(ts); - - float xyz[] = {y, -x, z}; - auto svec = event.initGyroUncalibrated(); - svec.setV(xyz); - svec.setStatus(true); - - return true; -} diff --git a/system/sensord/sensors/lsm6ds3_gyro.h b/system/sensord/sensors/lsm6ds3_gyro.h deleted file mode 100644 index adaae62dd2..0000000000 --- a/system/sensord/sensors/lsm6ds3_gyro.h +++ /dev/null @@ -1,45 +0,0 @@ -#pragma once - -#include "system/sensord/sensors/i2c_sensor.h" - -// Address of the chip on the bus -#define LSM6DS3_GYRO_I2C_ADDR 0x6A - -// Registers of the chip -#define LSM6DS3_GYRO_I2C_REG_DRDY_CFG 0x0B -#define LSM6DS3_GYRO_I2C_REG_ID 0x0F -#define LSM6DS3_GYRO_I2C_REG_INT1_CTRL 0x0D -#define LSM6DS3_GYRO_I2C_REG_CTRL2_G 0x11 -#define LSM6DS3_GYRO_I2C_REG_CTRL5_C 0x14 -#define LSM6DS3_GYRO_I2C_REG_STAT_REG 0x1E -#define LSM6DS3_GYRO_I2C_REG_OUTX_L_G 0x22 -#define LSM6DS3_GYRO_POSITIVE_TEST (0b01 << 2) -#define LSM6DS3_GYRO_NEGATIVE_TEST (0b11 << 2) - -// Constants -#define LSM6DS3_GYRO_CHIP_ID 0x69 -#define LSM6DS3TRC_GYRO_CHIP_ID 0x6A -#define LSM6DS3_GYRO_FS_2000dps (0b11 << 2) -#define LSM6DS3_GYRO_ODR_104HZ (0b0100 << 4) -#define LSM6DS3_GYRO_ODR_208HZ (0b0101 << 4) -#define LSM6DS3_GYRO_INT1_DRDY_G 0b10 -#define LSM6DS3_GYRO_DRDY_GDA 0b10 -#define LSM6DS3_GYRO_DRDY_PULSE_MODE (1 << 7) -#define LSM6DS3_GYRO_MIN_ST_LIMIT_mdps 150000.0f -#define LSM6DS3_GYRO_MAX_ST_LIMIT_mdps 700000.0f - - -class LSM6DS3_Gyro : public I2CSensor { - uint8_t get_device_address() {return LSM6DS3_GYRO_I2C_ADDR;} - cereal::SensorEventData::SensorSource source = cereal::SensorEventData::SensorSource::LSM6DS3; - - // self test functions - int self_test(int test_type); - void wait_for_data_ready(); - void read_and_avg_data(float* val_st_off); -public: - LSM6DS3_Gyro(I2CBus *bus, int gpio_nr = 0, bool shared_gpio = false); - int init(); - bool get_event(MessageBuilder &msg, uint64_t ts = 0); - int shutdown(); -}; diff --git a/system/sensord/sensors/lsm6ds3_gyro.py b/system/sensord/sensors/lsm6ds3_gyro.py new file mode 100644 index 0000000000..68fd267df2 --- /dev/null +++ b/system/sensord/sensors/lsm6ds3_gyro.py @@ -0,0 +1,141 @@ +import os +import math +import time + +from cereal import log +from openpilot.system.sensord.sensors.i2c_sensor import Sensor + +class LSM6DS3_Gyro(Sensor): + LSM6DS3_GYRO_I2C_REG_DRDY_CFG = 0x0B + LSM6DS3_GYRO_I2C_REG_INT1_CTRL = 0x0D + LSM6DS3_GYRO_I2C_REG_CTRL2_G = 0x11 + LSM6DS3_GYRO_I2C_REG_CTRL5_C = 0x14 + LSM6DS3_GYRO_I2C_REG_STAT_REG = 0x1E + LSM6DS3_GYRO_I2C_REG_OUTX_L_G = 0x22 + + LSM6DS3_GYRO_ODR_104HZ = (0b0100 << 4) + LSM6DS3_GYRO_INT1_DRDY_G = 0b10 + LSM6DS3_GYRO_DRDY_GDA = 0b10 + LSM6DS3_GYRO_DRDY_PULSE_MODE = (1 << 7) + + LSM6DS3_GYRO_ODR_208HZ = (0b0101 << 4) + LSM6DS3_GYRO_FS_2000dps = (0b11 << 2) + LSM6DS3_GYRO_POSITIVE_TEST = (0b01 << 2) + LSM6DS3_GYRO_NEGATIVE_TEST = (0b11 << 2) + LSM6DS3_GYRO_MIN_ST_LIMIT_mdps = 150000.0 + LSM6DS3_GYRO_MAX_ST_LIMIT_mdps = 700000.0 + + @property + def device_address(self) -> int: + return 0x6A + + def init(self): + chip_id = self.verify_chip_id(0x0F, [0x69, 0x6A]) + if chip_id == 0x6A: + self.source = log.SensorEventData.SensorSource.lsm6ds3trc + else: + self.source = log.SensorEventData.SensorSource.lsm6ds3 + + # self-test + if "LSM_SELF_TEST" in os.environ: + self.self_test(self.LSM6DS3_GYRO_POSITIVE_TEST) + self.self_test(self.LSM6DS3_GYRO_NEGATIVE_TEST) + + # actual init + self.writes(( + # TODO: set scale. Default is +- 250 deg/s + (self.LSM6DS3_GYRO_I2C_REG_CTRL2_G, self.LSM6DS3_GYRO_ODR_104HZ), + # Configure data ready signal to pulse mode + (self.LSM6DS3_GYRO_I2C_REG_DRDY_CFG, self.LSM6DS3_GYRO_DRDY_PULSE_MODE), + )) + value = self.read(self.LSM6DS3_GYRO_I2C_REG_INT1_CTRL, 1)[0] + value |= self.LSM6DS3_GYRO_INT1_DRDY_G + self.write(self.LSM6DS3_GYRO_I2C_REG_INT1_CTRL, value) + + def get_event(self, ts: int | None = None) -> log.SensorEventData: + assert ts is not None # must come from the IRQ event + + # Check if gyroscope data is ready, since it's shared with accelerometer + status_reg = self.read(self.LSM6DS3_GYRO_I2C_REG_STAT_REG, 1)[0] + if not (status_reg & self.LSM6DS3_GYRO_DRDY_GDA): + raise self.DataNotReady + + b = self.read(self.LSM6DS3_GYRO_I2C_REG_OUTX_L_G, 6) + x = self.parse_16bit(b[0], b[1]) + y = self.parse_16bit(b[2], b[3]) + z = self.parse_16bit(b[4], b[5]) + scale = (8.75 / 1000.0) * (math.pi / 180.0) + xyz = [y * scale, -x * scale, z * scale] + + event = log.SensorEventData.new_message() + event.timestamp = ts + event.version = 2 + event.sensor = 5 # SENSOR_GYRO_UNCALIBRATED + event.type = 16 # SENSOR_TYPE_GYROSCOPE_UNCALIBRATED + event.source = self.source + g = event.init('gyroUncalibrated') + g.v = xyz + g.status = 1 + return event + + def shutdown(self) -> None: + # Disable data ready interrupt on INT1 + value = self.read(self.LSM6DS3_GYRO_I2C_REG_INT1_CTRL, 1)[0] + value &= ~self.LSM6DS3_GYRO_INT1_DRDY_G + self.write(self.LSM6DS3_GYRO_I2C_REG_INT1_CTRL, value) + + # Power down by clearing ODR bits + value = self.read(self.LSM6DS3_GYRO_I2C_REG_CTRL2_G, 1)[0] + value &= 0x0F + self.write(self.LSM6DS3_GYRO_I2C_REG_CTRL2_G, value) + + # *** self-test stuff *** + def _wait_for_data_ready(self): + while True: + drdy = self.read(self.LSM6DS3_GYRO_I2C_REG_STAT_REG, 1)[0] + if drdy & self.LSM6DS3_GYRO_DRDY_GDA: + break + + def _read_and_avg_data(self) -> list[float]: + out_buf = [0.0, 0.0, 0.0] + for _ in range(5): + self._wait_for_data_ready() + b = self.read(self.LSM6DS3_GYRO_I2C_REG_OUTX_L_G, 6) + for j in range(3): + val = self.parse_16bit(b[j*2], b[j*2+1]) * 70.0 # mdps/LSB for 2000 dps + out_buf[j] += val + return [x / 5.0 for x in out_buf] + + def self_test(self, test_type: int): + # Set ODR to 208Hz, FS to 2000dps + self.write(self.LSM6DS3_GYRO_I2C_REG_CTRL2_G, self.LSM6DS3_GYRO_ODR_208HZ | self.LSM6DS3_GYRO_FS_2000dps) + + # Wait for stable output + time.sleep(0.15) + self._wait_for_data_ready() + val_st_off = self._read_and_avg_data() + + # Enable self-test + self.write(self.LSM6DS3_GYRO_I2C_REG_CTRL5_C, test_type) + + # Wait for stable output + time.sleep(0.05) + self._wait_for_data_ready() + val_st_on = self._read_and_avg_data() + + # Disable sensor and self-test + self.write(self.LSM6DS3_GYRO_I2C_REG_CTRL2_G, 0) + self.write(self.LSM6DS3_GYRO_I2C_REG_CTRL5_C, 0) + + # Calculate differences and check limits + test_val = [abs(on - off) for on, off in zip(val_st_on, val_st_off, strict=False)] + for val in test_val: + if val < self.LSM6DS3_GYRO_MIN_ST_LIMIT_mdps or val > self.LSM6DS3_GYRO_MAX_ST_LIMIT_mdps: + raise Exception(f"Gyroscope self-test failed for test type {test_type}") + +if __name__ == "__main__": + s = LSM6DS3_Gyro(1) + s.init() + time.sleep(0.1) + print(s.get_event(0)) + s.shutdown() diff --git a/system/sensord/sensors/lsm6ds3_temp.cc b/system/sensord/sensors/lsm6ds3_temp.cc deleted file mode 100644 index f481614154..0000000000 --- a/system/sensord/sensors/lsm6ds3_temp.cc +++ /dev/null @@ -1,37 +0,0 @@ -#include "system/sensord/sensors/lsm6ds3_temp.h" - -#include - -#include "common/swaglog.h" -#include "common/timing.h" - -LSM6DS3_Temp::LSM6DS3_Temp(I2CBus *bus) : I2CSensor(bus) {} - -int LSM6DS3_Temp::init() { - int ret = verify_chip_id(LSM6DS3_TEMP_I2C_REG_ID, {LSM6DS3_TEMP_CHIP_ID, LSM6DS3TRC_TEMP_CHIP_ID}); - if (ret == -1) return -1; - - if (ret == LSM6DS3TRC_TEMP_CHIP_ID) { - source = cereal::SensorEventData::SensorSource::LSM6DS3TRC; - } - return 0; -} - -bool LSM6DS3_Temp::get_event(MessageBuilder &msg, uint64_t ts) { - uint64_t start_time = nanos_since_boot(); - uint8_t buffer[2]; - int len = read_register(LSM6DS3_TEMP_I2C_REG_OUT_TEMP_L, buffer, sizeof(buffer)); - assert(len == sizeof(buffer)); - - float scale = (source == cereal::SensorEventData::SensorSource::LSM6DS3TRC) ? 256.0f : 16.0f; - float temp = 25.0f + read_16_bit(buffer[0], buffer[1]) / scale; - - auto event = msg.initEvent().initTemperatureSensor(); - event.setSource(source); - event.setVersion(1); - event.setType(SENSOR_TYPE_AMBIENT_TEMPERATURE); - event.setTimestamp(start_time); - event.setTemperature(temp); - - return true; -} diff --git a/system/sensord/sensors/lsm6ds3_temp.h b/system/sensord/sensors/lsm6ds3_temp.h deleted file mode 100644 index 1b5b621814..0000000000 --- a/system/sensord/sensors/lsm6ds3_temp.h +++ /dev/null @@ -1,26 +0,0 @@ -#pragma once - -#include "system/sensord/sensors/i2c_sensor.h" - -// Address of the chip on the bus -#define LSM6DS3_TEMP_I2C_ADDR 0x6A - -// Registers of the chip -#define LSM6DS3_TEMP_I2C_REG_ID 0x0F -#define LSM6DS3_TEMP_I2C_REG_OUT_TEMP_L 0x20 - -// Constants -#define LSM6DS3_TEMP_CHIP_ID 0x69 -#define LSM6DS3TRC_TEMP_CHIP_ID 0x6A - - -class LSM6DS3_Temp : public I2CSensor { - uint8_t get_device_address() {return LSM6DS3_TEMP_I2C_ADDR;} - cereal::SensorEventData::SensorSource source = cereal::SensorEventData::SensorSource::LSM6DS3; - -public: - LSM6DS3_Temp(I2CBus *bus); - int init(); - bool get_event(MessageBuilder &msg, uint64_t ts = 0); - int shutdown() { return 0; } -}; diff --git a/system/sensord/sensors/lsm6ds3_temp.py b/system/sensord/sensors/lsm6ds3_temp.py new file mode 100644 index 0000000000..7d7b1c2ce1 --- /dev/null +++ b/system/sensord/sensors/lsm6ds3_temp.py @@ -0,0 +1,33 @@ +import time + +from cereal import log +from openpilot.system.sensord.sensors.i2c_sensor import Sensor + +# https://content.arduino.cc/assets/st_imu_lsm6ds3_datasheet.pdf +class LSM6DS3_Temp(Sensor): + @property + def device_address(self) -> int: + return 0x6A # Default I2C address for LSM6DS3 + + def _read_temperature(self) -> float: + scale = 16.0 if log.SensorEventData.SensorSource.lsm6ds3 else 256.0 + data = self.read(0x20, 2) + return 25 + (self.parse_16bit(data[0], data[1]) / scale) + + def init(self): + chip_id = self.verify_chip_id(0x0F, [0x69, 0x6A]) + if chip_id == 0x6A: + self.source = log.SensorEventData.SensorSource.lsm6ds3trc + else: + self.source = log.SensorEventData.SensorSource.lsm6ds3 + + def get_event(self, ts: int | None = None) -> log.SensorEventData: + event = log.SensorEventData.new_message() + event.version = 1 + event.timestamp = int(time.monotonic() * 1e9) + event.source = self.source + event.temperature = self._read_temperature() + return event + + def shutdown(self) -> None: + pass diff --git a/system/sensord/sensors/mmc5603nj_magn.cc b/system/sensord/sensors/mmc5603nj_magn.cc deleted file mode 100644 index 0e8ba967e3..0000000000 --- a/system/sensord/sensors/mmc5603nj_magn.cc +++ /dev/null @@ -1,108 +0,0 @@ -#include "system/sensord/sensors/mmc5603nj_magn.h" - -#include -#include -#include - -#include "common/swaglog.h" -#include "common/timing.h" -#include "common/util.h" - -MMC5603NJ_Magn::MMC5603NJ_Magn(I2CBus *bus) : I2CSensor(bus) {} - -int MMC5603NJ_Magn::init() { - int ret = verify_chip_id(MMC5603NJ_I2C_REG_ID, {MMC5603NJ_CHIP_ID}); - if (ret == -1) return -1; - - // Set ODR to 0 - ret = set_register(MMC5603NJ_I2C_REG_ODR, 0); - if (ret < 0) { - goto fail; - } - - // Set BW to 0b01 for 1-150 Hz operation - ret = set_register(MMC5603NJ_I2C_REG_INTERNAL_1, 0b01); - if (ret < 0) { - goto fail; - } - -fail: - return ret; -} - -int MMC5603NJ_Magn::shutdown() { - int ret = 0; - - // disable auto reset of measurements - uint8_t value = 0; - ret = read_register(MMC5603NJ_I2C_REG_INTERNAL_0, &value, 1); - if (ret < 0) { - goto fail; - } - - value &= ~(MMC5603NJ_CMM_FREQ_EN | MMC5603NJ_AUTO_SR_EN); - ret = set_register(MMC5603NJ_I2C_REG_INTERNAL_0, value); - if (ret < 0) { - goto fail; - } - - // set ODR to 0 to leave continuous mode - ret = set_register(MMC5603NJ_I2C_REG_ODR, 0); - if (ret < 0) { - goto fail; - } - return ret; - -fail: - LOGE("Could not disable mmc5603nj auto set reset"); - return ret; -} - -void MMC5603NJ_Magn::start_measurement() { - set_register(MMC5603NJ_I2C_REG_INTERNAL_0, 0b01); - util::sleep_for(5); -} - -std::vector MMC5603NJ_Magn::read_measurement() { - int len; - uint8_t buffer[9]; - len = read_register(MMC5603NJ_I2C_REG_XOUT0, buffer, sizeof(buffer)); - assert(len == sizeof(buffer)); - float scale = 1.0 / 16384.0; - float x = (read_20_bit(buffer[6], buffer[1], buffer[0]) * scale) - 32.0; - float y = (read_20_bit(buffer[7], buffer[3], buffer[2]) * scale) - 32.0; - float z = (read_20_bit(buffer[8], buffer[5], buffer[4]) * scale) - 32.0; - std::vector xyz = {x, y, z}; - return xyz; -} - -bool MMC5603NJ_Magn::get_event(MessageBuilder &msg, uint64_t ts) { - uint64_t start_time = nanos_since_boot(); - // SET - RESET cycle - set_register(MMC5603NJ_I2C_REG_INTERNAL_0, MMC5603NJ_SET); - util::sleep_for(5); - MMC5603NJ_Magn::start_measurement(); - std::vector xyz = MMC5603NJ_Magn::read_measurement(); - - set_register(MMC5603NJ_I2C_REG_INTERNAL_0, MMC5603NJ_RESET); - util::sleep_for(5); - MMC5603NJ_Magn::start_measurement(); - std::vector reset_xyz = MMC5603NJ_Magn::read_measurement(); - - auto event = msg.initEvent().initMagnetometer(); - event.setSource(cereal::SensorEventData::SensorSource::MMC5603NJ); - event.setVersion(1); - event.setSensor(SENSOR_MAGNETOMETER_UNCALIBRATED); - event.setType(SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED); - event.setTimestamp(start_time); - - float vals[] = {xyz[0], xyz[1], xyz[2], reset_xyz[0], reset_xyz[1], reset_xyz[2]}; - bool valid = true; - if (std::any_of(std::begin(vals), std::end(vals), [](float val) { return val == -32.0; })) { - valid = false; - } - auto svec = event.initMagneticUncalibrated(); - svec.setV(vals); - svec.setStatus(valid); - return true; -} diff --git a/system/sensord/sensors/mmc5603nj_magn.h b/system/sensord/sensors/mmc5603nj_magn.h deleted file mode 100644 index 9c0fbd2521..0000000000 --- a/system/sensord/sensors/mmc5603nj_magn.h +++ /dev/null @@ -1,37 +0,0 @@ -#pragma once - -#include - -#include "system/sensord/sensors/i2c_sensor.h" - -// Address of the chip on the bus -#define MMC5603NJ_I2C_ADDR 0x30 - -// Registers of the chip -#define MMC5603NJ_I2C_REG_XOUT0 0x00 -#define MMC5603NJ_I2C_REG_ODR 0x1A -#define MMC5603NJ_I2C_REG_INTERNAL_0 0x1B -#define MMC5603NJ_I2C_REG_INTERNAL_1 0x1C -#define MMC5603NJ_I2C_REG_INTERNAL_2 0x1D -#define MMC5603NJ_I2C_REG_ID 0x39 - -// Constants -#define MMC5603NJ_CHIP_ID 0x10 -#define MMC5603NJ_CMM_FREQ_EN (1 << 7) -#define MMC5603NJ_AUTO_SR_EN (1 << 5) -#define MMC5603NJ_CMM_EN (1 << 4) -#define MMC5603NJ_EN_PRD_SET (1 << 3) -#define MMC5603NJ_SET (1 << 3) -#define MMC5603NJ_RESET (1 << 4) - -class MMC5603NJ_Magn : public I2CSensor { -private: - uint8_t get_device_address() {return MMC5603NJ_I2C_ADDR;} - void start_measurement(); - std::vector read_measurement(); -public: - MMC5603NJ_Magn(I2CBus *bus); - int init(); - bool get_event(MessageBuilder &msg, uint64_t ts = 0); - int shutdown(); -}; diff --git a/system/sensord/sensors/mmc5603nj_magn.py b/system/sensord/sensors/mmc5603nj_magn.py new file mode 100644 index 0000000000..255e99eb3e --- /dev/null +++ b/system/sensord/sensors/mmc5603nj_magn.py @@ -0,0 +1,76 @@ +import time + +from cereal import log +from openpilot.system.sensord.sensors.i2c_sensor import Sensor + +# https://www.mouser.com/datasheet/2/821/Memsic_09102019_Datasheet_Rev.B-1635324.pdf + +# Register addresses +REG_ODR = 0x1A +REG_INTERNAL_0 = 0x1B +REG_INTERNAL_1 = 0x1C + +# Control register settings +CMM_FREQ_EN = (1 << 7) +AUTO_SR_EN = (1 << 5) +SET = (1 << 3) +RESET = (1 << 4) + +class MMC5603NJ_Magn(Sensor): + @property + def device_address(self) -> int: + return 0x30 + + def init(self): + self.verify_chip_id(0x39, [0x10, ]) + self.writes(( + (REG_ODR, 0), + + # Set BW to 0b01 for 1-150 Hz operation + (REG_INTERNAL_1, 0b01), + )) + + def _read_data(self, cycle) -> list[float]: + # start measurement + self.write(REG_INTERNAL_0, cycle) + self.wait() + + # read out XYZ + scale = 1.0 / 16384.0 + b = self.read(0x00, 9) + return [ + (self.parse_20bit(b[6], b[1], b[0]) * scale) - 32.0, + (self.parse_20bit(b[7], b[3], b[2]) * scale) - 32.0, + (self.parse_20bit(b[8], b[5], b[4]) * scale) - 32.0, + ] + + def get_event(self, ts: int | None = None) -> log.SensorEventData: + ts = time.monotonic_ns() + + # SET - RESET cycle + xyz = self._read_data(SET) + reset_xyz = self._read_data(RESET) + vals = [*xyz, *reset_xyz] + + event = log.SensorEventData.new_message() + event.timestamp = ts + event.version = 1 + event.sensor = 3 # SENSOR_MAGNETOMETER_UNCALIBRATED + event.type = 14 # SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED + event.source = log.SensorEventData.SensorSource.mmc5603nj + + m = event.init('magneticUncalibrated') + m.v = vals + m.status = int(all(int(v) != -32 for v in vals)) + + return event + + def shutdown(self) -> None: + v = self.read(REG_INTERNAL_0, 1)[0] + self.writes(( + # disable auto-reset of measurements + (REG_INTERNAL_0, (v & (~(CMM_FREQ_EN | AUTO_SR_EN)))), + + # disable continuous mode + (REG_ODR, 0), + )) diff --git a/system/sensord/sensors/sensor.h b/system/sensord/sensors/sensor.h deleted file mode 100644 index ccf998d161..0000000000 --- a/system/sensord/sensors/sensor.h +++ /dev/null @@ -1,24 +0,0 @@ -#pragma once - -#include "cereal/messaging/messaging.h" - -class Sensor { -public: - int gpio_fd = -1; - bool enabled = false; - uint64_t start_ts = 0; - uint64_t init_delay = 500e6; // default dealy 500ms - - virtual ~Sensor() {} - virtual int init() = 0; - virtual bool get_event(MessageBuilder &msg, uint64_t ts = 0) = 0; - virtual bool has_interrupt_enabled() = 0; - virtual int shutdown() = 0; - - virtual bool is_data_valid(uint64_t current_ts) { - if (start_ts == 0) { - start_ts = current_ts; - } - return (current_ts - start_ts) > init_delay; - } -}; diff --git a/system/sensord/sensors_qcom2.cc b/system/sensord/sensors_qcom2.cc deleted file mode 100644 index 8ff9331b39..0000000000 --- a/system/sensord/sensors_qcom2.cc +++ /dev/null @@ -1,170 +0,0 @@ -#include - -#include -#include -#include -#include -#include -#include - -#include "cereal/services.h" -#include "cereal/messaging/messaging.h" -#include "common/i2c.h" -#include "common/ratekeeper.h" -#include "common/swaglog.h" -#include "common/timing.h" -#include "common/util.h" -#include "system/sensord/sensors/constants.h" -#include "system/sensord/sensors/lsm6ds3_accel.h" -#include "system/sensord/sensors/lsm6ds3_gyro.h" -#include "system/sensord/sensors/lsm6ds3_temp.h" -#include "system/sensord/sensors/mmc5603nj_magn.h" - -#define I2C_BUS_IMU 1 - -ExitHandler do_exit; - -void interrupt_loop(std::vector> sensors) { - PubMaster pm({"gyroscope", "accelerometer"}); - - int fd = -1; - for (auto &[sensor, msg_name] : sensors) { - if (sensor->has_interrupt_enabled()) { - fd = sensor->gpio_fd; - break; - } - } - - uint64_t offset = nanos_since_epoch() - nanos_since_boot(); - struct pollfd fd_list[1] = {0}; - fd_list[0].fd = fd; - fd_list[0].events = POLLIN | POLLPRI; - - while (!do_exit) { - int err = poll(fd_list, 1, 100); - if (err == -1) { - if (errno == EINTR) { - continue; - } - return; - } else if (err == 0) { - LOGE("poll timed out"); - continue; - } - - if ((fd_list[0].revents & (POLLIN | POLLPRI)) == 0) { - LOGE("no poll events set"); - continue; - } - - // Read all events - struct gpioevent_data evdata[16]; - err = HANDLE_EINTR(read(fd, evdata, sizeof(evdata))); - if (err < 0 || err % sizeof(*evdata) != 0) { - LOGE("error reading event data %d", err); - continue; - } - - uint64_t cur_offset = nanos_since_epoch() - nanos_since_boot(); - uint64_t diff = cur_offset > offset ? cur_offset - offset : offset - cur_offset; - if (diff > 10*1e6) { // 10ms - LOGW("time jumped: %lu %lu", cur_offset, offset); - offset = cur_offset; - - // we don't have a valid timestamp since the - // time jumped, so throw out this measurement. - continue; - } - - int num_events = err / sizeof(*evdata); - uint64_t ts = evdata[num_events - 1].timestamp - cur_offset; - - for (auto &[sensor, msg_name] : sensors) { - if (!sensor->has_interrupt_enabled()) { - continue; - } - - MessageBuilder msg; - if (!sensor->get_event(msg, ts)) { - continue; - } - - if (!sensor->is_data_valid(ts)) { - continue; - } - - pm.send(msg_name.c_str(), msg); - } - } -} - -void polling_loop(Sensor *sensor, std::string msg_name) { - PubMaster pm({msg_name.c_str()}); - RateKeeper rk(msg_name, services.at(msg_name).frequency); - while (!do_exit) { - MessageBuilder msg; - if (sensor->get_event(msg) && sensor->is_data_valid(nanos_since_boot())) { - pm.send(msg_name.c_str(), msg); - } - rk.keepTime(); - } -} - -int sensor_loop(I2CBus *i2c_bus_imu) { - // Sensor init - std::vector> sensors_init = { - {new LSM6DS3_Accel(i2c_bus_imu, GPIO_LSM_INT), "accelerometer"}, - {new LSM6DS3_Gyro(i2c_bus_imu, GPIO_LSM_INT, true), "gyroscope"}, - {new LSM6DS3_Temp(i2c_bus_imu), "temperatureSensor"}, - - {new MMC5603NJ_Magn(i2c_bus_imu), "magnetometer"}, - }; - - // Initialize sensors - std::vector threads; - for (auto &[sensor, msg_name] : sensors_init) { - int err = sensor->init(); - if (err < 0) { - continue; - } - - if (!sensor->has_interrupt_enabled()) { - threads.emplace_back(polling_loop, sensor, msg_name); - } - } - - // increase interrupt quality by pinning interrupt and process to core 1 - setpriority(PRIO_PROCESS, 0, -18); - util::set_core_affinity({1}); - - // TODO: get the IRQ number from gpiochip - std::string irq_path = "/proc/irq/336/smp_affinity_list"; - if (!util::file_exists(irq_path)) { - irq_path = "/proc/irq/335/smp_affinity_list"; - } - std::system(util::string_format("sudo su -c 'echo 1 > %s'", irq_path.c_str()).c_str()); - - // thread for reading events via interrupts - threads.emplace_back(&interrupt_loop, std::ref(sensors_init)); - - // wait for all threads to finish - for (auto &t : threads) { - t.join(); - } - - for (auto &[sensor, msg_name] : sensors_init) { - sensor->shutdown(); - delete sensor; - } - return 0; -} - -int main(int argc, char *argv[]) { - try { - auto i2c_bus_imu = std::make_unique(I2C_BUS_IMU); - return sensor_loop(i2c_bus_imu.get()); - } catch (std::exception &e) { - LOGE("I2CBus init failed"); - return -1; - } -} From b4c9964217d3be17ff1df150f39fe5d695a4261c Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Fri, 30 May 2025 15:44:03 -0700 Subject: [PATCH 54/60] gpu box test script --- scripts/usb.sh | 12 ++++++++++++ 1 file changed, 12 insertions(+) create mode 100755 scripts/usb.sh diff --git a/scripts/usb.sh b/scripts/usb.sh new file mode 100755 index 0000000000..5796cfa028 --- /dev/null +++ b/scripts/usb.sh @@ -0,0 +1,12 @@ +#!/usr/bin/env bash + +# testing the GPU box + +export XDG_CACHE_HOME=/data/tinycache +mkdir -p $XDG_CACHE_HOME + +cd /data/openpilot/tinygrad_repo/examples +while true; do + AMD=1 AMD_IFACE=usb python ./beautiful_cartpole.py + sleep 1 +done From ba9a478213828709c32a695efac389afc2affa6b Mon Sep 17 00:00:00 2001 From: James Vecellio-Grant <159560811+Discountchubbs@users.noreply.github.com> Date: Fri, 30 May 2025 20:13:55 -0700 Subject: [PATCH 55/60] Update Python packages (#953) * Change values to allow better 2mph to 0mph stopping * Update python package --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index 83d7cf4fd1..72e5c82795 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 83d7cf4fd128cc2dabb945d043e838ab404b16b6 +Subproject commit 72e5c82795caac8916c895fbc689ccbd3368b891 From 7cd6db6f1965b5b15b54e70f855a38b163f37fa1 Mon Sep 17 00:00:00 2001 From: programanichiro <99449198+programanichiro@users.noreply.github.com> Date: Sat, 31 May 2025 13:03:07 +0900 Subject: [PATCH 56/60] Multilang: update ja translation. (#35399) ja translation --- selfdrive/ui/translations/main_ja.ts | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/selfdrive/ui/translations/main_ja.ts b/selfdrive/ui/translations/main_ja.ts index 85d2494dfe..169df93c33 100644 --- a/selfdrive/ui/translations/main_ja.ts +++ b/selfdrive/ui/translations/main_ja.ts @@ -84,27 +84,27 @@ Prevent large data uploads when on a metered cellular connection - + モバイルデータ回線を使用しているときは大容量データをアップロードしません default - + 標準設定 metered - + 従量制 unmetered - + 定額制 Wi-Fi Network Metered - + 従量制のWi-Fiネットワーク Prevent large data uploads when on a metered Wi-Fi connection - + 通信制限のあるWi-Fi接続では大容量データをアップロードしません From 5e4a4ecec874be9227b8667adf61c2d2767dcdd9 Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Sat, 31 May 2025 14:42:35 +0800 Subject: [PATCH 57/60] system/ui/: add face detection box and driver state icon to DriverCameraView (#35402) add face detection box and driver state icons --- system/ui/widgets/driver_camera_view.py | 56 +++++++++++++++++++++++-- 1 file changed, 52 insertions(+), 4 deletions(-) diff --git a/system/ui/widgets/driver_camera_view.py b/system/ui/widgets/driver_camera_view.py index 174fac378d..56dcbf2b9e 100644 --- a/system/ui/widgets/driver_camera_view.py +++ b/system/ui/widgets/driver_camera_view.py @@ -1,18 +1,63 @@ import numpy as np import pyray as rl +from cereal import messaging from openpilot.system.ui.widgets.cameraview import CameraView from msgq.visionipc import VisionStreamType -from openpilot.system.ui.lib.application import gui_app +from openpilot.system.ui.lib.application import gui_app, FontWeight +from openpilot.system.ui.lib.label import gui_label +from openpilot.system.ui.onroad.driver_state import DriverStateRenderer class DriverCameraView(CameraView): def __init__(self, stream_type: VisionStreamType): super().__init__("camerad", stream_type) + self.driver_state_renderer = DriverStateRenderer() - def render(self, rect): + def render(self, rect, sm): super().render(rect) - # TODO: Add additional rendering logic + if not self.frame: + gui_label( + rect, + "camera starting", + font_size=100, + font_weight=FontWeight.BOLD, + alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER, + ) + return + + self._draw_face_detection(rect, sm) + self.driver_state_renderer.draw(rect, sm) + + def _draw_face_detection(self, rect: rl.Rectangle, sm) -> None: + driver_state = sm["driverStateV2"] + is_rhd = driver_state.wheelOnRightProb > 0.5 + driver_data = driver_state.rightDriverData if is_rhd else driver_state.leftDriverData + face_detect = driver_data.faceProb > 0.7 + if not face_detect: + return + + # Get face position and orientation + face_x, face_y = driver_data.facePosition + face_std = max(driver_data.faceOrientationStd[0], driver_data.faceOrientationStd[1]) + alpha = 0.7 + if face_std > 0.15: + alpha = max(0.7 - (face_std - 0.15) * 3.5, 0.0) + + # use approx instead of distort_points + # TODO: replace with distort_points + fbox_x = int(1080.0 - 1714.0 * face_x) + fbox_y = int(-135.0 + (504.0 + abs(face_x) * 112.0) + (1205.0 - abs(face_x) * 724.0) * face_y) + box_size = 220 + + line_color = rl.Color(255, 255, 255, int(alpha * 255)) + rl.draw_rectangle_rounded_lines_ex( + rl.Rectangle(fbox_x - box_size / 2, fbox_y - box_size / 2, box_size, box_size), + 35.0 / box_size / 2, + 10, + 10, + line_color, + ) def _calc_frame_matrix(self, rect: rl.Rectangle) -> np.ndarray: driver_view_ratio = 2.0 @@ -38,9 +83,12 @@ class DriverCameraView(CameraView): if __name__ == "__main__": gui_app.init_window("Driver Camera View") + sm = messaging.SubMaster(["selfdriveState", "driverStateV2", "driverMonitoringState"]) + driver_camera_view = DriverCameraView(VisionStreamType.VISION_STREAM_DRIVER) try: for _ in gui_app.render(): - driver_camera_view.render(rl.Rectangle(0, 0, gui_app.width, gui_app.height)) + sm.update() + driver_camera_view.render(rl.Rectangle(0, 0, gui_app.width, gui_app.height), sm) finally: driver_camera_view.close() From 62a7a19d27a0e787deb7e65d16bceffbb3e2e9a4 Mon Sep 17 00:00:00 2001 From: Maxime Desroches Date: Fri, 30 May 2025 23:43:28 -0700 Subject: [PATCH 58/60] Revert "system/ui/: add face detection box and driver state icon to DriverCameraView" (#35403) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Revert "system/ui/: add face detection box and driver state icon to DriverCam…" This reverts commit 5e4a4ecec874be9227b8667adf61c2d2767dcdd9. --- system/ui/widgets/driver_camera_view.py | 56 ++----------------------- 1 file changed, 4 insertions(+), 52 deletions(-) diff --git a/system/ui/widgets/driver_camera_view.py b/system/ui/widgets/driver_camera_view.py index 56dcbf2b9e..174fac378d 100644 --- a/system/ui/widgets/driver_camera_view.py +++ b/system/ui/widgets/driver_camera_view.py @@ -1,63 +1,18 @@ import numpy as np import pyray as rl -from cereal import messaging from openpilot.system.ui.widgets.cameraview import CameraView from msgq.visionipc import VisionStreamType -from openpilot.system.ui.lib.application import gui_app, FontWeight -from openpilot.system.ui.lib.label import gui_label -from openpilot.system.ui.onroad.driver_state import DriverStateRenderer +from openpilot.system.ui.lib.application import gui_app class DriverCameraView(CameraView): def __init__(self, stream_type: VisionStreamType): super().__init__("camerad", stream_type) - self.driver_state_renderer = DriverStateRenderer() - def render(self, rect, sm): + def render(self, rect): super().render(rect) - if not self.frame: - gui_label( - rect, - "camera starting", - font_size=100, - font_weight=FontWeight.BOLD, - alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER, - ) - return - - self._draw_face_detection(rect, sm) - self.driver_state_renderer.draw(rect, sm) - - def _draw_face_detection(self, rect: rl.Rectangle, sm) -> None: - driver_state = sm["driverStateV2"] - is_rhd = driver_state.wheelOnRightProb > 0.5 - driver_data = driver_state.rightDriverData if is_rhd else driver_state.leftDriverData - face_detect = driver_data.faceProb > 0.7 - if not face_detect: - return - - # Get face position and orientation - face_x, face_y = driver_data.facePosition - face_std = max(driver_data.faceOrientationStd[0], driver_data.faceOrientationStd[1]) - alpha = 0.7 - if face_std > 0.15: - alpha = max(0.7 - (face_std - 0.15) * 3.5, 0.0) - - # use approx instead of distort_points - # TODO: replace with distort_points - fbox_x = int(1080.0 - 1714.0 * face_x) - fbox_y = int(-135.0 + (504.0 + abs(face_x) * 112.0) + (1205.0 - abs(face_x) * 724.0) * face_y) - box_size = 220 - - line_color = rl.Color(255, 255, 255, int(alpha * 255)) - rl.draw_rectangle_rounded_lines_ex( - rl.Rectangle(fbox_x - box_size / 2, fbox_y - box_size / 2, box_size, box_size), - 35.0 / box_size / 2, - 10, - 10, - line_color, - ) + # TODO: Add additional rendering logic def _calc_frame_matrix(self, rect: rl.Rectangle) -> np.ndarray: driver_view_ratio = 2.0 @@ -83,12 +38,9 @@ class DriverCameraView(CameraView): if __name__ == "__main__": gui_app.init_window("Driver Camera View") - sm = messaging.SubMaster(["selfdriveState", "driverStateV2", "driverMonitoringState"]) - driver_camera_view = DriverCameraView(VisionStreamType.VISION_STREAM_DRIVER) try: for _ in gui_app.render(): - sm.update() - driver_camera_view.render(rl.Rectangle(0, 0, gui_app.width, gui_app.height), sm) + driver_camera_view.render(rl.Rectangle(0, 0, gui_app.width, gui_app.height)) finally: driver_camera_view.close() From 04115b6417387136c28293a8df4075c440222da2 Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Sat, 31 May 2025 14:44:13 +0800 Subject: [PATCH 59/60] system/ui: Increase font size to reduce edge aliasing (#35401) improve text's antialiasing --- system/ui/lib/application.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/system/ui/lib/application.py b/system/ui/lib/application.py index 37da413806..9bb56fec8d 100644 --- a/system/ui/lib/application.py +++ b/system/ui/lib/application.py @@ -202,7 +202,7 @@ class GuiApplication: for index, font_file in enumerate(font_files): with as_file(FONT_DIR.joinpath(font_file)) as fspath: - font = rl.load_font_ex(fspath.as_posix(), 120, codepoints, codepoint_count[0]) + font = rl.load_font_ex(fspath.as_posix(), 200, codepoints, codepoint_count[0]) rl.set_texture_filter(font.texture, rl.TextureFilter.TEXTURE_FILTER_BILINEAR) self._fonts[index] = font From c2bd95c2af2a4979c2779c4350a685bd08f87699 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sat, 31 May 2025 14:49:40 -0400 Subject: [PATCH 60/60] Tinygrad: revert bump (#969) --- tinygrad_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tinygrad_repo b/tinygrad_repo index 76eb130d8c..519dec6677 160000 --- a/tinygrad_repo +++ b/tinygrad_repo @@ -1 +1 @@ -Subproject commit 76eb130d8c6567679641ff53a7215d1f6f88eb9a +Subproject commit 519dec6677f98718ee4f2d07be1936eb91dde73b