Support FreePIE IMU UDP protocol

Issue: #48

Note, pushed to stable since won't cause regressions, as it's purely
freestanding code.

1 files changed, 97 insertions, 0 deletions

diff --git a/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.cpp b/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.cpp
new file mode 100644
index 00000000..811a08d9
--- /dev/null
+++ b/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.cpp
@@ -0,0 +1,97 @@
+#include "ftnoir_tracker_freepie-udp.h"
+#include "facetracknoir/plugin-support.h"
+
+#include <cinttypes>
+
+TrackerImpl::TrackerImpl() : pose { 0,0,0, 0,0,0 }, should_quit(false)
+{
+}
+
+TrackerImpl::~TrackerImpl()
+{
+    should_quit = true;
+    wait();
+}
+
+void TrackerImpl::run() {
+    struct {
+        uint8_t pad;
+        uint8_t flags;
+        union {
+            float rot[6];
+            struct {
+                float pad[9];
+                float rot[6];                
+            } raw_rot;
+        };
+    } data;
+    
+    enum F {
+        flag_Raw = 1 << 0,
+        flag_Orient = 1 << 1,
+        Mask = flag_Raw | flag_Orient
+    };
+
+	while (1) {
+        if (should_quit)
+            break;
+        {
+            float* orient = nullptr;
+            
+            while (sock.hasPendingDatagrams()) {
+                data = decltype(data){0,0, 0,0,0};
+                (void) sock.readDatagram(reinterpret_cast<char*>(&data), sizeof(data));
+                
+                int flags = data.flags & F::Mask;
+                switch (flags)
+                {
+                case flag_Raw:
+                    continue;
+                case flag_Raw | flag_Orient:
+                    orient = data.raw_rot.rot;
+                    break;
+                case flag_Orient:
+                    orient = data.rot;
+                    break;
+                }
+            }
+            if (orient)
+            {
+                QMutexLocker foo(&mtx);
+                for (int i = 0; i < 3; i++)
+                    pose[Yaw + i] = orient[i];
+            }
+        }
+		usleep(4000);
+	}
+}
+
+void TrackerImpl::StartTracker(QFrame*)
+{
+    (void) sock.bind(QHostAddress::Any, (int) s.port, QUdpSocket::ShareAddress | QUdpSocket::ReuseAddressHint);
+	start();
+}
+
+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
+    if (s.enable_yaw)
+        data[Yaw] = pose[Yaw];
+    if (s.enable_pitch)
+        data[Pitch] = pose[Pitch];
+    if (s.enable_roll)
+        data[Roll] = pose[Roll];
+}
+
+extern "C" FTNOIR_TRACKER_BASE_EXPORT ITracker* CALLING_CONVENTION GetConstructor()
+{
+    return new TrackerImpl;
+}