summaryrefslogtreecommitdiffhomepage
path: root/tracker-s2bot/ftnoir_tracker_s2bot.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'tracker-s2bot/ftnoir_tracker_s2bot.cpp')
-rw-r--r--tracker-s2bot/ftnoir_tracker_s2bot.cpp138
1 files changed, 138 insertions, 0 deletions
diff --git a/tracker-s2bot/ftnoir_tracker_s2bot.cpp b/tracker-s2bot/ftnoir_tracker_s2bot.cpp
new file mode 100644
index 00000000..c33ac9c5
--- /dev/null
+++ b/tracker-s2bot/ftnoir_tracker_s2bot.cpp
@@ -0,0 +1,138 @@
+#include "ftnoir_tracker_s2bot.h"
+#include "api/plugin-api.hpp"
+
+#include <cinttypes>
+#include <algorithm>
+#include <cmath>
+#include <QNetworkRequest>
+#include <QNetworkReply>
+
+tracker_s2bot::tracker_s2bot() : pose { 0,0,0, 0,0,0 }, m_nam (std::make_unique<QNetworkAccessManager>())
+{
+}
+
+tracker_s2bot::~tracker_s2bot()
+{
+ 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;
+}
+
+#include <QFile>
+#include <QTextStream>
+
+void logtext(QString txt) {
+ //QFile caFile("c:/tools/log.txt");
+ //caFile.open(QIODevice::WriteOnly | QIODevice::Text | QIODevice::Append);
+ //QTextStream outStream(&caFile);
+ //outStream << txt;
+ //caFile.close();
+}
+
+void tracker_s2bot::run() {
+#pragma pack(push, 1)
+ struct {
+ uint8_t pad1;
+ uint8_t flags;
+ float fl[12];
+ } data;
+#pragma pack(pop)
+ enum F {
+ flag_Raw = 1 << 0,
+ flag_Orient = 1 << 1,
+ Mask = flag_Raw | flag_Orient
+ };
+
+ logtext("GO");
+
+ if (s.freq == 0) s.freq = 10;
+ timer.setInterval(1000.0/s.freq);
+ timer.setSingleShot(false);
+ connect(&timer, &QTimer::timeout, [this]() {
+ logtext("timer ");
+ auto reply = m_nam->get(QNetworkRequest(QUrl("http://localhost:17317/poll")));
+ connect(reply, &QNetworkReply::finished, [this, reply]() {
+ logtext("reply ");
+ if (reply->error() == QNetworkReply::NoError) {
+ qDebug() << "Request submitted OK";
+ }
+ else {
+ qWarning() << "Request bounced:" << reply->attribute(QNetworkRequest::HttpStatusCodeAttribute) << reply->errorString();
+ return;
+ }
+ QByteArray ba = reply->readAll();
+ logtext("\nlength " + ba.length());
+ 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)
+ };
+ logtext(QString::number(s.idx_x()) + "\n\r");
+ logtext(QString::number(order[0]) + "\n\r");
+ double orient[4] = { 0, 0, 0, 0 };
+ static const 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(" ");
+ if (keyval.count() < 2) continue;
+ if (keyval[0].startsWith("accelerometerX")) orient[0] = keyval[1].toInt();
+ else if (keyval[0].startsWith("accelerometerY")) orient[1] = keyval[1].toInt();
+ else if (keyval[0].startsWith("accelerometerZ")) orient[2] = keyval[1].toInt();
+ else if (keyval[0].startsWith("bearing")) orient[3] = keyval[1].toInt();
+ }
+ logtext("RAW: " + QString::number(orient[0]) + " " + QString::number(orient[1]) + " " + QString::number(orient[2]) + " " + QString::number(orient[3]) + "\n\r");
+ QMutexLocker foo(&mtx);
+ static 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] = orient[order[i]] + val; // * r2d if it was radians
+ }
+ logtext("Processed: " + QString::number(pose[Yaw]) + " " + QString::number(pose[Yaw + 1]) + " " + QString::number(pose[Yaw + 2]) + "\n\r");
+
+
+
+ reply->close();
+ reply->deleteLater();
+ });
+ });
+ timer.start();
+ exec();
+ //sock.bind(QHostAddress::Any, (unsigned short) s.freq, QUdpSocket::ShareAddress | QUdpSocket::ReuseAddressHint);
+}
+
+void tracker_s2bot::start_tracker(QFrame*)
+{
+ start();
+ timer.moveToThread(this);
+}
+
+void tracker_s2bot::data(double *data)
+{
+ QMutexLocker foo(&mtx);
+
+ data[Yaw] = pose[Yaw];
+ data[Pitch] = pose[Pitch];
+ data[Roll] = pose[Roll];
+}
+
+OPENTRACK_DECLARE_TRACKER(tracker_s2bot, dialog_s2bot, meta_s2bot)