1 files changed, 36 insertions, 9 deletions

diff --git a/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.cpp b/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.cpp
index eff1103b..540ef616 100644
--- a/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.cpp
+++ b/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.cpp
@@ -14,12 +14,21 @@ TrackerImpl::~TrackerImpl()
     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 TrackerImpl::run() {
 #pragma pack(push, 1)
     struct {
         uint8_t pad1;
         uint8_t flags;
-        uint8_t pad2;
         float fl[12];
     } data;
 #pragma pack(pop)
@@ -28,24 +37,29 @@ void TrackerImpl::run() {
         flag_Orient = 1 << 1,
         Mask = flag_Raw | flag_Orient
     };
+    
+    (void) sock.bind(QHostAddress::Any, (int) s.port, QUdpSocket::ShareAddress | QUdpSocket::ReuseAddressHint);
 
-    while (1) {
-        if (should_quit)
-            break;
-
+    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)
+        };
         float orient[3];
         bool filled = false;
 
         while (sock.hasPendingDatagrams())
         {
             using t = decltype(data);
-            t tmp {0,0,0, {0,0,0, 0,0,0}};
+            t tmp {0,0, {0,0,0, 0,0,0, 0,0,0, 0,0,0}};
             (void) sock.readDatagram(reinterpret_cast<char*>(&tmp), sizeof(data));
 
             int flags = tmp.flags & F::Mask;
 
             switch (flags)
             {
+            //default:
             case flag_Raw:
             continue;
             case flag_Raw | flag_Orient:
@@ -64,10 +78,24 @@ void TrackerImpl::run() {
 
         if (filled)
         {
+            static const int add_cbx[] = {
+                0,
+                90,
+                -90,
+                180,
+                -180,
+            };
+            int indices[] = { s.add_yaw, s.add_pitch, s.add_roll };
             QMutexLocker foo(&mtx);
-            static constexpr double d2r = 57.295781;
+            static constexpr double r2d = 57.295781;
             for (int i = 0; i < 3; i++)
-                pose[Yaw + i] = d2r * orient[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;
+            }
         }
         usleep(4000);
     }
@@ -75,7 +103,6 @@ void TrackerImpl::run() {
 
 void TrackerImpl::start_tracker(QFrame*)
 {
-    (void) sock.bind(QHostAddress::Any, (int) s.port, QUdpSocket::ShareAddress | QUdpSocket::ReuseAddressHint);
     start();
 }