s2bot tracking

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)