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

d/vive: Add controller rotation pose prediction

Browse source
This commit is contained in:
Christoph Haag 2021-02-17 19:58:24 +01:00
parent ab70115a89
commit 4d1b5f033a
2 changed files with 24 additions and 6 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

29
src/xrt/drivers/vive/vive_controller.c
View file

@ -22,6 +22,7 @@
#include "xrt/xrt_prober.h"
#include "math/m_api.h"
#include "math/m_predict.h"
#include "util/u_debug.h"
#include "util/u_device.h"
#include "util/u_json.h"
@ -350,6 +351,25 @@ vive_controller_get_hand_tracking(struct xrt_device *xdev,
u_hand_joints_set_out_data(&d->hand_tracking, hand, &controller_relation, &hand_on_handle_pose, out_value);
}
static void
predict_pose(struct vive_controller_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_controller_device_get_tracked_pose(struct xrt_device *xdev,
enum xrt_input_name name,
@ -376,12 +396,7 @@ vive_controller_device_get_tracked_pose(struct xrt_device *xdev,
return;
}
out_relation->pose.orientation = d->rot_filtered;
//! @todo assuming that orientation is actually currently tracked.
out_relation->relation_flags = (enum xrt_space_relation_flags)(
XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT |
XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT);
predict_pose(d, at_timestamp_ns, out_relation);
os_thread_helper_unlock(&d->controller_thread);
@ -531,6 +546,8 @@ vive_controller_handle_imu_sample(struct vive_controller_device *d, struct watch
uint32_t dt_raw = calc_dt_raw_and_handle_overflow(d, time_raw);
uint64_t dt_ns = cald_dt_ns(dt_raw);
d->imu.ts_received_ns = os_monotonic_get_ns();
int16_t acc[3] = {
__le16_to_cpu(sample->acc[0]),
__le16_to_cpu(sample->acc[1]),

1
src/xrt/drivers/vive/vive_controller.h
View file

@ -54,6 +54,7 @@ struct vive_controller_device
{
uint64_t time_ns;
uint32_t last_sample_time_raw;
timepoint_ns ts_received_ns;
} imu;
struct m_imu_3dof fusion;