diff options
Diffstat (limited to 'tracker-freepie-udp/ftnoir_tracker_freepie-udp.cpp')
| -rw-r--r-- | tracker-freepie-udp/ftnoir_tracker_freepie-udp.cpp | 16 |
1 files changed, 10 insertions, 6 deletions
diff --git a/tracker-freepie-udp/ftnoir_tracker_freepie-udp.cpp b/tracker-freepie-udp/ftnoir_tracker_freepie-udp.cpp index a93a22cb..3950af0c 100644 --- a/tracker-freepie-udp/ftnoir_tracker_freepie-udp.cpp +++ b/tracker-freepie-udp/ftnoir_tracker_freepie-udp.cpp @@ -25,6 +25,8 @@ void tracker_freepie::run() { } data; #pragma pack(pop) + static_assert(sizeof(data) == 50); + enum F { flag_Raw = 1 << 0, @@ -38,9 +40,9 @@ void tracker_freepie::run() { { int order[] = { - clamp(s.idx_x, 0, 2), - clamp(s.idx_y, 0, 2), - clamp(s.idx_z, 0, 2) + std::clamp(*s.idx_x, 0, 2), + std::clamp(*s.idx_y, 0, 2), + std::clamp(*s.idx_z, 0, 2) }; double orient[3] = {0, 0, 0}; @@ -49,7 +51,7 @@ void tracker_freepie::run() { while (sock.hasPendingDatagrams()) { using t = decltype(data); - t tmp {0,0, {0,0,0, 0,0,0, 0,0,0, 0,0,0}}; + t tmp {}; (void) sock.readDatagram(reinterpret_cast<char*>(&tmp), sizeof(data)); int flags = tmp.flags & F::Mask; @@ -61,12 +63,13 @@ void tracker_freepie::run() { continue; case flag_Raw | flag_Orient: for (int i = 0; i < 3; i++) - orient[i] = tmp.fl[i+9]; + orient[i] = (double)tmp.fl[i+9]; break; case flag_Orient: for (int i = 0; i < 3; i++) - orient[i] = tmp.fl[i]; + orient[i] = (double)tmp.fl[i]; break; + default: goto fail; } filled = true; @@ -100,6 +103,7 @@ void tracker_freepie::run() { pose[Yaw + i] = r2d * orient[axis] + add; } } +fail: usleep(4000); } } |