Files
WiVRn/server/driver/clock_offset.cpp
T
2024-03-30 17:00:34 +01:00

139 lines
3.6 KiB
C++

/*
* WiVRn VR streaming
* Copyright (C) 2022-2024 Guillaume Meunier <[email protected]>
* Copyright (C) 2022-2024 Patrick Nicolas <[email protected]>
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
#include "clock_offset.h"
#include "driver/wivrn_connection.h"
#include "os/os_time.h"
#include "util/u_logging.h"
static const size_t num_samples = 100;
void clock_offset_estimator::request_sample(wivrn_connection & connection)
{
if (std::chrono::steady_clock::now() < next_sample)
return;
next_sample = std::chrono::steady_clock::now() + sample_interval.load();
connection.send_stream(
xrt::drivers::wivrn::to_headset::timesync_query{
.query = XrTime(os_monotonic_get_ns()),
});
}
void clock_offset_estimator::add_sample(const xrt::drivers::wivrn::from_headset::timesync_response & base_sample)
{
XrTime now = os_monotonic_get_ns();
clock_offset_estimator::sample sample{base_sample, now};
std::lock_guard lock(mutex);
if (samples.size() < num_samples)
{
samples.push_back(sample);
}
else
{
sample_interval = std::chrono::seconds(1);
int64_t latency = 0;
for (const auto & s: samples)
latency += s.received - s.query;
latency /= samples.size();
// packets with too high latency are likely to be retransmitted
if (sample.received - sample.query > 3 * latency)
{
U_LOG_D("drop packet for latency %ldµs > %ldµs", (sample.received - sample.query) / 1000, latency / 1000);
return;
}
samples[sample_index] = sample;
sample_index = (sample_index + 1) % num_samples;
}
// Linear regression
// X = time on server
// Y = time on headset
// in order to maintain accuracy, use x = X-x0, y = Y-y0
// where x0 and y0 are means of X an Y
size_t n = samples.size();
double inv_n = 1. / n;
double x0 = 0;
double y0 = 0;
for (const auto & s: samples)
{
x0 += (s.query + s.received) * 0.5;
y0 += s.response;
}
x0 /= n;
y0 /= n;
if (samples.size() < num_samples)
{
offset.a = 1;
offset.b = y0 - x0;
return;
}
double sum_x = 0;
double sum_y = 0;
double sum_x2 = 0;
double sum_xy = 0;
for (const auto & s: samples)
{
#if 1
// assume symmetrical latency
double x = (s.query + s.received) * 0.5 - x0;
#else
// assume latency is only on server -> headset link
double x = s.received - x0;
#endif
double y = s.response - y0;
sum_x += x;
sum_y += y;
sum_x2 += x * x;
sum_xy += x * y;
}
double mean_x = sum_x * inv_n;
double mean_y = sum_y * inv_n;
// y = ax + b
double cov = inv_n * sum_xy - mean_x * mean_y;
double v = inv_n * sum_x2 - mean_x * mean_x;
double a = cov / v;
double b = mean_y - a * mean_x;
offset.a = a;
offset.b = y0 + b - int64_t(a * x0);
U_LOG_D("clock relations: headset = a*x+b where a=%f b=%ldµs", offset.a, offset.b / 1000);
}
clock_offset clock_offset_estimator::get_offset()
{
std::lock_guard lock(mutex);
return offset;
}
XrTime clock_offset::from_headset(XrTime ts) const
{
return (ts - b) / a;
}
XrTime clock_offset::to_headset(XrTime timestamp_ns) const
{
return a * timestamp_ns + b;
}