1 files changed, 35 insertions, 36 deletions

diff --git a/tracker-s2bot/ftnoir_tracker_s2bot.cpp b/tracker-s2bot/ftnoir_tracker_s2bot.cpp
index 66c4fecf..88362aa4 100644
--- a/tracker-s2bot/ftnoir_tracker_s2bot.cpp
+++ b/tracker-s2bot/ftnoir_tracker_s2bot.cpp
@@ -1,5 +1,6 @@
 #include "ftnoir_tracker_s2bot.h"
 #include "api/plugin-api.hpp"
+#include "compat/util.hpp"
 
 #include <cinttypes>
 #include <algorithm>
@@ -17,16 +18,6 @@ tracker_s2bot::~tracker_s2bot()
     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_s2bot::run() {
     if (s.freq == 0) s.freq = 10;
     timer.setInterval(1000.0/s.freq);
@@ -35,55 +26,63 @@ void tracker_s2bot::run() {
         auto reply = m_nam->get(QNetworkRequest(QUrl("http://localhost:17317/poll")));
         connect(reply, &QNetworkReply::finished, [this, reply]() {
             if (reply->error() == QNetworkReply::NoError) {
-                qDebug() << "Request submitted OK";
+                //qDebug() << "Request submitted OK";
             }
             else {
                 qWarning() << "Request bounced:" << reply->attribute(QNetworkRequest::HttpStatusCodeAttribute) << reply->errorString();
                 return;
             }
-            QByteArray ba = reply->readAll();
-            QStringList slist = QString(ba).split(QRegExp("[\r\n]"), QString::SkipEmptyParts);
-            int order[] = {
-                bound<int>(s.idx_x, 0, 3),
-                bound<int>(s.idx_y, 0, 3),
-                bound<int>(s.idx_z, 0, 3)
+
+            const QStringList slist = QString::fromLatin1(reply->readAll()).split(QRegExp("[\r\n]+"), QString::SkipEmptyParts);
+            reply->close();
+            reply->deleteLater();
+
+            int order[] =
+            {
+                clamp(s.idx_x, 0, 3),
+                clamp(s.idx_y, 0, 3),
+                clamp(s.idx_z, 0, 3),
             };
-            double orient[4] = { 0, 0, 0, 0 };
-            static const int add_cbx[] = {
+
+            static constexpr int add_cbx[] =
+            {
                 0,
                 90,
                 -90,
                 180,
                 -180,
             };
-            int indices[] = { s.add_yaw, s.add_pitch, s.add_roll };
-            for (auto line : slist) {
-                QStringList keyval = line.split(" ");
+
+            int add_indices[] = { s.add_yaw, s.add_pitch, s.add_roll, };
+            double orient[4] {};
+
+            for (auto line : slist)
+            {
+                QStringList keyval = line.split(' ');
                 if (keyval.count() < 2) continue;
-                if (keyval[0].startsWith("accelerometerZ")) orient[0] = keyval[1].toInt();
-                else if (keyval[0].startsWith("accelerometerY")) orient[1] = keyval[1].toInt();
-                else if (keyval[0].startsWith("accelerometerX")) orient[2] = keyval[1].toInt();
-                else if (keyval[0].startsWith("bearing")) orient[3] = keyval[1].toInt();
+                if (keyval[0].startsWith("accelerometerZ")) orient[0] = keyval[1].toDouble();
+                else if (keyval[0].startsWith("accelerometerY")) orient[1] = keyval[1].toDouble();
+                else if (keyval[0].startsWith("accelerometerX")) orient[2] = keyval[1].toDouble();
+                else if (keyval[0].startsWith("bearing")) orient[3] = keyval[1].toDouble();
             }
+
             QMutexLocker foo(&mtx);
-            static constexpr double r2d = 180 / M_PI;
-                        (void)r2d;
+
             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] = orient[order[i]] + val; // * r2d if it was radians
+                const int axis = order[i];
+                const int add_idx = add_indices[i];
+                int add = 0;
+                if (add_idx >= 0 && add_idx < (int)(sizeof(add_cbx) / sizeof(*add_cbx)))
+                    add = add_cbx[add_idx];
+                pose[Yaw + i] = orient[axis] + add; // * r2d if it was radians
             }
-            reply->close();
-            reply->deleteLater();
         });
     });
 
     timer.start();
     exec();
-        timer.stop();
+    timer.stop();
 }
 
 void tracker_s2bot::start_tracker(QFrame*)