diff --git a/src/xrt/auxiliary/util/CMakeLists.txt b/src/xrt/auxiliary/util/CMakeLists.txt index 95bdf4524..96c5b6b0f 100644 --- a/src/xrt/auxiliary/util/CMakeLists.txt +++ b/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 diff --git a/src/xrt/auxiliary/util/u_tracked_imu_3dof.c b/src/xrt/auxiliary/util/u_tracked_imu_3dof.c new file mode 100644 index 000000000..f8b16f603 --- /dev/null +++ b/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 + * @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 diff --git a/src/xrt/auxiliary/util/u_tracked_imu_3dof.h b/src/xrt/auxiliary/util/u_tracked_imu_3dof.h new file mode 100644 index 000000000..68a863725 --- /dev/null +++ b/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 + * @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