Rewrite entire codebase into Go #2
61 changed files with 5786 additions and 1094 deletions
293
.gitignore
vendored
293
.gitignore
vendored
|
|
@ -1,281 +1,32 @@
|
|||
# ---> 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
|
||||
# artifacts
|
||||
uxr
|
||||
|
|
|
|||
9
.gitmodules
vendored
9
.gitmodules
vendored
|
|
@ -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
|
||||
13
.zed/settings.json
Normal file
13
.zed/settings.json
Normal file
|
|
@ -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"]
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
13
Makefile
Normal file
13
Makefile
Normal file
|
|
@ -0,0 +1,13 @@
|
|||
APP_DIR := ./app
|
||||
OUTPUT := uxr
|
||||
TAGS := xreal noaudio drm drm_leasing drm_disable_input
|
||||
|
||||
.PHONY: all build clean
|
||||
|
||||
all: build
|
||||
|
||||
build:
|
||||
go build -v -tags '$(TAGS)' -o $(OUTPUT) $(APP_DIR)
|
||||
|
||||
clean:
|
||||
rm -f $(OUTPUT)
|
||||
39
README.md
39
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`
|
||||
Just run `make` in the root directory.
|
||||
|
|
|
|||
80
app/config/config.go
Normal file
80
app/config/config.go
Normal file
|
|
@ -0,0 +1,80 @@
|
|||
package config
|
||||
|
||||
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
|
||||
}
|
||||
}
|
||||
18
app/config/default_config.yml
Normal file
18
app/config/default_config.yml
Normal file
|
|
@ -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. 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.
|
||||
72
app/edidtools/edid_parser.go
Normal file
72
app/edidtools/edid_parser.go
Normal file
|
|
@ -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)
|
||||
}
|
||||
127
app/edidtools/patching_tools_linux.go
Normal file
127
app/edidtools/patching_tools_linux.go
Normal file
|
|
@ -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(monitor.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.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)
|
||||
}
|
||||
|
||||
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.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)
|
||||
}
|
||||
|
||||
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
|
||||
}
|
||||
21
app/edidtools/patching_tools_macos.go
Normal file
21
app/edidtools/patching_tools_macos.go
Normal file
|
|
@ -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")
|
||||
}
|
||||
21
app/edidtools/patching_tools_win.go
Normal file
21
app/edidtools/patching_tools_win.go
Normal file
|
|
@ -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")
|
||||
}
|
||||
14
app/edidtools/quirks.go
Normal file
14
app/edidtools/quirks.go
Normal file
|
|
@ -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,
|
||||
},
|
||||
},
|
||||
}
|
||||
20
app/edidtools/struct.go
Normal file
20
app/edidtools/struct.go
Normal file
|
|
@ -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
|
||||
}
|
||||
219
app/main.go
Normal file
219
app/main.go
Normal file
|
|
@ -0,0 +1,219 @@
|
|||
package main
|
||||
|
||||
import (
|
||||
"bufio"
|
||||
"context"
|
||||
"fmt"
|
||||
"os"
|
||||
"os/signal"
|
||||
"path"
|
||||
"syscall"
|
||||
"time"
|
||||
|
||||
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")
|
||||
}
|
||||
|
||||
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"), libconfig.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 := &libconfig.Config{}
|
||||
err = yaml.Unmarshal(configBytes, config)
|
||||
|
||||
if err != nil {
|
||||
return fmt.Errorf("failed to parse config file: %w", err)
|
||||
}
|
||||
|
||||
libconfig.InitializePotentiallyMissingConfigValues(config)
|
||||
log.Debug("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.Debug("Got EDID file and metadata")
|
||||
log.Debug("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)
|
||||
}
|
||||
|
||||
atexit.Register(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")
|
||||
rl.SetTargetFPS(int32(displayMetadata.MaxRefreshRate * 2))
|
||||
rl.InitWindow(int32(displayMetadata.MaxWidth), int32(displayMetadata.MaxHeight), "UnrealXR")
|
||||
|
||||
atexit.Register(func() {
|
||||
rl.CloseWindow()
|
||||
})
|
||||
|
||||
log.Info("Initializing virtual displays")
|
||||
|
||||
libevdi.SetupLogger(&libevdi.EvdiLogger{
|
||||
Log: func(msg string) {
|
||||
log.Debugf("EVDI: %s", msg)
|
||||
},
|
||||
})
|
||||
|
||||
evdiCards := make([]*renderer.EvdiDisplayMetadata, *config.DisplayConfig.Count)
|
||||
|
||||
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, libevdi.StridePixelFormatRGBA32, displayRect)
|
||||
|
||||
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)
|
||||
|
||||
atexit.Exit(0)
|
||||
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.Fatalf("Fatal error during execution: %s", err.Error())
|
||||
}
|
||||
}
|
||||
254
app/renderer/renderer.go
Normal file
254
app/renderer/renderer.go
Normal file
|
|
@ -0,0 +1,254 @@
|
|||
package renderer
|
||||
|
||||
import (
|
||||
"image/color"
|
||||
"math"
|
||||
"time"
|
||||
"unsafe"
|
||||
|
||||
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"
|
||||
|
||||
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()
|
||||
|
||||
if err != nil {
|
||||
log.Errorf("Failed to get device: %s", err.Error())
|
||||
atexit.Exit(1)
|
||||
}
|
||||
|
||||
log.Info("Initialized")
|
||||
|
||||
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) {
|
||||
if !hasGottenPitchCallbackBefore {
|
||||
hasGottenPitchCallbackBefore = true
|
||||
currentPitch = newPitch
|
||||
previousPitch = newPitch
|
||||
} else {
|
||||
previousPitch = currentPitch
|
||||
currentPitch = newPitch
|
||||
}
|
||||
},
|
||||
YawCallback: func(newYaw float32) {
|
||||
if !hasGottenYawCallbackBefore {
|
||||
hasGottenYawCallbackBefore = true
|
||||
currentYaw = newYaw
|
||||
previousYaw = newYaw
|
||||
} else {
|
||||
previousYaw = currentYaw
|
||||
currentYaw = newYaw
|
||||
}
|
||||
},
|
||||
RollCallback: func(newRoll float32) {
|
||||
if !hasGottenRollCallbackBefore {
|
||||
hasGottenRollCallbackBefore = true
|
||||
currentRoll = newRoll
|
||||
previousRoll = newRoll
|
||||
} else {
|
||||
previousRoll = currentRoll
|
||||
currentRoll = 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)
|
||||
|
||||
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()
|
||||
}
|
||||
14
app/renderer/struct.go
Normal file
14
app/renderer/struct.go
Normal file
|
|
@ -0,0 +1,14 @@
|
|||
package renderer
|
||||
|
||||
import (
|
||||
"git.terah.dev/imterah/goevdi/libevdi"
|
||||
)
|
||||
|
||||
type EvdiDisplayMetadata struct {
|
||||
EvdiNode *libevdi.EvdiNode
|
||||
Rect *libevdi.EvdiDisplayRect
|
||||
Buffer *libevdi.EvdiBuffer
|
||||
EventContext *libevdi.EvdiEventContext
|
||||
ShouldRequestUpdate bool
|
||||
IsUpdateReady bool
|
||||
}
|
||||
23
ardriver/ardriver.go
Normal file
23
ardriver/ardriver.go
Normal file
|
|
@ -0,0 +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")
|
||||
}
|
||||
22
ardriver/commons/interface.go
Normal file
22
ardriver/commons/interface.go
Normal file
|
|
@ -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)
|
||||
}
|
||||
32
ardriver/xreal/Fusion.h
Normal file
32
ardriver/xreal/Fusion.h
Normal file
|
|
@ -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
|
||||
510
ardriver/xreal/FusionAhrs.c
Normal file
510
ardriver/xreal/FusionAhrs.c
Normal file
|
|
@ -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 <float.h>
|
||||
#include "FusionAhrs.h"
|
||||
#include <math.h>
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
// 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
|
||||
111
ardriver/xreal/FusionAhrs.h
Normal file
111
ardriver/xreal/FusionAhrs.h
Normal file
|
|
@ -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 <stdbool.h>
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
// 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
|
||||
187
ardriver/xreal/FusionAxes.h
Normal file
187
ardriver/xreal/FusionAxes.h
Normal file
|
|
@ -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
|
||||
44
ardriver/xreal/FusionCalibration.h
Normal file
44
ardriver/xreal/FusionCalibration.h
Normal file
|
|
@ -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
|
||||
49
ardriver/xreal/FusionCompass.c
Normal file
49
ardriver/xreal/FusionCompass.c
Normal file
|
|
@ -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 <math.h>
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
// 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
|
||||
25
ardriver/xreal/FusionCompass.h
Normal file
25
ardriver/xreal/FusionCompass.h
Normal file
|
|
@ -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
|
||||
25
ardriver/xreal/FusionConvention.h
Normal file
25
ardriver/xreal/FusionConvention.h
Normal file
|
|
@ -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
|
||||
481
ardriver/xreal/FusionMath.h
Normal file
481
ardriver/xreal/FusionMath.h
Normal file
|
|
@ -0,0 +1,481 @@
|
|||
/**
|
||||
* @file FusionMath.h
|
||||
* @author Seb Madgwick
|
||||
* @brief Math library.
|
||||
*/
|
||||
|
||||
#ifndef FUSION_MATH_H
|
||||
#define FUSION_MATH_H
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
// Includes
|
||||
|
||||
#include <math.h>
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
// 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
|
||||
77
ardriver/xreal/FusionOffset.c
Normal file
77
ardriver/xreal/FusionOffset.c
Normal file
|
|
@ -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 <math.h>
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
// 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
|
||||
40
ardriver/xreal/FusionOffset.h
Normal file
40
ardriver/xreal/FusionOffset.h
Normal file
|
|
@ -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
|
||||
5
ardriver/xreal/README.md
Normal file
5
ardriver/xreal/README.md
Normal file
|
|
@ -0,0 +1,5 @@
|
|||
# Xreal driver
|
||||
## Attribution
|
||||
|
||||
- [Xreal driver](https://gitlab.com/TheJackiMonster/nrealAirLinuxDriver)
|
||||
- [Fusion](https://github.com/xioTechnologies/Fusion)
|
||||
102
ardriver/xreal/crc32.c
Normal file
102
ardriver/xreal/crc32.c
Normal file
|
|
@ -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;
|
||||
}
|
||||
40
ardriver/xreal/crc32.h
Normal file
40
ardriver/xreal/crc32.h
Normal file
|
|
@ -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 <stdint.h>
|
||||
#else
|
||||
#include <cstdint>
|
||||
#endif
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
uint32_t crc32_checksum(const uint8_t* buf, uint32_t len);
|
||||
|
||||
#ifdef __cplusplus
|
||||
} // extern "C"
|
||||
#endif
|
||||
50
ardriver/xreal/device.c
Normal file
50
ardriver/xreal/device.c
Normal file
|
|
@ -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 <hidapi/hidapi.h>
|
||||
|
||||
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();
|
||||
}
|
||||
}
|
||||
46
ardriver/xreal/device.h
Normal file
46
ardriver/xreal/device.h
Normal file
|
|
@ -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 <stdbool.h>
|
||||
#endif
|
||||
|
||||
#ifndef __cplusplus
|
||||
#include <stdint.h>
|
||||
#else
|
||||
#include <cstdint>
|
||||
#endif
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
bool device_init();
|
||||
|
||||
void device_exit();
|
||||
|
||||
#ifdef __cplusplus
|
||||
} // extern "C"
|
||||
#endif
|
||||
1272
ardriver/xreal/device_imu.c
Normal file
1272
ardriver/xreal/device_imu.c
Normal file
File diff suppressed because it is too large
Load diff
227
ardriver/xreal/device_imu.h
Normal file
227
ardriver/xreal/device_imu.h
Normal file
|
|
@ -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 <stdbool.h>
|
||||
#endif
|
||||
|
||||
#ifndef __cplusplus
|
||||
#include <stdint.h>
|
||||
#else
|
||||
#include <cstdint>
|
||||
#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
|
||||
731
ardriver/xreal/device_mcu.c
Normal file
731
ardriver/xreal/device_mcu.c
Normal file
|
|
@ -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 <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <time.h>
|
||||
|
||||
#include <hidapi/hidapi.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
212
ardriver/xreal/device_mcu.h
Normal file
212
ardriver/xreal/device_mcu.h
Normal file
|
|
@ -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 <stdbool.h>
|
||||
#endif
|
||||
|
||||
#ifndef __cplusplus
|
||||
#include <stdint.h>
|
||||
#else
|
||||
#include <cstdint>
|
||||
#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
|
||||
7
ardriver/xreal/go_ffi.c
Normal file
7
ardriver/xreal/go_ffi.c
Normal file
|
|
@ -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);
|
||||
}
|
||||
3
ardriver/xreal/go_ffi.h
Normal file
3
ardriver/xreal/go_ffi.h
Normal file
|
|
@ -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);
|
||||
108
ardriver/xreal/hid_ids.c
Normal file
108
ardriver/xreal/hid_ids.c
Normal file
|
|
@ -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 <stdbool.h>
|
||||
#endif
|
||||
|
||||
#ifndef __cplusplus
|
||||
#include <stdint.h>
|
||||
#else
|
||||
#include <cstdint>
|
||||
#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;
|
||||
}
|
||||
}
|
||||
54
ardriver/xreal/hid_ids.h
Normal file
54
ardriver/xreal/hid_ids.h
Normal file
|
|
@ -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 <stdbool.h>
|
||||
#endif
|
||||
|
||||
#ifndef __cplusplus
|
||||
#include <stdint.h>
|
||||
#else
|
||||
#include <cstdint>
|
||||
#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
|
||||
134
ardriver/xreal/xreal.go
Normal file
134
ardriver/xreal/xreal.go
Normal file
|
|
@ -0,0 +1,134 @@
|
|||
//go:build xreal
|
||||
// +build xreal
|
||||
|
||||
package xreal
|
||||
|
||||
// #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"
|
||||
|
||||
"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
|
||||
}
|
||||
7
ardriver/xreal/xreal_debug_logging.go
Normal file
7
ardriver/xreal/xreal_debug_logging.go
Normal file
|
|
@ -0,0 +1,7 @@
|
|||
//go:build xreal && !xreal_debug_logging
|
||||
// +build xreal,!xreal_debug_logging
|
||||
|
||||
package xreal
|
||||
|
||||
// #cgo CFLAGS: -DNDEBUG
|
||||
import "C"
|
||||
44
ardriver/xreal/xreal_disabled.go
Normal file
44
ardriver/xreal/xreal_disabled.go
Normal file
|
|
@ -0,0 +1,44 @@
|
|||
//go:build !xreal
|
||||
// +build !xreal
|
||||
|
||||
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")
|
||||
}
|
||||
102
edidpatcher/edidpatcher.go
Normal file
102
edidpatcher/edidpatcher.go
Normal file
|
|
@ -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
|
||||
}
|
||||
34
go.mod
Normal file
34
go.mod
Normal file
|
|
@ -0,0 +1,34 @@
|
|||
module git.terah.dev/UnrealXR/unrealxr
|
||||
|
||||
go 1.24.3
|
||||
|
||||
require (
|
||||
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/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/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/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
|
||||
)
|
||||
65
go.sum
Normal file
65
go.sum
Normal file
|
|
@ -0,0 +1,65 @@
|
|||
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 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=
|
||||
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/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=
|
||||
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=
|
||||
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/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-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.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=
|
||||
|
|
@ -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")
|
||||
|
|
@ -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
|
||||
|
|
@ -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)
|
||||
|
|
@ -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()
|
||||
|
|
@ -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,
|
||||
}
|
||||
}
|
||||
}
|
||||
188
main.py
188
main.py
|
|
@ -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()
|
||||
|
|
@ -1 +0,0 @@
|
|||
Subproject commit 3673a4b34d386921fc323ddbd2ef0e000022e2d4
|
||||
|
|
@ -1 +0,0 @@
|
|||
Subproject commit 36ad789c9b5893653c523d4af6e24120ec19ab33
|
||||
|
|
@ -1 +0,0 @@
|
|||
Subproject commit 7c982c0d4e2773739f00eb91a2d2cbe87eb7c76d
|
||||
235
render.py
235
render.py
|
|
@ -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()
|
||||
|
|
@ -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
|
||||
11
shell.nix
11
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
|
||||
'';
|
||||
}
|
||||
|
|
|
|||
2
unrealxr
2
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" ./uxr
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue