mirrors/monado
1
0
Fork 0
mirror of https://gitlab.freedesktop.org/monado/monado.git synced 2024-12-29 11:06:18 +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;
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;

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

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