Support FreePIE IMU UDP protocol

Issue: #48

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

7 files changed, 415 insertions, 0 deletions

diff --git a/ftnoir_tracker_freepie-udp/freepie-udp-controls.ui b/ftnoir_tracker_freepie-udp/freepie-udp-controls.ui
new file mode 100644
index 00000000..937dafaa
--- /dev/null
+++ b/ftnoir_tracker_freepie-udp/freepie-udp-controls.ui
@@ -0,0 +1,186 @@
+<?xml version="1.0" encoding="UTF-8"?>

+<ui version="4.0">

+ <class>UI_freepie_udp_dialog</class>

+ <widget class="QWidget" name="UI_freepie_udp_dialog">

+  <property name="windowModality">

+   <enum>Qt::NonModal</enum>

+  </property>

+  <property name="geometry">

+   <rect>

+    <x>0</x>

+    <y>0</y>

+    <width>208</width>

+    <height>240</height>

+   </rect>

+  </property>

+  <property name="windowTitle">

+   <string>UDP tracker settings</string>

+  </property>

+  <property name="windowIcon">

+   <iconset>

+    <normaloff>../facetracknoir/images/facetracknoir.png</normaloff>../facetracknoir/images/facetracknoir.png</iconset>

+  </property>

+  <property name="layoutDirection">

+   <enum>Qt::LeftToRight</enum>

+  </property>

+  <property name="autoFillBackground">

+   <bool>false</bool>

+  </property>

+  <layout class="QGridLayout" name="gridLayout_3">

+   <property name="topMargin">

+    <number>0</number>

+   </property>

+   <item row="0" column="0" colspan="3">

+    <widget class="QGroupBox" name="groupbox_port">

+     <layout class="QGridLayout" name="gridLayout">

+      <item row="0" column="1">

+       <widget class="QSpinBox" name="port">

+        <property name="minimum">

+         <number>0</number>

+        </property>

+        <property name="maximum">

+         <number>65535</number>

+        </property>

+       </widget>

+      </item>

+      <item row="0" column="0">

+       <widget class="QLabel" name="label_5">

+        <property name="sizePolicy">

+         <sizepolicy hsizetype="Maximum" vsizetype="Preferred">

+          <horstretch>0</horstretch>

+          <verstretch>0</verstretch>

+         </sizepolicy>

+        </property>

+        <property name="text">

+         <string>UDP port</string>

+        </property>

+       </widget>

+      </item>

+     </layout>

+    </widget>

+   </item>

+   <item row="1" column="0" rowspan="2" colspan="3">

+    <widget class="QGroupBox" name="groupBox_3">

+     <property name="minimumSize">

+      <size>

+       <width>0</width>

+       <height>85</height>

+      </size>

+     </property>

+     <property name="title">

+      <string>Enable axis</string>

+     </property>

+     <layout class="QGridLayout" name="gridLayout_2">

+      <item row="2" column="0">

+       <widget class="QLabel" name="label_6">

+        <property name="sizePolicy">

+         <sizepolicy hsizetype="Maximum" vsizetype="Preferred">

+          <horstretch>0</horstretch>

+          <verstretch>0</verstretch>

+         </sizepolicy>

+        </property>

+        <property name="text">

+         <string>Roll</string>

+        </property>

+       </widget>

+      </item>

+      <item row="1" column="1">

+       <widget class="QCheckBox" name="chkEnablePitch">

+        <property name="maximumSize">

+         <size>

+          <width>20</width>

+          <height>16777215</height>

+         </size>

+        </property>

+        <property name="layoutDirection">

+         <enum>Qt::LeftToRight</enum>

+        </property>

+        <property name="text">

+         <string/>

+        </property>

+       </widget>

+      </item>

+      <item row="0" column="0">

+       <widget class="QLabel" name="label_11">

+        <property name="sizePolicy">

+         <sizepolicy hsizetype="Maximum" vsizetype="Preferred">

+          <horstretch>0</horstretch>

+          <verstretch>0</verstretch>

+         </sizepolicy>

+        </property>

+        <property name="text">

+         <string>Yaw</string>

+        </property>

+       </widget>

+      </item>

+      <item row="1" column="0">

+       <widget class="QLabel" name="label_9">

+        <property name="sizePolicy">

+         <sizepolicy hsizetype="Maximum" vsizetype="Preferred">

+          <horstretch>0</horstretch>

+          <verstretch>0</verstretch>

+         </sizepolicy>

+        </property>

+        <property name="text">

+         <string>Pitch</string>

+        </property>

+       </widget>

+      </item>

+      <item row="0" column="1">

+       <widget class="QCheckBox" name="chkEnableYaw">

+        <property name="maximumSize">

+         <size>

+          <width>20</width>

+          <height>16777215</height>

+         </size>

+        </property>

+        <property name="layoutDirection">

+         <enum>Qt::LeftToRight</enum>

+        </property>

+        <property name="text">

+         <string/>

+        </property>

+       </widget>

+      </item>

+      <item row="2" column="1">

+       <widget class="QCheckBox" name="chkEnableRoll">

+        <property name="maximumSize">

+         <size>

+          <width>20</width>

+          <height>16777215</height>

+         </size>

+        </property>

+        <property name="layoutDirection">

+         <enum>Qt::LeftToRight</enum>

+        </property>

+        <property name="styleSheet">

+         <string notr="true"/>

+        </property>

+        <property name="text">

+         <string/>

+        </property>

+       </widget>

+      </item>

+     </layout>

+    </widget>

+   </item>

+   <item row="3" column="1">

+    <widget class="QDialogButtonBox" name="buttonBox">

+     <property name="standardButtons">

