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;