summaryrefslogtreecommitdiffhomepage
path: root/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.cpp')
-rw-r--r--ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.cpp17
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);