d/vive: Improve ticks-to-nanoseconds conversion

src/xrt/drivers/vive/vive.h

@@ -10,6 +10,11 @@
 
 #pragma once
 
+#include <stdint.h>
+#include <asm/byteorder.h>
+
+#include "util/u_time.h"
+
 /*
  *
  * Printing functions.
@@ -21,3 +26,36 @@
 #define VIVE_INFO(d, ...) U_LOG_IFL_I(d->log_level, __VA_ARGS__)
 #define VIVE_WARN(d, ...) U_LOG_IFL_W(d->log_level, __VA_ARGS__)
 #define VIVE_ERROR(d, ...) U_LOG_IFL_E(d->log_level, __VA_ARGS__)
+
+#define VIVE_CLOCK_FREQ 48e6 // 48 MHz
+
+/*!
+ * Helper function to convert raw device ticks to nanosecond timestamps.
+ *
+ * `inout` params should start as the results from the previous sample call.
+ * For the first call, pass zero for the `inout` params.
+ *
+ * @param sample_ticks_raw Current sample ticks
+ * @param[in, out] inout_prev_ticks Overwritten with `sample_ticks_raw`
+ * @param[in, out] inout_ts_ns Resulting timestamp in nanoseconds
+ */
+static inline void
+ticks_to_ns(uint32_t sample_ticks_raw, uint32_t *inout_prev_ticks, timepoint_ns *inout_ts_ns)
+{
+	uint32_t sample_ticks = __le32_to_cpu(sample_ticks_raw);
+	uint32_t prev_ticks = *inout_prev_ticks;
+
+	uint32_t delta_ticks = 0;
+	if (prev_ticks < sample_ticks) {
+		delta_ticks = sample_ticks - prev_ticks;
+	} else { // Handle overflow
+		delta_ticks = (UINT32_MAX - prev_ticks) + sample_ticks;
+	}
+
+	const double one_tick_in_s = (1 / VIVE_CLOCK_FREQ);
+	const double one_tick_in_ns = one_tick_in_s * U_TIME_1S_IN_NS;
+	time_duration_ns delta_ns = delta_ticks * one_tick_in_ns;
+
+	*inout_prev_ticks = sample_ticks;
+	*inout_ts_ns += delta_ns;
+}

src/xrt/drivers/vive/vive_controller.c

@@ -92,8 +92,6 @@ enum vive_controller_input_index
 
 
 
-#define VIVE_CLOCK_FREQ 48000000.0f // Hz = 48 MHz
-
 #define DEFAULT_HAPTIC_FREQ 150.0f
 #define MIN_HAPTIC_DURATION 0.05f
 
@@ -529,29 +527,6 @@ controller_handle_analog_trigger(struct vive_controller_device *d, struct vive_c
 	VIVE_TRACE(d, "Trigger %f\n", d->state.trigger);
 }
 
-static inline uint32_t
-calc_dt_raw_and_handle_overflow(struct vive_controller_device *d, uint32_t sample_time)
-{
-	uint64_t dt_raw = (uint64_t)sample_time - (uint64_t)d->imu.last_sample_time_raw;
-	d->imu.last_sample_time_raw = sample_time;
-
-	// The 32-bit tick counter has rolled over,
-	// adjust the "negative" value to be positive.
-	// It's easiest to do this with 64-bits.
-	if (dt_raw > 0xFFFFFFFF) {
-		dt_raw += 0x100000000;
-	}
-
-	return (uint32_t)dt_raw;
-}
-
-static inline uint64_t
-cald_dt_ns(uint32_t dt_raw)
-{
-	double f = (double)(dt_raw) / VIVE_CLOCK_FREQ;
-	uint64_t diff_ns = (uint64_t)(f * 1000.0 * 1000.0 * 1000.0);
-	return diff_ns;
-}
 
 static void
 vive_controller_handle_imu_sample(struct vive_controller_device *d, struct watchman_imu_sample *sample)
@@ -560,8 +535,7 @@ vive_controller_handle_imu_sample(struct vive_controller_device *d, struct watch
 
 	/* ouvrt: "Time in 48 MHz ticks, but we are missing the low byte" */
 	uint32_t time_raw = d->last_ticks | (sample->timestamp_hi << 8);
-	uint32_t dt_raw = calc_dt_raw_and_handle_overflow(d, time_raw);
-	uint64_t dt_ns = cald_dt_ns(dt_raw);
+	ticks_to_ns(time_raw, &d->imu.last_sample_ticks, &d->imu.last_sample_ts_ns);
 
 	d->imu.ts_received_ns = os_monotonic_get_ns();
 
@@ -619,11 +593,10 @@ vive_controller_handle_imu_sample(struct vive_controller_device *d, struct watch
 		angular_velocity = fixed_angular_velocity;
 	}
 
-	d->imu.time_ns += dt_ns;
 	d->last.acc = acceleration;
 	d->last.gyro = angular_velocity;
 
-	m_imu_3dof_update(&d->fusion, d->imu.time_ns, &acceleration, &angular_velocity);
+	m_imu_3dof_update(&d->fusion, d->imu.last_sample_ts_ns, &acceleration, &angular_velocity);
 
 	d->rot_filtered = d->fusion.rot;
 

src/xrt/drivers/vive/vive_controller.h

@@ -53,8 +53,8 @@ struct vive_controller_device
 
 	struct
 	{
-		uint64_t time_ns;
-		uint32_t last_sample_time_raw;
+		timepoint_ns last_sample_ts_ns;
+		uint32_t last_sample_ticks;
 		timepoint_ns ts_received_ns;
 	} imu;
 

src/xrt/drivers/vive/vive_device.c

@@ -30,7 +30,6 @@
 #include "vive_protocol.h"
 
 
-#define VIVE_CLOCK_FREQ 48e6 // 48 MHz
 
 DEBUG_GET_ONCE_LOG_OPTION(vive_log, "VIVE_LOG", U_LOGGING_WARN)
 
@@ -241,39 +240,16 @@ oldest_sequence_index(uint8_t a, uint8_t b, uint8_t c)
 	return 0;
 }
 
