d/psvr: Port PSVR to use timestamps

src/xrt/auxiliary/tracking/t_tracker_psvr.cpp

@@ -41,6 +41,8 @@ public:
 	//! Have we received a new IMU sample.
 	bool has_imu = false;
 
+	timepoint_ns last_imu{0};
+
 	struct
 	{
 		struct xrt_vec3 pos = {};
@@ -124,7 +126,7 @@ get_pose(TrackerPSVR &t,
 
 static void
 imu_data(TrackerPSVR &t,
-         time_duration_ns delta_ns,
+         timepoint_ns timestamp_ns,
          struct xrt_tracking_sample *sample)
 {
 	os_thread_helper_lock(&t.oth);
@@ -134,11 +136,14 @@ imu_data(TrackerPSVR &t,
 		os_thread_helper_unlock(&t.oth);
 		return;
 	}
-
-	float dt = time_ns_to_s(delta_ns);
-	// Super simple fusion.
-	math_quat_integrate_velocity(&t.fusion.rot, &sample->gyro_rad_secs, dt,
-	                             &t.fusion.rot);
+	if (t.last_imu != 0) {
+		time_duration_ns delta_ns = timestamp_ns - t.last_imu;
+		float dt = time_ns_to_s(delta_ns);
+		// Super simple fusion.
+		math_quat_integrate_velocity(
+		    &t.fusion.rot, &sample->gyro_rad_secs, dt, &t.fusion.rot);
+	}
+	t.last_imu = timestamp_ns;
 
 	os_thread_helper_unlock(&t.oth);
 }
@@ -177,11 +182,11 @@ break_apart(TrackerPSVR &t)
 
 extern "C" void
 t_psvr_push_imu(struct xrt_tracked_psvr *xtmv,
-                time_duration_ns delta_ns,
+                timepoint_ns timestamp_ns,
                 struct xrt_tracking_sample *sample)
 {
 	auto &t = *container_of(xtmv, TrackerPSVR, base);
-	imu_data(t, delta_ns, sample);
+	imu_data(t, timestamp_ns, sample);
 }
 
 extern "C" void

src/xrt/drivers/psvr/psvr_device.c

@@ -59,6 +59,9 @@ struct psvr_device
 
 	struct xrt_tracked_psvr *tracker;
 
+	struct time_state *timekeeping;
+	timepoint_ns last_sensor_time;
+
 	struct psvr_parsed_sensor last;
 
 	struct
@@ -300,7 +303,8 @@ read_sample_and_apply_calibration(struct psvr_device *psvr,
 static void
 update_fusion(struct psvr_device *psvr,
               struct psvr_parsed_sample *sample,
-              uint32_t tick_delta)
+              uint32_t tick_delta,
+              timepoint_ns timestamp_ns)
 {
 	struct xrt_vec3 mag = {0.0f, 0.0f, 0.0f};
 	(void)mag;
@@ -309,14 +313,11 @@ update_fusion(struct psvr_device *psvr,
 	                                  &psvr->read.gyro);
 
 	if (psvr->tracker != NULL) {
-		time_duration_ns delta_ns =
-		    tick_delta * (1000000000.0 / PSVR_TICKS_PER_SECOND);
-
 		struct xrt_tracking_sample sample;
 		sample.accel_m_s2 = psvr->read.accel;
 		sample.gyro_rad_secs = psvr->read.gyro;
 
-		xrt_tracked_psvr_push_imu(psvr->tracker, delta_ns, &sample);
+		xrt_tracked_psvr_push_imu(psvr->tracker, timestamp_ns, &sample);
 	} else {
 		float delta_secs = tick_delta / PSVR_TICKS_PER_SECOND;
 
@@ -355,6 +356,7 @@ handle_tracker_sensor_msg(struct psvr_device *psvr,
                           unsigned char *buffer,
                           int size)
 {
+	timepoint_ns now = time_state_get_now_and_update(psvr->timekeeping);
 	uint32_t last_sample_tick = psvr->last.samples[1].tick;
 
 	if (!psvr_parse_sensor_packet(&psvr->last, buffer, size)) {
@@ -382,16 +384,19 @@ handle_tracker_sensor_msg(struct psvr_device *psvr,
 			tick_delta = 500;
 		}
 	}
-
-	// Update the fusion with first sample.
-	update_fusion(psvr, &s->samples[0], tick_delta);
-
 	// New delta between the two samples.
-	tick_delta = calc_delta_and_handle_rollover(s->samples[1].tick,
-	                                            s->samples[0].tick);
+	uint32_t tick_delta2 = calc_delta_and_handle_rollover(
+	    s->samples[1].tick, s->samples[0].tick);
+
+	time_duration_ns inter_sample_duration_ns =
+	    tick_delta2 * PSVR_NS_PER_TICK;
+	// Update the fusion with first sample.
+	update_fusion(psvr, &s->samples[0], tick_delta,
+	              now - inter_sample_duration_ns);
 
 	// Update the fusion with second sample.
-	update_fusion(psvr, &s->samples[1], tick_delta);
+	update_fusion(psvr, &s->samples[1], tick_delta2, now);
+	psvr->last_sensor_time = now;
 }
 
 static void
@@ -837,6 +842,8 @@ teardown(struct psvr_device *psvr)
 	// Stop the variable tracking.
 	u_var_remove_root(psvr);
 
+	time_state_destroy(psvr->timekeeping);
+
 	// Includes null check, and sets to null.
 	xrt_tracked_psvr_destroy(&psvr->tracker);
 
@@ -1003,6 +1010,9 @@ psvr_device_create(struct hid_device_info *hmd_handle_info,
 		u_distortion_mesh_from_panotools(&vals, &vals, psvr->base.hmd);
 	}
 
+	//! @todo inject this, don't create it
+	psvr->timekeeping = time_state_create();
+
 #if 0
 	psvr->fusion = imu_fusion_create();
 #else

src/xrt/drivers/psvr/psvr_device.h

@@ -53,6 +53,7 @@ enum psvr_status_bits
 #define PSVR_STATUS_VR_MODE_ON 1
 
 #define PSVR_TICKS_PER_SECOND (1000000.0) // 1 MHz ticks
+#define PSVR_NS_PER_TICK (1000)           // Each tick is a microsecond
 
 #define PSVR_PKG_STATUS 0xF0
 #define PSVR_PKG_DEVICE_NAME 0x80

src/xrt/include/xrt/xrt_tracking.h

@@ -155,7 +155,7 @@ struct xrt_tracked_psvr
 	 * Push a IMU sample into the tracking system.
 	 */
 	void (*push_imu)(struct xrt_tracked_psvr *,
-	                 time_duration_ns delta_ns,
+	                 timepoint_ns timestamp_ns,
 	                 struct xrt_tracking_sample *sample);
 
 	/*!
@@ -221,10 +221,10 @@ xrt_tracked_psvr_get_tracked_pose(struct xrt_tracked_psvr *psvr,
 
 static inline void
 xrt_tracked_psvr_push_imu(struct xrt_tracked_psvr *psvr,
-                          time_duration_ns delta_ns,
+                          timepoint_ns timestamp_ns,
                           struct xrt_tracking_sample *sample)
 {
-	psvr->push_imu(psvr, delta_ns, sample);
+	psvr->push_imu(psvr, timestamp_ns, sample);
 }
 
 static inline void