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:
Mateo de Mayo 2022-11-29 15:58:15 -03:00 committed by Jakob Bornecrantz
parent 35061c1d80
commit dad7957fb2
5 changed files with 68 additions and 80 deletions

View file

@ -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

View 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

View file

@ -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;

View file

@ -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;

View file

@ -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);