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, +};