+      <set>QDialogButtonBox::Cancel|QDialogButtonBox::Ok</set>

+     </property>

+    </widget>

+   </item>

+  </layout>

+ </widget>

+ <tabstops>

+  <tabstop>port</tabstop>

+ </tabstops>

+ <resources/>

+ <connections/>

+ <slots>

+  <slot>startEngineClicked()</slot>

+  <slot>stopEngineClicked()</slot>

+  <slot>cameraSettingsClicked()</slot>

+ </slots>

+</ui>

diff --git a/ftnoir_tracker_freepie-udp/freepie-udp-res.qrc b/ftnoir_tracker_freepie-udp/freepie-udp-res.qrc
new file mode 100644
index 00000000..3fd3edc4
--- /dev/null
+++ b/ftnoir_tracker_freepie-udp/freepie-udp-res.qrc
@@ -0,0 +1,5 @@
+<RCC>
+    <qresource prefix="/">
+        <file>glovepie.png</file>
+    </qresource>
+</RCC>
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;
+}
diff --git a/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.h b/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.h
new file mode 100644
index 00000000..b6fd544e
--- /dev/null
+++ b/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.h
@@ -0,0 +1,69 @@
+/* Copyright (c) 2014 Stanislaw Halik <sthalik@misaki.pl>
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ */
+
+#include "ftnoir_tracker_base/ftnoir_tracker_base.h"
+#include "ui_freepie-udp-controls.h"
+#include <QUdpSocket>
+#include <QThread>
+#include "facetracknoir/plugin-support.h"
+#include "facetracknoir/options.h"
+using namespace options;
+
+struct settings {
+    pbundle b;
+    value<int> port;
+    value<bool> enable_roll, enable_pitch, enable_yaw;
+    settings() :
+        b(bundle("freepie-udp-tracker")),
+        port(b, "port", 4237),
+        enable_roll(b, "enable-roll", true),
+        enable_pitch(b, "enable-pitch", true),
+        enable_yaw(b, "enable-yaw", true)
+    {}
+};
+
+class TrackerImpl : public ITracker, protected QThread
+{
+public:
+	TrackerImpl();
+    virtual ~TrackerImpl() override;
+    void StartTracker(QFrame *);
+    void GetHeadPoseData(double *data);
+protected:
+	void run();
+private:
+    double pose[6];
+    QUdpSocket sock;
+    settings s;
+    QMutex mtx;
+    volatile bool should_quit;
+};
+
+class TrackerDialog : public QWidget, public ITrackerDialog
+{
+    Q_OBJECT
+public:
+	TrackerDialog();
+    void registerTracker(ITracker *) {}
+    void unRegisterTracker() {}
+private:
+	Ui::UI_freepie_udp_dialog ui;
+    settings s;
+private slots:
+	void doOK();
+	void doCancel();
+};
+
+class TrackerMeta : public Metadata
+{
+public:
+	void getFullName(QString *strToBeFilled);
+	void getShortName(QString *strToBeFilled);
+	void getDescription(QString *strToBeFilled);
+	void getIcon(QIcon *icon);
+};
+
diff --git a/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp_dialog.cpp b/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp_dialog.cpp
new file mode 100644
index 00000000..702cc8a0
--- /dev/null
+++ b/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp_dialog.cpp
@@ -0,0 +1,30 @@
+#include "ftnoir_tracker_freepie-udp.h"
+#include "facetracknoir/plugin-support.h"
+
+TrackerDialog::TrackerDialog()
+{
+	ui.setupUi(this);
+
+	connect(ui.buttonBox, SIGNAL(accepted()), this, SLOT(doOK()));
+	connect(ui.buttonBox, SIGNAL(rejected()), this, SLOT(doCancel()));
+
+    tie_setting(s.port, ui.port);
+    tie_setting(s.enable_yaw, ui.chkEnableYaw);
+    tie_setting(s.enable_pitch, ui.chkEnablePitch);
+    tie_setting(s.enable_roll, ui.chkEnableRoll);
+}
+
+void TrackerDialog::doOK() {
+    s.b->save();
+	this->close();
+}
+
+void TrackerDialog::doCancel() {
+    s.b->revert();
+    this->close();
+}
+
+extern "C" FTNOIR_TRACKER_BASE_EXPORT ITrackerDialog* CALLING_CONVENTION GetDialog()
+{
+    return new TrackerDialog;
+}
diff --git a/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp_dll.cpp b/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp_dll.cpp
new file mode 100644
index 00000000..717975e3
--- /dev/null
+++ b/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp_dll.cpp
@@ -0,0 +1,28 @@
+#include "ftnoir_tracker_freepie-udp.h"
+#include <QDebug>
+#include "facetracknoir/plugin-support.h"
+
+void TrackerMeta::getFullName(QString *strToBeFilled)
+{
+    *strToBeFilled = "FreePIE UDP";
+}
+
+void TrackerMeta::getShortName(QString *strToBeFilled)
+{
+    *strToBeFilled = "FreePIE";
+}
+
+void TrackerMeta::getDescription(QString *strToBeFilled)
+{
+    *strToBeFilled = "FreePIE UDP";
+}
+
+void TrackerMeta::getIcon(QIcon *icon)
+{
+    *icon = QIcon(":/glovepie.png");
+}
+
+extern "C" FTNOIR_TRACKER_BASE_EXPORT Metadata* CALLING_CONVENTION GetMetadata()
+{
+	return new TrackerMeta;
+}
diff --git a/ftnoir_tracker_freepie-udp/glovepie.png b/ftnoir_tracker_freepie-udp/glovepie.png
new file mode 100644
index 00000000..2156b7af
--- /dev/null
+++ b/ftnoir_tracker_freepie-udp/glovepie.png