The open source OpenXR runtime
u/tracked: Add u_tracked_imu_3dof
authored by
Moses Turner
and committed by
Jakob Bornecrantz
833787c4
d48792d9
+131
+2
src/xrt/auxiliary/util/CMakeLists.txt
+2
src/xrt/auxiliary/util/CMakeLists.txt
+83
src/xrt/auxiliary/util/u_tracked_imu_3dof.c
+83
src/xrt/auxiliary/util/u_tracked_imu_3dof.c
···
1
+
// Copyright 2019-2022, Collabora, Ltd.
2
+
// SPDX-License-Identifier: BSL-1.0
3
+
/*!
4
+
* @file
5
+
* @brief Wrapper for m_imu_3dof that can be placed inside (and freed along with!) an `xrt_imu_sink` pipeline.
6
+
* @author Moses Turner <moses@collabora.com>
7
+
* @ingroup aux_util
8
+
*/
9
+
10
+
#pragma once
11
+
12
+
#include "u_tracked_imu_3dof.h"
13
+
14
+
15
+
#ifdef __cplusplus
16
+
extern "C" {
17
+
#endif
18
+
19
+
static void
20
+
u_tracked_imu_receive_imu_sample(struct xrt_imu_sink *imu_sink, struct xrt_imu_sample *imu_sample)
21
+
{
22
+
struct u_tracked_imu_3dof *dof3 = container_of(imu_sink, struct u_tracked_imu_3dof, sink);
23
+
24
+
struct xrt_vec3 a;
25
+
struct xrt_vec3 g;
26
+
27
+
a.x = imu_sample->accel_m_s2.x;
28
+
a.y = imu_sample->accel_m_s2.y;
29
+
a.z = imu_sample->accel_m_s2.z;
30
+
31
+
g.x = imu_sample->gyro_rad_secs.x;
32
+
g.y = imu_sample->gyro_rad_secs.y;
33
+
g.z = imu_sample->gyro_rad_secs.z;
34
+
35
+
m_imu_3dof_update(&dof3->fusion, imu_sample->timestamp_ns, &a, &g);
36
+
37
+
struct xrt_space_relation rel = {0};
38
+
rel.relation_flags = (enum xrt_space_relation_flags)(XRT_SPACE_RELATION_ORIENTATION_VALID_BIT |
39
+
XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT);
40
+
rel.pose.orientation = dof3->fusion.rot;
41
+
42
+
m_relation_history_push(dof3->rh, &rel, imu_sample->timestamp_ns);
43
+
}
44
+
45
+
static void
46
+
u_tracked_imu_node_break_apart(struct xrt_frame_node *imu_node)
47
+
{}
48
+
49
+
static void
50
+
u_tracked_imu_node_destroy(struct xrt_frame_node *imu_node)
51
+
{
52
+
struct u_tracked_imu_3dof *dof3 = container_of(imu_node, struct u_tracked_imu_3dof, node);
53
+
54
+
m_imu_3dof_close(&dof3->fusion);
55
+
m_relation_history_destroy(&dof3->rh);
56
+
57
+
free(dof3);
58
+
}
59
+
60
+
void
61
+
u_tracked_imu_3dof_create(struct xrt_frame_context *xfctx, struct u_tracked_imu_3dof **out_3dof, void *debug_var_root)
62
+
{
63
+
struct u_tracked_imu_3dof *dof3 = U_TYPED_CALLOC(struct u_tracked_imu_3dof);
64
+
65
+
m_relation_history_create(&dof3->rh);
66
+
67
+
m_imu_3dof_init(&dof3->fusion, M_IMU_3DOF_USE_GRAVITY_DUR_300MS);
68
+
m_imu_3dof_add_vars(&dof3->fusion, debug_var_root, "");
69
+
70
+
dof3->sink.push_imu = u_tracked_imu_receive_imu_sample;
71
+
72
+
dof3->node.break_apart = u_tracked_imu_node_break_apart;
73
+
dof3->node.destroy = u_tracked_imu_node_destroy;
74
+
75
+
xrt_frame_context_add(xfctx, &dof3->node);
76
+
77
+
*out_3dof = dof3;
78
+
}
79
+
80
+
81
+
#ifdef __cplusplus
82
+
}
83
+
#endif
+46
src/xrt/auxiliary/util/u_tracked_imu_3dof.h
+46
src/xrt/auxiliary/util/u_tracked_imu_3dof.h
···
1
+
// Copyright 2019-2022, Collabora, Ltd.
2
+
// SPDX-License-Identifier: BSL-1.0
3
+
/*!
4
+
* @file
5
+
* @brief Wrapper for m_imu_3dof that can be placed inside (and freed along with!) an `xrt_imu_sink` pipeline.
6
+
* @author Moses Turner <moses@collabora.com>
7
+
* @ingroup aux_util
8
+
*/
9
+
10
+
#pragma once
11
+
12
+
#include "math/m_imu_3dof.h"
13
+
#include "math/m_relation_history.h"
14
+
#include "xrt/xrt_frame.h"
15
+
#include "xrt/xrt_tracking.h"
16
+
17
+
18
+
#ifdef __cplusplus
19
+
extern "C" {
20
+
#endif
21
+
22
+
/*!
23
+
* @see u_tracked_imu_3dof_create
24
+
*/
25
+
struct u_tracked_imu_3dof
26
+
{
27
+
struct xrt_imu_sink sink;
28
+
struct xrt_frame_node node;
29
+
30
+
struct m_imu_3dof fusion;
31
+
struct m_relation_history *rh;
32
+
};
33
+
34
+
35
+
/*!
36
+
* @see xrt_frame_context
37
+
* Creates a wrapper for m_imu_3dof that can be placed inside (and freed along with!) an `xrt_imu_sink` pipeline.
38
+
* Useful when your frameserver is significantly separated from your xrt_device
39
+
*/
40
+
void
41
+
u_tracked_imu_3dof_create(struct xrt_frame_context *xfctx, struct u_tracked_imu_3dof **out_3dof, void *debug_var_root);
42
+
43
+
44
+
#ifdef __cplusplus
45
+
}
46
+
#endif