compat/timer-resolution: we want higher timer resolution

Windows scheduler performs badly with 1000 Hz.

4 files changed, 67 insertions, 58 deletions

diff --git a/compat/CMakeLists.txt b/compat/CMakeLists.txt
index 33cb02bd..7e5dd4cd 100644
--- a/compat/CMakeLists.txt
+++ b/compat/CMakeLists.txt
@@ -4,6 +4,10 @@ if(NOT WIN32 AND NOT APPLE)
     target_link_libraries(opentrack-compat rt)
 endif()
 
+if(WIN32)
+    target_link_libraries(opentrack-compat winmm)
+endif()
+
 if(CMAKE_COMPILER_IS_GNUCXX)
     otr_prop(SOURCE nan.cpp COMPILE_FLAGS "-fno-lto -fno-fast-math -fno-finite-math-only -O0")
 endif()
diff --git a/compat/timer-resolution.cpp b/compat/timer-resolution.cpp
index d59c250d..eece39e9 100644
--- a/compat/timer-resolution.cpp
+++ b/compat/timer-resolution.cpp
@@ -5,16 +5,21 @@
  * copyright notice and this permission notice appear in all copies.
  */
 
+#ifdef _WIN32
+
 #include "timer-resolution.hpp"
+#include "time.hpp"
+#include "compat/util.hpp"
 
-#if defined _WIN32
-#   include <QLibrary>
-#   include <QDebug>
+#include <utility>
 
-using namespace timer_impl;
+#include <QLibrary>
+#include <QDebug>
 
-static funptr_NtSetTimerResolution init_timer_resolution_funptr();
-static funptr_NtSetTimerResolution get_funptr();
+#include <windows.h>
+
+using namespace time_units;
+using namespace timer_impl;
 
 static funptr_NtSetTimerResolution init_timer_resolution_funptr()
 {
@@ -49,37 +54,42 @@ static funptr_NtSetTimerResolution get_funptr()
     return ret;
 }
 
-timer_resolution::timer_resolution(int msecs) : old_value(fail_())
+timer_resolution::timer_resolution()
 {
-    if (msecs <= 0 || msecs > 100)
-    {
-        qDebug() << "can't set timer resolution to" << msecs << "ms";
-        return;
-    }
+    for (int k = 14; k > 0; k--)
+        if (unlikely(timeEndPeriod(k) == 0))
+        {
+            qDebug() << "removed mm timer for" << k << "ms";
+            k++;
+        }
 
-    funptr_NtSetTimerResolution set_timer_res = get_funptr();
-    if (set_timer_res == nullptr)
-        return;
+    static funptr_NtSetTimerResolution set_timer_res = get_funptr();
 
     // hundredth of a nanosecond
-    const ulong_ value = msecs * ulong_(10000);
+    //const ulong_ value = msecs * ulong_(10000);
+
+    const ulong_ value = 156250; // default win32 timer length
     ntstatus_ res;
+    ulong_ old_value = fail_();
 
     res = set_timer_res(value, true_(), &old_value);
 
-    if (res < 0)
+    if (unlikely(res != 0))
     {
         old_value = fail_();
         qDebug() << "NtSetTimerResolution erred with" << res;
         return;
     }
 
+    if (likely(std::abs(long(old_value) - long(value)) < 10000))
+        return;
+
     // see if it stuck
 
     ulong_ old_value_ = fail_();
 
     res = set_timer_res(value, true_(), &old_value_);
-    if (res < 0)
+    if (unlikely(res != 0))
     {
         old_value = fail_();
         qDebug() << "NtSetTimerResolution check erred with" << res;
@@ -92,21 +102,10 @@ timer_resolution::timer_resolution(int msecs) : old_value(fail_())
 
         qDebug() << "NtSetTimerResolution:"
                  << "old value didn't stick"
-                 << "current resolution" << (t(old_value_) * t(100))
-                 << "ns";
+                 << "current resolution" << time_cast<ms>(ns(t(old_value_) * t(100))).count() << "ms";
         old_value = fail_();
         return;
     }
 }
 
-timer_resolution::~timer_resolution()
-{
-    if (old_value != fail_())
-    {
-        funptr_NtSetTimerResolution f = get_funptr();
-        ulong_ fuzz = fail_();
-        (void) f(old_value, true_(), &fuzz);
-    }
-}
-
 #endif
