mirrors/monado
1
0
Fork 0
mirror of https://gitlab.freedesktop.org/monado/monado.git synced 2025-01-28 01:18:31 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

u/sink: Add u_imu_sink_force_monotonic and u_imu_sink_split

Browse source
This commit is contained in:
Moses Turner 2022-09-20 15:31:57 -05:00 committed by Jakob Bornecrantz
parent 48e8894b74
commit 17e0c39df2
4 changed files with 187 additions and 0 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
src/xrt/auxiliary/util/CMakeLists.txt
View file

@ -46,6 +46,8 @@ add_library(
u_hashset.h
u_id_ringbuffer.cpp
u_id_ringbuffer.h
u_imu_sink_split.c
u_imu_sink_force_monotonic.c
u_json.c
u_json.h
u_json.hpp

85
src/xrt/auxiliary/util/u_imu_sink_force_monotonic.c Normal file
View file

@ -0,0 +1,85 @@
// Copyright 2019-2021, Collabora, Ltd.
// SPDX-License-Identifier: BSL-1.0
/*!
* @file
* @brief A @ref xrt_imu_sink that forces the samples to be monotonically increasing.
* @author Moses Turner <moses@collabora.com>
* @ingroup aux_util
*/
#include "util/u_sink.h"
#include "util/u_trace_marker.h"
#include "util/u_logging.h"
/*!
* An @ref xrt_imu_sink splitter.
* @implements xrt_imu_sink
* @implements xrt_frame_node
*/
struct u_imu_sink_force_monotonic
{
struct xrt_imu_sink base;
struct xrt_frame_node node;
timepoint_ns last_ts;
struct xrt_imu_sink *downstream;
};
static void
split_sample(struct xrt_imu_sink *xfs, struct xrt_imu_sample *sample)
{
SINK_TRACE_MARKER();
struct u_imu_sink_force_monotonic *s = (struct u_imu_sink_force_monotonic *)xfs;
if (sample->timestamp_ns == s->last_ts) {
U_LOG_W("Got an IMU sample with a duplicate timestamp! Old: %lu; New: %lu", s->last_ts,
sample->timestamp_ns);
return;
} else if (sample->timestamp_ns < s->last_ts) {
U_LOG_W("Got an IMU sample with a non-monotonically-increasing timestamp! Old: %lu; New: %lu",
s->last_ts, sample->timestamp_ns);
return;
}
s->last_ts = sample->timestamp_ns;
xrt_sink_push_imu(s->downstream, sample);
}
static void
split_break_apart(struct xrt_frame_node *node)
{
// Noop
}
static void
split_destroy(struct xrt_frame_node *node)
{
struct u_imu_sink_force_monotonic *s = container_of(node, struct u_imu_sink_force_monotonic, node);
free(s);
}
/*
*
* Exported functions.
*
*/
void
u_imu_sink_force_monotonic_create(struct xrt_frame_context *xfctx,
struct xrt_imu_sink *downstream,
struct xrt_imu_sink **out_imu_sink)
{
struct u_imu_sink_force_monotonic *s = U_TYPED_CALLOC(struct u_imu_sink_force_monotonic);
s->base.push_imu = split_sample;
s->node.break_apart = split_break_apart;
s->node.destroy = split_destroy;
s->downstream = downstream;
xrt_frame_context_add(xfctx, &s->node);
*out_imu_sink = &s->base;
}

75
src/xrt/auxiliary/util/u_imu_sink_split.c Normal file
View file

@ -0,0 +1,75 @@
// Copyright 2019-2021, Collabora, Ltd.
// SPDX-License-Identifier: BSL-1.0
/*!
* @file
* @brief An @ref xrt_imu_sink splitter.
* @author Moses Turner <moses@collabora.com>
* @ingroup aux_util
*/
#include "util/u_sink.h"
#include "util/u_trace_marker.h"
/*!
* An @ref xrt_imu_sink splitter.
* @implements xrt_imu_sink
* @implements xrt_frame_node
*/
struct u_imu_sink_split
{
struct xrt_imu_sink base;
struct xrt_frame_node node;
struct xrt_imu_sink *downstream_one;
struct xrt_imu_sink *downstream_two;
};
static void
split_sample(struct xrt_imu_sink *xfs, struct xrt_imu_sample *sample)
{
SINK_TRACE_MARKER();
struct u_imu_sink_split *s = (struct u_imu_sink_split *)xfs;
xrt_sink_push_imu(s->downstream_one, sample);
xrt_sink_push_imu(s->downstream_two, sample);
}
static void
split_break_apart(struct xrt_frame_node *node)
{
// Noop
}
static void
split_destroy(struct xrt_frame_node *node)
{
struct u_imu_sink_split *s = container_of(node, struct u_imu_sink_split, node);
free(s);
}
/*
*
* Exported functions.
*
*/
void
u_imu_sink_split_create(struct xrt_frame_context *xfctx,
struct xrt_imu_sink *downstream_one,
struct xrt_imu_sink *downstream_two,
struct xrt_imu_sink **out_imu_sink)
{
struct u_imu_sink_split *s = U_TYPED_CALLOC(struct u_imu_sink_split);
s->base.push_imu = split_sample;
s->node.break_apart = split_break_apart;
s->node.destroy = split_destroy;
s->downstream_one = downstream_one;
s->downstream_two = downstream_two;
xrt_frame_context_add(xfctx, &s->node);
*out_imu_sink = &s->base;
}

25
src/xrt/auxiliary/util/u_sink.h
View file

@ -12,6 +12,7 @@
#include "os/os_threading.h"
#include "xrt/xrt_frame.h"
#include "xrt/xrt_tracking.h"
#ifdef __cplusplus
@ -230,6 +231,30 @@ u_sink_debug_destroy(struct u_sink_debug *usd)
}
/*!
* @public @memberof xrt_imu_sink
* @see xrt_frame_context
* Takes an IMU sample and pushes it to two sinks
*/
void
u_imu_sink_split_create(struct xrt_frame_context *xfctx,
struct xrt_imu_sink *downstream_one,
struct xrt_imu_sink *downstream_two,
struct xrt_imu_sink **out_imu_sink);
/*!
* @public @memberof xrt_imu_sink
* @see xrt_frame_context
* Takes an IMU sample and only pushes it if its timestamp has monotonically increased.
* Useful for handling hardware inconsistencies.
*/
void
u_imu_sink_force_monotonic_create(struct xrt_frame_context *xfctx,
struct xrt_imu_sink *downstream,
struct xrt_imu_sink **out_imu_sink);
#ifdef __cplusplus
}
#endif