From c1ce306e50d96d4221b6e7194d3e49c62709db52 Mon Sep 17 00:00:00 2001 From: Attila Csipa Date: Fri, 7 Jul 2017 14:06:19 +0100 Subject: s2bot tracking --- tracker-s2bot/CMakeLists.txt | 1 + tracker-s2bot/ftnoir_tracker_s2bot.cpp | 138 ++++++++++++ tracker-s2bot/ftnoir_tracker_s2bot.h | 72 +++++++ tracker-s2bot/ftnoir_tracker_s2bot_dialog.cpp | 30 +++ tracker-s2bot/lang/nl_NL.ts | 121 +++++++++++ tracker-s2bot/lang/ru_RU.ts | 121 +++++++++++ tracker-s2bot/lang/stub.ts | 121 +++++++++++ tracker-s2bot/s2bot-controls.ui | 300 ++++++++++++++++++++++++++ tracker-s2bot/s2bot-res.qrc | 5 + tracker-s2bot/s2bot.png | Bin 0 -> 3943 bytes 10 files changed, 909 insertions(+) create mode 100644 tracker-s2bot/CMakeLists.txt create mode 100644 tracker-s2bot/ftnoir_tracker_s2bot.cpp create mode 100644 tracker-s2bot/ftnoir_tracker_s2bot.h create mode 100644 tracker-s2bot/ftnoir_tracker_s2bot_dialog.cpp create mode 100644 tracker-s2bot/lang/nl_NL.ts create mode 100644 tracker-s2bot/lang/ru_RU.ts create mode 100644 tracker-s2bot/lang/stub.ts create mode 100644 tracker-s2bot/s2bot-controls.ui create mode 100644 tracker-s2bot/s2bot-res.qrc create mode 100644 tracker-s2bot/s2bot.png diff --git a/tracker-s2bot/CMakeLists.txt b/tracker-s2bot/CMakeLists.txt new file mode 100644 index 00000000..6d162828 --- /dev/null +++ b/tracker-s2bot/CMakeLists.txt @@ -0,0 +1 @@ +otr_module(tracker-s2bot) 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 +#include +#include +#include +#include + +tracker_s2bot::tracker_s2bot() : pose { 0,0,0, 0,0,0 }, m_nam (std::make_unique()) +{ +} + +tracker_s2bot::~tracker_s2bot() +{ + requestInterruption(); + wait(); +} + +template +static const t bound(t datum, t least, t max) +{ + if (datum < least) + return least; + if (datum > max) + return max; + return datum; +} + +#include +#include + +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(s.idx_x, 0, 3), + bound(s.idx_y, 0, 3), + bound(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) diff --git a/tracker-s2bot/ftnoir_tracker_s2bot.h b/tracker-s2bot/ftnoir_tracker_s2bot.h new file mode 100644 index 00000000..d4d3fd2a --- /dev/null +++ b/tracker-s2bot/ftnoir_tracker_s2bot.h @@ -0,0 +1,72 @@ +/* Copyright (c) 2014 Stanislaw Halik + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + */ +#pragma once +#include +#include +#include +#include +#include +#include "ui_s2bot-controls.h" +#include "api/plugin-api.hpp" +#include "options/options.hpp" +using namespace options; + +struct settings : opts { + value freq, idx_x, idx_y, idx_z; + value add_yaw, add_pitch, add_roll; + settings() : + opts("s2bot-tracker"), + freq(b, "freq", 30), + idx_x(b, "axis-index-x", 0), + idx_y(b, "axis-index-y", 1), + idx_z(b, "axis-index-z", 2), + add_yaw(b, "add-yaw-degrees", 0), + add_pitch(b, "add-pitch-degrees", 0), + add_roll(b, "add-roll-degrees", 0) + {} +}; + +class tracker_s2bot : public ITracker, private QThread +{ +public: + tracker_s2bot(); + ~tracker_s2bot() override; + void start_tracker(QFrame *) override; + void data(double *data) override; +protected: + void run() override; +private: + double pose[6]; + QTimer timer; + settings s; + QMutex mtx; + std::unique_ptr m_nam; + +}; + +class dialog_s2bot : public ITrackerDialog +{ + Q_OBJECT +public: + dialog_s2bot(); + void register_tracker(ITracker *) override {} + void unregister_tracker() override {} +private: + Ui::UI_s2bot_dialog ui; + settings s; +private slots: + void doOK(); + void doCancel(); +}; + +class meta_s2bot : public Metadata +{ +public: + QString name() { return QString(QCoreApplication::translate("meta_s2bot", "S2Bot receiver")); } + QIcon icon() { return QIcon(":/s2bot.png"); } +}; + diff --git a/tracker-s2bot/ftnoir_tracker_s2bot_dialog.cpp b/tracker-s2bot/ftnoir_tracker_s2bot_dialog.cpp new file mode 100644 index 00000000..2255156b --- /dev/null +++ b/tracker-s2bot/ftnoir_tracker_s2bot_dialog.cpp @@ -0,0 +1,30 @@ +#include "ftnoir_tracker_s2bot.h" +#include "api/plugin-api.hpp" + +dialog_s2bot::dialog_s2bot() +{ + ui.setupUi(this); + + connect(ui.buttonBox, SIGNAL(accepted()), this, SLOT(doOK())); + connect(ui.buttonBox, SIGNAL(rejected()), this, SLOT(doCancel())); + + tie_setting(s.freq, ui.freq); + tie_setting(s.idx_x, ui.input_x); + tie_setting(s.idx_y, ui.input_y); + tie_setting(s.idx_z, ui.input_z); + + tie_setting(s.add_yaw, ui.add_yaw); + tie_setting(s.add_pitch, ui.add_pitch); + tie_setting(s.add_roll, ui.add_roll); +} + +void dialog_s2bot::doOK() { + s.b->save(); + close(); +} + +void dialog_s2bot::doCancel() +{ + close(); +} + diff --git a/tracker-s2bot/lang/nl_NL.ts b/tracker-s2bot/lang/nl_NL.ts new file mode 100644 index 00000000..dc59f914 --- /dev/null +++ b/tracker-s2bot/lang/nl_NL.ts @@ -0,0 +1,121 @@ + + + + + UI_s2bot_dialog + + + Tracker settings + + + + + UDP port + + + + + Axis order + + + + + output yaw + + + + + + + input yaw + + + + + + + input pitch + + + + + + + input roll + + + + + output pitch + + + + + output roll + + + + + Add to axis + + + + + yaw + + + + + + + 0 + + + + + + + +90 + + + + + + + -90 + + + + + + + +180 + + + + + + + -180 + + + + + pitch + + + + + roll + + + + + meta_freepie + + + FreePIE UDP receiver + + + + diff --git a/tracker-s2bot/lang/ru_RU.ts b/tracker-s2bot/lang/ru_RU.ts new file mode 100644 index 00000000..0505d28d --- /dev/null +++ b/tracker-s2bot/lang/ru_RU.ts @@ -0,0 +1,121 @@ + + + + + UI_s2bot_dialog + + + Tracker settings + + + + + UDP port + + + + + Axis order + + + + + output yaw + + + + + + + input yaw + + + + + + + input pitch + + + + + + + input roll + + + + + output pitch + + + + + output roll + + + + + Add to axis + + + + + yaw + + + + + + + 0 + + + + + + + +90 + + + + + + + -90 + + + + + + + +180 + + + + + + + -180 + + + + + pitch + + + + + roll + + + + + meta_freepie + + + FreePIE UDP receiver + + + + diff --git a/tracker-s2bot/lang/stub.ts b/tracker-s2bot/lang/stub.ts new file mode 100644 index 00000000..3550bdae --- /dev/null +++ b/tracker-s2bot/lang/stub.ts @@ -0,0 +1,121 @@ + + + + + UI_s2bot_dialog + + + Tracker settings + + + + + Frequency + + + + + Axis order + + + + + output yaw + + + + + + + input yaw + + + + + + + input pitch + + + + + + + input roll + + + + + output pitch + + + + + output roll + + + + + Add to axis + + + + + yaw + + + + + + + 0 + + + + + + + +90 + + + + + + + -90 + + + + + + + +180 + + + + + + + -180 + + + + + pitch + + + + + roll + + + + + meta_freepie + + + FreePIE UDP receiver + + + + diff --git a/tracker-s2bot/s2bot-controls.ui b/tracker-s2bot/s2bot-controls.ui new file mode 100644 index 00000000..9124ddd9 --- /dev/null +++ b/tracker-s2bot/s2bot-controls.ui @@ -0,0 +1,300 @@ + + + UI_s2bot_dialog + + + Qt::NonModal + + + + 0 + 0 + 227 + 372 + + + + Tracker settings + + + + ../gui/images/facetracknoir.png../gui/images/facetracknoir.png + + + + + + + 0 + 0 + + + + QFrame::NoFrame + + + QFrame::Raised + + + + + + + 0 + 0 + + + + Update frequency (Hz) + + + + + + + 1 + + + 120 + + + + + + + + + + Axis order + + + + + + output yaw + + + + + + + + input yaw + + + + + input pitch + + + + + input roll + + + + + input bearing + + + + + + + + + input yaw + + + + + input pitch + + + + + input roll + + + + + input bearing + + + + + + + + output pitch + + + + + + + + input yaw + + + + + input pitch + + + + + input roll + + + + + input bearing + + + + + + + + output roll + + + + + + + + + + Add to axis + + + + + + yaw + + + + + + + + 0 + + + + + +90 + + + + + -90 + + + + + +180 + + + + + -180 + + + + + + + + pitch + + + + + + + roll + + + + + + + + 0 + + + + + +90 + + + + + -90 + + + + + +180 + + + + + -180 + + + + + + + + + 0 + + + + + +90 + + + + + -90 + + + + + +180 + + + + + -180 + + + + + + + + + + + QDialogButtonBox::Cancel|QDialogButtonBox::Ok + + + + + + + + + startEngineClicked() + stopEngineClicked() + cameraSettingsClicked() + + diff --git a/tracker-s2bot/s2bot-res.qrc b/tracker-s2bot/s2bot-res.qrc new file mode 100644 index 00000000..40fc3aa9 --- /dev/null +++ b/tracker-s2bot/s2bot-res.qrc @@ -0,0 +1,5 @@ + + + s2bot.png + + diff --git a/tracker-s2bot/s2bot.png b/tracker-s2bot/s2bot.png new file mode 100644 index 00000000..1501bf13 Binary files /dev/null and b/tracker-s2bot/s2bot.png differ -- cgit v1.2.3