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

d/vive: Use m_relation history for HMD

Browse source
This commit is contained in:
Moses Turner 2021-09-15 01:30:33 -05:00
parent 7f88583d37
commit 56f34d23b9
2 changed files with 22 additions and 36 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

52
src/xrt/drivers/vive/vive_device.c
View file

@ -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!");

6
src/xrt/drivers/vive/vive_device.h
View file

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