From 2843048fcb63485989bfc82d47280c1a5c94d689 Mon Sep 17 00:00:00 2001
From: Stanislaw Halik <sthalik@misaki.pl>
Date: Sat, 27 Sep 2014 22:22:20 +0200
Subject: fix FreePIE udp sender finally

With enormous thanks to @KyokushinPL for resources invested.

Issue: #48
---
 .../ftnoir_tracker_freepie-udp.cpp                 | 63 ++++++++--------------
 1 file changed, 21 insertions(+), 42 deletions(-)

(limited to 'ftnoir_tracker_freepie-udp')

diff --git a/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.cpp b/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.cpp
index 26726b18..ad505229 100644
--- a/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.cpp
+++ b/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.cpp
@@ -15,73 +15,59 @@ TrackerImpl::~TrackerImpl()
 }
 
 void TrackerImpl::run() {
+#pragma pack(push, 1)
     struct {
-        uint8_t pad;
+        uint8_t pad1;
         uint8_t flags;
-        union {
-            float rot[6];
-            struct {
-                float pad[9];
-                float rot[6];
-            } raw_rot;
-        };
+        uint8_t pad2;
+        float fl[12];
     } data;
-
+#pragma pack(pop)
     enum F {
         flag_Raw = 1 << 0,
         flag_Orient = 1 << 1,
         Mask = flag_Raw | flag_Orient
     };
 
-    struct check {
-        union {
-            std::uint16_t half;
-            unsigned char bytes[2];
-        };
-        bool convertp;
-        check() : bytes { 0, 255 }, convertp(half > 255) {}
-    } crapola;
-
     while (1) {
         if (should_quit)
             break;
 
-        float* orient = nullptr;
+        float orient[3];
+        bool filled = false;
 
         while (sock.hasPendingDatagrams())
         {
             using t = decltype(data);
-            t tmp {0,0, {0,0,0, 0,0,0}};
+            t tmp {0,0,0, {0,0,0, 0,0,0}};
             (void) sock.readDatagram(reinterpret_cast<char*>(&tmp), sizeof(data));
-            int flags = data.flags & F::Mask;
+
+            int flags = tmp.flags & F::Mask;
 
             switch (flags)
             {
             case flag_Raw:
             continue;
             case flag_Raw | flag_Orient:
-                orient = data.raw_rot.rot;
+                for (int i = 0; i < 3; i++)
+                    orient[i] = tmp.fl[i+9];
             break;
             case flag_Orient:
-                orient = data.rot;
+                for (int i = 0; i < 3; i++)
+                    orient[i] = tmp.fl[i];
             break;
             }
+
+            filled = true;
             data = tmp;
         }
-        if (orient)
-        {
-            if (crapola.convertp)
-            {
-                unsigned char* alias = reinterpret_cast<unsigned char*>(orient);
-                constexpr int sz = sizeof(float[6]);
-                std::reverse(alias, alias + sz);
-            }
 
+        if (filled)
+        {
             QMutexLocker foo(&mtx);
+            static constexpr double d2r = 57.295781;
             for (int i = 0; i < 3; i++)
-                pose[Yaw + i] = orient[i];
-
-            orient = nullptr;
+                pose[Yaw + i] = d2r * orient[i];
         }
         usleep(4000);
     }
@@ -96,14 +82,7 @@ void TrackerImpl::StartTracker(QFrame*)
 void TrackerImpl::GetHeadPoseData(double *data)
 {
     QMutexLocker foo(&mtx);
-#if 0
-    if (s.enable_x)
-        data[TX] = pose[TX];
-    if (s.enable_y)
-        data[TY] = pose[TY];
-    if (s.enable_z)
-        data[TZ] = pose[TZ];
-#endif
+
     data[Yaw] = pose[Yaw];
     data[Pitch] = pose[Pitch];
     data[Roll] = pose[Roll];
-- 
cgit v1.2.3