Commit 8269eaa2 authored by Jakob Bornecrantz's avatar Jakob Bornecrantz

WIP

parent 2c4fa013
Pipeline #191259 passed with stages
in 1 minute and 50 seconds
......@@ -12,6 +12,8 @@
#include "xrt/xrt_defines.h"
#include "xrt/xrt_device.h"
#include "math/m_predict.h"
#include "os/os_time.h"
#include "os/os_threading.h"
......@@ -58,6 +60,12 @@ rs_6dof(struct xrt_device *xdev)
return (struct rs_6dof *)xdev;
}
static uint32_t
ns_to_ms(uint64_t val)
{
return (uint32_t)(val / 1000000);
}
/*!
* Simple helper to check and print error messages.
*/
......@@ -326,8 +334,33 @@ rs_6dof_get_tracked_pose(struct xrt_device *xdev,
uint64_t relation_timestamp_ns = rs->relation_timestamp_ns;
os_thread_helper_unlock(&rs->oth);
*out_relation_timestamp_ns = relation_timestamp_ns;
*out_relation = relation;
int64_t diff_prediction_ns = 0;
diff_prediction_ns = at_timestamp_ns - relation_timestamp_ns;
#if 1
if (diff_prediction_ns < 0) {
fprintf(stderr, "\tBefore the timestamp %ums\n",
ns_to_ms(-diff_prediction_ns));
}
#else
(void)ns_to_ms;
#endif
if (diff_prediction_ns > 0) {
double delta_s = time_ns_to_s(diff_prediction_ns);
#if 1
fprintf(stderr, "\tPredicting %fs %ums (%" PRIu64 "ns)\n",
delta_s, ns_to_ms(diff_prediction_ns),
diff_prediction_ns);
#endif
m_predict_relation(&relation, delta_s, out_relation);
} else {
*out_relation = relation;
}
*out_relation_timestamp_ns = at_timestamp_ns;
}
static void
......@@ -339,6 +372,59 @@ rs_6dof_get_view_pose(struct xrt_device *xdev,
assert(false);
}
#if 0
fprintf(stderr, "Predict-a-tron\n");
if (now_ns < relation_timestamp_ns) {
fprintf(stderr, "\tRelation from the future?!\n");
}
int64_t diff_now_ns = 0;
diff_now_ns = at_timestamp_ns - now_ns;
diff_prediction_ns = at_timestamp_ns - relation_timestamp_ns;
if (diff_now_ns < 0) {
fprintf(stderr, "\tIn the past %ums\n", ns_to_ms(-diff_now_ns));
}
if (diff_prediction_ns < 0) {
fprintf(stderr, "\tBefore the timestamp %ums\n",
ns_to_ms(-diff_prediction_ns));
}
if (diff_prediction_ns > 0) {
fprintf(stderr, "\tPredicting %ums (%" PRIu64 "ns)\n",
ns_to_ms(diff_prediction_ns), diff_prediction_ns);
}
/*
if (at_timestamp_ns < now_ns) {
uint64_t diff_ns = now_ns - at_timestamp_ns;
uint64_t diff_ms = diff_ns / (1000 * 1000);
if (at_timestamp_ns < relation_timestamp_ns) {
fprintf(stderr,
"No predict, in the past, after relation "
"timestamp! %ums (%" PRIu64 "ns)\n",
(uint32_t)diff_ms, diff_ns);
} else {
fprintf(stderr,
"Predict, in the past %ums (%" PRIu64 "ns)\n",
(uint32_t)diff_ms, diff_ns);
}
} else {
uint64_t diff_now_ns = now_ns - relation_timestamp_ns;
uint64_t diff_pred_ns = at_timestamp_ns - relation_timestamp_ns;
uint64_t diff_pred_ms = diff_pred_ns / (1000 * 1000);
fprintf(stderr,
"Predict %ums (%" PRIu64 "ns) (from now %" PRIu64 ")\n",
(uint32_t)diff_pred_ms, diff_pred_ns, diff_now_ns);
}
*/
#endif
static void
rs_6dof_destroy(struct xrt_device *xdev)
{
......
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