-static inline uint32_t
-calc_dt_raw_and_handle_overflow(struct vive_device *d, uint32_t sample_time)
-{
-	uint64_t dt_raw = (uint64_t)sample_time - (uint64_t)d->imu.last_sample_time_raw;
-	d->imu.last_sample_time_raw = sample_time;
-
-	// The 32-bit tick counter has rolled over,
-	// adjust the "negative" value to be positive.
-	// It's easiest to do this with 64-bits.
-	if (dt_raw > 0xFFFFFFFF) {
-		dt_raw += 0x100000000;
-	}
-
-	return (uint32_t)dt_raw;
-}
-
-static inline uint64_t
-cald_dt_ns(uint32_t dt_raw)
-{
-	double f = (double)(dt_raw) / VIVE_CLOCK_FREQ;
-	uint64_t diff_ns = (uint64_t)(f * 1000.0 * 1000.0 * 1000.0);
-	return diff_ns;
-}
-
 static void
 update_imu(struct vive_device *d, const void *buffer)
 {
 	XRT_TRACE_MARKER();
 
+	uint64_t now_ns = os_monotonic_get_ns();
+
 	const struct vive_imu_report *report = buffer;
 	const struct vive_imu_sample *sample = report->sample;
 	uint8_t last_seq = d->imu.sequence;
-	d->imu.ts_received_ns = os_monotonic_get_ns();
 	int i;
 	int j;
 
@@ -298,10 +274,7 @@ update_imu(struct vive_device *d, const void *buffer)
 			continue;
 		}
 
-		uint32_t time_raw = __le32_to_cpu(sample->time);
-		uint32_t dt_raw = calc_dt_raw_and_handle_overflow(d, time_raw);
-		uint64_t dt_ns = cald_dt_ns(dt_raw);
-
+		ticks_to_ns(sample->time, &d->imu.last_sample_ticks, &d->imu.last_sample_ts_ns);
 
 		int16_t acc[3] = {
 		    (int16_t)__le16_to_cpu(sample->acc[0]),
@@ -371,7 +344,6 @@ update_imu(struct vive_device *d, const void *buffer)
 		default: VIVE_ERROR(d, "Unhandled Vive variant"); return;
 		}
 
-		d->imu.time_ns += dt_ns;
 		d->imu.sequence = seq;
 
 		struct xrt_space_relation rel = {0};
@@ -379,9 +351,9 @@ update_imu(struct vive_device *d, const void *buffer)
 		    XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT;
 
 		os_mutex_lock(&d->fusion.mutex);
-		m_imu_3dof_update(&d->fusion.i3dof, d->imu.time_ns, &acceleration, &angular_velocity);
+		m_imu_3dof_update(&d->fusion.i3dof, d->imu.last_sample_ts_ns, &acceleration, &angular_velocity);
 		rel.pose.orientation = d->fusion.i3dof.rot;
-		m_relation_history_push(d->fusion.relation_hist, &rel, d->imu.ts_received_ns);
+		m_relation_history_push(d->fusion.relation_hist, &rel, now_ns);
 		os_mutex_unlock(&d->fusion.mutex);
 	}
 }

src/xrt/drivers/vive/vive_device.h

@@ -10,10 +10,10 @@
 #pragma once
 
 #include "xrt/xrt_device.h"
-#include "math/m_imu_3dof.h"
 #include "os/os_threading.h"
 #include "util/u_logging.h"
 #include "util/u_distortion_mesh.h"
+#include "math/m_imu_3dof.h"
 #include "math/m_relation_history.h"
 
 #include "vive/vive_config.h"
@@ -42,10 +42,9 @@ struct vive_device
 
 	struct
 	{
-		uint64_t time_ns;
+		timepoint_ns last_sample_ts_ns;
+		uint32_t last_sample_ticks;
 		uint8_t sequence;
-		uint32_t last_sample_time_raw;
-		timepoint_ns ts_received_ns;
 	} imu;
 
 	struct
@@ -56,7 +55,6 @@ struct vive_device
 		uint8_t button;
 	} board;
 
-
 	enum u_logging_level log_level;
 	bool disconnect_notified;