udp tracker: allow for adding +-90/+-180 to each axis

Follows similar code in freepie-udp tracker. Helps with centering on
inertial devices.

1 files changed, 21 insertions, 3 deletions

diff --git a/ftnoir_tracker_udp/ftnoir_tracker_udp.cpp b/ftnoir_tracker_udp/ftnoir_tracker_udp.cpp
index c900fb67..ab8c8c86 100644
--- a/ftnoir_tracker_udp/ftnoir_tracker_udp.cpp
+++ b/ftnoir_tracker_udp/ftnoir_tracker_udp.cpp
@@ -16,9 +16,11 @@ void FTNoIR_Tracker::run() {
     for (;;) {
         if (should_quit)
             break;
-        QMutexLocker foo(&mutex);
-        while (sock.hasPendingDatagrams()) {
-            sock.readDatagram((char * ) last_recv_pose, sizeof(double[6]));
+        {
+            QMutexLocker foo(&mutex);
+            while (sock.hasPendingDatagrams()) {
+                sock.readDatagram((char * ) last_recv_pose, sizeof(double[6]));
+            }
         }
         msleep(1);
     }
@@ -34,6 +36,22 @@ void FTNoIR_Tracker::data(double *data)
     QMutexLocker foo(&mutex);
     for (int i = 0; i < 6; i++)
         data[i] = last_recv_pose[i];
+    
+    int values[] = {
+        0,
+        90,
+        -90,
+        180,
+        -180,
+    };
+    int indices[] = {
+        s.add_yaw,
+        s.add_pitch,
+        s.add_roll,
+    };
+    
+    for (int i = 0; i < 3; i++)
+        data[Yaw + i] += values[indices[i]];
 }
 
 extern "C" OPENTRACK_EXPORT ITracker* GetConstructor()