logic/tracker: try keep constant Hz

- the timekeeping handling was incorrect and Hz was lower than expected
- change Hz from 333 to 250 Hz
- adjust track-logger usage

2 files changed, 40 insertions, 27 deletions

diff --git a/opentrack-logic/tracker.cpp b/opentrack-logic/tracker.cpp
index ab1a7406..d060fff1 100644
--- a/opentrack-logic/tracker.cpp
+++ b/opentrack-logic/tracker.cpp
@@ -12,10 +12,12 @@
  * originally written by Wim Vriend.
  */
 
+#include "opentrack-compat/sleep.hpp"
 
 #include "tracker.h"
 #include <cmath>
 #include <algorithm>
+#include <cstdio>
 
 #if defined(_WIN32)
 #   include <windows.h>
@@ -24,15 +26,15 @@
 Tracker::Tracker(Mappings &m, SelectedLibraries &libs, TrackLogger &logger) :
     m(m),
     newpose {0,0,0, 0,0,0},
-    centerp(s.center_at_startup),
-    enabledp(true),
-    zero_(false),
-    should_quit(false),
     libs(libs),
     logger(logger),
     r_b(get_camera_offset_matrix(c_div).t()),
     r_b_real(get_camera_offset_matrix(1).t()),
-    t_b {0,0,0}
+    t_b {0,0,0},
+    centerp(s.center_at_startup),
+    enabledp(true),
+    zero_(false),
+    should_quit(false)
 {
 }
 
@@ -117,6 +119,9 @@ void Tracker::logic()
 {
     using namespace euler;
 
+    logger.write_dt();
+    logger.reset_dt();
+
     Pose value, raw;
 
     for (int i = 0; i < 6; i++)
@@ -284,8 +289,6 @@ void Tracker::logic()
             (void) map(value(i), m(i));
     }
 
-    logger.next_line();
-
     libs.pProtocol->pose(value);
 
     last_mapped = value;
@@ -294,16 +297,19 @@ void Tracker::logic()
     QMutexLocker foo(&mtx);
     output_pose = value;
     raw_6dof = raw;
+
+    logger.reset_dt();
+    logger.next_line();
 }
 
 void Tracker::run()
 {
-    const int sleep_ms = 3;
-
 #if defined(_WIN32)
     (void) timeBeginPeriod(1);
 #endif
 
+    setPriority(QThread::HighPriority);
+
     {
         static constexpr const char* posechannels[6] = { "TX", "TY", "TZ", "Yaw", "Pitch", "Roll" };
         static constexpr const char* datachannels[5] = { "dt", "raw", "corrected", "filtered", "mapped" };
@@ -313,21 +319,18 @@ void Tracker::run()
         {
             for (unsigned i = 0; i < 6; ++i)
             {
-                snprintf(buffer, 128, "%s%s", datachannels[j], posechannels[i]);
+                std::sprintf(buffer, "%s%s", datachannels[j], posechannels[i]);
                 logger.write(buffer);
             }
         }
+        logger.next_line();
     }
-    logger.next_line();
+
+    t.start();
+    logger.reset_dt();
 
     while (!should_quit)
     {
-        {
-            double dt = t.elapsed_seconds();
-            logger.write(&dt, 1);
-        }
-        t.start();
-
         double tmp[6] {0,0,0, 0,0,0};
         libs.pTracker->data(tmp);
 
@@ -337,10 +340,19 @@ void Tracker::run()
 
         logic();
 
-        long q = long(sleep_ms * 1000L - t.elapsed()/1000L);
+        static constexpr long const_sleep_us = 4000;
+
         using std::max;
-        using ulong = unsigned long;
-        usleep(ulong(max(1L, q)));
+        using std::min;
+
+        const long elapsed_usecs = t.elapsed_usecs();
+        const long sleep_us = const_sleep_us * 2 - elapsed_usecs;
+
+        const unsigned sleep_time = unsigned(max(1l, min(const_sleep_us * 4, max(1l, (sleep_us + 200l)/1000l))));
+
+        t.start();
+
+        portable::sleep(unsigned(max(1u, sleep_time)));
     }
 
     {
diff --git a/opentrack-logic/tracker.h b/opentrack-logic/tracker.h
index 2153f7c0..6dc48074 100644
--- a/opentrack-logic/tracker.h
+++ b/opentrack-logic/tracker.h
@@ -33,6 +33,9 @@ class OPENTRACK_LOGIC_EXPORT Tracker : private QThread
 {
     Q_OBJECT
 private:
+    using rmat = euler::rmat;
+    using euler_t = euler::euler_t;
+
     QMutex mtx;
     main_settings s;
     Mappings& m;
@@ -41,22 +44,20 @@ private:
     Pose output_pose, raw_6dof, last_mapped, last_raw;
 
     double newpose[6];
-    volatile bool centerp;
-    volatile bool enabledp;
-    volatile bool zero_;
-    volatile bool should_quit;
     SelectedLibraries const& libs;
     // The owner of the reference is the main window.
     // This design might be usefull if we decide later on to swap out
     // the logger while the tracker is running.
     TrackLogger &logger;
 
-    using rmat = euler::rmat;
-    using euler_t = euler::euler_t;
-
     rmat r_b, r_b_real;
     double t_b[3];
 
+    volatile bool centerp;
+    volatile bool enabledp;
+    volatile bool zero_;
+    volatile bool should_quit;
+
     double map(double pos, Map& axis);
     void logic();
     void t_compensate(const rmat& rmat, const euler_t& ypr, euler_t& output, bool rz);