Move tracking origin to prevent warp on reconnect

This commit is contained in:
Patrick Nicolas committed 2024-08-01 09:03:19 +02:00
1 parent a90e763934
commit 4cfdc4fa24
2 files changed
+41

No files matched your search

+38
View File
@@ -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)
+3
View File
@@ -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 <atomic>
#include <fstream>
@@ -69,6 +70,8 @@ class wivrn_session : public std::enable_shared_from_this<wivrn_session>
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<bool> quit = false;
std::thread thread;