Send origin pose to protocol on stopping

Requested-by: @Antmax

1 files changed, 11 insertions, 2 deletions

diff --git a/opentrack/tracker.cpp b/opentrack/tracker.cpp
index 24482bfc..7e6919a3 100644
--- a/opentrack/tracker.cpp
+++ b/opentrack/tracker.cpp
@@ -103,8 +103,6 @@ void Tracker::t_compensate(const cv::Matx33d& rmat, const double* xyz, double* o
 
 void Tracker::logic()
 {
-    libs.pTracker->data(newpose);
-
     if (enabledp)
         for (int i = 0; i < 6; i++)
             final_raw(i) = newpose[i];
@@ -196,6 +194,7 @@ void Tracker::run() {
     {
         t.start();
 
+        libs.pTracker->data(newpose);
         logic();
 
         double q = sleep_ms * 1000L;
@@ -204,6 +203,16 @@ void Tracker::run() {
         usleep((long)q);
     }
 
+    {
+        // do one last pass with origin pose
+        for (int i = 0; i < 6; i++)
+            newpose[i] = 0;
+        logic();
+        // filter may inhibit exact origin
+        Pose p;
+        libs.pProtocol->pose(p);
+    }
+
 #if defined(_WIN32)
     (void) timeEndPeriod(1);
 #endif