diff options
| -rw-r--r-- | tracker-s2bot/ftnoir_tracker_s2bot.cpp | 71 |
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*) |