diff --git a/src/xrt/drivers/vive/vive_device.c b/src/xrt/drivers/vive/vive_device.c index d9030be7f..092347fbe 100644 --- a/src/xrt/drivers/vive/vive_device.c +++ b/src/xrt/drivers/vive/vive_device.c @@ -432,7 +432,10 @@ update_imu(struct vive_device *d, const void *buffer) m_relation_history_push(d->fusion.relation_hist, &rel, now_ns); os_mutex_unlock(&d->fusion.mutex); - vive_source_push_imu_packet(d->source, d->imu.last_sample_ts_ns, acceleration, angular_velocity); + assert(j > 0); + uint32_t age = j <= 0 ? 0 : (uint32_t)(j - 1); + + vive_source_push_imu_packet(d->source, age, d->imu.last_sample_ts_ns, acceleration, angular_velocity); } } diff --git a/src/xrt/drivers/vive/vive_source.c b/src/xrt/drivers/vive/vive_source.c index dc5acbd58..2633a38ea 100644 --- a/src/xrt/drivers/vive/vive_source.c +++ b/src/xrt/drivers/vive/vive_source.c @@ -1,9 +1,10 @@ -// Copyright 2022, Collabora, Ltd. +// Copyright 2022-2023, Collabora, Ltd. // SPDX-License-Identifier: BSL-1.0 /*! * @file * @brief Interface for vive data sources * @author Mateo de Mayo + * @author Jakob Bornecrantz * @ingroup drv_vive */ @@ -16,6 +17,7 @@ #include "util/u_deque.h" #include "util/u_logging.h" +#include "util/u_trace_marker.h" #include "vive.h" @@ -143,7 +145,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 = 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; @@ -206,12 +208,69 @@ vive_source_create(struct xrt_frame_context *xfctx) } void -vive_source_push_imu_packet(struct vive_source *vs, timepoint_ns t, struct xrt_vec3 a, struct xrt_vec3 g) +vive_source_push_imu_packet(struct vive_source *vs, uint32_t age, timepoint_ns t, struct xrt_vec3 a, struct xrt_vec3 g) { - struct xrt_vec3_f64 a64 = {a.x, a.y, a.z}; - struct xrt_vec3_f64 g64 = {g.x, g.y, g.z}; - struct xrt_imu_sample sample = {.timestamp_ns = t, .accel_m_s2 = a64, .gyro_rad_secs = g64}; + /* + * We want the samples to be on sometime in the past, not future. This + * is due to USB latency, which we don't know, so we are guessing here. + * We also don't know if the timestamp given for the start of the sample + * or the end. + * + * We picked 2 here because that's about what the best gaming mice can + * do, it also seems to feel good with what seems to be reasonable + * present to display offset in the compositor. + * + * We also adjust for the "age" of a sample, the vive sends out 3 + * samples per packet, most often only one is a new sample. But + * sometimes we get up to 3 new samples in one packet. So if age is + * greater then 0, adjust with that many MS (1000Hz sampler rate). + */ + + // 2 ms in value. + const timepoint_ns t2ms_ns = U_TIME_1MS_IN_NS * 2; + + // Extra in the past for age. + timepoint_ns age_diff_ns = age * U_TIME_1MS_IN_NS; + + // Now. + timepoint_ns now_ns = (timepoint_ns)os_monotonic_get_ns(); + + // Calculated sample point. + timepoint_ns sample_point = now_ns - t2ms_ns - age_diff_ns; + + // Time adjustment. + t = m_clock_offset_a2b(IMU_FREQUENCY, t, sample_point, &vs->hw2mono); + + // Finished sample. + struct xrt_imu_sample sample = { + .timestamp_ns = t, + .accel_m_s2 = (struct xrt_vec3_f64){a.x, a.y, a.z}, + .gyro_rad_secs = (struct xrt_vec3_f64){g.x, g.y, g.z}, + }; + + // Push it out! xrt_sink_push_imu(&vs->imu_sink, &sample); + + // Only do this if we are really debugging stuff. +#ifdef XRT_FEATURE_TRACING + timepoint_ns diff_ns = t - (now_ns - age_diff_ns); + static timepoint_ns last_ns = 0; + if (last_ns == 0) { + last_ns = t; + } + + double now_diff_ms = time_ns_to_ms_f(diff_ns); + double last_diff_ms = time_ns_to_ms_f(t - last_ns); + last_ns = t; + +#ifdef U_TRACE_TRACY + TracyCPlot("Vive IMU to now(ms)", now_diff_ms); + TracyCPlot("Vive IMU to last(ms)", last_diff_ms); + TracyCPlot("Vive IMU age", age); +#endif + + VIVE_TRACE(vs, "Sample diffs, now: %+.4fms, last: %+.4f, age: %u", now_diff_ms, last_diff_ms, age); +#endif } void diff --git a/src/xrt/drivers/vive/vive_source.h b/src/xrt/drivers/vive/vive_source.h index 1b0158552..6b58794e8 100644 --- a/src/xrt/drivers/vive/vive_source.h +++ b/src/xrt/drivers/vive/vive_source.h @@ -1,4 +1,4 @@ -// Copyright 2022, Collabora, Ltd. +// Copyright 2022-2023, Collabora, Ltd. // SPDX-License-Identifier: BSL-1.0 /*! * @file @@ -28,7 +28,7 @@ struct vive_source * vive_source_create(struct xrt_frame_context *xfctx); void -vive_source_push_imu_packet(struct vive_source *vs, timepoint_ns t, struct xrt_vec3 a, struct xrt_vec3 g); +vive_source_push_imu_packet(struct vive_source *vs, uint32_t age, timepoint_ns t, struct xrt_vec3 a, struct xrt_vec3 g); void vive_source_push_frame_ticks(struct vive_source *vs, timepoint_ns ticks);