124 lines
3.6 KiB
C
124 lines
3.6 KiB
C
#include "imu_protocol.h"
|
|
#include "device_imu.h"
|
|
|
|
#include <hidapi/hidapi.h>
|
|
#include <math.h>
|
|
#include <string.h>
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <stdint.h>
|
|
#include <time.h>
|
|
|
|
#include <xreal_one_driver.h>
|
|
|
|
#define M_PI 3.14159265358979323846
|
|
#define RADIANS_TO_DEGREES (180.0 / M_PI)
|
|
|
|
typedef struct {
|
|
XrealOneHandle* h;
|
|
} xo_ctx;
|
|
|
|
static bool xo_open_impl(device_imu_type* device, const struct imu_hid_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
|
|
|
|
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 || !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) {
|
|
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;
|
|
if (!ctx || !ctx->h) return -1;
|
|
|
|
XOImu imu = (XOImu){0};
|
|
int rc = xo_next(ctx->h, &imu);
|
|
if (rc != 0) return rc; // propagate non-zero (e.g., error or no-sample)
|
|
|
|
memset(out, 0, sizeof(*out));
|
|
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];
|
|
out->mx = out->my = out->mz = NAN; // XO protocol doesn't provide mag
|
|
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 {
|
|
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;
|
|
}
|
|
|
|
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,
|
|
};
|