The open source OpenXR runtime
u/sink: Add u_imu_sink_force_monotonic and u_imu_sink_split
authored by
Moses Turner
and committed by
Jakob Bornecrantz
17e0c39d
48e8894b
+187
+2
src/xrt/auxiliary/util/CMakeLists.txt
+2
src/xrt/auxiliary/util/CMakeLists.txt
+85
src/xrt/auxiliary/util/u_imu_sink_force_monotonic.c
+85
src/xrt/auxiliary/util/u_imu_sink_force_monotonic.c
···
1
+
// Copyright 2019-2021, Collabora, Ltd.
2
+
// SPDX-License-Identifier: BSL-1.0
3
+
/*!
4
+
* @file
5
+
* @brief A @ref xrt_imu_sink that forces the samples to be monotonically increasing.
6
+
* @author Moses Turner <moses@collabora.com>
7
+
* @ingroup aux_util
8
+
*/
9
+
10
+
#include "util/u_sink.h"
11
+
#include "util/u_trace_marker.h"
12
+
#include "util/u_logging.h"
13
+
14
+
15
+
/*!
16
+
* An @ref xrt_imu_sink splitter.
17
+
* @implements xrt_imu_sink
18
+
* @implements xrt_frame_node
19
+
*/
20
+
struct u_imu_sink_force_monotonic
21
+
{
22
+
struct xrt_imu_sink base;
23
+
struct xrt_frame_node node;
24
+
25
+
timepoint_ns last_ts;
26
+
struct xrt_imu_sink *downstream;
27
+
};
28
+
29
+
static void
30
+
split_sample(struct xrt_imu_sink *xfs, struct xrt_imu_sample *sample)
31
+
{
32
+
SINK_TRACE_MARKER();
33
+
34
+
struct u_imu_sink_force_monotonic *s = (struct u_imu_sink_force_monotonic *)xfs;
35
+
36
+
if (sample->timestamp_ns == s->last_ts) {
37
+
U_LOG_W("Got an IMU sample with a duplicate timestamp! Old: %lu; New: %lu", s->last_ts,
38
+
sample->timestamp_ns);
39
+
return;
40
+
} else if (sample->timestamp_ns < s->last_ts) {
41
+
U_LOG_W("Got an IMU sample with a non-monotonically-increasing timestamp! Old: %lu; New: %lu",
42
+
s->last_ts, sample->timestamp_ns);
43
+
return;
44
+
}
45
+
46
+
s->last_ts = sample->timestamp_ns;
47
+
48
+
xrt_sink_push_imu(s->downstream, sample);
49
+
}
50
+
51
+
static void
52
+
split_break_apart(struct xrt_frame_node *node)
53
+
{
54
+
// Noop
55
+
}
56
+
57
+
static void
58
+
split_destroy(struct xrt_frame_node *node)
59
+
{
60
+
struct u_imu_sink_force_monotonic *s = container_of(node, struct u_imu_sink_force_monotonic, node);
61
+
62
+
free(s);
63
+
}
64
+
65
+
/*
66
+
*
67
+
* Exported functions.
68
+
*
69
+
*/
70
+
71
+
void
72
+
u_imu_sink_force_monotonic_create(struct xrt_frame_context *xfctx,
73
+
struct xrt_imu_sink *downstream,
74
+
struct xrt_imu_sink **out_imu_sink)
75
+
{
76
+
77
+
struct u_imu_sink_force_monotonic *s = U_TYPED_CALLOC(struct u_imu_sink_force_monotonic);
78
+
s->base.push_imu = split_sample;
79
+
s->node.break_apart = split_break_apart;
80
+
s->node.destroy = split_destroy;
81
+
s->downstream = downstream;
82
+
83
+
xrt_frame_context_add(xfctx, &s->node);
84
+
*out_imu_sink = &s->base;
85
+
}
+75
src/xrt/auxiliary/util/u_imu_sink_split.c
+75
src/xrt/auxiliary/util/u_imu_sink_split.c
···
1
+
// Copyright 2019-2021, Collabora, Ltd.
2
+
// SPDX-License-Identifier: BSL-1.0
3
+
/*!
4
+
* @file
5
+
* @brief An @ref xrt_imu_sink splitter.
6
+
* @author Moses Turner <moses@collabora.com>
7
+
* @ingroup aux_util
8
+
*/
9
+
10
+
#include "util/u_sink.h"
11
+
#include "util/u_trace_marker.h"
12
+
13
+
14
+
/*!
15
+
* An @ref xrt_imu_sink splitter.
16
+
* @implements xrt_imu_sink
17
+
* @implements xrt_frame_node
18
+
*/
19
+
struct u_imu_sink_split
20
+
{
21
+
struct xrt_imu_sink base;
22
+
struct xrt_frame_node node;
23
+
24
+
struct xrt_imu_sink *downstream_one;
25
+
struct xrt_imu_sink *downstream_two;
26
+
};
27
+
28
+
static void
29
+
split_sample(struct xrt_imu_sink *xfs, struct xrt_imu_sample *sample)
30
+
{
31
+
SINK_TRACE_MARKER();
32
+
33
+
struct u_imu_sink_split *s = (struct u_imu_sink_split *)xfs;
34
+
35
+
xrt_sink_push_imu(s->downstream_one, sample);
36
+
xrt_sink_push_imu(s->downstream_two, sample);
37
+
}
38
+
39
+
static void
40
+
split_break_apart(struct xrt_frame_node *node)
41
+
{
42
+
// Noop
43
+
}
44
+
45
+
static void
46
+
split_destroy(struct xrt_frame_node *node)
47
+
{
48
+
struct u_imu_sink_split *s = container_of(node, struct u_imu_sink_split, node);
49
+
50
+
free(s);
51
+
}
52
+
53
+
/*
54
+
*
55
+
* Exported functions.
56
+
*
57
+
*/
58
+
59
+
void
60
+
u_imu_sink_split_create(struct xrt_frame_context *xfctx,
61
+
struct xrt_imu_sink *downstream_one,
62
+
struct xrt_imu_sink *downstream_two,
63
+
struct xrt_imu_sink **out_imu_sink)
64
+
{
65
+
66
+
struct u_imu_sink_split *s = U_TYPED_CALLOC(struct u_imu_sink_split);
67
+
s->base.push_imu = split_sample;
68
+
s->node.break_apart = split_break_apart;
69
+
s->node.destroy = split_destroy;
70
+
s->downstream_one = downstream_one;
71
+
s->downstream_two = downstream_two;
72
+
73
+
xrt_frame_context_add(xfctx, &s->node);
74
+
*out_imu_sink = &s->base;
75
+
}
+25
src/xrt/auxiliary/util/u_sink.h
+25
src/xrt/auxiliary/util/u_sink.h
···
12
12
13
13
#include "os/os_threading.h"
14
14
#include "xrt/xrt_frame.h"
15
+
#include "xrt/xrt_tracking.h"
15
16
16
17
17
18
#ifdef __cplusplus
···
228
229
{
229
230
os_mutex_destroy(&usd->mutex);
230
231
}
232
+
233
+
234
+
/*!
235
+
* @public @memberof xrt_imu_sink
236
+
* @see xrt_frame_context
237
+
* Takes an IMU sample and pushes it to two sinks
238
+
*/
239
+
void
240
+
u_imu_sink_split_create(struct xrt_frame_context *xfctx,
241
+
struct xrt_imu_sink *downstream_one,
242
+
struct xrt_imu_sink *downstream_two,
243
+
struct xrt_imu_sink **out_imu_sink);
244
+
245
+
246
+
/*!
247
+
* @public @memberof xrt_imu_sink
248
+
* @see xrt_frame_context
249
+
* Takes an IMU sample and only pushes it if its timestamp has monotonically increased.
250
+
* Useful for handling hardware inconsistencies.
251
+
*/
252
+
void
253
+
u_imu_sink_force_monotonic_create(struct xrt_frame_context *xfctx,
254
+
struct xrt_imu_sink *downstream,
255
+
struct xrt_imu_sink **out_imu_sink);
231
256
232
257
233
258
#ifdef __cplusplus