summaryrefslogtreecommitdiffhomepage
path: root/ftnoir_tracker_freepie-udp
diff options
context:
space:
mode:
authorStanislaw Halik <sthalik@misaki.pl>2014-09-27 22:22:20 +0200
committerStanislaw Halik <sthalik@misaki.pl>2014-09-28 00:59:09 +0200
commit2843048fcb63485989bfc82d47280c1a5c94d689 (patch)
tree8897287bafb8028ef740a71d2f7d8f445425eb37 /ftnoir_tracker_freepie-udp
parent64c4efe12e7cc1739551578b242b73481b99931b (diff)
fix FreePIE udp sender finally
With enormous thanks to @KyokushinPL for resources invested. Issue: #48
Diffstat (limited to 'ftnoir_tracker_freepie-udp')
-rw-r--r--ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.cpp63
1 files changed, 21 insertions, 42 deletions
diff --git a/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.cpp b/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.cpp
index 26726b18..ad505229 100644
--- a/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.cpp
+++ b/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.cpp
@@ -15,73 +15,59 @@ TrackerImpl::~TrackerImpl()
}
void TrackerImpl::run() {
+#pragma pack(push, 1)
struct {
- uint8_t pad;
+ uint8_t pad1;
uint8_t flags;
- union {
- float rot[6];
- struct {
- float pad[9];
- float rot[6];
- } raw_rot;
- };
+ uint8_t pad2;
+ float fl[12];
} data;
-
+#pragma pack(pop)
enum F {
flag_Raw = 1 << 0,
flag_Orient = 1 << 1,
Mask = flag_Raw | flag_Orient
};
- struct check {
- union {
- std::uint16_t half;
- unsigned char bytes[2];
- };
- bool convertp;
- check() : bytes { 0, 255 }, convertp(half > 255) {}
- } crapola;
-
while (1) {
if (should_quit)
break;
- float* orient = nullptr;
+ float orient[3];
+ bool filled = false;
while (sock.hasPendingDatagrams())
{
using t = decltype(data);
- t tmp {0,0, {0,0,0, 0,0,0}};
+ t tmp {0,0,0, {0,0,0, 0,0,0}};
(void) sock.readDatagram(reinterpret_cast<char*>(&tmp), sizeof(data));
- int flags = data.flags & F::Mask;
+
+ int flags = tmp.flags & F::Mask;
switch (flags)
{
case flag_Raw:
continue;
case flag_Raw | flag_Orient:
- orient = data.raw_rot.rot;
+ for (int i = 0; i < 3; i++)
+ orient[i] = tmp.fl[i+9];
break;
case flag_Orient:
- orient = data.rot;
+ for (int i = 0; i < 3; i++)
+ orient[i] = tmp.fl[i];
break;
}
+
+ filled = true;
data = tmp;
}
- if (orient)
- {
- if (crapola.convertp)
- {
- unsigned char* alias = reinterpret_cast<unsigned char*>(orient);
- constexpr int sz = sizeof(float[6]);
- std::reverse(alias, alias + sz);
- }
+ if (filled)
+ {
QMutexLocker foo(&mtx);
+ static constexpr double d2r = 57.295781;
for (int i = 0; i < 3; i++)
- pose[Yaw + i] = orient[i];
-
- orient = nullptr;
+ pose[Yaw + i] = d2r * orient[i];
}
usleep(4000);
}
@@ -96,14 +82,7 @@ void TrackerImpl::StartTracker(QFrame*)
void TrackerImpl::GetHeadPoseData(double *data)
{
QMutexLocker foo(&mtx);
-#if 0
- if (s.enable_x)
- data[TX] = pose[TX];
- if (s.enable_y)
- data[TY] = pose[TY];
- if (s.enable_z)
- data[TZ] = pose[TZ];
-#endif
+
data[Yaw] = pose[Yaw];
data[Pitch] = pose[Pitch];
data[Roll] = pose[Roll];