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] 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 <hidapi/hidapi.h>
 #include <string.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <math.h>
+#include <stdint.h>
+#include <time.h>
 
 #include <xreal_one_driver.h>
 
@@ -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;
 }