diff --git a/src/xrt/drivers/rift_s/rift_s_hmd.c b/src/xrt/drivers/rift_s/rift_s_hmd.c
index 3461c3fa9800ee7bf09f0e0e0c305b4165d84781..d5d936935f0cb2cfc261e9b3ac30f5b6628a0bbb 100644
--- a/src/xrt/drivers/rift_s/rift_s_hmd.c
+++ b/src/xrt/drivers/rift_s/rift_s_hmd.c
@@ -87,8 +87,17 @@ rift_s_hmd_handle_report(struct rift_s_hmd *hmd, timepoint_ns local_ts, rift_s_h
const uint32_t TICK_LEN_US = 1000000 / imu_config->imu_hz;
uint32_t dt = TICK_LEN_US;
+ int n_samples = 0;
+
+ for (int i = 0; i < 3; i++) {
+ rift_s_hmd_imu_sample_t *s = report->samples + i;
+ if (s->marker & 0x80)
+ break; /* Sample (and remaining ones) are invalid */
+ n_samples++;
+ }
+
/* Check that there's at least 1 valid sample */
- if (report->samples[0].marker & 0x80)
+ if (n_samples == 0)
return;
if (hmd->last_imu_timestamp_ns != 0) {
@@ -99,6 +108,15 @@ rift_s_hmd_handle_report(struct rift_s_hmd *hmd, timepoint_ns local_ts, rift_s_h
hmd->last_imu_timestamp32 = report->timestamp;
}
+ /* Give the tracker an update for matching local clock to device. The sample ts we're
+ * given seems to be the time the first IMU sample was captured, but the local_ts
+ * is USB packet arrival time, which is after the last IMU sample was captured,
+ * so calculate the correct imu timestamp accordingly */
+ uint64_t packet_duration_us = (n_samples - 1) * TICK_LEN_US + dt;
+ uint64_t end_imu_timestamp_ns = hmd->last_imu_timestamp_ns + (OS_NS_PER_USEC * packet_duration_us);
+
+ rift_s_tracker_clock_update(hmd->tracker, end_imu_timestamp_ns, local_ts);
+
const float gyro_scale = 1.0 / imu_config->gyro_scale;
const float accel_scale = MATH_GRAVITY_M_S2 / imu_config->accel_scale;
const float temperature_scale = 1.0 / imu_config->temperature_scale;
@@ -141,7 +159,7 @@ rift_s_hmd_handle_report(struct rift_s_hmd *hmd, timepoint_ns local_ts, rift_s_h
#endif
// Send the sample to the pose tracker
- rift_s_tracker_imu_update(hmd->tracker, hmd->last_imu_timestamp_ns, local_ts, &accel, &gyro);
+ rift_s_tracker_imu_update(hmd->tracker, hmd->last_imu_timestamp_ns, &accel, &gyro);
hmd->last_imu_timestamp_ns += (uint64_t)dt * OS_NS_PER_USEC;
hmd->last_imu_timestamp32 += dt;
diff --git a/src/xrt/drivers/rift_s/rift_s_tracker.c b/src/xrt/drivers/rift_s/rift_s_tracker.c
index 6cff08ef7eb26a2f77362bc2dc0018f138aed34e..bdad2e16034a181b60d975a84632acf23e849450 100644
--- a/src/xrt/drivers/rift_s/rift_s_tracker.c
+++ b/src/xrt/drivers/rift_s/rift_s_tracker.c
@@ -246,7 +246,9 @@ rift_s_create_hand_tracker(struct rift_s_tracker *t,
#ifdef XRT_BUILD_DRIVER_HANDTRACKING
//!@todo What's a sensible boundary for Rift S?
- struct t_camera_extra_info extra_camera_info;
+ struct t_camera_extra_info extra_camera_info = {
+ 0,
+ };
extra_camera_info.views[0].boundary_type = HT_IMAGE_BOUNDARY_NONE;
extra_camera_info.views[1].boundary_type = HT_IMAGE_BOUNDARY_NONE;
@@ -463,48 +465,41 @@ rift_s_tracker_get_hand_tracking_device(struct rift_s_tracker *t)
return t->handtracker;
}
-/*!
- * 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 t struct rift_s_tracker
- * @param local_timestamp_ns Monotonic timestamp at which the IMU sample was received
- * @param device_ts HMD Hardware timestamp, gets converted to local monotonic clock.
- */
-static timepoint_ns
-clock_hw2mono_update(struct rift_s_tracker *t, timepoint_ns local_timestamp_ns, uint64_t device_ts)
+//! 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)
{
- const double alpha = 0.9995; // Weight to put on accumulated hw2mono clock offset
- timepoint_ns hw = device_ts;
-
- /* Only do updates if the monotonic time increased
- * (otherwise we're processing packets that arrived
- * at the same time - so only take the earliest) */
- if (local_timestamp_ns > t->last_hw2mono_local_ts) {
- time_duration_ns old_hw2mono = t->hw2mono;
- time_duration_ns got_hw2mono = local_timestamp_ns - 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;
- }
+ // 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;
+}
- time_duration_ns new_hw2mono_out = hw + new_hw2mono;
+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);
- if (new_hw2mono_out >= t->last_hw2mono_out) {
- t->last_hw2mono_out = new_hw2mono_out;
- t->hw2mono = new_hw2mono;
+ if (!t->have_hw2mono) {
+ time_duration_ns change_ns = last_hw2mono - t->hw2mono;
+ if (change_ns >= -U_TIME_HALF_MS_IN_NS && change_ns <= U_TIME_HALF_MS_IN_NS) {
+ RIFT_S_INFO("HMD device to local clock map stabilised");
t->have_hw2mono = true;
- t->last_hw2mono_local_ts = local_timestamp_ns;
- } else {
- RIFT_S_WARN("Monotonic time map went backward (%" PRIu64 ", %" PRIu64 ") => %" PRIu64
- " < %" PRIu64 ". Reporting %" PRIu64,
- hw, local_timestamp_ns, new_hw2mono_out, t->last_hw2mono_out, hw + t->hw2mono);
}
- } else {
- t->last_hw2mono_out = hw + t->hw2mono;
}
-
- return t->last_hw2mono_out;
+ os_mutex_unlock(&t->mutex);
}
//! Camera specific logic for clock conversion
@@ -516,36 +511,37 @@ clock_hw2mono_get(struct rift_s_tracker *t, uint64_t device_ts, timepoint_ns *ou
void
rift_s_tracker_imu_update(struct rift_s_tracker *t,
- uint64_t timestamp_ns,
- timepoint_ns local_timestamp_ns_orig,
+ uint64_t device_timestamp_ns,
const struct xrt_vec3 *accel,
const struct xrt_vec3 *gyro)
{
os_mutex_lock(&t->mutex);
- /* Ignore packets before we're ready */
- if (!t->ready_for_data) {
+ /* Ignore packets before we're ready and clock is stable */
+ if (!t->ready_for_data || !t->have_hw2mono) {
os_mutex_unlock(&t->mutex);
return;
}
/* Get the smoothed monotonic time estimate for this IMU sample */
- timepoint_ns local_timestamp_ns = clock_hw2mono_update(t, local_timestamp_ns_orig, timestamp_ns);
+ timepoint_ns local_timestamp_ns;
+
+ clock_hw2mono_get(t, device_timestamp_ns, &local_timestamp_ns);
if (t->fusion.last_imu_local_timestamp_ns != 0 && local_timestamp_ns < t->fusion.last_imu_local_timestamp_ns) {
RIFT_S_WARN("IMU time went backward by %" PRId64 " ns",
local_timestamp_ns - t->fusion.last_imu_local_timestamp_ns);
} else {
- m_imu_3dof_update(&t->fusion.i3dof, timestamp_ns, accel, gyro);
+ m_imu_3dof_update(&t->fusion.i3dof, local_timestamp_ns, accel, gyro);
}
- RIFT_S_TRACE("IMU timestamp %" PRIu64 " (dt %f) local %" PRIu64 " hw2mono %" PRIu64 " (dt %f) offset %" PRId64,
- timestamp_ns, (double)(timestamp_ns - t->fusion.last_imu_timestamp_ns) / 1000000000.0,
- local_timestamp_ns_orig, local_timestamp_ns,
+ RIFT_S_TRACE("IMU timestamp %" PRIu64 " (dt %f) hw2mono local ts %" PRIu64 " (dt %f) offset %" PRId64,
+ device_timestamp_ns,
+ (double)(device_timestamp_ns - t->fusion.last_imu_timestamp_ns) / 1000000000.0, local_timestamp_ns,
(double)(local_timestamp_ns - t->fusion.last_imu_local_timestamp_ns) / 1000000000.0, t->hw2mono);
t->fusion.last_angular_velocity = *gyro;
- t->fusion.last_imu_timestamp_ns = timestamp_ns;
+ t->fusion.last_imu_timestamp_ns = device_timestamp_ns;
t->fusion.last_imu_local_timestamp_ns = local_timestamp_ns;
t->pose.orientation = t->fusion.i3dof.rot;
diff --git a/src/xrt/drivers/rift_s/rift_s_tracker.h b/src/xrt/drivers/rift_s/rift_s_tracker.h
index 52af77689d38a376fb81da54e6dc671ab5d5934b..068aa7eac899c4243e727076bb1b0a269895da30 100644
--- a/src/xrt/drivers/rift_s/rift_s_tracker.h
+++ b/src/xrt/drivers/rift_s/rift_s_tracker.h
@@ -86,8 +86,6 @@ struct rift_s_tracker
//!< Estimated offset from HMD device timestamp to local monotonic clock
bool have_hw2mono;
time_duration_ns hw2mono;
- time_duration_ns last_hw2mono_out;
- timepoint_ns last_hw2mono_local_ts;
timepoint_ns last_frame_time;
//! Adjustment to apply to camera timestamps to bring them into the
@@ -138,10 +136,12 @@ rift_s_tracker_get_slam_sinks(struct rift_s_tracker *t);
struct xrt_device *
rift_s_tracker_get_hand_tracking_device(struct rift_s_tracker *t);
+void
+rift_s_tracker_clock_update(struct rift_s_tracker *t, uint64_t device_timestamp_ns, timepoint_ns local_timestamp_ns);
+
void
rift_s_tracker_imu_update(struct rift_s_tracker *t,
- uint64_t timestamp_ns,
- timepoint_ns local_timestamp_ns,
+ uint64_t device_timestamp_ns,
const struct xrt_vec3 *accel,
const struct xrt_vec3 *gyro);