diff --git a/src/xrt/auxiliary/CMakeLists.txt b/src/xrt/auxiliary/CMakeLists.txt index 5beab1b47..c3a36f316 100644 --- a/src/xrt/auxiliary/CMakeLists.txt +++ b/src/xrt/auxiliary/CMakeLists.txt @@ -25,6 +25,7 @@ set(TRACKING_SOURCE_FILES tracking/t_debug_hsv_viewer.cpp tracking/t_hsv_filter.c tracking/t_tracker_psmv.cpp + tracking/t_tracker_psvr.cpp tracking/t_tracking.h ) diff --git a/src/xrt/auxiliary/tracking/t_tracker_psvr.cpp b/src/xrt/auxiliary/tracking/t_tracker_psvr.cpp new file mode 100644 index 000000000..b8f80c1a4 --- /dev/null +++ b/src/xrt/auxiliary/tracking/t_tracker_psvr.cpp @@ -0,0 +1,309 @@ +// Copyright 2019, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief PSVR tracker code. + * @author Jakob Bornecrantz + */ + +#include "xrt/xrt_tracking.h" + +#include "tracking/t_tracking.h" + +#include "util/u_misc.h" +#include "util/u_debug.h" +#include "util/u_frame.h" +#include "util/u_format.h" + +#include "math/m_api.h" + +#include "os/os_threading.h" + +#include +#include +#include + + +class TrackerPSVR +{ +public: + struct xrt_tracked_psvr base = {}; + struct xrt_frame_sink sink = {}; + struct xrt_frame_node node = {}; + + //! Frame waiting to be processed. + struct xrt_frame *frame; + + //! Thread and lock helper. + struct os_thread_helper oth; + + //! Have we received a new IMU sample. + bool has_imu = false; + + struct + { + struct xrt_vec3 pos = {}; + struct xrt_quat rot = {}; + } fusion; +}; + +static void +procces(TrackerPSVR &t, struct xrt_frame *xf) +{ + // Only IMU data + if (xf == NULL) { + return; + } + + xrt_frame_reference(&xf, NULL); +} + +static void +run(TrackerPSVR &t) +{ + struct xrt_frame *frame = NULL; + + os_thread_helper_lock(&t.oth); + + while (os_thread_helper_is_running_locked(&t.oth)) { + // No data + if (!t.has_imu || t.frame == NULL) { + os_thread_helper_wait_locked(&t.oth); + } + + if (!os_thread_helper_is_running_locked(&t.oth)) { + break; + } + + // Take a reference on the current frame, this keeps it alive + // if it is replaced during the consumer processing it, but + // we no longer need to hold onto the frame on the queue we + // just move the pointer. + frame = t.frame; + t.frame = NULL; + + // Unlock the mutex when we do the work. + os_thread_helper_unlock(&t.oth); + + procces(t, frame); + + // Have to lock it again. + os_thread_helper_lock(&t.oth); + } + + os_thread_helper_unlock(&t.oth); +} + +static void +get_pose(TrackerPSVR &t, + struct time_state *timestate, + timepoint_ns when_ns, + struct xrt_space_relation *out_relation) +{ + os_thread_helper_lock(&t.oth); + + // Don't do anything if we have stopped. + if (!os_thread_helper_is_running_locked(&t.oth)) { + os_thread_helper_unlock(&t.oth); + return; + } + + out_relation->pose.position = t.fusion.pos; + out_relation->pose.orientation = t.fusion.rot; + + //! @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); + + os_thread_helper_unlock(&t.oth); +} + +static void +imu_data(TrackerPSVR &t, + time_duration_ns delta_ns, + struct xrt_tracking_sample *sample) +{ + os_thread_helper_lock(&t.oth); + + // Don't do anything if we have stopped. + if (!os_thread_helper_is_running_locked(&t.oth)) { + os_thread_helper_unlock(&t.oth); + return; + } + + float dt = time_ns_to_s(delta_ns); + // Super simple fusion. + math_quat_integrate_velocity(&t.fusion.rot, &sample->gyro_rad_secs, dt, + &t.fusion.rot); + + os_thread_helper_unlock(&t.oth); +} + +static void +frame(TrackerPSVR &t, struct xrt_frame *xf) +{ + os_thread_helper_lock(&t.oth); + + // Don't do anything if we have stopped. + if (!os_thread_helper_is_running_locked(&t.oth)) { + os_thread_helper_unlock(&t.oth); + return; + } + + xrt_frame_reference(&t.frame, xf); + + // Wake up the thread. + os_thread_helper_signal_locked(&t.oth); + + os_thread_helper_unlock(&t.oth); +} + +static void +break_apart(TrackerPSVR &t) +{ + os_thread_helper_stop(&t.oth); +} + + +/* + * + * C wrapper functions. + * + */ + +extern "C" void +t_psvr_push_imu(struct xrt_tracked_psvr *xtmv, + time_duration_ns delta_ns, + struct xrt_tracking_sample *sample) +{ + auto &t = *container_of(xtmv, TrackerPSVR, base); + imu_data(t, delta_ns, sample); +} + +extern "C" void +t_psvr_get_tracked_pose(struct xrt_tracked_psvr *xtmv, + struct time_state *timestate, + timepoint_ns when_ns, + struct xrt_space_relation *out_relation) +{ + auto &t = *container_of(xtmv, TrackerPSVR, base); + get_pose(t, timestate, when_ns, out_relation); +} + +extern "C" void +t_psvr_fake_destroy(struct xrt_tracked_psvr *xtmv) +{ + auto &t = *container_of(xtmv, TrackerPSVR, base); + (void)t; + // Not the real destroy function +} + +extern "C" void +t_psvr_sink_push_frame(struct xrt_frame_sink *xsink, struct xrt_frame *xf) +{ + auto &t = *container_of(xsink, TrackerPSVR, sink); + frame(t, xf); +} + +extern "C" void +t_psvr_node_break_apart(struct xrt_frame_node *node) +{ + auto &t = *container_of(node, TrackerPSVR, node); + break_apart(t); +} + +extern "C" void +t_psvr_node_destroy(struct xrt_frame_node *node) +{ + auto t_ptr = container_of(node, TrackerPSVR, node); + + os_thread_helper_destroy(&t_ptr->oth); + + delete t_ptr; +} + +extern "C" void * +t_psvr_run(void *ptr) +{ + auto &t = *(TrackerPSVR *)ptr; + run(t); + return NULL; +} + + +/* + * + * Exported functions. + * + */ + +extern "C" int +t_psvr_start(struct xrt_tracked_psvr *xtmv) +{ + auto &t = *container_of(xtmv, TrackerPSVR, base); + int ret; + + ret = os_thread_helper_start(&t.oth, t_psvr_run, &t); + if (ret != 0) { + return ret; + } + + return ret; +} + +extern "C" int +t_psvr_create(struct xrt_frame_context *xfctx, + struct xrt_tracked_psvr **out_xtmv, + struct xrt_frame_sink **out_sink) +{ + fprintf(stderr, "%s\n", __func__); + + auto &t = *(new TrackerPSVR()); + int ret; + + t.base.get_tracked_pose = t_psvr_get_tracked_pose; + t.base.push_imu = t_psvr_push_imu; + t.base.destroy = t_psvr_fake_destroy; + t.sink.push_frame = t_psvr_sink_push_frame; + t.node.break_apart = t_psvr_node_break_apart; + t.node.destroy = t_psvr_node_destroy; + t.fusion.rot.x = 0.0f; + t.fusion.rot.y = 0.0f; + t.fusion.rot.z = 0.0f; + t.fusion.rot.w = 1.0f; + + ret = os_thread_helper_init(&t.oth); + if (ret != 0) { + delete (&t); + return ret; + } + + static int hack = 0; + switch (hack++) { + case 0: + t.fusion.pos.x = -0.3f; + t.fusion.pos.y = 1.3f; + t.fusion.pos.z = -0.5f; + break; + case 1: + t.fusion.pos.x = 0.3f; + t.fusion.pos.y = 1.3f; + t.fusion.pos.z = -0.5f; + break; + default: + t.fusion.pos.x = 0.0f; + t.fusion.pos.y = 0.8f + hack * 0.1f; + t.fusion.pos.z = -0.5f; + break; + } + + xrt_frame_context_add(xfctx, &t.node); + + *out_sink = &t.sink; + *out_xtmv = &t.base; + + return 0; +} diff --git a/src/xrt/auxiliary/tracking/t_tracking.h b/src/xrt/auxiliary/tracking/t_tracking.h index 116c88e42..71d118bf4 100644 --- a/src/xrt/auxiliary/tracking/t_tracking.h +++ b/src/xrt/auxiliary/tracking/t_tracking.h @@ -22,6 +22,7 @@ extern "C" { */ struct xrt_tracked_psmv; +struct xrt_tracked_psvr; /* @@ -159,6 +160,14 @@ t_psmv_create(struct xrt_frame_context *xfctx, struct xrt_tracked_psmv **out_xtmv, struct xrt_frame_sink **out_sink); +int +t_psvr_start(struct xrt_tracked_psvr *xtvr); + +int +t_psvr_create(struct xrt_frame_context *xfctx, + struct xrt_tracked_psvr **out_xtvr, + struct xrt_frame_sink **out_sink); + /* * diff --git a/src/xrt/drivers/psvr/psvr_device.c b/src/xrt/drivers/psvr/psvr_device.c index 16c6a9029..fbce9b68a 100644 --- a/src/xrt/drivers/psvr/psvr_device.c +++ b/src/xrt/drivers/psvr/psvr_device.c @@ -205,21 +205,25 @@ update_fusion(struct psvr_device *psvr, uint32_t tick_delta) { struct xrt_vec3 mag = {0.0f, 0.0f, 0.0f}; - float delta_ns = tick_delta * PSVR_TICK_PERIOD; (void)mag; accel_from_psvr_vec(&sample->accel, &psvr->read.accel); gyro_from_psvr_vec(&sample->gyro, &psvr->read.gyro); if (psvr->tracker != NULL) { + time_duration_ns delta_ns = + tick_delta * (1000000000.0 / PSVR_TICKS_PER_SECOND); + struct xrt_tracking_sample sample; sample.accel_m_s2 = psvr->read.accel; sample.gyro_rad_secs = psvr->read.gyro; xrt_tracked_psvr_push_imu(psvr->tracker, delta_ns, &sample); } else { + float delta_secs = tick_delta / PSVR_TICKS_PER_SECOND; + math_quat_integrate_velocity(&psvr->fusion.rot, - &psvr->read.gyro, delta_ns, + &psvr->read.gyro, delta_secs, &psvr->fusion.rot); } } diff --git a/src/xrt/drivers/psvr/psvr_device.h b/src/xrt/drivers/psvr/psvr_device.h index 87132c1e6..25953d6e5 100644 --- a/src/xrt/drivers/psvr/psvr_device.h +++ b/src/xrt/drivers/psvr/psvr_device.h @@ -52,7 +52,7 @@ enum psvr_status_bits #define PSVR_STATUS_VR_MODE_OFF 0 #define PSVR_STATUS_VR_MODE_ON 1 -#define PSVR_TICK_PERIOD (1.0f / 1000000.0f) // 1 MHz ticks +#define PSVR_TICKS_PER_SECOND (1000000.0) // 1 MHz ticks #define PSVR_PKG_STATUS 0xF0 #define PSVR_PKG_0xA0 0xA0