mirror of
https://gitlab.freedesktop.org/monado/monado.git
synced 2025-02-15 02:00:22 +00:00
a/math: Add clock_offset.h
For now it only has our simple exponential smoothing clock offset estimator. But more advanced ones can go here too.
This commit is contained in:
parent
35061c1d80
commit
dad7957fb2
src/xrt
auxiliary/math
drivers
|
@ -5,6 +5,7 @@ add_library(
|
|||
aux_math STATIC
|
||||
m_api.h
|
||||
m_base.cpp
|
||||
m_clock_offset.h
|
||||
m_eigen_interop.hpp
|
||||
m_filter_fifo.c
|
||||
m_filter_fifo.h
|
||||
|
|
50
src/xrt/auxiliary/math/m_clock_offset.h
Normal file
50
src/xrt/auxiliary/math/m_clock_offset.h
Normal file
|
@ -0,0 +1,50 @@
|
|||
// Copyright 2022, Collabora, Ltd.
|
||||
// SPDX-License-Identifier: BSL-1.0
|
||||
/*!
|
||||
* @file
|
||||
* @brief Helpers to estimate offsets between clocks
|
||||
* @author Mateo de Mayo <mateo.demayo@collabora.com>
|
||||
* @ingroup aux_math
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "util/u_time.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/*!
|
||||
* Helper to estimate the offset between two clocks using exponential smoothing.
|
||||
*
|
||||
* Given a sample from two timestamp domains A and B that should have been
|
||||
* sampled as close as possible, together with an estimate of the offset between
|
||||
* A clock and B clock (or zero), it applies a smoothing average on the
|
||||
* estimated offset and returns @p a in B clock.
|
||||
*
|
||||
* @param freq About how many times per second this function is called. Helps setting a good decay value.
|
||||
* @param a Timestamp in clock A of the event
|
||||
* @param b Timestamp in clock B of the event
|
||||
* @param[in,out] inout_a2b Pointer to the current offset estimate from A to B, or 0 if unknown.
|
||||
* Value pointed-to will be updated.
|
||||
* @return timepoint_ns @p a in B clock
|
||||
*/
|
||||
static inline timepoint_ns
|
||||
m_clock_offset_a2b(float freq, timepoint_ns a, timepoint_ns b, time_duration_ns *inout_a2b)
|
||||
{
|
||||
// Totally arbitrary way of computing alpha, if you have a better one, replace it
|
||||
const float alpha = 1.0 - 12.5 / freq; // Weight to put on accumulated a2b
|
||||
time_duration_ns old_a2b = *inout_a2b;
|
||||
time_duration_ns got_a2b = b - a;
|
||||
time_duration_ns new_a2b = old_a2b * alpha + got_a2b * (1.0 - alpha);
|
||||
if (old_a2b == 0) { // a2b has not been set yet
|
||||
new_a2b = got_a2b;
|
||||
}
|
||||
*inout_a2b = new_a2b;
|
||||
return a + new_a2b;
|
||||
}
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
|
@ -23,6 +23,7 @@
|
|||
#include <inttypes.h>
|
||||
|
||||
#include "math/m_api.h"
|
||||
#include "math/m_clock_offset.h"
|
||||
#include "math/m_space.h"
|
||||
#include "math/m_vec3.h"
|
||||
|
||||
|
@ -465,32 +466,12 @@ rift_s_tracker_get_hand_tracking_device(struct rift_s_tracker *t)
|
|||
return t->handtracker;
|
||||
}
|
||||
|
||||
//! Given a sample from two timestamp domains a and b that should have been
|
||||
//! sampled as close as possible, together with an estimate of the offset
|
||||
//! between a clock and b clock (or zero), it applies a smoothing average on the
|
||||
//! estimated offset and returns a in b clock.
|
||||
//! @todo Copy of clock_hw2mono in wmr_source.c and vive_source.c, unify into a utility.
|
||||
static inline timepoint_ns
|
||||
clock_offset_a2b(double freq, timepoint_ns a, timepoint_ns b, time_duration_ns *inout_a2b)
|
||||
{
|
||||
// Totally arbitrary way of computing alpha, if you have a better one, replace it
|
||||
const double alpha = 1.0 - 12.5 / freq; // Weight to put on accumulated a2b
|
||||
time_duration_ns old_a2b = *inout_a2b;
|
||||
time_duration_ns got_a2b = b - a;
|
||||
time_duration_ns new_a2b = old_a2b * alpha + got_a2b * (1.0 - alpha);
|
||||
if (old_a2b == 0) { // a2b has not been set yet
|
||||
new_a2b = got_a2b;
|
||||
}
|
||||
*inout_a2b = new_a2b;
|
||||
return a + new_a2b;
|
||||
}
|
||||
|
||||
void
|
||||
rift_s_tracker_clock_update(struct rift_s_tracker *t, uint64_t device_timestamp_ns, timepoint_ns local_timestamp_ns)
|
||||
{
|
||||
os_mutex_lock(&t->mutex);
|
||||
time_duration_ns last_hw2mono = t->hw2mono;
|
||||
clock_offset_a2b(25000, device_timestamp_ns, local_timestamp_ns, &t->hw2mono);
|
||||
m_clock_offset_a2b(25000, device_timestamp_ns, local_timestamp_ns, &t->hw2mono);
|
||||
|
||||
if (!t->have_hw2mono) {
|
||||
time_duration_ns change_ns = last_hw2mono - t->hw2mono;
|
||||
|
|
|
@ -7,6 +7,7 @@
|
|||
* @ingroup drv_vive
|
||||
*/
|
||||
|
||||
#include "math/m_clock_offset.h"
|
||||
#include "os/os_threading.h"
|
||||
#include "util/u_deque.h"
|
||||
#include "util/u_logging.h"
|
||||
|
@ -46,26 +47,6 @@ struct vive_source
|
|||
time_duration_ns hw2v4l2; //!< Estimated offset from IMU to V4L2 clock
|
||||
};
|
||||
|
||||
//! Given a sample from two timestamp domains a and b that should have been
|
||||
//! sampled as close as possible, together with an estimate of the offset
|
||||
//! between a clock and b clock (or zero), it applies a smoothing average on the
|
||||
//! estimated offset and returns a in b clock.
|
||||
//! @todo Copy of clock_hw2mono in wmr_source.c, unify into a utility.
|
||||
static inline timepoint_ns
|
||||
clock_offset_a2b(double freq, timepoint_ns a, timepoint_ns b, time_duration_ns *inout_a2b)
|
||||
{
|
||||
// Totally arbitrary way of computing alpha, if you have a better one, replace it
|
||||
const double alpha = 1.0 - 12.5 / freq; // Weight to put on accumulated a2b
|
||||
time_duration_ns old_a2b = *inout_a2b;
|
||||
time_duration_ns got_a2b = b - a;
|
||||
time_duration_ns new_a2b = old_a2b * alpha + got_a2b * (1.0 - alpha);
|
||||
if (old_a2b == 0) { // a2b has not been set yet
|
||||
new_a2b = got_a2b;
|
||||
}
|
||||
*inout_a2b = new_a2b;
|
||||
return a + new_a2b;
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
* Vive source methods
|
||||
|
@ -126,7 +107,7 @@ vive_source_try_convert_v4l2_timestamp(struct vive_source *vs, struct xrt_frame
|
|||
vs->waiting_for_first_nonempty_frame = false;
|
||||
|
||||
// Update estimate of hw2v4l2 clock offset, only used for matching timestamps
|
||||
clock_offset_a2b(CAMERA_FREQUENCY, vive_timestamp, xf->timestamp, &vs->hw2v4l2);
|
||||
m_clock_offset_a2b(CAMERA_FREQUENCY, vive_timestamp, xf->timestamp, &vs->hw2v4l2);
|
||||
|
||||
// Use vive_timestamp and put it in monotonic clock
|
||||
xf->timestamp = vive_timestamp + vs->hw2mono; // Notice that we don't use hw2v4l2
|
||||
|
@ -156,7 +137,7 @@ static void
|
|||
vive_source_receive_imu_sample(struct xrt_imu_sink *sink, struct xrt_imu_sample *s)
|
||||
{
|
||||
struct vive_source *vs = container_of(sink, struct vive_source, imu_sink);
|
||||
s->timestamp_ns = clock_offset_a2b(IMU_FREQUENCY, s->timestamp_ns, os_monotonic_get_ns(), &vs->hw2mono);
|
||||
s->timestamp_ns = m_clock_offset_a2b(IMU_FREQUENCY, s->timestamp_ns, os_monotonic_get_ns(), &vs->hw2mono);
|
||||
timepoint_ns ts = s->timestamp_ns;
|
||||
struct xrt_vec3_f64 a = s->accel_m_s2;
|
||||
struct xrt_vec3_f64 w = s->gyro_rad_secs;
|
||||
|
|
|
@ -12,6 +12,7 @@
|
|||
#include "wmr_protocol.h"
|
||||
|
||||
#include "math/m_api.h"
|
||||
#include "math/m_clock_offset.h"
|
||||
#include "math/m_filter_fifo.h"
|
||||
#include "util/u_debug.h"
|
||||
#include "util/u_sink.h"
|
||||
|
@ -84,44 +85,12 @@ struct wmr_source
|
|||
*
|
||||
*/
|
||||
|
||||
/*!
|
||||
* Convert a hardware timestamp into monotonic clock. Updates offset estimate.
|
||||
* @note Only used with IMU samples as they have the smallest USB transmission time.
|
||||
*
|
||||
* @param ws wmr_source
|
||||
* @param[in, out] ts Hardware timestamp, gets converted to monotonic clock.
|
||||
*/
|
||||
static inline void
|
||||
clock_hw2mono(struct wmr_source *ws, timepoint_ns *ts)
|
||||
{
|
||||
const double alpha = 0.95; // Weight to put on accumulated hw2mono clock offset
|
||||
timepoint_ns mono = os_monotonic_get_ns();
|
||||
timepoint_ns hw = *ts;
|
||||
time_duration_ns old_hw2mono = ws->hw2mono;
|
||||
time_duration_ns got_hw2mono = mono - hw;
|
||||
time_duration_ns new_hw2mono = old_hw2mono * alpha + got_hw2mono * (1.0 - alpha);
|
||||
if (old_hw2mono == 0) { // hw2mono was not set for the first time yet
|
||||
new_hw2mono = got_hw2mono;
|
||||
}
|
||||
ws->hw2mono = new_hw2mono;
|
||||
*ts = hw + new_hw2mono;
|
||||
}
|
||||
|
||||
//! Camera specific logic for clock conversion
|
||||
static inline void
|
||||
clock_cam_hw2mono(struct wmr_source *ws, struct xrt_frame *xf, bool is_left)
|
||||
{
|
||||
if (is_left) {
|
||||
ws->cam_hw2mono = ws->hw2mono; // Cache last hw2mono used for right frame
|
||||
}
|
||||
xf->timestamp += ws->cam_hw2mono;
|
||||
}
|
||||
|
||||
static void
|
||||
receive_left_frame(struct xrt_frame_sink *sink, struct xrt_frame *xf)
|
||||
{
|
||||
struct wmr_source *ws = container_of(sink, struct wmr_source, left_sink);
|
||||
clock_cam_hw2mono(ws, xf, true);
|
||||
ws->cam_hw2mono = ws->hw2mono; // We want the right frame to use the same offset
|
||||
xf->timestamp += ws->cam_hw2mono;
|
||||
WMR_TRACE(ws, "left img t=%ld source_t=%ld", xf->timestamp, xf->source_timestamp);
|
||||
u_sink_debug_push_frame(&ws->ui_left_sink, xf);
|
||||
if (ws->out_sinks.left && ws->first_imu_received) {
|
||||
|
@ -133,7 +102,7 @@ static void
|
|||
receive_right_frame(struct xrt_frame_sink *sink, struct xrt_frame *xf)
|
||||
{
|
||||
struct wmr_source *ws = container_of(sink, struct wmr_source, right_sink);
|
||||
clock_cam_hw2mono(ws, xf, false);
|
||||
xf->timestamp += ws->cam_hw2mono;
|
||||
WMR_TRACE(ws, "right img t=%ld source_t=%ld", xf->timestamp, xf->source_timestamp);
|
||||
u_sink_debug_push_frame(&ws->ui_right_sink, xf);
|
||||
if (ws->out_sinks.right && ws->first_imu_received) {
|
||||
|
@ -145,8 +114,14 @@ static void
|
|||
receive_imu_sample(struct xrt_imu_sink *sink, struct xrt_imu_sample *s)
|
||||
{
|
||||
struct wmr_source *ws = container_of(sink, struct wmr_source, imu_sink);
|
||||
clock_hw2mono(ws, &s->timestamp_ns);
|
||||
timepoint_ns ts = s->timestamp_ns;
|
||||
|
||||
// Convert hardware timestamp into monotonic clock. Update offset estimate hw2mono.
|
||||
// Note this is only done with IMU samples as they have the smallest USB transmission time.
|
||||
const double IMU_FREQ = 250; //!< @todo use 1000 if "average_imus" is false
|
||||
timepoint_ns now_hw = s->timestamp_ns;
|
||||
timepoint_ns now_mono = (timepoint_ns)os_monotonic_get_ns();
|
||||
timepoint_ns ts = m_clock_offset_a2b(IMU_FREQ, now_hw, now_mono, &ws->hw2mono);
|
||||
|
||||
struct xrt_vec3_f64 a = s->accel_m_s2;
|
||||
struct xrt_vec3_f64 w = s->gyro_rad_secs;
|
||||
WMR_TRACE(ws, "imu t=%ld a=(%f %f %f) w=(%f %f %f)", ts, a.x, a.y, a.z, w.x, w.y, w.z);
|
||||
|
|
Loading…
Reference in a new issue