diff --git a/src/xrt/drivers/realsense/rs_6dof.c b/src/xrt/drivers/realsense/rs_6dof.c index dda8c0a1908be5754ed9b73bdb2e1bb1a81ce99b..ad2fbbf9a37116d0a83d262d8ededa8416b1fa91 100644 --- a/src/xrt/drivers/realsense/rs_6dof.c +++ b/src/xrt/drivers/realsense/rs_6dof.c @@ -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) {