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

d/rift_s: Rework hw2mono clock tracking

Browse source 
Use the common hw2mono/a2b clock code from vive_source
This commit is contained in:
Jan Schmidt 2022-09-25 00:35:18 +10:00
parent 942df72c26
commit 3b7f95b39e
3 changed files with 70 additions and 56 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

22
src/xrt/drivers/rift_s/rift_s_hmd.c
View file

@ -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; const uint32_t TICK_LEN_US = 1000000 / imu_config->imu_hz;
uint32_t dt = TICK_LEN_US; 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 */ /* Check that there's at least 1 valid sample */
if (report->samples[0].marker & 0x80) if (n_samples == 0)
return; return;
if (hmd->last_imu_timestamp_ns != 0) { 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; 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 gyro_scale = 1.0 / imu_config->gyro_scale;
const float accel_scale = MATH_GRAVITY_M_S2 / imu_config->accel_scale; const float accel_scale = MATH_GRAVITY_M_S2 / imu_config->accel_scale;
const float temperature_scale = 1.0 / imu_config->temperature_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 #endif
// Send the sample to the pose tracker // 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_timestamp_ns += (uint64_t)dt * OS_NS_PER_USEC;
hmd->last_imu_timestamp32 += dt; hmd->last_imu_timestamp32 += dt;

88
src/xrt/drivers/rift_s/rift_s_tracker.c
View file

@ -246,7 +246,9 @@ rift_s_create_hand_tracker(struct rift_s_tracker *t,
#ifdef XRT_BUILD_DRIVER_HANDTRACKING #ifdef XRT_BUILD_DRIVER_HANDTRACKING
//!@todo What's a sensible boundary for Rift S? //!@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[0].boundary_type = HT_IMAGE_BOUNDARY_NONE;
extra_camera_info.views[1].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; return t->handtracker;
} }
/*! //! Given a sample from two timestamp domains a and b that should have been
* Convert a hardware timestamp into monotonic clock. Updates offset estimate. //! sampled as close as possible, together with an estimate of the offset
* @note Only used with IMU samples as they have the smallest USB transmission time. //! between a clock and b clock (or zero), it applies a smoothing average on the
* //! estimated offset and returns a in b clock.
* @param t struct rift_s_tracker //! @todo Copy of clock_hw2mono in wmr_source.c and vive_source.c, unify into a utility.
* @param local_timestamp_ns Monotonic timestamp at which the IMU sample was received static inline timepoint_ns
* @param device_ts HMD Hardware timestamp, gets converted to local monotonic clock. clock_offset_a2b(double freq, timepoint_ns a, timepoint_ns b, time_duration_ns *inout_a2b)
*/
static timepoint_ns
clock_hw2mono_update(struct rift_s_tracker *t, timepoint_ns local_timestamp_ns, uint64_t device_ts)
{ {
const double alpha = 0.9995; // Weight to put on accumulated hw2mono clock offset // Totally arbitrary way of computing alpha, if you have a better one, replace it
timepoint_ns hw = device_ts; const double alpha = 1.0 - 12.5 / freq; // Weight to put on accumulated a2b
time_duration_ns old_a2b = *inout_a2b;
/* Only do updates if the monotonic time increased time_duration_ns got_a2b = b - a;
* (otherwise we're processing packets that arrived time_duration_ns new_a2b = old_a2b * alpha + got_a2b * (1.0 - alpha);
* at the same time - so only take the earliest) */ if (old_a2b == 0) { // a2b has not been set yet
if (local_timestamp_ns > t->last_hw2mono_local_ts) { new_a2b = got_a2b;
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;
} }
*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) { if (!t->have_hw2mono) {
t->last_hw2mono_out = new_hw2mono_out; time_duration_ns change_ns = last_hw2mono - t->hw2mono;
t->hw2mono = new_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->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;
} }
os_mutex_unlock(&t->mutex);
return t->last_hw2mono_out;
} }
//! Camera specific logic for clock conversion //! 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 void
rift_s_tracker_imu_update(struct rift_s_tracker *t, rift_s_tracker_imu_update(struct rift_s_tracker *t,
uint64_t timestamp_ns, uint64_t device_timestamp_ns,
timepoint_ns local_timestamp_ns_orig,
const struct xrt_vec3 *accel, const struct xrt_vec3 *accel,
const struct xrt_vec3 *gyro) const struct xrt_vec3 *gyro)
{ {
os_mutex_lock(&t->mutex); os_mutex_lock(&t->mutex);
/* Ignore packets before we're ready */ /* Ignore packets before we're ready and clock is stable */
if (!t->ready_for_data) { if (!t->ready_for_data || !t->have_hw2mono) {
os_mutex_unlock(&t->mutex); os_mutex_unlock(&t->mutex);
return; return;
} }
/* Get the smoothed monotonic time estimate for this IMU sample */ /* 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) { 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", RIFT_S_WARN("IMU time went backward by %" PRId64 " ns",
local_timestamp_ns - t->fusion.last_imu_local_timestamp_ns); local_timestamp_ns - t->fusion.last_imu_local_timestamp_ns);
} else { } 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, RIFT_S_TRACE("IMU timestamp %" PRIu64 " (dt %f) hw2mono local ts %" PRIu64 " (dt %f) offset %" PRId64,
timestamp_ns, (double)(timestamp_ns - t->fusion.last_imu_timestamp_ns) / 1000000000.0, device_timestamp_ns,
local_timestamp_ns_orig, local_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); (double)(local_timestamp_ns - t->fusion.last_imu_local_timestamp_ns) / 1000000000.0, t->hw2mono);
t->fusion.last_angular_velocity = *gyro; 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->fusion.last_imu_local_timestamp_ns = local_timestamp_ns;
t->pose.orientation = t->fusion.i3dof.rot; t->pose.orientation = t->fusion.i3dof.rot;

8
src/xrt/drivers/rift_s/rift_s_tracker.h
View file

@ -86,8 +86,6 @@ struct rift_s_tracker
//!< Estimated offset from HMD device timestamp to local monotonic clock //!< Estimated offset from HMD device timestamp to local monotonic clock
bool have_hw2mono; bool have_hw2mono;
time_duration_ns hw2mono; time_duration_ns hw2mono;
time_duration_ns last_hw2mono_out;
timepoint_ns last_hw2mono_local_ts;
timepoint_ns last_frame_time; timepoint_ns last_frame_time;
//! Adjustment to apply to camera timestamps to bring them into the //! 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 * struct xrt_device *
rift_s_tracker_get_hand_tracking_device(struct rift_s_tracker *t); 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 void
rift_s_tracker_imu_update(struct rift_s_tracker *t, rift_s_tracker_imu_update(struct rift_s_tracker *t,
uint64_t timestamp_ns, uint64_t device_timestamp_ns,
timepoint_ns local_timestamp_ns,
const struct xrt_vec3 *accel, const struct xrt_vec3 *accel,
const struct xrt_vec3 *gyro); const struct xrt_vec3 *gyro);