u/tracked: Add u_tracked_imu_3dof

src/xrt/auxiliary/util/CMakeLists.txt

@@ -83,6 +83,8 @@ add_library(
 	u_time.h
 	u_trace_marker.c
 	u_trace_marker.h
+	u_tracked_imu_3dof.c
+	u_tracked_imu_3dof.h
 	u_var.cpp
 	u_var.h
 	u_vector.cpp

src/xrt/auxiliary/util/u_tracked_imu_3dof.c

@@ -0,0 +1,83 @@
+// Copyright 2019-2022, Collabora, Ltd.
+// SPDX-License-Identifier: BSL-1.0
+/*!
+ * @file
+ * @brief Wrapper for m_imu_3dof that can be placed inside (and freed along with!) an `xrt_imu_sink` pipeline.
+ * @author Moses Turner <moses@collabora.com>
+ * @ingroup aux_util
+ */
+
+#pragma once
+
+#include "u_tracked_imu_3dof.h"
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+static void
+u_tracked_imu_receive_imu_sample(struct xrt_imu_sink *imu_sink, struct xrt_imu_sample *imu_sample)
+{
+	struct u_tracked_imu_3dof *dof3 = container_of(imu_sink, struct u_tracked_imu_3dof, sink);
+
+	struct xrt_vec3 a;
+	struct xrt_vec3 g;
+
+	a.x = imu_sample->accel_m_s2.x;
+	a.y = imu_sample->accel_m_s2.y;
+	a.z = imu_sample->accel_m_s2.z;
+
+	g.x = imu_sample->gyro_rad_secs.x;
+	g.y = imu_sample->gyro_rad_secs.y;
+	g.z = imu_sample->gyro_rad_secs.z;
+
+	m_imu_3dof_update(&dof3->fusion, imu_sample->timestamp_ns, &a, &g);
+
+	struct xrt_space_relation rel = {0};
+	rel.relation_flags = (enum xrt_space_relation_flags)(XRT_SPACE_RELATION_ORIENTATION_VALID_BIT |
+	                                                     XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT);
+	rel.pose.orientation = dof3->fusion.rot;
+
+	m_relation_history_push(dof3->rh, &rel, imu_sample->timestamp_ns);
+}
+
+static void
+u_tracked_imu_node_break_apart(struct xrt_frame_node *imu_node)
+{}
+
+static void
+u_tracked_imu_node_destroy(struct xrt_frame_node *imu_node)
+{
+	struct u_tracked_imu_3dof *dof3 = container_of(imu_node, struct u_tracked_imu_3dof, node);
+
+	m_imu_3dof_close(&dof3->fusion);
+	m_relation_history_destroy(&dof3->rh);
+
+	free(dof3);
+}
+
+void
+u_tracked_imu_3dof_create(struct xrt_frame_context *xfctx, struct u_tracked_imu_3dof **out_3dof, void *debug_var_root)
+{
+	struct u_tracked_imu_3dof *dof3 = U_TYPED_CALLOC(struct u_tracked_imu_3dof);
+
+	m_relation_history_create(&dof3->rh);
+
+	m_imu_3dof_init(&dof3->fusion, M_IMU_3DOF_USE_GRAVITY_DUR_300MS);
+	m_imu_3dof_add_vars(&dof3->fusion, debug_var_root, "");
+
+	dof3->sink.push_imu = u_tracked_imu_receive_imu_sample;
+
+	dof3->node.break_apart = u_tracked_imu_node_break_apart;
+	dof3->node.destroy = u_tracked_imu_node_destroy;
+
+	xrt_frame_context_add(xfctx, &dof3->node);
+
+	*out_3dof = dof3;
+}
+
+
+#ifdef __cplusplus
+}
+#endif

src/xrt/auxiliary/util/u_tracked_imu_3dof.h

@@ -0,0 +1,46 @@
+// Copyright 2019-2022, Collabora, Ltd.
+// SPDX-License-Identifier: BSL-1.0
+/*!
+ * @file
+ * @brief Wrapper for m_imu_3dof that can be placed inside (and freed along with!) an `xrt_imu_sink` pipeline.
+ * @author Moses Turner <moses@collabora.com>
+ * @ingroup aux_util
+ */
+
+#pragma once
+
+#include "math/m_imu_3dof.h"
+#include "math/m_relation_history.h"
+#include "xrt/xrt_frame.h"
+#include "xrt/xrt_tracking.h"
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*!
+ * @see u_tracked_imu_3dof_create
+ */
+struct u_tracked_imu_3dof
+{
+	struct xrt_imu_sink sink;
+	struct xrt_frame_node node;
+
+	struct m_imu_3dof fusion;
+	struct m_relation_history *rh;
+};
+
+
+/*!
+ * @see xrt_frame_context
+ * Creates a wrapper for m_imu_3dof that can be placed inside (and freed along with!) an `xrt_imu_sink` pipeline.
+ * Useful when your frameserver is significantly separated from your xrt_device
+ */
+void
+u_tracked_imu_3dof_create(struct xrt_frame_context *xfctx, struct u_tracked_imu_3dof **out_3dof, void *debug_var_root);
+
+
+#ifdef __cplusplus
+}
+#endif