summaryrefslogtreecommitdiffhomepage
path: root/tracker-s2bot
diff options
context:
space:
mode:
Diffstat (limited to 'tracker-s2bot')
-rw-r--r--tracker-s2bot/ftnoir_tracker_s2bot.cpp71
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*)