From 643fd3c61cad5d4290dd4225f898e1f795a9e575 Mon Sep 17 00:00:00 2001 From: imterah Date: Sat, 21 Jun 2025 12:58:41 -0400 Subject: [PATCH 01/12] chore: Remove legacy Python codebase --- .gitignore | 291 +++------------------------------ .gitmodules | 9 - libunreal/__init__.py | 10 -- libunreal/edid.py | 86 ---------- libunreal/linux/__init__.py | 84 ---------- libunreal/mcu_driver.py | 145 ---------------- libunreal/supported_devices.py | 13 -- main.py | 188 --------------------- modules/evdi | 1 - modules/nreal-driver | 1 - modules/raylib-python-cffi | 1 - render.py | 235 -------------------------- requirements.txt | 7 - shell.nix | 11 +- 14 files changed, 24 insertions(+), 1058 deletions(-) delete mode 100644 .gitmodules delete mode 100644 libunreal/__init__.py delete mode 100644 libunreal/edid.py delete mode 100644 libunreal/linux/__init__.py delete mode 100644 libunreal/mcu_driver.py delete mode 100644 libunreal/supported_devices.py delete mode 100755 main.py delete mode 160000 modules/evdi delete mode 160000 modules/nreal-driver delete mode 160000 modules/raylib-python-cffi delete mode 100644 render.py delete mode 100644 requirements.txt diff --git a/.gitignore b/.gitignore index 8e93e62..dcbec21 100644 --- a/.gitignore +++ b/.gitignore @@ -1,281 +1,30 @@ -# ---> Python -# Byte-compiled / optimized / DLL files -__pycache__/ -*.py[cod] -*$py.class - -# C extensions +# ---> Go +# If you prefer the allow list template instead of the deny list, see community template: +# https://github.com/github/gitignore/blob/main/community/Golang/Go.AllowList.gitignore +# +# Binaries for programs and plugins +*.exe +*.exe~ +*.dll *.so +*.dylib -# Distribution / packaging -.Python -build/ -develop-eggs/ -dist/ -downloads/ -eggs/ -.eggs/ -lib/ -lib64/ -parts/ -sdist/ -var/ -wheels/ -share/python-wheels/ -*.egg-info/ -.installed.cfg -*.egg -MANIFEST +# Test binary, built with `go test -c` +*.test -# PyInstaller -# Usually these files are written by a python script from a template -# before PyInstaller builds the exe, so as to inject date/other infos into it. -*.manifest -*.spec +# Output of the go coverage tool, specifically when used with LiteIDE +*.out -# Installer logs -pip-log.txt -pip-delete-this-directory.txt +# Dependency directories (remove the comment below to include it) +# vendor/ -# Unit test / coverage reports -htmlcov/ -.tox/ -.nox/ -.coverage -.coverage.* -.cache -nosetests.xml -coverage.xml -*.cover -*.py,cover -.hypothesis/ -.pytest_cache/ -cover/ +# Go workspace file +go.work +go.work.sum -# Translations -*.mo -*.pot - -# Django stuff: -*.log -local_settings.py -db.sqlite3 -db.sqlite3-journal - -# Flask stuff: -instance/ -.webassets-cache - -# Scrapy stuff: -.scrapy - -# Sphinx documentation -docs/_build/ - -# PyBuilder -.pybuilder/ -target/ - -# Jupyter Notebook -.ipynb_checkpoints - -# IPython -profile_default/ -ipython_config.py - -# pyenv -# For a library or package, you might want to ignore these files since the code is -# intended to run in multiple environments; otherwise, check them in: -# .python-version - -# pipenv -# According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. -# However, in case of collaboration, if having platform-specific dependencies or dependencies -# having no cross-platform support, pipenv may install dependencies that don't work, or not -# install all needed dependencies. -#Pipfile.lock - -# UV -# Similar to Pipfile.lock, it is generally recommended to include uv.lock in version control. -# This is especially recommended for binary packages to ensure reproducibility, and is more -# commonly ignored for libraries. -#uv.lock - -# poetry -# Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. -# This is especially recommended for binary packages to ensure reproducibility, and is more -# commonly ignored for libraries. -# https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control -#poetry.lock -#poetry.toml - -# pdm -# Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. -#pdm.lock -# pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it -# in version control. -# https://pdm.fming.dev/latest/usage/project/#working-with-version-control -.pdm.toml -.pdm-python -.pdm-build/ - -# PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm -__pypackages__/ - -# Celery stuff -celerybeat-schedule -celerybeat.pid - -# SageMath parsed files -*.sage.py - -# Environments +# env file .env -.venv -env/ -venv/ -ENV/ -env.bak/ -venv.bak/ - -# Spyder project settings -.spyderproject -.spyproject - -# Rope project settings -.ropeproject - -# mkdocs documentation -/site - -# mypy -.mypy_cache/ -.dmypy.json -dmypy.json - -# Pyre type checker -.pyre/ - -# pytype static type analyzer -.pytype/ - -# Cython debug symbols -cython_debug/ - -# PyCharm -# JetBrains specific template is maintained in a separate JetBrains.gitignore that can -# be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore -# and can be added to the global gitignore or merged into this file. For a more nuclear -# option (not recommended) you can uncomment the following to ignore the entire idea folder. -#.idea/ - -# Abstra -# Abstra is an AI-powered process automation framework. -# Ignore directories containing user credentials, local state, and settings. -# Learn more at https://abstra.io/docs -.abstra/ - -# Visual Studio Code -# Visual Studio Code specific template is maintained in a separate VisualStudioCode.gitignore -# that can be found at https://github.com/github/gitignore/blob/main/Global/VisualStudioCode.gitignore -# and can be added to the global gitignore or merged into this file. However, if you prefer, -# you could uncomment the following to ignore the entire vscode folder -# .vscode/ - -# Ruff stuff: -.ruff_cache/ - -# PyPI configuration file -.pypirc - -# ---> C -# Prerequisites -*.d - -# Object files -*.o -*.ko -*.obj -*.elf - -# Linker output -*.ilk -*.map -*.exp - -# Precompiled Headers -*.gch -*.pch - -# Libraries -*.lib -*.a -*.la -*.lo - -# Shared objects (inc. Windows DLLs) -*.dll -*.so -*.so.* -*.dylib - -# Executables -*.exe -*.out -*.app -*.i*86 -*.x86_64 -*.hex - -# Debug files -*.dSYM/ -*.su -*.idb -*.pdb - -# Kernel Module Compile Results -*.mod* -*.cmd -.tmp_versions/ -modules.order -Module.symvers -Mkfile.old -dkms.conf - -# ---> C++ -# Prerequisites -*.d - -# Compiled Object files -*.slo -*.lo -*.o -*.obj - -# Precompiled Headers -*.gch -*.pch - -# Compiled Dynamic libraries -*.so -*.dylib -*.dll - -# Fortran module files -*.mod -*.smod - -# Compiled Static libraries -*.lai -*.la -*.a -*.lib - -# Executables -*.exe -*.out -*.app # ---> UnrealXR +# development dirs data -drivers diff --git a/.gitmodules b/.gitmodules deleted file mode 100644 index 72cd7c1..0000000 --- a/.gitmodules +++ /dev/null @@ -1,9 +0,0 @@ -[submodule "modules/raylib-python-cffi"] - path = modules/raylib-python-cffi - url = https://git.terah.dev/UnrealXR/raylib-python-cffi.git -[submodule "modules/evdi"] - path = modules/evdi - url = https://github.com/DisplayLink/evdi.git -[submodule "modules/nreal-driver"] - path = modules/nreal-driver - url = https://git.terah.dev/UnrealXR/nrealAirLinuxDriver.git diff --git a/libunreal/__init__.py b/libunreal/__init__.py deleted file mode 100644 index c1abd7d..0000000 --- a/libunreal/__init__.py +++ /dev/null @@ -1,10 +0,0 @@ -from libunreal.supported_devices import * -from libunreal.mcu_driver import * -from libunreal.edid import * - -from sys import platform - -if platform == "linux" or platform == "linux2": - from libunreal.linux import * -else: - raise OSError("Unsupported operating system") diff --git a/libunreal/edid.py b/libunreal/edid.py deleted file mode 100644 index f103585..0000000 --- a/libunreal/edid.py +++ /dev/null @@ -1,86 +0,0 @@ -from loguru import logger - -from dataclasses import dataclass -import uuid - -@dataclass -class UnrealXRDisplayMetadata: - edid: bytes - device_vendor: str - device_quirks: dict[str, str | int] - max_width: int - max_height: int - max_refresh_rate: int - linux_drm_card: str - linux_drm_connector: str - -calculated_msft_payload_size = 22+4 - -def calculate_checksum(block): - checksum = (-sum(block[:-1])) & 0xFF - return checksum - -def patch_edid_to_be_specialized(edid_data: bytes | bytearray) -> bytes | bytearray: - mutable_edid = bytearray(edid_data) - is_enhanced_mode = len(edid_data) > 128 - - found_extension_base = 0 - extension_base_existed = False - - if is_enhanced_mode: - for i in range(128, len(edid_data), 128): - if edid_data[i] == 0x02: - logger.warning("Detected existing ANSI CTA data section. Patching in place but untested! Please report any issues you discover") - - if edid_data[i+1] != 0x03: - logger.warning("Incompatible version detected for ANSI CTA data section in EDID") - - found_extension_base = i - extension_base_existed = True - - if found_extension_base == 0: - found_extension_base = len(edid_data) - mutable_edid.extend([0]*128) - else: - mutable_edid.extend([0]*128) - found_extension_base = 128 - - generated_uuid = uuid.uuid4() - - mutable_edid[found_extension_base] = 0x02 - mutable_edid[found_extension_base+1] = 0x03 - - if extension_base_existed and mutable_edid[found_extension_base+2] != calculated_msft_payload_size and mutable_edid[found_extension_base+2] != 0: - # We try our best to move our data into place - current_base = mutable_edid[found_extension_base+2] - mutable_edid[found_extension_base+2] = calculated_msft_payload_size+1 - - mutable_edid[found_extension_base+4:found_extension_base+current_base-1] = [0]*(current_base-1) - mutable_edid[found_extension_base+calculated_msft_payload_size:found_extension_base+127] = mutable_edid[found_extension_base+current_base:found_extension_base+127] - else: - mutable_edid[found_extension_base+2] = calculated_msft_payload_size - - if not extension_base_existed: - mutable_edid[126] += 1 - mutable_edid[127] = calculate_checksum(mutable_edid[:128]) - - mutable_edid[found_extension_base+3] = 0 # We don't know any of these properties - - # Implemented using https://learn.microsoft.com/en-us/windows-hardware/drivers/display/specialized-monitors-edid-extension - # VST & Length - mutable_edid[found_extension_base+4] = 0x3 << 5 | 0x15 # 0x3: vendor specific tag; 0x15: length - # Assigned IEEE OUI - mutable_edid[found_extension_base+5] = 0x5C - mutable_edid[found_extension_base+6] = 0x12 - mutable_edid[found_extension_base+7] = 0xCA - # Actual data - mutable_edid[found_extension_base+8] = 0x2 # Using version 0x2 for better compatibility - mutable_edid[found_extension_base+9] = 0x7 # Using VR tag for better compatibility even though it probably doesn't matter - mutable_edid[found_extension_base+10:found_extension_base+10+16] = generated_uuid.bytes - - mutable_edid[found_extension_base+127] = calculate_checksum(mutable_edid[found_extension_base:found_extension_base+127]) - - if isinstance(edid_data, bytes): - return bytes(mutable_edid) - else: - return mutable_edid diff --git a/libunreal/linux/__init__.py b/libunreal/linux/__init__.py deleted file mode 100644 index cecdae4..0000000 --- a/libunreal/linux/__init__.py +++ /dev/null @@ -1,84 +0,0 @@ -import subprocess -import os - -from libunreal.supported_devices import supported_devices -from libunreal.edid import UnrealXRDisplayMetadata -import pyedid - -def upload_new_device_edid(display_spec: UnrealXRDisplayMetadata, edid: bytes | bytearray): - pass - -def fetch_xr_glass_edid(allow_unsupported_devices) -> UnrealXRDisplayMetadata: - # Scan for all VGA devices and their IDs - pci_device_comand = subprocess.run(["lspci"], capture_output=True) - - if pci_device_comand.returncode != 0: - raise OSError("Failed to scan PCI devices") - - pci_devices: list[str] = pci_device_comand.stdout.decode("utf-8").split("\n") - pci_devices = pci_devices[:-1] - - vga_devices: list[str] = [] - - for pci_device in pci_devices: - if "VGA compatible controller:" in pci_device: - vga_devices.append(pci_device[:pci_device.index(" ")]) - - # Attempt to find any XR glasses - for vga_device in vga_devices: - card_devices = list(os.listdir(f"/sys/devices/pci0000:00/0000:{vga_device}/drm/")) - - for card_device in card_devices: - if "card" not in card_device: - continue - - monitors = list(os.listdir(f"/sys/devices/pci0000:00/0000:{vga_device}/drm/{card_device}/")) - - for monitor in monitors: - if card_device not in monitor: - continue - - with open(f"/sys/devices/pci0000:00/0000:{vga_device}/drm/{card_device}/{monitor}/edid", "rb") as edid: - raw_edid_file = edid.read() - - if len(raw_edid_file) == 0: - continue - - edid = pyedid.parse_edid(raw_edid_file) - - for manufacturer, manufacturer_supported_devices in supported_devices.items(): - if edid.manufacturer_pnp_id == manufacturer and (edid.name in manufacturer_supported_devices or allow_unsupported_devices): - max_width = 0 - max_height = 0 - max_refresh = 0 - - for resolution in edid.resolutions: - if resolution[0] > max_width and resolution[1] > max_height: - max_width = resolution[0] - max_height = resolution[1] - - max_refresh = max(max_refresh, int(resolution[2])) - - if max_width == 0 or max_height == 0: - if "max_width" not in manufacturer_supported_devices[edid.name] or "max_height" not in manufacturer_supported_devices[edid.name]: - raise ValueError("Couldn't determine maximum width and height, and the maximum width and height isn't defined in the device quirks section") - - max_width = int(manufacturer_supported_devices[edid.name]["max_width"]) - max_height = int(manufacturer_supported_devices[edid.name]["max_height"]) - - if max_refresh == 0: - if "max_refresh" not in manufacturer_supported_devices[edid.name]: - raise ValueError("Couldn't determine maximum refresh rate, and the maximum refresh rate isn't defined in the device quirks section") - - max_refresh = int(manufacturer_supported_devices[edid.name]["max_refresh"]) - - return UnrealXRDisplayMetadata(raw_edid_file, edid.manufacturer_pnp_id, manufacturer_supported_devices[edid.name], max_width, max_height, max_refresh, card_device, monitor.replace(f"{card_device}-", "")) - - raise ValueError("Could not find supported device. Check if the device is plugged in. If it is plugged in and working correctly, check the README or open an issue.") - -def upload_edid_firmware(display: UnrealXRDisplayMetadata, fw: bytes | bytearray): - if display.linux_drm_connector == "" or display.linux_drm_card == "": - raise ValueError("Linux DRM connector and/or Linux DRM card not specified!") - - with open(f"/sys/kernel/debug/dri/{display.linux_drm_card.replace("card", "")}/{display.linux_drm_connector}/edid_override", "wb") as kernel_edid: - kernel_edid.write(fw) diff --git a/libunreal/mcu_driver.py b/libunreal/mcu_driver.py deleted file mode 100644 index ec426c5..0000000 --- a/libunreal/mcu_driver.py +++ /dev/null @@ -1,145 +0,0 @@ -from tempfile import TemporaryDirectory -from dataclasses import dataclass -from os import path, environ -from typing import Callable -from loguru import logger -from shutil import which -from time import sleep -from enum import Enum -import subprocess -import threading -import socket -import struct -import signal -import atexit -import os - -class MCUCommandTypes(Enum): - ROLL = 0 - PITCH = 1 - YAW = 2 - GENERIC_MESSAGE = 3 - BRIGHTNESS_UP = 4 - BRIGHTNESS_DOWN = 5 - -@dataclass -class MCUCallbackWrapper: - OnRollUpdate: Callable[[float], None] - OnPitchUpdate: Callable[[float], None] - OnYawUpdate: Callable[[float], None] - OnTextMessageRecieved: Callable[[str], None] - OnBrightnessUp: Callable[[int], None] - OnBrightnessDown: Callable[[int], None] - -vendor_to_driver_table: dict[str, str] = { - "MRG": "xreal_ar_driver", -} - -def find_executable_path_from_driver_name(driver_name) -> str: - # First try the normal driver path - try: - driver_path = path.join("drivers", driver_name) - - file = open(driver_path) - file.close() - - return driver_path - except OSError: - # Then search the system path - driver_path = which(driver_name) - - if driver_path == "": - raise OSError("Could not find driver executable in driver directory or in PATH") - - return driver_path - -def start_mcu_event_listener(driver_vendor: str, events: MCUCallbackWrapper): - driver_executable = find_executable_path_from_driver_name(vendor_to_driver_table[driver_vendor]) - - created_temp_dir = TemporaryDirectory() - sock_path = path.join(created_temp_dir.name, "mcu_socket") - - def on_socket_event(sock: socket.socket): - while True: - message_type = sock.recv(1) - - if message_type[0] == MCUCommandTypes.ROLL.value: - roll_data = sock.recv(4) - roll_value = struct.unpack("!f", roll_data)[0] - - if not isinstance(roll_value, float): - logger.warning("Expected roll value to be a float but got other type instead") - continue - - events.OnRollUpdate(roll_value) - elif message_type[0] == MCUCommandTypes.PITCH.value: - pitch_data = sock.recv(4) - pitch_value = struct.unpack("!f", pitch_data)[0] - - if not isinstance(pitch_value, float): - logger.warning("Expected pitch value to be a float but got other type instead") - continue - - events.OnPitchUpdate(pitch_value) - elif message_type[0] == MCUCommandTypes.YAW.value: - yaw_data = sock.recv(4) - yaw_value = struct.unpack("!f", yaw_data)[0] - - if not isinstance(yaw_value, float): - logger.warning("Expected yaw value to be a float but got other type instead") - continue - - events.OnYawUpdate(yaw_value) - elif message_type[0] == MCUCommandTypes.GENERIC_MESSAGE.value: - length_bytes = sock.recv(4) - - msg_len = struct.unpack("!I", length_bytes)[0] - msg_bytes = sock.recv(msg_len) - - msg = msg_bytes.decode("utf-8", errors="replace") - events.OnTextMessageRecieved(msg) - elif message_type[0] == MCUCommandTypes.BRIGHTNESS_UP.value: - brightness_bytes = sock.recv(1) - events.OnBrightnessUp(int.from_bytes(brightness_bytes, byteorder='big')) - elif message_type[0] == MCUCommandTypes.BRIGHTNESS_DOWN.value: - brightness_bytes = sock.recv(1) - events.OnBrightnessDown(int.from_bytes(brightness_bytes, byteorder='big')) - else: - logger.warning(f"Unknown message type recieved: {str(message_type[0])}") - - def start_socket_handout(): - server = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) - server.bind(sock_path) - server.listen(1) - - sock: socket.socket | None = None - - while True: - sock, _ = server.accept() - - threaded_connection_processing = threading.Thread(target=on_socket_event, args=(sock,), daemon=True) - threaded_connection_processing.start() - - created_temp_dir.cleanup() - - def spawn_child_process(): - custom_env = environ.copy() - custom_env["UNREALXR_NREAL_DRIVER_SOCK"] = sock_path - - process = subprocess.Popen([driver_executable], env=custom_env) - - def kill_child(): - if process.pid is None: - pass - else: - os.kill(process.pid, signal.SIGTERM) - - atexit.register(kill_child) - - threaded_socket_handout = threading.Thread(target=start_socket_handout, daemon=True) - threaded_socket_handout.start() - - sleep(0.01) # Give the socket server time to initialize - - threaded_child_process = threading.Thread(target=spawn_child_process, daemon=True) - threaded_child_process.start() diff --git a/libunreal/supported_devices.py b/libunreal/supported_devices.py deleted file mode 100644 index 7eafd62..0000000 --- a/libunreal/supported_devices.py +++ /dev/null @@ -1,13 +0,0 @@ -# Sourced from "https://uefi.org/uefi-pnp-export" -supported_devices: dict[str, dict[str, dict[str, str | int]]] = { - "MRG": { - # Quirks section - "Air": { - "max_width": 1920, - "max_height": 1080, - "max_refresh": 120, - "sensor_init_delay": 10, - "z_vector_disabled": True, - } - } -} diff --git a/main.py b/main.py deleted file mode 100755 index ca51df5..0000000 --- a/main.py +++ /dev/null @@ -1,188 +0,0 @@ -#!/usr/bin/env python3 -from sys import platform -import logging -import atexit -import json -import os - -# Silence pyray init messages -raylib_python_logger = logging.getLogger("raylib") -raylib_python_logger.setLevel(logging.ERROR) - -from platformdirs import user_data_dir, user_config_dir -from loguru import logger -import PyEvdi -import pyedid -import pyray -import time - -from render import render_loop -import libunreal - -default_configuration: dict[str, str | int] = { - "display_angle": 45, - "display_spacing": 1, - "display_count": 3, - "allow_unsupported_devices": False, - "allow_unsupported_vendors": False, - "override_default_edid": False, - "edid_override_path": "/file/here", - "override_width": 0, - "override_height": 0, - "override_refresh_rate": 0, -} - -def initialize_configuration(): - pass - -used_cards: list[int] = [] - -def find_suitable_evdi_card() -> int: - for i in range(20): - if PyEvdi.check_device(i) == PyEvdi.AVAILABLE and i not in used_cards: - used_cards.append(i) - return i - - PyEvdi.add_device() - - for i in range(20): - if PyEvdi.check_device(i) == PyEvdi.AVAILABLE and i not in used_cards: - used_cards.append(i) - return i - - raise ValueError("Failed to allocate virtual display device") - -@logger.catch -def main(): - configuration = {} - - logger.info("Loading configuration") - config_dir = os.environ["UNREALXR_CONFIG_PATH"] if "UNREALXR_CONFIG_PATH" in os.environ else "" - data_dir = os.environ["UNREALXR_DATA_PATH"] if "UNREALXR_DATA_PATH" in os.environ else "" - - # Use OS defaults if we weren't overriden in env - if config_dir == "": - config_dir = user_config_dir("UnrealXR", "Tera") - - if data_dir == "": - data_dir = user_data_dir("UnrealXR", "Tera") - - try: - os.stat(data_dir) - except OSError: - os.makedirs(data_dir) - - # Read config and create it if it doesn't exist - config_path = os.path.join(config_dir, "config.json") - - try: - os.stat(config_path) - - with open(config_path, "r") as config_file: - configuration = json.load(config_file) - except OSError: - try: - os.makedirs(config_dir) - except OSError: - pass - - with open(config_path, "w") as config_file: - json.dump(default_configuration, config_file, indent=4) - - configuration = default_configuration - - # Set unbound values (ie. if user is using an older version) - for key, default_value in default_configuration.items(): - if key not in configuration: - # Add quotes if we're a string - value = default_value - - if isinstance(value, str): - value = f'"{value}"' - - logger.warning(f"Setting unbound key '{key}' with default value {value}. You might want to define this! If not, this warning can be safely ignored.") - configuration[key] = default_value - - # Initialize logging to files - logger.add(os.path.join(data_dir, "unrealxr.log"), format="{time:YYYY-MM-DD at HH:mm:ss} | {level} | {message}") - logger.info("Loaded configuration") - - if os.geteuid() != 0: - raise OSError("You are not running as root! Running as root is necessary to talk to the EVDI service") - - # Get the display EDID - logger.info("Attempting to read display EDID file") - edid: libunreal.UnrealXRDisplayMetadata | None = None - - if configuration["override_default_edid"] or configuration["allow_unsupported_vendors"]: - # We need to parse it to get the maximum width, height, and refresh rate for EVDI's calculations - with open(configuration["edid_override_path"], "rb") as edid_file: - edid_file = edid_file.read() - parsed_edid_file = pyedid.parse_edid(edid_file) - - max_width = int(configuration["override_width"]) - max_height = int(configuration["override_height"]) - max_refresh = int(configuration["override_refresh_rate"]) - - if configuration["override_width"] == 0 or configuration["override_height"] == 0 or configuration["override_refresh_rate"] == 0: - for resolution in parsed_edid_file.resolutions: - if configuration["override_width"] == 0 or configuration["override_height"] == 0: - if resolution[0] > max_width and resolution[1] > max_height: - max_width = resolution[0] - max_height = resolution[1] - - if configuration["override_refresh_rate"] == 0: - max_refresh = max(max_refresh, int(resolution[2])) - - if max_width == 0 or max_height == 0: - raise ValueError("Could not determine maximum width and/or height from EDID file, and the width and/or height overrides aren't set!") - - if max_refresh == 0: - raise ValueError("Could not determine maximum refresh rate from EDID file, and the refresh rate overrides aren't set!") - - edid = libunreal.UnrealXRDisplayMetadata(edid_file, parsed_edid_file.name if parsed_edid_file.name else "", {}, max_width, max_height, max_refresh, "", "") - else: - edid = libunreal.fetch_xr_glass_edid(configuration["allow_unsupported_devices"]) - - assert(edid is not None) - logger.info("Got EDID file") - - if platform == "linux" or platform == "linux2": - # TODO: implement EDID patching for overridden displays - logger.info("Patching EDID firmware") - patched_edid = libunreal.patch_edid_to_be_specialized(edid.edid) - libunreal.upload_edid_firmware(edid, patched_edid) - - def unload_custom_fw(): - with open(f"/sys/kernel/debug/dri/{edid.linux_drm_card.replace("card", "")}/{edid.linux_drm_connector}/edid_override", "w") as kernel_edid: - kernel_edid.write("reset") - - logger.info("Please unplug and plug in your XR device to restore it back to normal settings.") - - atexit.register(unload_custom_fw) - input("Press the Enter key to continue loading after you unplug and plug in your XR device.") - - # Raylib gets confused if there's multiple dri devices so we initialize the window before anything - logger.info("Initializing XR headset") - pyray.set_target_fps(edid.max_refresh_rate*2) # we need more headroom... - pyray.init_window(edid.max_width, edid.max_height, "UnrealXR") - - logger.info("Initializing virtual displays") - cards = [] - - for i in range(int(configuration["display_count"])): - suitable_card_id = find_suitable_evdi_card() - card = PyEvdi.Card(suitable_card_id) - card.connect(edid.edid, len(edid.edid), edid.max_width*edid.max_height, edid.max_width*edid.max_height*edid.max_refresh_rate) - cards.append(card) - - logger.debug(f"Initialized card #{str(i+1)}") - - atexit.register(lambda: card.close()) - - logger.info("Initialized displays. Entering rendering loop") - render_loop(edid, configuration, cards) - -if __name__ == "__main__": - print("Welcome to UnrealXR!\n") - main() diff --git a/modules/evdi b/modules/evdi deleted file mode 160000 index 3673a4b..0000000 --- a/modules/evdi +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 3673a4b34d386921fc323ddbd2ef0e000022e2d4 diff --git a/modules/nreal-driver b/modules/nreal-driver deleted file mode 160000 index 36ad789..0000000 --- a/modules/nreal-driver +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 36ad789c9b5893653c523d4af6e24120ec19ab33 diff --git a/modules/raylib-python-cffi b/modules/raylib-python-cffi deleted file mode 160000 index 7c982c0..0000000 --- a/modules/raylib-python-cffi +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 7c982c0d4e2773739f00eb91a2d2cbe87eb7c76d diff --git a/render.py b/render.py deleted file mode 100644 index 21cdffe..0000000 --- a/render.py +++ /dev/null @@ -1,235 +0,0 @@ -from dataclasses import dataclass -from io import BufferedWriter -from sys import int_info -from typing import Union -import ctypes -import time -import math - -from loguru import logger -from raylib import rl -import PyEvdi -import pyray - -from libunreal import UnrealXRDisplayMetadata, MCUCallbackWrapper, start_mcu_event_listener - -vertical_size = 0.0 -horizontal_sizing_constant = 1 - -previous_pitch = 0.0 -previous_yaw = 0.0 -previous_roll = 0.0 - -current_pitch = 0.0 -current_yaw = 0.0 -current_roll = 0.0 - -has_gotten_pitch_callback_before = False -has_gotten_yaw_callback_before = False -has_gotten_roll_callback_before = False - -@dataclass -class RectMetadata: - card: PyEvdi.Card - buffer_ptr: pyray.ffi.CData | None - texture: Union[pyray.Texture, None] - model: Union[pyray.Model, None] - angle: int - relative_position: int - -def pitch_callback(new_pitch: float): - global current_pitch - global previous_pitch - global has_gotten_pitch_callback_before - - if not has_gotten_pitch_callback_before: - has_gotten_pitch_callback_before = True - previous_pitch = new_pitch - current_pitch = new_pitch - else: - previous_pitch = current_pitch - current_pitch = new_pitch - -def yaw_callback(new_yaw: float): - global current_yaw - global previous_yaw - global has_gotten_yaw_callback_before - - if not has_gotten_yaw_callback_before: - has_gotten_yaw_callback_before = True - previous_yaw = new_yaw - current_yaw = new_yaw - else: - previous_yaw = current_yaw - current_yaw = new_yaw - -def roll_callback(new_roll: float): - global current_roll - global previous_roll - global has_gotten_roll_callback_before - - if not has_gotten_roll_callback_before: - has_gotten_roll_callback_before = True - previous_roll = new_roll - roll = new_roll - else: - previous_roll = current_roll - current_roll = new_roll - -def text_message(message: str): - logger.debug(f"Got message from AR's MCU: {message}") - -def stub_brightness_function(brightness: int): - pass - -def find_max_vertical_size(fovy_deg: float, distance: float) -> float: - fovy_rad = math.radians(fovy_deg) - return 2 * distance * math.tan(fovy_rad / 2) - -def find_optimal_horizonal_res(vertical_display_res: int, horizontal_display_res: int) -> float: - aspect_ratio = horizontal_display_res/vertical_display_res - horizontal_size = vertical_size * aspect_ratio - horizontal_size = horizontal_size * horizontal_sizing_constant - - return horizontal_size - -def render_loop(display_metadata: UnrealXRDisplayMetadata, config: dict[str, str | int], cards: list[PyEvdi.Card]): - global vertical_size - global core_mesh - logger.info("Starting sensor event listener") - - mcu_callbacks = MCUCallbackWrapper(roll_callback, pitch_callback, yaw_callback, text_message, stub_brightness_function, stub_brightness_function) - start_mcu_event_listener(display_metadata.device_vendor, mcu_callbacks) - - logger.info("Beginning sensor initialization. Awaiting first sensor update") - - while (not has_gotten_pitch_callback_before) or (not has_gotten_yaw_callback_before) or (not has_gotten_roll_callback_before): - time.sleep(0.01) - - logger.info("Initialized sensors") - - camera = pyray.Camera3D() - camera.fovy = 45.0 - - vertical_size = find_max_vertical_size(camera.fovy, 5.0) - - camera.position = pyray.Vector3(0.0, vertical_size/2, 5.0) - camera.target = pyray.Vector3(0.0, vertical_size/2, 0.0) - camera.up = pyray.Vector3(0.0, 1.0, 0.0) - camera.projection = pyray.CameraProjection.CAMERA_PERSPECTIVE - - core_mesh = pyray.gen_mesh_plane(find_optimal_horizonal_res(display_metadata.max_height, display_metadata.max_width), vertical_size, 1, 1) - - movement_vector = pyray.Vector3() - look_vector = pyray.Vector3() - - has_z_vector_disabled_quirk = False - has_sensor_init_delay_quirk = False - sensor_init_start_time = time.time() - - if "z_vector_disabled" in display_metadata.device_quirks and bool(display_metadata.device_quirks["z_vector_disabled"]): - logger.warning("QUIRK: The Z vector has been disabled for your specific device") - has_z_vector_disabled_quirk = True - - if "sensor_init_delay" in display_metadata.device_quirks: - logger.warning(f"QUIRK: Waiting {str(display_metadata.device_quirks["sensor_init_delay"])} second(s) before reading sensors") - logger.warning("|| MOVEMENT WILL NOT BE OPERATIONAL DURING THIS TIME. ||") - sensor_init_start_time = time.time() - has_sensor_init_delay_quirk = True - - rects: list[RectMetadata] = [] - - if int(config["display_count"]) >= 2: - display_angle = int(config["display_angle"]) - display_spacing = int(config["display_spacing"]) - total_displays = int(config["display_count"]) - - highest_possible_angle_on_both_sides = (total_displays-1)*display_angle - highest_possible_pixel_spacing_on_both_sides = (total_displays-1)*display_spacing - - for i in range(total_displays): - current_angle = (-highest_possible_angle_on_both_sides)+(display_angle*i) - current_display_spacing = (-highest_possible_pixel_spacing_on_both_sides)+(display_spacing*i) - - rect_metadata = RectMetadata(cards[i], None, None, None, current_angle, current_display_spacing) - - has_acquired_fb = False - - def fb_acquire_handler(evdi_buffer: PyEvdi.Buffer): - nonlocal has_acquired_fb - - if has_acquired_fb: - return - - has_acquired_fb = True - logger.info(f"Acquired buffer for card #{i+1} with ID {evdi_buffer.id}") - - address = ctypes.pythonapi.PyCapsule_GetPointer - address.restype = ctypes.c_void_p - address.argtypes = [ctypes.py_object, ctypes.c_char_p] - - buffer_void_ptr = address(evdi_buffer.bytes, None) - rect_metadata.buffer_ptr = pyray.ffi.cast("void *", buffer_void_ptr) - - pyray_image = pyray.Image() - - pyray_image.data = rect_metadata.buffer_ptr - pyray_image.width = display_metadata.max_width - pyray_image.height = display_metadata.max_height - pyray_image.mipmaps = 1 - pyray_image.format = pyray.PixelFormat.PIXELFORMAT_UNCOMPRESSED_R8G8B8A8 - - rect_metadata.texture = pyray.load_texture_from_image(pyray_image) - rect_metadata.model = pyray.load_model_from_mesh(core_mesh) - - pyray.set_material_texture(rect_metadata.model.materials[0], pyray.MaterialMapIndex.MATERIAL_MAP_ALBEDO, rect_metadata.texture) - - cards[i].acquire_framebuffer_handler = fb_acquire_handler - cards[i].handle_events(1000) - - rects.append(rect_metadata) - - while not pyray.window_should_close(): - if has_sensor_init_delay_quirk: - if time.time() - sensor_init_start_time >= int(display_metadata.device_quirks["sensor_init_delay"]): - # Unset the quirk state - logger.info("Movement is now enabled.") - has_sensor_init_delay_quirk = False - else: - look_vector.x = (current_yaw-previous_yaw)*6.5 - look_vector.y = -(current_pitch-previous_pitch)*6.5 - - if not has_z_vector_disabled_quirk: - look_vector.z = (current_roll-previous_roll)*6.5 - - pyray.update_camera_pro(camera, movement_vector, look_vector, 0.0) - - pyray.begin_drawing() - pyray.clear_background(pyray.BLACK) - pyray.begin_mode_3d(camera) - - for rect_count in range(len(rects)): - rect = rects[rect_count] - - if rect.buffer_ptr is None or rect.texture is None or rect.model is None: - continue - - cards[rect_count].handle_events(1) - pyray.update_texture(rect.texture, rect.buffer_ptr) - - pyray.draw_model_ex( - rect.model, - pyray.Vector3(0, vertical_size/2, 0), - pyray.Vector3(1, 0, 0), # rotate around X to make it vertical - 90, - pyray.Vector3(1, 1, 1), - pyray.WHITE - ) - - break - - pyray.end_mode_3d() - pyray.end_drawing() - - logger.info("Goodbye!") - pyray.close_window() diff --git a/requirements.txt b/requirements.txt deleted file mode 100644 index c2a938b..0000000 --- a/requirements.txt +++ /dev/null @@ -1,7 +0,0 @@ -setuptools==80.9.0 -pybind11==2.13.6 -pyedid==1.0.3 -loguru==0.7.3 -platformdirs==4.3.8 -cffi=1.17.1 -wheel=0.45.1 diff --git a/shell.nix b/shell.nix index 1cd42af..b1e1b78 100644 --- a/shell.nix +++ b/shell.nix @@ -3,9 +3,12 @@ }: pkgs.mkShell { buildInputs = with pkgs; [ # Runtime dependencies - python3 pciutils + # UnrealXR build dependencies + go + gopls + # evdi build dependencies libdrm linuxHeaders @@ -37,11 +40,5 @@ mkdir -p "$PWD/data/config" "$PWD/data/data" export UNREALXR_CONFIG_PATH="$PWD/data/config" export UNREALXR_DATA_PATH="$PWD/data/data" - - if [ ! -d ".venv" ]; then - python3 -m venv .venv - fi - - source .venv/bin/activate ''; } From a88b75a4bcfbce7f622e673ac46ac8f4ffb8b728 Mon Sep 17 00:00:00 2001 From: imterah Date: Sat, 21 Jun 2025 13:02:45 -0400 Subject: [PATCH 02/12] chore: Cleanup README --- README.md | 39 ++++----------------------------------- 1 file changed, 4 insertions(+), 35 deletions(-) diff --git a/README.md b/README.md index 8c72056..f64c367 100644 --- a/README.md +++ b/README.md @@ -22,41 +22,10 @@ UnrealXR is a spatial multi-display renderer for the Xreal line of devices, enab Before anything, this depends on the `evdi` Linux kernel module. This is packaged in Debian-based distributions as `evdi-dkms`. If you already have DisplayLink drivers installed for their devices, you likely do not need to do this step. After installing this, please reboot your computer. -First, install the runtime dependencies. For Debian-based distros, the dependency list should be: `python3 python3-pip` +You'll need to install build dependencies after this. For Debian-based distros, the dependency list should be: `git golang build-essential libdrm libdrm-dev linux-headers-$(uname -r) cmake clang-tools pkg-config libwayland-client++1 libgl1-mesa-dev libglu1-mesa-dev libwayland-dev libxkbcommon-dev libhidapi-dev libjson-c-dev libudev-dev libusb-1.0-0 libusb-1.0-0-dev libopencv-dev` -You'll need to install build dependencies after this. For Debian-based distros, the dependency list should be: `git build-essential libdrm libdrm-dev linux-headers-$(uname -r) cmake clang-tools pkg-config libwayland-client++1 libgl1-mesa-dev libglu1-mesa-dev libwayland-dev libxkbcommon-dev libhidapi-dev libjson-c-dev libudev-dev libusb-1.0-0 libusb-1.0-0-dev libopencv-dev` +If you're using Nix/NixOS, all you need to do is use `nix-shell` to enter the development environment. -If you're using Nix/NixOS, use the `nix-shell` to enter the development environment. +## Building -After that, create a virtual environment for Python (done automatically in Nix): `python3 -m venv .venv`. Then, activate it: `source .venv/bin/activate` - -Finally, install the Python dependencies: `pip install -r requirements.txt` - -From there, you need to follow all the below steps if applicable to your current platform. -. -### Building patched `raylib` and `PyRay` (all platforms) - -1. First, you need to build the native version of raylib. To do that, go inside the `modules/raylib-python-cffi/raylib-c` directory. -2. Then, make the build directories and go into them: `mkdir -p build/out; cd build` -3. Configure raylib: `cmake -DCUSTOMIZE_BUILD=ON -DSUPPORT_FILEFORMAT_JPG=ON -DSUPPORT_FILEFORMAT_FLAC=ON -DWITH_PIC=ON -DCMAKE_BUILD_TYPE=Release -DPLATFORM=DRM -DENABLE_WAYLAND_DRM_LEASING=ON -DSUPPORT_CLIPBOARD_IMAGE=ON -DBUILD_EXAMPLES=OFF -DSUPPORT_SSH_KEYBOARD_RPI=OFF -DDISABLE_EVDEV_INPUT=ON -DCMAKE_INSTALL_PREFIX:PATH=$PWD/out ..` -4. Finally, build and install raylib: `make install -j$(nproc)` -5. After that, you need to build the Python bindings. To do that, go to the `modules/raylib-python-cffi` directory. Assuming you did everything correctly, you should be able to go 2 directories back (`../..`) to get there. -6. If you're on normal Linux and are not using Nix, do this command to build the package: `PKG_CONFIG_PATH="$PKG_CONFIG_PATH:$PWD/raylib-c/build/out/lib64/pkgconfig/" ENABLE_WAYLAND_DRM_LEASING=YES RAYLIB_PLATFORM=DRM python3 setup.py bdist_wheel` -7. If you are using Nix/NixOS, do this command to build the package: `PKG_CONFIG_PATH_FOR_TARGET="$PKG_CONFIG_PATH_FOR_TARGET:$PWD/raylib-c/build/out/lib64/pkgconfig/" ENABLE_WAYLAND_DRM_LEASING=YES RAYLIB_PLATFORM=DRM python3 setup.py bdist_wheel` -8. Finally, install the package: `pip install dist/*.whl` - -### Building `PyEvdi` (Linux) - -1. First, build the original libevdi. To start that, go inside the `modules/evdi/library` directory. -2. Then, build `libevdi`: `make -j$(nproc)` -3. After that, you need to build the Python bindings. To do that, go to the `modules/evdi/pyevdi` directory. Assuming you did everything correctly, you should be able to go a directory back (`../pyevdi`) to get there. -4. Then, build `PyEvdi`: `make -j$(nproc); make install` - -### Building `nreal-driver` (all platforms) - -1. First, create the `drivers` directory in the project root. -2. Then, go inside the `modules/nreal-driver` directory. -3. After that, make the build directories and go into them: `mkdir -p build/out; cd build` -4. Configure nreal-driver: `cmake -DCMAKE_BUILD_TYPE=Release ..` -5. Build the driver: `make -j$(nproc)` -6. Move the driver to the correct directory: `mv xrealAirLinuxDriver ../../../drivers/xreal_ar_driver` +TODO. From d0a4d2608261d11b46b4f279f02f0c1f3ccd525c Mon Sep 17 00:00:00 2001 From: imterah Date: Sun, 22 Jun 2025 12:28:00 -0400 Subject: [PATCH 03/12] chore: Add basic config creation and reading --- .gitignore | 2 + README.md | 2 +- app/config.go | 80 +++++++++++++++++++++++++++++++++++ app/default_config.yml | 18 ++++++++ app/main.go | 95 ++++++++++++++++++++++++++++++++++++++++++ ardriver/ardriver.go | 1 + go.mod | 25 +++++++++++ go.sum | 40 ++++++++++++++++++ unrealxr | 2 +- 9 files changed, 263 insertions(+), 2 deletions(-) create mode 100644 app/config.go create mode 100644 app/default_config.yml create mode 100644 app/main.go create mode 100644 ardriver/ardriver.go create mode 100644 go.mod create mode 100644 go.sum diff --git a/.gitignore b/.gitignore index dcbec21..4df5f94 100644 --- a/.gitignore +++ b/.gitignore @@ -28,3 +28,5 @@ go.work.sum # ---> UnrealXR # development dirs data +# artifacts +app/app diff --git a/README.md b/README.md index f64c367..a87c344 100644 --- a/README.md +++ b/README.md @@ -22,7 +22,7 @@ UnrealXR is a spatial multi-display renderer for the Xreal line of devices, enab Before anything, this depends on the `evdi` Linux kernel module. This is packaged in Debian-based distributions as `evdi-dkms`. If you already have DisplayLink drivers installed for their devices, you likely do not need to do this step. After installing this, please reboot your computer. -You'll need to install build dependencies after this. For Debian-based distros, the dependency list should be: `git golang build-essential libdrm libdrm-dev linux-headers-$(uname -r) cmake clang-tools pkg-config libwayland-client++1 libgl1-mesa-dev libglu1-mesa-dev libwayland-dev libxkbcommon-dev libhidapi-dev libjson-c-dev libudev-dev libusb-1.0-0 libusb-1.0-0-dev libopencv-dev` +You'll need to install build dependencies after this. For Debian-based distros, the dependency list should be: `git golang build-essential libdrm libdrm-dev linux-headers-$(uname -r) cmake clang-tools pkg-config libwayland-client++1 libgl1-mesa-dev libglu1-mesa-dev libwayland-dev libxkbcommon-dev libhidapi-dev libjson-c-dev libudev-dev libusb-1.0-0 libusb-1.0-0-dev libopencv-dev` If you're using Nix/NixOS, all you need to do is use `nix-shell` to enter the development environment. diff --git a/app/config.go b/app/config.go new file mode 100644 index 0000000..1f6b07d --- /dev/null +++ b/app/config.go @@ -0,0 +1,80 @@ +package main + +import _ "embed" + +//go:embed default_config.yml +var InitialConfig []byte + +type DisplayConfig struct { + Angle *int `yaml:"angle"` + FOV *int `yaml:"fov"` + Spacing *int `yaml:"spacing"` + Count *int `yaml:"count"` +} + +type AppOverrides struct { + AllowUnsupportedDevices *bool `yaml:"allow_unsupported_devices"` + OverrideWidth *int `yaml:"width"` + OverrideHeight *int `yaml:"height"` + OverrideRefreshRate *int `yaml:"refresh_rate"` +} + +type Config struct { + DisplayConfig DisplayConfig `yaml:"display"` + Overrides AppOverrides `yaml:"overrides"` +} + +func getPtrToInt(int int) *int { + return &int +} + +func getPtrToBool(bool bool) *bool { + return &bool +} + +var DefaultConfig = &Config{ + DisplayConfig: DisplayConfig{ + Angle: getPtrToInt(45), + FOV: getPtrToInt(45), + Spacing: getPtrToInt(1), + Count: getPtrToInt(3), + }, + Overrides: AppOverrides{ + AllowUnsupportedDevices: getPtrToBool(false), + }, +} + +func InitializePotentiallyMissingConfigValues(config *Config) { + // TODO: is there a better way to do this? + if config.DisplayConfig.Angle == nil { + config.DisplayConfig.Angle = DefaultConfig.DisplayConfig.Angle + } + + if config.DisplayConfig.FOV == nil { + config.DisplayConfig.FOV = DefaultConfig.DisplayConfig.FOV + } + + if config.DisplayConfig.Spacing == nil { + config.DisplayConfig.Spacing = DefaultConfig.DisplayConfig.Spacing + } + + if config.DisplayConfig.Count == nil { + config.DisplayConfig.Count = DefaultConfig.DisplayConfig.Count + } + + if config.Overrides.AllowUnsupportedDevices == nil { + config.Overrides.AllowUnsupportedDevices = DefaultConfig.Overrides.AllowUnsupportedDevices + } + + if config.Overrides.OverrideWidth == nil { + config.Overrides.OverrideWidth = DefaultConfig.Overrides.OverrideWidth + } + + if config.Overrides.OverrideHeight == nil { + config.Overrides.OverrideHeight = DefaultConfig.Overrides.OverrideHeight + } + + if config.Overrides.OverrideRefreshRate == nil { + config.Overrides.OverrideRefreshRate = DefaultConfig.Overrides.OverrideRefreshRate + } +} diff --git a/app/default_config.yml b/app/default_config.yml new file mode 100644 index 0000000..bbe2ef2 --- /dev/null +++ b/app/default_config.yml @@ -0,0 +1,18 @@ +# __ __ ___ __ ____ +# / / / /___ ________ ____ _/ / |/ // __ \ +# / / / / __ \/ ___/ _ \/ __ `/ /| // /_/ / +# / /_/ / / / / / / __/ /_/ / // |/ _, _/ +# \____/_/ /_/_/ \___/\__,_/_//_/|_/_/ |_| +# +# Welcome to UnrealXR! This is the configuration file to configure various UnrealXR settings. + +display: + angle: 45 # Angle of the virtual displays + fov: 45 # FOV of the 3D camera + spacing: 1 # Spacing between virtual displays + count: 3 # Count of virtual displays +overrides: + allow_unsupported_devices: false # If true, allows unsupported devices to be used as long as they're a compatible vendor (Xreal) + # width: 1920 # If set, overrides the width of the screen and virtual displays + # height: 1080 # If set, overrides the height of the screen and virtual displays + # refresh_rate: 120 # If set, overrides the refresh rate of the screen and the maximum refresh rate of the virtual displays diff --git a/app/main.go b/app/main.go new file mode 100644 index 0000000..1506e22 --- /dev/null +++ b/app/main.go @@ -0,0 +1,95 @@ +package main + +import ( + "context" + "fmt" + "os" + "path" + + "github.com/charmbracelet/log" + "github.com/goccy/go-yaml" + "github.com/kirsle/configdir" + "github.com/urfave/cli/v3" +) + +func mainEntrypoint(context.Context, *cli.Command) error { + if os.Geteuid() != 0 { + return fmt.Errorf("this program must be run as root") + } + + log.Info("Initializing UnrealXR") + + // Allow for overriding the config directory + configDir := os.Getenv("UNREALXR_CONFIG_PATH") + + if configDir == "" { + configDir = configdir.LocalConfig("unrealxr") + err := configdir.MakePath(configDir) + + if err != nil { + return fmt.Errorf("failed to ensure config directory exists: %w", err) + } + } + + _, err := os.Stat(path.Join(configDir, "config.yml")) + + if err != nil { + log.Debug("Creating default config file") + err := os.WriteFile(path.Join(configDir, "config.yml"), InitialConfig, 0644) + + if err != nil { + return fmt.Errorf("failed to create initial config file: %w", err) + } + } + + // Read and parse the config file + configBytes, err := os.ReadFile(path.Join(configDir, "config.yml")) + + if err != nil { + return fmt.Errorf("failed to read config file: %w", err) + } + + config := &Config{} + err = yaml.Unmarshal(configBytes, config) + + if err != nil { + return fmt.Errorf("failed to parse config file: %w", err) + } + + InitializePotentiallyMissingConfigValues(config) + return nil +} + +func main() { + logLevel := os.Getenv("UNREALXR_LOG_LEVEL") + + if logLevel != "" { + switch logLevel { + case "debug": + log.SetLevel(log.DebugLevel) + + case "info": + log.SetLevel(log.InfoLevel) + + case "warn": + log.SetLevel(log.WarnLevel) + + case "error": + log.SetLevel(log.ErrorLevel) + + case "fatal": + log.SetLevel(log.FatalLevel) + } + } + + // Initialize the CLI + cmd := &cli.Command{ + Name: "unrealxr", + Usage: "A spatial multi-display renderer for XR devices", + Action: mainEntrypoint, + } + + if err := cmd.Run(context.Background(), os.Args); err != nil { + log.Fatal(err) + } +} diff --git a/ardriver/ardriver.go b/ardriver/ardriver.go new file mode 100644 index 0000000..b58104c --- /dev/null +++ b/ardriver/ardriver.go @@ -0,0 +1 @@ +package ardriver diff --git a/go.mod b/go.mod new file mode 100644 index 0000000..3078cf8 --- /dev/null +++ b/go.mod @@ -0,0 +1,25 @@ +module git.terah.dev/UnrealXR/unrealxr + +go 1.24.3 + +require ( + github.com/aymanbagabas/go-osc52/v2 v2.0.1 // indirect + github.com/charmbracelet/colorprofile v0.2.3-0.20250311203215-f60798e515dc // indirect + github.com/charmbracelet/lipgloss v1.1.0 // indirect + github.com/charmbracelet/log v0.4.2 // indirect + github.com/charmbracelet/x/ansi v0.8.0 // indirect + github.com/charmbracelet/x/cellbuf v0.0.13-0.20250311204145-2c3ea96c31dd // indirect + github.com/charmbracelet/x/term v0.2.1 // indirect + github.com/go-logfmt/logfmt v0.6.0 // indirect + github.com/goccy/go-yaml v1.18.0 // indirect + github.com/kirsle/configdir v0.0.0-20170128060238-e45d2f54772f // indirect + github.com/lucasb-eyer/go-colorful v1.2.0 // indirect + github.com/mattn/go-isatty v0.0.20 // indirect + github.com/mattn/go-runewidth v0.0.16 // indirect + github.com/muesli/termenv v0.16.0 // indirect + github.com/rivo/uniseg v0.4.7 // indirect + github.com/urfave/cli/v3 v3.3.8 // indirect + github.com/xo/terminfo v0.0.0-20220910002029-abceb7e1c41e // indirect + golang.org/x/exp v0.0.0-20231006140011-7918f672742d // indirect + golang.org/x/sys v0.30.0 // indirect +) diff --git a/go.sum b/go.sum new file mode 100644 index 0000000..7aa150c --- /dev/null +++ b/go.sum @@ -0,0 +1,40 @@ +github.com/aymanbagabas/go-osc52/v2 v2.0.1 h1:HwpRHbFMcZLEVr42D4p7XBqjyuxQH5SMiErDT4WkJ2k= +github.com/aymanbagabas/go-osc52/v2 v2.0.1/go.mod h1:uYgXzlJ7ZpABp8OJ+exZzJJhRNQ2ASbcXHWsFqH8hp8= +github.com/charmbracelet/colorprofile v0.2.3-0.20250311203215-f60798e515dc h1:4pZI35227imm7yK2bGPcfpFEmuY1gc2YSTShr4iJBfs= +github.com/charmbracelet/colorprofile v0.2.3-0.20250311203215-f60798e515dc/go.mod h1:X4/0JoqgTIPSFcRA/P6INZzIuyqdFY5rm8tb41s9okk= +github.com/charmbracelet/lipgloss v1.1.0 h1:vYXsiLHVkK7fp74RkV7b2kq9+zDLoEU4MZoFqR/noCY= +github.com/charmbracelet/lipgloss v1.1.0/go.mod h1:/6Q8FR2o+kj8rz4Dq0zQc3vYf7X+B0binUUBwA0aL30= +github.com/charmbracelet/log v0.4.2 h1:hYt8Qj6a8yLnvR+h7MwsJv/XvmBJXiueUcI3cIxsyig= +github.com/charmbracelet/log v0.4.2/go.mod h1:qifHGX/tc7eluv2R6pWIpyHDDrrb/AG71Pf2ysQu5nw= +github.com/charmbracelet/x/ansi v0.8.0 h1:9GTq3xq9caJW8ZrBTe0LIe2fvfLR/bYXKTx2llXn7xE= +github.com/charmbracelet/x/ansi v0.8.0/go.mod h1:wdYl/ONOLHLIVmQaxbIYEC/cRKOQyjTkowiI4blgS9Q= +github.com/charmbracelet/x/cellbuf v0.0.13-0.20250311204145-2c3ea96c31dd h1:vy0GVL4jeHEwG5YOXDmi86oYw2yuYUGqz6a8sLwg0X8= +github.com/charmbracelet/x/cellbuf v0.0.13-0.20250311204145-2c3ea96c31dd/go.mod h1:xe0nKWGd3eJgtqZRaN9RjMtK7xUYchjzPr7q6kcvCCs= +github.com/charmbracelet/x/term v0.2.1 h1:AQeHeLZ1OqSXhrAWpYUtZyX1T3zVxfpZuEQMIQaGIAQ= +github.com/charmbracelet/x/term v0.2.1/go.mod h1:oQ4enTYFV7QN4m0i9mzHrViD7TQKvNEEkHUMCmsxdUg= +github.com/go-logfmt/logfmt v0.6.0 h1:wGYYu3uicYdqXVgoYbvnkrPVXkuLM1p1ifugDMEdRi4= +github.com/go-logfmt/logfmt v0.6.0/go.mod h1:WYhtIu8zTZfxdn5+rREduYbwxfcBr/Vr6KEVveWlfTs= +github.com/goccy/go-yaml v1.18.0 h1:8W7wMFS12Pcas7KU+VVkaiCng+kG8QiFeFwzFb+rwuw= +github.com/goccy/go-yaml v1.18.0/go.mod h1:XBurs7gK8ATbW4ZPGKgcbrY1Br56PdM69F7LkFRi1kA= +github.com/kirsle/configdir v0.0.0-20170128060238-e45d2f54772f h1:dKccXx7xA56UNqOcFIbuqFjAWPVtP688j5QMgmo6OHU= +github.com/kirsle/configdir v0.0.0-20170128060238-e45d2f54772f/go.mod h1:4rEELDSfUAlBSyUjPG0JnaNGjf13JySHFeRdD/3dLP0= +github.com/lucasb-eyer/go-colorful v1.2.0 h1:1nnpGOrhyZZuNyfu1QjKiUICQ74+3FNCN69Aj6K7nkY= +github.com/lucasb-eyer/go-colorful v1.2.0/go.mod h1:R4dSotOR9KMtayYi1e77YzuveK+i7ruzyGqttikkLy0= +github.com/mattn/go-isatty v0.0.20 h1:xfD0iDuEKnDkl03q4limB+vH+GxLEtL/jb4xVJSWWEY= +github.com/mattn/go-isatty v0.0.20/go.mod h1:W+V8PltTTMOvKvAeJH7IuucS94S2C6jfK/D7dTCTo3Y= +github.com/mattn/go-runewidth v0.0.16 h1:E5ScNMtiwvlvB5paMFdw9p4kSQzbXFikJ5SQO6TULQc= +github.com/mattn/go-runewidth v0.0.16/go.mod h1:Jdepj2loyihRzMpdS35Xk/zdY8IAYHsh153qUoGf23w= +github.com/muesli/termenv v0.16.0 h1:S5AlUN9dENB57rsbnkPyfdGuWIlkmzJjbFf0Tf5FWUc= +github.com/muesli/termenv v0.16.0/go.mod h1:ZRfOIKPFDYQoDFF4Olj7/QJbW60Ol/kL1pU3VfY/Cnk= +github.com/rivo/uniseg v0.2.0/go.mod h1:J6wj4VEh+S6ZtnVlnTBMWIodfgj8LQOQFoIToxlJtxc= +github.com/rivo/uniseg v0.4.7 h1:WUdvkW8uEhrYfLC4ZzdpI2ztxP1I582+49Oc5Mq64VQ= +github.com/rivo/uniseg v0.4.7/go.mod h1:FN3SvrM+Zdj16jyLfmOkMNblXMcoc8DfTHruCPUcx88= +github.com/urfave/cli/v3 v3.3.8 h1:BzolUExliMdet9NlJ/u4m5vHSotJ3PzEqSAZ1oPMa/E= +github.com/urfave/cli/v3 v3.3.8/go.mod h1:FJSKtM/9AiiTOJL4fJ6TbMUkxBXn7GO9guZqoZtpYpo= +github.com/xo/terminfo v0.0.0-20220910002029-abceb7e1c41e h1:JVG44RsyaB9T2KIHavMF/ppJZNG9ZpyihvCd0w101no= +github.com/xo/terminfo v0.0.0-20220910002029-abceb7e1c41e/go.mod h1:RbqR21r5mrJuqunuUZ/Dhy/avygyECGrLceyNeo4LiM= +golang.org/x/exp v0.0.0-20231006140011-7918f672742d h1:jtJma62tbqLibJ5sFQz8bKtEM8rJBtfilJ2qTU199MI= +golang.org/x/exp v0.0.0-20231006140011-7918f672742d/go.mod h1:ldy0pHrwJyGW56pPQzzkH36rKxoZW1tw7ZJpeKx+hdo= +golang.org/x/sys v0.6.0/go.mod h1:oPkhp1MJrh7nUepCBck5+mAzfO9JrbApNNgaTdGDITg= +golang.org/x/sys v0.30.0 h1:QjkSwP/36a20jFYWkSue1YwXzLmsV5Gfq7Eiy72C1uc= +golang.org/x/sys v0.30.0/go.mod h1:/VUhepiaJMQUp4+oa/7Zr1D23ma6VTLIYjOOTFZPUcA= diff --git a/unrealxr b/unrealxr index 3af86ef..764a6d9 100755 --- a/unrealxr +++ b/unrealxr @@ -1 +1 @@ -sudo LD_LIBRARY_PATH="$LD_LIBRARY_PATH" UNREALXR_CONFIG_PATH="$UNREALXR_CONFIG_PATH" UNREALXR_DATA_PATH="$UNREALXR_DATA_PATH" WAYLAND_DISPLAY="$XDG_RUNTIME_DIR/$WAYLAND_DISPLAY" XDG_RUNTIME_DIR="/user/run/0" python3 main.py +sudo UNREALXR_LOG_LEVEL="$UNREALXR_LOG_LEVEL" LD_LIBRARY_PATH="$LD_LIBRARY_PATH" UNREALXR_CONFIG_PATH="$UNREALXR_CONFIG_PATH" UNREALXR_DATA_PATH="$UNREALXR_DATA_PATH" WAYLAND_DISPLAY="$XDG_RUNTIME_DIR/$WAYLAND_DISPLAY" XDG_RUNTIME_DIR="/user/run/0" ./app/app From 183d2606dc1334f3fa69212dc035b50d56da1a70 Mon Sep 17 00:00:00 2001 From: imterah Date: Sun, 22 Jun 2025 20:14:04 -0400 Subject: [PATCH 04/12] feature: Add basic EDID reading, writing, and patching capabilities --- app/{ => config}/config.go | 2 +- app/{ => config}/default_config.yml | 0 app/edidtools/edid_parser.go | 72 +++++++++++++++ app/edidtools/patching_tools_linux.go | 127 ++++++++++++++++++++++++++ app/edidtools/patching_tools_macos.go | 21 +++++ app/edidtools/patching_tools_win.go | 21 +++++ app/edidtools/quirks.go | 14 +++ app/edidtools/struct.go | 20 ++++ app/main.go | 51 ++++++++++- edidpatcher/edidpatcher.go | 102 +++++++++++++++++++++ go.mod | 2 + go.sum | 4 + 12 files changed, 431 insertions(+), 5 deletions(-) rename app/{ => config}/config.go (99%) rename app/{ => config}/default_config.yml (100%) create mode 100644 app/edidtools/edid_parser.go create mode 100644 app/edidtools/patching_tools_linux.go create mode 100644 app/edidtools/patching_tools_macos.go create mode 100644 app/edidtools/patching_tools_win.go create mode 100644 app/edidtools/quirks.go create mode 100644 app/edidtools/struct.go create mode 100644 edidpatcher/edidpatcher.go diff --git a/app/config.go b/app/config/config.go similarity index 99% rename from app/config.go rename to app/config/config.go index 1f6b07d..307a14b 100644 --- a/app/config.go +++ b/app/config/config.go @@ -1,4 +1,4 @@ -package main +package config import _ "embed" diff --git a/app/default_config.yml b/app/config/default_config.yml similarity index 100% rename from app/default_config.yml rename to app/config/default_config.yml diff --git a/app/edidtools/edid_parser.go b/app/edidtools/edid_parser.go new file mode 100644 index 0000000..fbb7eba --- /dev/null +++ b/app/edidtools/edid_parser.go @@ -0,0 +1,72 @@ +package edidtools + +import ( + "fmt" + + edidparser "github.com/anoopengineer/edidparser/edid" +) + +func ParseEDID(rawEDIDFile []byte, allowUnsupportedDevices bool) (*DisplayMetadata, error) { + parsedEDID, err := edidparser.NewEdid(rawEDIDFile) + + if err != nil { + return nil, fmt.Errorf("failed to parse EDID file: %w", err) + } + + for manufacturer, manufacturerSupportedDevices := range QuirksRegistry { + if parsedEDID.ManufacturerId == manufacturer { + if deviceQuirks, ok := manufacturerSupportedDevices[parsedEDID.MonitorName]; ok || allowUnsupportedDevices { + maxWidth := 0 + maxHeight := 0 + maxRefreshRate := 0 + + for _, resolution := range parsedEDID.DetailedTimingDescriptors { + if int(resolution.HorizontalActive) > maxWidth && int(resolution.VerticalActive) > maxHeight { + maxWidth = int(resolution.HorizontalActive) + maxHeight = int(resolution.VerticalActive) + } + + // Convert pixel clock to refresh rate + // Refresh Rate = Pixel Clock / ((Horizontal Active + Horizontal Blanking) * (Vertical Active + Vertical Blanking)) + hTotal := int(resolution.HorizontalActive + resolution.HorizontalBlanking) + vTotal := int(resolution.VerticalActive + resolution.VerticalBlanking) + refreshRate := int(int(resolution.PixelClock*1000) / (hTotal * vTotal)) + + if refreshRate > maxRefreshRate { + maxRefreshRate = refreshRate + } + } + + if maxWidth == 0 || maxHeight == 0 { + if deviceQuirks.MaxWidth == 0 || deviceQuirks.MaxHeight == 0 { + return nil, fmt.Errorf("failed to determine maximum resolution for monitor '%s'", parsedEDID.MonitorName) + } + + maxWidth = deviceQuirks.MaxWidth + maxHeight = deviceQuirks.MaxHeight + } + + if maxRefreshRate == 0 { + if deviceQuirks.MaxRefreshRate == 0 { + return nil, fmt.Errorf("failed to determine maximum refresh rate for monitor '%s'", parsedEDID.MonitorName) + } + + maxRefreshRate = deviceQuirks.MaxRefreshRate + } + + displayMetadata := &DisplayMetadata{ + EDID: rawEDIDFile, + DeviceVendor: parsedEDID.ManufacturerId, + DeviceQuirks: deviceQuirks, + MaxWidth: maxWidth, + MaxHeight: maxHeight, + MaxRefreshRate: maxRefreshRate, + } + + return displayMetadata, nil + } + } + } + + return nil, fmt.Errorf("failed to match manufacturer for monitor vendor: '%s'", parsedEDID.ManufacturerId) +} diff --git a/app/edidtools/patching_tools_linux.go b/app/edidtools/patching_tools_linux.go new file mode 100644 index 0000000..6940321 --- /dev/null +++ b/app/edidtools/patching_tools_linux.go @@ -0,0 +1,127 @@ +//go:build linux +// +build linux + +package edidtools + +import ( + "fmt" + "os" + "os/exec" + "strings" + + "github.com/charmbracelet/log" +) + +// Attempts to fetch the EDID firmware for any supported XR glasses device +func FetchXRGlassEDID(allowUnsupportedDevices bool) (*DisplayMetadata, error) { + // Implementation goes here + pciDeviceCommand, err := exec.Command("lspci").Output() + + if err != nil { + return nil, fmt.Errorf("failed to execute lspci command: %w", err) + } + + pciDevices := strings.Split(string(pciDeviceCommand), "\n") + pciDevices = pciDevices[:len(pciDevices)-1] + + vgaDevices := []string{} + + for _, pciDevice := range pciDevices { + if strings.Contains(pciDevice, "VGA compatible controller:") { + vgaDevices = append(vgaDevices, pciDevice[:strings.Index(pciDevice, " ")]) + } + } + + for _, vgaDevice := range vgaDevices { + cardDevices, err := os.ReadDir("/sys/devices/pci0000:00/0000:" + vgaDevice + "/drm/") + + if err != nil { + return nil, fmt.Errorf("failed to read directory for device '%s': %w", vgaDevice, err) + } + + for _, cardDevice := range cardDevices { + if !strings.Contains(cardDevice.Name(), "card") { + continue + } + + monitors, err := os.ReadDir("/sys/devices/pci0000:00/0000:" + vgaDevice + "/drm/" + cardDevice.Name()) + + if err != nil { + return nil, fmt.Errorf("failed to read directory for card device '%s': %w", cardDevice.Name(), err) + } + + for _, monitor := range monitors { + if !strings.Contains(monitor.Name(), cardDevice.Name()) { + continue + } + + rawEDIDFile, err := os.ReadFile("/sys/devices/pci0000:00/0000:" + vgaDevice + "/drm/" + cardDevice.Name() + "/" + monitor.Name() + "/edid") + + if err != nil { + return nil, fmt.Errorf("failed to read EDID file for monitor '%s': %w", monitor.Name(), err) + } + + if len(rawEDIDFile) == 0 { + continue + } + + parsedEDID, err := ParseEDID(rawEDIDFile, allowUnsupportedDevices) + + if err != nil { + if !strings.HasPrefix(err.Error(), "failed to match manufacturer for monitor vendor") { + log.Warnf("Failed to parse EDID for monitor '%s': %s", monitor.Name(), err.Error()) + } + } else { + parsedEDID.LinuxDRMCard = cardDevice.Name() + parsedEDID.LinuxDRMConnector = strings.Replace(cardDevice.Name(), cardDevice.Name()+"-", "", 1) + + return parsedEDID, nil + } + } + } + } + + return nil, fmt.Errorf("could not find supported device! Check if the XR device is plugged in. If it is plugged in and working correctly, check the README or open an issue.") +} + +// Loads custom firmware for a supported XR glass device +func LoadCustomEDIDFirmware(displayMetadata *DisplayMetadata, edidFirmware []byte) error { + if displayMetadata.LinuxDRMCard == "" || displayMetadata.LinuxDRMConnector == "" { + return fmt.Errorf("missing Linux DRM card or connector information") + } + + drmFile, err := os.Open("/sys/kernel/debug/dri/" + strings.Replace(displayMetadata.LinuxDRMCard, "card", "", 1) + "/" + displayMetadata.LinuxDRMConnector + "/edid_override") + + if err != nil { + return fmt.Errorf("failed to open EDID override file for monitor '%s': %w", displayMetadata.LinuxDRMConnector, err) + } + + defer drmFile.Close() + + if _, err := drmFile.Write(edidFirmware); err != nil { + return fmt.Errorf("failed to write EDID firmware for monitor '%s': %w", displayMetadata.LinuxDRMConnector, err) + } + + return nil +} + +// Unloads custom firmware for a supported XR glass device +func UnloadCustomEDIDFirmware(displayMetadata *DisplayMetadata) error { + if displayMetadata.LinuxDRMCard == "" || displayMetadata.LinuxDRMConnector == "" { + return fmt.Errorf("missing Linux DRM card or connector information") + } + + drmFile, err := os.Open("/sys/kernel/debug/dri/" + strings.Replace(displayMetadata.LinuxDRMCard, "card", "", 1) + "/" + displayMetadata.LinuxDRMConnector + "/edid_override") + + if err != nil { + return fmt.Errorf("failed to open EDID override file for monitor '%s': %w", displayMetadata.LinuxDRMConnector, err) + } + + defer drmFile.Close() + + if _, err := drmFile.Write([]byte("reset")); err != nil { + return fmt.Errorf("failed to unload EDID firmware for monitor '%s': %w", displayMetadata.LinuxDRMConnector, err) + } + + return nil +} diff --git a/app/edidtools/patching_tools_macos.go b/app/edidtools/patching_tools_macos.go new file mode 100644 index 0000000..d41de0b --- /dev/null +++ b/app/edidtools/patching_tools_macos.go @@ -0,0 +1,21 @@ +//go:build darwin +// +build darwin + +package edidtools + +import "fmt" + +// Attempts to fetch the EDID firmware for any supported XR glasses device +func FetchXRGlassEDID(allowUnsupportedDevices bool) (*DisplayMetadata, error) { + return nil, fmt.Errorf("automatic fetching of EDID data is not supported on macOS") +} + +// Loads custom firmware for a supported XR glass device +func LoadCustomEDIDFirmware(displayMetadata *DisplayMetadata, edidFirmware []byte) error { + return fmt.Errorf("loading custom EDID firmware is not supported on macOS") +} + +// Unloads custom firmware for a supported XR glass device +func UnloadCustomEDIDFirmware(displayMetadata *DisplayMetadata) error { + return fmt.Errorf("unloading custom EDID firmware is not supported on macOS") +} diff --git a/app/edidtools/patching_tools_win.go b/app/edidtools/patching_tools_win.go new file mode 100644 index 0000000..e9f515f --- /dev/null +++ b/app/edidtools/patching_tools_win.go @@ -0,0 +1,21 @@ +//go:build windows +// +build windows + +package edidtools + +import "fmt" + +// Attempts to fetch the EDID firmware for any supported XR glasses device +func FetchXRGlassEDID(allowUnsupportedDevices bool) (*DisplayMetadata, error) { + return nil, fmt.Errorf("automatic fetching of EDID data is not supported on Windows") +} + +// Loads custom firmware for a supported XR glass device +func LoadCustomEDIDFirmware(displayMetadata *DisplayMetadata, edidFirmware []byte) error { + return fmt.Errorf("loading custom EDID firmware is not supported on Windows") +} + +// Unloads custom firmware for a supported XR glass device +func UnloadCustomEDIDFirmware(displayMetadata *DisplayMetadata) error { + return fmt.Errorf("unloading custom EDID firmware is not supported on Windows") +} diff --git a/app/edidtools/quirks.go b/app/edidtools/quirks.go new file mode 100644 index 0000000..bd7618b --- /dev/null +++ b/app/edidtools/quirks.go @@ -0,0 +1,14 @@ +package edidtools + +// Vendor and devices names sourced from "https://uefi.org/uefi-pnp-export" +var QuirksRegistry = map[string]map[string]DisplayQuirks{ + "MRG": { + "Air": { + MaxWidth: 1920, + MaxHeight: 1080, + MaxRefreshRate: 120, + SensorInitDelay: 10, + ZVectorDisabled: true, + }, + }, +} diff --git a/app/edidtools/struct.go b/app/edidtools/struct.go new file mode 100644 index 0000000..361c440 --- /dev/null +++ b/app/edidtools/struct.go @@ -0,0 +1,20 @@ +package edidtools + +type DisplayQuirks struct { + MaxWidth int + MaxHeight int + MaxRefreshRate int + SensorInitDelay int + ZVectorDisabled bool +} + +type DisplayMetadata struct { + EDID []byte + DeviceVendor string + DeviceQuirks DisplayQuirks + MaxWidth int + MaxHeight int + MaxRefreshRate int + LinuxDRMCard string + LinuxDRMConnector string +} diff --git a/app/main.go b/app/main.go index 1506e22..a7bf8f4 100644 --- a/app/main.go +++ b/app/main.go @@ -1,11 +1,15 @@ package main import ( + "bufio" "context" "fmt" "os" "path" + libconfig "git.terah.dev/UnrealXR/unrealxr/app/config" + "git.terah.dev/UnrealXR/unrealxr/app/edidtools" + "git.terah.dev/UnrealXR/unrealxr/edidpatcher" "github.com/charmbracelet/log" "github.com/goccy/go-yaml" "github.com/kirsle/configdir" @@ -35,7 +39,7 @@ func mainEntrypoint(context.Context, *cli.Command) error { if err != nil { log.Debug("Creating default config file") - err := os.WriteFile(path.Join(configDir, "config.yml"), InitialConfig, 0644) + err := os.WriteFile(path.Join(configDir, "config.yml"), libconfig.InitialConfig, 0644) if err != nil { return fmt.Errorf("failed to create initial config file: %w", err) @@ -49,14 +53,53 @@ func mainEntrypoint(context.Context, *cli.Command) error { return fmt.Errorf("failed to read config file: %w", err) } - config := &Config{} + config := &libconfig.Config{} err = yaml.Unmarshal(configBytes, config) if err != nil { return fmt.Errorf("failed to parse config file: %w", err) } - InitializePotentiallyMissingConfigValues(config) + libconfig.InitializePotentiallyMissingConfigValues(config) + log.Info("Attempting to read display EDID file and fetch metadata") + + displayMetadata, err := edidtools.FetchXRGlassEDID(*config.Overrides.AllowUnsupportedDevices) + + if err != nil { + return fmt.Errorf("failed to fetch EDID or get metadata: %w", err) + } + + log.Info("Got EDID file and metadata") + log.Info("Patching EDID firmware to be specialized") + + patchedFirmware, err := edidpatcher.PatchEDIDToBeSpecialized(displayMetadata.EDID) + + if err != nil { + return fmt.Errorf("failed to patch EDID firmware: %w", err) + } + + log.Info("Uploading patched EDID firmware") + err = edidtools.LoadCustomEDIDFirmware(displayMetadata, patchedFirmware) + + if err != nil { + return fmt.Errorf("failed to upload patched EDID firmware: %w", err) + } + + defer func() { + err := edidtools.UnloadCustomEDIDFirmware(displayMetadata) + + if err != nil { + log.Errorf("Failed to unload custom EDID firmware: %s", err.Error()) + } + + log.Info("Please unplug and plug in your XR device to restore it back to normal settings.") + }() + + fmt.Print("Press the Enter key to continue loading after you unplug and plug in your XR device.") + bufio.NewReader(os.Stdin).ReadBytes('\n') // Wait for Enter key press before continuing + + log.Info("Initializing XR headset") + return nil } @@ -90,6 +133,6 @@ func main() { } if err := cmd.Run(context.Background(), os.Args); err != nil { - log.Fatal(err) + log.Fatalf("Fatal error during execution: %s", err.Error()) } } diff --git a/edidpatcher/edidpatcher.go b/edidpatcher/edidpatcher.go new file mode 100644 index 0000000..a56b386 --- /dev/null +++ b/edidpatcher/edidpatcher.go @@ -0,0 +1,102 @@ +package edidpatcher + +import ( + "fmt" + + "github.com/charmbracelet/log" + "github.com/google/uuid" +) + +// Calculates a checksum for a given EDID block (base EDID, extension blocks, etc.) +func CalculateEDIDChecksum(edidBlock []byte) byte { + sum := 0 + + for _, value := range edidBlock[:len(edidBlock)-1] { + sum += int(value) + } + + return byte((-sum) & 0xFF) +} + +var MSFTPayloadSize = byte(22 + 4) + +// Patch a given EDID to be a "specialized display", allowing for the display to be used by third-party window-managers/compositors/applications directly. +func PatchEDIDToBeSpecialized(edid []byte) ([]byte, error) { + newEDID := make([]byte, len(edid)) + copy(newEDID, edid) + + isAnEnhancedEDID := len(newEDID) > 128 + + foundExtensionBase := 0 + extensionBaseExists := false + + // Find an appropriate extension base + if isAnEnhancedEDID { + for currentExtensionPosition := 128; currentExtensionPosition < len(newEDID); currentExtensionPosition += 128 { + if newEDID[currentExtensionPosition] != 0x02 { + continue + } + + if newEDID[currentExtensionPosition+1] != 0x03 { + log.Warn("Incompatible version detected for ANSI CTA data section in EDID") + } + + foundExtensionBase = currentExtensionPosition + extensionBaseExists = true + } + + if foundExtensionBase == 0 { + foundExtensionBase = len(newEDID) + newEDID = append(newEDID, make([]byte, 128)...) + } + } else { + foundExtensionBase = 128 + newEDID = append(newEDID, make([]byte, 128)...) + } + + newEDID[foundExtensionBase+2] = MSFTPayloadSize + + if !extensionBaseExists { + // Add another extension to the original EDID + if newEDID[126] == 255 { + return nil, fmt.Errorf("EDID extension block limit reached, but we need to add another extension") + } + + newEDID[126] += 1 + newEDID[127] = CalculateEDIDChecksum(newEDID[:128]) + + newEDID[foundExtensionBase] = 0x02 + newEDID[foundExtensionBase+1] = 0x03 + newEDID[foundExtensionBase+3] = 0x00 + } else { + if newEDID[foundExtensionBase+2] != MSFTPayloadSize && newEDID[foundExtensionBase+2] != 0 { + currentBase := newEDID[foundExtensionBase+2] + + copy(newEDID[foundExtensionBase+4:foundExtensionBase+int(currentBase)-1], make([]byte, int(currentBase)-1)) + copy(newEDID[foundExtensionBase+int(MSFTPayloadSize):foundExtensionBase+127], newEDID[foundExtensionBase+int(currentBase):foundExtensionBase+127]) + } + } + + generatedUUID := uuid.New() + uuidBytes, err := generatedUUID.MarshalBinary() + + if err != nil { + return nil, fmt.Errorf("failed to marshal UUID: %w", err) + } + + // Implemented using https://learn.microsoft.com/en-us/windows-hardware/drivers/display/specialized-monitors-edid-extension + // VST & Length + newEDID[foundExtensionBase+4] = 0x3<<5 | 0x15 // 0x3: vendor specific tag; 0x15: length + // Assigned IEEE OUI + newEDID[foundExtensionBase+5] = 0x5C + newEDID[foundExtensionBase+6] = 0x12 + newEDID[foundExtensionBase+7] = 0xCA + // Actual data + newEDID[foundExtensionBase+8] = 0x2 // Using version 0x2 for better compatibility + newEDID[foundExtensionBase+9] = 0x7 // Using VR tag for better compatibility even though it probably doesn't matter + copy(newEDID[foundExtensionBase+10:foundExtensionBase+10+16], uuidBytes) + + newEDID[foundExtensionBase+127] = CalculateEDIDChecksum(newEDID[foundExtensionBase : foundExtensionBase+127]) + + return newEDID, nil +} diff --git a/go.mod b/go.mod index 3078cf8..b09e4fc 100644 --- a/go.mod +++ b/go.mod @@ -3,6 +3,7 @@ module git.terah.dev/UnrealXR/unrealxr go 1.24.3 require ( + github.com/anoopengineer/edidparser v0.0.0-20240602223913-86ca9ed3d2b0 // indirect github.com/aymanbagabas/go-osc52/v2 v2.0.1 // indirect github.com/charmbracelet/colorprofile v0.2.3-0.20250311203215-f60798e515dc // indirect github.com/charmbracelet/lipgloss v1.1.0 // indirect @@ -12,6 +13,7 @@ require ( github.com/charmbracelet/x/term v0.2.1 // indirect github.com/go-logfmt/logfmt v0.6.0 // indirect github.com/goccy/go-yaml v1.18.0 // indirect + github.com/google/uuid v1.6.0 // indirect github.com/kirsle/configdir v0.0.0-20170128060238-e45d2f54772f // indirect github.com/lucasb-eyer/go-colorful v1.2.0 // indirect github.com/mattn/go-isatty v0.0.20 // indirect diff --git a/go.sum b/go.sum index 7aa150c..63b1134 100644 --- a/go.sum +++ b/go.sum @@ -1,3 +1,5 @@ +github.com/anoopengineer/edidparser v0.0.0-20240602223913-86ca9ed3d2b0 h1:rTfysyBCL7LPbq9GFpQbllvKT8vEI93lQUwksMMxHMI= +github.com/anoopengineer/edidparser v0.0.0-20240602223913-86ca9ed3d2b0/go.mod h1:fEt61NePh3ZMxA+g3iC4CaGzY9lEsHRUkYJY2x0lBAw= github.com/aymanbagabas/go-osc52/v2 v2.0.1 h1:HwpRHbFMcZLEVr42D4p7XBqjyuxQH5SMiErDT4WkJ2k= github.com/aymanbagabas/go-osc52/v2 v2.0.1/go.mod h1:uYgXzlJ7ZpABp8OJ+exZzJJhRNQ2ASbcXHWsFqH8hp8= github.com/charmbracelet/colorprofile v0.2.3-0.20250311203215-f60798e515dc h1:4pZI35227imm7yK2bGPcfpFEmuY1gc2YSTShr4iJBfs= @@ -16,6 +18,8 @@ github.com/go-logfmt/logfmt v0.6.0 h1:wGYYu3uicYdqXVgoYbvnkrPVXkuLM1p1ifugDMEdRi github.com/go-logfmt/logfmt v0.6.0/go.mod h1:WYhtIu8zTZfxdn5+rREduYbwxfcBr/Vr6KEVveWlfTs= github.com/goccy/go-yaml v1.18.0 h1:8W7wMFS12Pcas7KU+VVkaiCng+kG8QiFeFwzFb+rwuw= github.com/goccy/go-yaml v1.18.0/go.mod h1:XBurs7gK8ATbW4ZPGKgcbrY1Br56PdM69F7LkFRi1kA= +github.com/google/uuid v1.6.0 h1:NIvaJDMOsjHA8n1jAhLSgzrAzy1Hgr+hNrb57e+94F0= +github.com/google/uuid v1.6.0/go.mod h1:TIyPZe4MgqvfeYDBFedMoGGpEw/LqOeaOT+nhxU+yHo= github.com/kirsle/configdir v0.0.0-20170128060238-e45d2f54772f h1:dKccXx7xA56UNqOcFIbuqFjAWPVtP688j5QMgmo6OHU= github.com/kirsle/configdir v0.0.0-20170128060238-e45d2f54772f/go.mod h1:4rEELDSfUAlBSyUjPG0JnaNGjf13JySHFeRdD/3dLP0= github.com/lucasb-eyer/go-colorful v1.2.0 h1:1nnpGOrhyZZuNyfu1QjKiUICQ74+3FNCN69Aj6K7nkY= From 1168edfd5283bf37c7aea7059d0f00c5197f667d Mon Sep 17 00:00:00 2001 From: imterah Date: Sun, 22 Jun 2025 20:19:47 -0400 Subject: [PATCH 05/12] chore: Change wording in default_config to not imply overclocking capabilities --- app/config/default_config.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/app/config/default_config.yml b/app/config/default_config.yml index bbe2ef2..a367a51 100644 --- a/app/config/default_config.yml +++ b/app/config/default_config.yml @@ -13,6 +13,6 @@ display: count: 3 # Count of virtual displays overrides: allow_unsupported_devices: false # If true, allows unsupported devices to be used as long as they're a compatible vendor (Xreal) - # width: 1920 # If set, overrides the width of the screen and virtual displays - # height: 1080 # If set, overrides the height of the screen and virtual displays - # refresh_rate: 120 # If set, overrides the refresh rate of the screen and the maximum refresh rate of the virtual displays + # width: 1920 # If set, overrides the width of the screen and virtual displays. This does not do any overclocking. + # height: 1080 # If set, overrides the height of the screen and virtual displays. This does not do any overclocking. + # refresh_rate: 120 # If set, overrides the refresh rate of the screen and the maximum refresh rate of the virtual displays. This does not do any overclocking. From b1c83dbf6931f1d3e43c8876d73b236d24a56793 Mon Sep 17 00:00:00 2001 From: imterah Date: Mon, 23 Jun 2025 11:23:11 -0400 Subject: [PATCH 06/12] fix: Fixes Linux patching tools to upload custom EDID data correctly --- app/edidtools/patching_tools_linux.go | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/app/edidtools/patching_tools_linux.go b/app/edidtools/patching_tools_linux.go index 6940321..cea8cbd 100644 --- a/app/edidtools/patching_tools_linux.go +++ b/app/edidtools/patching_tools_linux.go @@ -73,7 +73,7 @@ func FetchXRGlassEDID(allowUnsupportedDevices bool) (*DisplayMetadata, error) { } } else { parsedEDID.LinuxDRMCard = cardDevice.Name() - parsedEDID.LinuxDRMConnector = strings.Replace(cardDevice.Name(), cardDevice.Name()+"-", "", 1) + parsedEDID.LinuxDRMConnector = strings.Replace(monitor.Name(), cardDevice.Name()+"-", "", 1) return parsedEDID, nil } @@ -90,7 +90,7 @@ func LoadCustomEDIDFirmware(displayMetadata *DisplayMetadata, edidFirmware []byt return fmt.Errorf("missing Linux DRM card or connector information") } - drmFile, err := os.Open("/sys/kernel/debug/dri/" + strings.Replace(displayMetadata.LinuxDRMCard, "card", "", 1) + "/" + displayMetadata.LinuxDRMConnector + "/edid_override") + drmFile, err := os.OpenFile("/sys/kernel/debug/dri/"+strings.Replace(displayMetadata.LinuxDRMCard, "card", "", 1)+"/"+displayMetadata.LinuxDRMConnector+"/edid_override", os.O_WRONLY, 0644) if err != nil { return fmt.Errorf("failed to open EDID override file for monitor '%s': %w", displayMetadata.LinuxDRMConnector, err) @@ -111,7 +111,7 @@ func UnloadCustomEDIDFirmware(displayMetadata *DisplayMetadata) error { return fmt.Errorf("missing Linux DRM card or connector information") } - drmFile, err := os.Open("/sys/kernel/debug/dri/" + strings.Replace(displayMetadata.LinuxDRMCard, "card", "", 1) + "/" + displayMetadata.LinuxDRMConnector + "/edid_override") + drmFile, err := os.OpenFile("/sys/kernel/debug/dri/"+strings.Replace(displayMetadata.LinuxDRMCard, "card", "", 1)+"/"+displayMetadata.LinuxDRMConnector+"/edid_override", os.O_WRONLY, 0644) if err != nil { return fmt.Errorf("failed to open EDID override file for monitor '%s': %w", displayMetadata.LinuxDRMConnector, err) From 6d5444a31cf9ce53995183669456c168cf2b5e18 Mon Sep 17 00:00:00 2001 From: imterah Date: Mon, 23 Jun 2025 12:00:57 -0400 Subject: [PATCH 07/12] chore: Get raylib POC working --- .gitignore | 2 +- Makefile | 13 +++++++++++++ README.md | 2 +- app/main.go | 16 ++++++++++++++++ go.mod | 7 +++++-- go.sum | 10 ++++++++++ unrealxr | 2 +- 7 files changed, 47 insertions(+), 5 deletions(-) create mode 100644 Makefile diff --git a/.gitignore b/.gitignore index 4df5f94..3ef1354 100644 --- a/.gitignore +++ b/.gitignore @@ -29,4 +29,4 @@ go.work.sum # development dirs data # artifacts -app/app +uxr diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..b5d2de6 --- /dev/null +++ b/Makefile @@ -0,0 +1,13 @@ +APP_DIR := ./app +OUTPUT := uxr +TAGS := drm drm_leasing drm_disable_input + +.PHONY: all build clean + +all: build + +build: + go build -tags '$(TAGS)' -o $(OUTPUT) $(APP_DIR) + +clean: + rm -f $(OUTPUT) diff --git a/README.md b/README.md index a87c344..785e5a7 100644 --- a/README.md +++ b/README.md @@ -28,4 +28,4 @@ If you're using Nix/NixOS, all you need to do is use `nix-shell` to enter the de ## Building -TODO. +Just run `make` in the root directory. diff --git a/app/main.go b/app/main.go index a7bf8f4..11b66ad 100644 --- a/app/main.go +++ b/app/main.go @@ -14,6 +14,8 @@ import ( "github.com/goccy/go-yaml" "github.com/kirsle/configdir" "github.com/urfave/cli/v3" + + rl "git.terah.dev/UnrealXR/raylib-go/raylib" ) func mainEntrypoint(context.Context, *cli.Command) error { @@ -100,6 +102,20 @@ func mainEntrypoint(context.Context, *cli.Command) error { log.Info("Initializing XR headset") + rl.InitWindow(800, 450, "raylib [core] example - basic window") + defer rl.CloseWindow() + + rl.SetTargetFPS(60) + + for !rl.WindowShouldClose() { + rl.BeginDrawing() + + rl.ClearBackground(rl.RayWhite) + rl.DrawText("Congrats! You created your first window!", 190, 200, 20, rl.LightGray) + + rl.EndDrawing() + } + return nil } diff --git a/go.mod b/go.mod index b09e4fc..4df2f71 100644 --- a/go.mod +++ b/go.mod @@ -3,6 +3,8 @@ module git.terah.dev/UnrealXR/unrealxr go 1.24.3 require ( + git.terah.dev/UnrealXR/raylib-go/raylib v0.55.2-0.20250623002739-1468af2636e1 // indirect + git.terah.dev/imterah/goevdi v0.0.0-20250211014657-96d98ad0735c // indirect github.com/anoopengineer/edidparser v0.0.0-20240602223913-86ca9ed3d2b0 // indirect github.com/aymanbagabas/go-osc52/v2 v2.0.1 // indirect github.com/charmbracelet/colorprofile v0.2.3-0.20250311203215-f60798e515dc // indirect @@ -11,6 +13,7 @@ require ( github.com/charmbracelet/x/ansi v0.8.0 // indirect github.com/charmbracelet/x/cellbuf v0.0.13-0.20250311204145-2c3ea96c31dd // indirect github.com/charmbracelet/x/term v0.2.1 // indirect + github.com/ebitengine/purego v0.8.4 // indirect github.com/go-logfmt/logfmt v0.6.0 // indirect github.com/goccy/go-yaml v1.18.0 // indirect github.com/google/uuid v1.6.0 // indirect @@ -22,6 +25,6 @@ require ( github.com/rivo/uniseg v0.4.7 // indirect github.com/urfave/cli/v3 v3.3.8 // indirect github.com/xo/terminfo v0.0.0-20220910002029-abceb7e1c41e // indirect - golang.org/x/exp v0.0.0-20231006140011-7918f672742d // indirect - golang.org/x/sys v0.30.0 // indirect + golang.org/x/exp v0.0.0-20250620022241-b7579e27df2b // indirect + golang.org/x/sys v0.33.0 // indirect ) diff --git a/go.sum b/go.sum index 63b1134..4040ae3 100644 --- a/go.sum +++ b/go.sum @@ -1,3 +1,7 @@ +git.terah.dev/UnrealXR/raylib-go/raylib v0.55.2-0.20250623002739-1468af2636e1 h1:cO/veKc8mon3Zlueq1sEc047B04fk/hoBJSn+btMLDI= +git.terah.dev/UnrealXR/raylib-go/raylib v0.55.2-0.20250623002739-1468af2636e1/go.mod h1:ZRirF2UuVWSbl2ux7oyHwXcinni9msejCvtIsXbT8yY= +git.terah.dev/imterah/goevdi v0.0.0-20250211014657-96d98ad0735c h1:Ys33r6kjCyCs8N3ofSzCGxxL1gLIfetIiO1uL5UIsAM= +git.terah.dev/imterah/goevdi v0.0.0-20250211014657-96d98ad0735c/go.mod h1:RmxqlNaEJ+6qbHpyw6ivJhXlRYT1RcsZ9gzNOsFXfOA= github.com/anoopengineer/edidparser v0.0.0-20240602223913-86ca9ed3d2b0 h1:rTfysyBCL7LPbq9GFpQbllvKT8vEI93lQUwksMMxHMI= github.com/anoopengineer/edidparser v0.0.0-20240602223913-86ca9ed3d2b0/go.mod h1:fEt61NePh3ZMxA+g3iC4CaGzY9lEsHRUkYJY2x0lBAw= github.com/aymanbagabas/go-osc52/v2 v2.0.1 h1:HwpRHbFMcZLEVr42D4p7XBqjyuxQH5SMiErDT4WkJ2k= @@ -14,6 +18,8 @@ github.com/charmbracelet/x/cellbuf v0.0.13-0.20250311204145-2c3ea96c31dd h1:vy0G github.com/charmbracelet/x/cellbuf v0.0.13-0.20250311204145-2c3ea96c31dd/go.mod h1:xe0nKWGd3eJgtqZRaN9RjMtK7xUYchjzPr7q6kcvCCs= github.com/charmbracelet/x/term v0.2.1 h1:AQeHeLZ1OqSXhrAWpYUtZyX1T3zVxfpZuEQMIQaGIAQ= github.com/charmbracelet/x/term v0.2.1/go.mod h1:oQ4enTYFV7QN4m0i9mzHrViD7TQKvNEEkHUMCmsxdUg= +github.com/ebitengine/purego v0.8.4 h1:CF7LEKg5FFOsASUj0+QwaXf8Ht6TlFxg09+S9wz0omw= +github.com/ebitengine/purego v0.8.4/go.mod h1:iIjxzd6CiRiOG0UyXP+V1+jWqUXVjPKLAI0mRfJZTmQ= github.com/go-logfmt/logfmt v0.6.0 h1:wGYYu3uicYdqXVgoYbvnkrPVXkuLM1p1ifugDMEdRi4= github.com/go-logfmt/logfmt v0.6.0/go.mod h1:WYhtIu8zTZfxdn5+rREduYbwxfcBr/Vr6KEVveWlfTs= github.com/goccy/go-yaml v1.18.0 h1:8W7wMFS12Pcas7KU+VVkaiCng+kG8QiFeFwzFb+rwuw= @@ -39,6 +45,10 @@ github.com/xo/terminfo v0.0.0-20220910002029-abceb7e1c41e h1:JVG44RsyaB9T2KIHavM github.com/xo/terminfo v0.0.0-20220910002029-abceb7e1c41e/go.mod h1:RbqR21r5mrJuqunuUZ/Dhy/avygyECGrLceyNeo4LiM= golang.org/x/exp v0.0.0-20231006140011-7918f672742d h1:jtJma62tbqLibJ5sFQz8bKtEM8rJBtfilJ2qTU199MI= golang.org/x/exp v0.0.0-20231006140011-7918f672742d/go.mod h1:ldy0pHrwJyGW56pPQzzkH36rKxoZW1tw7ZJpeKx+hdo= +golang.org/x/exp v0.0.0-20250620022241-b7579e27df2b h1:M2rDM6z3Fhozi9O7NWsxAkg/yqS/lQJ6PmkyIV3YP+o= +golang.org/x/exp v0.0.0-20250620022241-b7579e27df2b/go.mod h1:3//PLf8L/X+8b4vuAfHzxeRUl04Adcb341+IGKfnqS8= golang.org/x/sys v0.6.0/go.mod h1:oPkhp1MJrh7nUepCBck5+mAzfO9JrbApNNgaTdGDITg= golang.org/x/sys v0.30.0 h1:QjkSwP/36a20jFYWkSue1YwXzLmsV5Gfq7Eiy72C1uc= golang.org/x/sys v0.30.0/go.mod h1:/VUhepiaJMQUp4+oa/7Zr1D23ma6VTLIYjOOTFZPUcA= +golang.org/x/sys v0.33.0 h1:q3i8TbbEz+JRD9ywIRlyRAQbM0qF7hu24q3teo2hbuw= +golang.org/x/sys v0.33.0/go.mod h1:BJP2sWEmIv4KK5OTEluFJCKSidICx8ciO85XgH3Ak8k= diff --git a/unrealxr b/unrealxr index 764a6d9..512cfa2 100755 --- a/unrealxr +++ b/unrealxr @@ -1 +1 @@ -sudo UNREALXR_LOG_LEVEL="$UNREALXR_LOG_LEVEL" LD_LIBRARY_PATH="$LD_LIBRARY_PATH" UNREALXR_CONFIG_PATH="$UNREALXR_CONFIG_PATH" UNREALXR_DATA_PATH="$UNREALXR_DATA_PATH" WAYLAND_DISPLAY="$XDG_RUNTIME_DIR/$WAYLAND_DISPLAY" XDG_RUNTIME_DIR="/user/run/0" ./app/app +sudo UNREALXR_LOG_LEVEL="$UNREALXR_LOG_LEVEL" LD_LIBRARY_PATH="$LD_LIBRARY_PATH" UNREALXR_CONFIG_PATH="$UNREALXR_CONFIG_PATH" UNREALXR_DATA_PATH="$UNREALXR_DATA_PATH" WAYLAND_DISPLAY="$XDG_RUNTIME_DIR/$WAYLAND_DISPLAY" XDG_RUNTIME_DIR="/user/run/0" ./uxr From a9156f7ec218d817cad45d9e00f20a4a0f121882 Mon Sep 17 00:00:00 2001 From: imterah Date: Mon, 23 Jun 2025 13:17:44 -0400 Subject: [PATCH 08/12] feature: Add EVDI display initialization --- app/main.go | 74 +++++++++++++++++++++++++++++++++++++++++++---------- go.mod | 3 ++- go.sum | 10 ++++++++ 3 files changed, 73 insertions(+), 14 deletions(-) diff --git a/app/main.go b/app/main.go index 11b66ad..8a6c89c 100644 --- a/app/main.go +++ b/app/main.go @@ -5,20 +5,36 @@ import ( "context" "fmt" "os" + "os/signal" "path" + "syscall" libconfig "git.terah.dev/UnrealXR/unrealxr/app/config" "git.terah.dev/UnrealXR/unrealxr/app/edidtools" + "git.terah.dev/UnrealXR/unrealxr/app/renderer" "git.terah.dev/UnrealXR/unrealxr/edidpatcher" + "git.terah.dev/imterah/goevdi/libevdi" "github.com/charmbracelet/log" "github.com/goccy/go-yaml" "github.com/kirsle/configdir" + "github.com/tebeka/atexit" "github.com/urfave/cli/v3" rl "git.terah.dev/UnrealXR/raylib-go/raylib" ) func mainEntrypoint(context.Context, *cli.Command) error { + // Allow for clean exits + c := make(chan os.Signal, 1) + signal.Notify(c, os.Interrupt, syscall.SIGTERM) + + go func() { + <-c + log.Info("Exiting...") + atexit.Exit(1) + }() + + // TODO: add built-in privesc if os.Geteuid() != 0 { return fmt.Errorf("this program must be run as root") } @@ -63,7 +79,7 @@ func mainEntrypoint(context.Context, *cli.Command) error { } libconfig.InitializePotentiallyMissingConfigValues(config) - log.Info("Attempting to read display EDID file and fetch metadata") + log.Debug("Attempting to read display EDID file and fetch metadata") displayMetadata, err := edidtools.FetchXRGlassEDID(*config.Overrides.AllowUnsupportedDevices) @@ -71,8 +87,8 @@ func mainEntrypoint(context.Context, *cli.Command) error { return fmt.Errorf("failed to fetch EDID or get metadata: %w", err) } - log.Info("Got EDID file and metadata") - log.Info("Patching EDID firmware to be specialized") + log.Debug("Got EDID file and metadata") + log.Debug("Patching EDID firmware to be specialized") patchedFirmware, err := edidpatcher.PatchEDIDToBeSpecialized(displayMetadata.EDID) @@ -87,7 +103,7 @@ func mainEntrypoint(context.Context, *cli.Command) error { return fmt.Errorf("failed to upload patched EDID firmware: %w", err) } - defer func() { + atexit.Register(func() { err := edidtools.UnloadCustomEDIDFirmware(displayMetadata) if err != nil { @@ -95,27 +111,59 @@ func mainEntrypoint(context.Context, *cli.Command) error { } log.Info("Please unplug and plug in your XR device to restore it back to normal settings.") - }() + }) fmt.Print("Press the Enter key to continue loading after you unplug and plug in your XR device.") bufio.NewReader(os.Stdin).ReadBytes('\n') // Wait for Enter key press before continuing log.Info("Initializing XR headset") + rl.SetTargetFPS(int32(displayMetadata.MaxRefreshRate * 2)) + rl.InitWindow(int32(displayMetadata.MaxWidth), int32(displayMetadata.MaxHeight), "UnrealXR") - rl.InitWindow(800, 450, "raylib [core] example - basic window") - defer rl.CloseWindow() + atexit.Register(func() { + rl.CloseWindow() + }) - rl.SetTargetFPS(60) + log.Info("Initializing virtual displays") - for !rl.WindowShouldClose() { - rl.BeginDrawing() + libevdi.SetupLogger(&libevdi.EvdiLogger{ + Log: func(msg string) { + log.Debugf("EVDI: %s", msg) + }, + }) - rl.ClearBackground(rl.RayWhite) - rl.DrawText("Congrats! You created your first window!", 190, 200, 20, rl.LightGray) + displayMetadataBlock := make([]*renderer.EvdiDisplayMetadata, *config.DisplayConfig.Count) - rl.EndDrawing() + for currentDisplay := range *config.DisplayConfig.Count { + openedDevice, err := libevdi.Open(nil) + + if err != nil { + log.Errorf("Failed to open EVDI device: %s", err.Error()) + } + + openedDevice.Connect(displayMetadata.EDID, uint(displayMetadata.MaxWidth), uint(displayMetadata.MaxHeight), uint(displayMetadata.MaxRefreshRate)) + + atexit.Register(func() { + openedDevice.Disconnect() + }) + + displayRect := &libevdi.EvdiDisplayRect{ + X1: 0, + Y1: 0, + X2: displayMetadata.MaxWidth, + Y2: displayMetadata.MaxHeight, + } + + displayBuffer := openedDevice.CreateBuffer(displayMetadata.MaxWidth, displayMetadata.MaxHeight, 4, displayRect) + + displayMetadataBlock[currentDisplay] = &renderer.EvdiDisplayMetadata{ + EvdiNode: openedDevice, + Rect: displayRect, + Buffer: displayBuffer, + } } + atexit.Exit(0) return nil } diff --git a/go.mod b/go.mod index 4df2f71..aa4a6e5 100644 --- a/go.mod +++ b/go.mod @@ -4,7 +4,7 @@ go 1.24.3 require ( git.terah.dev/UnrealXR/raylib-go/raylib v0.55.2-0.20250623002739-1468af2636e1 // indirect - git.terah.dev/imterah/goevdi v0.0.0-20250211014657-96d98ad0735c // indirect + git.terah.dev/imterah/goevdi v1.14.10 // indirect github.com/anoopengineer/edidparser v0.0.0-20240602223913-86ca9ed3d2b0 // indirect github.com/aymanbagabas/go-osc52/v2 v2.0.1 // indirect github.com/charmbracelet/colorprofile v0.2.3-0.20250311203215-f60798e515dc // indirect @@ -23,6 +23,7 @@ require ( github.com/mattn/go-runewidth v0.0.16 // indirect github.com/muesli/termenv v0.16.0 // indirect github.com/rivo/uniseg v0.4.7 // indirect + github.com/tebeka/atexit v0.3.0 // indirect github.com/urfave/cli/v3 v3.3.8 // indirect github.com/xo/terminfo v0.0.0-20220910002029-abceb7e1c41e // indirect golang.org/x/exp v0.0.0-20250620022241-b7579e27df2b // indirect diff --git a/go.sum b/go.sum index 4040ae3..72d89f4 100644 --- a/go.sum +++ b/go.sum @@ -2,6 +2,8 @@ git.terah.dev/UnrealXR/raylib-go/raylib v0.55.2-0.20250623002739-1468af2636e1 h1 git.terah.dev/UnrealXR/raylib-go/raylib v0.55.2-0.20250623002739-1468af2636e1/go.mod h1:ZRirF2UuVWSbl2ux7oyHwXcinni9msejCvtIsXbT8yY= git.terah.dev/imterah/goevdi v0.0.0-20250211014657-96d98ad0735c h1:Ys33r6kjCyCs8N3ofSzCGxxL1gLIfetIiO1uL5UIsAM= git.terah.dev/imterah/goevdi v0.0.0-20250211014657-96d98ad0735c/go.mod h1:RmxqlNaEJ+6qbHpyw6ivJhXlRYT1RcsZ9gzNOsFXfOA= +git.terah.dev/imterah/goevdi v1.14.10 h1:3zr3E/WRkSQ//+Gcrh0wfy0LVgm4Pcglc5K4MVcZ7n8= +git.terah.dev/imterah/goevdi v1.14.10/go.mod h1:z6GnBn2qHVV5N6fKZXPgUka8H9g5mZA5BRxCzUxLaNk= github.com/anoopengineer/edidparser v0.0.0-20240602223913-86ca9ed3d2b0 h1:rTfysyBCL7LPbq9GFpQbllvKT8vEI93lQUwksMMxHMI= github.com/anoopengineer/edidparser v0.0.0-20240602223913-86ca9ed3d2b0/go.mod h1:fEt61NePh3ZMxA+g3iC4CaGzY9lEsHRUkYJY2x0lBAw= github.com/aymanbagabas/go-osc52/v2 v2.0.1 h1:HwpRHbFMcZLEVr42D4p7XBqjyuxQH5SMiErDT4WkJ2k= @@ -18,6 +20,7 @@ github.com/charmbracelet/x/cellbuf v0.0.13-0.20250311204145-2c3ea96c31dd h1:vy0G github.com/charmbracelet/x/cellbuf v0.0.13-0.20250311204145-2c3ea96c31dd/go.mod h1:xe0nKWGd3eJgtqZRaN9RjMtK7xUYchjzPr7q6kcvCCs= github.com/charmbracelet/x/term v0.2.1 h1:AQeHeLZ1OqSXhrAWpYUtZyX1T3zVxfpZuEQMIQaGIAQ= github.com/charmbracelet/x/term v0.2.1/go.mod h1:oQ4enTYFV7QN4m0i9mzHrViD7TQKvNEEkHUMCmsxdUg= +github.com/davecgh/go-spew v1.1.0/go.mod h1:J7Y8YcW2NihsgmVo/mv3lAwl/skON4iLHjSsI+c5H38= github.com/ebitengine/purego v0.8.4 h1:CF7LEKg5FFOsASUj0+QwaXf8Ht6TlFxg09+S9wz0omw= github.com/ebitengine/purego v0.8.4/go.mod h1:iIjxzd6CiRiOG0UyXP+V1+jWqUXVjPKLAI0mRfJZTmQ= github.com/go-logfmt/logfmt v0.6.0 h1:wGYYu3uicYdqXVgoYbvnkrPVXkuLM1p1ifugDMEdRi4= @@ -36,9 +39,14 @@ github.com/mattn/go-runewidth v0.0.16 h1:E5ScNMtiwvlvB5paMFdw9p4kSQzbXFikJ5SQO6T github.com/mattn/go-runewidth v0.0.16/go.mod h1:Jdepj2loyihRzMpdS35Xk/zdY8IAYHsh153qUoGf23w= github.com/muesli/termenv v0.16.0 h1:S5AlUN9dENB57rsbnkPyfdGuWIlkmzJjbFf0Tf5FWUc= github.com/muesli/termenv v0.16.0/go.mod h1:ZRfOIKPFDYQoDFF4Olj7/QJbW60Ol/kL1pU3VfY/Cnk= +github.com/pmezard/go-difflib v1.0.0/go.mod h1:iKH77koFhYxTK1pcRnkKkqfTogsbg7gZNVY4sRDYZ/4= github.com/rivo/uniseg v0.2.0/go.mod h1:J6wj4VEh+S6ZtnVlnTBMWIodfgj8LQOQFoIToxlJtxc= github.com/rivo/uniseg v0.4.7 h1:WUdvkW8uEhrYfLC4ZzdpI2ztxP1I582+49Oc5Mq64VQ= github.com/rivo/uniseg v0.4.7/go.mod h1:FN3SvrM+Zdj16jyLfmOkMNblXMcoc8DfTHruCPUcx88= +github.com/stretchr/objx v0.1.0/go.mod h1:HFkY916IF+rwdDfMAkV7OtwuqBVzrE8GR6GFx+wExME= +github.com/stretchr/testify v1.5.1/go.mod h1:5W2xD1RspED5o8YsWQXVCued0rvSQ+mT+I5cxcmMvtA= +github.com/tebeka/atexit v0.3.0 h1:jleL99H7Ywt80oJKR+VWmJNnezcCOG0CuzcN3CIpsdI= +github.com/tebeka/atexit v0.3.0/go.mod h1:WJmSUSmMT7WoR7etUOaGBVXk+f5/ZJ+67qwuedq7Fbs= github.com/urfave/cli/v3 v3.3.8 h1:BzolUExliMdet9NlJ/u4m5vHSotJ3PzEqSAZ1oPMa/E= github.com/urfave/cli/v3 v3.3.8/go.mod h1:FJSKtM/9AiiTOJL4fJ6TbMUkxBXn7GO9guZqoZtpYpo= github.com/xo/terminfo v0.0.0-20220910002029-abceb7e1c41e h1:JVG44RsyaB9T2KIHavMF/ppJZNG9ZpyihvCd0w101no= @@ -52,3 +60,5 @@ golang.org/x/sys v0.30.0 h1:QjkSwP/36a20jFYWkSue1YwXzLmsV5Gfq7Eiy72C1uc= golang.org/x/sys v0.30.0/go.mod h1:/VUhepiaJMQUp4+oa/7Zr1D23ma6VTLIYjOOTFZPUcA= golang.org/x/sys v0.33.0 h1:q3i8TbbEz+JRD9ywIRlyRAQbM0qF7hu24q3teo2hbuw= golang.org/x/sys v0.33.0/go.mod h1:BJP2sWEmIv4KK5OTEluFJCKSidICx8ciO85XgH3Ak8k= +gopkg.in/check.v1 v0.0.0-20161208181325-20d25e280405/go.mod h1:Co6ibVJAznAaIkqp8huTwlJQCZ016jof/cbN4VW5Yz0= +gopkg.in/yaml.v2 v2.2.2/go.mod h1:hI93XBmqTisBFMUTm0b8Fm+jr3Dg1NNxqwp+5A1VGuI= From b22931ffaf681babba9575f603c2a7c3605ae8f3 Mon Sep 17 00:00:00 2001 From: imterah Date: Mon, 23 Jun 2025 13:19:43 -0400 Subject: [PATCH 09/12] fix: Adds missing EvdiDisplayMetadata struct --- app/renderer/struct.go | 9 +++++++++ 1 file changed, 9 insertions(+) create mode 100644 app/renderer/struct.go diff --git a/app/renderer/struct.go b/app/renderer/struct.go new file mode 100644 index 0000000..a1dd2d0 --- /dev/null +++ b/app/renderer/struct.go @@ -0,0 +1,9 @@ +package renderer + +import "git.terah.dev/imterah/goevdi/libevdi" + +type EvdiDisplayMetadata struct { + EvdiNode *libevdi.EvdiNode + Rect *libevdi.EvdiDisplayRect + Buffer *libevdi.EvdiBuffer +} From ad3045fc290fb6762ce5980f3757cf5012208c10 Mon Sep 17 00:00:00 2001 From: imterah Date: Mon, 23 Jun 2025 13:49:53 -0400 Subject: [PATCH 10/12] chore: Add basic skeleton of the Xreal driver --- Makefile | 4 +- ardriver/commons/interface.go | 22 + ardriver/xreal/Fusion.h | 32 + ardriver/xreal/FusionAhrs.c | 510 +++++++++++ ardriver/xreal/FusionAhrs.h | 111 +++ ardriver/xreal/FusionAxes.h | 187 ++++ ardriver/xreal/FusionCalibration.h | 44 + ardriver/xreal/FusionCompass.c | 49 ++ ardriver/xreal/FusionCompass.h | 25 + ardriver/xreal/FusionConvention.h | 25 + ardriver/xreal/FusionMath.h | 481 +++++++++++ ardriver/xreal/FusionOffset.c | 77 ++ ardriver/xreal/FusionOffset.h | 40 + ardriver/xreal/README.md | 5 + ardriver/xreal/crc32.c | 102 +++ ardriver/xreal/crc32.h | 40 + ardriver/xreal/device.c | 50 ++ ardriver/xreal/device.h | 46 + ardriver/xreal/device_imu.c | 1272 ++++++++++++++++++++++++++++ ardriver/xreal/device_imu.h | 227 +++++ ardriver/xreal/device_mcu.c | 731 ++++++++++++++++ ardriver/xreal/device_mcu.h | 212 +++++ ardriver/xreal/hid_ids.c | 108 +++ ardriver/xreal/hid_ids.h | 54 ++ ardriver/xreal/xreal.go | 12 + ardriver/xreal/xreal_disabled.go | 6 + 26 files changed, 4470 insertions(+), 2 deletions(-) create mode 100644 ardriver/commons/interface.go create mode 100644 ardriver/xreal/Fusion.h create mode 100644 ardriver/xreal/FusionAhrs.c create mode 100644 ardriver/xreal/FusionAhrs.h create mode 100644 ardriver/xreal/FusionAxes.h create mode 100644 ardriver/xreal/FusionCalibration.h create mode 100644 ardriver/xreal/FusionCompass.c create mode 100644 ardriver/xreal/FusionCompass.h create mode 100644 ardriver/xreal/FusionConvention.h create mode 100644 ardriver/xreal/FusionMath.h create mode 100644 ardriver/xreal/FusionOffset.c create mode 100644 ardriver/xreal/FusionOffset.h create mode 100644 ardriver/xreal/README.md create mode 100644 ardriver/xreal/crc32.c create mode 100644 ardriver/xreal/crc32.h create mode 100644 ardriver/xreal/device.c create mode 100644 ardriver/xreal/device.h create mode 100644 ardriver/xreal/device_imu.c create mode 100644 ardriver/xreal/device_imu.h create mode 100644 ardriver/xreal/device_mcu.c create mode 100644 ardriver/xreal/device_mcu.h create mode 100644 ardriver/xreal/hid_ids.c create mode 100644 ardriver/xreal/hid_ids.h create mode 100644 ardriver/xreal/xreal.go create mode 100644 ardriver/xreal/xreal_disabled.go diff --git a/Makefile b/Makefile index b5d2de6..822f8a8 100644 --- a/Makefile +++ b/Makefile @@ -1,13 +1,13 @@ APP_DIR := ./app OUTPUT := uxr -TAGS := drm drm_leasing drm_disable_input +TAGS := xreal drm drm_leasing drm_disable_input .PHONY: all build clean all: build build: - go build -tags '$(TAGS)' -o $(OUTPUT) $(APP_DIR) + go build -v -tags '$(TAGS)' -o $(OUTPUT) $(APP_DIR) clean: rm -f $(OUTPUT) diff --git a/ardriver/commons/interface.go b/ardriver/commons/interface.go new file mode 100644 index 0000000..4a778b6 --- /dev/null +++ b/ardriver/commons/interface.go @@ -0,0 +1,22 @@ +package commons + +type AREventListener struct { + PitchCallback func(float32) + YawCallback func(float32) + RollCallback func(float32) +} + +type ARDevice interface { + // Initializes the AR device's sensors. + Initialize() error + // Ends the AR device's sensors. + End() error + // Polls the AR device's sensors. + Poll() error + // Checks if the underlying AR library is polling-based. + IsPollingLibrary() bool + // Checks if the underlying AR library is event-based. + IsEventBasedLibrary() bool + // Registers event listeners for the AR device. + RegisterEventListeners(eventListener *AREventListener) +} diff --git a/ardriver/xreal/Fusion.h b/ardriver/xreal/Fusion.h new file mode 100644 index 0000000..8d38b8b --- /dev/null +++ b/ardriver/xreal/Fusion.h @@ -0,0 +1,32 @@ +/** + * @file Fusion.h + * @author Seb Madgwick + * @brief Main header file for the Fusion library. This is the only file that + * needs to be included when using the library. + */ + +#ifndef FUSION_H +#define FUSION_H + +//------------------------------------------------------------------------------ +// Includes + +#ifdef __cplusplus +extern "C" { +#endif + +#include "FusionAhrs.h" +#include "FusionAxes.h" +#include "FusionCalibration.h" +#include "FusionCompass.h" +#include "FusionConvention.h" +#include "FusionMath.h" +#include "FusionOffset.h" + +#ifdef __cplusplus +} +#endif + +#endif +//------------------------------------------------------------------------------ +// End of file diff --git a/ardriver/xreal/FusionAhrs.c b/ardriver/xreal/FusionAhrs.c new file mode 100644 index 0000000..a86f629 --- /dev/null +++ b/ardriver/xreal/FusionAhrs.c @@ -0,0 +1,510 @@ +/** + * @file FusionAhrs.c + * @author Seb Madgwick + * @brief AHRS algorithm to combine gyroscope, accelerometer, and magnetometer + * measurements into a single measurement of orientation relative to the Earth. + */ + +//------------------------------------------------------------------------------ +// Includes + +#include +#include "FusionAhrs.h" +#include + +//------------------------------------------------------------------------------ +// Definitions + +/** + * @brief Initial gain used during the initialisation. + */ +#define INITIAL_GAIN (10.0f) + +/** + * @brief Initialisation period in seconds. + */ +#define INITIALISATION_PERIOD (3.0f) + +//------------------------------------------------------------------------------ +// Function declarations + +static inline FusionVector HalfGravity(const FusionAhrs *const ahrs); + +static inline FusionVector HalfMagnetic(const FusionAhrs *const ahrs); + +static inline FusionVector Feedback(const FusionVector sensor, const FusionVector reference); + +static inline int Clamp(const int value, const int min, const int max); + +//------------------------------------------------------------------------------ +// Functions + +/** + * @brief Initialises the AHRS algorithm structure. + * @param ahrs AHRS algorithm structure. + */ +void FusionAhrsInitialise(FusionAhrs *const ahrs) { + const FusionAhrsSettings settings = { + .convention = FusionConventionNwu, + .gain = 0.5f, + .gyroscopeRange = 0.0f, + .accelerationRejection = 90.0f, + .magneticRejection = 90.0f, + .recoveryTriggerPeriod = 0, + }; + FusionAhrsSetSettings(ahrs, &settings); + FusionAhrsReset(ahrs); +} + +/** + * @brief Resets the AHRS algorithm. This is equivalent to reinitialising the + * algorithm while maintaining the current settings. + * @param ahrs AHRS algorithm structure. + */ +void FusionAhrsReset(FusionAhrs *const ahrs) { + ahrs->quaternion = FUSION_IDENTITY_QUATERNION; + ahrs->accelerometer = FUSION_VECTOR_ZERO; + ahrs->initialising = true; + ahrs->rampedGain = INITIAL_GAIN; + ahrs->angularRateRecovery = false; + ahrs->halfAccelerometerFeedback = FUSION_VECTOR_ZERO; + ahrs->halfMagnetometerFeedback = FUSION_VECTOR_ZERO; + ahrs->accelerometerIgnored = false; + ahrs->accelerationRecoveryTrigger = 0; + ahrs->accelerationRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod; + ahrs->magnetometerIgnored = false; + ahrs->magneticRecoveryTrigger = 0; + ahrs->magneticRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod; +} + +/** + * @brief Sets the AHRS algorithm settings. + * @param ahrs AHRS algorithm structure. + * @param settings Settings. + */ +void FusionAhrsSetSettings(FusionAhrs *const ahrs, const FusionAhrsSettings *const settings) { + ahrs->settings.convention = settings->convention; + ahrs->settings.gain = settings->gain; + ahrs->settings.gyroscopeRange = settings->gyroscopeRange == 0.0f ? FLT_MAX : 0.98f * settings->gyroscopeRange; + ahrs->settings.accelerationRejection = settings->accelerationRejection == 0.0f ? FLT_MAX : powf(0.5f * sinf(FusionDegreesToRadians(settings->accelerationRejection)), 2); + ahrs->settings.magneticRejection = settings->magneticRejection == 0.0f ? FLT_MAX : powf(0.5f * sinf(FusionDegreesToRadians(settings->magneticRejection)), 2); + ahrs->settings.recoveryTriggerPeriod = settings->recoveryTriggerPeriod; + ahrs->accelerationRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod; + ahrs->magneticRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod; + if ((settings->gain == 0.0f) || (settings->recoveryTriggerPeriod == 0)) { // disable acceleration and magnetic rejection features if gain is zero + ahrs->settings.accelerationRejection = FLT_MAX; + ahrs->settings.magneticRejection = FLT_MAX; + } + if (ahrs->initialising == false) { + ahrs->rampedGain = ahrs->settings.gain; + } + ahrs->rampedGainStep = (INITIAL_GAIN - ahrs->settings.gain) / INITIALISATION_PERIOD; +} + +/** + * @brief Updates the AHRS algorithm using the gyroscope, accelerometer, and + * magnetometer measurements. + * @param ahrs AHRS algorithm structure. + * @param gyroscope Gyroscope measurement in degrees per second. + * @param accelerometer Accelerometer measurement in g. + * @param magnetometer Magnetometer measurement in arbitrary units. + * @param deltaTime Delta time in seconds. + */ +void FusionAhrsUpdate(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const FusionVector magnetometer, const float deltaTime) { +#define Q ahrs->quaternion.element + + // Store accelerometer + ahrs->accelerometer = accelerometer; + + // Reinitialise if gyroscope range exceeded + if ((fabsf(gyroscope.axis.x) > ahrs->settings.gyroscopeRange) || (fabsf(gyroscope.axis.y) > ahrs->settings.gyroscopeRange) || (fabsf(gyroscope.axis.z) > ahrs->settings.gyroscopeRange)) { + const FusionQuaternion quaternion = ahrs->quaternion; + FusionAhrsReset(ahrs); + ahrs->quaternion = quaternion; + ahrs->angularRateRecovery = true; + } + + // Ramp down gain during initialisation + if (ahrs->initialising) { + ahrs->rampedGain -= ahrs->rampedGainStep * deltaTime; + if ((ahrs->rampedGain < ahrs->settings.gain) || (ahrs->settings.gain == 0.0f)) { + ahrs->rampedGain = ahrs->settings.gain; + ahrs->initialising = false; + ahrs->angularRateRecovery = false; + } + } + + // Calculate direction of gravity indicated by algorithm + const FusionVector halfGravity = HalfGravity(ahrs); + + // Calculate accelerometer feedback + FusionVector halfAccelerometerFeedback = FUSION_VECTOR_ZERO; + ahrs->accelerometerIgnored = true; + if (FusionVectorIsZero(accelerometer) == false) { + + // Calculate accelerometer feedback scaled by 0.5 + ahrs->halfAccelerometerFeedback = Feedback(FusionVectorNormalise(accelerometer), halfGravity); + + // Don't ignore accelerometer if acceleration error below threshold + if (ahrs->initialising || ((FusionVectorMagnitudeSquared(ahrs->halfAccelerometerFeedback) <= ahrs->settings.accelerationRejection))) { + ahrs->accelerometerIgnored = false; + ahrs->accelerationRecoveryTrigger -= 9; + } else { + ahrs->accelerationRecoveryTrigger += 1; + } + + // Don't ignore accelerometer during acceleration recovery + if (ahrs->accelerationRecoveryTrigger > ahrs->accelerationRecoveryTimeout) { + ahrs->accelerationRecoveryTimeout = 0; + ahrs->accelerometerIgnored = false; + } else { + ahrs->accelerationRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod; + } + ahrs->accelerationRecoveryTrigger = Clamp(ahrs->accelerationRecoveryTrigger, 0, ahrs->settings.recoveryTriggerPeriod); + + // Apply accelerometer feedback + if (ahrs->accelerometerIgnored == false) { + halfAccelerometerFeedback = ahrs->halfAccelerometerFeedback; + } + } + + // Calculate magnetometer feedback + FusionVector halfMagnetometerFeedback = FUSION_VECTOR_ZERO; + ahrs->magnetometerIgnored = true; + if (FusionVectorIsZero(magnetometer) == false) { + + // Calculate direction of magnetic field indicated by algorithm + const FusionVector halfMagnetic = HalfMagnetic(ahrs); + + // Calculate magnetometer feedback scaled by 0.5 + ahrs->halfMagnetometerFeedback = Feedback(FusionVectorNormalise(FusionVectorCrossProduct(halfGravity, magnetometer)), halfMagnetic); + + // Don't ignore magnetometer if magnetic error below threshold + if (ahrs->initialising || ((FusionVectorMagnitudeSquared(ahrs->halfMagnetometerFeedback) <= ahrs->settings.magneticRejection))) { + ahrs->magnetometerIgnored = false; + ahrs->magneticRecoveryTrigger -= 9; + } else { + ahrs->magneticRecoveryTrigger += 1; + } + + // Don't ignore magnetometer during magnetic recovery + if (ahrs->magneticRecoveryTrigger > ahrs->magneticRecoveryTimeout) { + ahrs->magneticRecoveryTimeout = 0; + ahrs->magnetometerIgnored = false; + } else { + ahrs->magneticRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod; + } + ahrs->magneticRecoveryTrigger = Clamp(ahrs->magneticRecoveryTrigger, 0, ahrs->settings.recoveryTriggerPeriod); + + // Apply magnetometer feedback + if (ahrs->magnetometerIgnored == false) { + halfMagnetometerFeedback = ahrs->halfMagnetometerFeedback; + } + } + + // Convert gyroscope to radians per second scaled by 0.5 + const FusionVector halfGyroscope = FusionVectorMultiplyScalar(gyroscope, FusionDegreesToRadians(0.5f)); + + // Apply feedback to gyroscope + const FusionVector adjustedHalfGyroscope = FusionVectorAdd(halfGyroscope, FusionVectorMultiplyScalar(FusionVectorAdd(halfAccelerometerFeedback, halfMagnetometerFeedback), ahrs->rampedGain)); + + // Integrate rate of change of quaternion + ahrs->quaternion = FusionQuaternionAdd(ahrs->quaternion, FusionQuaternionMultiplyVector(ahrs->quaternion, FusionVectorMultiplyScalar(adjustedHalfGyroscope, deltaTime))); + + // Normalise quaternion + ahrs->quaternion = FusionQuaternionNormalise(ahrs->quaternion); +#undef Q +} + +/** + * @brief Returns the direction of gravity scaled by 0.5. + * @param ahrs AHRS algorithm structure. + * @return Direction of gravity scaled by 0.5. + */ +static inline FusionVector HalfGravity(const FusionAhrs *const ahrs) { +#define Q ahrs->quaternion.element + switch (ahrs->settings.convention) { + case FusionConventionNwu: + case FusionConventionEnu: { + const FusionVector halfGravity = {.axis = { + .x = Q.x * Q.z - Q.w * Q.y, + .y = Q.y * Q.z + Q.w * Q.x, + .z = Q.w * Q.w - 0.5f + Q.z * Q.z, + }}; // third column of transposed rotation matrix scaled by 0.5 + return halfGravity; + } + case FusionConventionNed: { + const FusionVector halfGravity = {.axis = { + .x = Q.w * Q.y - Q.x * Q.z, + .y = -1.0f * (Q.y * Q.z + Q.w * Q.x), + .z = 0.5f - Q.w * Q.w - Q.z * Q.z, + }}; // third column of transposed rotation matrix scaled by -0.5 + return halfGravity; + } + } + return FUSION_VECTOR_ZERO; // avoid compiler warning +#undef Q +} + +/** + * @brief Returns the direction of the magnetic field scaled by 0.5. + * @param ahrs AHRS algorithm structure. + * @return Direction of the magnetic field scaled by 0.5. + */ +static inline FusionVector HalfMagnetic(const FusionAhrs *const ahrs) { +#define Q ahrs->quaternion.element + switch (ahrs->settings.convention) { + case FusionConventionNwu: { + const FusionVector halfMagnetic = {.axis = { + .x = Q.x * Q.y + Q.w * Q.z, + .y = Q.w * Q.w - 0.5f + Q.y * Q.y, + .z = Q.y * Q.z - Q.w * Q.x, + }}; // second column of transposed rotation matrix scaled by 0.5 + return halfMagnetic; + } + case FusionConventionEnu: { + const FusionVector halfMagnetic = {.axis = { + .x = 0.5f - Q.w * Q.w - Q.x * Q.x, + .y = Q.w * Q.z - Q.x * Q.y, + .z = -1.0f * (Q.x * Q.z + Q.w * Q.y), + }}; // first column of transposed rotation matrix scaled by -0.5 + return halfMagnetic; + } + case FusionConventionNed: { + const FusionVector halfMagnetic = {.axis = { + .x = -1.0f * (Q.x * Q.y + Q.w * Q.z), + .y = 0.5f - Q.w * Q.w - Q.y * Q.y, + .z = Q.w * Q.x - Q.y * Q.z, + }}; // second column of transposed rotation matrix scaled by -0.5 + return halfMagnetic; + } + } + return FUSION_VECTOR_ZERO; // avoid compiler warning +#undef Q +} + +/** + * @brief Returns the feedback. + * @param sensor Sensor. + * @param reference Reference. + * @return Feedback. + */ +static inline FusionVector Feedback(const FusionVector sensor, const FusionVector reference) { + if (FusionVectorDotProduct(sensor, reference) < 0.0f) { // if error is >90 degrees + return FusionVectorNormalise(FusionVectorCrossProduct(sensor, reference)); + } + return FusionVectorCrossProduct(sensor, reference); +} + +/** + * @brief Returns a value limited to maximum and minimum. + * @param value Value. + * @param min Minimum value. + * @param max Maximum value. + * @return Value limited to maximum and minimum. + */ +static inline int Clamp(const int value, const int min, const int max) { + if (value < min) { + return min; + } + if (value > max) { + return max; + } + return value; +} + +/** + * @brief Updates the AHRS algorithm using the gyroscope and accelerometer + * measurements only. + * @param ahrs AHRS algorithm structure. + * @param gyroscope Gyroscope measurement in degrees per second. + * @param accelerometer Accelerometer measurement in g. + * @param deltaTime Delta time in seconds. + */ +void FusionAhrsUpdateNoMagnetometer(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const float deltaTime) { + + // Update AHRS algorithm + FusionAhrsUpdate(ahrs, gyroscope, accelerometer, FUSION_VECTOR_ZERO, deltaTime); + + // Zero heading during initialisation + if (ahrs->initialising) { + FusionAhrsSetHeading(ahrs, 0.0f); + } +} + +/** + * @brief Updates the AHRS algorithm using the gyroscope, accelerometer, and + * heading measurements. + * @param ahrs AHRS algorithm structure. + * @param gyroscope Gyroscope measurement in degrees per second. + * @param accelerometer Accelerometer measurement in g. + * @param heading Heading measurement in degrees. + * @param deltaTime Delta time in seconds. + */ +void FusionAhrsUpdateExternalHeading(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const float heading, const float deltaTime) { +#define Q ahrs->quaternion.element + + // Calculate roll + const float roll = atan2f(Q.w * Q.x + Q.y * Q.z, 0.5f - Q.y * Q.y - Q.x * Q.x); + + // Calculate magnetometer + const float headingRadians = FusionDegreesToRadians(heading); + const float sinHeadingRadians = sinf(headingRadians); + const FusionVector magnetometer = {.axis = { + .x = cosf(headingRadians), + .y = -1.0f * cosf(roll) * sinHeadingRadians, + .z = sinHeadingRadians * sinf(roll), + }}; + + // Update AHRS algorithm + FusionAhrsUpdate(ahrs, gyroscope, accelerometer, magnetometer, deltaTime); +#undef Q +} + +/** + * @brief Returns the quaternion describing the sensor relative to the Earth. + * @param ahrs AHRS algorithm structure. + * @return Quaternion describing the sensor relative to the Earth. + */ +FusionQuaternion FusionAhrsGetQuaternion(const FusionAhrs *const ahrs) { + return ahrs->quaternion; +} + +/** + * @brief Sets the quaternion describing the sensor relative to the Earth. + * @param ahrs AHRS algorithm structure. + * @param quaternion Quaternion describing the sensor relative to the Earth. + */ +void FusionAhrsSetQuaternion(FusionAhrs *const ahrs, const FusionQuaternion quaternion) { + ahrs->quaternion = quaternion; +} + +/** + * @brief Returns the direction of gravity in the sensor coordinate frame. + * @param ahrs AHRS algorithm structure. + * @return Direction of gravity in the sensor coordinate frame. + */ +FusionVector FusionAhrsGetGravity(const FusionAhrs *const ahrs) { +#define Q ahrs->quaternion.element + const FusionVector gravity = {.axis = { + .x = 2.0f * (Q.x * Q.z - Q.w * Q.y), + .y = 2.0f * (Q.y * Q.z + Q.w * Q.x), + .z = 2.0f * (Q.w * Q.w - 0.5f + Q.z * Q.z), + }}; // third column of transposed rotation matrix + return gravity; +#undef Q +} + +/** + * @brief Returns the linear acceleration measurement equal to the accelerometer + * measurement with gravity removed. + * @param ahrs AHRS algorithm structure. + * @return Linear acceleration measurement in g. + */ +FusionVector FusionAhrsGetLinearAcceleration(const FusionAhrs *const ahrs) { + switch (ahrs->settings.convention) { + case FusionConventionNwu: + case FusionConventionEnu: { + return FusionVectorSubtract(ahrs->accelerometer, FusionAhrsGetGravity(ahrs)); + } + case FusionConventionNed: { + return FusionVectorAdd(ahrs->accelerometer, FusionAhrsGetGravity(ahrs)); + } + } + return FUSION_VECTOR_ZERO; // avoid compiler warning +} + +/** + * @brief Returns the Earth acceleration measurement equal to accelerometer + * measurement in the Earth coordinate frame with gravity removed. + * @param ahrs AHRS algorithm structure. + * @return Earth acceleration measurement in g. + */ +FusionVector FusionAhrsGetEarthAcceleration(const FusionAhrs *const ahrs) { +#define Q ahrs->quaternion.element +#define A ahrs->accelerometer.axis + + // Calculate accelerometer measurement in the Earth coordinate frame + const float qwqw = Q.w * Q.w; // calculate common terms to avoid repeated operations + const float qwqx = Q.w * Q.x; + const float qwqy = Q.w * Q.y; + const float qwqz = Q.w * Q.z; + const float qxqy = Q.x * Q.y; + const float qxqz = Q.x * Q.z; + const float qyqz = Q.y * Q.z; + FusionVector accelerometer = {.axis = { + .x = 2.0f * ((qwqw - 0.5f + Q.x * Q.x) * A.x + (qxqy - qwqz) * A.y + (qxqz + qwqy) * A.z), + .y = 2.0f * ((qxqy + qwqz) * A.x + (qwqw - 0.5f + Q.y * Q.y) * A.y + (qyqz - qwqx) * A.z), + .z = 2.0f * ((qxqz - qwqy) * A.x + (qyqz + qwqx) * A.y + (qwqw - 0.5f + Q.z * Q.z) * A.z), + }}; // rotation matrix multiplied with the accelerometer + + // Remove gravity from accelerometer measurement + switch (ahrs->settings.convention) { + case FusionConventionNwu: + case FusionConventionEnu: + accelerometer.axis.z -= 1.0f; + break; + case FusionConventionNed: + accelerometer.axis.z += 1.0f; + break; + } + return accelerometer; +#undef Q +#undef A +} + +/** + * @brief Returns the AHRS algorithm internal states. + * @param ahrs AHRS algorithm structure. + * @return AHRS algorithm internal states. + */ +FusionAhrsInternalStates FusionAhrsGetInternalStates(const FusionAhrs *const ahrs) { + const FusionAhrsInternalStates internalStates = { + .accelerationError = FusionRadiansToDegrees(FusionAsin(2.0f * FusionVectorMagnitude(ahrs->halfAccelerometerFeedback))), + .accelerometerIgnored = ahrs->accelerometerIgnored, + .accelerationRecoveryTrigger = ahrs->settings.recoveryTriggerPeriod == 0 ? 0.0f : (float) ahrs->accelerationRecoveryTrigger / (float) ahrs->settings.recoveryTriggerPeriod, + .magneticError = FusionRadiansToDegrees(FusionAsin(2.0f * FusionVectorMagnitude(ahrs->halfMagnetometerFeedback))), + .magnetometerIgnored = ahrs->magnetometerIgnored, + .magneticRecoveryTrigger = ahrs->settings.recoveryTriggerPeriod == 0 ? 0.0f : (float) ahrs->magneticRecoveryTrigger / (float) ahrs->settings.recoveryTriggerPeriod, + }; + return internalStates; +} + +/** + * @brief Returns the AHRS algorithm flags. + * @param ahrs AHRS algorithm structure. + * @return AHRS algorithm flags. + */ +FusionAhrsFlags FusionAhrsGetFlags(const FusionAhrs *const ahrs) { + const FusionAhrsFlags flags = { + .initialising = ahrs->initialising, + .angularRateRecovery = ahrs->angularRateRecovery, + .accelerationRecovery = ahrs->accelerationRecoveryTrigger > ahrs->accelerationRecoveryTimeout, + .magneticRecovery= ahrs->magneticRecoveryTrigger > ahrs->magneticRecoveryTimeout, + }; + return flags; +} + +/** + * @brief Sets the heading of the orientation measurement provided by the AHRS + * algorithm. This function can be used to reset drift in heading when the AHRS + * algorithm is being used without a magnetometer. + * @param ahrs AHRS algorithm structure. + * @param heading Heading angle in degrees. + */ +void FusionAhrsSetHeading(FusionAhrs *const ahrs, const float heading) { +#define Q ahrs->quaternion.element + const float yaw = atan2f(Q.w * Q.z + Q.x * Q.y, 0.5f - Q.y * Q.y - Q.z * Q.z); + const float halfYawMinusHeading = 0.5f * (yaw - FusionDegreesToRadians(heading)); + const FusionQuaternion rotation = {.element = { + .w = cosf(halfYawMinusHeading), + .x = 0.0f, + .y = 0.0f, + .z = -1.0f * sinf(halfYawMinusHeading), + }}; + ahrs->quaternion = FusionQuaternionMultiply(rotation, ahrs->quaternion); +#undef Q +} + +//------------------------------------------------------------------------------ +// End of file diff --git a/ardriver/xreal/FusionAhrs.h b/ardriver/xreal/FusionAhrs.h new file mode 100644 index 0000000..fca2476 --- /dev/null +++ b/ardriver/xreal/FusionAhrs.h @@ -0,0 +1,111 @@ +/** + * @file FusionAhrs.h + * @author Seb Madgwick + * @brief AHRS algorithm to combine gyroscope, accelerometer, and magnetometer + * measurements into a single measurement of orientation relative to the Earth. + */ + +#ifndef FUSION_AHRS_H +#define FUSION_AHRS_H + +//------------------------------------------------------------------------------ +// Includes + +#include "FusionConvention.h" +#include "FusionMath.h" +#include + +//------------------------------------------------------------------------------ +// Definitions + +/** + * @brief AHRS algorithm settings. + */ +typedef struct { + FusionConvention convention; + float gain; + float gyroscopeRange; + float accelerationRejection; + float magneticRejection; + unsigned int recoveryTriggerPeriod; +} FusionAhrsSettings; + +/** + * @brief AHRS algorithm structure. Structure members are used internally and + * must not be accessed by the application. + */ +typedef struct { + FusionAhrsSettings settings; + FusionQuaternion quaternion; + FusionVector accelerometer; + bool initialising; + float rampedGain; + float rampedGainStep; + bool angularRateRecovery; + FusionVector halfAccelerometerFeedback; + FusionVector halfMagnetometerFeedback; + bool accelerometerIgnored; + int accelerationRecoveryTrigger; + int accelerationRecoveryTimeout; + bool magnetometerIgnored; + int magneticRecoveryTrigger; + int magneticRecoveryTimeout; +} FusionAhrs; + +/** + * @brief AHRS algorithm internal states. + */ +typedef struct { + float accelerationError; + bool accelerometerIgnored; + float accelerationRecoveryTrigger; + float magneticError; + bool magnetometerIgnored; + float magneticRecoveryTrigger; +} FusionAhrsInternalStates; + +/** + * @brief AHRS algorithm flags. + */ +typedef struct { + bool initialising; + bool angularRateRecovery; + bool accelerationRecovery; + bool magneticRecovery; +} FusionAhrsFlags; + +//------------------------------------------------------------------------------ +// Function declarations + +void FusionAhrsInitialise(FusionAhrs *const ahrs); + +void FusionAhrsReset(FusionAhrs *const ahrs); + +void FusionAhrsSetSettings(FusionAhrs *const ahrs, const FusionAhrsSettings *const settings); + +void FusionAhrsUpdate(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const FusionVector magnetometer, const float deltaTime); + +void FusionAhrsUpdateNoMagnetometer(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const float deltaTime); + +void FusionAhrsUpdateExternalHeading(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const float heading, const float deltaTime); + +FusionQuaternion FusionAhrsGetQuaternion(const FusionAhrs *const ahrs); + +void FusionAhrsSetQuaternion(FusionAhrs *const ahrs, const FusionQuaternion quaternion); + +FusionVector FusionAhrsGetGravity(const FusionAhrs *const ahrs); + +FusionVector FusionAhrsGetLinearAcceleration(const FusionAhrs *const ahrs); + +FusionVector FusionAhrsGetEarthAcceleration(const FusionAhrs *const ahrs); + +FusionAhrsInternalStates FusionAhrsGetInternalStates(const FusionAhrs *const ahrs); + +FusionAhrsFlags FusionAhrsGetFlags(const FusionAhrs *const ahrs); + +void FusionAhrsSetHeading(FusionAhrs *const ahrs, const float heading); + +#endif + +//------------------------------------------------------------------------------ +// End of file diff --git a/ardriver/xreal/FusionAxes.h b/ardriver/xreal/FusionAxes.h new file mode 100644 index 0000000..32c07db --- /dev/null +++ b/ardriver/xreal/FusionAxes.h @@ -0,0 +1,187 @@ +/** + * @file FusionAxes.h + * @author Seb Madgwick + * @brief Swaps sensor axes for alignment with the body axes. + */ + +#ifndef FUSION_AXES_H +#define FUSION_AXES_H + +//------------------------------------------------------------------------------ +// Includes + +#include "FusionMath.h" + +//------------------------------------------------------------------------------ +// Definitions + +/** + * @brief Axes alignment describing the sensor axes relative to the body axes. + * For example, if the body X axis is aligned with the sensor Y axis and the + * body Y axis is aligned with sensor X axis but pointing the opposite direction + * then alignment is +Y-X+Z. + */ +typedef enum { + FusionAxesAlignmentPXPYPZ, /* +X+Y+Z */ + FusionAxesAlignmentPXNZPY, /* +X-Z+Y */ + FusionAxesAlignmentPXNYNZ, /* +X-Y-Z */ + FusionAxesAlignmentPXPZNY, /* +X+Z-Y */ + FusionAxesAlignmentNXPYNZ, /* -X+Y-Z */ + FusionAxesAlignmentNXPZPY, /* -X+Z+Y */ + FusionAxesAlignmentNXNYPZ, /* -X-Y+Z */ + FusionAxesAlignmentNXNZNY, /* -X-Z-Y */ + FusionAxesAlignmentPYNXPZ, /* +Y-X+Z */ + FusionAxesAlignmentPYNZNX, /* +Y-Z-X */ + FusionAxesAlignmentPYPXNZ, /* +Y+X-Z */ + FusionAxesAlignmentPYPZPX, /* +Y+Z+X */ + FusionAxesAlignmentNYPXPZ, /* -Y+X+Z */ + FusionAxesAlignmentNYNZPX, /* -Y-Z+X */ + FusionAxesAlignmentNYNXNZ, /* -Y-X-Z */ + FusionAxesAlignmentNYPZNX, /* -Y+Z-X */ + FusionAxesAlignmentPZPYNX, /* +Z+Y-X */ + FusionAxesAlignmentPZPXPY, /* +Z+X+Y */ + FusionAxesAlignmentPZNYPX, /* +Z-Y+X */ + FusionAxesAlignmentPZNXNY, /* +Z-X-Y */ + FusionAxesAlignmentNZPYPX, /* -Z+Y+X */ + FusionAxesAlignmentNZNXPY, /* -Z-X+Y */ + FusionAxesAlignmentNZNYNX, /* -Z-Y-X */ + FusionAxesAlignmentNZPXNY, /* -Z+X-Y */ +} FusionAxesAlignment; + +//------------------------------------------------------------------------------ +// Inline functions + +/** + * @brief Swaps sensor axes for alignment with the body axes. + * @param sensor Sensor axes. + * @param alignment Axes alignment. + * @return Sensor axes aligned with the body axes. + */ +static inline FusionVector FusionAxesSwap(const FusionVector sensor, const FusionAxesAlignment alignment) { + FusionVector result; + switch (alignment) { + case FusionAxesAlignmentPXPYPZ: + break; + case FusionAxesAlignmentPXNZPY: + result.axis.x = +sensor.axis.x; + result.axis.y = -sensor.axis.z; + result.axis.z = +sensor.axis.y; + return result; + case FusionAxesAlignmentPXNYNZ: + result.axis.x = +sensor.axis.x; + result.axis.y = -sensor.axis.y; + result.axis.z = -sensor.axis.z; + return result; + case FusionAxesAlignmentPXPZNY: + result.axis.x = +sensor.axis.x; + result.axis.y = +sensor.axis.z; + result.axis.z = -sensor.axis.y; + return result; + case FusionAxesAlignmentNXPYNZ: + result.axis.x = -sensor.axis.x; + result.axis.y = +sensor.axis.y; + result.axis.z = -sensor.axis.z; + return result; + case FusionAxesAlignmentNXPZPY: + result.axis.x = -sensor.axis.x; + result.axis.y = +sensor.axis.z; + result.axis.z = +sensor.axis.y; + return result; + case FusionAxesAlignmentNXNYPZ: + result.axis.x = -sensor.axis.x; + result.axis.y = -sensor.axis.y; + result.axis.z = +sensor.axis.z; + return result; + case FusionAxesAlignmentNXNZNY: + result.axis.x = -sensor.axis.x; + result.axis.y = -sensor.axis.z; + result.axis.z = -sensor.axis.y; + return result; + case FusionAxesAlignmentPYNXPZ: + result.axis.x = +sensor.axis.y; + result.axis.y = -sensor.axis.x; + result.axis.z = +sensor.axis.z; + return result; + case FusionAxesAlignmentPYNZNX: + result.axis.x = +sensor.axis.y; + result.axis.y = -sensor.axis.z; + result.axis.z = -sensor.axis.x; + return result; + case FusionAxesAlignmentPYPXNZ: + result.axis.x = +sensor.axis.y; + result.axis.y = +sensor.axis.x; + result.axis.z = -sensor.axis.z; + return result; + case FusionAxesAlignmentPYPZPX: + result.axis.x = +sensor.axis.y; + result.axis.y = +sensor.axis.z; + result.axis.z = +sensor.axis.x; + return result; + case FusionAxesAlignmentNYPXPZ: + result.axis.x = -sensor.axis.y; + result.axis.y = +sensor.axis.x; + result.axis.z = +sensor.axis.z; + return result; + case FusionAxesAlignmentNYNZPX: + result.axis.x = -sensor.axis.y; + result.axis.y = -sensor.axis.z; + result.axis.z = +sensor.axis.x; + return result; + case FusionAxesAlignmentNYNXNZ: + result.axis.x = -sensor.axis.y; + result.axis.y = -sensor.axis.x; + result.axis.z = -sensor.axis.z; + return result; + case FusionAxesAlignmentNYPZNX: + result.axis.x = -sensor.axis.y; + result.axis.y = +sensor.axis.z; + result.axis.z = -sensor.axis.x; + return result; + case FusionAxesAlignmentPZPYNX: + result.axis.x = +sensor.axis.z; + result.axis.y = +sensor.axis.y; + result.axis.z = -sensor.axis.x; + return result; + case FusionAxesAlignmentPZPXPY: + result.axis.x = +sensor.axis.z; + result.axis.y = +sensor.axis.x; + result.axis.z = +sensor.axis.y; + return result; + case FusionAxesAlignmentPZNYPX: + result.axis.x = +sensor.axis.z; + result.axis.y = -sensor.axis.y; + result.axis.z = +sensor.axis.x; + return result; + case FusionAxesAlignmentPZNXNY: + result.axis.x = +sensor.axis.z; + result.axis.y = -sensor.axis.x; + result.axis.z = -sensor.axis.y; + return result; + case FusionAxesAlignmentNZPYPX: + result.axis.x = -sensor.axis.z; + result.axis.y = +sensor.axis.y; + result.axis.z = +sensor.axis.x; + return result; + case FusionAxesAlignmentNZNXPY: + result.axis.x = -sensor.axis.z; + result.axis.y = -sensor.axis.x; + result.axis.z = +sensor.axis.y; + return result; + case FusionAxesAlignmentNZNYNX: + result.axis.x = -sensor.axis.z; + result.axis.y = -sensor.axis.y; + result.axis.z = -sensor.axis.x; + return result; + case FusionAxesAlignmentNZPXNY: + result.axis.x = -sensor.axis.z; + result.axis.y = +sensor.axis.x; + result.axis.z = -sensor.axis.y; + return result; + } + return sensor; // avoid compiler warning +} + +#endif + +//------------------------------------------------------------------------------ +// End of file diff --git a/ardriver/xreal/FusionCalibration.h b/ardriver/xreal/FusionCalibration.h new file mode 100644 index 0000000..733f572 --- /dev/null +++ b/ardriver/xreal/FusionCalibration.h @@ -0,0 +1,44 @@ +/** + * @file FusionCalibration.h + * @author Seb Madgwick + * @brief Gyroscope, accelerometer, and magnetometer calibration models. + */ + +#ifndef FUSION_CALIBRATION_H +#define FUSION_CALIBRATION_H + +//------------------------------------------------------------------------------ +// Includes + +#include "FusionMath.h" + +//------------------------------------------------------------------------------ +// Inline functions + +/** + * @brief Gyroscope and accelerometer calibration model. + * @param uncalibrated Uncalibrated measurement. + * @param misalignment Misalignment matrix. + * @param sensitivity Sensitivity. + * @param offset Offset. + * @return Calibrated measurement. + */ +static inline FusionVector FusionCalibrationInertial(const FusionVector uncalibrated, const FusionMatrix misalignment, const FusionVector sensitivity, const FusionVector offset) { + return FusionMatrixMultiplyVector(misalignment, FusionVectorHadamardProduct(FusionVectorSubtract(uncalibrated, offset), sensitivity)); +} + +/** + * @brief Magnetometer calibration model. + * @param uncalibrated Uncalibrated measurement. + * @param softIronMatrix Soft-iron matrix. + * @param hardIronOffset Hard-iron offset. + * @return Calibrated measurement. + */ +static inline FusionVector FusionCalibrationMagnetic(const FusionVector uncalibrated, const FusionMatrix softIronMatrix, const FusionVector hardIronOffset) { + return FusionMatrixMultiplyVector(softIronMatrix, FusionVectorSubtract(uncalibrated, hardIronOffset)); +} + +#endif + +//------------------------------------------------------------------------------ +// End of file diff --git a/ardriver/xreal/FusionCompass.c b/ardriver/xreal/FusionCompass.c new file mode 100644 index 0000000..e34617c --- /dev/null +++ b/ardriver/xreal/FusionCompass.c @@ -0,0 +1,49 @@ +/** + * @file FusionCompass.c + * @author Seb Madgwick + * @brief Tilt-compensated compass to calculate the magnetic heading using + * accelerometer and magnetometer measurements. + */ + +//------------------------------------------------------------------------------ +// Includes + +#include "FusionAxes.h" +#include "FusionCompass.h" +#include + +//------------------------------------------------------------------------------ +// Functions + +/** + * @brief Calculates the magnetic heading. + * @param convention Earth axes convention. + * @param accelerometer Accelerometer measurement in any calibrated units. + * @param magnetometer Magnetometer measurement in any calibrated units. + * @return Heading angle in degrees. + */ +float FusionCompassCalculateHeading(const FusionConvention convention, const FusionVector accelerometer, const FusionVector magnetometer) { + switch (convention) { + case FusionConventionNwu: { + const FusionVector west = FusionVectorNormalise(FusionVectorCrossProduct(accelerometer, magnetometer)); + const FusionVector north = FusionVectorNormalise(FusionVectorCrossProduct(west, accelerometer)); + return FusionRadiansToDegrees(atan2f(west.axis.x, north.axis.x)); + } + case FusionConventionEnu: { + const FusionVector west = FusionVectorNormalise(FusionVectorCrossProduct(accelerometer, magnetometer)); + const FusionVector north = FusionVectorNormalise(FusionVectorCrossProduct(west, accelerometer)); + const FusionVector east = FusionVectorMultiplyScalar(west, -1.0f); + return FusionRadiansToDegrees(atan2f(north.axis.x, east.axis.x)); + } + case FusionConventionNed: { + const FusionVector up = FusionVectorMultiplyScalar(accelerometer, -1.0f); + const FusionVector west = FusionVectorNormalise(FusionVectorCrossProduct(up, magnetometer)); + const FusionVector north = FusionVectorNormalise(FusionVectorCrossProduct(west, up)); + return FusionRadiansToDegrees(atan2f(west.axis.x, north.axis.x)); + } + } + return 0; // avoid compiler warning +} + +//------------------------------------------------------------------------------ +// End of file diff --git a/ardriver/xreal/FusionCompass.h b/ardriver/xreal/FusionCompass.h new file mode 100644 index 0000000..78326c0 --- /dev/null +++ b/ardriver/xreal/FusionCompass.h @@ -0,0 +1,25 @@ +/** + * @file FusionCompass.h + * @author Seb Madgwick + * @brief Tilt-compensated compass to calculate the magnetic heading using + * accelerometer and magnetometer measurements. + */ + +#ifndef FUSION_COMPASS_H +#define FUSION_COMPASS_H + +//------------------------------------------------------------------------------ +// Includes + +#include "FusionConvention.h" +#include "FusionMath.h" + +//------------------------------------------------------------------------------ +// Function declarations + +float FusionCompassCalculateHeading(const FusionConvention convention, const FusionVector accelerometer, const FusionVector magnetometer); + +#endif + +//------------------------------------------------------------------------------ +// End of file diff --git a/ardriver/xreal/FusionConvention.h b/ardriver/xreal/FusionConvention.h new file mode 100644 index 0000000..0b0d43a --- /dev/null +++ b/ardriver/xreal/FusionConvention.h @@ -0,0 +1,25 @@ +/** + * @file FusionConvention.h + * @author Seb Madgwick + * @brief Earth axes convention. + */ + +#ifndef FUSION_CONVENTION_H +#define FUSION_CONVENTION_H + +//------------------------------------------------------------------------------ +// Definitions + +/** + * @brief Earth axes convention. + */ +typedef enum { + FusionConventionNwu, /* North-West-Up */ + FusionConventionEnu, /* East-North-Up */ + FusionConventionNed, /* North-East-Down */ +} FusionConvention; + +#endif + +//------------------------------------------------------------------------------ +// End of file diff --git a/ardriver/xreal/FusionMath.h b/ardriver/xreal/FusionMath.h new file mode 100644 index 0000000..5ee92b0 --- /dev/null +++ b/ardriver/xreal/FusionMath.h @@ -0,0 +1,481 @@ +/** + * @file FusionMath.h + * @author Seb Madgwick + * @brief Math library. + */ + +#ifndef FUSION_MATH_H +#define FUSION_MATH_H + +//------------------------------------------------------------------------------ +// Includes + +#include +#include +#include + +//------------------------------------------------------------------------------ +// Definitions + +/** + * @brief 3D vector. + */ +typedef union { + float array[3]; + + struct { + float x; + float y; + float z; + } axis; +} FusionVector; + +/** + * @brief Quaternion. + */ +typedef union { + float array[4]; + + struct { + float w; + float x; + float y; + float z; + } element; +} FusionQuaternion; + +/** + * @brief 3x3 matrix in row-major order. + * See http://en.wikipedia.org/wiki/Row-major_order + */ +typedef union { + float array[3][3]; + + struct { + float xx; + float xy; + float xz; + float yx; + float yy; + float yz; + float zx; + float zy; + float zz; + } element; +} FusionMatrix; + +/** + * @brief Euler angles. Roll, pitch, and yaw correspond to rotations around + * X, Y, and Z respectively. + */ +typedef union { + float array[3]; + + struct { + float roll; + float pitch; + float yaw; + } angle; +} FusionEuler; + +/** + * @brief Vector of zeros. + */ +#define FUSION_VECTOR_ZERO ((FusionVector){ .array = {0.0f, 0.0f, 0.0f} }) + +/** + * @brief Vector of ones. + */ +#define FUSION_VECTOR_ONES ((FusionVector){ .array = {1.0f, 1.0f, 1.0f} }) + +/** + * @brief Identity quaternion. + */ +#define FUSION_IDENTITY_QUATERNION ((FusionQuaternion){ .array = {1.0f, 0.0f, 0.0f, 0.0f} }) + +/** + * @brief Identity matrix. + */ +#define FUSION_IDENTITY_MATRIX ((FusionMatrix){ .array = {{1.0f, 0.0f, 0.0f}, {0.0f, 1.0f, 0.0f}, {0.0f, 0.0f, 1.0f}} }) + +/** + * @brief Euler angles of zero. + */ +#define FUSION_EULER_ZERO ((FusionEuler){ .array = {0.0f, 0.0f, 0.0f} }) + +/** + * @brief Pi. May not be defined in math.h. + */ +#ifndef M_PI +#define M_PI (3.14159265358979323846) +#endif + +/** + * @brief Include this definition or add as a preprocessor definition to use + * normal square root operations. + */ +//#define FUSION_USE_NORMAL_SQRT + +//------------------------------------------------------------------------------ +// Inline functions - Degrees and radians conversion + +/** + * @brief Converts degrees to radians. + * @param degrees Degrees. + * @return Radians. + */ +static inline float FusionDegreesToRadians(const float degrees) { + return degrees * ((float) M_PI / 180.0f); +} + +/** + * @brief Converts radians to degrees. + * @param radians Radians. + * @return Degrees. + */ +static inline float FusionRadiansToDegrees(const float radians) { + return radians * (180.0f / (float) M_PI); +} + +//------------------------------------------------------------------------------ +// Inline functions - Arc sine + +/** + * @brief Returns the arc sine of the value. + * @param value Value. + * @return Arc sine of the value. + */ +static inline float FusionAsin(const float value) { + if (value <= -1.0f) { + return (float) M_PI / -2.0f; + } + if (value >= 1.0f) { + return (float) M_PI / 2.0f; + } + return asinf(value); +} + +//------------------------------------------------------------------------------ +// Inline functions - Fast inverse square root + +#ifndef FUSION_USE_NORMAL_SQRT + +/** + * @brief Calculates the reciprocal of the square root. + * See https://pizer.wordpress.com/2008/10/12/fast-inverse-square-root/ + * @param x Operand. + * @return Reciprocal of the square root of x. + */ +static inline float FusionFastInverseSqrt(const float x) { + + typedef union { + float f; + int32_t i; + } Union32; + + Union32 union32 = {.f = x}; + union32.i = 0x5F1F1412 - (union32.i >> 1); + return union32.f * (1.69000231f - 0.714158168f * x * union32.f * union32.f); +} + +#endif + +//------------------------------------------------------------------------------ +// Inline functions - Vector operations + +/** + * @brief Returns true if the vector is zero. + * @param vector Vector. + * @return True if the vector is zero. + */ +static inline bool FusionVectorIsZero(const FusionVector vector) { + return (vector.axis.x == 0.0f) && (vector.axis.y == 0.0f) && (vector.axis.z == 0.0f); +} + +/** + * @brief Returns the sum of two vectors. + * @param vectorA Vector A. + * @param vectorB Vector B. + * @return Sum of two vectors. + */ +static inline FusionVector FusionVectorAdd(const FusionVector vectorA, const FusionVector vectorB) { + const FusionVector result = {.axis = { + .x = vectorA.axis.x + vectorB.axis.x, + .y = vectorA.axis.y + vectorB.axis.y, + .z = vectorA.axis.z + vectorB.axis.z, + }}; + return result; +} + +/** + * @brief Returns vector B subtracted from vector A. + * @param vectorA Vector A. + * @param vectorB Vector B. + * @return Vector B subtracted from vector A. + */ +static inline FusionVector FusionVectorSubtract(const FusionVector vectorA, const FusionVector vectorB) { + const FusionVector result = {.axis = { + .x = vectorA.axis.x - vectorB.axis.x, + .y = vectorA.axis.y - vectorB.axis.y, + .z = vectorA.axis.z - vectorB.axis.z, + }}; + return result; +} + +/** + * @brief Returns the sum of the elements. + * @param vector Vector. + * @return Sum of the elements. + */ +static inline float FusionVectorSum(const FusionVector vector) { + return vector.axis.x + vector.axis.y + vector.axis.z; +} + +/** + * @brief Returns the multiplication of a vector by a scalar. + * @param vector Vector. + * @param scalar Scalar. + * @return Multiplication of a vector by a scalar. + */ +static inline FusionVector FusionVectorMultiplyScalar(const FusionVector vector, const float scalar) { + const FusionVector result = {.axis = { + .x = vector.axis.x * scalar, + .y = vector.axis.y * scalar, + .z = vector.axis.z * scalar, + }}; + return result; +} + +/** + * @brief Calculates the Hadamard product (element-wise multiplication). + * @param vectorA Vector A. + * @param vectorB Vector B. + * @return Hadamard product. + */ +static inline FusionVector FusionVectorHadamardProduct(const FusionVector vectorA, const FusionVector vectorB) { + const FusionVector result = {.axis = { + .x = vectorA.axis.x * vectorB.axis.x, + .y = vectorA.axis.y * vectorB.axis.y, + .z = vectorA.axis.z * vectorB.axis.z, + }}; + return result; +} + +/** + * @brief Returns the cross product. + * @param vectorA Vector A. + * @param vectorB Vector B. + * @return Cross product. + */ +static inline FusionVector FusionVectorCrossProduct(const FusionVector vectorA, const FusionVector vectorB) { +#define A vectorA.axis +#define B vectorB.axis + const FusionVector result = {.axis = { + .x = A.y * B.z - A.z * B.y, + .y = A.z * B.x - A.x * B.z, + .z = A.x * B.y - A.y * B.x, + }}; + return result; +#undef A +#undef B +} + +/** + * @brief Returns the dot product. + * @param vectorA Vector A. + * @param vectorB Vector B. + * @return Dot product. + */ +static inline float FusionVectorDotProduct(const FusionVector vectorA, const FusionVector vectorB) { + return FusionVectorSum(FusionVectorHadamardProduct(vectorA, vectorB)); +} + +/** + * @brief Returns the vector magnitude squared. + * @param vector Vector. + * @return Vector magnitude squared. + */ +static inline float FusionVectorMagnitudeSquared(const FusionVector vector) { + return FusionVectorSum(FusionVectorHadamardProduct(vector, vector)); +} + +/** + * @brief Returns the vector magnitude. + * @param vector Vector. + * @return Vector magnitude. + */ +static inline float FusionVectorMagnitude(const FusionVector vector) { + return sqrtf(FusionVectorMagnitudeSquared(vector)); +} + +/** + * @brief Returns the normalised vector. + * @param vector Vector. + * @return Normalised vector. + */ +static inline FusionVector FusionVectorNormalise(const FusionVector vector) { +#ifdef FUSION_USE_NORMAL_SQRT + const float magnitudeReciprocal = 1.0f / sqrtf(FusionVectorMagnitudeSquared(vector)); +#else + const float magnitudeReciprocal = FusionFastInverseSqrt(FusionVectorMagnitudeSquared(vector)); +#endif + return FusionVectorMultiplyScalar(vector, magnitudeReciprocal); +} + +//------------------------------------------------------------------------------ +// Inline functions - Quaternion operations + +/** + * @brief Returns the sum of two quaternions. + * @param quaternionA Quaternion A. + * @param quaternionB Quaternion B. + * @return Sum of two quaternions. + */ +static inline FusionQuaternion FusionQuaternionAdd(const FusionQuaternion quaternionA, const FusionQuaternion quaternionB) { + const FusionQuaternion result = {.element = { + .w = quaternionA.element.w + quaternionB.element.w, + .x = quaternionA.element.x + quaternionB.element.x, + .y = quaternionA.element.y + quaternionB.element.y, + .z = quaternionA.element.z + quaternionB.element.z, + }}; + return result; +} + +/** + * @brief Returns the multiplication of two quaternions. + * @param quaternionA Quaternion A (to be post-multiplied). + * @param quaternionB Quaternion B (to be pre-multiplied). + * @return Multiplication of two quaternions. + */ +static inline FusionQuaternion FusionQuaternionMultiply(const FusionQuaternion quaternionA, const FusionQuaternion quaternionB) { +#define A quaternionA.element +#define B quaternionB.element + const FusionQuaternion result = {.element = { + .w = A.w * B.w - A.x * B.x - A.y * B.y - A.z * B.z, + .x = A.w * B.x + A.x * B.w + A.y * B.z - A.z * B.y, + .y = A.w * B.y - A.x * B.z + A.y * B.w + A.z * B.x, + .z = A.w * B.z + A.x * B.y - A.y * B.x + A.z * B.w, + }}; + return result; +#undef A +#undef B +} + +/** + * @brief Returns the multiplication of a quaternion with a vector. This is a + * normal quaternion multiplication where the vector is treated a + * quaternion with a W element value of zero. The quaternion is post- + * multiplied by the vector. + * @param quaternion Quaternion. + * @param vector Vector. + * @return Multiplication of a quaternion with a vector. + */ +static inline FusionQuaternion FusionQuaternionMultiplyVector(const FusionQuaternion quaternion, const FusionVector vector) { +#define Q quaternion.element +#define V vector.axis + const FusionQuaternion result = {.element = { + .w = -Q.x * V.x - Q.y * V.y - Q.z * V.z, + .x = Q.w * V.x + Q.y * V.z - Q.z * V.y, + .y = Q.w * V.y - Q.x * V.z + Q.z * V.x, + .z = Q.w * V.z + Q.x * V.y - Q.y * V.x, + }}; + return result; +#undef Q +#undef V +} + +/** + * @brief Returns the normalised quaternion. + * @param quaternion Quaternion. + * @return Normalised quaternion. + */ +static inline FusionQuaternion FusionQuaternionNormalise(const FusionQuaternion quaternion) { +#define Q quaternion.element +#ifdef FUSION_USE_NORMAL_SQRT + const float magnitudeReciprocal = 1.0f / sqrtf(Q.w * Q.w + Q.x * Q.x + Q.y * Q.y + Q.z * Q.z); +#else + const float magnitudeReciprocal = FusionFastInverseSqrt(Q.w * Q.w + Q.x * Q.x + Q.y * Q.y + Q.z * Q.z); +#endif + const FusionQuaternion result = {.element = { + .w = Q.w * magnitudeReciprocal, + .x = Q.x * magnitudeReciprocal, + .y = Q.y * magnitudeReciprocal, + .z = Q.z * magnitudeReciprocal, + }}; + return result; +#undef Q +} + +//------------------------------------------------------------------------------ +// Inline functions - Matrix operations + +/** + * @brief Returns the multiplication of a matrix with a vector. + * @param matrix Matrix. + * @param vector Vector. + * @return Multiplication of a matrix with a vector. + */ +static inline FusionVector FusionMatrixMultiplyVector(const FusionMatrix matrix, const FusionVector vector) { +#define R matrix.element + const FusionVector result = {.axis = { + .x = R.xx * vector.axis.x + R.xy * vector.axis.y + R.xz * vector.axis.z, + .y = R.yx * vector.axis.x + R.yy * vector.axis.y + R.yz * vector.axis.z, + .z = R.zx * vector.axis.x + R.zy * vector.axis.y + R.zz * vector.axis.z, + }}; + return result; +#undef R +} + +//------------------------------------------------------------------------------ +// Inline functions - Conversion operations + +/** + * @brief Converts a quaternion to a rotation matrix. + * @param quaternion Quaternion. + * @return Rotation matrix. + */ +static inline FusionMatrix FusionQuaternionToMatrix(const FusionQuaternion quaternion) { +#define Q quaternion.element + const float qwqw = Q.w * Q.w; // calculate common terms to avoid repeated operations + const float qwqx = Q.w * Q.x; + const float qwqy = Q.w * Q.y; + const float qwqz = Q.w * Q.z; + const float qxqy = Q.x * Q.y; + const float qxqz = Q.x * Q.z; + const float qyqz = Q.y * Q.z; + const FusionMatrix matrix = {.element = { + .xx = 2.0f * (qwqw - 0.5f + Q.x * Q.x), + .xy = 2.0f * (qxqy - qwqz), + .xz = 2.0f * (qxqz + qwqy), + .yx = 2.0f * (qxqy + qwqz), + .yy = 2.0f * (qwqw - 0.5f + Q.y * Q.y), + .yz = 2.0f * (qyqz - qwqx), + .zx = 2.0f * (qxqz - qwqy), + .zy = 2.0f * (qyqz + qwqx), + .zz = 2.0f * (qwqw - 0.5f + Q.z * Q.z), + }}; + return matrix; +#undef Q +} + +/** + * @brief Converts a quaternion to ZYX Euler angles in degrees. + * @param quaternion Quaternion. + * @return Euler angles in degrees. + */ +static inline FusionEuler FusionQuaternionToEuler(const FusionQuaternion quaternion) { +#define Q quaternion.element + const float halfMinusQySquared = 0.5f - Q.y * Q.y; // calculate common terms to avoid repeated operations + const FusionEuler euler = {.angle = { + .roll = FusionRadiansToDegrees(atan2f(Q.w * Q.x + Q.y * Q.z, halfMinusQySquared - Q.x * Q.x)), + .pitch = FusionRadiansToDegrees(FusionAsin(2.0f * (Q.w * Q.y - Q.z * Q.x))), + .yaw = FusionRadiansToDegrees(atan2f(Q.w * Q.z + Q.x * Q.y, halfMinusQySquared - Q.z * Q.z)), + }}; + return euler; +#undef Q +} + +#endif + +//------------------------------------------------------------------------------ +// End of file diff --git a/ardriver/xreal/FusionOffset.c b/ardriver/xreal/FusionOffset.c new file mode 100644 index 0000000..1359c3d --- /dev/null +++ b/ardriver/xreal/FusionOffset.c @@ -0,0 +1,77 @@ +/** + * @file FusionOffset.c + * @author Seb Madgwick + * @brief Gyroscope offset correction algorithm for run-time calibration of the + * gyroscope offset. + */ + +//------------------------------------------------------------------------------ +// Includes + +#include "FusionOffset.h" +#include + +//------------------------------------------------------------------------------ +// Definitions + +/** + * @brief Cutoff frequency in Hz. + */ +#define CUTOFF_FREQUENCY (0.02f) + +/** + * @brief Timeout in seconds. + */ +#define TIMEOUT (5) + +/** + * @brief Threshold in degrees per second. + */ +#define THRESHOLD (3.0f) + +//------------------------------------------------------------------------------ +// Functions + +/** + * @brief Initialises the gyroscope offset algorithm. + * @param offset Gyroscope offset algorithm structure. + * @param sampleRate Sample rate in Hz. + */ +void FusionOffsetInitialise(FusionOffset *const offset, const unsigned int sampleRate) { + offset->filterCoefficient = 2.0f * (float) M_PI * CUTOFF_FREQUENCY * (1.0f / (float) sampleRate); + offset->timeout = TIMEOUT * sampleRate; + offset->timer = 0; + offset->gyroscopeOffset = FUSION_VECTOR_ZERO; +} + +/** + * @brief Updates the gyroscope offset algorithm and returns the corrected + * gyroscope measurement. + * @param offset Gyroscope offset algorithm structure. + * @param gyroscope Gyroscope measurement in degrees per second. + * @return Corrected gyroscope measurement in degrees per second. + */ +FusionVector FusionOffsetUpdate(FusionOffset *const offset, FusionVector gyroscope) { + + // Subtract offset from gyroscope measurement + gyroscope = FusionVectorSubtract(gyroscope, offset->gyroscopeOffset); + + // Reset timer if gyroscope not stationary + if ((fabsf(gyroscope.axis.x) > THRESHOLD) || (fabsf(gyroscope.axis.y) > THRESHOLD) || (fabsf(gyroscope.axis.z) > THRESHOLD)) { + offset->timer = 0; + return gyroscope; + } + + // Increment timer while gyroscope stationary + if (offset->timer < offset->timeout) { + offset->timer++; + return gyroscope; + } + + // Adjust offset if timer has elapsed + offset->gyroscopeOffset = FusionVectorAdd(offset->gyroscopeOffset, FusionVectorMultiplyScalar(gyroscope, offset->filterCoefficient)); + return gyroscope; +} + +//------------------------------------------------------------------------------ +// End of file diff --git a/ardriver/xreal/FusionOffset.h b/ardriver/xreal/FusionOffset.h new file mode 100644 index 0000000..4782356 --- /dev/null +++ b/ardriver/xreal/FusionOffset.h @@ -0,0 +1,40 @@ +/** + * @file FusionOffset.h + * @author Seb Madgwick + * @brief Gyroscope offset correction algorithm for run-time calibration of the + * gyroscope offset. + */ + +#ifndef FUSION_OFFSET_H +#define FUSION_OFFSET_H + +//------------------------------------------------------------------------------ +// Includes + +#include "FusionMath.h" + +//------------------------------------------------------------------------------ +// Definitions + +/** + * @brief Gyroscope offset algorithm structure. Structure members are used + * internally and must not be accessed by the application. + */ +typedef struct { + float filterCoefficient; + unsigned int timeout; + unsigned int timer; + FusionVector gyroscopeOffset; +} FusionOffset; + +//------------------------------------------------------------------------------ +// Function declarations + +void FusionOffsetInitialise(FusionOffset *const offset, const unsigned int sampleRate); + +FusionVector FusionOffsetUpdate(FusionOffset *const offset, FusionVector gyroscope); + +#endif + +//------------------------------------------------------------------------------ +// End of file diff --git a/ardriver/xreal/README.md b/ardriver/xreal/README.md new file mode 100644 index 0000000..48e024c --- /dev/null +++ b/ardriver/xreal/README.md @@ -0,0 +1,5 @@ +# Xreal driver +## Attribution + +- [Xreal driver](https://gitlab.com/TheJackiMonster/nrealAirLinuxDriver) +- [Fusion](https://github.com/xioTechnologies/Fusion) diff --git a/ardriver/xreal/crc32.c b/ardriver/xreal/crc32.c new file mode 100644 index 0000000..2e2d955 --- /dev/null +++ b/ardriver/xreal/crc32.c @@ -0,0 +1,102 @@ +// +// Created by thejackimonster on 21.04.23. +// +// Copyright (c) 2023-2024 thejackimonster. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. +// + +#include "crc32.h" + +const uint32_t crc32_table [256] = { + 0x00000000, 0x77073096, 0xEE0E612C, 0x990951BA, + 0x076DC419, 0x706AF48F, 0xE963A535, 0x9E6495A3, + 0x0EDB8832, 0x79DCB8A4, 0xE0D5E91E, 0x97D2D988, + 0x09B64C2B, 0x7EB17CBD, 0xE7B82D07, 0x90BF1D91, + 0x1DB71064, 0x6AB020F2, 0xF3B97148, 0x84BE41DE, + 0x1ADAD47D, 0x6DDDE4EB, 0xF4D4B551, 0x83D385C7, + 0x136C9856, 0x646BA8C0, 0xFD62F97A, 0x8A65C9EC, + 0x14015C4F, 0x63066CD9, 0xFA0F3D63, 0x8D080DF5, + 0x3B6E20C8, 0x4C69105E, 0xD56041E4, 0xA2677172, + 0x3C03E4D1, 0x4B04D447, 0xD20D85FD, 0xA50AB56B, + 0x35B5A8FA, 0x42B2986C, 0xDBBBC9D6, 0xACBCF940, + 0x32D86CE3, 0x45DF5C75, 0xDCD60DCF, 0xABD13D59, + 0x26D930AC, 0x51DE003A, 0xC8D75180, 0xBFD06116, + 0x21B4F4B5, 0x56B3C423, 0xCFBA9599, 0xB8BDA50F, + 0x2802B89E, 0x5F058808, 0xC60CD9B2, 0xB10BE924, + 0x2F6F7C87, 0x58684C11, 0xC1611DAB, 0xB6662D3D, + 0x76DC4190, 0x01DB7106, 0x98D220BC, 0xEFD5102A, + 0x71B18589, 0x06B6B51F, 0x9FBFE4A5, 0xE8B8D433, + 0x7807C9A2, 0x0F00F934, 0x9609A88E, 0xE10E9818, + 0x7F6A0DBB, 0x086D3D2D, 0x91646C97, 0xE6635C01, + 0x6B6B51F4, 0x1C6C6162, 0x856530D8, 0xF262004E, + 0x6C0695ED, 0x1B01A57B, 0x8208F4C1, 0xF50FC457, + 0x65B0D9C6, 0x12B7E950, 0x8BBEB8EA, 0xFCB9887C, + 0x62DD1DDF, 0x15DA2D49, 0x8CD37CF3, 0xFBD44C65, + 0x4DB26158, 0x3AB551CE, 0xA3BC0074, 0xD4BB30E2, + 0x4ADFA541, 0x3DD895D7, 0xA4D1C46D, 0xD3D6F4FB, + 0x4369E96A, 0x346ED9FC, 0xAD678846, 0xDA60B8D0, + 0x44042D73, 0x33031DE5, 0xAA0A4C5F, 0xDD0D7CC9, + 0x5005713C, 0x270241AA, 0xBE0B1010, 0xC90C2086, + 0x5768B525, 0x206F85B3, 0xB966D409, 0xCE61E49F, + 0x5EDEF90E, 0x29D9C998, 0xB0D09822, 0xC7D7A8B4, + 0x59B33D17, 0x2EB40D81, 0xB7BD5C3B, 0xC0BA6CAD, + 0xEDB88320, 0x9ABFB3B6, 0x03B6E20C, 0x74B1D29A, + 0xEAD54739, 0x9DD277AF, 0x04DB2615, 0x73DC1683, + 0xE3630B12, 0x94643B84, 0x0D6D6A3E, 0x7A6A5AA8, + 0xE40ECF0B, 0x9309FF9D, 0x0A00AE27, 0x7D079EB1, + 0xF00F9344, 0x8708A3D2, 0x1E01F268, 0x6906C2FE, + 0xF762575D, 0x806567CB, 0x196C3671, 0x6E6B06E7, + 0xFED41B76, 0x89D32BE0, 0x10DA7A5A, 0x67DD4ACC, + 0xF9B9DF6F, 0x8EBEEFF9, 0x17B7BE43, 0x60B08ED5, + 0xD6D6A3E8, 0xA1D1937E, 0x38D8C2C4, 0x4FDFF252, + 0xD1BB67F1, 0xA6BC5767, 0x3FB506DD, 0x48B2364B, + 0xD80D2BDA, 0xAF0A1B4C, 0x36034AF6, 0x41047A60, + 0xDF60EFC3, 0xA867DF55, 0x316E8EEF, 0x4669BE79, + 0xCB61B38C, 0xBC66831A, 0x256FD2A0, 0x5268E236, + 0xCC0C7795, 0xBB0B4703, 0x220216B9, 0x5505262F, + 0xC5BA3BBE, 0xB2BD0B28, 0x2BB45A92, 0x5CB36A04, + 0xC2D7FFA7, 0xB5D0CF31, 0x2CD99E8B, 0x5BDEAE1D, + 0x9B64C2B0, 0xEC63F226, 0x756AA39C, 0x026D930A, + 0x9C0906A9, 0xEB0E363F, 0x72076785, 0x05005713, + 0x95BF4A82, 0xE2B87A14, 0x7BB12BAE, 0x0CB61B38, + 0x92D28E9B, 0xE5D5BE0D, 0x7CDCEFB7, 0x0BDBDF21, + 0x86D3D2D4, 0xF1D4E242, 0x68DDB3F8, 0x1FDA836E, + 0x81BE16CD, 0xF6B9265B, 0x6FB077E1, 0x18B74777, + 0x88085AE6, 0xFF0F6A70, 0x66063BCA, 0x11010B5C, + 0x8F659EFF, 0xF862AE69, 0x616BFFD3, 0x166CCF45, + 0xA00AE278, 0xD70DD2EE, 0x4E048354, 0x3903B3C2, + 0xA7672661, 0xD06016F7, 0x4969474D, 0x3E6E77DB, + 0xAED16A4A, 0xD9D65ADC, 0x40DF0B66, 0x37D83BF0, + 0xA9BCAE53, 0xDEBB9EC5, 0x47B2CF7F, 0x30B5FFE9, + 0xBDBDF21C, 0xCABAC28A, 0x53B39330, 0x24B4A3A6, + 0xBAD03605, 0xCDD70693, 0x54DE5729, 0x23D967BF, + 0xB3667A2E, 0xC4614AB8, 0x5D681B02, 0x2A6F2B94, + 0xB40BBE37, 0xC30C8EA1, 0x5A05DF1B, 0x2D02EF8D +}; + +uint32_t crc32_checksum(const uint8_t* buf, uint32_t len) { + uint32_t CRC32_data = 0xFFFFFFFF; + for (uint32_t i = 0; i < len; i++) { + const uint8_t t = (CRC32_data ^ buf[i]) & 0xFF; + CRC32_data = ((CRC32_data >> 8) & 0xFFFFFF) ^ crc32_table[t]; + } + + return ~CRC32_data; +} diff --git a/ardriver/xreal/crc32.h b/ardriver/xreal/crc32.h new file mode 100644 index 0000000..50be774 --- /dev/null +++ b/ardriver/xreal/crc32.h @@ -0,0 +1,40 @@ +#pragma once +// +// Created by thejackimonster on 21.04.23. +// +// Copyright (c) 2023 thejackimonster. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. +// + +#ifndef __cplusplus +#include +#else +#include +#endif + +#ifdef __cplusplus +extern "C" { +#endif + +uint32_t crc32_checksum(const uint8_t* buf, uint32_t len); + +#ifdef __cplusplus +} // extern "C" +#endif diff --git a/ardriver/xreal/device.c b/ardriver/xreal/device.c new file mode 100644 index 0000000..ab377f3 --- /dev/null +++ b/ardriver/xreal/device.c @@ -0,0 +1,50 @@ +// +// Created by thejackimonster on 19.12.23. +// +// Copyright (c) 2023 thejackimonster. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. +// + +#include "device.h" + +#include + +static size_t hid_device_counter = 0; + +bool device_init() { + if ((!hid_device_counter) && (0 != hid_init())) { + return false; + } + + hid_device_counter++; + return true; +} + +void device_exit() { + if (!hid_device_counter) { + return; + } + + hid_device_counter--; + + if (0 == hid_device_counter) { + hid_exit(); + } +} diff --git a/ardriver/xreal/device.h b/ardriver/xreal/device.h new file mode 100644 index 0000000..7f2c781 --- /dev/null +++ b/ardriver/xreal/device.h @@ -0,0 +1,46 @@ +#pragma once +// +// Created by thejackimonster on 19.12.23. +// +// Copyright (c) 2023 thejackimonster. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. +// + +#ifndef __cplusplus +#include +#endif + +#ifndef __cplusplus +#include +#else +#include +#endif + +#ifdef __cplusplus +extern "C" { +#endif + +bool device_init(); + +void device_exit(); + +#ifdef __cplusplus +} // extern "C" +#endif diff --git a/ardriver/xreal/device_imu.c b/ardriver/xreal/device_imu.c new file mode 100644 index 0000000..add1338 --- /dev/null +++ b/ardriver/xreal/device_imu.c @@ -0,0 +1,1272 @@ +// +// Created by thejackimonster on 30.03.23. +// +// Copyright (c) 2023-2025 thejackimonster. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. +// + +#include "device_imu.h" +#include "device.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include "crc32.h" +#include "hid_ids.h" + +#define GRAVITY_G (9.806f) + +#ifndef NDEBUG +#define device_imu_error(msg) fprintf(stderr, "ERROR: %s\n", msg) +#else +#define device_imu_error(msg) (0) +#endif + +struct device_imu_camera_sensor_t { + FusionMatrix cameraMisalignment; + FusionVector cameraOffset; + + uint16_t resolution [2]; + + float cc [2]; + float fc [2]; + + uint32_t num_kc; + float* kc; +}; + +struct device_imu_camera_t { + uint32_t num_sensors; + device_imu_camera_sensor_type* sensors; +}; + +struct device_imu_camera_calibration_t { + uint32_t num_cameras; + device_imu_camera_type *cameras; +}; + +struct device_imu_calibration_t { + FusionMatrix gyroscopeMisalignment; + FusionVector gyroscopeSensitivity; + FusionVector gyroscopeOffset; + + FusionMatrix accelerometerMisalignment; + FusionVector accelerometerSensitivity; + FusionVector accelerometerOffset; + + FusionMatrix magnetometerMisalignment; + FusionVector magnetometerSensitivity; + FusionVector magnetometerOffset; + + FusionMatrix softIronMatrix; + FusionVector hardIronOffset; + + FusionQuaternion noises; + + device_imu_camera_calibration_type cam; +}; + +static bool send_payload(device_imu_type* device, uint16_t size, const uint8_t* payload) { + int payload_size = size; + if (payload_size > device->max_payload_size) { + payload_size = device->max_payload_size; + } + + int transferred = hid_write(device->handle, payload, payload_size); + + if (transferred != payload_size) { + device_imu_error("Sending payload failed"); + return false; + } + + return (transferred == size); +} + +static bool recv_payload(device_imu_type* device, uint16_t size, uint8_t* payload) { + int payload_size = size; + if (payload_size > device->max_payload_size) { + payload_size = device->max_payload_size; + } + + int transferred = hid_read(device->handle, payload, payload_size); + + if (transferred >= payload_size) { + transferred = payload_size; + } + + if (transferred == 0) { + return false; + } + + if (transferred != payload_size) { + device_imu_error("Receiving payload failed"); + return false; + } + + return (transferred == size); +} + +struct __attribute__((__packed__)) device_imu_payload_packet_t { + uint8_t head; + uint32_t checksum; + uint16_t length; + uint8_t msgid; + uint8_t data [512 - 8]; +}; + +typedef struct device_imu_payload_packet_t device_imu_payload_packet_type; + +static bool send_payload_msg(device_imu_type* device, uint8_t msgid, uint16_t len, const uint8_t* data) { + static device_imu_payload_packet_type packet; + + const uint16_t packet_len = 3 + len; + const uint16_t payload_len = 5 + packet_len; + + packet.head = 0xAA; + packet.length = htole16(packet_len); + packet.msgid = msgid; + + memcpy(packet.data, data, len); + packet.checksum = htole32( + crc32_checksum( + (const uint8_t*) (&packet.length), + packet.length + ) + ); + + return send_payload(device, payload_len, (uint8_t*) (&packet)); +} + +static bool send_payload_msg_signal(device_imu_type* device, uint8_t msgid, uint8_t signal) { + return send_payload_msg(device, msgid, 1, &signal); +} + +static bool recv_payload_msg(device_imu_type* device, uint8_t msgid, uint16_t len, uint8_t* data) { + static device_imu_payload_packet_type packet; + + packet.head = 0; + packet.length = 0; + packet.msgid = 0; + + const uint16_t packet_len = 3 + len; + const uint16_t payload_len = 5 + packet_len; + + do { + if (!recv_payload(device, payload_len, (uint8_t*) (&packet))) { + return false; + } + } while (packet.msgid != msgid); + + memcpy(data, packet.data, len); + return true; +} + +static FusionVector json_object_get_vector(struct json_object* obj) { + if ((!json_object_is_type(obj, json_type_array)) || + (json_object_array_length(obj) != 3)) { + return FUSION_VECTOR_ZERO; + } + + FusionVector vector; + vector.axis.x = (float) json_object_get_double(json_object_array_get_idx(obj, 0)); + vector.axis.y = (float) json_object_get_double(json_object_array_get_idx(obj, 1)); + vector.axis.z = (float) json_object_get_double(json_object_array_get_idx(obj, 2)); + return vector; +} + +static FusionQuaternion json_object_get_quaternion(struct json_object* obj) { + if ((!json_object_is_type(obj, json_type_array)) || + (json_object_array_length(obj) != 4)) { + return FUSION_IDENTITY_QUATERNION; + } + + FusionQuaternion quaternion; + quaternion.element.x = (float) json_object_get_double(json_object_array_get_idx(obj, 0)); + quaternion.element.y = (float) json_object_get_double(json_object_array_get_idx(obj, 1)); + quaternion.element.z = (float) json_object_get_double(json_object_array_get_idx(obj, 2)); + quaternion.element.w = (float) json_object_get_double(json_object_array_get_idx(obj, 3)); + return quaternion; +} + +static uint32_t json_object_get_array_f32(struct json_object* obj, float** array, uint32_t n) { + if ((!json_object_is_type(obj, json_type_array)) || + ((n > 0) && (json_object_array_length(obj) != n))) { + return 0; + } + + if (n == 0) { + n = json_object_array_length(obj); + *array = malloc(sizeof(float) * n); + } + + for (uint32_t i = 0; i < n; i++) { + (*array)[i] = (float) json_object_get_double(json_object_array_get_idx(obj, i)); + } + + return n; +} + +static uint32_t json_object_get_array_u16(struct json_object* obj, uint16_t** array, uint32_t n) { + if ((!json_object_is_type(obj, json_type_array)) || + ((n > 0) && (json_object_array_length(obj) != n))) { + return 0; + } + + if (n == 0) { + n = json_object_array_length(obj); + *array = malloc(sizeof(uint16_t) * n); + } + + for (uint32_t i = 0; i < n; i++) { + (*array)[i] = (uint16_t) json_object_get_int(json_object_array_get_idx(obj, i)); + } + + return n; +} + +static void init_device_imu_camera(device_imu_camera_type *camera, json_object *cam) { + uint32_t num_sensors = json_object_get_int(json_object_object_get(cam, "num_of_cameras")); + + device_imu_camera_sensor_type *sensors = NULL; + + if (num_sensors > 0) { + sensors = malloc(sizeof(device_imu_camera_sensor_type) * num_sensors); + } + + if (!sensors) { + num_sensors = 0; + } + + for (uint32_t n = 0; n < num_sensors; n++) { + device_imu_camera_sensor_type *sensor = &(sensors[n]); + + char device_name [64]; + snprintf(device_name, 64, "device_%u", (n + 1)); + + struct json_object* dev = json_object_object_get(cam, device_name); + + FusionQuaternion imu_q_cam = json_object_get_quaternion(json_object_object_get(dev, "imu_q_cam")); + FusionVector cam_offset = json_object_get_vector(json_object_object_get(dev, "imu_p_cam")); + + sensor->cameraMisalignment = FusionQuaternionToMatrix(imu_q_cam); + sensor->cameraOffset = cam_offset; + + uint16_t* resolution = sensor->resolution; + float* cc = sensor->cc; + float* fc = sensor->fc; + float* kc = sensor->kc; + + json_object_get_array_u16(json_object_object_get(dev, "resolution"), &resolution, 2); + + json_object_get_array_f32(json_object_object_get(dev, "cc"), &cc, 2); + json_object_get_array_f32(json_object_object_get(dev, "fc"), &fc, 2); + + sensor->num_kc = json_object_get_array_f32(json_object_object_get(dev, "kc"), &kc, 0); + sensor->kc = kc; + } + + camera->num_sensors = num_sensors; + camera->sensors = sensors; +} + +device_imu_error_type device_imu_open(device_imu_type* device, device_imu_event_callback callback) { + if (!device) { + device_imu_error("No device"); + return DEVICE_IMU_ERROR_NO_DEVICE; + } + + memset(device, 0, sizeof(device_imu_type)); + device->vendor_id = xreal_vendor_id; + device->product_id = 0; + device->callback = callback; + + if (!device_init()) { + device_imu_error("Not initialized"); + return DEVICE_IMU_ERROR_NOT_INITIALIZED; + } + + struct hid_device_info* info = hid_enumerate( + device->vendor_id, + device->product_id + ); + + struct hid_device_info* it = info; + while (it) { + int interface_id = xreal_imu_interface_id(it->product_id); + if (interface_id != -1 && it->interface_number == interface_id) { +#ifndef NDEBUG + printf("Found IMU device with product_id 0x%x on interface %d\n", it->product_id, interface_id); +#endif + device->product_id = it->product_id; + device->handle = hid_open_path(it->path); + device->max_payload_size = xreal_imu_max_payload_size(device->product_id); + break; + } + + it = it->next; + } + + hid_free_enumeration(info); + + if (!device->handle) { + device_imu_error("No handle"); + return DEVICE_IMU_ERROR_NO_HANDLE; + } + + if ((!send_payload_msg_signal(device, DEVICE_IMU_MSG_START_IMU_DATA, 0x0)) || + (!recv_payload_msg(device, DEVICE_IMU_MSG_START_IMU_DATA, 0, NULL))) { + device_imu_error("Failed sending payload to stop imu data stream"); + return DEVICE_IMU_ERROR_PAYLOAD_FAILED; + } + + device_imu_clear(device); + + if (!send_payload_msg(device, DEVICE_IMU_MSG_GET_STATIC_ID, 0, NULL)) { + device_imu_error("Failed sending payload to get static id"); + return DEVICE_IMU_ERROR_PAYLOAD_FAILED; + } + + uint32_t static_id = 0; + if (recv_payload_msg(device, DEVICE_IMU_MSG_GET_STATIC_ID, 4, (uint8_t*) &static_id)) { + device->static_id = static_id; + } else { + device->static_id = 0x20220101; + } + + device->calibration = malloc(sizeof(device_imu_calibration_type)); + memset(device->calibration, 0, sizeof(device_imu_calibration_type)); + + device_imu_reset_calibration(device); + + if (!send_payload_msg(device, DEVICE_IMU_MSG_GET_CAL_DATA_LENGTH, 0, NULL)) { + device_imu_error("Failed sending payload to get calibration data length"); + return DEVICE_IMU_ERROR_PAYLOAD_FAILED; + } + + uint32_t calibration_len = 0; + if (recv_payload_msg(device, DEVICE_IMU_MSG_GET_CAL_DATA_LENGTH, 4, (uint8_t*) &calibration_len)) { + const uint16_t max_packet_size = (device->max_payload_size - 8); + char* calibration_data = malloc(calibration_len + 1); + + uint32_t position = 0; + while (position < calibration_len) { + const uint32_t remaining = (calibration_len - position); + + if (!send_payload_msg(device, DEVICE_IMU_MSG_CAL_DATA_GET_NEXT_SEGMENT, 0, NULL)) { + break; + } + + const uint16_t next = (remaining > max_packet_size? max_packet_size : remaining); + + if (!recv_payload_msg(device, DEVICE_IMU_MSG_CAL_DATA_GET_NEXT_SEGMENT, next, (uint8_t*) calibration_data + position)) { + break; + } + + position += next; + } + + calibration_data[calibration_len] = '\0'; + + struct json_tokener* tokener = json_tokener_new(); + struct json_object* root = json_tokener_parse_ex(tokener, calibration_data, calibration_len); + struct json_object* imu = json_object_object_get(root, "IMU"); + struct json_object* dev1 = json_object_object_get(imu, "device_1"); + + FusionVector accel_bias = json_object_get_vector(json_object_object_get(dev1, "accel_bias")); + FusionQuaternion accel_q_gyro = json_object_get_quaternion(json_object_object_get(dev1, "accel_q_gyro")); + FusionVector gyro_bias = json_object_get_vector(json_object_object_get(dev1, "gyro_bias")); + FusionQuaternion gyro_q_mag = json_object_get_quaternion(json_object_object_get(dev1, "gyro_q_mag")); + FusionVector mag_bias = json_object_get_vector(json_object_object_get(dev1, "mag_bias")); + FusionQuaternion imu_noises = json_object_get_quaternion(json_object_object_get(dev1, "imu_noises")); + FusionVector scale_accel = json_object_get_vector(json_object_object_get(dev1, "scale_accel")); + FusionVector scale_gyro = json_object_get_vector(json_object_object_get(dev1, "scale_gyro")); + FusionVector scale_mag = json_object_get_vector(json_object_object_get(dev1, "scale_mag")); + + const FusionQuaternion accel_q_mag = FusionQuaternionMultiply(accel_q_gyro, gyro_q_mag); + + device->calibration->gyroscopeMisalignment = FusionQuaternionToMatrix(accel_q_gyro); + device->calibration->gyroscopeSensitivity = scale_gyro; + device->calibration->gyroscopeOffset = gyro_bias; + + device->calibration->accelerometerMisalignment = FUSION_IDENTITY_MATRIX; + device->calibration->accelerometerSensitivity = scale_accel; + device->calibration->accelerometerOffset = accel_bias; + + device->calibration->magnetometerMisalignment = FusionQuaternionToMatrix(accel_q_mag); + device->calibration->magnetometerSensitivity = scale_mag; + device->calibration->magnetometerOffset = mag_bias; + + device->calibration->noises = imu_noises; + + struct json_object* rgb = json_object_object_get(root, "RGB_camera"); + struct json_object* slam = json_object_object_get(root, "SLAM_camera"); + + uint32_t num_cameras_rgb = json_object_get_int(json_object_object_get(rgb, "num_of_cameras")); + uint32_t num_cameras_slam = json_object_get_int(json_object_object_get(slam, "num_of_cameras")); + + const uint32_t num_cameras = (num_cameras_rgb > 0? 1 : 0) + (num_cameras_slam > 0? 1 : 0); + + device_imu_camera_type *cameras = NULL; + uint32_t camera_index = 0; + + if (num_cameras > 0) { + cameras = malloc(sizeof(device_imu_camera_type) * num_cameras); + } + + if ((cameras) && (num_cameras_rgb > 0)) { + init_device_imu_camera(&(cameras[camera_index++]), rgb); + } + + if ((cameras) && (num_cameras_slam > 0)) { + init_device_imu_camera(&(cameras[camera_index++]), slam); + } + + device->calibration->cam.num_cameras = cameras? num_cameras : 0; + device->calibration->cam.cameras = cameras; + + json_tokener_free(tokener); + free(calibration_data); + } + + if ((!send_payload_msg_signal(device, DEVICE_IMU_MSG_START_IMU_DATA, 0x1)) || + (!recv_payload_msg(device, DEVICE_IMU_MSG_START_IMU_DATA, 0, NULL))) { + device_imu_error("Failed sending payload to start imu data stream"); + return DEVICE_IMU_ERROR_PAYLOAD_FAILED; + } + + const uint32_t SAMPLE_RATE = 1000; + + device->offset = malloc(sizeof(FusionOffset)); + device->ahrs = malloc(sizeof(FusionAhrs)); + + if (device->offset) { + FusionOffsetInitialise((FusionOffset*) device->offset, SAMPLE_RATE); + } + + FusionAhrsInitialise((FusionAhrs*) device->ahrs); + + const FusionAhrsSettings settings = { + .convention = FusionConventionNed, + .gain = 0.5f, + .accelerationRejection = 10.0f, + .magneticRejection = 20.0f, + .recoveryTriggerPeriod = 5 * SAMPLE_RATE, /* 5 seconds */ + }; + + FusionAhrsSetSettings((FusionAhrs*) device->ahrs, &settings); + return DEVICE_IMU_ERROR_NO_ERROR; +} + +device_imu_error_type device_imu_reset_calibration(device_imu_type* device) { + if (!device) { + device_imu_error("No device"); + return DEVICE_IMU_ERROR_NO_DEVICE; + } + + if (!device->calibration) { + device_imu_error("Not allocated"); + return DEVICE_IMU_ERROR_NO_ALLOCATION; + } + + device->calibration->gyroscopeMisalignment = FUSION_IDENTITY_MATRIX; + device->calibration->gyroscopeSensitivity = FUSION_VECTOR_ONES; + device->calibration->gyroscopeOffset = FUSION_VECTOR_ZERO; + + device->calibration->accelerometerMisalignment = FUSION_IDENTITY_MATRIX; + device->calibration->accelerometerSensitivity = FUSION_VECTOR_ONES; + device->calibration->accelerometerOffset = FUSION_VECTOR_ZERO; + + device->calibration->magnetometerMisalignment = FUSION_IDENTITY_MATRIX; + device->calibration->magnetometerSensitivity = FUSION_VECTOR_ONES; + device->calibration->magnetometerOffset = FUSION_VECTOR_ZERO; + + device->calibration->softIronMatrix = FUSION_IDENTITY_MATRIX; + device->calibration->hardIronOffset = FUSION_VECTOR_ZERO; + + device->calibration->noises = FUSION_IDENTITY_QUATERNION; + device->calibration->noises.element.w = 0.0f; + + if (device->calibration->cam.cameras) { + for (uint32_t i = 0; i < device->calibration->cam.num_cameras; i++) { + if (!device->calibration->cam.cameras[i].sensors) { + continue; + } + + for (uint32_t j = 0; j < device->calibration->cam.cameras[i].num_sensors; j++) { + if (!device->calibration->cam.cameras[i].sensors[j].kc) { + continue; + } + + free(device->calibration->cam.cameras[i].sensors[j].kc); + + device->calibration->cam.cameras[i].sensors[j].num_kc = 0; + device->calibration->cam.cameras[i].sensors[j].kc = NULL; + } + + free(device->calibration->cam.cameras[i].sensors); + + device->calibration->cam.cameras[i].num_sensors = 0; + device->calibration->cam.cameras[i].sensors = NULL; + } + + free(device->calibration->cam.cameras); + } + + device->calibration->cam.num_cameras = 0; + device->calibration->cam.cameras = NULL; +} + +device_imu_error_type device_imu_load_calibration(device_imu_type* device, const char* path) { + if (!device) { + device_imu_error("No device"); + return DEVICE_IMU_ERROR_NO_DEVICE; + } + + if (!device->calibration) { + device_imu_error("Not allocated"); + return DEVICE_IMU_ERROR_NO_ALLOCATION; + } + + FILE* file = fopen(path, "rb"); + if (!file) { + device_imu_error("No file opened"); + return DEVICE_IMU_ERROR_FILE_NOT_OPEN; + } + + device_imu_error_type result = DEVICE_IMU_ERROR_NO_ERROR; + const size_t calibration_size = ( + sizeof(device_imu_calibration_type) - sizeof(device_imu_camera_calibration_type) + ); + + size_t count; + count = fread(device->calibration, 1, calibration_size, file); + + if (calibration_size != count) { + device_imu_error("Not fully loaded"); + result = DEVICE_IMU_ERROR_LOADING_FAILED; + } + + if (0 != fclose(file)) { + device_imu_error("No file closed"); + return DEVICE_IMU_ERROR_FILE_NOT_CLOSED; + } + + return result; +} + +device_imu_error_type device_imu_save_calibration(device_imu_type* device, const char* path) { + if (!device) { + device_imu_error("No device"); + return DEVICE_IMU_ERROR_NO_DEVICE; + } + + if (!device->calibration) { + device_imu_error("Not allocated"); + return DEVICE_IMU_ERROR_NO_ALLOCATION; + } + + FILE* file = fopen(path, "wb"); + if (!file) { + device_imu_error("No file opened"); + return DEVICE_IMU_ERROR_FILE_NOT_OPEN; + } + + device_imu_error_type result = DEVICE_IMU_ERROR_NO_ERROR; + const size_t calibration_size = ( + sizeof(device_imu_calibration_type) - sizeof(device_imu_camera_calibration_type) + ); + + size_t count; + count = fwrite(device->calibration, 1, calibration_size, file); + + if (calibration_size != count) { + device_imu_error("Not fully saved"); + result = DEVICE_IMU_ERROR_SAVING_FAILED; + } + + if (0 != fclose(file)) { + device_imu_error("No file closed"); + return DEVICE_IMU_ERROR_FILE_NOT_CLOSED; + } + + return result; +} + +static void device_imu_callback(device_imu_type* device, + uint64_t timestamp, + device_imu_event_type event) { + if (!device->callback) { + return; + } + + device->callback(timestamp, event, device->ahrs); +} + +static int32_t pack32bit_signed(const uint8_t* data) { + uint32_t unsigned_value = (data[0]) | (data[1] << 8) | (data[2] << 16) | (data[3] << 24); + return ((int32_t) unsigned_value); +} + +static int32_t pack24bit_signed(const uint8_t* data) { + uint32_t unsigned_value = (data[0]) | (data[1] << 8) | (data[2] << 16); + if ((data[2] & 0x80) != 0) unsigned_value |= (0xFF << 24); + return ((int32_t) unsigned_value); +} + +static int16_t pack16bit_signed(const uint8_t* data) { + uint16_t unsigned_value = (data[1] << 8) | (data[0]); + return (int16_t) unsigned_value; +} + +static int32_t pack32bit_signed_swap(const uint8_t* data) { + uint32_t unsigned_value = (data[0] << 24) | (data[1] << 16) | (data[2] << 8) | (data[3]); + return ((int32_t) unsigned_value); +} + +static int16_t pack16bit_signed_swap(const uint8_t* data) { + uint16_t unsigned_value = (data[0] << 8) | (data[1]); + return (int16_t) unsigned_value; +} + +static int16_t pack16bit_signed_bizarre(const uint8_t* data) { + uint16_t unsigned_value = (data[0]) | ((data[1] ^ 0x80) << 8); + return (int16_t) unsigned_value; +} + +static void readIMU_from_packet(const device_imu_packet_type* packet, + FusionVector* gyroscope, + FusionVector* accelerometer, + FusionVector* magnetometer) { + int32_t vel_m = pack16bit_signed(packet->angular_multiplier); + int32_t vel_d = pack32bit_signed(packet->angular_divisor); + + int32_t vel_x = pack24bit_signed(packet->angular_velocity_x); + int32_t vel_y = pack24bit_signed(packet->angular_velocity_y); + int32_t vel_z = pack24bit_signed(packet->angular_velocity_z); + + gyroscope->axis.x = (float) vel_x * (float) vel_m / (float) vel_d; + gyroscope->axis.y = (float) vel_y * (float) vel_m / (float) vel_d; + gyroscope->axis.z = (float) vel_z * (float) vel_m / (float) vel_d; + + int32_t accel_m = pack16bit_signed(packet->acceleration_multiplier); + int32_t accel_d = pack32bit_signed(packet->acceleration_divisor); + + int32_t accel_x = pack24bit_signed(packet->acceleration_x); + int32_t accel_y = pack24bit_signed(packet->acceleration_y); + int32_t accel_z = pack24bit_signed(packet->acceleration_z); + + accelerometer->axis.x = (float) accel_x * (float) accel_m / (float) accel_d; + accelerometer->axis.y = (float) accel_y * (float) accel_m / (float) accel_d; + accelerometer->axis.z = (float) accel_z * (float) accel_m / (float) accel_d; + + int32_t magnet_m = pack16bit_signed_swap(packet->magnetic_multiplier); + int32_t magnet_d = pack32bit_signed_swap(packet->magnetic_divisor); + + int16_t magnet_x = pack16bit_signed_bizarre(packet->magnetic_x); + int16_t magnet_y = pack16bit_signed_bizarre(packet->magnetic_y); + int16_t magnet_z = pack16bit_signed_bizarre(packet->magnetic_z); + + magnetometer->axis.x = (float) magnet_x * (float) magnet_m / (float) magnet_d; + magnetometer->axis.y = (float) magnet_y * (float) magnet_m / (float) magnet_d; + magnetometer->axis.z = (float) magnet_z * (float) magnet_m / (float) magnet_d; +} + +#define min(x, y) ((x) < (y)? (x) : (y)) +#define max(x, y) ((x) > (y)? (x) : (y)) + +static void pre_biased_coordinate_system(FusionVector* v) { + *v = FusionAxesSwap(*v, FusionAxesAlignmentNXNZNY); +} + +static void post_biased_coordinate_system(const FusionVector* v, FusionVector* res) { + *res = FusionAxesSwap(*v, FusionAxesAlignmentPZPXPY); +} + +static void iterate_iron_offset_estimation(const FusionVector* magnetometer, FusionMatrix* softIronMatrix, FusionVector* hardIronOffset) { + static FusionVector max = { FLT_MIN, FLT_MIN, FLT_MIN }; + static FusionVector min = { FLT_MAX, FLT_MAX, FLT_MAX }; + + for (int i = 0; i < 3; i++) { + max.array[i] = max(max.array[i], magnetometer->array[i]); + min.array[i] = min(min.array[i], magnetometer->array[i]); + } + + const float mx = (max.axis.x - min.axis.x) / 2.0f; + const float my = (max.axis.y - min.axis.y) / 2.0f; + const float mz = (max.axis.z - min.axis.z) / 2.0f; + + const float cx = (min.axis.x + max.axis.x) / 2.0f; + const float cy = (min.axis.y + max.axis.y) / 2.0f; + const float cz = (min.axis.z + max.axis.z) / 2.0f; + + memset(softIronMatrix, 0, sizeof(*softIronMatrix)); + + softIronMatrix->element.xx = 1.0f / mx; + softIronMatrix->element.yy = 1.0f / my; + softIronMatrix->element.zz = 1.0f / mz; + + hardIronOffset->axis.x = cx; + hardIronOffset->axis.y = cy; + hardIronOffset->axis.z = cz; +} + +static void apply_calibration(const device_imu_type* device, + FusionVector* gyroscope, + FusionVector* accelerometer, + FusionVector* magnetometer) { + FusionMatrix gyroscopeMisalignment; + FusionVector gyroscopeSensitivity; + FusionVector gyroscopeOffset; + + FusionMatrix accelerometerMisalignment; + FusionVector accelerometerSensitivity; + FusionVector accelerometerOffset; + + FusionMatrix magnetometerMisalignment; + FusionVector magnetometerSensitivity; + FusionVector magnetometerOffset; + + FusionMatrix softIronMatrix; + FusionVector hardIronOffset; + + if (device->calibration) { + gyroscopeMisalignment = device->calibration->gyroscopeMisalignment; + gyroscopeSensitivity = device->calibration->gyroscopeSensitivity; + gyroscopeOffset = device->calibration->gyroscopeOffset; + + accelerometerMisalignment = device->calibration->accelerometerMisalignment; + accelerometerSensitivity = device->calibration->accelerometerSensitivity; + accelerometerOffset = device->calibration->accelerometerOffset; + + magnetometerMisalignment = device->calibration->magnetometerMisalignment; + magnetometerSensitivity = device->calibration->magnetometerSensitivity; + magnetometerOffset = device->calibration->magnetometerOffset; + + softIronMatrix = device->calibration->softIronMatrix; + hardIronOffset = device->calibration->hardIronOffset; + } else { + gyroscopeMisalignment = FUSION_IDENTITY_MATRIX; + gyroscopeSensitivity = FUSION_VECTOR_ONES; + gyroscopeOffset = FUSION_VECTOR_ZERO; + + accelerometerMisalignment = FUSION_IDENTITY_MATRIX; + accelerometerSensitivity = FUSION_VECTOR_ONES; + accelerometerOffset = FUSION_VECTOR_ZERO; + + magnetometerMisalignment = FUSION_IDENTITY_MATRIX; + magnetometerSensitivity = FUSION_VECTOR_ONES; + magnetometerOffset = FUSION_VECTOR_ZERO; + + softIronMatrix = FUSION_IDENTITY_MATRIX; + hardIronOffset = FUSION_VECTOR_ZERO; + } + + gyroscopeOffset = FusionVectorMultiplyScalar( + gyroscopeOffset, + FusionRadiansToDegrees(1.0f) + ); + + accelerometerOffset = FusionVectorMultiplyScalar( + accelerometerOffset, + 1.0f / GRAVITY_G + ); + + FusionVector g = *gyroscope; + FusionVector a = *accelerometer; + FusionVector m = *magnetometer; + + pre_biased_coordinate_system(&g); + pre_biased_coordinate_system(&a); + pre_biased_coordinate_system(&m); + + g = FusionCalibrationInertial( + g, + gyroscopeMisalignment, + gyroscopeSensitivity, + gyroscopeOffset + ); + + a = FusionCalibrationInertial( + a, + accelerometerMisalignment, + accelerometerSensitivity, + accelerometerOffset + ); + + m = FusionCalibrationInertial( + m, + magnetometerMisalignment, + magnetometerSensitivity, + magnetometerOffset + ); + + iterate_iron_offset_estimation( + &m, + &softIronMatrix, + &hardIronOffset + ); + + if (device->calibration) { + device->calibration->softIronMatrix = softIronMatrix; + device->calibration->hardIronOffset = hardIronOffset; + } + + m = FusionCalibrationMagnetic( + m, + softIronMatrix, + hardIronOffset + ); + + post_biased_coordinate_system(&g, gyroscope); + post_biased_coordinate_system(&a, accelerometer); + post_biased_coordinate_system(&m, magnetometer); +} + +device_imu_error_type device_imu_clear(device_imu_type* device) { + return device_imu_read(device, 10); +} + +device_imu_error_type device_imu_calibrate(device_imu_type* device, uint32_t iterations, bool gyro, bool accel, bool magnet) { + if (!device) { + device_imu_error("No device"); + return DEVICE_IMU_ERROR_NO_DEVICE; + } + + if (!device->handle) { + device_imu_error("No handle"); + return DEVICE_IMU_ERROR_NO_HANDLE; + } + + if (!device->calibration) { + device_imu_error("No calibration allocated"); + return DEVICE_IMU_ERROR_NO_ALLOCATION; + } + + if (sizeof(device_imu_packet_type) > device->max_payload_size) { + device_imu_error("Not proper size"); + return DEVICE_IMU_ERROR_WRONG_SIZE; + } + + device_imu_packet_type packet; + int transferred; + + bool initialized = false; + + FusionVector cal_gyroscope; + FusionVector cal_accelerometer; + + FusionMatrix softIronMatrix; + FusionVector hardIronOffset; + + const float factor = iterations > 0? 1.0f / ((float) iterations) : 0.0f; + + FusionVector prev_accel; + while (iterations > 0) { + memset(&packet, 0, sizeof(device_imu_packet_type)); + + transferred = hid_read( + device->handle, + (uint8_t*) &packet, + sizeof(device_imu_packet_type) + ); + + if (transferred == -1) { + device_imu_error("Device may be unplugged"); + return DEVICE_IMU_ERROR_UNPLUGGED; + } + + if (transferred == 0) { + continue; + } + + if (sizeof(device_imu_packet_type) != transferred) { + device_imu_error("Unexpected packet size"); + return DEVICE_IMU_ERROR_UNEXPECTED; + } + + if ((packet.signature[0] != 0x01) || (packet.signature[1] != 0x02)) { + continue; + } + + FusionVector gyroscope; + FusionVector accelerometer; + FusionVector magnetometer; + + readIMU_from_packet(&packet, &gyroscope, &accelerometer, &magnetometer); + + pre_biased_coordinate_system(&gyroscope); + pre_biased_coordinate_system(&accelerometer); + pre_biased_coordinate_system(&magnetometer); + + if (initialized) { + cal_gyroscope = FusionVectorAdd(cal_gyroscope, gyroscope); + cal_accelerometer = FusionVectorAdd(cal_accelerometer, FusionVectorSubtract(accelerometer, prev_accel)); + } else { + cal_gyroscope = gyroscope; + cal_accelerometer = FUSION_VECTOR_ZERO; + } + + prev_accel = accelerometer; + + iterate_iron_offset_estimation( + &magnetometer, + &softIronMatrix, + &hardIronOffset + ); + + iterations--; + } + + if (factor > 0.0f) { + if (gyro) { + device->calibration->gyroscopeOffset = FusionVectorAdd( + device->calibration->gyroscopeOffset, + FusionVectorMultiplyScalar( + cal_gyroscope, + FusionDegreesToRadians(factor) + ) + ); + } + + if (accel) { + device->calibration->accelerometerOffset = FusionVectorAdd( + device->calibration->accelerometerOffset, + FusionVectorMultiplyScalar( + cal_accelerometer, + factor * GRAVITY_G + ) + ); + } + + if (magnet) { + device->calibration->softIronMatrix = softIronMatrix; + device->calibration->hardIronOffset = hardIronOffset; + } + } + + return DEVICE_IMU_ERROR_NO_ERROR; +} + +device_imu_error_type device_imu_read(device_imu_type* device, int timeout) { + if (!device) { + device_imu_error("No device"); + return DEVICE_IMU_ERROR_NO_DEVICE; + } + + if (!device->handle) { + device_imu_error("No handle"); + return DEVICE_IMU_ERROR_NO_HANDLE; + } + + if (sizeof(device_imu_packet_type) > device->max_payload_size) { + device_imu_error("Not proper size"); + return DEVICE_IMU_ERROR_WRONG_SIZE; + } + + device_imu_packet_type packet; + memset(&packet, 0, sizeof(device_imu_packet_type)); + + int transferred = hid_read_timeout( + device->handle, + (uint8_t*) &packet, + sizeof(device_imu_packet_type), + timeout + ); + + if (transferred == -1) { + device_imu_error("Device may be unplugged"); + return DEVICE_IMU_ERROR_UNPLUGGED; + } + + if (transferred == 0) { + return DEVICE_IMU_ERROR_NO_ERROR; + } + + if (sizeof(device_imu_packet_type) != transferred) { + device_imu_error("Unexpected packet size"); + return DEVICE_IMU_ERROR_UNEXPECTED; + } + + const uint64_t timestamp = le64toh(packet.timestamp); + + if ((packet.signature[0] == 0xaa) && (packet.signature[1] == 0x53)) { + device_imu_callback(device, timestamp, DEVICE_IMU_EVENT_INIT); + return DEVICE_IMU_ERROR_NO_ERROR; + } + + if ((packet.signature[0] != 0x01) || (packet.signature[1] != 0x02)) { + device_imu_error("Not matching signature"); + return DEVICE_IMU_ERROR_WRONG_SIGNATURE; + } + + const uint64_t delta = timestamp - device->last_timestamp; + const float deltaTime = (float) ((double) delta / 1e9); + + device->last_timestamp = timestamp; + + int16_t temperature = pack16bit_signed(packet.temperature); + + // According to the ICM-42688-P datasheet: (offset: 25 °C, sensitivity: 132.48 LSB/°C) + device->temperature = ((float) temperature) / 132.48f + 25.0f; + + FusionVector gyroscope; + FusionVector accelerometer; + FusionVector magnetometer; + + readIMU_from_packet(&packet, &gyroscope, &accelerometer, &magnetometer); + apply_calibration(device, &gyroscope, &accelerometer, &magnetometer); + + if (device->offset) { + gyroscope = FusionOffsetUpdate((FusionOffset*) device->offset, gyroscope); + } + +#ifndef NDEBUG + printf("G: %.2f %.2f %.2f\n", gyroscope.axis.x, gyroscope.axis.y, gyroscope.axis.z); + printf("A: %.2f %.2f %.2f\n", accelerometer.axis.x, accelerometer.axis.y, accelerometer.axis.z); + printf("M: %.2f %.2f %.2f\n", magnetometer.axis.x, magnetometer.axis.y, magnetometer.axis.z); +#endif + + if (device->ahrs) { + if (isnan(magnetometer.axis.x) || isnan(magnetometer.axis.x) || isnan(magnetometer.axis.x)) { + FusionAhrsUpdateNoMagnetometer((FusionAhrs*) device->ahrs, gyroscope, accelerometer, deltaTime); + } else { + /* The magnetometer seems to make results of sensor fusion generally worse. So it is not used currently. */ + // FusionAhrsUpdate((FusionAhrs*) device->ahrs, gyroscope, accelerometer, magnetometer, deltaTime); + FusionAhrsUpdateNoMagnetometer((FusionAhrs*) device->ahrs, gyroscope, accelerometer, deltaTime); + } + + const device_imu_quat_type orientation = device_imu_get_orientation(device->ahrs); + + // TODO: fix detection of this case; quat.x as a nan value is only a side-effect of some issue with ahrs or + // the gyro/accel/magnet readings + if (isnan(orientation.x) || isnan(orientation.y) || isnan(orientation.z) || isnan(orientation.w)) { + device_imu_error("Invalid orientation reading"); + return DEVICE_IMU_ERROR_INVALID_VALUE; + } + } + + device_imu_callback(device, timestamp, DEVICE_IMU_EVENT_UPDATE); + return DEVICE_IMU_ERROR_NO_ERROR; +} + +device_imu_vec3_type device_imu_get_earth_acceleration(const device_imu_ahrs_type* ahrs) { + FusionVector acceleration = ahrs? FusionAhrsGetEarthAcceleration((const FusionAhrs*) ahrs) : FUSION_VECTOR_ZERO; + device_imu_vec3_type a; + a.x = acceleration.axis.x; + a.y = acceleration.axis.y; + a.z = acceleration.axis.z; + return a; +} + +device_imu_vec3_type device_imu_get_linear_acceleration(const device_imu_ahrs_type* ahrs) { + FusionVector acceleration = ahrs? FusionAhrsGetLinearAcceleration((const FusionAhrs*) ahrs) : FUSION_VECTOR_ZERO; + device_imu_vec3_type a; + a.x = acceleration.axis.x; + a.y = acceleration.axis.y; + a.z = acceleration.axis.z; + return a; +} + +device_imu_quat_type device_imu_get_orientation(const device_imu_ahrs_type* ahrs) { + FusionQuaternion quaternion = ahrs? FusionAhrsGetQuaternion((const FusionAhrs*) ahrs) : FUSION_IDENTITY_QUATERNION; + device_imu_quat_type q; + q.x = quaternion.element.x; + q.y = quaternion.element.y; + q.z = quaternion.element.z; + q.w = quaternion.element.w; + return q; +} + +device_imu_euler_type device_imu_get_euler(device_imu_quat_type quat) { + FusionQuaternion quaternion; + quaternion.element.x = quat.x; + quaternion.element.y = quat.y; + quaternion.element.z = quat.z; + quaternion.element.w = quat.w; + FusionEuler euler = FusionQuaternionToEuler(quaternion); + device_imu_euler_type e; + e.roll = euler.angle.roll; + e.pitch = euler.angle.pitch; + e.yaw = euler.angle.yaw; + return e; +} + +uint32_t device_imu_get_num_of_cameras(device_imu_type *device) { + if (!device->calibration) { + return 0; + } + + return device->calibration->cam.num_cameras; +} + +const device_imu_camera_type* device_imu_get_camera(const device_imu_type *device, uint32_t index) { + if ((!device->calibration) || (!device->calibration->cam.cameras)) { + return NULL; + } + + return &(device->calibration->cam.cameras[index]); +} + +uint32_t device_imu_camera_get_num_of_sensors(const device_imu_camera_type *camera) { + if (!camera) { + return 0; + } + + return camera->num_sensors; +} + +const device_imu_camera_sensor_type* device_imu_camera_get_sensor(const device_imu_camera_type *camera, uint32_t index) { + if (!camera->sensors) { + return NULL; + } + + return &(camera->sensors[index]); +} + +device_imu_mat3x3_type device_imu_sensor_get_rotation(const device_imu_camera_sensor_type *sensor) { + device_imu_mat3x3_type rotation; + rotation.m[0] = sensor->cameraMisalignment.array[0][0]; + rotation.m[1] = sensor->cameraMisalignment.array[0][1]; + rotation.m[2] = sensor->cameraMisalignment.array[0][2]; + rotation.m[3] = sensor->cameraMisalignment.array[1][0]; + rotation.m[4] = sensor->cameraMisalignment.array[1][1]; + rotation.m[5] = sensor->cameraMisalignment.array[1][2]; + rotation.m[6] = sensor->cameraMisalignment.array[2][0]; + rotation.m[7] = sensor->cameraMisalignment.array[2][1]; + rotation.m[8] = sensor->cameraMisalignment.array[2][2]; + return rotation; +} + +device_imu_vec3_type device_imu_sensor_get_position(const device_imu_camera_sensor_type *sensor) { + device_imu_vec3_type position; + position.x = sensor->cameraOffset.axis.x; + position.y = sensor->cameraOffset.axis.y; + position.z = sensor->cameraOffset.axis.z; + return position; +} + +device_imu_size_type device_imu_sensor_get_resolution(const device_imu_camera_sensor_type *sensor) { + device_imu_size_type resolution; + resolution.width = sensor->resolution[0]; + resolution.height = sensor->resolution[1]; + return resolution; +} + +device_imu_vec2_type device_imu_sensor_get_cc(const device_imu_camera_sensor_type *sensor) { + device_imu_vec2_type cc; + cc.x = sensor->cc[0]; + cc.y = sensor->cc[1]; + return cc; +} + +device_imu_vec2_type device_imu_sensor_get_fc(const device_imu_camera_sensor_type *sensor) { + device_imu_vec2_type fc; + fc.x = sensor->fc[0]; + fc.y = sensor->fc[1]; + return fc; +} + +device_imu_error_type device_imu_sensor_get_kc(const device_imu_camera_sensor_type *sensor, uint32_t *num_kc, float *kc) { + if ((!sensor) || (!num_kc)) { + device_imu_error("Wrong argument"); + return DEVICE_IMU_ERROR_NO_ALLOCATION; + } + + if (!kc) { + *num_kc = sensor->num_kc; + } else { + uint32_t n = *num_kc; + + if (sensor->num_kc < n) { + n = sensor->num_kc; + } + + for (uint32_t i = 0; i < *num_kc; i++) { + kc[i] = sensor->kc[i]; + } + } + + return DEVICE_IMU_ERROR_NO_ERROR; +} + +device_imu_error_type device_imu_close(device_imu_type* device) { + if (!device) { + device_imu_error("No device"); + return DEVICE_IMU_ERROR_NO_DEVICE; + } + + if (device->calibration) { + if (device->calibration->cam.cameras) { + for (uint32_t i = 0; i < device->calibration->cam.num_cameras; i++) { + if (!device->calibration->cam.cameras[i].sensors) { + continue; + } + + for (uint32_t j = 0; j < device->calibration->cam.cameras[i].num_sensors; j++) { + if (!device->calibration->cam.cameras[i].sensors[j].kc) { + continue; + } + + free(device->calibration->cam.cameras[i].sensors[j].kc); + } + + free(device->calibration->cam.cameras[i].sensors); + } + + free(device->calibration->cam.cameras); + } + + free(device->calibration); + } + + if (device->ahrs) { + free(device->ahrs); + } + + if (device->offset) { + free(device->offset); + } + + if (device->handle) { + if ((!send_payload_msg_signal(device, DEVICE_IMU_MSG_START_IMU_DATA, 0x0)) || + (!recv_payload_msg(device, DEVICE_IMU_MSG_START_IMU_DATA, 0, NULL))) { + device_imu_error("Failed sending payload to stop imu data stream"); + } + + hid_close(device->handle); + } + + memset(device, 0, sizeof(device_imu_type)); + device_exit(); + + return DEVICE_IMU_ERROR_NO_ERROR; +} diff --git a/ardriver/xreal/device_imu.h b/ardriver/xreal/device_imu.h new file mode 100644 index 0000000..85ceb6e --- /dev/null +++ b/ardriver/xreal/device_imu.h @@ -0,0 +1,227 @@ +#pragma once +// +// Created by thejackimonster on 30.03.23. +// +// Copyright (c) 2023-2025 thejackimonster. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. +// + +#ifndef __cplusplus +#include +#endif + +#ifndef __cplusplus +#include +#else +#include +#endif + +#define DEVICE_IMU_MSG_GET_CAL_DATA_LENGTH 0x14 +#define DEVICE_IMU_MSG_CAL_DATA_GET_NEXT_SEGMENT 0x15 +#define DEVICE_IMU_MSG_ALLOCATE_CAL_DATA_BUFFER 0x16 +#define DEVICE_IMU_MSG_WRITE_CAL_DATA_SEGMENT 0x17 +#define DEVICE_IMU_MSG_FREE_CAL_BUFFER 0x18 +#define DEVICE_IMU_MSG_START_IMU_DATA 0x19 +#define DEVICE_IMU_MSG_GET_STATIC_ID 0x1A +#define DEVICE_IMU_MSG_UNKNOWN 0x1D + +#ifdef __cplusplus +extern "C" { +#endif + +enum device_imu_error_t { + DEVICE_IMU_ERROR_NO_ERROR = 0, + DEVICE_IMU_ERROR_NO_DEVICE = 1, + DEVICE_IMU_ERROR_NO_HANDLE = 2, + DEVICE_IMU_ERROR_NO_ALLOCATION = 3, + DEVICE_IMU_ERROR_WRONG_SIZE = 4, + DEVICE_IMU_ERROR_FILE_NOT_OPEN = 5, + DEVICE_IMU_ERROR_FILE_NOT_CLOSED = 6, + DEVICE_IMU_ERROR_LOADING_FAILED = 7, + DEVICE_IMU_ERROR_SAVING_FAILED = 8, + DEVICE_IMU_ERROR_UNPLUGGED = 9, + DEVICE_IMU_ERROR_UNEXPECTED = 10, + DEVICE_IMU_ERROR_WRONG_SIGNATURE = 11, + DEVICE_IMU_ERROR_INVALID_VALUE = 12, + DEVICE_IMU_ERROR_NOT_INITIALIZED = 13, + DEVICE_IMU_ERROR_PAYLOAD_FAILED = 14, + DEVICE_IMU_ERROR_UNKNOWN = 15, +}; + +struct __attribute__((__packed__)) device_imu_packet_t { + uint8_t signature [2]; + uint8_t temperature [2]; + uint64_t timestamp; + uint8_t angular_multiplier [2]; + uint8_t angular_divisor [4]; + uint8_t angular_velocity_x [3]; + uint8_t angular_velocity_y [3]; + uint8_t angular_velocity_z [3]; + uint8_t acceleration_multiplier [2]; + uint8_t acceleration_divisor [4]; + uint8_t acceleration_x [3]; + uint8_t acceleration_y [3]; + uint8_t acceleration_z [3]; + uint8_t magnetic_multiplier [2]; + uint8_t magnetic_divisor [4]; + uint8_t magnetic_x [2]; + uint8_t magnetic_y [2]; + uint8_t magnetic_z [2]; + uint32_t checksum; + uint8_t _padding [6]; +}; + +enum device_imu_event_t { + DEVICE_IMU_EVENT_UNKNOWN = 0, + DEVICE_IMU_EVENT_INIT = 1, + DEVICE_IMU_EVENT_UPDATE = 2, +}; + +struct device_imu_ahrs_t; +struct device_imu_camera_sensor_t; +struct device_imu_camera_t; +struct device_imu_camera_calibration_t; +struct device_imu_calibration_t; + +struct device_imu_vec2_t { + float x; + float y; +}; + +struct device_imu_vec3_t { + float x; + float y; + float z; +}; + +struct device_imu_quat_t { + float x; + float y; + float z; + float w; +}; + +struct device_imu_euler_t { + float roll; + float pitch; + float yaw; +}; + +struct device_imu_mat3x3_t { + float m [9]; +}; + +struct device_imu_size_t { + uint16_t width; + uint16_t height; +}; + +typedef enum device_imu_error_t device_imu_error_type; +typedef struct device_imu_packet_t device_imu_packet_type; +typedef enum device_imu_event_t device_imu_event_type; + +typedef struct device_imu_ahrs_t device_imu_ahrs_type; + +typedef struct device_imu_camera_sensor_t device_imu_camera_sensor_type; +typedef struct device_imu_camera_t device_imu_camera_type; +typedef struct device_imu_camera_calibration_t device_imu_camera_calibration_type; +typedef struct device_imu_calibration_t device_imu_calibration_type; + +typedef struct device_imu_vec2_t device_imu_vec2_type; +typedef struct device_imu_vec3_t device_imu_vec3_type; +typedef struct device_imu_quat_t device_imu_quat_type; +typedef struct device_imu_euler_t device_imu_euler_type; +typedef struct device_imu_mat3x3_t device_imu_mat3x3_type; +typedef struct device_imu_size_t device_imu_size_type; + +typedef void (*device_imu_event_callback)( + uint64_t timestamp, + device_imu_event_type event, + const device_imu_ahrs_type* ahrs +); + +struct device_imu_t { + uint16_t vendor_id; + uint16_t product_id; + + void* handle; + uint16_t max_payload_size; + + uint32_t static_id; + + uint64_t last_timestamp; + float temperature; // (in °C) + + void* offset; + device_imu_ahrs_type* ahrs; + + device_imu_event_callback callback; + device_imu_calibration_type* calibration; +}; + +typedef struct device_imu_t device_imu_type; + +device_imu_error_type device_imu_open(device_imu_type* device, device_imu_event_callback callback); + +device_imu_error_type device_imu_reset_calibration(device_imu_type* device); + +device_imu_error_type device_imu_load_calibration(device_imu_type* device, const char* path); + +device_imu_error_type device_imu_save_calibration(device_imu_type* device, const char* path); + +device_imu_error_type device_imu_clear(device_imu_type* device); + +device_imu_error_type device_imu_calibrate(device_imu_type* device, uint32_t iterations, bool gyro, bool accel, bool magnet); + +device_imu_error_type device_imu_read(device_imu_type* device, int timeout); + +device_imu_vec3_type device_imu_get_earth_acceleration(const device_imu_ahrs_type* ahrs); + +device_imu_vec3_type device_imu_get_linear_acceleration(const device_imu_ahrs_type* ahrs); + +device_imu_quat_type device_imu_get_orientation(const device_imu_ahrs_type* ahrs); + +device_imu_euler_type device_imu_get_euler(device_imu_quat_type quat); + +uint32_t device_imu_get_num_of_cameras(device_imu_type *device); + +const device_imu_camera_type* device_imu_get_camera(const device_imu_type *device, uint32_t index); + +uint32_t device_imu_camera_get_num_of_sensors(const device_imu_camera_type *camera); + +const device_imu_camera_sensor_type* device_imu_camera_get_sensor(const device_imu_camera_type *camera, uint32_t index); + +device_imu_mat3x3_type device_imu_sensor_get_rotation(const device_imu_camera_sensor_type *sensor); + +device_imu_vec3_type device_imu_sensor_get_position(const device_imu_camera_sensor_type *sensor); + +device_imu_size_type device_imu_sensor_get_resolution(const device_imu_camera_sensor_type *sensor); + +device_imu_vec2_type device_imu_sensor_get_cc(const device_imu_camera_sensor_type *sensor); + +device_imu_vec2_type device_imu_sensor_get_fc(const device_imu_camera_sensor_type *sensor); + +device_imu_error_type device_imu_sensor_get_kc(const device_imu_camera_sensor_type *sensor, uint32_t *num_kc, float *kc); + +device_imu_error_type device_imu_close(device_imu_type* device); + +#ifdef __cplusplus +} // extern "C" +#endif diff --git a/ardriver/xreal/device_mcu.c b/ardriver/xreal/device_mcu.c new file mode 100644 index 0000000..551617d --- /dev/null +++ b/ardriver/xreal/device_mcu.c @@ -0,0 +1,731 @@ +// +// Created by thejackimonster on 29.03.23. +// +// Copyright (c) 2023-2024 thejackimonster. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. +// + +#include "device_mcu.h" +#include "device.h" + +#include +#include +#include +#include +#include +#include + +#include + +#include "crc32.h" +#include "hid_ids.h" + +#ifndef NDEBUG +#define device_mcu_error(msg) fprintf(stderr, "ERROR: %s\n", msg) +#else +#define device_mcu_error(msg) (0) +#endif + +#define device_mcu_warning(msg) device_mcu_error(msg) + +#define MAX_PACKET_SIZE 64 +#define PACKET_HEAD 0xFD + +static bool send_payload(device_mcu_type* device, uint8_t size, const uint8_t* payload) { + int payload_size = size; + if (payload_size > MAX_PACKET_SIZE) { + payload_size = MAX_PACKET_SIZE; + } + + int transferred = hid_write(device->handle, payload, payload_size); + + if (transferred != payload_size) { + device_mcu_error("Sending payload failed"); + return false; + } + + return (transferred == size); +} + +static bool recv_payload(device_mcu_type* device, uint8_t size, uint8_t* payload) { + int payload_size = size; + if (payload_size > MAX_PACKET_SIZE) { + payload_size = MAX_PACKET_SIZE; + } + + int transferred = hid_read(device->handle, payload, payload_size); + + if (transferred >= payload_size) { + transferred = payload_size; + } + + if (transferred == 0) { + return false; + } + + if (transferred != payload_size) { + device_mcu_error("Receiving payload failed"); + return false; + } + + return (transferred == size); +} + +static bool send_payload_action(device_mcu_type* device, uint16_t msgid, uint8_t len, const uint8_t* data) { + static device_mcu_packet_type packet; + + const uint16_t packet_len = 17 + len; + const uint16_t payload_len = 5 + packet_len; + + packet.head = PACKET_HEAD; + packet.length = htole16(packet_len); + packet.timestamp = htole64(0); + packet.msgid = htole16(msgid); + memset(packet.reserved, 0, 5); + + memcpy(packet.data, data, len); + packet.checksum = htole32( + crc32_checksum( + (const uint8_t*) (&packet.length), + packet.length + ) + ); + + return send_payload(device, payload_len, (uint8_t*) (&packet)); +} + +static bool recv_payload_msg(device_mcu_type* device, uint16_t msgid, uint8_t len, uint8_t* data) { + static device_mcu_packet_type packet; + + packet.head = 0; + packet.length = 0; + packet.msgid = 0; + + const uint16_t packet_len = 18 + len; + const uint16_t payload_len = 5 + packet_len; + + if (!recv_payload(device, payload_len, (uint8_t*) (&packet))) { + return false; + } + + if (packet.head != PACKET_HEAD) { + device_mcu_error("Invalid payload received"); + return false; + } + + if (le16toh(packet.msgid) != msgid) { + device_mcu_error("Unexpected payload received"); + return false; + } + + const uint8_t status = packet.data[0]; + + if (status != 0) { + device_mcu_error("Payload status failed"); + return false; + } + + const uint16_t data_len = le16toh(packet.length) - 18; + + if (len <= data_len) { + memcpy(data, packet.data + 1, len); + } else { + memcpy(data, packet.data + 1, data_len); + memset(data + data_len, 0, len - data_len); + } + + return true; +} + +static bool do_payload_action(device_mcu_type* device, uint16_t msgid, uint8_t len, const uint8_t* data) { + if (!send_payload_action(device, msgid, len, data)) { + return false; + } + + const uint16_t attempts_per_second = (device->active? 60 : 1); + + uint16_t attempts = attempts_per_second * 5; + + while (attempts > 0) { + if (recv_payload_msg(device, msgid, 0, NULL)) { + return true; + } + + attempts--; + } + + return false; +} + +device_mcu_error_type device_mcu_open(device_mcu_type* device, device_mcu_event_callback callback) { + if (!device) { + device_mcu_error("No device"); + return DEVICE_MCU_ERROR_NO_DEVICE; + } + + memset(device, 0, sizeof(device_mcu_type)); + device->vendor_id = xreal_vendor_id; + device->product_id = 0; + device->callback = callback; + + if (!device_init()) { + device_mcu_error("Not initialized"); + return DEVICE_MCU_ERROR_NOT_INITIALIZED; + } + + struct hid_device_info* info = hid_enumerate( + device->vendor_id, + device->product_id + ); + + struct hid_device_info* it = info; + while (it) { + int interface_id = xreal_mcu_interface_id(it->product_id); + if (interface_id != -1 && it->interface_number == interface_id) { +#ifndef NDEBUG + printf("Found MCU device with product_id 0x%x on interface %d\n", it->product_id, interface_id); +#endif + device->product_id = it->product_id; + device->handle = hid_open_path(it->path); + break; + } + + it = it->next; + } + + hid_free_enumeration(info); + + if (!device->handle) { + device_mcu_error("No handle"); + return DEVICE_MCU_ERROR_NO_HANDLE; + } + + device_mcu_clear(device); + + if (!send_payload_action(device, DEVICE_MCU_MSG_R_ACTIVATION_TIME, 0, NULL)) { + device_mcu_error("Requesting activation time failed"); + return DEVICE_MCU_ERROR_PAYLOAD_FAILED; + } + + uint8_t activated; + if (!recv_payload_msg(device, DEVICE_MCU_MSG_R_ACTIVATION_TIME, 1, &activated)) { + device_mcu_error("Receiving activation time failed"); + return DEVICE_MCU_ERROR_PAYLOAD_FAILED; + } + + device->activated = (activated != 0); + + if (!device->activated) { + device_mcu_warning("Device is not activated"); + } + + if (!send_payload_action(device, DEVICE_MCU_MSG_R_MCU_APP_FW_VERSION, 0, NULL)) { + device_mcu_error("Requesting current MCU app firmware version"); + return DEVICE_MCU_ERROR_PAYLOAD_FAILED; + } + + if (!recv_payload_msg(device, DEVICE_MCU_MSG_R_MCU_APP_FW_VERSION, 41, (uint8_t*) device->mcu_app_fw_version)) { + device_mcu_error("Receiving current MCU app firmware version failed"); + return DEVICE_MCU_ERROR_PAYLOAD_FAILED; + } + + if (!send_payload_action(device, DEVICE_MCU_MSG_R_DP7911_FW_VERSION, 0, NULL)) { + device_mcu_error("Requesting current DP firmware version"); + return DEVICE_MCU_ERROR_PAYLOAD_FAILED; + } + + if (!recv_payload_msg(device, DEVICE_MCU_MSG_R_DP7911_FW_VERSION, 41, (uint8_t*) device->dp_fw_version)) { + device_mcu_error("Receiving current DP firmware version failed"); + return DEVICE_MCU_ERROR_PAYLOAD_FAILED; + } + + if (!send_payload_action(device, DEVICE_MCU_MSG_R_DSP_APP_FW_VERSION, 0, NULL)) { + device_mcu_error("Requesting current DSP app firmware version"); + return DEVICE_MCU_ERROR_PAYLOAD_FAILED; + } + + if (!recv_payload_msg(device, DEVICE_MCU_MSG_R_DSP_APP_FW_VERSION, 41, (uint8_t*) device->dsp_fw_version)) { + device_mcu_error("Receiving current DSP app firmware version failed"); + return DEVICE_MCU_ERROR_PAYLOAD_FAILED; + } + +#ifndef NDEBUG + printf("MCU: %s\n", device->mcu_app_fw_version); + printf("DP: %s\n", device->dp_fw_version); + printf("DSP: %s\n", device->dsp_fw_version); +#endif + + if (!send_payload_action(device, DEVICE_MCU_MSG_R_BRIGHTNESS, 0, NULL)) { + device_mcu_error("Requesting initial brightness failed"); + return DEVICE_MCU_ERROR_PAYLOAD_FAILED; + } + + if (!recv_payload_msg(device, DEVICE_MCU_MSG_R_BRIGHTNESS, 1, &device->brightness)) { + device_mcu_error("Receiving initial brightness failed"); + return DEVICE_MCU_ERROR_PAYLOAD_FAILED; + } + + if (!send_payload_action(device, DEVICE_MCU_MSG_R_DISP_MODE, 0, NULL)) { + device_mcu_error("Requesting display mode failed"); + return DEVICE_MCU_ERROR_PAYLOAD_FAILED; + } + + if (!recv_payload_msg(device, DEVICE_MCU_MSG_R_DISP_MODE, 1, &device->disp_mode)) { + device_mcu_error("Receiving display mode failed"); + return DEVICE_MCU_ERROR_PAYLOAD_FAILED; + } + +#ifndef NDEBUG + printf("Brightness: %d\n", device->brightness); + printf("Disp-Mode: %d\n", device->disp_mode); +#endif + + return DEVICE_MCU_ERROR_NO_ERROR; +} + +static void device_mcu_callback(device_mcu_type* device, + uint64_t timestamp, + device_mcu_event_type event, + uint8_t brightness, + const char* msg) { + if (!device->callback) { + return; + } + + device->callback(timestamp, event, brightness, msg); +} + +device_mcu_error_type device_mcu_clear(device_mcu_type* device) { + return device_mcu_read(device, 10); +} + +device_mcu_error_type device_mcu_read(device_mcu_type* device, int timeout) { + if (!device) { + device_mcu_error("No device"); + return DEVICE_MCU_ERROR_NO_DEVICE; + } + + if (!device->handle) { + device_mcu_error("No handle"); + return DEVICE_MCU_ERROR_NO_HANDLE; + } + + if (MAX_PACKET_SIZE != sizeof(device_mcu_packet_type)) { + device_mcu_error("Not proper size"); + return DEVICE_MCU_ERROR_WRONG_SIZE; + } + + device_mcu_packet_type packet; + memset(&packet, 0, sizeof(device_mcu_packet_type)); + + int transferred = hid_read_timeout( + device->handle, + (uint8_t*) &packet, + MAX_PACKET_SIZE, + timeout + ); + + if (transferred == -1) { + device_mcu_error("Device may be unplugged"); + return DEVICE_MCU_ERROR_UNPLUGGED; + } + + if (transferred == 0) { + return DEVICE_MCU_ERROR_NO_ERROR; + } + + if (MAX_PACKET_SIZE != transferred) { + device_mcu_error("Unexpected packet size"); + return DEVICE_MCU_ERROR_UNEXPECTED; + } + + if (packet.head != PACKET_HEAD) { + device_mcu_error("Wrong packet head"); + return DEVICE_MCU_ERROR_WRONG_HEAD; + } + + const uint32_t timestamp = le32toh(packet.timestamp); + const uint16_t msgid = le16toh(packet.msgid); + const uint16_t length = le16toh(packet.length); + + const size_t data_len = (size_t) &(packet.data) - (size_t) &(packet.length); + +#ifndef NDEBUG + printf("MSG: %d = %04x (%d)\n", msgid, msgid, length); + + if (length > 11) { + for (int i = 0; i < length - 11; i++) { + printf("%02x ", packet.data[i]); + } + + printf("\n"); + } +#endif + + switch (msgid) { + case DEVICE_MCU_MSG_P_START_HEARTBEAT: { + break; + } + case DEVICE_MCU_MSG_P_DISPLAY_TOGGLED: { + const uint8_t value = packet.data[0]; + + device->active = value; + + if (device->active) { + device_mcu_callback( + device, + timestamp, + DEVICE_MCU_EVENT_SCREEN_ON, + device->brightness, + NULL + ); + } else { + device_mcu_callback( + device, + timestamp, + DEVICE_MCU_EVENT_SCREEN_OFF, + device->brightness, + NULL + ); + } + break; + } + case DEVICE_MCU_MSG_P_BUTTON_PRESSED: { + const uint8_t phys_button = packet.data[0]; + const uint8_t virt_button = packet.data[4]; + const uint8_t value = packet.data[8]; + + switch (virt_button) { + case DEVICE_MCU_BUTTON_VIRT_DISPLAY_TOGGLE: + device->active = value; + + if (device->active) { + device_mcu_callback( + device, + timestamp, + DEVICE_MCU_EVENT_SCREEN_ON, + device->brightness, + NULL + ); + } else { + device_mcu_callback( + device, + timestamp, + DEVICE_MCU_EVENT_SCREEN_OFF, + device->brightness, + NULL + ); + } + break; + case DEVICE_MCU_BUTTON_VIRT_BRIGHTNESS_UP: + device->brightness = value; + + device_mcu_callback( + device, + timestamp, + DEVICE_MCU_EVENT_BRIGHTNESS_UP, + device->brightness, + NULL + ); + break; + case DEVICE_MCU_BUTTON_VIRT_BRIGHTNESS_DOWN: + device->brightness = value; + + device_mcu_callback( + device, + timestamp, + DEVICE_MCU_EVENT_BRIGHTNESS_DOWN, + device->brightness, + NULL + ); + break; + case DEVICE_MCU_BUTTON_VIRT_UP: + if (device->control_mode == DEVICE_MCU_CONTROL_MODE_VOLUME) + device_mcu_callback( + device, + timestamp, + DEVICE_MCU_EVENT_VOLUME_UP, + device->brightness, + NULL + ); + break; + case DEVICE_MCU_BUTTON_VIRT_DOWN: + if (device->control_mode == DEVICE_MCU_CONTROL_MODE_VOLUME) + device_mcu_callback( + device, + timestamp, + DEVICE_MCU_EVENT_VOLUME_DOWN, + device->brightness, + NULL + ); + break; + case DEVICE_MCU_BUTTON_VIRT_MODE_2D: + device_mcu_callback( + device, + timestamp, + DEVICE_MCU_EVENT_DISPLAY_MODE_2D, + device->brightness, + NULL + ); + break; + case DEVICE_MCU_BUTTON_VIRT_MODE_3D: + device_mcu_callback( + device, + timestamp, + DEVICE_MCU_EVENT_DISPLAY_MODE_3D, + device->brightness, + NULL + ); + break; + case DEVICE_MCU_BUTTON_VIRT_BLEND_CYCLE: + device->blend_state = value; + + device_mcu_callback( + device, + timestamp, + DEVICE_MCU_EVENT_BLEND_CYCLE, + device->brightness, + NULL + ); + break; + case DEVICE_MCU_BUTTON_VIRT_CONTROL_TOGGLE: + device->control_mode = value; + + device_mcu_callback( + device, + timestamp, + DEVICE_MCU_EVENT_CONTROL_TOGGLE, + device->brightness, + NULL + ); + break; + default: + break; + } + + break; + } + case DEVICE_MCU_MSG_P_ASYNC_TEXT_LOG: { + const char* text = packet.text; + const size_t text_len = strlen(text); + + device->active = true; + + if (data_len + text_len != length) { + device_mcu_error("Not matching length"); + return DEVICE_MCU_ERROR_INVALID_LENGTH; + } + + device_mcu_callback( + device, + timestamp, + DEVICE_MCU_EVENT_MESSAGE, + device->brightness, + text + ); + break; + } + case DEVICE_MCU_MSG_P_END_HEARTBEAT: { + break; + } + default: + device_mcu_callback( + device, + timestamp, + DEVICE_MCU_EVENT_UNKNOWN, + device->brightness, + NULL + ); + break; + } + + return DEVICE_MCU_ERROR_NO_ERROR; +} + +device_mcu_error_type device_mcu_poll_display_mode(device_mcu_type* device) { + if (!device) { + device_mcu_error("No device"); + return DEVICE_MCU_ERROR_NO_DEVICE; + } + + if (!device->handle) { + device_mcu_error("No handle"); + return DEVICE_MCU_ERROR_NO_HANDLE; + } + + if (!send_payload_action(device, DEVICE_MCU_MSG_R_DISP_MODE, 0, NULL)) { + device_mcu_error("Requesting display mode failed"); + return DEVICE_MCU_ERROR_PAYLOAD_FAILED; + } + + if (!recv_payload_msg(device, DEVICE_MCU_MSG_R_DISP_MODE, 1, &device->disp_mode)) { + device_mcu_error("Receiving display mode failed"); + return DEVICE_MCU_ERROR_PAYLOAD_FAILED; + } + + return DEVICE_MCU_ERROR_NO_ERROR; +} + +device_mcu_error_type device_mcu_update_display_mode(device_mcu_type* device) { + if (!device) { + device_mcu_error("No device"); + return DEVICE_MCU_ERROR_NO_DEVICE; + } + + if (!device->handle) { + device_mcu_error("No handle"); + return DEVICE_MCU_ERROR_NO_HANDLE; + } + + if (!do_payload_action(device, DEVICE_MCU_MSG_W_DISP_MODE, 1, &device->disp_mode)) { + device_mcu_error("Sending display mode failed"); + return DEVICE_MCU_ERROR_PAYLOAD_FAILED; + } + + return DEVICE_MCU_ERROR_NO_ERROR; +} + +device_mcu_error_type device_mcu_update_firmware(device_mcu_type* device, const char* path) { + if (!device) { + device_mcu_error("No device"); + return DEVICE_MCU_ERROR_NO_DEVICE; + } + + if (!device->handle) { + device_mcu_error("No handle"); + return DEVICE_MCU_ERROR_NO_HANDLE; + } + + if (!device->activated) { + device_mcu_error("Device is not activated"); + return DEVICE_MCU_ERROR_NO_ACTIVATION; + } + + size_t firmware_len = 0; + uint8_t* firmware = NULL; + + FILE* file = fopen(path, "rb"); + bool result = DEVICE_MCU_ERROR_UNKNOWN; + + if (file) { + fseek(file, 0, SEEK_END); + firmware_len = ftell(file); + + if (firmware_len > 0) { + firmware = (uint8_t*) malloc(firmware_len); + fread(firmware, sizeof(uint8_t), firmware_len, file); + } + + fclose(file); + } + + device_mcu_clear(device); + + printf("Prepare upload: %lu\n", firmware_len); + + if (!do_payload_action(device, DEVICE_MCU_MSG_W_UPDATE_MCU_APP_FW_PREPARE, 0, NULL)) { + device_mcu_error("Failed preparing the device for MCU firmware update!\n"); + goto cleanup; + } + + if (!do_payload_action(device, DEVICE_MCU_MSG_W_MCU_APP_JUMP_TO_BOOT, 0, NULL)) { + device_mcu_error("Failed mcu app jumping to boot"); + goto cleanup; + } + + if (!send_payload_action(device, DEVICE_MCU_MSG_R_ACTIVATION_TIME, 0, NULL)) { + device_mcu_error("Requesting activation time failed"); + goto cleanup; + } + + uint8_t activated; + if (!recv_payload_msg(device, DEVICE_MCU_MSG_R_ACTIVATION_TIME, 1, &activated)) { + device_mcu_error("Receiving activation time failed"); + goto cleanup; + } + + if (!activated) { + device_mcu_error("Device is not activated"); + goto jump_to_app; + } + + size_t offset = 0; + + while (offset < firmware_len) { + const size_t remaining = firmware_len - offset; + + printf("Upload: %lu / %lu\n", offset, firmware_len); + + uint8_t len; + uint16_t msgid; + if (offset == 0) { + len = remaining > 24? 24 : remaining; + msgid = DEVICE_MCU_MSG_W_UPDATE_MCU_APP_FW_START; + } else { + len = remaining > 42? 42 : remaining; + msgid = DEVICE_MCU_MSG_W_UPDATE_MCU_APP_FW_TRANSMIT; + } + + if (!do_payload_action(device, msgid, len, firmware + offset)) { + device_mcu_error("Failed sending firmware upload"); + goto jump_to_app; + } + + offset += len; + } + + printf("Finish upload"); + + if (!do_payload_action(device, DEVICE_MCU_MSG_W_UPDATE_MCU_APP_FW_FINISH, 0, NULL)) { + device_mcu_error("Failed finishing firmware upload"); + goto jump_to_app; + } + + result = DEVICE_MCU_ERROR_NO_ERROR; + +jump_to_app: + if (!do_payload_action(device, DEVICE_MCU_MSG_W_BOOT_JUMP_TO_APP, 0, NULL)) { + device_mcu_error("Failed boot jumping back to app"); + goto cleanup; + } + +cleanup: + if (firmware) { + free(firmware); + } + + return result; +} + +device_mcu_error_type device_mcu_close(device_mcu_type* device) { + if (!device) { + device_mcu_error("No device"); + return DEVICE_MCU_ERROR_NO_DEVICE; + } + + if (device->handle) { + hid_close(device->handle); + } + + memset(device, 0, sizeof(device_mcu_type)); + device_exit(); + + return DEVICE_MCU_ERROR_NO_ERROR; +} diff --git a/ardriver/xreal/device_mcu.h b/ardriver/xreal/device_mcu.h new file mode 100644 index 0000000..a253317 --- /dev/null +++ b/ardriver/xreal/device_mcu.h @@ -0,0 +1,212 @@ +#pragma once +// +// Created by thejackimonster on 29.03.23. +// +// Copyright (c) 2023-2024 thejackimonster. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. +// + +#ifndef __cplusplus +#include +#endif + +#ifndef __cplusplus +#include +#else +#include +#endif + +#define DEVICE_MCU_MSG_R_BRIGHTNESS 0x03 +#define DEVICE_MCU_MSG_W_BRIGHTNESS 0x04 +#define DEVICE_MCU_MSG_R_DISP_MODE 0x07 +#define DEVICE_MCU_MSG_W_DISP_MODE 0x08 + +#define DEVICE_MCU_MSG_R_GLASSID 0x15 +#define DEVICE_MCU_MSG_R_DP7911_FW_VERSION 0x16 +#define DEVICE_MCU_MSG_R_DSP_VERSION 0x18 +#define DEVICE_MCU_MSG_W_CANCEL_ACTIVATION 0x19 +#define DEVICE_MCU_MSG_P_HEARTBEAT 0x1A +#define DEVICE_MCU_MSG_W_SLEEP_TIME 0x1E + +#define DEVICE_MCU_MSG_R_DSP_APP_FW_VERSION 0x21 +#define DEVICE_MCU_MSG_R_MCU_APP_FW_VERSION 0x26 +#define DEVICE_MCU_MSG_R_ACTIVATION_TIME 0x29 +#define DEVICE_MCU_MSG_W_ACTIVATION_TIME 0x2A + +#define DEVICE_MCU_MSG_R_DP7911_FW_IS_UPDATE 0x3C +#define DEVICE_MCU_MSG_W_UPDATE_DP 0x3D +#define DEVICE_MCU_MSG_W_UPDATE_MCU_APP_FW_PREPARE 0x3E +#define DEVICE_MCU_MSG_W_UPDATE_MCU_APP_FW_START 0x3F + +#define DEVICE_MCU_MSG_W_UPDATE_MCU_APP_FW_TRANSMIT 0x40 +#define DEVICE_MCU_MSG_W_UPDATE_MCU_APP_FW_FINISH 0x41 +#define DEVICE_MCU_MSG_W_BOOT_JUMP_TO_APP 0x42 +#define DEVICE_MCU_MSG_W_MCU_APP_JUMP_TO_BOOT 0x44 +#define DEVICE_MCU_MSG_W_UPDATE_DSP_APP_FW_PREPARE 0x45 +#define DEVICE_MCU_MSG_W_UPDATE_DSP_APP_FW_START 0x46 +#define DEVICE_MCU_MSG_W_UPDATE_DSP_APP_FW_TRANSMIT 0x47 +#define DEVICE_MCU_MSG_W_UPDATE_DSP_APP_FW_FINISH 0x48 +#define DEVICE_MCU_MSG_R_IS_NEED_UPGRADE_DSP_FW 0x49 + +#define DEVICE_MCU_MSG_W_FORCE_UPGRADE_DSP_FW 0x69 + +#define DEVICE_MCU_MSG_W_BOOT_UPDATE_MODE 0x1100 +#define DEVICE_MCU_MSG_W_BOOT_UPDATE_CONFIRM 0x1101 +#define DEVICE_MCU_MSG_W_BOOT_UPDATE_PREPARE 0x1102 +#define DEVICE_MCU_MSG_W_BOOT_UPDATE_START 0x1103 +#define DEVICE_MCU_MSG_W_BOOT_UPDATE_TRANSMIT 0x1104 +#define DEVICE_MCU_MSG_W_BOOT_UPDATE_FINISH 0x1105 + +#define DEVICE_MCU_MSG_P_START_HEARTBEAT 0x6c02 +#define DEVICE_MCU_MSG_P_DISPLAY_TOGGLED 0x6C04 +#define DEVICE_MCU_MSG_P_BUTTON_PRESSED 0x6C05 +#define DEVICE_MCU_MSG_P_END_HEARTBEAT 0x6c12 +#define DEVICE_MCU_MSG_P_ASYNC_TEXT_LOG 0x6c09 + +#define DEVICE_MCU_MSG_E_DSP_ONE_PACKGE_WRITE_FINISH 0x6C0E +#define DEVICE_MCU_MSG_E_DSP_UPDATE_PROGRES 0x6C10 +#define DEVICE_MCU_MSG_E_DSP_UPDATE_ENDING 0x6C11 + +#define DEVICE_MCU_BUTTON_PHYS_DISPLAY_TOGGLE 0x1 +#define DEVICE_MCU_BUTTON_PHYS_BRIGHTNESS_UP 0x2 +#define DEVICE_MCU_BUTTON_PHYS_BRIGHTNESS_DOWN 0x3 + +#define DEVICE_MCU_DISPLAY_MODE_1920x1080_60 0x1 +#define DEVICE_MCU_DISPLAY_MODE_3840x1080_60_SBS 0x3 +#define DEVICE_MCU_DISPLAY_MODE_3840x1080_72_SBS 0x4 +#define DEVICE_MCU_DISPLAY_MODE_1920x1080_72 0x5 +#define DEVICE_MCU_DISPLAY_MODE_1920x1080_60_SBS 0x8 +#define DEVICE_MCU_DISPLAY_MODE_3840x1080_90_SBS 0x9 +#define DEVICE_MCU_DISPLAY_MODE_1920x1080_90 0xA +#define DEVICE_MCU_DISPLAY_MODE_1920x1080_120 0xB + +#define DEVICE_MCU_BUTTON_VIRT_DISPLAY_TOGGLE 0x1 +#define DEVICE_MCU_BUTTON_VIRT_BRIGHTNESS_UP 0x6 +#define DEVICE_MCU_BUTTON_VIRT_BRIGHTNESS_DOWN 0x7 +#define DEVICE_MCU_BUTTON_VIRT_UP 0x8 +#define DEVICE_MCU_BUTTON_VIRT_DOWN 0x9 +#define DEVICE_MCU_BUTTON_VIRT_MODE_2D 0xA +#define DEVICE_MCU_BUTTON_VIRT_MODE_3D 0xB +#define DEVICE_MCU_BUTTON_VIRT_BLEND_CYCLE 0xC +#define DEVICE_MCU_BUTTON_VIRT_CONTROL_TOGGLE 0xF + +#define DEVICE_MCU_BLEND_STATE_LOW 0x0 +#define DEVICE_MCU_BLEND_STATE_MEDIUM 0x2 +#define DEVICE_MCU_BLEND_STATE_FULL 0x3 + +#define DEVICE_MCU_CONTROL_MODE_BRIGHTNESS 0x0 +#define DEVICE_MCU_CONTROL_MODE_VOLUME 0x1 + +#ifdef __cplusplus +extern "C" { +#endif + +enum device_mcu_error_t { + DEVICE_MCU_ERROR_NO_ERROR = 0, + DEVICE_MCU_ERROR_NO_DEVICE = 1, + DEVICE_MCU_ERROR_NO_HANDLE = 2, + DEVICE_MCU_ERROR_NO_ACTIVATION = 3, + DEVICE_MCU_ERROR_WRONG_SIZE = 4, + DEVICE_MCU_ERROR_UNPLUGGED = 5, + DEVICE_MCU_ERROR_UNEXPECTED = 6, + DEVICE_MCU_ERROR_WRONG_HEAD = 7, + DEVICE_MCU_ERROR_INVALID_LENGTH = 8, + DEVICE_MCU_ERROR_NOT_INITIALIZED = 9, + DEVICE_MCU_ERROR_PAYLOAD_FAILED = 10, + DEVICE_MCU_ERROR_UNKNOWN = 11, +}; + +struct __attribute__((__packed__)) device_mcu_packet_t { + uint8_t head; + uint32_t checksum; + uint16_t length; + uint64_t timestamp; + uint16_t msgid; + uint8_t reserved [5]; + union { + char text [42]; + uint8_t data [42]; + }; +}; + +enum device_mcu_event_t { + DEVICE_MCU_EVENT_UNKNOWN = 0, + DEVICE_MCU_EVENT_SCREEN_ON = 1, + DEVICE_MCU_EVENT_SCREEN_OFF = 2, + DEVICE_MCU_EVENT_BRIGHTNESS_UP = 3, + DEVICE_MCU_EVENT_BRIGHTNESS_DOWN = 4, + DEVICE_MCU_EVENT_MESSAGE = 5, + DEVICE_MCU_EVENT_DISPLAY_MODE_2D = 6, + DEVICE_MCU_EVENT_DISPLAY_MODE_3D = 7, + DEVICE_MCU_EVENT_BLEND_CYCLE = 8, + DEVICE_MCU_EVENT_CONTROL_TOGGLE = 9, + DEVICE_MCU_EVENT_VOLUME_UP = 10, + DEVICE_MCU_EVENT_VOLUME_DOWN = 11, +}; + +typedef enum device_mcu_error_t device_mcu_error_type; +typedef struct device_mcu_packet_t device_mcu_packet_type; +typedef enum device_mcu_event_t device_mcu_event_type; +typedef void (*device_mcu_event_callback)( + uint64_t timestamp, + device_mcu_event_type event, + uint8_t brightness, + const char* msg +); + +struct device_mcu_t { + uint16_t vendor_id; + uint16_t product_id; + + void* handle; + + bool activated; + char mcu_app_fw_version [42]; + char dp_fw_version [42]; + char dsp_fw_version [42]; + + bool active; + uint8_t brightness; + uint8_t disp_mode; + uint8_t blend_state; + uint8_t control_mode; + + device_mcu_event_callback callback; +}; + +typedef struct device_mcu_t device_mcu_type; + +device_mcu_error_type device_mcu_open(device_mcu_type* device, device_mcu_event_callback callback); + +device_mcu_error_type device_mcu_clear(device_mcu_type* device); + +device_mcu_error_type device_mcu_read(device_mcu_type* device, int timeout); + +device_mcu_error_type device_mcu_poll_display_mode(device_mcu_type* device); + +device_mcu_error_type device_mcu_update_display_mode(device_mcu_type* device); + +device_mcu_error_type device_mcu_update_firmware(device_mcu_type* device, const char* path); + +device_mcu_error_type device_mcu_close(device_mcu_type* device); + +#ifdef __cplusplus +} // extern "C" +#endif diff --git a/ardriver/xreal/hid_ids.c b/ardriver/xreal/hid_ids.c new file mode 100644 index 0000000..b913d1b --- /dev/null +++ b/ardriver/xreal/hid_ids.c @@ -0,0 +1,108 @@ +// +// Created by wheaney on 12.11.23. +// +// Copyright (c) 2023-2024 thejackimonster. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. +// + +#include "hid_ids.h" + +#ifndef __cplusplus +#include +#endif + +#ifndef __cplusplus +#include +#else +#include +#endif + +const uint16_t xreal_vendor_id = 0x3318; +const uint16_t xreal_product_ids[NUM_SUPPORTED_PRODUCTS] = { + 0x0424, // XREAL Air + 0x0428, // XREAL Air 2 + 0x0432, // XREAL Air 2 Pro + 0x0426 // XREAL Air 2 Ultra +}; + +const int xreal_imu_interface_ids[NUM_SUPPORTED_PRODUCTS] = { + 3, // XREAL Air + 3, // XREAL Air 2 + 3, // XREAL Air 2 Pro + 2 // XREAL Air 2 Ultra +}; + +const int xreal_mcu_interface_ids[NUM_SUPPORTED_PRODUCTS] = { + 4, // XREAL Air + 4, // XREAL Air 2 + 4, // XREAL Air 2 Pro + 0 // XREAL Air 2 Ultra MCU +}; + +const uint16_t xreal_imu_max_payload_sizes[NUM_SUPPORTED_PRODUCTS] = { + 64, // XREAL Air + 64, // XREAL Air 2 + 64, // XREAL Air 2 Pro + 512 // XREAL Air 2 Ultra +}; + +static int xreal_product_index(uint16_t product_id) { + for (int i = 0; i < NUM_SUPPORTED_PRODUCTS; i++) { + if (xreal_product_ids[i] == product_id) { + return i; + } + } + + return -1; +} + +bool is_xreal_product_id(uint16_t product_id) { + return xreal_product_index(product_id) >= 0; +} + +int xreal_imu_interface_id(uint16_t product_id) { + const int index = xreal_product_index(product_id); + + if (index >= 0) { + return xreal_imu_interface_ids[index]; + } else { + return -1; + } +} + +int xreal_mcu_interface_id(uint16_t product_id) { + const int index = xreal_product_index(product_id); + + if (index >= 0) { + return xreal_mcu_interface_ids[index]; + } else { + return -1; + } +} + +uint16_t xreal_imu_max_payload_size(uint16_t product_id) { + const int index = xreal_product_index(product_id); + + if (index >= 0) { + return xreal_imu_max_payload_sizes[index]; + } else { + return 0; + } +} diff --git a/ardriver/xreal/hid_ids.h b/ardriver/xreal/hid_ids.h new file mode 100644 index 0000000..7c6aa5c --- /dev/null +++ b/ardriver/xreal/hid_ids.h @@ -0,0 +1,54 @@ +#pragma once +// +// Created by wheaney on 20.11.23. +// +// Copyright (c) 2023-2024 thejackimonster. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. +// + +#ifndef __cplusplus +#include +#endif + +#ifndef __cplusplus +#include +#else +#include +#endif + +#define NUM_SUPPORTED_PRODUCTS 4 + +#ifdef __cplusplus +extern "C" { +#endif + +extern const uint16_t xreal_vendor_id; +extern const uint16_t xreal_product_ids [NUM_SUPPORTED_PRODUCTS]; + +bool is_xreal_product_id(uint16_t product_id); + +int xreal_imu_interface_id(uint16_t product_id); +int xreal_mcu_interface_id(uint16_t product_id); + +uint16_t xreal_imu_max_payload_size(uint16_t product_id); + +#ifdef __cplusplus +} // extern "C" +#endif diff --git a/ardriver/xreal/xreal.go b/ardriver/xreal/xreal.go new file mode 100644 index 0000000..a99fe12 --- /dev/null +++ b/ardriver/xreal/xreal.go @@ -0,0 +1,12 @@ +//go:build xreal +// +build xreal + +package xreal + +// #include "evdi_lib.h" +// #include "go_ffi.h" +// #cgo CFLAGS: -w +// #cgo pkg-config: json-c libusb-1.0 hidapi-libusb +import "C" + +var IsXrealEnabled = true diff --git a/ardriver/xreal/xreal_disabled.go b/ardriver/xreal/xreal_disabled.go new file mode 100644 index 0000000..1cbe85c --- /dev/null +++ b/ardriver/xreal/xreal_disabled.go @@ -0,0 +1,6 @@ +//go:build !xreal +// +build !xreal + +package xreal + +var IsXrealEnabled = false From 243d595a3585a09f4104d5a35b441038a0ebdfc9 Mon Sep 17 00:00:00 2001 From: imterah Date: Tue, 24 Jun 2025 16:30:16 -0400 Subject: [PATCH 11/12] feature: Get sensor data reading working --- .zed/settings.json | 13 ++ Makefile | 2 +- app/main.go | 7 +- app/renderer/renderer.go | 51 +++++ ardriver/ardriver.go | 22 ++ ardriver/xreal/device_imu.c | 280 +++++++++++++------------- ardriver/xreal/go_ffi.c | 7 + ardriver/xreal/go_ffi.h | 3 + ardriver/xreal/xreal.go | 130 +++++++++++- ardriver/xreal/xreal_debug_logging.go | 7 + ardriver/xreal/xreal_disabled.go | 38 ++++ 11 files changed, 413 insertions(+), 147 deletions(-) create mode 100644 .zed/settings.json create mode 100644 app/renderer/renderer.go create mode 100644 ardriver/xreal/go_ffi.c create mode 100644 ardriver/xreal/go_ffi.h create mode 100644 ardriver/xreal/xreal_debug_logging.go diff --git a/.zed/settings.json b/.zed/settings.json new file mode 100644 index 0000000..11ceaea --- /dev/null +++ b/.zed/settings.json @@ -0,0 +1,13 @@ +// Folder-specific settings +// +// For a full list of overridable settings, and general information on folder-specific settings, +// see the documentation: https://zed.dev/docs/configuring-zed#settings-files +{ + "lsp": { + "gopls": { + "initialization_options": { + "buildFlags": ["-tags=xreal,noaudio,drm,drm_leasing,drm_disable_input"] + } + } + } +} diff --git a/Makefile b/Makefile index 822f8a8..0d040fb 100644 --- a/Makefile +++ b/Makefile @@ -1,6 +1,6 @@ APP_DIR := ./app OUTPUT := uxr -TAGS := xreal drm drm_leasing drm_disable_input +TAGS := xreal noaudio drm drm_leasing drm_disable_input .PHONY: all build clean diff --git a/app/main.go b/app/main.go index 8a6c89c..cd762c0 100644 --- a/app/main.go +++ b/app/main.go @@ -132,7 +132,7 @@ func mainEntrypoint(context.Context, *cli.Command) error { }, }) - displayMetadataBlock := make([]*renderer.EvdiDisplayMetadata, *config.DisplayConfig.Count) + evdiCards := make([]*renderer.EvdiDisplayMetadata, *config.DisplayConfig.Count) for currentDisplay := range *config.DisplayConfig.Count { openedDevice, err := libevdi.Open(nil) @@ -156,13 +156,16 @@ func mainEntrypoint(context.Context, *cli.Command) error { displayBuffer := openedDevice.CreateBuffer(displayMetadata.MaxWidth, displayMetadata.MaxHeight, 4, displayRect) - displayMetadataBlock[currentDisplay] = &renderer.EvdiDisplayMetadata{ + evdiCards[currentDisplay] = &renderer.EvdiDisplayMetadata{ EvdiNode: openedDevice, Rect: displayRect, Buffer: displayBuffer, } } + log.Info("Initialized displays. Entering rendering loop") + renderer.EnterRenderLoop(config, displayMetadata, evdiCards) + atexit.Exit(0) return nil } diff --git a/app/renderer/renderer.go b/app/renderer/renderer.go new file mode 100644 index 0000000..e3cb39a --- /dev/null +++ b/app/renderer/renderer.go @@ -0,0 +1,51 @@ +package renderer + +import ( + "time" + + libconfig "git.terah.dev/UnrealXR/unrealxr/app/config" + "git.terah.dev/UnrealXR/unrealxr/app/edidtools" + "git.terah.dev/UnrealXR/unrealxr/ardriver" + arcommons "git.terah.dev/UnrealXR/unrealxr/ardriver/commons" + "github.com/charmbracelet/log" + "github.com/tebeka/atexit" +) + +func EnterRenderLoop(config *libconfig.Config, displayMetadata *edidtools.DisplayMetadata, evdiCards []*EvdiDisplayMetadata) { + log.Info("Initializing AR driver") + headset, err := ardriver.GetDevice() + + if err != nil { + log.Errorf("Failed to get device: %s", err.Error()) + atexit.Exit(1) + } + log.Info("Initialized") + + var pitch float32 + var yaw float32 + var roll float32 + + arEventListner := &arcommons.AREventListener{ + PitchCallback: func(newPitch float32) { + pitch = newPitch + }, + YawCallback: func(newYaw float32) { + yaw = newYaw + }, + RollCallback: func(newRoll float32) { + roll = newRoll + }, + } + + if headset.IsPollingLibrary() { + log.Error("Connected AR headset requires polling but polling is not implemented in the renderer!") + atexit.Exit(1) + } + + headset.RegisterEventListeners(arEventListner) + + for { + log.Debugf("pitch: %f, yaw: %f, roll: %f", pitch, yaw, roll) + time.Sleep(10 * time.Millisecond) + } +} diff --git a/ardriver/ardriver.go b/ardriver/ardriver.go index b58104c..913e40a 100644 --- a/ardriver/ardriver.go +++ b/ardriver/ardriver.go @@ -1 +1,23 @@ package ardriver + +import ( + "fmt" + + "git.terah.dev/UnrealXR/unrealxr/ardriver/commons" + "git.terah.dev/UnrealXR/unrealxr/ardriver/xreal" +) + +func GetDevice() (commons.ARDevice, error) { + if xreal.IsXrealEnabled { + device, err := xreal.New() + + if err != nil { + fmt.Printf("failed to initialize xreal device: %w\n", err) + return nil, err + } + + return device, nil + } + + return nil, fmt.Errorf("failed to initialize any device") +} diff --git a/ardriver/xreal/device_imu.c b/ardriver/xreal/device_imu.c index add1338..1ed84bd 100644 --- a/ardriver/xreal/device_imu.c +++ b/ardriver/xreal/device_imu.c @@ -9,10 +9,10 @@ // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell // copies of the Software, and to permit persons to whom the Software is // furnished to do so, subject to the following conditions: -// +// // The above copyright notice and this permission notice shall be included in // all copies or substantial portions of the Software. -// +// // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE @@ -25,8 +25,8 @@ #include "device_imu.h" #include "device.h" -#include -#include +#include +#include #include #include #include @@ -35,7 +35,7 @@ #include #include -#include +#include #include #include @@ -78,18 +78,18 @@ struct device_imu_calibration_t { FusionMatrix gyroscopeMisalignment; FusionVector gyroscopeSensitivity; FusionVector gyroscopeOffset; - + FusionMatrix accelerometerMisalignment; FusionVector accelerometerSensitivity; FusionVector accelerometerOffset; - + FusionMatrix magnetometerMisalignment; FusionVector magnetometerSensitivity; FusionVector magnetometerOffset; - + FusionMatrix softIronMatrix; FusionVector hardIronOffset; - + FusionQuaternion noises; device_imu_camera_calibration_type cam; @@ -100,14 +100,14 @@ static bool send_payload(device_imu_type* device, uint16_t size, const uint8_t* if (payload_size > device->max_payload_size) { payload_size = device->max_payload_size; } - + int transferred = hid_write(device->handle, payload, payload_size); - + if (transferred != payload_size) { device_imu_error("Sending payload failed"); return false; } - + return (transferred == size); } @@ -116,9 +116,9 @@ static bool recv_payload(device_imu_type* device, uint16_t size, uint8_t* payloa if (payload_size > device->max_payload_size) { payload_size = device->max_payload_size; } - + int transferred = hid_read(device->handle, payload, payload_size); - + if (transferred >= payload_size) { transferred = payload_size; } @@ -126,12 +126,12 @@ static bool recv_payload(device_imu_type* device, uint16_t size, uint8_t* payloa if (transferred == 0) { return false; } - + if (transferred != payload_size) { device_imu_error("Receiving payload failed"); return false; } - + return (transferred == size); } @@ -147,14 +147,14 @@ typedef struct device_imu_payload_packet_t device_imu_payload_packet_type; static bool send_payload_msg(device_imu_type* device, uint8_t msgid, uint16_t len, const uint8_t* data) { static device_imu_payload_packet_type packet; - + const uint16_t packet_len = 3 + len; const uint16_t payload_len = 5 + packet_len; - + packet.head = 0xAA; packet.length = htole16(packet_len); packet.msgid = msgid; - + memcpy(packet.data, data, len); packet.checksum = htole32( crc32_checksum( @@ -162,7 +162,7 @@ static bool send_payload_msg(device_imu_type* device, uint8_t msgid, uint16_t le packet.length ) ); - + return send_payload(device, payload_len, (uint8_t*) (&packet)); } @@ -172,20 +172,20 @@ static bool send_payload_msg_signal(device_imu_type* device, uint8_t msgid, uint static bool recv_payload_msg(device_imu_type* device, uint8_t msgid, uint16_t len, uint8_t* data) { static device_imu_payload_packet_type packet; - + packet.head = 0; packet.length = 0; packet.msgid = 0; - + const uint16_t packet_len = 3 + len; const uint16_t payload_len = 5 + packet_len; - + do { if (!recv_payload(device, payload_len, (uint8_t*) (&packet))) { return false; } } while (packet.msgid != msgid); - + memcpy(data, packet.data, len); return true; } @@ -195,7 +195,7 @@ static FusionVector json_object_get_vector(struct json_object* obj) { (json_object_array_length(obj) != 3)) { return FUSION_VECTOR_ZERO; } - + FusionVector vector; vector.axis.x = (float) json_object_get_double(json_object_array_get_idx(obj, 0)); vector.axis.y = (float) json_object_get_double(json_object_array_get_idx(obj, 1)); @@ -208,7 +208,7 @@ static FusionQuaternion json_object_get_quaternion(struct json_object* obj) { (json_object_array_length(obj) != 4)) { return FUSION_IDENTITY_QUATERNION; } - + FusionQuaternion quaternion; quaternion.element.x = (float) json_object_get_double(json_object_array_get_idx(obj, 0)); quaternion.element.y = (float) json_object_get_double(json_object_array_get_idx(obj, 1)); @@ -284,7 +284,7 @@ static void init_device_imu_camera(device_imu_camera_type *camera, json_object * float* cc = sensor->cc; float* fc = sensor->fc; float* kc = sensor->kc; - + json_object_get_array_u16(json_object_object_get(dev, "resolution"), &resolution, 2); json_object_get_array_f32(json_object_object_get(dev, "cc"), &cc, 2); @@ -303,19 +303,19 @@ device_imu_error_type device_imu_open(device_imu_type* device, device_imu_event_ device_imu_error("No device"); return DEVICE_IMU_ERROR_NO_DEVICE; } - + memset(device, 0, sizeof(device_imu_type)); device->vendor_id = xreal_vendor_id; device->product_id = 0; device->callback = callback; - + if (!device_init()) { device_imu_error("Not initialized"); return DEVICE_IMU_ERROR_NOT_INITIALIZED; } struct hid_device_info* info = hid_enumerate( - device->vendor_id, + device->vendor_id, device->product_id ); @@ -336,7 +336,7 @@ device_imu_error_type device_imu_open(device_imu_type* device, device_imu_event_ } hid_free_enumeration(info); - + if (!device->handle) { device_imu_error("No handle"); return DEVICE_IMU_ERROR_NO_HANDLE; @@ -349,19 +349,19 @@ device_imu_error_type device_imu_open(device_imu_type* device, device_imu_event_ } device_imu_clear(device); - + if (!send_payload_msg(device, DEVICE_IMU_MSG_GET_STATIC_ID, 0, NULL)) { device_imu_error("Failed sending payload to get static id"); return DEVICE_IMU_ERROR_PAYLOAD_FAILED; } - + uint32_t static_id = 0; if (recv_payload_msg(device, DEVICE_IMU_MSG_GET_STATIC_ID, 4, (uint8_t*) &static_id)) { device->static_id = static_id; } else { device->static_id = 0x20220101; } - + device->calibration = malloc(sizeof(device_imu_calibration_type)); memset(device->calibration, 0, sizeof(device_imu_calibration_type)); @@ -371,12 +371,12 @@ device_imu_error_type device_imu_open(device_imu_type* device, device_imu_event_ device_imu_error("Failed sending payload to get calibration data length"); return DEVICE_IMU_ERROR_PAYLOAD_FAILED; } - + uint32_t calibration_len = 0; if (recv_payload_msg(device, DEVICE_IMU_MSG_GET_CAL_DATA_LENGTH, 4, (uint8_t*) &calibration_len)) { const uint16_t max_packet_size = (device->max_payload_size - 8); char* calibration_data = malloc(calibration_len + 1); - + uint32_t position = 0; while (position < calibration_len) { const uint32_t remaining = (calibration_len - position); @@ -384,23 +384,23 @@ device_imu_error_type device_imu_open(device_imu_type* device, device_imu_event_ if (!send_payload_msg(device, DEVICE_IMU_MSG_CAL_DATA_GET_NEXT_SEGMENT, 0, NULL)) { break; } - + const uint16_t next = (remaining > max_packet_size? max_packet_size : remaining); - + if (!recv_payload_msg(device, DEVICE_IMU_MSG_CAL_DATA_GET_NEXT_SEGMENT, next, (uint8_t*) calibration_data + position)) { break; } - + position += next; } calibration_data[calibration_len] = '\0'; - + struct json_tokener* tokener = json_tokener_new(); struct json_object* root = json_tokener_parse_ex(tokener, calibration_data, calibration_len); struct json_object* imu = json_object_object_get(root, "IMU"); struct json_object* dev1 = json_object_object_get(imu, "device_1"); - + FusionVector accel_bias = json_object_get_vector(json_object_object_get(dev1, "accel_bias")); FusionQuaternion accel_q_gyro = json_object_get_quaternion(json_object_object_get(dev1, "accel_q_gyro")); FusionVector gyro_bias = json_object_get_vector(json_object_object_get(dev1, "gyro_bias")); @@ -412,19 +412,19 @@ device_imu_error_type device_imu_open(device_imu_type* device, device_imu_event_ FusionVector scale_mag = json_object_get_vector(json_object_object_get(dev1, "scale_mag")); const FusionQuaternion accel_q_mag = FusionQuaternionMultiply(accel_q_gyro, gyro_q_mag); - + device->calibration->gyroscopeMisalignment = FusionQuaternionToMatrix(accel_q_gyro); device->calibration->gyroscopeSensitivity = scale_gyro; device->calibration->gyroscopeOffset = gyro_bias; - + device->calibration->accelerometerMisalignment = FUSION_IDENTITY_MATRIX; device->calibration->accelerometerSensitivity = scale_accel; device->calibration->accelerometerOffset = accel_bias; - + device->calibration->magnetometerMisalignment = FusionQuaternionToMatrix(accel_q_mag); device->calibration->magnetometerSensitivity = scale_mag; device->calibration->magnetometerOffset = mag_bias; - + device->calibration->noises = imu_noises; struct json_object* rgb = json_object_object_get(root, "RGB_camera"); @@ -452,7 +452,7 @@ device_imu_error_type device_imu_open(device_imu_type* device, device_imu_event_ device->calibration->cam.num_cameras = cameras? num_cameras : 0; device->calibration->cam.cameras = cameras; - + json_tokener_free(tokener); free(calibration_data); } @@ -464,16 +464,16 @@ device_imu_error_type device_imu_open(device_imu_type* device, device_imu_event_ } const uint32_t SAMPLE_RATE = 1000; - + device->offset = malloc(sizeof(FusionOffset)); device->ahrs = malloc(sizeof(FusionAhrs)); - + if (device->offset) { FusionOffsetInitialise((FusionOffset*) device->offset, SAMPLE_RATE); } FusionAhrsInitialise((FusionAhrs*) device->ahrs); - + const FusionAhrsSettings settings = { .convention = FusionConventionNed, .gain = 0.5f, @@ -481,7 +481,7 @@ device_imu_error_type device_imu_open(device_imu_type* device, device_imu_event_ .magneticRejection = 20.0f, .recoveryTriggerPeriod = 5 * SAMPLE_RATE, /* 5 seconds */ }; - + FusionAhrsSetSettings((FusionAhrs*) device->ahrs, &settings); return DEVICE_IMU_ERROR_NO_ERROR; } @@ -491,27 +491,27 @@ device_imu_error_type device_imu_reset_calibration(device_imu_type* device) { device_imu_error("No device"); return DEVICE_IMU_ERROR_NO_DEVICE; } - + if (!device->calibration) { device_imu_error("Not allocated"); return DEVICE_IMU_ERROR_NO_ALLOCATION; } - + device->calibration->gyroscopeMisalignment = FUSION_IDENTITY_MATRIX; device->calibration->gyroscopeSensitivity = FUSION_VECTOR_ONES; device->calibration->gyroscopeOffset = FUSION_VECTOR_ZERO; - + device->calibration->accelerometerMisalignment = FUSION_IDENTITY_MATRIX; device->calibration->accelerometerSensitivity = FUSION_VECTOR_ONES; device->calibration->accelerometerOffset = FUSION_VECTOR_ZERO; - + device->calibration->magnetometerMisalignment = FUSION_IDENTITY_MATRIX; device->calibration->magnetometerSensitivity = FUSION_VECTOR_ONES; device->calibration->magnetometerOffset = FUSION_VECTOR_ZERO; - + device->calibration->softIronMatrix = FUSION_IDENTITY_MATRIX; device->calibration->hardIronOffset = FUSION_VECTOR_ZERO; - + device->calibration->noises = FUSION_IDENTITY_QUATERNION; device->calibration->noises.element.w = 0.0f; @@ -550,12 +550,12 @@ device_imu_error_type device_imu_load_calibration(device_imu_type* device, const device_imu_error("No device"); return DEVICE_IMU_ERROR_NO_DEVICE; } - + if (!device->calibration) { device_imu_error("Not allocated"); return DEVICE_IMU_ERROR_NO_ALLOCATION; } - + FILE* file = fopen(path, "rb"); if (!file) { device_imu_error("No file opened"); @@ -566,20 +566,20 @@ device_imu_error_type device_imu_load_calibration(device_imu_type* device, const const size_t calibration_size = ( sizeof(device_imu_calibration_type) - sizeof(device_imu_camera_calibration_type) ); - + size_t count; count = fread(device->calibration, 1, calibration_size, file); - + if (calibration_size != count) { device_imu_error("Not fully loaded"); result = DEVICE_IMU_ERROR_LOADING_FAILED; } - + if (0 != fclose(file)) { device_imu_error("No file closed"); return DEVICE_IMU_ERROR_FILE_NOT_CLOSED; } - + return result; } @@ -588,12 +588,12 @@ device_imu_error_type device_imu_save_calibration(device_imu_type* device, const device_imu_error("No device"); return DEVICE_IMU_ERROR_NO_DEVICE; } - + if (!device->calibration) { device_imu_error("Not allocated"); return DEVICE_IMU_ERROR_NO_ALLOCATION; } - + FILE* file = fopen(path, "wb"); if (!file) { device_imu_error("No file opened"); @@ -604,20 +604,20 @@ device_imu_error_type device_imu_save_calibration(device_imu_type* device, const const size_t calibration_size = ( sizeof(device_imu_calibration_type) - sizeof(device_imu_camera_calibration_type) ); - + size_t count; count = fwrite(device->calibration, 1, calibration_size, file); - + if (calibration_size != count) { device_imu_error("Not fully saved"); result = DEVICE_IMU_ERROR_SAVING_FAILED; } - + if (0 != fclose(file)) { device_imu_error("No file closed"); return DEVICE_IMU_ERROR_FILE_NOT_CLOSED; } - + return result; } @@ -627,7 +627,7 @@ static void device_imu_callback(device_imu_type* device, if (!device->callback) { return; } - + device->callback(timestamp, event, device->ahrs); } @@ -668,33 +668,33 @@ static void readIMU_from_packet(const device_imu_packet_type* packet, FusionVector* magnetometer) { int32_t vel_m = pack16bit_signed(packet->angular_multiplier); int32_t vel_d = pack32bit_signed(packet->angular_divisor); - + int32_t vel_x = pack24bit_signed(packet->angular_velocity_x); int32_t vel_y = pack24bit_signed(packet->angular_velocity_y); int32_t vel_z = pack24bit_signed(packet->angular_velocity_z); - + gyroscope->axis.x = (float) vel_x * (float) vel_m / (float) vel_d; gyroscope->axis.y = (float) vel_y * (float) vel_m / (float) vel_d; gyroscope->axis.z = (float) vel_z * (float) vel_m / (float) vel_d; - + int32_t accel_m = pack16bit_signed(packet->acceleration_multiplier); int32_t accel_d = pack32bit_signed(packet->acceleration_divisor); - + int32_t accel_x = pack24bit_signed(packet->acceleration_x); int32_t accel_y = pack24bit_signed(packet->acceleration_y); int32_t accel_z = pack24bit_signed(packet->acceleration_z); - + accelerometer->axis.x = (float) accel_x * (float) accel_m / (float) accel_d; accelerometer->axis.y = (float) accel_y * (float) accel_m / (float) accel_d; accelerometer->axis.z = (float) accel_z * (float) accel_m / (float) accel_d; - + int32_t magnet_m = pack16bit_signed_swap(packet->magnetic_multiplier); int32_t magnet_d = pack32bit_signed_swap(packet->magnetic_divisor); - + int16_t magnet_x = pack16bit_signed_bizarre(packet->magnetic_x); int16_t magnet_y = pack16bit_signed_bizarre(packet->magnetic_y); int16_t magnet_z = pack16bit_signed_bizarre(packet->magnetic_z); - + magnetometer->axis.x = (float) magnet_x * (float) magnet_m / (float) magnet_d; magnetometer->axis.y = (float) magnet_y * (float) magnet_m / (float) magnet_d; magnetometer->axis.z = (float) magnet_z * (float) magnet_m / (float) magnet_d; @@ -719,17 +719,17 @@ static void iterate_iron_offset_estimation(const FusionVector* magnetometer, Fus max.array[i] = max(max.array[i], magnetometer->array[i]); min.array[i] = min(min.array[i], magnetometer->array[i]); } - + const float mx = (max.axis.x - min.axis.x) / 2.0f; const float my = (max.axis.y - min.axis.y) / 2.0f; const float mz = (max.axis.z - min.axis.z) / 2.0f; - + const float cx = (min.axis.x + max.axis.x) / 2.0f; const float cy = (min.axis.y + max.axis.y) / 2.0f; const float cz = (min.axis.z + max.axis.z) / 2.0f; memset(softIronMatrix, 0, sizeof(*softIronMatrix)); - + softIronMatrix->element.xx = 1.0f / mx; softIronMatrix->element.yy = 1.0f / my; softIronMatrix->element.zz = 1.0f / mz; @@ -746,57 +746,57 @@ static void apply_calibration(const device_imu_type* device, FusionMatrix gyroscopeMisalignment; FusionVector gyroscopeSensitivity; FusionVector gyroscopeOffset; - + FusionMatrix accelerometerMisalignment; FusionVector accelerometerSensitivity; FusionVector accelerometerOffset; - + FusionMatrix magnetometerMisalignment; FusionVector magnetometerSensitivity; FusionVector magnetometerOffset; - + FusionMatrix softIronMatrix; FusionVector hardIronOffset; - + if (device->calibration) { gyroscopeMisalignment = device->calibration->gyroscopeMisalignment; gyroscopeSensitivity = device->calibration->gyroscopeSensitivity; gyroscopeOffset = device->calibration->gyroscopeOffset; - + accelerometerMisalignment = device->calibration->accelerometerMisalignment; accelerometerSensitivity = device->calibration->accelerometerSensitivity; accelerometerOffset = device->calibration->accelerometerOffset; - + magnetometerMisalignment = device->calibration->magnetometerMisalignment; magnetometerSensitivity = device->calibration->magnetometerSensitivity; magnetometerOffset = device->calibration->magnetometerOffset; - + softIronMatrix = device->calibration->softIronMatrix; hardIronOffset = device->calibration->hardIronOffset; } else { gyroscopeMisalignment = FUSION_IDENTITY_MATRIX; gyroscopeSensitivity = FUSION_VECTOR_ONES; gyroscopeOffset = FUSION_VECTOR_ZERO; - + accelerometerMisalignment = FUSION_IDENTITY_MATRIX; accelerometerSensitivity = FUSION_VECTOR_ONES; accelerometerOffset = FUSION_VECTOR_ZERO; - + magnetometerMisalignment = FUSION_IDENTITY_MATRIX; magnetometerSensitivity = FUSION_VECTOR_ONES; magnetometerOffset = FUSION_VECTOR_ZERO; - + softIronMatrix = FUSION_IDENTITY_MATRIX; hardIronOffset = FUSION_VECTOR_ZERO; } gyroscopeOffset = FusionVectorMultiplyScalar( - gyroscopeOffset, + gyroscopeOffset, FusionRadiansToDegrees(1.0f) ); accelerometerOffset = FusionVectorMultiplyScalar( - accelerometerOffset, + accelerometerOffset, 1.0f / GRAVITY_G ); @@ -814,14 +814,14 @@ static void apply_calibration(const device_imu_type* device, gyroscopeSensitivity, gyroscopeOffset ); - + a = FusionCalibrationInertial( a, accelerometerMisalignment, accelerometerSensitivity, accelerometerOffset ); - + m = FusionCalibrationInertial( m, magnetometerMisalignment, @@ -830,16 +830,16 @@ static void apply_calibration(const device_imu_type* device, ); iterate_iron_offset_estimation( - &m, - &softIronMatrix, + &m, + &softIronMatrix, &hardIronOffset ); - + if (device->calibration) { device->calibration->softIronMatrix = softIronMatrix; device->calibration->hardIronOffset = hardIronOffset; } - + m = FusionCalibrationMagnetic( m, softIronMatrix, @@ -870,32 +870,32 @@ device_imu_error_type device_imu_calibrate(device_imu_type* device, uint32_t ite device_imu_error("No calibration allocated"); return DEVICE_IMU_ERROR_NO_ALLOCATION; } - + if (sizeof(device_imu_packet_type) > device->max_payload_size) { device_imu_error("Not proper size"); return DEVICE_IMU_ERROR_WRONG_SIZE; } - + device_imu_packet_type packet; int transferred; - + bool initialized = false; - + FusionVector cal_gyroscope; FusionVector cal_accelerometer; FusionMatrix softIronMatrix; FusionVector hardIronOffset; - + const float factor = iterations > 0? 1.0f / ((float) iterations) : 0.0f; FusionVector prev_accel; while (iterations > 0) { memset(&packet, 0, sizeof(device_imu_packet_type)); - + transferred = hid_read( - device->handle, - (uint8_t*) &packet, + device->handle, + (uint8_t*) &packet, sizeof(device_imu_packet_type) ); @@ -912,17 +912,17 @@ device_imu_error_type device_imu_calibrate(device_imu_type* device, uint32_t ite device_imu_error("Unexpected packet size"); return DEVICE_IMU_ERROR_UNEXPECTED; } - + if ((packet.signature[0] != 0x01) || (packet.signature[1] != 0x02)) { continue; } - + FusionVector gyroscope; FusionVector accelerometer; FusionVector magnetometer; - + readIMU_from_packet(&packet, &gyroscope, &accelerometer, &magnetometer); - + pre_biased_coordinate_system(&gyroscope); pre_biased_coordinate_system(&accelerometer); pre_biased_coordinate_system(&magnetometer); @@ -938,14 +938,14 @@ device_imu_error_type device_imu_calibrate(device_imu_type* device, uint32_t ite prev_accel = accelerometer; iterate_iron_offset_estimation( - &magnetometer, - &softIronMatrix, + &magnetometer, + &softIronMatrix, &hardIronOffset ); - + iterations--; } - + if (factor > 0.0f) { if (gyro) { device->calibration->gyroscopeOffset = FusionVectorAdd( @@ -956,7 +956,7 @@ device_imu_error_type device_imu_calibrate(device_imu_type* device, uint32_t ite ) ); } - + if (accel) { device->calibration->accelerometerOffset = FusionVectorAdd( device->calibration->accelerometerOffset, @@ -966,13 +966,13 @@ device_imu_error_type device_imu_calibrate(device_imu_type* device, uint32_t ite ) ); } - + if (magnet) { device->calibration->softIronMatrix = softIronMatrix; device->calibration->hardIronOffset = hardIronOffset; } } - + return DEVICE_IMU_ERROR_NO_ERROR; } @@ -986,18 +986,18 @@ device_imu_error_type device_imu_read(device_imu_type* device, int timeout) { device_imu_error("No handle"); return DEVICE_IMU_ERROR_NO_HANDLE; } - + if (sizeof(device_imu_packet_type) > device->max_payload_size) { device_imu_error("Not proper size"); return DEVICE_IMU_ERROR_WRONG_SIZE; } - + device_imu_packet_type packet; memset(&packet, 0, sizeof(device_imu_packet_type)); - + int transferred = hid_read_timeout( - device->handle, - (uint8_t*) &packet, + device->handle, + (uint8_t*) &packet, sizeof(device_imu_packet_type), timeout ); @@ -1006,49 +1006,49 @@ device_imu_error_type device_imu_read(device_imu_type* device, int timeout) { device_imu_error("Device may be unplugged"); return DEVICE_IMU_ERROR_UNPLUGGED; } - + if (transferred == 0) { return DEVICE_IMU_ERROR_NO_ERROR; } - + if (sizeof(device_imu_packet_type) != transferred) { device_imu_error("Unexpected packet size"); return DEVICE_IMU_ERROR_UNEXPECTED; } - + const uint64_t timestamp = le64toh(packet.timestamp); - + if ((packet.signature[0] == 0xaa) && (packet.signature[1] == 0x53)) { device_imu_callback(device, timestamp, DEVICE_IMU_EVENT_INIT); return DEVICE_IMU_ERROR_NO_ERROR; } - + if ((packet.signature[0] != 0x01) || (packet.signature[1] != 0x02)) { device_imu_error("Not matching signature"); return DEVICE_IMU_ERROR_WRONG_SIGNATURE; } - + const uint64_t delta = timestamp - device->last_timestamp; const float deltaTime = (float) ((double) delta / 1e9); - + device->last_timestamp = timestamp; - + int16_t temperature = pack16bit_signed(packet.temperature); - + // According to the ICM-42688-P datasheet: (offset: 25 °C, sensitivity: 132.48 LSB/°C) device->temperature = ((float) temperature) / 132.48f + 25.0f; - + FusionVector gyroscope; FusionVector accelerometer; FusionVector magnetometer; - + readIMU_from_packet(&packet, &gyroscope, &accelerometer, &magnetometer); apply_calibration(device, &gyroscope, &accelerometer, &magnetometer); - + if (device->offset) { gyroscope = FusionOffsetUpdate((FusionOffset*) device->offset, gyroscope); } - + #ifndef NDEBUG printf("G: %.2f %.2f %.2f\n", gyroscope.axis.x, gyroscope.axis.y, gyroscope.axis.z); printf("A: %.2f %.2f %.2f\n", accelerometer.axis.x, accelerometer.axis.y, accelerometer.axis.z); @@ -1073,7 +1073,7 @@ device_imu_error_type device_imu_read(device_imu_type* device, int timeout) { return DEVICE_IMU_ERROR_INVALID_VALUE; } } - + device_imu_callback(device, timestamp, DEVICE_IMU_EVENT_UPDATE); return DEVICE_IMU_ERROR_NO_ERROR; } @@ -1223,7 +1223,7 @@ device_imu_error_type device_imu_close(device_imu_type* device) { device_imu_error("No device"); return DEVICE_IMU_ERROR_NO_DEVICE; } - + if (device->calibration) { if (device->calibration->cam.cameras) { for (uint32_t i = 0; i < device->calibration->cam.num_cameras; i++) { @@ -1247,11 +1247,11 @@ device_imu_error_type device_imu_close(device_imu_type* device) { free(device->calibration); } - + if (device->ahrs) { free(device->ahrs); } - + if (device->offset) { free(device->offset); } @@ -1264,7 +1264,7 @@ device_imu_error_type device_imu_close(device_imu_type* device) { hid_close(device->handle); } - + memset(device, 0, sizeof(device_imu_type)); device_exit(); diff --git a/ardriver/xreal/go_ffi.c b/ardriver/xreal/go_ffi.c new file mode 100644 index 0000000..558a65b --- /dev/null +++ b/ardriver/xreal/go_ffi.c @@ -0,0 +1,7 @@ +#include "device_imu.h" + +extern void goIMUEventHandler(uint64_t, device_imu_event_type, device_imu_ahrs_type*); + +void imuEventHandler(uint64_t timestamp, device_imu_event_type event, const device_imu_ahrs_type* ahrs) { + goIMUEventHandler(timestamp, event, (device_imu_ahrs_type*)ahrs); +} diff --git a/ardriver/xreal/go_ffi.h b/ardriver/xreal/go_ffi.h new file mode 100644 index 0000000..7972a47 --- /dev/null +++ b/ardriver/xreal/go_ffi.h @@ -0,0 +1,3 @@ +#include "device_imu.h" +extern void goIMUEventHandler(uint64_t, device_imu_event_type, device_imu_ahrs_type*); +void imuEventHandler(uint64_t timestamp, device_imu_event_type event, const device_imu_ahrs_type* ahrs); diff --git a/ardriver/xreal/xreal.go b/ardriver/xreal/xreal.go index a99fe12..08343b3 100644 --- a/ardriver/xreal/xreal.go +++ b/ardriver/xreal/xreal.go @@ -3,10 +3,132 @@ package xreal -// #include "evdi_lib.h" -// #include "go_ffi.h" -// #cgo CFLAGS: -w +// #cgo CFLAGS: -w -I./Fusion/ // #cgo pkg-config: json-c libusb-1.0 hidapi-libusb +// #include "go_ffi.h" +// #include "device.h" +// #include "device_imu.h" +// #include "device_mcu.h" import "C" +import ( + "fmt" + "sync" + "time" -var IsXrealEnabled = true + "git.terah.dev/UnrealXR/unrealxr/ardriver/commons" +) + +var ( + IsXrealEnabled = true + deviceEventHandlerMutex = sync.Mutex{} + deviceEventListener *commons.AREventListener +) + +//export goIMUEventHandler +func goIMUEventHandler(_ C.uint64_t, event_type C.device_imu_event_type, ahrs *C.struct_device_imu_ahrs_t) { + if deviceEventListener == nil { + return + } + + if event_type != C.DEVICE_IMU_EVENT_UPDATE { + return + } + + orientation := C.device_imu_get_orientation(ahrs) + euler := C.device_imu_get_euler(orientation) + + deviceEventListener.PitchCallback(float32(euler.pitch)) + deviceEventListener.RollCallback(float32(euler.roll)) + deviceEventListener.YawCallback(float32(euler.yaw)) +} + +// Implements commons.ARDevice +type XrealDevice struct { + eventListener *commons.AREventListener + imuDevice *C.struct_device_imu_t + deviceIsOpen bool +} + +func (device *XrealDevice) Initialize() error { + if device.deviceIsOpen { + return fmt.Errorf("device is already open") + } + + device.imuDevice = &C.struct_device_imu_t{} + + // (*[0]byte) is a FUBAR way to cast a pointer to a function, but unsafe.Pointer doesn't work: + // cannot use unsafe.Pointer(_Cgo_ptr(_Cfpvar_fp_imuEventHandler)) (value of type unsafe.Pointer) as *[0]byte value in variable declaration + if C.DEVICE_IMU_ERROR_NO_ERROR != C.device_imu_open(device.imuDevice, (*[0]byte)(C.imuEventHandler)) { + return fmt.Errorf("failed to open IMU device") + } + + C.device_imu_clear(device.imuDevice) + C.device_imu_calibrate(device.imuDevice, 1000, true, true, false) + + device.deviceIsOpen = true + + // let's hope this doesn't cause race conditions + go func() { + for device.eventListener == nil { + time.Sleep(time.Millisecond * 10) + } + + for { + if !device.deviceIsOpen { + break + } + + // I'm sorry. + deviceEventHandlerMutex.Lock() + deviceEventListener = device.eventListener + status := C.device_imu_read(device.imuDevice, -1) + deviceEventHandlerMutex.Unlock() + + if status != C.DEVICE_IMU_ERROR_NO_ERROR { + break + } + } + + device.deviceIsOpen = false + C.device_imu_close(device.imuDevice) + }() + + return nil +} + +func (device *XrealDevice) End() error { + if !device.deviceIsOpen { + return fmt.Errorf("device is not open") + } + + C.device_imu_close(device.imuDevice) + device.deviceIsOpen = false + return nil +} + +func (device *XrealDevice) IsPollingLibrary() bool { + return false +} + +func (device *XrealDevice) IsEventBasedLibrary() bool { + return true +} + +func (device *XrealDevice) Poll() error { + return fmt.Errorf("not supported") +} + +func (device *XrealDevice) RegisterEventListeners(listener *commons.AREventListener) { + device.eventListener = listener +} + +func New() (*XrealDevice, error) { + device := &XrealDevice{} + err := device.Initialize() + + if err != nil { + return nil, err + } + + return device, nil +} diff --git a/ardriver/xreal/xreal_debug_logging.go b/ardriver/xreal/xreal_debug_logging.go new file mode 100644 index 0000000..0d4eab9 --- /dev/null +++ b/ardriver/xreal/xreal_debug_logging.go @@ -0,0 +1,7 @@ +//go:build xreal && !xreal_debug_logging +// +build xreal,!xreal_debug_logging + +package xreal + +// #cgo CFLAGS: -DNDEBUG +import "C" diff --git a/ardriver/xreal/xreal_disabled.go b/ardriver/xreal/xreal_disabled.go index 1cbe85c..c6ca903 100644 --- a/ardriver/xreal/xreal_disabled.go +++ b/ardriver/xreal/xreal_disabled.go @@ -3,4 +3,42 @@ package xreal +import ( + "fmt" + + "git.terah.dev/UnrealXR/unrealxr/ardriver/commons" +) + var IsXrealEnabled = false + +// Implements commons.ARDevice +type XrealDevice struct { +} + +func (device *XrealDevice) Initialize() error { + return fmt.Errorf("xreal is not enabled") +} + +func (device *XrealDevice) End() error { + return fmt.Errorf("xreal is not enabled") +} + +func (device *XrealDevice) IsPollingLibrary() bool { + return false +} + +func (device *XrealDevice) IsEventBasedLibrary() bool { + return false +} + +func (device *XrealDevice) Poll() error { + return fmt.Errorf("xreal is not enabled") +} + +func (device *XrealDevice) RegisterEventListeners(*commons.AREventListener) error { + return fmt.Errorf("xreal is not enabled") +} + +func New() (*XrealDevice, error) { + return nil, fmt.Errorf("xreal is not enabled") +} From 47e693a7b9ff240d3a5c8e204b8c073a62d7e394 Mon Sep 17 00:00:00 2001 From: imterah Date: Wed, 25 Jun 2025 21:48:40 -0400 Subject: [PATCH 12/12] feature: Get initial multi-display working in Go --- app/main.go | 24 ++++- app/renderer/renderer.go | 221 +++++++++++++++++++++++++++++++++++++-- app/renderer/struct.go | 13 ++- go.mod | 21 ++-- go.sum | 17 +-- 5 files changed, 261 insertions(+), 35 deletions(-) diff --git a/app/main.go b/app/main.go index cd762c0..471b548 100644 --- a/app/main.go +++ b/app/main.go @@ -8,6 +8,7 @@ import ( "os/signal" "path" "syscall" + "time" libconfig "git.terah.dev/UnrealXR/unrealxr/app/config" "git.terah.dev/UnrealXR/unrealxr/app/edidtools" @@ -154,15 +155,28 @@ func mainEntrypoint(context.Context, *cli.Command) error { Y2: displayMetadata.MaxHeight, } - displayBuffer := openedDevice.CreateBuffer(displayMetadata.MaxWidth, displayMetadata.MaxHeight, 4, displayRect) + displayBuffer := openedDevice.CreateBuffer(displayMetadata.MaxWidth, displayMetadata.MaxHeight, libevdi.StridePixelFormatRGBA32, displayRect) - evdiCards[currentDisplay] = &renderer.EvdiDisplayMetadata{ - EvdiNode: openedDevice, - Rect: displayRect, - Buffer: displayBuffer, + displayMetadata := &renderer.EvdiDisplayMetadata{ + EvdiNode: openedDevice, + Rect: displayRect, + Buffer: displayBuffer, + ShouldRequestUpdate: true, } + + displayMetadata.EventContext = &libevdi.EvdiEventContext{ + UpdateReadyHandler: func(bufferToBeUpdated int) { + displayMetadata.IsUpdateReady = true + }, + } + + openedDevice.RegisterEventHandler(displayMetadata.EventContext) + evdiCards[currentDisplay] = displayMetadata } + // HACK: sometimes the buffer doesn't get initialized properly if we don't wait a bit... + time.Sleep(time.Millisecond * 100) + log.Info("Initialized displays. Entering rendering loop") renderer.EnterRenderLoop(config, displayMetadata, evdiCards) diff --git a/app/renderer/renderer.go b/app/renderer/renderer.go index e3cb39a..d1ebbf9 100644 --- a/app/renderer/renderer.go +++ b/app/renderer/renderer.go @@ -1,7 +1,10 @@ package renderer import ( + "image/color" + "math" "time" + "unsafe" libconfig "git.terah.dev/UnrealXR/unrealxr/app/config" "git.terah.dev/UnrealXR/unrealxr/app/edidtools" @@ -9,8 +12,27 @@ import ( arcommons "git.terah.dev/UnrealXR/unrealxr/ardriver/commons" "github.com/charmbracelet/log" "github.com/tebeka/atexit" + + rl "git.terah.dev/UnrealXR/raylib-go/raylib" ) +type TextureModelPair struct { + Texture rl.Texture2D + Model rl.Model +} + +func findMaxVerticalSize(fovyDeg float32, distance float32) float32 { + fovyRad := float64(fovyDeg * math.Pi / 180.0) + return 2 * distance * float32(math.Tan(fovyRad/2)) +} + +func findOptimalHorizontalRes(verticalDisplayRes float32, horizontalDisplayRes float32, verticalSize float32) float32 { + aspectRatio := horizontalDisplayRes / verticalDisplayRes + horizontalSize := verticalSize * aspectRatio + + return horizontalSize +} + func EnterRenderLoop(config *libconfig.Config, displayMetadata *edidtools.DisplayMetadata, evdiCards []*EvdiDisplayMetadata) { log.Info("Initializing AR driver") headset, err := ardriver.GetDevice() @@ -19,21 +41,52 @@ func EnterRenderLoop(config *libconfig.Config, displayMetadata *edidtools.Displa log.Errorf("Failed to get device: %s", err.Error()) atexit.Exit(1) } + log.Info("Initialized") - var pitch float32 - var yaw float32 - var roll float32 + var ( + currentPitch float32 + previousPitch float32 + currentYaw float32 + previousYaw float32 + currentRoll float32 + previousRoll float32 + + hasGottenPitchCallbackBefore bool + hasGottenYawCallbackBefore bool + hasGottenRollCallbackBefore bool + ) arEventListner := &arcommons.AREventListener{ PitchCallback: func(newPitch float32) { - pitch = newPitch + if !hasGottenPitchCallbackBefore { + hasGottenPitchCallbackBefore = true + currentPitch = newPitch + previousPitch = newPitch + } else { + previousPitch = currentPitch + currentPitch = newPitch + } }, YawCallback: func(newYaw float32) { - yaw = newYaw + if !hasGottenYawCallbackBefore { + hasGottenYawCallbackBefore = true + currentYaw = newYaw + previousYaw = newYaw + } else { + previousYaw = currentYaw + currentYaw = newYaw + } }, RollCallback: func(newRoll float32) { - roll = newRoll + if !hasGottenRollCallbackBefore { + hasGottenRollCallbackBefore = true + currentRoll = newRoll + previousRoll = newRoll + } else { + previousRoll = currentRoll + currentRoll = newRoll + } }, } @@ -44,8 +97,158 @@ func EnterRenderLoop(config *libconfig.Config, displayMetadata *edidtools.Displa headset.RegisterEventListeners(arEventListner) - for { - log.Debugf("pitch: %f, yaw: %f, roll: %f", pitch, yaw, roll) - time.Sleep(10 * time.Millisecond) + fovY := float32(45.0) + verticalSize := findMaxVerticalSize(fovY, 5.0) + + camera := rl.NewCamera3D( + rl.Vector3{ + X: 0.0, + Y: verticalSize / 2, + Z: 5.0, + }, + rl.Vector3{ + X: 0.0, + Y: verticalSize / 2, + Z: 0.0, + }, + rl.Vector3{ + X: 0.0, + Y: 1.0, + Z: 0.0, + }, + fovY, + rl.CameraPerspective, + ) + + // Disable front and back face culling. It caused issues involving the entire virtual display dissappearing + // If this issue still happens I *am* going to cry + rl.DisableBackfaceCulling() + rl.DisableDepthTest() + + coreMesh := rl.GenMeshPlane(findOptimalHorizontalRes(float32(displayMetadata.MaxHeight), float32(displayMetadata.MaxWidth), verticalSize), verticalSize, 1, 1) + + movementVector := rl.Vector3{ + X: 0.0, + Y: 0.0, + Z: 0.0, } + + lookVector := rl.Vector3{ + X: 0.0, + Y: 0.0, + Z: 0.0, + } + + hasZVectorDisabledQuirk := false + hasSensorInitDelayQuirk := false + sensorInitStartTime := time.Now() + + if displayMetadata.DeviceQuirks.ZVectorDisabled { + log.Warn("QUIRK: The Z vector has been disabled for your specific device") + hasZVectorDisabledQuirk = true + } + + if displayMetadata.DeviceQuirks.SensorInitDelay != 0 { + log.Warnf("QUIRK: Waiting %d second(s) before reading sensors", displayMetadata.DeviceQuirks.SensorInitDelay) + log.Warn("|| MOVEMENT WILL NOT BE OPERATIONAL DURING THIS TIME. ||") + hasSensorInitDelayQuirk = true + } + + rects := make([]*TextureModelPair, len(evdiCards)) + + for i, card := range evdiCards { + image := rl.NewImage(card.Buffer.Buffer, int32(displayMetadata.MaxWidth), int32(displayMetadata.MaxHeight), 1, rl.UncompressedR8g8b8a8) + + texture := rl.LoadTextureFromImage(image) + model := rl.LoadModelFromMesh(coreMesh) + + rl.SetMaterialTexture(model.Materials, rl.MapAlbedo, texture) + + rects[i] = &TextureModelPair{ + Texture: texture, + Model: model, + } + } + + eventTimeoutDuration := 0 * time.Millisecond + + for !rl.WindowShouldClose() { + if hasSensorInitDelayQuirk { + if time.Now().Sub(sensorInitStartTime) > time.Duration(displayMetadata.DeviceQuirks.SensorInitDelay)*time.Second { + log.Info("Movement is now enabled.") + hasSensorInitDelayQuirk = false + } + } else { + lookVector.X = (currentYaw - previousYaw) * 6.5 + lookVector.Y = -(currentPitch - previousPitch) * 6.5 + + if !hasZVectorDisabledQuirk { + lookVector.Z = (currentRoll - previousRoll) * 6.5 + } + + rl.UpdateCameraPro(&camera, movementVector, lookVector, 0) + } + + rl.BeginDrawing() + rl.ClearBackground(rl.Black) + rl.BeginMode3D(camera) + + for rectPos, rect := range rects { + card := evdiCards[rectPos] + + ready, err := card.EvdiNode.WaitUntilEventsAreReadyToHandle(eventTimeoutDuration) + + if err != nil { + log.Errorf("Failed to wait for display events: %s", err.Error()) + break + } + + if ready { + if err := card.EvdiNode.HandleEvents(card.EventContext); err != nil { + log.Errorf("Failed to handle display events: %s", err.Error()) + break + } + + card.EvdiNode.GrabPixels(card.Rect) + + pixels := unsafe.Slice( + (*color.RGBA)(unsafe.Pointer(&card.Buffer.Buffer[0])), + len(card.Buffer.Buffer)/4, + ) + + rl.UpdateTexture(rect.Texture, pixels) + card.EvdiNode.RequestUpdate(card.Buffer) + } + + rl.DrawModelEx( + rect.Model, + rl.Vector3{ + X: 0, + Y: verticalSize / 2, + Z: 0, + }, + // rotate around X to make it vertical + rl.Vector3{ + X: 1, + Y: 0, + Z: 0, + }, + 90, + rl.Vector3{ + X: 1, + Y: 1, + Z: 1, + }, + rl.White, + ) + + break + } + + rl.EndMode3D() + rl.EndDrawing() + } + + log.Info("Goodbye!") + rl.CloseWindow() } diff --git a/app/renderer/struct.go b/app/renderer/struct.go index a1dd2d0..0acbde4 100644 --- a/app/renderer/struct.go +++ b/app/renderer/struct.go @@ -1,9 +1,14 @@ package renderer -import "git.terah.dev/imterah/goevdi/libevdi" +import ( + "git.terah.dev/imterah/goevdi/libevdi" +) type EvdiDisplayMetadata struct { - EvdiNode *libevdi.EvdiNode - Rect *libevdi.EvdiDisplayRect - Buffer *libevdi.EvdiBuffer + EvdiNode *libevdi.EvdiNode + Rect *libevdi.EvdiDisplayRect + Buffer *libevdi.EvdiBuffer + EventContext *libevdi.EvdiEventContext + ShouldRequestUpdate bool + IsUpdateReady bool } diff --git a/go.mod b/go.mod index aa4a6e5..aedc610 100644 --- a/go.mod +++ b/go.mod @@ -3,28 +3,31 @@ module git.terah.dev/UnrealXR/unrealxr go 1.24.3 require ( - git.terah.dev/UnrealXR/raylib-go/raylib v0.55.2-0.20250623002739-1468af2636e1 // indirect - git.terah.dev/imterah/goevdi v1.14.10 // indirect - github.com/anoopengineer/edidparser v0.0.0-20240602223913-86ca9ed3d2b0 // indirect + git.terah.dev/UnrealXR/raylib-go/raylib v0.55.2-0.20250623002739-1468af2636e1 + git.terah.dev/imterah/goevdi v1.14.11-0.20250626004148-bdbef2a68ff9 + github.com/anoopengineer/edidparser v0.0.0-20240602223913-86ca9ed3d2b0 + github.com/charmbracelet/log v0.4.2 + github.com/goccy/go-yaml v1.18.0 + github.com/google/uuid v1.6.0 + github.com/kirsle/configdir v0.0.0-20170128060238-e45d2f54772f + github.com/tebeka/atexit v0.3.0 + github.com/urfave/cli/v3 v3.3.8 +) + +require ( github.com/aymanbagabas/go-osc52/v2 v2.0.1 // indirect github.com/charmbracelet/colorprofile v0.2.3-0.20250311203215-f60798e515dc // indirect github.com/charmbracelet/lipgloss v1.1.0 // indirect - github.com/charmbracelet/log v0.4.2 // indirect github.com/charmbracelet/x/ansi v0.8.0 // indirect github.com/charmbracelet/x/cellbuf v0.0.13-0.20250311204145-2c3ea96c31dd // indirect github.com/charmbracelet/x/term v0.2.1 // indirect github.com/ebitengine/purego v0.8.4 // indirect github.com/go-logfmt/logfmt v0.6.0 // indirect - github.com/goccy/go-yaml v1.18.0 // indirect - github.com/google/uuid v1.6.0 // indirect - github.com/kirsle/configdir v0.0.0-20170128060238-e45d2f54772f // indirect github.com/lucasb-eyer/go-colorful v1.2.0 // indirect github.com/mattn/go-isatty v0.0.20 // indirect github.com/mattn/go-runewidth v0.0.16 // indirect github.com/muesli/termenv v0.16.0 // indirect github.com/rivo/uniseg v0.4.7 // indirect - github.com/tebeka/atexit v0.3.0 // indirect - github.com/urfave/cli/v3 v3.3.8 // indirect github.com/xo/terminfo v0.0.0-20220910002029-abceb7e1c41e // indirect golang.org/x/exp v0.0.0-20250620022241-b7579e27df2b // indirect golang.org/x/sys v0.33.0 // indirect diff --git a/go.sum b/go.sum index 72d89f4..aad411c 100644 --- a/go.sum +++ b/go.sum @@ -1,9 +1,7 @@ git.terah.dev/UnrealXR/raylib-go/raylib v0.55.2-0.20250623002739-1468af2636e1 h1:cO/veKc8mon3Zlueq1sEc047B04fk/hoBJSn+btMLDI= git.terah.dev/UnrealXR/raylib-go/raylib v0.55.2-0.20250623002739-1468af2636e1/go.mod h1:ZRirF2UuVWSbl2ux7oyHwXcinni9msejCvtIsXbT8yY= -git.terah.dev/imterah/goevdi v0.0.0-20250211014657-96d98ad0735c h1:Ys33r6kjCyCs8N3ofSzCGxxL1gLIfetIiO1uL5UIsAM= -git.terah.dev/imterah/goevdi v0.0.0-20250211014657-96d98ad0735c/go.mod h1:RmxqlNaEJ+6qbHpyw6ivJhXlRYT1RcsZ9gzNOsFXfOA= -git.terah.dev/imterah/goevdi v1.14.10 h1:3zr3E/WRkSQ//+Gcrh0wfy0LVgm4Pcglc5K4MVcZ7n8= -git.terah.dev/imterah/goevdi v1.14.10/go.mod h1:z6GnBn2qHVV5N6fKZXPgUka8H9g5mZA5BRxCzUxLaNk= +git.terah.dev/imterah/goevdi v1.14.11-0.20250626004148-bdbef2a68ff9 h1:TYcPZ62CR3keYf/dE9KyV5X5krh+riDyZ3fnhkeSRyA= +git.terah.dev/imterah/goevdi v1.14.11-0.20250626004148-bdbef2a68ff9/go.mod h1:4scjAuFakx/2gTRSeCtTNHnj1v9FdF3XiOMmWsz4FDs= github.com/anoopengineer/edidparser v0.0.0-20240602223913-86ca9ed3d2b0 h1:rTfysyBCL7LPbq9GFpQbllvKT8vEI93lQUwksMMxHMI= github.com/anoopengineer/edidparser v0.0.0-20240602223913-86ca9ed3d2b0/go.mod h1:fEt61NePh3ZMxA+g3iC4CaGzY9lEsHRUkYJY2x0lBAw= github.com/aymanbagabas/go-osc52/v2 v2.0.1 h1:HwpRHbFMcZLEVr42D4p7XBqjyuxQH5SMiErDT4WkJ2k= @@ -21,6 +19,8 @@ github.com/charmbracelet/x/cellbuf v0.0.13-0.20250311204145-2c3ea96c31dd/go.mod github.com/charmbracelet/x/term v0.2.1 h1:AQeHeLZ1OqSXhrAWpYUtZyX1T3zVxfpZuEQMIQaGIAQ= github.com/charmbracelet/x/term v0.2.1/go.mod h1:oQ4enTYFV7QN4m0i9mzHrViD7TQKvNEEkHUMCmsxdUg= github.com/davecgh/go-spew v1.1.0/go.mod h1:J7Y8YcW2NihsgmVo/mv3lAwl/skON4iLHjSsI+c5H38= +github.com/davecgh/go-spew v1.1.1 h1:vj9j/u1bqnvCEfJOwUhtlOARqs3+rkHYY13jYWTU97c= +github.com/davecgh/go-spew v1.1.1/go.mod h1:J7Y8YcW2NihsgmVo/mv3lAwl/skON4iLHjSsI+c5H38= github.com/ebitengine/purego v0.8.4 h1:CF7LEKg5FFOsASUj0+QwaXf8Ht6TlFxg09+S9wz0omw= github.com/ebitengine/purego v0.8.4/go.mod h1:iIjxzd6CiRiOG0UyXP+V1+jWqUXVjPKLAI0mRfJZTmQ= github.com/go-logfmt/logfmt v0.6.0 h1:wGYYu3uicYdqXVgoYbvnkrPVXkuLM1p1ifugDMEdRi4= @@ -39,26 +39,27 @@ github.com/mattn/go-runewidth v0.0.16 h1:E5ScNMtiwvlvB5paMFdw9p4kSQzbXFikJ5SQO6T github.com/mattn/go-runewidth v0.0.16/go.mod h1:Jdepj2loyihRzMpdS35Xk/zdY8IAYHsh153qUoGf23w= github.com/muesli/termenv v0.16.0 h1:S5AlUN9dENB57rsbnkPyfdGuWIlkmzJjbFf0Tf5FWUc= github.com/muesli/termenv v0.16.0/go.mod h1:ZRfOIKPFDYQoDFF4Olj7/QJbW60Ol/kL1pU3VfY/Cnk= +github.com/pmezard/go-difflib v1.0.0 h1:4DBwDE0NGyQoBHbLQYPwSUPoCMWR5BEzIk/f1lZbAQM= github.com/pmezard/go-difflib v1.0.0/go.mod h1:iKH77koFhYxTK1pcRnkKkqfTogsbg7gZNVY4sRDYZ/4= github.com/rivo/uniseg v0.2.0/go.mod h1:J6wj4VEh+S6ZtnVlnTBMWIodfgj8LQOQFoIToxlJtxc= github.com/rivo/uniseg v0.4.7 h1:WUdvkW8uEhrYfLC4ZzdpI2ztxP1I582+49Oc5Mq64VQ= github.com/rivo/uniseg v0.4.7/go.mod h1:FN3SvrM+Zdj16jyLfmOkMNblXMcoc8DfTHruCPUcx88= github.com/stretchr/objx v0.1.0/go.mod h1:HFkY916IF+rwdDfMAkV7OtwuqBVzrE8GR6GFx+wExME= github.com/stretchr/testify v1.5.1/go.mod h1:5W2xD1RspED5o8YsWQXVCued0rvSQ+mT+I5cxcmMvtA= +github.com/stretchr/testify v1.10.0 h1:Xv5erBjTwe/5IxqUQTdXv5kgmIvbHo3QQyRwhJsOfJA= +github.com/stretchr/testify v1.10.0/go.mod h1:r2ic/lqez/lEtzL7wO/rwa5dbSLXVDPFyf8C91i36aY= github.com/tebeka/atexit v0.3.0 h1:jleL99H7Ywt80oJKR+VWmJNnezcCOG0CuzcN3CIpsdI= github.com/tebeka/atexit v0.3.0/go.mod h1:WJmSUSmMT7WoR7etUOaGBVXk+f5/ZJ+67qwuedq7Fbs= github.com/urfave/cli/v3 v3.3.8 h1:BzolUExliMdet9NlJ/u4m5vHSotJ3PzEqSAZ1oPMa/E= github.com/urfave/cli/v3 v3.3.8/go.mod h1:FJSKtM/9AiiTOJL4fJ6TbMUkxBXn7GO9guZqoZtpYpo= github.com/xo/terminfo v0.0.0-20220910002029-abceb7e1c41e h1:JVG44RsyaB9T2KIHavMF/ppJZNG9ZpyihvCd0w101no= github.com/xo/terminfo v0.0.0-20220910002029-abceb7e1c41e/go.mod h1:RbqR21r5mrJuqunuUZ/Dhy/avygyECGrLceyNeo4LiM= -golang.org/x/exp v0.0.0-20231006140011-7918f672742d h1:jtJma62tbqLibJ5sFQz8bKtEM8rJBtfilJ2qTU199MI= -golang.org/x/exp v0.0.0-20231006140011-7918f672742d/go.mod h1:ldy0pHrwJyGW56pPQzzkH36rKxoZW1tw7ZJpeKx+hdo= golang.org/x/exp v0.0.0-20250620022241-b7579e27df2b h1:M2rDM6z3Fhozi9O7NWsxAkg/yqS/lQJ6PmkyIV3YP+o= golang.org/x/exp v0.0.0-20250620022241-b7579e27df2b/go.mod h1:3//PLf8L/X+8b4vuAfHzxeRUl04Adcb341+IGKfnqS8= golang.org/x/sys v0.6.0/go.mod h1:oPkhp1MJrh7nUepCBck5+mAzfO9JrbApNNgaTdGDITg= -golang.org/x/sys v0.30.0 h1:QjkSwP/36a20jFYWkSue1YwXzLmsV5Gfq7Eiy72C1uc= -golang.org/x/sys v0.30.0/go.mod h1:/VUhepiaJMQUp4+oa/7Zr1D23ma6VTLIYjOOTFZPUcA= golang.org/x/sys v0.33.0 h1:q3i8TbbEz+JRD9ywIRlyRAQbM0qF7hu24q3teo2hbuw= golang.org/x/sys v0.33.0/go.mod h1:BJP2sWEmIv4KK5OTEluFJCKSidICx8ciO85XgH3Ak8k= gopkg.in/check.v1 v0.0.0-20161208181325-20d25e280405/go.mod h1:Co6ibVJAznAaIkqp8huTwlJQCZ016jof/cbN4VW5Yz0= gopkg.in/yaml.v2 v2.2.2/go.mod h1:hI93XBmqTisBFMUTm0b8Fm+jr3Dg1NNxqwp+5A1VGuI= +gopkg.in/yaml.v3 v3.0.1 h1:fxVm/GzAzEWqLHuvctI91KS9hhNmmWOoWu0XTYJS7CA= +gopkg.in/yaml.v3 v3.0.1/go.mod h1:K4uyk7z7BCEPqu6E+C64Yfv1cQ7kz7rIZviUmN+EgEM=