WIP

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)
 {