Commit 871b06cd authored by Jakob Bornecrantz's avatar Jakob Bornecrantz
Browse files

aux/tracking: Add stub PSVR tracker

parent 37590e75
......@@ -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
)
......
// Copyright 2019, Collabora, Ltd.
// SPDX-License-Identifier: BSL-1.0
/*!
* @file
* @brief PSVR tracker code.
* @author Jakob Bornecrantz <jakob@collabora.com>
*/
#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 <stdio.h>
#include <assert.h>
#include <pthread.h>
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;
}
......@@ -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);
/*
*
......
......@@ -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);
}
}
......
......@@ -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
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment