From 243d595a3585a09f4104d5a35b441038a0ebdfc9 Mon Sep 17 00:00:00 2001 From: imterah Date: Tue, 24 Jun 2025 16:30:16 -0400 Subject: [PATCH] feature: Get sensor data reading working --- .zed/settings.json | 13 ++ Makefile | 2 +- app/main.go | 7 +- app/renderer/renderer.go | 51 +++++ ardriver/ardriver.go | 22 ++ ardriver/xreal/device_imu.c | 280 +++++++++++++------------- ardriver/xreal/go_ffi.c | 7 + ardriver/xreal/go_ffi.h | 3 + ardriver/xreal/xreal.go | 130 +++++++++++- ardriver/xreal/xreal_debug_logging.go | 7 + ardriver/xreal/xreal_disabled.go | 38 ++++ 11 files changed, 413 insertions(+), 147 deletions(-) create mode 100644 .zed/settings.json create mode 100644 app/renderer/renderer.go create mode 100644 ardriver/xreal/go_ffi.c create mode 100644 ardriver/xreal/go_ffi.h create mode 100644 ardriver/xreal/xreal_debug_logging.go diff --git a/.zed/settings.json b/.zed/settings.json new file mode 100644 index 0000000..11ceaea --- /dev/null +++ b/.zed/settings.json @@ -0,0 +1,13 @@ +// Folder-specific settings +// +// For a full list of overridable settings, and general information on folder-specific settings, +// see the documentation: https://zed.dev/docs/configuring-zed#settings-files +{ + "lsp": { + "gopls": { + "initialization_options": { + "buildFlags": ["-tags=xreal,noaudio,drm,drm_leasing,drm_disable_input"] + } + } + } +} diff --git a/Makefile b/Makefile index 822f8a8..0d040fb 100644 --- a/Makefile +++ b/Makefile @@ -1,6 +1,6 @@ APP_DIR := ./app OUTPUT := uxr -TAGS := xreal drm drm_leasing drm_disable_input +TAGS := xreal noaudio drm drm_leasing drm_disable_input .PHONY: all build clean diff --git a/app/main.go b/app/main.go index 8a6c89c..cd762c0 100644 --- a/app/main.go +++ b/app/main.go @@ -132,7 +132,7 @@ func mainEntrypoint(context.Context, *cli.Command) error { }, }) - displayMetadataBlock := make([]*renderer.EvdiDisplayMetadata, *config.DisplayConfig.Count) + evdiCards := make([]*renderer.EvdiDisplayMetadata, *config.DisplayConfig.Count) for currentDisplay := range *config.DisplayConfig.Count { openedDevice, err := libevdi.Open(nil) @@ -156,13 +156,16 @@ func mainEntrypoint(context.Context, *cli.Command) error { displayBuffer := openedDevice.CreateBuffer(displayMetadata.MaxWidth, displayMetadata.MaxHeight, 4, displayRect) - displayMetadataBlock[currentDisplay] = &renderer.EvdiDisplayMetadata{ + evdiCards[currentDisplay] = &renderer.EvdiDisplayMetadata{ EvdiNode: openedDevice, Rect: displayRect, Buffer: displayBuffer, } } + log.Info("Initialized displays. Entering rendering loop") + renderer.EnterRenderLoop(config, displayMetadata, evdiCards) + atexit.Exit(0) return nil } diff --git a/app/renderer/renderer.go b/app/renderer/renderer.go new file mode 100644 index 0000000..e3cb39a --- /dev/null +++ b/app/renderer/renderer.go @@ -0,0 +1,51 @@ +package renderer + +import ( + "time" + + libconfig "git.terah.dev/UnrealXR/unrealxr/app/config" + "git.terah.dev/UnrealXR/unrealxr/app/edidtools" + "git.terah.dev/UnrealXR/unrealxr/ardriver" + arcommons "git.terah.dev/UnrealXR/unrealxr/ardriver/commons" + "github.com/charmbracelet/log" + "github.com/tebeka/atexit" +) + +func EnterRenderLoop(config *libconfig.Config, displayMetadata *edidtools.DisplayMetadata, evdiCards []*EvdiDisplayMetadata) { + log.Info("Initializing AR driver") + headset, err := ardriver.GetDevice() + + if err != nil { + log.Errorf("Failed to get device: %s", err.Error()) + atexit.Exit(1) + } + log.Info("Initialized") + + var pitch float32 + var yaw float32 + var roll float32 + + arEventListner := &arcommons.AREventListener{ + PitchCallback: func(newPitch float32) { + pitch = newPitch + }, + YawCallback: func(newYaw float32) { + yaw = newYaw + }, + RollCallback: func(newRoll float32) { + roll = newRoll + }, + } + + if headset.IsPollingLibrary() { + log.Error("Connected AR headset requires polling but polling is not implemented in the renderer!") + atexit.Exit(1) + } + + headset.RegisterEventListeners(arEventListner) + + for { + log.Debugf("pitch: %f, yaw: %f, roll: %f", pitch, yaw, roll) + time.Sleep(10 * time.Millisecond) + } +} diff --git a/ardriver/ardriver.go b/ardriver/ardriver.go index b58104c..913e40a 100644 --- a/ardriver/ardriver.go +++ b/ardriver/ardriver.go @@ -1 +1,23 @@ package ardriver + +import ( + "fmt" + + "git.terah.dev/UnrealXR/unrealxr/ardriver/commons" + "git.terah.dev/UnrealXR/unrealxr/ardriver/xreal" +) + +func GetDevice() (commons.ARDevice, error) { + if xreal.IsXrealEnabled { + device, err := xreal.New() + + if err != nil { + fmt.Printf("failed to initialize xreal device: %w\n", err) + return nil, err + } + + return device, nil + } + + return nil, fmt.Errorf("failed to initialize any device") +} diff --git a/ardriver/xreal/device_imu.c b/ardriver/xreal/device_imu.c index add1338..1ed84bd 100644 --- a/ardriver/xreal/device_imu.c +++ b/ardriver/xreal/device_imu.c @@ -9,10 +9,10 @@ // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell // copies of the Software, and to permit persons to whom the Software is // furnished to do so, subject to the following conditions: -// +// // The above copyright notice and this permission notice shall be included in // all copies or substantial portions of the Software. -// +// // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE @@ -25,8 +25,8 @@ #include "device_imu.h" #include "device.h" -#include -#include +#include +#include #include #include #include @@ -35,7 +35,7 @@ #include #include -#include +#include #include #include @@ -78,18 +78,18 @@ struct device_imu_calibration_t { FusionMatrix gyroscopeMisalignment; FusionVector gyroscopeSensitivity; FusionVector gyroscopeOffset; - + FusionMatrix accelerometerMisalignment; FusionVector accelerometerSensitivity; FusionVector accelerometerOffset; - + FusionMatrix magnetometerMisalignment; FusionVector magnetometerSensitivity; FusionVector magnetometerOffset; - + FusionMatrix softIronMatrix; FusionVector hardIronOffset; - + FusionQuaternion noises; device_imu_camera_calibration_type cam; @@ -100,14 +100,14 @@ static bool send_payload(device_imu_type* device, uint16_t size, const uint8_t* if (payload_size > device->max_payload_size) { payload_size = device->max_payload_size; } - + int transferred = hid_write(device->handle, payload, payload_size); - + if (transferred != payload_size) { device_imu_error("Sending payload failed"); return false; } - + return (transferred == size); } @@ -116,9 +116,9 @@ static bool recv_payload(device_imu_type* device, uint16_t size, uint8_t* payloa if (payload_size > device->max_payload_size) { payload_size = device->max_payload_size; } - + int transferred = hid_read(device->handle, payload, payload_size); - + if (transferred >= payload_size) { transferred = payload_size; } @@ -126,12 +126,12 @@ static bool recv_payload(device_imu_type* device, uint16_t size, uint8_t* payloa if (transferred == 0) { return false; } - + if (transferred != payload_size) { device_imu_error("Receiving payload failed"); return false; } - + return (transferred == size); } @@ -147,14 +147,14 @@ typedef struct device_imu_payload_packet_t device_imu_payload_packet_type; static bool send_payload_msg(device_imu_type* device, uint8_t msgid, uint16_t len, const uint8_t* data) { static device_imu_payload_packet_type packet; - + const uint16_t packet_len = 3 + len; const uint16_t payload_len = 5 + packet_len; - + packet.head = 0xAA; packet.length = htole16(packet_len); packet.msgid = msgid; - + memcpy(packet.data, data, len); packet.checksum = htole32( crc32_checksum( @@ -162,7 +162,7 @@ static bool send_payload_msg(device_imu_type* device, uint8_t msgid, uint16_t le packet.length ) ); - + return send_payload(device, payload_len, (uint8_t*) (&packet)); } @@ -172,20 +172,20 @@ static bool send_payload_msg_signal(device_imu_type* device, uint8_t msgid, uint static bool recv_payload_msg(device_imu_type* device, uint8_t msgid, uint16_t len, uint8_t* data) { static device_imu_payload_packet_type packet; - + packet.head = 0; packet.length = 0; packet.msgid = 0; - + const uint16_t packet_len = 3 + len; const uint16_t payload_len = 5 + packet_len; - + do { if (!recv_payload(device, payload_len, (uint8_t*) (&packet))) { return false; } } while (packet.msgid != msgid); - + memcpy(data, packet.data, len); return true; } @@ -195,7 +195,7 @@ static FusionVector json_object_get_vector(struct json_object* obj) { (json_object_array_length(obj) != 3)) { return FUSION_VECTOR_ZERO; } - + FusionVector vector; vector.axis.x = (float) json_object_get_double(json_object_array_get_idx(obj, 0)); vector.axis.y = (float) json_object_get_double(json_object_array_get_idx(obj, 1)); @@ -208,7 +208,7 @@ static FusionQuaternion json_object_get_quaternion(struct json_object* obj) { (json_object_array_length(obj) != 4)) { return FUSION_IDENTITY_QUATERNION; } - + FusionQuaternion quaternion; quaternion.element.x = (float) json_object_get_double(json_object_array_get_idx(obj, 0)); quaternion.element.y = (float) json_object_get_double(json_object_array_get_idx(obj, 1)); @@ -284,7 +284,7 @@ static void init_device_imu_camera(device_imu_camera_type *camera, json_object * float* cc = sensor->cc; float* fc = sensor->fc; float* kc = sensor->kc; - + json_object_get_array_u16(json_object_object_get(dev, "resolution"), &resolution, 2); json_object_get_array_f32(json_object_object_get(dev, "cc"), &cc, 2); @@ -303,19 +303,19 @@ device_imu_error_type device_imu_open(device_imu_type* device, device_imu_event_ device_imu_error("No device"); return DEVICE_IMU_ERROR_NO_DEVICE; } - + memset(device, 0, sizeof(device_imu_type)); device->vendor_id = xreal_vendor_id; device->product_id = 0; device->callback = callback; - + if (!device_init()) { device_imu_error("Not initialized"); return DEVICE_IMU_ERROR_NOT_INITIALIZED; } struct hid_device_info* info = hid_enumerate( - device->vendor_id, + device->vendor_id, device->product_id ); @@ -336,7 +336,7 @@ device_imu_error_type device_imu_open(device_imu_type* device, device_imu_event_ } hid_free_enumeration(info); - + if (!device->handle) { device_imu_error("No handle"); return DEVICE_IMU_ERROR_NO_HANDLE; @@ -349,19 +349,19 @@ device_imu_error_type device_imu_open(device_imu_type* device, device_imu_event_ } device_imu_clear(device); - + if (!send_payload_msg(device, DEVICE_IMU_MSG_GET_STATIC_ID, 0, NULL)) { device_imu_error("Failed sending payload to get static id"); return DEVICE_IMU_ERROR_PAYLOAD_FAILED; } - + uint32_t static_id = 0; if (recv_payload_msg(device, DEVICE_IMU_MSG_GET_STATIC_ID, 4, (uint8_t*) &static_id)) { device->static_id = static_id; } else { device->static_id = 0x20220101; } - + device->calibration = malloc(sizeof(device_imu_calibration_type)); memset(device->calibration, 0, sizeof(device_imu_calibration_type)); @@ -371,12 +371,12 @@ device_imu_error_type device_imu_open(device_imu_type* device, device_imu_event_ device_imu_error("Failed sending payload to get calibration data length"); return DEVICE_IMU_ERROR_PAYLOAD_FAILED; } - + uint32_t calibration_len = 0; if (recv_payload_msg(device, DEVICE_IMU_MSG_GET_CAL_DATA_LENGTH, 4, (uint8_t*) &calibration_len)) { const uint16_t max_packet_size = (device->max_payload_size - 8); char* calibration_data = malloc(calibration_len + 1); - + uint32_t position = 0; while (position < calibration_len) { const uint32_t remaining = (calibration_len - position); @@ -384,23 +384,23 @@ device_imu_error_type device_imu_open(device_imu_type* device, device_imu_event_ if (!send_payload_msg(device, DEVICE_IMU_MSG_CAL_DATA_GET_NEXT_SEGMENT, 0, NULL)) { break; } - + const uint16_t next = (remaining > max_packet_size? max_packet_size : remaining); - + if (!recv_payload_msg(device, DEVICE_IMU_MSG_CAL_DATA_GET_NEXT_SEGMENT, next, (uint8_t*) calibration_data + position)) { break; } - + position += next; } calibration_data[calibration_len] = '\0'; - + struct json_tokener* tokener = json_tokener_new(); struct json_object* root = json_tokener_parse_ex(tokener, calibration_data, calibration_len); struct json_object* imu = json_object_object_get(root, "IMU"); struct json_object* dev1 = json_object_object_get(imu, "device_1"); - + FusionVector accel_bias = json_object_get_vector(json_object_object_get(dev1, "accel_bias")); FusionQuaternion accel_q_gyro = json_object_get_quaternion(json_object_object_get(dev1, "accel_q_gyro")); FusionVector gyro_bias = json_object_get_vector(json_object_object_get(dev1, "gyro_bias")); @@ -412,19 +412,19 @@ device_imu_error_type device_imu_open(device_imu_type* device, device_imu_event_ FusionVector scale_mag = json_object_get_vector(json_object_object_get(dev1, "scale_mag")); const FusionQuaternion accel_q_mag = FusionQuaternionMultiply(accel_q_gyro, gyro_q_mag); - + device->calibration->gyroscopeMisalignment = FusionQuaternionToMatrix(accel_q_gyro); device->calibration->gyroscopeSensitivity = scale_gyro; device->calibration->gyroscopeOffset = gyro_bias; - + device->calibration->accelerometerMisalignment = FUSION_IDENTITY_MATRIX; device->calibration->accelerometerSensitivity = scale_accel; device->calibration->accelerometerOffset = accel_bias; - + device->calibration->magnetometerMisalignment = FusionQuaternionToMatrix(accel_q_mag); device->calibration->magnetometerSensitivity = scale_mag; device->calibration->magnetometerOffset = mag_bias; - + device->calibration->noises = imu_noises; struct json_object* rgb = json_object_object_get(root, "RGB_camera"); @@ -452,7 +452,7 @@ device_imu_error_type device_imu_open(device_imu_type* device, device_imu_event_ device->calibration->cam.num_cameras = cameras? num_cameras : 0; device->calibration->cam.cameras = cameras; - + json_tokener_free(tokener); free(calibration_data); } @@ -464,16 +464,16 @@ device_imu_error_type device_imu_open(device_imu_type* device, device_imu_event_ } const uint32_t SAMPLE_RATE = 1000; - + device->offset = malloc(sizeof(FusionOffset)); device->ahrs = malloc(sizeof(FusionAhrs)); - + if (device->offset) { FusionOffsetInitialise((FusionOffset*) device->offset, SAMPLE_RATE); } FusionAhrsInitialise((FusionAhrs*) device->ahrs); - + const FusionAhrsSettings settings = { .convention = FusionConventionNed, .gain = 0.5f, @@ -481,7 +481,7 @@ device_imu_error_type device_imu_open(device_imu_type* device, device_imu_event_ .magneticRejection = 20.0f, .recoveryTriggerPeriod = 5 * SAMPLE_RATE, /* 5 seconds */ }; - + FusionAhrsSetSettings((FusionAhrs*) device->ahrs, &settings); return DEVICE_IMU_ERROR_NO_ERROR; } @@ -491,27 +491,27 @@ device_imu_error_type device_imu_reset_calibration(device_imu_type* device) { device_imu_error("No device"); return DEVICE_IMU_ERROR_NO_DEVICE; } - + if (!device->calibration) { device_imu_error("Not allocated"); return DEVICE_IMU_ERROR_NO_ALLOCATION; } - + device->calibration->gyroscopeMisalignment = FUSION_IDENTITY_MATRIX; device->calibration->gyroscopeSensitivity = FUSION_VECTOR_ONES; device->calibration->gyroscopeOffset = FUSION_VECTOR_ZERO; - + device->calibration->accelerometerMisalignment = FUSION_IDENTITY_MATRIX; device->calibration->accelerometerSensitivity = FUSION_VECTOR_ONES; device->calibration->accelerometerOffset = FUSION_VECTOR_ZERO; - + device->calibration->magnetometerMisalignment = FUSION_IDENTITY_MATRIX; device->calibration->magnetometerSensitivity = FUSION_VECTOR_ONES; device->calibration->magnetometerOffset = FUSION_VECTOR_ZERO; - + device->calibration->softIronMatrix = FUSION_IDENTITY_MATRIX; device->calibration->hardIronOffset = FUSION_VECTOR_ZERO; - + device->calibration->noises = FUSION_IDENTITY_QUATERNION; device->calibration->noises.element.w = 0.0f; @@ -550,12 +550,12 @@ device_imu_error_type device_imu_load_calibration(device_imu_type* device, const device_imu_error("No device"); return DEVICE_IMU_ERROR_NO_DEVICE; } - + if (!device->calibration) { device_imu_error("Not allocated"); return DEVICE_IMU_ERROR_NO_ALLOCATION; } - + FILE* file = fopen(path, "rb"); if (!file) { device_imu_error("No file opened"); @@ -566,20 +566,20 @@ device_imu_error_type device_imu_load_calibration(device_imu_type* device, const const size_t calibration_size = ( sizeof(device_imu_calibration_type) - sizeof(device_imu_camera_calibration_type) ); - + size_t count; count = fread(device->calibration, 1, calibration_size, file); - + if (calibration_size != count) { device_imu_error("Not fully loaded"); result = DEVICE_IMU_ERROR_LOADING_FAILED; } - + if (0 != fclose(file)) { device_imu_error("No file closed"); return DEVICE_IMU_ERROR_FILE_NOT_CLOSED; } - + return result; } @@ -588,12 +588,12 @@ device_imu_error_type device_imu_save_calibration(device_imu_type* device, const device_imu_error("No device"); return DEVICE_IMU_ERROR_NO_DEVICE; } - + if (!device->calibration) { device_imu_error("Not allocated"); return DEVICE_IMU_ERROR_NO_ALLOCATION; } - + FILE* file = fopen(path, "wb"); if (!file) { device_imu_error("No file opened"); @@ -604,20 +604,20 @@ device_imu_error_type device_imu_save_calibration(device_imu_type* device, const const size_t calibration_size = ( sizeof(device_imu_calibration_type) - sizeof(device_imu_camera_calibration_type) ); - + size_t count; count = fwrite(device->calibration, 1, calibration_size, file); - + if (calibration_size != count) { device_imu_error("Not fully saved"); result = DEVICE_IMU_ERROR_SAVING_FAILED; } - + if (0 != fclose(file)) { device_imu_error("No file closed"); return DEVICE_IMU_ERROR_FILE_NOT_CLOSED; } - + return result; } @@ -627,7 +627,7 @@ static void device_imu_callback(device_imu_type* device, if (!device->callback) { return; } - + device->callback(timestamp, event, device->ahrs); } @@ -668,33 +668,33 @@ static void readIMU_from_packet(const device_imu_packet_type* packet, FusionVector* magnetometer) { int32_t vel_m = pack16bit_signed(packet->angular_multiplier); int32_t vel_d = pack32bit_signed(packet->angular_divisor); - + int32_t vel_x = pack24bit_signed(packet->angular_velocity_x); int32_t vel_y = pack24bit_signed(packet->angular_velocity_y); int32_t vel_z = pack24bit_signed(packet->angular_velocity_z); - + gyroscope->axis.x = (float) vel_x * (float) vel_m / (float) vel_d; gyroscope->axis.y = (float) vel_y * (float) vel_m / (float) vel_d; gyroscope->axis.z = (float) vel_z * (float) vel_m / (float) vel_d; - + int32_t accel_m = pack16bit_signed(packet->acceleration_multiplier); int32_t accel_d = pack32bit_signed(packet->acceleration_divisor); - + int32_t accel_x = pack24bit_signed(packet->acceleration_x); int32_t accel_y = pack24bit_signed(packet->acceleration_y); int32_t accel_z = pack24bit_signed(packet->acceleration_z); - + accelerometer->axis.x = (float) accel_x * (float) accel_m / (float) accel_d; accelerometer->axis.y = (float) accel_y * (float) accel_m / (float) accel_d; accelerometer->axis.z = (float) accel_z * (float) accel_m / (float) accel_d; - + int32_t magnet_m = pack16bit_signed_swap(packet->magnetic_multiplier); int32_t magnet_d = pack32bit_signed_swap(packet->magnetic_divisor); - + int16_t magnet_x = pack16bit_signed_bizarre(packet->magnetic_x); int16_t magnet_y = pack16bit_signed_bizarre(packet->magnetic_y); int16_t magnet_z = pack16bit_signed_bizarre(packet->magnetic_z); - + magnetometer->axis.x = (float) magnet_x * (float) magnet_m / (float) magnet_d; magnetometer->axis.y = (float) magnet_y * (float) magnet_m / (float) magnet_d; magnetometer->axis.z = (float) magnet_z * (float) magnet_m / (float) magnet_d; @@ -719,17 +719,17 @@ static void iterate_iron_offset_estimation(const FusionVector* magnetometer, Fus max.array[i] = max(max.array[i], magnetometer->array[i]); min.array[i] = min(min.array[i], magnetometer->array[i]); } - + const float mx = (max.axis.x - min.axis.x) / 2.0f; const float my = (max.axis.y - min.axis.y) / 2.0f; const float mz = (max.axis.z - min.axis.z) / 2.0f; - + const float cx = (min.axis.x + max.axis.x) / 2.0f; const float cy = (min.axis.y + max.axis.y) / 2.0f; const float cz = (min.axis.z + max.axis.z) / 2.0f; memset(softIronMatrix, 0, sizeof(*softIronMatrix)); - + softIronMatrix->element.xx = 1.0f / mx; softIronMatrix->element.yy = 1.0f / my; softIronMatrix->element.zz = 1.0f / mz; @@ -746,57 +746,57 @@ static void apply_calibration(const device_imu_type* device, FusionMatrix gyroscopeMisalignment; FusionVector gyroscopeSensitivity; FusionVector gyroscopeOffset; - + FusionMatrix accelerometerMisalignment; FusionVector accelerometerSensitivity; FusionVector accelerometerOffset; - + FusionMatrix magnetometerMisalignment; FusionVector magnetometerSensitivity; FusionVector magnetometerOffset; - + FusionMatrix softIronMatrix; FusionVector hardIronOffset; - + if (device->calibration) { gyroscopeMisalignment = device->calibration->gyroscopeMisalignment; gyroscopeSensitivity = device->calibration->gyroscopeSensitivity; gyroscopeOffset = device->calibration->gyroscopeOffset; - + accelerometerMisalignment = device->calibration->accelerometerMisalignment; accelerometerSensitivity = device->calibration->accelerometerSensitivity; accelerometerOffset = device->calibration->accelerometerOffset; - + magnetometerMisalignment = device->calibration->magnetometerMisalignment; magnetometerSensitivity = device->calibration->magnetometerSensitivity; magnetometerOffset = device->calibration->magnetometerOffset; - + softIronMatrix = device->calibration->softIronMatrix; hardIronOffset = device->calibration->hardIronOffset; } else { gyroscopeMisalignment = FUSION_IDENTITY_MATRIX; gyroscopeSensitivity = FUSION_VECTOR_ONES; gyroscopeOffset = FUSION_VECTOR_ZERO; - + accelerometerMisalignment = FUSION_IDENTITY_MATRIX; accelerometerSensitivity = FUSION_VECTOR_ONES; accelerometerOffset = FUSION_VECTOR_ZERO; - + magnetometerMisalignment = FUSION_IDENTITY_MATRIX; magnetometerSensitivity = FUSION_VECTOR_ONES; magnetometerOffset = FUSION_VECTOR_ZERO; - + softIronMatrix = FUSION_IDENTITY_MATRIX; hardIronOffset = FUSION_VECTOR_ZERO; } gyroscopeOffset = FusionVectorMultiplyScalar( - gyroscopeOffset, + gyroscopeOffset, FusionRadiansToDegrees(1.0f) ); accelerometerOffset = FusionVectorMultiplyScalar( - accelerometerOffset, + accelerometerOffset, 1.0f / GRAVITY_G ); @@ -814,14 +814,14 @@ static void apply_calibration(const device_imu_type* device, gyroscopeSensitivity, gyroscopeOffset ); - + a = FusionCalibrationInertial( a, accelerometerMisalignment, accelerometerSensitivity, accelerometerOffset ); - + m = FusionCalibrationInertial( m, magnetometerMisalignment, @@ -830,16 +830,16 @@ static void apply_calibration(const device_imu_type* device, ); iterate_iron_offset_estimation( - &m, - &softIronMatrix, + &m, + &softIronMatrix, &hardIronOffset ); - + if (device->calibration) { device->calibration->softIronMatrix = softIronMatrix; device->calibration->hardIronOffset = hardIronOffset; } - + m = FusionCalibrationMagnetic( m, softIronMatrix, @@ -870,32 +870,32 @@ device_imu_error_type device_imu_calibrate(device_imu_type* device, uint32_t ite device_imu_error("No calibration allocated"); return DEVICE_IMU_ERROR_NO_ALLOCATION; } - + if (sizeof(device_imu_packet_type) > device->max_payload_size) { device_imu_error("Not proper size"); return DEVICE_IMU_ERROR_WRONG_SIZE; } - + device_imu_packet_type packet; int transferred; - + bool initialized = false; - + FusionVector cal_gyroscope; FusionVector cal_accelerometer; FusionMatrix softIronMatrix; FusionVector hardIronOffset; - + const float factor = iterations > 0? 1.0f / ((float) iterations) : 0.0f; FusionVector prev_accel; while (iterations > 0) { memset(&packet, 0, sizeof(device_imu_packet_type)); - + transferred = hid_read( - device->handle, - (uint8_t*) &packet, + device->handle, + (uint8_t*) &packet, sizeof(device_imu_packet_type) ); @@ -912,17 +912,17 @@ device_imu_error_type device_imu_calibrate(device_imu_type* device, uint32_t ite device_imu_error("Unexpected packet size"); return DEVICE_IMU_ERROR_UNEXPECTED; } - + if ((packet.signature[0] != 0x01) || (packet.signature[1] != 0x02)) { continue; } - + FusionVector gyroscope; FusionVector accelerometer; FusionVector magnetometer; - + readIMU_from_packet(&packet, &gyroscope, &accelerometer, &magnetometer); - + pre_biased_coordinate_system(&gyroscope); pre_biased_coordinate_system(&accelerometer); pre_biased_coordinate_system(&magnetometer); @@ -938,14 +938,14 @@ device_imu_error_type device_imu_calibrate(device_imu_type* device, uint32_t ite prev_accel = accelerometer; iterate_iron_offset_estimation( - &magnetometer, - &softIronMatrix, + &magnetometer, + &softIronMatrix, &hardIronOffset ); - + iterations--; } - + if (factor > 0.0f) { if (gyro) { device->calibration->gyroscopeOffset = FusionVectorAdd( @@ -956,7 +956,7 @@ device_imu_error_type device_imu_calibrate(device_imu_type* device, uint32_t ite ) ); } - + if (accel) { device->calibration->accelerometerOffset = FusionVectorAdd( device->calibration->accelerometerOffset, @@ -966,13 +966,13 @@ device_imu_error_type device_imu_calibrate(device_imu_type* device, uint32_t ite ) ); } - + if (magnet) { device->calibration->softIronMatrix = softIronMatrix; device->calibration->hardIronOffset = hardIronOffset; } } - + return DEVICE_IMU_ERROR_NO_ERROR; } @@ -986,18 +986,18 @@ device_imu_error_type device_imu_read(device_imu_type* device, int timeout) { device_imu_error("No handle"); return DEVICE_IMU_ERROR_NO_HANDLE; } - + if (sizeof(device_imu_packet_type) > device->max_payload_size) { device_imu_error("Not proper size"); return DEVICE_IMU_ERROR_WRONG_SIZE; } - + device_imu_packet_type packet; memset(&packet, 0, sizeof(device_imu_packet_type)); - + int transferred = hid_read_timeout( - device->handle, - (uint8_t*) &packet, + device->handle, + (uint8_t*) &packet, sizeof(device_imu_packet_type), timeout ); @@ -1006,49 +1006,49 @@ device_imu_error_type device_imu_read(device_imu_type* device, int timeout) { device_imu_error("Device may be unplugged"); return DEVICE_IMU_ERROR_UNPLUGGED; } - + if (transferred == 0) { return DEVICE_IMU_ERROR_NO_ERROR; } - + if (sizeof(device_imu_packet_type) != transferred) { device_imu_error("Unexpected packet size"); return DEVICE_IMU_ERROR_UNEXPECTED; } - + const uint64_t timestamp = le64toh(packet.timestamp); - + if ((packet.signature[0] == 0xaa) && (packet.signature[1] == 0x53)) { device_imu_callback(device, timestamp, DEVICE_IMU_EVENT_INIT); return DEVICE_IMU_ERROR_NO_ERROR; } - + if ((packet.signature[0] != 0x01) || (packet.signature[1] != 0x02)) { device_imu_error("Not matching signature"); return DEVICE_IMU_ERROR_WRONG_SIGNATURE; } - + const uint64_t delta = timestamp - device->last_timestamp; const float deltaTime = (float) ((double) delta / 1e9); - + device->last_timestamp = timestamp; - + int16_t temperature = pack16bit_signed(packet.temperature); - + // According to the ICM-42688-P datasheet: (offset: 25 °C, sensitivity: 132.48 LSB/°C) device->temperature = ((float) temperature) / 132.48f + 25.0f; - + FusionVector gyroscope; FusionVector accelerometer; FusionVector magnetometer; - + readIMU_from_packet(&packet, &gyroscope, &accelerometer, &magnetometer); apply_calibration(device, &gyroscope, &accelerometer, &magnetometer); - + if (device->offset) { gyroscope = FusionOffsetUpdate((FusionOffset*) device->offset, gyroscope); } - + #ifndef NDEBUG printf("G: %.2f %.2f %.2f\n", gyroscope.axis.x, gyroscope.axis.y, gyroscope.axis.z); printf("A: %.2f %.2f %.2f\n", accelerometer.axis.x, accelerometer.axis.y, accelerometer.axis.z); @@ -1073,7 +1073,7 @@ device_imu_error_type device_imu_read(device_imu_type* device, int timeout) { return DEVICE_IMU_ERROR_INVALID_VALUE; } } - + device_imu_callback(device, timestamp, DEVICE_IMU_EVENT_UPDATE); return DEVICE_IMU_ERROR_NO_ERROR; } @@ -1223,7 +1223,7 @@ device_imu_error_type device_imu_close(device_imu_type* device) { device_imu_error("No device"); return DEVICE_IMU_ERROR_NO_DEVICE; } - + if (device->calibration) { if (device->calibration->cam.cameras) { for (uint32_t i = 0; i < device->calibration->cam.num_cameras; i++) { @@ -1247,11 +1247,11 @@ device_imu_error_type device_imu_close(device_imu_type* device) { free(device->calibration); } - + if (device->ahrs) { free(device->ahrs); } - + if (device->offset) { free(device->offset); } @@ -1264,7 +1264,7 @@ device_imu_error_type device_imu_close(device_imu_type* device) { hid_close(device->handle); } - + memset(device, 0, sizeof(device_imu_type)); device_exit(); diff --git a/ardriver/xreal/go_ffi.c b/ardriver/xreal/go_ffi.c new file mode 100644 index 0000000..558a65b --- /dev/null +++ b/ardriver/xreal/go_ffi.c @@ -0,0 +1,7 @@ +#include "device_imu.h" + +extern void goIMUEventHandler(uint64_t, device_imu_event_type, device_imu_ahrs_type*); + +void imuEventHandler(uint64_t timestamp, device_imu_event_type event, const device_imu_ahrs_type* ahrs) { + goIMUEventHandler(timestamp, event, (device_imu_ahrs_type*)ahrs); +} diff --git a/ardriver/xreal/go_ffi.h b/ardriver/xreal/go_ffi.h new file mode 100644 index 0000000..7972a47 --- /dev/null +++ b/ardriver/xreal/go_ffi.h @@ -0,0 +1,3 @@ +#include "device_imu.h" +extern void goIMUEventHandler(uint64_t, device_imu_event_type, device_imu_ahrs_type*); +void imuEventHandler(uint64_t timestamp, device_imu_event_type event, const device_imu_ahrs_type* ahrs); diff --git a/ardriver/xreal/xreal.go b/ardriver/xreal/xreal.go index a99fe12..08343b3 100644 --- a/ardriver/xreal/xreal.go +++ b/ardriver/xreal/xreal.go @@ -3,10 +3,132 @@ package xreal -// #include "evdi_lib.h" -// #include "go_ffi.h" -// #cgo CFLAGS: -w +// #cgo CFLAGS: -w -I./Fusion/ // #cgo pkg-config: json-c libusb-1.0 hidapi-libusb +// #include "go_ffi.h" +// #include "device.h" +// #include "device_imu.h" +// #include "device_mcu.h" import "C" +import ( + "fmt" + "sync" + "time" -var IsXrealEnabled = true + "git.terah.dev/UnrealXR/unrealxr/ardriver/commons" +) + +var ( + IsXrealEnabled = true + deviceEventHandlerMutex = sync.Mutex{} + deviceEventListener *commons.AREventListener +) + +//export goIMUEventHandler +func goIMUEventHandler(_ C.uint64_t, event_type C.device_imu_event_type, ahrs *C.struct_device_imu_ahrs_t) { + if deviceEventListener == nil { + return + } + + if event_type != C.DEVICE_IMU_EVENT_UPDATE { + return + } + + orientation := C.device_imu_get_orientation(ahrs) + euler := C.device_imu_get_euler(orientation) + + deviceEventListener.PitchCallback(float32(euler.pitch)) + deviceEventListener.RollCallback(float32(euler.roll)) + deviceEventListener.YawCallback(float32(euler.yaw)) +} + +// Implements commons.ARDevice +type XrealDevice struct { + eventListener *commons.AREventListener + imuDevice *C.struct_device_imu_t + deviceIsOpen bool +} + +func (device *XrealDevice) Initialize() error { + if device.deviceIsOpen { + return fmt.Errorf("device is already open") + } + + device.imuDevice = &C.struct_device_imu_t{} + + // (*[0]byte) is a FUBAR way to cast a pointer to a function, but unsafe.Pointer doesn't work: + // cannot use unsafe.Pointer(_Cgo_ptr(_Cfpvar_fp_imuEventHandler)) (value of type unsafe.Pointer) as *[0]byte value in variable declaration + if C.DEVICE_IMU_ERROR_NO_ERROR != C.device_imu_open(device.imuDevice, (*[0]byte)(C.imuEventHandler)) { + return fmt.Errorf("failed to open IMU device") + } + + C.device_imu_clear(device.imuDevice) + C.device_imu_calibrate(device.imuDevice, 1000, true, true, false) + + device.deviceIsOpen = true + + // let's hope this doesn't cause race conditions + go func() { + for device.eventListener == nil { + time.Sleep(time.Millisecond * 10) + } + + for { + if !device.deviceIsOpen { + break + } + + // I'm sorry. + deviceEventHandlerMutex.Lock() + deviceEventListener = device.eventListener + status := C.device_imu_read(device.imuDevice, -1) + deviceEventHandlerMutex.Unlock() + + if status != C.DEVICE_IMU_ERROR_NO_ERROR { + break + } + } + + device.deviceIsOpen = false + C.device_imu_close(device.imuDevice) + }() + + return nil +} + +func (device *XrealDevice) End() error { + if !device.deviceIsOpen { + return fmt.Errorf("device is not open") + } + + C.device_imu_close(device.imuDevice) + device.deviceIsOpen = false + return nil +} + +func (device *XrealDevice) IsPollingLibrary() bool { + return false +} + +func (device *XrealDevice) IsEventBasedLibrary() bool { + return true +} + +func (device *XrealDevice) Poll() error { + return fmt.Errorf("not supported") +} + +func (device *XrealDevice) RegisterEventListeners(listener *commons.AREventListener) { + device.eventListener = listener +} + +func New() (*XrealDevice, error) { + device := &XrealDevice{} + err := device.Initialize() + + if err != nil { + return nil, err + } + + return device, nil +} diff --git a/ardriver/xreal/xreal_debug_logging.go b/ardriver/xreal/xreal_debug_logging.go new file mode 100644 index 0000000..0d4eab9 --- /dev/null +++ b/ardriver/xreal/xreal_debug_logging.go @@ -0,0 +1,7 @@ +//go:build xreal && !xreal_debug_logging +// +build xreal,!xreal_debug_logging + +package xreal + +// #cgo CFLAGS: -DNDEBUG +import "C" diff --git a/ardriver/xreal/xreal_disabled.go b/ardriver/xreal/xreal_disabled.go index 1cbe85c..c6ca903 100644 --- a/ardriver/xreal/xreal_disabled.go +++ b/ardriver/xreal/xreal_disabled.go @@ -3,4 +3,42 @@ package xreal +import ( + "fmt" + + "git.terah.dev/UnrealXR/unrealxr/ardriver/commons" +) + var IsXrealEnabled = false + +// Implements commons.ARDevice +type XrealDevice struct { +} + +func (device *XrealDevice) Initialize() error { + return fmt.Errorf("xreal is not enabled") +} + +func (device *XrealDevice) End() error { + return fmt.Errorf("xreal is not enabled") +} + +func (device *XrealDevice) IsPollingLibrary() bool { + return false +} + +func (device *XrealDevice) IsEventBasedLibrary() bool { + return false +} + +func (device *XrealDevice) Poll() error { + return fmt.Errorf("xreal is not enabled") +} + +func (device *XrealDevice) RegisterEventListeners(*commons.AREventListener) error { + return fmt.Errorf("xreal is not enabled") +} + +func New() (*XrealDevice, error) { + return nil, fmt.Errorf("xreal is not enabled") +}