From 21352ca338b52673689b59924cedcdfe5aa9ce51 Mon Sep 17 00:00:00 2001
From: Ryan Pavlik <ryan.pavlik@collabora.com>
Date: Tue, 12 Nov 2019 11:43:17 -0600
Subject: [PATCH] t_imu: Add param for accelerometer output

---
 src/xrt/auxiliary/tracking/t_imu.cpp | 13 +++++++++++--
 src/xrt/auxiliary/tracking/t_imu.h   | 10 ++++++++--
 src/xrt/drivers/psmv/psmv_driver.c   |  2 +-
 3 files changed, 20 insertions(+), 5 deletions(-)

diff --git a/src/xrt/auxiliary/tracking/t_imu.cpp b/src/xrt/auxiliary/tracking/t_imu.cpp
index 0db6858f6..fd600e888 100644
--- a/src/xrt/auxiliary/tracking/t_imu.cpp
+++ b/src/xrt/auxiliary/tracking/t_imu.cpp
@@ -11,6 +11,7 @@
 #include "t_imu.h"
 #include "t_imu_fusion.h"
 #include "math/m_eigen_interop.h"
+#include "util/u_misc.h"
 
 #include <memory>
 
@@ -69,13 +70,17 @@ int
 imu_fusion_incorporate_accelerometer(struct imu_fusion *fusion,
                                      uint64_t timestamp_ns,
                                      struct xrt_vec3 const *accel,
-                                     struct xrt_vec3 const *accel_variance)
+                                     struct xrt_vec3 const *accel_variance,
+                                     struct xrt_vec3 *out_world_accel)
 {
 	try {
 		assert(fusion);
 		assert(accel);
 		assert(accel_variance);
 		assert(timestamp_ns != 0);
+		if (out_world_accel != NULL) {
+			U_ZERO(out_world_accel);
+		}
 
 		fusion->simple_fusion.handleAccel(
 		    map_vec3(*accel).cast<double>(), timestamp_ns);
@@ -162,7 +167,8 @@ imu_fusion_incorporate_gyros_and_accelerometer(
     struct xrt_vec3 const *ang_vel,
     struct xrt_vec3 const *ang_vel_variance,
     struct xrt_vec3 const *accel,
-    struct xrt_vec3 const *accel_variance)
+    struct xrt_vec3 const *accel_variance,
+    struct xrt_vec3 *out_world_accel)
 {
 	try {
 		assert(fusion);
@@ -171,6 +177,9 @@ imu_fusion_incorporate_gyros_and_accelerometer(
 		assert(accel);
 		assert(accel_variance);
 		assert(timestamp_ns != 0);
+		if (out_world_accel != NULL) {
+			U_ZERO(out_world_accel);
+		}
 
 		Eigen::Vector3d accelVec = map_vec3(*accel).cast<double>();
 		Eigen::Vector3d angVelVec = map_vec3(*ang_vel).cast<double>();
diff --git a/src/xrt/auxiliary/tracking/t_imu.h b/src/xrt/auxiliary/tracking/t_imu.h
index cfb13440a..4462cb2d3 100644
--- a/src/xrt/auxiliary/tracking/t_imu.h
+++ b/src/xrt/auxiliary/tracking/t_imu.h
@@ -84,6 +84,8 @@ imu_fusion_incorporate_gyros(struct imu_fusion *fusion,
  * assumed to be +y when aligned with the world.
  * @param accel_variance The variance of the accelerometer measurements: part of
  * the characteristics of the IMU being used.
+ * @param out_world_accel Optional output parameter: will contain the
+ * non-gravity acceleration in the world frame.
  *
  * @public @memberof imu_fusion
  * @ingroup aux_tracking
@@ -92,7 +94,8 @@ int
 imu_fusion_incorporate_accelerometer(struct imu_fusion *fusion,
                                      uint64_t timestamp_ns,
                                      struct xrt_vec3 const *accel,
-                                     struct xrt_vec3 const *accel_variance);
+                                     struct xrt_vec3 const *accel_variance,
+                                     struct xrt_vec3 *out_world_accel);
 
 /*!
  * Predict and correct fusion with a simultaneous accelerometer and gyroscope
@@ -112,6 +115,8 @@ imu_fusion_incorporate_accelerometer(struct imu_fusion *fusion,
  * assumed to be +y when aligned with the world.
  * @param accel_variance The variance of the accelerometer measurements: part of
  * the characteristics of the IMU being used.
+ * @param out_world_accel Optional output parameter: will contain the
+ * non-gravity acceleration in the world frame.
  *
  * @public @memberof imu_fusion
  * @ingroup aux_tracking
@@ -123,7 +128,8 @@ imu_fusion_incorporate_gyros_and_accelerometer(
     struct xrt_vec3 const *ang_vel,
     struct xrt_vec3 const *ang_vel_variance,
     struct xrt_vec3 const *accel,
-    struct xrt_vec3 const *accel_variance);
+    struct xrt_vec3 const *accel_variance,
+    struct xrt_vec3 *out_world_accel);
 
 /*!
  * Get the predicted state. Does not advance the internal state clock.
diff --git a/src/xrt/drivers/psmv/psmv_driver.c b/src/xrt/drivers/psmv/psmv_driver.c
index f3c38aa69..ce948ee95 100644
--- a/src/xrt/drivers/psmv/psmv_driver.c
+++ b/src/xrt/drivers/psmv/psmv_driver.c
@@ -651,7 +651,7 @@ update_fusion(struct psmv_device *psmv,
 		imu_fusion_incorporate_gyros_and_accelerometer(
 		    psmv->fusion.fusion, timestamp_ns, &psmv->read.gyro,
 		    &psmv->fusion.variance.gyro, &psmv->read.accel,
-		    &psmv->fusion.variance.accel);
+		    &psmv->fusion.variance.accel, NULL);
 		struct xrt_vec3 angvel_dummy;
 		imu_fusion_get_prediction(psmv->fusion.fusion, timestamp_ns,
 		                          &psmv->fusion.rot, &angvel_dummy);