From 33f87340a873eda4a8add41d9a06c32e7824f725 Mon Sep 17 00:00:00 2001 From: wheaney <42350981+wheaney@users.noreply.github.com> Date: Thu, 25 Sep 2025 21:42:30 -0700 Subject: [PATCH 1/5] Add XREAL One support Refactor IMU to allow for differing protocols --- .gitmodules | 3 + examples/debug_cam/CMakeLists.txt | 7 +- interface_lib/CMakeLists.txt | 21 ++ interface_lib/include/device_imu.h | 11 +- interface_lib/include/hid_ids.h | 5 +- interface_lib/include/imu_protocol.h | 49 ++++ interface_lib/modules/xreal_one_driver | 1 + interface_lib/src/device_imu.c | 317 +++++-------------------- interface_lib/src/hid_ids.c | 65 ++++- interface_lib/src/imu_protocol_hid.c | 278 ++++++++++++++++++++++ interface_lib/src/imu_protocol_xo.c | 72 ++++++ 11 files changed, 553 insertions(+), 276 deletions(-) create mode 100644 interface_lib/include/imu_protocol.h create mode 160000 interface_lib/modules/xreal_one_driver create mode 100644 interface_lib/src/imu_protocol_hid.c create mode 100644 interface_lib/src/imu_protocol_xo.c diff --git a/.gitmodules b/.gitmodules index 8696ec6..1d3a4ac 100644 --- a/.gitmodules +++ b/.gitmodules @@ -4,3 +4,6 @@ [submodule "interface_lib/modules/hidapi"] path = interface_lib/modules/hidapi url = https://github.com/libusb/hidapi.git +[submodule "interface_lib/modules/xreal_one_driver"] + path = interface_lib/modules/xreal_one_driver + url = https://github.com/wheaney/xreal_one_driver.git diff --git a/examples/debug_cam/CMakeLists.txt b/examples/debug_cam/CMakeLists.txt index b83ee7c..24d903e 100644 --- a/examples/debug_cam/CMakeLists.txt +++ b/examples/debug_cam/CMakeLists.txt @@ -3,7 +3,12 @@ project(xrealAirDebugCamera CXX) set(CMAKE_CXX_STANDARD 17) -find_package(OpenCV REQUIRED) +# Only require the OpenCV components actually used to avoid pulling optional deps (viz/hdf) +find_package(OpenCV QUIET COMPONENTS core imgproc highgui videoio calib3d) +if(NOT OpenCV_FOUND) + message(WARNING "OpenCV components (core,imgproc,highgui,videoio,calib3d) not found; skipping xrealAirDebugCamera example") + return() +endif() include_directories( ${OpenCV_INCLUDE_DIRS} diff --git a/interface_lib/CMakeLists.txt b/interface_lib/CMakeLists.txt index 07aeace..903f0dd 100644 --- a/interface_lib/CMakeLists.txt +++ b/interface_lib/CMakeLists.txt @@ -8,6 +8,16 @@ find_package(json-c REQUIRED CONFIG) add_subdirectory(modules/hidapi) add_subdirectory(modules/Fusion/Fusion) +set(PROTOCOL_SOURCES src/imu_protocol_hid.c) + +# Conditionally include xreal_one protocol if header exists +set(XOD_ROOT "${CMAKE_CURRENT_SOURCE_DIR}/modules/xreal_one_driver") +set(XOD_HAVE_HEADER OFF) +if(EXISTS "${XOD_ROOT}/include/xreal_one_driver.h") + set(XOD_HAVE_HEADER ON) + list(APPEND PROTOCOL_SOURCES src/imu_protocol_xo.c) +endif() + add_library( xrealAirLibrary src/crc32.c @@ -15,6 +25,7 @@ add_library( src/device_imu.c src/device_mcu.c src/hid_ids.c + ${PROTOCOL_SOURCES} ) target_compile_options(xrealAirLibrary PRIVATE -fPIC) @@ -29,10 +40,20 @@ target_include_directories(xrealAirLibrary ${CMAKE_CURRENT_SOURCE_DIR}/modules/Fusion ) +if(XOD_HAVE_HEADER) + target_include_directories(xrealAirLibrary SYSTEM BEFORE PRIVATE "${XOD_ROOT}/include") +endif() + target_link_libraries(xrealAirLibrary PRIVATE hidapi::hidapi json-c::json-c Fusion m ) +# Optionally bring in xreal_one_driver to satisfy the XO protocol implementation +if(EXISTS "${XOD_ROOT}/CMakeLists.txt") + add_subdirectory("${XOD_ROOT}" "${CMAKE_CURRENT_BINARY_DIR}/xreal_one_driver") + target_link_libraries(xrealAirLibrary PRIVATE xreal_one_driver) +endif() + set(XREAL_AIR_INCLUDE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/include PARENT_SCOPE) set(XREAL_AIR_LIBRARY xrealAirLibrary PARENT_SCOPE) diff --git a/interface_lib/include/device_imu.h b/interface_lib/include/device_imu.h index 7474343..83f96cc 100644 --- a/interface_lib/include/device_imu.h +++ b/interface_lib/include/device_imu.h @@ -33,14 +33,7 @@ #include #endif -#define DEVICE_IMU_MSG_GET_CAL_DATA_LENGTH 0x14 -#define DEVICE_IMU_MSG_CAL_DATA_GET_NEXT_SEGMENT 0x15 -#define DEVICE_IMU_MSG_ALLOCATE_CAL_DATA_BUFFER 0x16 -#define DEVICE_IMU_MSG_WRITE_CAL_DATA_SEGMENT 0x17 -#define DEVICE_IMU_MSG_FREE_CAL_BUFFER 0x18 -#define DEVICE_IMU_MSG_START_IMU_DATA 0x19 -#define DEVICE_IMU_MSG_GET_STATIC_ID 0x1A -#define DEVICE_IMU_MSG_UNKNOWN 0x1D +#include "imu_protocol.h" #ifdef __cplusplus extern "C" { @@ -174,6 +167,8 @@ struct device_imu_t { device_imu_event_callback callback; device_imu_calibration_type* calibration; + + const struct imu_protocol* protocol; }; typedef struct device_imu_t device_imu_type; diff --git a/interface_lib/include/hid_ids.h b/interface_lib/include/hid_ids.h index 7c6aa5c..50036ac 100644 --- a/interface_lib/include/hid_ids.h +++ b/interface_lib/include/hid_ids.h @@ -33,7 +33,9 @@ #include #endif -#define NUM_SUPPORTED_PRODUCTS 4 +#include "imu_protocol.h" + +#define NUM_SUPPORTED_PRODUCTS 8 #ifdef __cplusplus extern "C" { @@ -44,6 +46,7 @@ extern const uint16_t xreal_product_ids [NUM_SUPPORTED_PRODUCTS]; bool is_xreal_product_id(uint16_t product_id); +const imu_protocol* xreal_imu_protocol(uint16_t product_id); int xreal_imu_interface_id(uint16_t product_id); int xreal_mcu_interface_id(uint16_t product_id); diff --git a/interface_lib/include/imu_protocol.h b/interface_lib/include/imu_protocol.h new file mode 100644 index 0000000..1d85607 --- /dev/null +++ b/interface_lib/include/imu_protocol.h @@ -0,0 +1,49 @@ +#pragma once +// Abstraction for IMU transport protocols (HID and XREAL ONE) + +#ifndef __cplusplus +#include +#include +#else +#include +#include +#endif + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +struct device_imu_t; +struct device_imu_packet_t; + +typedef struct imu_sample { + float gx, gy, gz; + float ax, ay, az; + float mx, my, mz; + float temperature_c; + uint64_t timestamp_ns; + uint32_t flags; +} imu_sample; + +typedef struct imu_protocol { + bool (*open)(struct device_imu_t* dev, struct hid_device_info* info); + void (*close)(struct device_imu_t* dev); + + bool (*start_stream)(struct device_imu_t* dev); + bool (*stop_stream)(struct device_imu_t* dev); + + bool (*get_static_id)(struct device_imu_t* dev, uint32_t* out_id); + + bool (*load_calibration_json)(struct device_imu_t* dev, uint32_t* len, char** data); + + int (*next_sample)(struct device_imu_t* dev, struct imu_sample* out, int timeout_ms); +} imu_protocol; + +extern const imu_protocol imu_protocol_hid; +extern const imu_protocol imu_protocol_xreal_one; + +#ifdef __cplusplus +} +#endif diff --git a/interface_lib/modules/xreal_one_driver b/interface_lib/modules/xreal_one_driver new file mode 160000 index 0000000..87e2897 --- /dev/null +++ b/interface_lib/modules/xreal_one_driver @@ -0,0 +1 @@ +Subproject commit 87e2897acc0e44be1b273165f237af24d3c67c74 diff --git a/interface_lib/src/device_imu.c b/interface_lib/src/device_imu.c index 5d7e76e..5232dea 100644 --- a/interface_lib/src/device_imu.c +++ b/interface_lib/src/device_imu.c @@ -34,6 +34,7 @@ #include #include #include +#include #include #include @@ -43,6 +44,7 @@ #include "crc32.h" #include "hid_ids.h" #include "endian_compat.h" +#include "imu_protocol.h" #define GRAVITY_G (9.806f) @@ -96,152 +98,15 @@ struct device_imu_calibration_t { device_imu_camera_calibration_type cam; }; -static bool send_payload(device_imu_type* device, uint16_t size, const uint8_t* payload) { - int payload_size = size; - if (payload_size > device->max_payload_size) { - payload_size = device->max_payload_size; - } - - int transferred = hid_write(device->handle, payload, payload_size); - - if (transferred != payload_size) { - device_imu_error("Sending payload failed"); - return false; - } - - return (transferred == size); -} - -static bool recv_payload(device_imu_type* device, uint16_t size, uint8_t* payload) { - int payload_size = size; - if (payload_size > device->max_payload_size) { - payload_size = device->max_payload_size; - } - - int transferred = hid_read(device->handle, payload, payload_size); - - if (transferred >= payload_size) { - transferred = payload_size; - } - - if (transferred == 0) { - return false; - } - - if (transferred != payload_size) { - device_imu_error("Receiving payload failed"); - return false; - } - - return (transferred == size); -} - -struct __attribute__((__packed__)) device_imu_payload_packet_t { - uint8_t head; - uint32_t checksum; - uint16_t length; - uint8_t msgid; - uint8_t data [512 - 8]; -}; - -typedef struct device_imu_payload_packet_t device_imu_payload_packet_type; - -static bool send_payload_msg(device_imu_type* device, uint8_t msgid, uint16_t len, const uint8_t* data) { - static device_imu_payload_packet_type packet; - - const uint16_t packet_len = 3 + len; - const uint16_t payload_len = 5 + packet_len; - - packet.head = 0xAA; - packet.length = htole16(packet_len); - packet.msgid = msgid; - - memcpy(packet.data, data, len); - packet.checksum = htole32( - crc32_checksum( - (const uint8_t*) (&packet.length), - packet.length - ) - ); - - return send_payload(device, payload_len, (uint8_t*) (&packet)); -} - -static bool send_payload_msg_signal(device_imu_type* device, uint8_t msgid, uint8_t signal) { - return send_payload_msg(device, msgid, 1, &signal); -} - -static bool recv_payload_msg(device_imu_type* device, uint8_t msgid, uint16_t len, uint8_t* data) { - static device_imu_payload_packet_type packet; - - packet.head = 0; - packet.length = 0; - packet.msgid = 0; - - const uint16_t packet_len = 3 + len; - const uint16_t payload_len = 5 + packet_len; - - do { - if (!recv_payload(device, payload_len, (uint8_t*) (&packet))) { - return false; - } - } while (packet.msgid != msgid); - - memcpy(data, packet.data, len); - return true; -} static device_imu_error_type load_device_imu_calibration_data(device_imu_type* device, uint32_t* len, char** data) { if (!device) { return DEVICE_IMU_ERROR_NO_DEVICE; } - if ((!len) || (!data)) { - return DEVICE_IMU_ERROR_NO_ALLOCATION; - } - - if (!send_payload_msg(device, DEVICE_IMU_MSG_GET_CAL_DATA_LENGTH, 0, NULL)) { - device_imu_error("Failed sending payload to get calibration data length"); - return DEVICE_IMU_ERROR_PAYLOAD_FAILED; - } - - *len = 0; - if (!recv_payload_msg(device, DEVICE_IMU_MSG_GET_CAL_DATA_LENGTH, 4, (uint8_t*) len)) { - *data = NULL; - return DEVICE_IMU_ERROR_LOADING_FAILED; - } - - const uint16_t max_packet_size = (device->max_payload_size - 8); - *data = malloc(*len + 1); - - if (!(*data)) { - return DEVICE_IMU_ERROR_NO_ALLOCATION; - } - - uint32_t position = 0; - while (position < *len) { - const uint32_t remaining = (*len - position); - - if (!send_payload_msg(device, DEVICE_IMU_MSG_CAL_DATA_GET_NEXT_SEGMENT, 0, NULL)) { - break; - } - - const uint16_t next = (remaining > max_packet_size? max_packet_size : remaining); - - if (!recv_payload_msg(device, DEVICE_IMU_MSG_CAL_DATA_GET_NEXT_SEGMENT, next, (uint8_t*) (*data) + position)) { - break; - } - - position += next; - } - - if (position > *len) { - (*data)[*len] = '\0'; - } else { - (*data)[position] = '\0'; - } - - return DEVICE_IMU_ERROR_NO_ERROR; + return device->protocol->load_calibration_json(device, len, data) + ? DEVICE_IMU_ERROR_NO_ERROR + : DEVICE_IMU_ERROR_LOADING_FAILED; } static FusionVector json_object_get_vector(struct json_object* obj) { @@ -375,14 +240,10 @@ device_imu_error_type device_imu_open(device_imu_type* device, device_imu_event_ struct hid_device_info* it = info; while (it) { - int interface_id = xreal_imu_interface_id(it->product_id); - if (interface_id != -1 && it->interface_number == interface_id) { -#ifndef NDEBUG - printf("Found IMU device with product_id 0x%x on interface %d\n", it->product_id, interface_id); -#endif + const imu_protocol* protocol = xreal_imu_protocol(it->product_id); + if (protocol && protocol->open(device, it)) { device->product_id = it->product_id; - device->handle = hid_open_path(it->path); - device->max_payload_size = xreal_imu_max_payload_size(device->product_id); + device->protocol = protocol; break; } @@ -391,26 +252,20 @@ device_imu_error_type device_imu_open(device_imu_type* device, device_imu_event_ hid_free_enumeration(info); - if (!device->handle) { + if (!device->protocol) { device_imu_error("No handle"); return DEVICE_IMU_ERROR_NO_HANDLE; } - if ((!send_payload_msg_signal(device, DEVICE_IMU_MSG_START_IMU_DATA, 0x0)) || - (!recv_payload_msg(device, DEVICE_IMU_MSG_START_IMU_DATA, 0, NULL))) { + if (!device->protocol->stop_stream(device)) { device_imu_error("Failed sending payload to stop imu data stream"); return DEVICE_IMU_ERROR_PAYLOAD_FAILED; } device_imu_clear(device); - if (!send_payload_msg(device, DEVICE_IMU_MSG_GET_STATIC_ID, 0, NULL)) { - device_imu_error("Failed sending payload to get static id"); - return DEVICE_IMU_ERROR_PAYLOAD_FAILED; - } - uint32_t static_id = 0; - if (recv_payload_msg(device, DEVICE_IMU_MSG_GET_STATIC_ID, 4, (uint8_t*) &static_id)) { + if (device->protocol->get_static_id(device, &static_id)) { device->static_id = static_id; } else { device->static_id = 0x20220101; @@ -486,8 +341,7 @@ device_imu_error_type device_imu_open(device_imu_type* device, device_imu_event_ free(calibration_data); } - if ((!send_payload_msg_signal(device, DEVICE_IMU_MSG_START_IMU_DATA, 0x1)) || - (!recv_payload_msg(device, DEVICE_IMU_MSG_START_IMU_DATA, 0, NULL))) { + if (!device->protocol->start_stream(device)) { device_imu_error("Failed sending payload to start imu data stream"); return DEVICE_IMU_ERROR_PAYLOAD_FAILED; } @@ -651,8 +505,7 @@ device_imu_error_type device_imu_save_calibration(device_imu_type* device, const } device_imu_error_type device_imu_export_calibration(device_imu_type* device, const char *path) { - if ((!send_payload_msg_signal(device, DEVICE_IMU_MSG_START_IMU_DATA, 0x0)) || - (!recv_payload_msg(device, DEVICE_IMU_MSG_START_IMU_DATA, 0, NULL))) { + if (!device->protocol->stop_stream(device)) { device_imu_error("Failed sending payload to stop imu data stream"); return DEVICE_IMU_ERROR_PAYLOAD_FAILED; } @@ -695,9 +548,8 @@ free_data: device_imu_clear(device); - if ((!send_payload_msg_signal(device, DEVICE_IMU_MSG_START_IMU_DATA, 0x1)) || - (!recv_payload_msg(device, DEVICE_IMU_MSG_START_IMU_DATA, 0, NULL))) { - device_imu_error("Failed sending payload to stop imu data stream"); + if (!device->protocol->start_stream(device)) { + device_imu_error("Failed sending payload to start imu data stream"); result = DEVICE_IMU_ERROR_PAYLOAD_FAILED; } @@ -944,7 +796,7 @@ device_imu_error_type device_imu_calibrate(device_imu_type* device, uint32_t ite return DEVICE_IMU_ERROR_NO_DEVICE; } - if (!device->handle) { + if (!device->protocol) { device_imu_error("No handle"); return DEVICE_IMU_ERROR_NO_HANDLE; } @@ -974,38 +826,26 @@ device_imu_error_type device_imu_calibrate(device_imu_type* device, uint32_t ite FusionVector prev_accel; while (iterations > 0) { - memset(&packet, 0, sizeof(device_imu_packet_type)); - - transferred = hid_read( - device->handle, - (uint8_t*) &packet, - sizeof(device_imu_packet_type) - ); - - if (transferred == -1) { - device_imu_error("Device may be unplugged"); - return DEVICE_IMU_ERROR_UNPLUGGED; - } - - if (transferred == 0) { - continue; - } - - if (sizeof(device_imu_packet_type) != transferred) { - device_imu_error("Unexpected packet size"); - return DEVICE_IMU_ERROR_UNEXPECTED; - } - - if ((packet.signature[0] != 0x01) || (packet.signature[1] != 0x02)) { - continue; - } - FusionVector gyroscope; FusionVector accelerometer; FusionVector magnetometer; - - readIMU_from_packet(&packet, &gyroscope, &accelerometer, &magnetometer); - + + imu_sample s = {0}; + int n = device->protocol->next_sample(device, &s, -1); + if (n < 0) { + device_imu_error("Device may be unplugged"); + return DEVICE_IMU_ERROR_UNPLUGGED; + } + if (n == 0) { + continue; // timeout, try again + } + if (s.flags & 1u) { + continue; // init frame; ignore for calibration + } + gyroscope.axis.x = s.gx; gyroscope.axis.y = s.gy; gyroscope.axis.z = s.gz; + accelerometer.axis.x = s.ax; accelerometer.axis.y = s.ay; accelerometer.axis.z = s.az; + magnetometer.axis.x = s.mx; magnetometer.axis.y = s.my; magnetometer.axis.z = s.mz; + pre_biased_coordinate_system(&gyroscope); pre_biased_coordinate_system(&accelerometer); pre_biased_coordinate_system(&magnetometer); @@ -1016,6 +856,7 @@ device_imu_error_type device_imu_calibrate(device_imu_type* device, uint32_t ite } else { cal_gyroscope = gyroscope; cal_accelerometer = FUSION_VECTOR_ZERO; + initialized = true; } prev_accel = accelerometer; @@ -1065,67 +906,42 @@ device_imu_error_type device_imu_read(device_imu_type* device, int timeout) { return DEVICE_IMU_ERROR_NO_DEVICE; } - if (!device->handle) { + if (!device->protocol) { device_imu_error("No handle"); return DEVICE_IMU_ERROR_NO_HANDLE; } - - if (sizeof(device_imu_packet_type) > device->max_payload_size) { - device_imu_error("Not proper size"); - return DEVICE_IMU_ERROR_WRONG_SIZE; - } - - device_imu_packet_type packet; - memset(&packet, 0, sizeof(device_imu_packet_type)); - - int transferred = hid_read_timeout( - device->handle, - (uint8_t*) &packet, - sizeof(device_imu_packet_type), - timeout - ); - - if (transferred == -1) { + + imu_sample s = {0}; + int n = device->protocol->next_sample(device, &s, timeout); + if (n < 0) { device_imu_error("Device may be unplugged"); return DEVICE_IMU_ERROR_UNPLUGGED; } - - if (transferred == 0) { + if (n == 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); + + // Handle init frames + if (s.flags & 1u) { + device_imu_callback(device, s.timestamp_ns, 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; - + + const uint64_t delta_ns = s.timestamp_ns - device->last_timestamp; + const float deltaTime = (float)((double)delta_ns / 1e9); + device->last_timestamp = s.timestamp_ns; + FusionVector gyroscope; FusionVector accelerometer; FusionVector magnetometer; - - readIMU_from_packet(&packet, &gyroscope, &accelerometer, &magnetometer); + gyroscope.axis.x = s.gx; gyroscope.axis.y = s.gy; gyroscope.axis.z = s.gz; + accelerometer.axis.x = s.ax; accelerometer.axis.y = s.ay; accelerometer.axis.z = s.az; + magnetometer.axis.x = s.mx; magnetometer.axis.y = s.my; magnetometer.axis.z = s.mz; + + if (!isnan(s.temperature_c)) { + device->temperature = s.temperature_c; + } + apply_calibration(device, &gyroscope, &accelerometer, &magnetometer); if (device->offset) { @@ -1139,7 +955,7 @@ device_imu_error_type device_imu_read(device_imu_type* device, int timeout) { #endif if (device->ahrs) { - if (isnan(magnetometer.axis.x) || isnan(magnetometer.axis.x) || isnan(magnetometer.axis.x)) { + if (isnan(magnetometer.axis.x) || isnan(magnetometer.axis.y) || isnan(magnetometer.axis.z)) { FusionAhrsUpdateNoMagnetometer((FusionAhrs*) device->ahrs, gyroscope, accelerometer, deltaTime); } else { /* The magnetometer seems to make results of sensor fusion generally worse. So it is not used currently. */ @@ -1148,16 +964,13 @@ device_imu_error_type device_imu_read(device_imu_type* device, int timeout) { } const device_imu_quat_type orientation = device_imu_get_orientation(device->ahrs); - - // TODO: fix detection of this case; quat.x as a nan value is only a side-effect of some issue with ahrs or - // the gyro/accel/magnet readings if (isnan(orientation.x) || isnan(orientation.y) || isnan(orientation.z) || isnan(orientation.w)) { device_imu_error("Invalid orientation reading"); return DEVICE_IMU_ERROR_INVALID_VALUE; } } - device_imu_callback(device, timestamp, DEVICE_IMU_EVENT_UPDATE); + device_imu_callback(device, s.timestamp_ns, DEVICE_IMU_EVENT_UPDATE); return DEVICE_IMU_ERROR_NO_ERROR; } @@ -1339,17 +1152,15 @@ device_imu_error_type device_imu_close(device_imu_type* device) { free(device->offset); } - if (device->handle) { - if ((!send_payload_msg_signal(device, DEVICE_IMU_MSG_START_IMU_DATA, 0x0)) || - (!recv_payload_msg(device, DEVICE_IMU_MSG_START_IMU_DATA, 0, NULL))) { + if (device->protocol) { + if (!device->protocol->stop_stream(device)) { device_imu_error("Failed sending payload to stop imu data stream"); } - - hid_close(device->handle); + + device->protocol->close(device); } - + memset(device, 0, sizeof(device_imu_type)); - device_exit(); return DEVICE_IMU_ERROR_NO_ERROR; } diff --git a/interface_lib/src/hid_ids.c b/interface_lib/src/hid_ids.c index b913d1b..27d6d5d 100644 --- a/interface_lib/src/hid_ids.c +++ b/interface_lib/src/hid_ids.c @@ -23,9 +23,11 @@ // #include "hid_ids.h" +#include "imu_protocol.h" #ifndef __cplusplus #include +#include #endif #ifndef __cplusplus @@ -39,28 +41,55 @@ 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 + 0x0426, // XREAL Air 2 Ultra + 0x0435, // XREAL One Pro + 0x0436, // XREAL One Pro + 0x0437, // XREAL One + 0x0438 // XREAL One }; -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 imu_protocol* xreal_imu_protocols[NUM_SUPPORTED_PRODUCTS] = { + &imu_protocol_hid, // XREAL Air + &imu_protocol_hid, // XREAL Air 2 + &imu_protocol_hid, // XREAL Air 2 Pro + &imu_protocol_hid, // XREAL Air 2 Ultra + &imu_protocol_xreal_one, // XREAL One Pro + &imu_protocol_xreal_one, // XREAL One Pro + &imu_protocol_xreal_one, // XREAL One + &imu_protocol_xreal_one // XREAL One }; -const int xreal_mcu_interface_ids[NUM_SUPPORTED_PRODUCTS] = { +const int xreal_imu_hid_interface_ids[NUM_SUPPORTED_PRODUCTS] = { + 3, // XREAL Air + 3, // XREAL Air 2 + 3, // XREAL Air 2 Pro + 2, // XREAL Air 2 Ultra + -1, // XREAL One Pro + -1, // XREAL One Pro + -1, // XREAL One + -1 // XREAL One +}; + +const int xreal_mcu_hid_interface_ids[NUM_SUPPORTED_PRODUCTS] = { 4, // XREAL Air 4, // XREAL Air 2 4, // XREAL Air 2 Pro - 0 // XREAL Air 2 Ultra MCU + 0, // XREAL Air 2 Ultra MCU + -1, // XREAL One Pro + -1, // XREAL One Pro + -1, // XREAL One + -1 // XREAL One }; -const uint16_t xreal_imu_max_payload_sizes[NUM_SUPPORTED_PRODUCTS] = { +const uint16_t xreal_imu_hid_max_payload_sizes[NUM_SUPPORTED_PRODUCTS] = { 64, // XREAL Air 64, // XREAL Air 2 64, // XREAL Air 2 Pro - 512 // XREAL Air 2 Ultra + 512,// XREAL Air 2 Ultra + -1, // XREAL One Pro + -1, // XREAL One Pro + -1, // XREAL One + -1 // XREAL One }; static int xreal_product_index(uint16_t product_id) { @@ -77,11 +106,21 @@ bool is_xreal_product_id(uint16_t product_id) { return xreal_product_index(product_id) >= 0; } +const imu_protocol* xreal_imu_protocol(uint16_t product_id) { + const int index = xreal_product_index(product_id); + + if (index >= 0) { + return xreal_imu_protocols[index]; + } else { + return NULL; + } +} + 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]; + return xreal_imu_hid_interface_ids[index]; } else { return -1; } @@ -91,7 +130,7 @@ 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]; + return xreal_mcu_hid_interface_ids[index]; } else { return -1; } @@ -101,7 +140,7 @@ 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]; + return xreal_imu_hid_max_payload_sizes[index]; } else { return 0; } diff --git a/interface_lib/src/imu_protocol_hid.c b/interface_lib/src/imu_protocol_hid.c new file mode 100644 index 0000000..ed5d1ca --- /dev/null +++ b/interface_lib/src/imu_protocol_hid.c @@ -0,0 +1,278 @@ +#include "imu_protocol.h" +#include "hid_ids.h" +#include "device_imu.h" +#include "device.h" + +#include +#include +#include +#include +#include "crc32.h" +#include "endian_compat.h" +#include + +#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 + +#ifndef NDEBUG +#define device_imu_error(msg) fprintf(stderr, "ERROR: %s\n", msg) +#else +#define device_imu_error(msg) (0) +#endif + +static bool hid_open_impl(device_imu_type* device, struct hid_device_info* info) { + int iface = xreal_imu_interface_id(info->product_id); + if (iface != -1 && info->interface_number == iface) { + #ifndef NDEBUG + printf("[hid] Found IMU device pid=0x%x iface=%d\n", info->product_id, iface); + #endif + device->handle = hid_open_path(info->path); + + if (device->handle) { + device->max_payload_size = xreal_imu_max_payload_size(info->product_id); + } + } + + return device->handle != NULL; +} + +static void hid_close_impl(device_imu_type* device) { + if (device->handle) { + hid_close((hid_device*)device->handle); + device->handle = NULL; + } + device_exit(); +} + +static bool send_payload(device_imu_type* device, uint16_t size, const uint8_t* payload) { + int payload_size = size; + if (payload_size > device->max_payload_size) { + payload_size = device->max_payload_size; + } + + int transferred = hid_write(device->handle, payload, payload_size); + if (transferred != payload_size) { + device_imu_error("Sending payload failed"); + return false; + } + + return (transferred == size); +} + +static bool recv_payload(device_imu_type* device, uint16_t size, uint8_t* payload) { + int payload_size = size; + if (payload_size > device->max_payload_size) { + payload_size = device->max_payload_size; + } + + int transferred = hid_read(device->handle, payload, payload_size); + + if (transferred >= payload_size) { + transferred = payload_size; + } + + if (transferred == 0) { + return false; + } + + if (transferred != payload_size) { + device_imu_error("Receiving payload failed"); + return false; + } + + return (transferred == size); +} + +struct __attribute__((__packed__)) payload_packet_t { + uint8_t head; + uint32_t checksum; + uint16_t length; + uint8_t msgid; + uint8_t data [512 - 8]; +}; + +typedef struct payload_packet_t payload_packet_type; + +static bool send_payload_msg(device_imu_type* device, uint8_t msgid, uint16_t len, const uint8_t* data) { + static payload_packet_type packet; + + const uint16_t packet_len = 3 + len; + const uint16_t payload_len = 5 + packet_len; + + packet.head = 0xAA; + packet.length = htole16(packet_len); + packet.msgid = msgid; + + memcpy(packet.data, data, len); + packet.checksum = htole32( + crc32_checksum( + (const uint8_t*) (&packet.length), + packet.length + ) + ); + + return send_payload(device, payload_len, (uint8_t*) (&packet)); +} + +static bool send_payload_msg_signal(device_imu_type* device, uint8_t msgid, uint8_t signal) { + return send_payload_msg(device, msgid, 1, &signal); +} + +static bool recv_payload_msg(device_imu_type* device, uint8_t msgid, uint16_t len, uint8_t* data) { + static payload_packet_type packet; + + packet.head = 0; + packet.length = 0; + packet.msgid = 0; + + const uint16_t packet_len = 3 + len; + const uint16_t payload_len = 5 + packet_len; + + do { + if (!recv_payload(device, payload_len, (uint8_t*) (&packet))) { + return false; + } + } while (packet.msgid != msgid); + + memcpy(data, packet.data, len); + return true; +} + +static bool hid_start_stream(device_imu_type* dev) { + return send_payload_msg_signal(dev, DEVICE_IMU_MSG_START_IMU_DATA, 0x1) && + recv_payload_msg(dev, DEVICE_IMU_MSG_START_IMU_DATA, 0, NULL); +} + +static bool hid_stop_stream(device_imu_type* dev) { + return send_payload_msg_signal(dev, DEVICE_IMU_MSG_START_IMU_DATA, 0x0) && + recv_payload_msg(dev, DEVICE_IMU_MSG_START_IMU_DATA, 0, NULL); +} + +static bool hid_get_static_id(device_imu_type* dev, uint32_t* out_id) { + return send_payload_msg(dev, DEVICE_IMU_MSG_GET_STATIC_ID, 0, NULL) && + recv_payload_msg(dev, DEVICE_IMU_MSG_GET_STATIC_ID, 4, (uint8_t*)out_id); +} + +static bool hid_load_calibration_json(device_imu_type* dev, uint32_t* len, char** data) { + if (!send_payload_msg(dev, DEVICE_IMU_MSG_GET_CAL_DATA_LENGTH, 0, NULL)) return false; + *len = 0; + if (!recv_payload_msg(dev, DEVICE_IMU_MSG_GET_CAL_DATA_LENGTH, 4, (uint8_t*)len)) return false; + const uint16_t max_packet_size = (dev->max_payload_size - 8); + *data = (char*)malloc(*len + 1); + if (!*data) return false; + uint32_t pos = 0; + while (pos < *len) { + if (!send_payload_msg(dev, DEVICE_IMU_MSG_CAL_DATA_GET_NEXT_SEGMENT, 0, NULL)) break; + const uint16_t next = (uint16_t)((*len - pos) > max_packet_size ? max_packet_size : (*len - pos)); + if (!recv_payload_msg(dev, DEVICE_IMU_MSG_CAL_DATA_GET_NEXT_SEGMENT, next, (uint8_t*)(*data + pos))) break; + pos += next; + } + (*data)[pos] = '\0'; + return true; +} + +static int32_t pack32bit_signed(const uint8_t* data) { + uint32_t unsigned_value = (data[0]) | (data[1] << 8) | (data[2] << 16) | (data[3] << 24); + return ((int32_t) unsigned_value); +} + +static int32_t pack24bit_signed(const uint8_t* data) { + uint32_t unsigned_value = (data[0]) | (data[1] << 8) | (data[2] << 16); + if ((data[2] & 0x80) != 0) unsigned_value |= (0xFF << 24); + return ((int32_t) unsigned_value); +} + +static int16_t pack16bit_signed(const uint8_t* data) { + uint16_t unsigned_value = (data[1] << 8) | (data[0]); + return (int16_t) unsigned_value; +} + +static int32_t pack32bit_signed_swap(const uint8_t* data) { + uint32_t unsigned_value = (data[0] << 24) | (data[1] << 16) | (data[2] << 8) | (data[3]); + return ((int32_t) unsigned_value); +} + +static int16_t pack16bit_signed_swap(const uint8_t* data) { + uint16_t unsigned_value = (data[0] << 8) | (data[1]); + return (int16_t) unsigned_value; +} + +static int16_t pack16bit_signed_bizarre(const uint8_t* data) { + uint16_t unsigned_value = (data[0]) | ((data[1] ^ 0x80) << 8); + return (int16_t) unsigned_value; +} + +static int hid_next_sample(device_imu_type* device, struct imu_sample* out, int timeout_ms) { + struct device_imu_packet_t p = {0}; + int n = hid_read_timeout((hid_device*)device->handle, (unsigned char*)&p, sizeof(p), timeout_ms); + if (n <= 0) return n; // 0 timeout, -1 error + if (n != (int)sizeof(p)) return -1; + + // Special init packet + if (p.signature[0] == 0xaa && p.signature[1] == 0x53) { + memset(out, 0, sizeof(*out)); + out->flags = 1; + out->timestamp_ns = le64toh(p.timestamp); + out->temperature_c = NAN; + out->mx = out->my = out->mz = NAN; + return 1; + } + + if ((p.signature[0] != 0x01) || (p.signature[1] != 0x02)) { + return 0; // skip unknown + } + + int32_t vel_m = pack16bit_signed(p.angular_multiplier); + int32_t vel_d = pack32bit_signed(p.angular_divisor); + int32_t vel_x = pack24bit_signed(p.angular_velocity_x); + int32_t vel_y = pack24bit_signed(p.angular_velocity_y); + int32_t vel_z = pack24bit_signed(p.angular_velocity_z); + + int32_t accel_m = pack16bit_signed(p.acceleration_multiplier); + int32_t accel_d = pack32bit_signed(p.acceleration_divisor); + int32_t accel_x = pack24bit_signed(p.acceleration_x); + int32_t accel_y = pack24bit_signed(p.acceleration_y); + int32_t accel_z = pack24bit_signed(p.acceleration_z); + + int32_t magnet_m = pack16bit_signed_swap(p.magnetic_multiplier); + int32_t magnet_d = pack32bit_signed_swap(p.magnetic_divisor); + int16_t magnet_x = pack16bit_signed_bizarre(p.magnetic_x); + int16_t magnet_y = pack16bit_signed_bizarre(p.magnetic_y); + int16_t magnet_z = pack16bit_signed_bizarre(p.magnetic_z); + + int16_t temperature = pack16bit_signed(p.temperature); + + memset(out, 0, sizeof(*out)); + out->gx = (float) vel_x * (float) vel_m / (float) vel_d; + out->gy = (float) vel_y * (float) vel_m / (float) vel_d; + out->gz = (float) vel_z * (float) vel_m / (float) vel_d; + + out->ax = (float) accel_x * (float) accel_m / (float) accel_d; + out->ay = (float) accel_y * (float) accel_m / (float) accel_d; + out->az = (float) accel_z * (float) accel_m / (float) accel_d; + + out->mx = (float) magnet_x * (float) magnet_m / (float) magnet_d; + out->my = (float) magnet_y * (float) magnet_m / (float) magnet_d; + out->mz = (float) magnet_z * (float) magnet_m / (float) magnet_d; + + out->temperature_c = ((float) temperature) / 132.48f + 25.0f; + out->timestamp_ns = le64toh(p.timestamp); + out->flags = 0; + return 1; +} + +const imu_protocol imu_protocol_hid = { + .open = hid_open_impl, + .close = hid_close_impl, + .start_stream = hid_start_stream, + .stop_stream = hid_stop_stream, + .get_static_id = hid_get_static_id, + .load_calibration_json = hid_load_calibration_json, + .next_sample = hid_next_sample, +}; diff --git a/interface_lib/src/imu_protocol_xo.c b/interface_lib/src/imu_protocol_xo.c new file mode 100644 index 0000000..8e3ed8b --- /dev/null +++ b/interface_lib/src/imu_protocol_xo.c @@ -0,0 +1,72 @@ +#include "imu_protocol.h" +#include "device_imu.h" +#include "imu_protocol.h" + +#include +#include +#include +#include +#include + +#include + +typedef struct { + XrealOneHandle* h; +} xo_ctx; + +static bool xo_open_impl(device_imu_type* device, struct hid_device_info* info) { + #ifndef NDEBUG + printf("[xreal_one] Found IMU device pid=0x%x\n", info->product_id); + #endif + + (void)device; + xo_ctx* ctx = (xo_ctx*)malloc(sizeof(xo_ctx)); + if (!ctx) return false; + ctx->h = xo_new(); + if (!ctx->h) { free(ctx); return false; } + device->handle = ctx; // store context in handle for symmetry + return true; +} + +static void xo_close_impl(device_imu_type* device) { + if (!device->handle) return; + xo_ctx* ctx = (xo_ctx*)device->handle; + if (ctx->h) xo_free(ctx->h); + free(ctx); + device->handle = NULL; +} + +static bool xo_start_stream(device_imu_type* dev) { (void)dev; return true; } +static bool xo_stop_stream(device_imu_type* dev) { (void)dev; return true; } +static bool xo_get_static_id(device_imu_type* dev, uint32_t* out_id) { if (out_id) *out_id = 0; return true; } +static bool xo_load_calibration_json(device_imu_type* dev, uint32_t* len, char** data) { (void)dev; if(len) *len=0; if(data) *data=NULL; return false; } + +static int xo_next_sample(device_imu_type* device, struct imu_sample* out, int timeout_ms) { + (void)timeout_ms; + xo_ctx* ctx = (xo_ctx*)device->handle; + XOImu imu = {0}; + int rc = xo_next(ctx->h, &imu); + if (rc != 0) return rc; // negative on error or non-zero + memset(out, 0, sizeof(*out)); + out->gx = imu.gyro[0]; + out->gy = imu.gyro[1]; + out->gz = imu.gyro[2]; + out->ax = imu.accel[0]; + out->ay = imu.accel[1]; + out->az = imu.accel[2]; + out->mx = out->my = out->mz = NAN; // XO protocol doesn't provide mag + out->temperature_c = NAN; + out->timestamp_ns = imu.timestamp * 1000000; // incoming value in ms, convert to ns + out->flags = 0; + return 1; +} + +const imu_protocol imu_protocol_xreal_one = { + .open = xo_open_impl, + .close = xo_close_impl, + .start_stream = xo_start_stream, + .stop_stream = xo_stop_stream, + .get_static_id = xo_get_static_id, + .load_calibration_json = xo_load_calibration_json, + .next_sample = xo_next_sample, +}; From 33a9a2f9d94eb4876c5b0ee8c7829f8345a8694b Mon Sep 17 00:00:00 2001 From: wheaney <42350981+wheaney@users.noreply.github.com> Date: Fri, 3 Oct 2025 11:27:18 -0700 Subject: [PATCH 2/5] More robust error checking in XO integration, fix timestamp usage --- interface_lib/src/imu_protocol_xo.c | 73 +++++++++++++++++++++++++---- 1 file changed, 63 insertions(+), 10 deletions(-) diff --git a/interface_lib/src/imu_protocol_xo.c b/interface_lib/src/imu_protocol_xo.c index 8e3ed8b..6ab399c 100644 --- a/interface_lib/src/imu_protocol_xo.c +++ b/interface_lib/src/imu_protocol_xo.c @@ -1,12 +1,18 @@ +// Ensure POSIX clock_gettime and CLOCK_MONOTONIC are exposed from headers +#ifndef _POSIX_C_SOURCE +#define _POSIX_C_SOURCE 199309L +#endif + #include "imu_protocol.h" #include "device_imu.h" -#include "imu_protocol.h" #include #include #include #include #include +#include +#include #include @@ -15,11 +21,13 @@ typedef struct { } xo_ctx; static bool xo_open_impl(device_imu_type* device, struct hid_device_info* info) { + if (!device) return false; + device->handle = NULL; + #ifndef NDEBUG printf("[xreal_one] Found IMU device pid=0x%x\n", info->product_id); #endif - (void)device; xo_ctx* ctx = (xo_ctx*)malloc(sizeof(xo_ctx)); if (!ctx) return false; ctx->h = xo_new(); @@ -29,24 +37,48 @@ static bool xo_open_impl(device_imu_type* device, struct hid_device_info* info) } static void xo_close_impl(device_imu_type* device) { - if (!device->handle) return; + if (!device || !device->handle) return; xo_ctx* ctx = (xo_ctx*)device->handle; if (ctx->h) xo_free(ctx->h); free(ctx); device->handle = NULL; } -static bool xo_start_stream(device_imu_type* dev) { (void)dev; return true; } -static bool xo_stop_stream(device_imu_type* dev) { (void)dev; return true; } -static bool xo_get_static_id(device_imu_type* dev, uint32_t* out_id) { if (out_id) *out_id = 0; return true; } -static bool xo_load_calibration_json(device_imu_type* dev, uint32_t* len, char** data) { (void)dev; if(len) *len=0; if(data) *data=NULL; return false; } +static bool xo_start_stream(device_imu_type* dev) { + if (!dev || !dev->handle) return false; + return true; +} +static bool xo_stop_stream(device_imu_type* dev) { + if (!dev || !dev->handle) return false; + return true; +} + +static bool xo_get_static_id(device_imu_type* dev, uint32_t* out_id) { + if (!dev || !dev->handle) return false; + if (out_id) *out_id = 0; + return true; +} + +static bool xo_load_calibration_json(device_imu_type* dev, uint32_t* len, char** data) { + if (!dev || !dev->handle) return false; + if (len) *len = 0; + if (data) *data = NULL; + return false; // not supported +} + +static uint64_t start_timestamp_ns = 0; +static bool time_debug = false; static int xo_next_sample(device_imu_type* device, struct imu_sample* out, int timeout_ms) { (void)timeout_ms; + if (!device || !out) return -1; xo_ctx* ctx = (xo_ctx*)device->handle; - XOImu imu = {0}; + if (!ctx || !ctx->h) return -1; + + XOImu imu = (XOImu){0}; int rc = xo_next(ctx->h, &imu); - if (rc != 0) return rc; // negative on error or non-zero + if (rc != 0) return rc; // propagate non-zero (e.g., error or no-sample) + memset(out, 0, sizeof(*out)); out->gx = imu.gyro[0]; out->gy = imu.gyro[1]; @@ -56,7 +88,28 @@ static int xo_next_sample(device_imu_type* device, struct imu_sample* out, int t out->az = imu.accel[2]; out->mx = out->my = out->mz = NAN; // XO protocol doesn't provide mag out->temperature_c = NAN; - out->timestamp_ns = imu.timestamp * 1000000; // incoming value in ms, convert to ns + + // avoid using IMU timestamp, if possible, as it's apparently inconsistent + struct timespec ts; + if (clock_gettime(CLOCK_MONOTONIC, &ts) == 0) { + const uint64_t ts_ns = (uint64_t)ts.tv_sec * 1000000000ull + (uint64_t)ts.tv_nsec; + if (start_timestamp_ns != 0) { + out->timestamp_ns = ts_ns - start_timestamp_ns; + } else { + start_timestamp_ns = ts_ns; + out->timestamp_ns = 0; + } + if (!time_debug) { + printf("[xreal_one] Using system time for IMU timestamps\n"); + } + } else { + out->timestamp_ns = imu.timestamp * 1000; + + if (!time_debug) { + printf("[xreal_one] Using IMU time for IMU timestamps\n"); + } + } + time_debug = true; out->flags = 0; return 1; } From 203c9e77d11cc478f5283aa581723c7be9b34691 Mon Sep 17 00:00:00 2001 From: wheaney <42350981+wheaney@users.noreply.github.com> Date: Sat, 4 Oct 2025 19:49:56 -0700 Subject: [PATCH 3/5] Fix XREAL One integration with missing rad-to-deg conversion for gyro readings --- interface_lib/src/imu_protocol_xo.c | 50 ++++++++++++++--------------- 1 file changed, 24 insertions(+), 26 deletions(-) diff --git a/interface_lib/src/imu_protocol_xo.c b/interface_lib/src/imu_protocol_xo.c index 6ab399c..a77258d 100644 --- a/interface_lib/src/imu_protocol_xo.c +++ b/interface_lib/src/imu_protocol_xo.c @@ -1,21 +1,19 @@ -// Ensure POSIX clock_gettime and CLOCK_MONOTONIC are exposed from headers -#ifndef _POSIX_C_SOURCE -#define _POSIX_C_SOURCE 199309L -#endif - #include "imu_protocol.h" #include "device_imu.h" #include +#include #include #include #include -#include #include #include #include +#define M_PI 3.14159265358979323846 +#define RADIANS_TO_DEGREES (180.0 / M_PI) + typedef struct { XrealOneHandle* h; } xo_ctx; @@ -80,9 +78,9 @@ static int xo_next_sample(device_imu_type* device, struct imu_sample* out, int t if (rc != 0) return rc; // propagate non-zero (e.g., error or no-sample) memset(out, 0, sizeof(*out)); - out->gx = imu.gyro[0]; - out->gy = imu.gyro[1]; - out->gz = imu.gyro[2]; + out->gx = imu.gyro[0] * RADIANS_TO_DEGREES; + out->gy = imu.gyro[1] * RADIANS_TO_DEGREES; + out->gz = imu.gyro[2] * RADIANS_TO_DEGREES; out->ax = imu.accel[0]; out->ay = imu.accel[1]; out->az = imu.accel[2]; @@ -90,25 +88,25 @@ static int xo_next_sample(device_imu_type* device, struct imu_sample* out, int t out->temperature_c = NAN; // avoid using IMU timestamp, if possible, as it's apparently inconsistent - struct timespec ts; - if (clock_gettime(CLOCK_MONOTONIC, &ts) == 0) { - const uint64_t ts_ns = (uint64_t)ts.tv_sec * 1000000000ull + (uint64_t)ts.tv_nsec; - if (start_timestamp_ns != 0) { - out->timestamp_ns = ts_ns - start_timestamp_ns; - } else { - start_timestamp_ns = ts_ns; - out->timestamp_ns = 0; - } - if (!time_debug) { - printf("[xreal_one] Using system time for IMU timestamps\n"); - } - } else { + // struct timespec ts; + // if (clock_gettime(CLOCK_MONOTONIC, &ts) == 0) { + // const uint64_t ts_ns = (uint64_t)ts.tv_sec * 1000000000ull + (uint64_t)ts.tv_nsec; + // if (start_timestamp_ns != 0) { + // out->timestamp_ns = ts_ns - start_timestamp_ns; + // } else { + // start_timestamp_ns = ts_ns; + // out->timestamp_ns = 0; + // } + // if (!time_debug) { + // printf("[xreal_one] Using system time for IMU timestamps\n"); + // } + // } else { out->timestamp_ns = imu.timestamp * 1000; - if (!time_debug) { - printf("[xreal_one] Using IMU time for IMU timestamps\n"); - } - } + // if (!time_debug) { + // printf("[xreal_one] Using IMU time for IMU timestamps\n"); + // } + // } time_debug = true; out->flags = 0; return 1; From e4ef95366279e0d922da1f589e83abc25f60efa0 Mon Sep 17 00:00:00 2001 From: wheaney <42350981+wheaney@users.noreply.github.com> Date: Sat, 4 Oct 2025 20:05:44 -0700 Subject: [PATCH 4/5] Update xreal_one_driver to the latest commit --- .gitmodules | 2 +- interface_lib/modules/xreal_one_driver | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.gitmodules b/.gitmodules index 1d3a4ac..1371da0 100644 --- a/.gitmodules +++ b/.gitmodules @@ -6,4 +6,4 @@ url = https://github.com/libusb/hidapi.git [submodule "interface_lib/modules/xreal_one_driver"] path = interface_lib/modules/xreal_one_driver - url = https://github.com/wheaney/xreal_one_driver.git + url = https://github.com/rohitsangwan01/xreal_one_driver.git diff --git a/interface_lib/modules/xreal_one_driver b/interface_lib/modules/xreal_one_driver index 87e2897..3e91231 160000 --- a/interface_lib/modules/xreal_one_driver +++ b/interface_lib/modules/xreal_one_driver @@ -1 +1 @@ -Subproject commit 87e2897acc0e44be1b273165f237af24d3c67c74 +Subproject commit 3e912315ec16e94e9a47688770d649cfef293624 From 62d51d9efbb574c128e3a1415235d74989d16554 Mon Sep 17 00:00:00 2001 From: wheaney <42350981+wheaney@users.noreply.github.com> Date: Sat, 4 Oct 2025 21:02:28 -0700 Subject: [PATCH 5/5] Remove hidapi headers from our headers --- interface_lib/include/imu_protocol.h | 8 ++++++-- interface_lib/src/device_imu.c | 8 +++++++- interface_lib/src/imu_protocol_hid.c | 2 +- interface_lib/src/imu_protocol_xo.c | 2 +- 4 files changed, 15 insertions(+), 5 deletions(-) diff --git a/interface_lib/include/imu_protocol.h b/interface_lib/include/imu_protocol.h index 1d85607..8db0cc1 100644 --- a/interface_lib/include/imu_protocol.h +++ b/interface_lib/include/imu_protocol.h @@ -9,7 +9,11 @@ #include #endif -#include +typedef struct imu_hid_info { + uint16_t product_id; + int interface_number; + const char* path; +} imu_hid_info; #ifdef __cplusplus extern "C" { @@ -28,7 +32,7 @@ typedef struct imu_sample { } imu_sample; typedef struct imu_protocol { - bool (*open)(struct device_imu_t* dev, struct hid_device_info* info); + bool (*open)(struct device_imu_t* dev, const struct imu_hid_info* info); void (*close)(struct device_imu_t* dev); bool (*start_stream)(struct device_imu_t* dev); diff --git a/interface_lib/src/device_imu.c b/interface_lib/src/device_imu.c index 5232dea..1f05ec7 100644 --- a/interface_lib/src/device_imu.c +++ b/interface_lib/src/device_imu.c @@ -241,7 +241,13 @@ device_imu_error_type device_imu_open(device_imu_type* device, device_imu_event_ struct hid_device_info* it = info; while (it) { const imu_protocol* protocol = xreal_imu_protocol(it->product_id); - if (protocol && protocol->open(device, it)) { + // Convert to minimal info for protocol open + imu_hid_info mini = { + .product_id = it->product_id, + .interface_number = it->interface_number, + .path = it->path, + }; + if (protocol && protocol->open(device, &mini)) { device->product_id = it->product_id; device->protocol = protocol; break; diff --git a/interface_lib/src/imu_protocol_hid.c b/interface_lib/src/imu_protocol_hid.c index ed5d1ca..1f27952 100644 --- a/interface_lib/src/imu_protocol_hid.c +++ b/interface_lib/src/imu_protocol_hid.c @@ -26,7 +26,7 @@ #define device_imu_error(msg) (0) #endif -static bool hid_open_impl(device_imu_type* device, struct hid_device_info* info) { +static bool hid_open_impl(device_imu_type* device, const struct imu_hid_info* info) { int iface = xreal_imu_interface_id(info->product_id); if (iface != -1 && info->interface_number == iface) { #ifndef NDEBUG diff --git a/interface_lib/src/imu_protocol_xo.c b/interface_lib/src/imu_protocol_xo.c index a77258d..485c034 100644 --- a/interface_lib/src/imu_protocol_xo.c +++ b/interface_lib/src/imu_protocol_xo.c @@ -18,7 +18,7 @@ typedef struct { XrealOneHandle* h; } xo_ctx; -static bool xo_open_impl(device_imu_type* device, struct hid_device_info* info) { +static bool xo_open_impl(device_imu_type* device, const struct imu_hid_info* info) { if (!device) return false; device->handle = NULL;