summaryrefslogtreecommitdiffhomepage
path: root/tracker-freepie-udp/ftnoir_tracker_freepie-udp.cpp
diff options
context:
space:
mode:
authorStanislaw Halik <sthalik@misaki.pl>2015-10-30 07:37:41 +0100
committerStanislaw Halik <sthalik@misaki.pl>2015-10-30 08:39:32 +0100
commitaa066bdd4622d4f6824fee864f6be6806813f04d (patch)
tree3df328b8b364cba2373a85827191b259bd78d546 /tracker-freepie-udp/ftnoir_tracker_freepie-udp.cpp
parentd6a54431d178632a2bf466c9904f74abd143afe6 (diff)
move to subdirectory-based build system
Closes #224
Diffstat (limited to 'tracker-freepie-udp/ftnoir_tracker_freepie-udp.cpp')
-rw-r--r--tracker-freepie-udp/ftnoir_tracker_freepie-udp.cpp119
1 files changed, 119 insertions, 0 deletions
diff --git a/tracker-freepie-udp/ftnoir_tracker_freepie-udp.cpp b/tracker-freepie-udp/ftnoir_tracker_freepie-udp.cpp
new file mode 100644
index 00000000..12cf9bca
--- /dev/null
+++ b/tracker-freepie-udp/ftnoir_tracker_freepie-udp.cpp
@@ -0,0 +1,119 @@
+#include "ftnoir_tracker_freepie-udp.h"
+#include "opentrack/plugin-api.hpp"
+
+#include <cinttypes>
+#include <algorithm>
+
+TrackerImpl::TrackerImpl() : pose { 0,0,0, 0,0,0 }, should_quit(false)
+{
+}
+
+TrackerImpl::~TrackerImpl()
+{
+ should_quit = true;
+ 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 TrackerImpl::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
+ };
+
+ (void) sock.bind(QHostAddress::Any, (int) s.port, QUdpSocket::ShareAddress | QUdpSocket::ReuseAddressHint);
+
+ while (!should_quit) {
+ int order[] = {
+ bound<int>(s.idx_x, 0, 2),
+ bound<int>(s.idx_y, 0, 2),
+ bound<int>(s.idx_z, 0, 2)
+ };
+ float orient[3];
+ bool filled = false;
+
+ while (sock.hasPendingDatagrams())
+ {
+ using t = decltype(data);
+ t tmp {0,0, {0,0,0, 0,0,0, 0,0,0, 0,0,0}};
+ (void) sock.readDatagram(reinterpret_cast<char*>(&tmp), sizeof(data));
+
+ int flags = tmp.flags & F::Mask;
+
+ switch (flags)
+ {
+ //default:
+ case flag_Raw:
+ continue;
+ case flag_Raw | flag_Orient:
+ for (int i = 0; i < 3; i++)
+ orient[i] = tmp.fl[i+9];
+ break;
+ case flag_Orient:
+ for (int i = 0; i < 3; i++)
+ orient[i] = tmp.fl[i];
+ break;
+ }
+
+ filled = true;
+ data = tmp;
+ }
+
+ if (filled)
+ {
+ static const int add_cbx[] = {
+ 0,
+ 90,
+ -90,
+ 180,
+ -180,
+ };
+ int indices[] = { s.add_yaw, s.add_pitch, s.add_roll };
+ QMutexLocker foo(&mtx);
+ static constexpr double r2d = 57.295781;
+ 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] = r2d * orient[order[i]] + val;
+ }
+ }
+ usleep(4000);
+ }
+}
+
+void TrackerImpl::start_tracker(QFrame*)
+{
+ start();
+ sock.moveToThread(this);
+}
+
+void TrackerImpl::data(double *data)
+{
+ QMutexLocker foo(&mtx);
+
+ data[Yaw] = pose[Yaw];
+ data[Pitch] = pose[Pitch];
+ data[Roll] = pose[Roll];
+}
+
+OPENTRACK_DECLARE_TRACKER(TrackerImpl, TrackerDialog, TrackerMeta)