happyz
/
nrealAirLinuxDriver
mirror of https://gitlab.com/TheJackiMonster/nrealAirLinuxDriver.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Add XREAL One support 

Refactor IMU to allow for differing protocols
This commit is contained in:
wheaney 2025-09-25 21:42:30 -07:00
parent 9a1f55c983
commit 33f87340a8
11 changed files with 553 additions and 276 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
.gitmodules vendored
View File

@ -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

7
examples/debug_cam/CMakeLists.txt
View File

@ -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}

21
interface_lib/CMakeLists.txt
View File

@ -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)

11
interface_lib/include/device_imu.h
View File

@ -33,14 +33,7 @@
#include <cstdint>
#endif
#define DEVICE_IMU_MSG_GET_CAL_DATA_LENGTH 0x14
#define DEVICE_IMU_MSG_CAL_DATA_GET_NEXT_SEGMENT 0x15
#define DEVICE_IMU_MSG_ALLOCATE_CAL_DATA_BUFFER 0x16
#define DEVICE_IMU_MSG_WRITE_CAL_DATA_SEGMENT 0x17
#define DEVICE_IMU_MSG_FREE_CAL_BUFFER 0x18
#define DEVICE_IMU_MSG_START_IMU_DATA 0x19
#define DEVICE_IMU_MSG_GET_STATIC_ID 0x1A
#define DEVICE_IMU_MSG_UNKNOWN 0x1D
#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;

5
interface_lib/include/hid_ids.h
View File

@ -33,7 +33,9 @@
#include <cstdint>
#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);

49
interface_lib/include/imu_protocol.h Normal file
View File

@ -0,0 +1,49 @@
#pragma once
// Abstraction for IMU transport protocols (HID and XREAL ONE)
#ifndef __cplusplus
#include <stdbool.h>
#include <stdint.h>
#else
#include <cstdbool>
#include <cstdint>
#endif
#include <hidapi/hidapi.h>
#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

1
interface_lib/modules/xreal_one_driver Submodule

@ -0,0 +1 @@
Subproject commit 87e2897acc0e44be1b273165f237af24d3c67c74

317
interface_lib/src/device_imu.c
View File

@ -34,6 +34,7 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <Fusion/Fusion.h>
#include <json-c/json.h>
@ -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;
}

65
interface_lib/src/hid_ids.c
View File

@ -23,9 +23,11 @@
//
#include "hid_ids.h"
#include "imu_protocol.h"
#ifndef __cplusplus
#include <stdbool.h>
#include <stddef.h>
#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;
}

278
interface_lib/src/imu_protocol_hid.c Normal file
View File

@ -0,0 +1,278 @@
#include "imu_protocol.h"
#include "hid_ids.h"
#include "device_imu.h"
#include "device.h"
#include <hidapi/hidapi.h>
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include "crc32.h"
#include "endian_compat.h"
#include <math.h>
#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,
};

72
interface_lib/src/imu_protocol_xo.c Normal file
View File

@ -0,0 +1,72 @@
#include "imu_protocol.h"
#include "device_imu.h"
#include "imu_protocol.h"
#include <hidapi/hidapi.h>
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <xreal_one_driver.h>
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,
};