From 6b99e2fbdc949a7aabce7e16caf566fcbfcb9dbf Mon Sep 17 00:00:00 2001
From: Jakob Bornecrantz <jakob@collabora.com>
Date: Tue, 22 Oct 2019 19:51:05 +0100
Subject: [PATCH] d/psvr: Add new imu code, but it's stubbed out... Unless?

---
 src/xrt/drivers/psvr/psvr_device.c | 28 ++++++++++++++++++++++++++++
 1 file changed, 28 insertions(+)

diff --git a/src/xrt/drivers/psvr/psvr_device.c b/src/xrt/drivers/psvr/psvr_device.c
index 9695da186..396f6f6f5 100644
--- a/src/xrt/drivers/psvr/psvr_device.c
+++ b/src/xrt/drivers/psvr/psvr_device.c
@@ -23,6 +23,8 @@
 #include "util/u_debug.h"
 #include "util/u_device.h"
 
+#include "tracking/t_imu.h"
+
 #include <math.h>
 #include <stdio.h>
 #include <stdlib.h>
@@ -89,10 +91,14 @@ struct psvr_device
 		bool control;
 	} gui;
 
+#if 0
+	struct imu_fusion *fusion;
+#else
 	struct
 	{
 		struct xrt_quat rot;
 	} fusion;
+#endif
 };
 
 
@@ -225,9 +231,19 @@ update_fusion(struct psvr_device *psvr,
 	} else {
 		float delta_secs = tick_delta / PSVR_TICKS_PER_SECOND;
 
+#if 0
+		struct xrt_vec3 ident = {0.0f, 1.0f, 0.0f};
+		struct xrt_vec3 var1 = {0};
+		struct xrt_vec3 var2 = {0};
+
+		imu_fusion_incorporate_gyros_and_accelerometer(
+		    psvr->fusion, delta_secs, &psvr->read.gyro, &var1,
+		    &psvr->read.accel, 0.0f, &ident, &var2);
+#else
 		math_quat_integrate_velocity(&psvr->fusion.rot,
 		                             &psvr->read.gyro, delta_secs,
 		                             &psvr->fusion.rot);
+#endif
 	}
 }
 
@@ -683,7 +699,15 @@ psvr_device_get_tracked_pose(struct xrt_device *xdev,
 
 	// We have no tracking, don't return a position.
 	if (psvr->tracker == NULL) {
+#if 0
+		struct xrt_vec3 ang_vel = {0};
+		imu_fusion_get_prediction(psvr->fusion, 0.0f,
+		                          &out_relation->pose.orientation,
+		                          &ang_vel);
+#else
 		out_relation->pose.orientation = psvr->fusion.rot;
+#endif
+
 		out_relation->relation_flags = (enum xrt_space_relation_flags)(
 		    XRT_SPACE_RELATION_ORIENTATION_VALID_BIT |
 		    XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT);
@@ -764,7 +788,11 @@ psvr_device_create(struct hid_device_info *hmd_handle_info,
 	psvr->base.hmd->distortion.mesh.num_vertex =
 	    ARRAY_SIZE(psvr_both_uvs) / 8;
 
+#if 0
+	psvr->fusion = imu_fusion_create();
+#else
 	psvr->fusion.rot.w = 1.0f;
+#endif
 
 	snprintf(psvr->base.str, XRT_DEVICE_NAME_LEN, "PS VR Headset");