From a6b4e6a830f25b24f46ad9274d1d2819a6412444 Mon Sep 17 00:00:00 2001
From: Jakob Bornecrantz <jakob@collabora.com>
Date: Sun, 5 Dec 2021 13:38:18 +0000
Subject: [PATCH] m/3dof: Show more in the variable tracker

---
 src/xrt/auxiliary/math/m_imu_3dof.c | 22 +++++++++++++++++++++-
 src/xrt/auxiliary/math/m_imu_3dof.h |  5 +++++
 2 files changed, 26 insertions(+), 1 deletion(-)

diff --git a/src/xrt/auxiliary/math/m_imu_3dof.c b/src/xrt/auxiliary/math/m_imu_3dof.c
index 2c0cca19f..2dd4b8c0b 100644
--- a/src/xrt/auxiliary/math/m_imu_3dof.c
+++ b/src/xrt/auxiliary/math/m_imu_3dof.c
@@ -58,8 +58,14 @@ m_imu_3dof_add_vars(struct m_imu_3dof *f, void *root, const char *prefix)
 	u_var_add_ro_u64(root, &f->last.timestamp_ns, tmp);
 	snprintf(tmp, sizeof(tmp), "%slast.gyro", prefix);
 	u_var_add_ro_vec3_f32(root, &f->last.gyro, tmp);
+	snprintf(tmp, sizeof(tmp), "%slast.gyro_length", prefix);
+	u_var_add_ro_f32(root, &f->last.gyro_length, tmp);
+	snprintf(tmp, sizeof(tmp), "%slast.gyro_biased_length", prefix);
+	u_var_add_ro_f32(root, &f->last.gyro_biased_length, tmp);
 	snprintf(tmp, sizeof(tmp), "%slast.accel", prefix);
 	u_var_add_ro_vec3_f32(root, &f->last.accel, tmp);
+	snprintf(tmp, sizeof(tmp), "%slast.accel_length", prefix);
+	u_var_add_ro_f32(root, &f->last.accel_length, tmp);
 	snprintf(tmp, sizeof(tmp), "%slast.delta_ms", prefix);
 	u_var_add_ro_f32(root, &f->last.delta_ms, tmp);
 
@@ -69,6 +75,10 @@ m_imu_3dof_add_vars(struct m_imu_3dof *f, void *root, const char *prefix)
 	u_var_add_ro_vec3_f32(root, &f->grav.error_axis, tmp);
 	snprintf(tmp, sizeof(tmp), "%sgrav.error_angle", prefix);
 	u_var_add_ro_f32(root, &f->grav.error_angle, tmp);
+	snprintf(tmp, sizeof(tmp), "%sgrav.is_accel", prefix);
+	u_var_add_bool(root, &f->grav.is_accel, tmp);
+	snprintf(tmp, sizeof(tmp), "%sgrav.is_rotating", prefix);
+	u_var_add_bool(root, &f->grav.is_rotating, tmp);
 
 	snprintf(tmp, sizeof(tmp), "%sgyro_bias.value", prefix);
 	u_var_add_ro_vec3_f32(root, &f->gyro_bias.value, tmp);
@@ -102,11 +112,14 @@ gravity_correction(struct m_imu_3dof *f,
 	 * reset the counter and start over.
 	 */
 
-	bool is_accel = fabsf(m_vec3_len(*accel) - 9.82f) >= gravity_tolerance;
+	float accel_length = m_vec3_len(*accel);
+	bool is_accel = fabsf(accel_length - 9.82f) >= gravity_tolerance;
 	bool is_rotating = gyro_length >= gyro_tolerance;
 	if (is_accel || is_rotating) {
 		f->grav.level_timestamp_ns = timestamp_ns;
 	}
+	f->grav.is_accel = is_accel;
+	f->grav.is_rotating = is_rotating;
 
 	/*
 	 * Device has been level for long enough, grab mean from the
@@ -229,6 +242,13 @@ m_imu_3dof_update(struct m_imu_3dof *f,
 
 	struct xrt_vec3 gyro_biased = m_vec3_sub(*gyro, f->gyro_bias.value);
 	float gyro_biased_length = m_vec3_len(gyro_biased);
+	float gyro_length = m_vec3_len(*gyro);
+	float accel_length = m_vec3_len(*accel);
+
+	f->last.accel_length = accel_length;
+	f->last.gyro_length = gyro_length;
+	f->last.gyro_biased_length = gyro_biased_length;
+
 
 	if (gyro_biased_length > 0.0001f) {
 #if 0
diff --git a/src/xrt/auxiliary/math/m_imu_3dof.h b/src/xrt/auxiliary/math/m_imu_3dof.h
index 074349041..20632546a 100644
--- a/src/xrt/auxiliary/math/m_imu_3dof.h
+++ b/src/xrt/auxiliary/math/m_imu_3dof.h
@@ -41,6 +41,9 @@ struct m_imu_3dof
 		struct xrt_vec3 gyro;  //!< Angular velocity
 		struct xrt_vec3 accel; //!< Acceleration
 		float delta_ms;
+		float accel_length;
+		float gyro_length;
+		float gyro_biased_length;
 	} last;
 
 	enum m_imu_3dof_state state;
@@ -57,6 +60,8 @@ struct m_imu_3dof
 		uint64_t level_timestamp_ns;
 		struct xrt_vec3 error_axis;
 		float error_angle;
+		bool is_accel;
+		bool is_rotating;
 	} grav;
 
 	// gyro bias correction