diff --git a/compat/timer-resolution.hpp b/compat/timer-resolution.hpp
index 8c18a600..8f1d4bc7 100644
--- a/compat/timer-resolution.hpp
+++ b/compat/timer-resolution.hpp
@@ -43,11 +43,11 @@ using funptr_NtSetTimerResolution = ntstatus_ (__stdcall *)(ulong_, boolean_, ul
 
 class OTR_COMPAT_EXPORT timer_resolution final
 {
-    ulong_ old_value;
-
 public:
-    timer_resolution(int msecs);
+    timer_resolution();
+#if 0
     ~timer_resolution();
+#endif
 };
 
 }
@@ -55,8 +55,5 @@ public:
 using timer_impl::timer_resolution;
 
 #else
-struct timer_resolution final
-{
-    inline timer_resolution(int) {}
-};
+#   error "this is win32-only header"
 #endif
diff --git a/logic/tracker.cpp b/logic/tracker.cpp
index 1a37a8a1..a7b19a48 100644
--- a/logic/tracker.cpp
+++ b/logic/tracker.cpp
@@ -12,9 +12,13 @@
  * originally written by Wim Vriend.
  */
 
+#include "compat/nan.hpp"
 #include "compat/sleep.hpp"
 #include "compat/util.hpp"
-#include "compat/timer-resolution.hpp"
+
+#if defined _WIN32
+#   include "compat/timer-resolution.hpp"
+#endif
 
 #include "tracker.h"
 
@@ -22,6 +26,10 @@
 #include <algorithm>
 #include <cstdio>
 
+#ifdef _WIN32
+#   include <windows.h>
+#endif
+
 using namespace euler;
 using namespace gui_tracker_impl;
 using namespace time_units;
@@ -90,8 +98,6 @@ void Tracker::t_compensate(const rmat& rmat, const euler_t& xyz, euler_t& output
         output(TX) = -ret(tb_X);
 }
 
-#include "compat/nan.hpp"
-
 static inline double elide_nan(double value, double def)
 {
     if (nanp(value))
@@ -376,8 +382,6 @@ void Tracker::run()
 {
     setPriority(QThread::HighPriority);
 
-    timer_resolution res(1);
-
     {
         static constexpr const char* posechannels[6] = { "TX", "TY", "TZ", "Yaw", "Pitch", "Roll" };
         static constexpr const char* datachannels[5] = { "dt", "raw", "corrected", "filtered", "mapped" };
@@ -398,15 +402,18 @@ void Tracker::run()
 
     t.start();
 
+#if defined _WIN32
+    timer_resolution res;
+    (void) res;
+#endif
+
     while (!get(f_should_quit))
     {
         logic();
 
-        static constexpr ns const_sleep_ms(time_cast<ns>(ms(4)));
+        static constexpr ns const_sleep_ms(time_cast<ns>(ms_(4)));
         const ns elapsed_nsecs = prog1(t.elapsed<ns>(), t.start());
 
-        backlog_time += ns(elapsed_nsecs - const_sleep_ms);
-
         if (backlog_time > secs_(3) || backlog_time < secs_(-3))
         {
             qDebug() << "tracker: backlog interval overflow"
@@ -414,23 +421,25 @@ void Tracker::run()
             backlog_time = backlog_time.zero();
         }
 
-        const int sleep_time_ms = iround(time_cast<ms>(clamp(const_sleep_ms - backlog_time,
-                                                             ns(0), ms(50)))
-                                         .count());
+        backlog_time += ns(elapsed_nsecs - const_sleep_ms);
+
+        const int sleep_time_ms = iround(std::fmax(0.,
+                                                   (time_cast<ms>(clamp(const_sleep_ms - backlog_time,
+                                                                        ms_::zero(), ms_(50)))).count()));
 
         portable::sleep(sleep_time_ms);
-    }
 
-    {
-        // filter may inhibit exact origin
-        Pose p;
-        libs.pProtocol->pose(p);
-    }
+        {
+            // filter may inhibit exact origin
+            Pose p;
+            libs.pProtocol->pose(p);
+        }
 
-    for (int i = 0; i < 6; i++)
-    {
-        m(i).spline_main.set_tracking_active(false);
-        m(i).spline_alt.set_tracking_active(false);
+        for (int i = 0; i < 6; i++)
+        {
+            m(i).spline_main.set_tracking_active(false);
+            m(i).spline_alt.set_tracking_active(false);
+        }
     }
 }