mirror of
https://gitlab.freedesktop.org/monado/monado.git
synced 2025-01-23 23:21:46 +00:00
73 lines
2.1 KiB
C
73 lines
2.1 KiB
C
|
// 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
|
||
|
|
*/
|
||
|
|
|
||
|
|
#include "u_tracked_imu_3dof.h"
|
||
|
|
|
||
|
|
|
||
|
|
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;
|
||
|
|
}
|