1 files changed, 38 insertions, 31 deletions

diff --git a/tracker-freepie-udp/ftnoir_tracker_freepie-udp.cpp b/tracker-freepie-udp/ftnoir_tracker_freepie-udp.cpp
index 4c5d14ef..3950af0c 100644
--- a/tracker-freepie-udp/ftnoir_tracker_freepie-udp.cpp
+++ b/tracker-freepie-udp/ftnoir_tracker_freepie-udp.cpp
@@ -1,31 +1,21 @@
 #include "ftnoir_tracker_freepie-udp.h"
 #include "api/plugin-api.hpp"
+#include "compat/math.hpp"
 
 #include <cinttypes>
 #include <algorithm>
 #include <cmath>
 
-
-tracker_freepie::tracker_freepie() : pose { 0,0,0, 0,0,0 }, should_quit(false)
+tracker_freepie::tracker_freepie() : pose { 0,0,0, 0,0,0 }
 {
 }
 
 tracker_freepie::~tracker_freepie()
 {
-    should_quit = true;
+    requestInterruption();
     wait();
 }
 
-template<typename t>
-static const t bound(t datum, t least, t max)
-{
-    if (datum < least)
-        return least;
-    if (datum > max)
-        return max;
-    return datum;
-}
-
 void tracker_freepie::run() {
 #pragma pack(push, 1)
     struct {
@@ -34,7 +24,11 @@ void tracker_freepie::run() {
         float fl[12];
     } data;
 #pragma pack(pop)
-    enum F {
+
+    static_assert(sizeof(data) == 50);
+
+    enum F
+    {
         flag_Raw = 1 << 0,
         flag_Orient = 1 << 1,
         Mask = flag_Raw | flag_Orient
@@ -42,19 +36,22 @@ void tracker_freepie::run() {
 
     sock.bind(QHostAddress::Any, (unsigned short) s.port, QUdpSocket::ShareAddress | QUdpSocket::ReuseAddressHint);
 
-    while (!should_quit) {
-        int order[] = {
-            bound<int>(s.idx_x, 0, 2),
-            bound<int>(s.idx_y, 0, 2),
-            bound<int>(s.idx_z, 0, 2)
+    while (!isInterruptionRequested())
+    {
+        int order[] =
+        {
+            std::clamp(*s.idx_x, 0, 2),
+            std::clamp(*s.idx_y, 0, 2),
+            std::clamp(*s.idx_z, 0, 2)
         };
+
         double orient[3] = {0, 0, 0};
         bool filled = false;
 
         while (sock.hasPendingDatagrams())
         {
             using t = decltype(data);
-            t tmp {0,0, {0,0,0, 0,0,0, 0,0,0, 0,0,0}};
+            t tmp {};
             (void) sock.readDatagram(reinterpret_cast<char*>(&tmp), sizeof(data));
 
             int flags = tmp.flags & F::Mask;
@@ -66,12 +63,13 @@ void tracker_freepie::run() {
             continue;
             case flag_Raw | flag_Orient:
                 for (int i = 0; i < 3; i++)
-                    orient[i] = tmp.fl[i+9];
+                    orient[i] = (double)tmp.fl[i+9];
             break;
             case flag_Orient:
                 for (int i = 0; i < 3; i++)
-                    orient[i] = tmp.fl[i];
+                    orient[i] = (double)tmp.fl[i];
             break;
+            default: goto fail;
             }
 
             filled = true;
@@ -80,33 +78,42 @@ void tracker_freepie::run() {
 
         if (filled)
         {
-            static const int add_cbx[] = {
+            constexpr int add_cbx[] =
+            {
                 0,
                 90,
                 -90,
                 180,
                 -180,
             };
-            int indices[] = { s.add_yaw, s.add_pitch, s.add_roll };
+
+            int add_indices[] = { s.add_yaw, s.add_pitch, s.add_roll };
+
             QMutexLocker foo(&mtx);
-            static constexpr double r2d = 180 / M_PI;
+
+            constexpr double r2d = 180 / M_PI;
+
             for (int i = 0; i < 3; i++)
             {
-                int val = 0;
-                int idx = indices[order[i]];
-                if (idx >= 0 && idx < (int)(sizeof(add_cbx) / sizeof(*add_cbx)))
-                    val = add_cbx[idx];
-                pose[Yaw + i] = r2d * orient[order[i]] + val;
+                const int axis = order[i];
+                const int add_idx = add_indices[i];
+                int add = 0;
+                if (add_idx >= 0 && add_idx < (int)std::size(add_cbx))
+                    add = add_cbx[add_idx];
+                pose[Yaw + i] = r2d * orient[axis] + add;
             }
         }
+fail:
         usleep(4000);
     }
 }
 
-void tracker_freepie::start_tracker(QFrame*)
+module_status tracker_freepie::start_tracker(QFrame*)
 {
     start();
     sock.moveToThread(this);
+
+    return status_ok();
 }
 
 void tracker_freepie::data(double *data)