From 56f34d23b9547e642212cdf149ab3b24fe645a71 Mon Sep 17 00:00:00 2001 From: Moses Turner <moses@collabora.com> Date: Wed, 15 Sep 2021 01:30:33 -0500 Subject: [PATCH] d/vive: Use m_relation history for HMD --- src/xrt/drivers/vive/vive_device.c | 52 +++++++++++------------------- src/xrt/drivers/vive/vive_device.h | 6 ++-- 2 files changed, 22 insertions(+), 36 deletions(-) diff --git a/src/xrt/drivers/vive/vive_device.c b/src/xrt/drivers/vive/vive_device.c index 316577eef..4c94ad148 100644 --- a/src/xrt/drivers/vive/vive_device.c +++ b/src/xrt/drivers/vive/vive_device.c @@ -55,7 +55,6 @@ vive_device_destroy(struct xrt_device *xdev) os_thread_helper_destroy(&d->mainboard_thread); // Now that the thread is not running we can destroy the lock. - os_mutex_destroy(&d->lock); m_imu_3dof_close(&d->fusion); @@ -76,6 +75,8 @@ vive_device_destroy(struct xrt_device *xdev) vive_config_teardown(&d->config); + m_relation_history_destroy(&d->relation_hist); + // Remove the variable tracking. u_var_remove_root(d); @@ -89,25 +90,6 @@ vive_device_update_inputs(struct xrt_device *xdev) VIVE_TRACE(d, "ENTER!"); } -static void -predict_pose(struct vive_device *d, uint64_t at_timestamp_ns, struct xrt_space_relation *out_relation) -{ - timepoint_ns prediction_ns = at_timestamp_ns - d->imu.ts_received_ns; - double prediction_s = time_ns_to_s(prediction_ns); - - timepoint_ns monotonic_now_ns = os_monotonic_get_ns(); - timepoint_ns remaining_ns = at_timestamp_ns - monotonic_now_ns; - VIVE_TRACE(d, "dev %s At %ldns: Pose requested for +%ldns (%ldns), predicting %ldns", d->base.str, - monotonic_now_ns, remaining_ns, at_timestamp_ns, prediction_ns); - - //! @todo integrate position here - struct xrt_space_relation relation = {0}; - relation.pose.orientation = d->rot_filtered; - relation.relation_flags = XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT; - - m_predict_relation(&relation, prediction_s, out_relation); -} - static void vive_device_get_tracked_pose(struct xrt_device *xdev, enum xrt_input_name name, @@ -127,9 +109,7 @@ vive_device_get_tracked_pose(struct xrt_device *xdev, //! @todo Use this properly. (void)at_timestamp_ns; - os_mutex_lock(&d->lock); - predict_pose(d, at_timestamp_ns, out_relation); - os_mutex_unlock(&d->lock); + m_relation_history_get(d->relation_hist, out_relation, at_timestamp_ns); } static void @@ -377,6 +357,20 @@ update_imu(struct vive_device *d, const void *buffer) m_imu_3dof_update(&d->fusion, d->imu.time_ns, &acceleration, &angular_velocity); d->rot_filtered = d->fusion.rot; + + struct xrt_space_relation rel = {0}; + rel.relation_flags = + XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT; + rel.pose.orientation = d->rot_filtered; + + // Use d->imu.ts_received_ns instead of d->imu.time_ns. + // d->imu.time_ns is offset by an arbitrary value (I think so we get more floating-point precision in + // the 3dof fusion) - so it's not what we want in the global "time-space". + + // In contrast, d->imu.ts_received_ns is just "when we got the IMU timestamp" in the normal + // os_monotonic_get_ns() "time-space". Which is what we want. So we use it. + + m_relation_history_push(d->relation_hist, &rel, d->imu.ts_received_ns); } } @@ -566,9 +560,7 @@ vive_sensors_read_one_msg(struct vive_device *d, if (!_is_report_size_valid(d, ret, report_size, report_id)) return false; - os_mutex_lock(&d->lock); process_cb(d, buffer); - os_mutex_unlock(&d->lock); } else { VIVE_ERROR(d, "Unexpected sensor report type %s (0x%x).", _sensors_get_report_string(buffer[0]), @@ -739,6 +731,8 @@ vive_device_create(struct os_hid_device *mainboard_dev, (enum u_device_alloc_flags)(U_DEVICE_ALLOC_HMD | U_DEVICE_ALLOC_TRACKING_NONE); struct vive_device *d = U_DEVICE_ALLOCATE(struct vive_device, flags, 1, 0); + m_relation_history_create(&d->relation_hist); + size_t idx = 0; d->base.hmd->blend_modes[idx++] = XRT_BLEND_MODE_OPAQUE; d->base.hmd->num_blend_modes = idx; @@ -893,14 +887,6 @@ vive_device_create(struct os_hid_device *mainboard_dev, } snprintf(d->base.serial, XRT_DEVICE_NAME_LEN, "%s", d->config.firmware.device_serial_number); - // Mutex before thread. - ret = os_mutex_init(&d->lock); - if (ret != 0) { - VIVE_ERROR(d, "Failed to init mutex!"); - vive_device_destroy(&d->base); - return NULL; - } - ret = os_thread_helper_start(&d->sensors_thread, vive_sensors_run_thread, d); if (ret != 0) { VIVE_ERROR(d, "Failed to start sensors thread!"); diff --git a/src/xrt/drivers/vive/vive_device.h b/src/xrt/drivers/vive/vive_device.h index 93700b56c..9f7f2d67e 100644 --- a/src/xrt/drivers/vive/vive_device.h +++ b/src/xrt/drivers/vive/vive_device.h @@ -14,6 +14,8 @@ #include "os/os_threading.h" #include "util/u_logging.h" #include "util/u_distortion_mesh.h" +#include "math/m_relation_history.h" + #include "vive/vive_config.h" #include "vive_lighthouse.h" @@ -38,9 +40,6 @@ struct vive_device struct os_thread_helper watchman_thread; struct os_thread_helper mainboard_thread; - //! Lock for last and fusion. - struct os_mutex lock; - struct { uint64_t time_ns; @@ -60,6 +59,7 @@ struct vive_device } board; struct xrt_quat rot_filtered; + struct m_relation_history *relation_hist; enum u_logging_level ll; bool disconnect_notified;