From 4cfdc4fa2452d3f8bcaefbd5c314cace41f0d763 Mon Sep 17 00:00:00 2001 From: Patrick Nicolas Date: Wed, 31 Jul 2024 10:41:30 +0200 Subject: [PATCH] Move tracking origin to prevent warp on reconnect --- server/driver/wivrn_session.cpp | 38 +++++++++++++++++++++++++++++++++ server/driver/wivrn_session.h | 3 +++ 2 files changed, 41 insertions(+) diff --git a/server/driver/wivrn_session.cpp b/server/driver/wivrn_session.cpp index 5946a82b..4ca771b5 100644 --- a/server/driver/wivrn_session.cpp +++ b/server/driver/wivrn_session.cpp @@ -173,6 +173,7 @@ xrt_result_t xrt::drivers::wivrn::wivrn_session::create_session(xrt::drivers::wi devices->xdev_count, false, out_xspovrs); + self->space_overseer = *out_xspovrs; devices->destroy = [](xrt_system_devices * xsd) { // TODO @@ -204,6 +205,40 @@ void wivrn_session::operator()(from_headset::headset_info_packet &&) } void wivrn_session::operator()(from_headset::tracking && tracking) { + // Reset the tracking origin on reconnect + if (tracking.timestamp == tracking.production_timestamp) + { + for (const auto & p: tracking.device_poses) + { + if (p.device == device_id::HEAD) + { + if (p.flags & from_headset::tracking::flags::position_valid) + { + const auto & pos = p.pose.position; + if (reconnect_offset.z != 0) + { + // Apply offset + xrt_pose offset; + auto tracking_origin = ((xrt_device *)hmd.get())->tracking_origin; + space_overseer->get_tracking_origin_offset(space_overseer, tracking_origin, &offset); + offset.position.x = reconnect_offset.x - pos.x; + offset.position.y = reconnect_offset.y - pos.y; + auto res = space_overseer->set_tracking_origin_offset(space_overseer, tracking_origin, &offset); + U_LOG_I("Updated tracking origin, offset %f,%f", + reconnect_offset.x - pos.x, + reconnect_offset.y - pos.y); + } + // Store latest position + reconnect_offset = { + .x = pos.x, + .y = pos.y, + }; + } + break; + } + } + } + auto offset = offset_est.get_offset(); if (not offset) return; @@ -332,6 +367,9 @@ void wivrn_session::reconnect() U_LOG_W("Failed to notify session state change"); } + // Flag offset to be applied + reconnect_offset.z = 1; + U_LOG_I("Waiting for new connection"); auto tcp = accept_connection(0 /*stdin*/, [this]() { return quit_if_no_client(xrt_system); }); if (not tcp) diff --git a/server/driver/wivrn_session.h b/server/driver/wivrn_session.h index 7faf6dd3..22154567 100644 --- a/server/driver/wivrn_session.h +++ b/server/driver/wivrn_session.h @@ -22,6 +22,7 @@ #include "clock_offset.h" #include "wivrn_connection.h" #include "wivrn_packets.h" +#include "xrt/xrt_defines.h" #include "xrt/xrt_results.h" #include #include @@ -69,6 +70,8 @@ class wivrn_session : public std::enable_shared_from_this wivrn_connection connection; u_system & xrt_system; + xrt_space_overseer * space_overseer; + xrt_vec3 reconnect_offset{}; // z is used to store reconnect flag: 0 is none, non 0 means offset needs to be applied std::atomic quit = false; std::thread thread;