diff options
Diffstat (limited to 'ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.cpp')
| -rw-r--r-- | ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.cpp | 17 |
1 files changed, 15 insertions, 2 deletions
diff --git a/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.cpp b/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.cpp index 295cb65b..a0f3cf88 100644 --- a/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.cpp +++ b/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.cpp @@ -58,6 +58,7 @@ void TrackerImpl::run() { switch (flags) { + //default: case flag_Raw: continue; case flag_Raw | flag_Orient: @@ -76,11 +77,23 @@ void TrackerImpl::run() { 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 d2r = 57.295781; + static constexpr double r2d = 57.295781; for (int i = 0; i < 3; i++) { - pose[Yaw + i] = d2r * orient[order[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); |