add rotation value addition to freepie-udp

Otherwise gimbal lock singularities occur.

Tested-by: @KyokushinPL

4 files changed, 149 insertions, 5 deletions

diff --git a/ftnoir_tracker_freepie-udp/freepie-udp-controls.ui b/ftnoir_tracker_freepie-udp/freepie-udp-controls.ui
index c0b1de8a..74e5a6f1 100644
--- a/ftnoir_tracker_freepie-udp/freepie-udp-controls.ui
+++ b/ftnoir_tracker_freepie-udp/freepie-udp-controls.ui
@@ -9,8 +9,8 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>302</width>
-    <height>231</height>
+    <width>227</width>
+    <height>372</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -23,6 +23,12 @@
   <layout class="QVBoxLayout" name="verticalLayout">
    <item>
     <widget class="QFrame" name="frame">
+     <property name="sizePolicy">
+      <sizepolicy hsizetype="Preferred" vsizetype="Maximum">
+       <horstretch>0</horstretch>
+       <verstretch>0</verstretch>
+      </sizepolicy>
+     </property>
      <property name="frameShape">
       <enum>QFrame::NoFrame</enum>
      </property>
@@ -144,6 +150,123 @@
     </widget>
    </item>
    <item>
+    <widget class="QGroupBox" name="groupBox_2">
+     <property name="title">
+      <string>Add to axis</string>
+     </property>
+     <layout class="QGridLayout" name="gridLayout_2">
+      <item row="0" column="0">
+       <widget class="QLabel" name="label_4">
+        <property name="text">
+         <string>yaw</string>
+        </property>
+       </widget>
+      </item>
+      <item row="0" column="1">
+       <widget class="QComboBox" name="add_yaw">
+        <item>
+         <property name="text">
+          <string>0</string>
+         </property>
+        </item>
+        <item>
+         <property name="text">
+          <string>+90</string>
+         </property>
+        </item>
+        <item>
+         <property name="text">
+          <string>-90</string>
+         </property>
+        </item>
+        <item>
+         <property name="text">
+          <string>+180</string>
+         </property>
+        </item>
+        <item>
+         <property name="text">
+          <string>-180</string>
+         </property>
+        </item>
+       </widget>
+      </item>
+      <item row="1" column="0">
+       <widget class="QLabel" name="label_6">
+        <property name="text">
+         <string>pitch</string>
+        </property>
+       </widget>
+      </item>
+      <item row="2" column="0">
+       <widget class="QLabel" name="label_9">
+        <property name="text">
+         <string>roll</string>
+        </property>
+       </widget>
+      </item>
+      <item row="1" column="1">
+       <widget class="QComboBox" name="add_pitch">
+        <item>
+         <property name="text">
+          <string>0</string>
+         </property>
+        </item>
+        <item>
+         <property name="text">
+          <string>+90</string>
+         </property>
+        </item>
+        <item>
+         <property name="text">
+          <string>-90</string>
+         </property>
+        </item>
+        <item>
+         <property name="text">
+          <string>+180</string>
+         </property>
+        </item>
+        <item>
+         <property name="text">
+          <string>-180</string>
+         </property>
+        </item>
+       </widget>
+      </item>
+      <item row="2" column="1">
+       <widget class="QComboBox" name="add_roll">
+        <item>
+         <property name="text">
+          <string>0</string>
+         </property>
+        </item>
+        <item>
+         <property name="text">
+          <string>+90</string>
+         </property>
+        </item>
+        <item>
+         <property name="text">
+          <string>-90</string>
+         </property>
+        </item>
+        <item>
+         <property name="text">
+          <string>+180</string>
+         </property>
+        </item>
+        <item>
+         <property name="text">
+          <string>-180</string>
+         </property>
+        </item>
+       </widget>
+      </item>
+     </layout>
+    </widget>
+   </item>
+   <item>
     <widget class="QDialogButtonBox" name="buttonBox">
      <property name="standardButtons">
       <set>QDialogButtonBox::Cancel|QDialogButtonBox::Ok</set>
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);
diff --git a/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.h b/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.h
index 2dfb4e8f..d56c9f52 100644
--- a/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.h
+++ b/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.h
@@ -16,12 +16,16 @@ using namespace options;
 struct settings {
     pbundle b;
     value<int> port, idx_x, idx_y, idx_z;
+    value<int> add_yaw, add_pitch, add_roll;
     settings() :
         b(bundle("freepie-udp-tracker")),
         port(b, "port", 4237),
         idx_x(b, "axis-index-x", 0),
         idx_y(b, "axis-index-y", 1),
-        idx_z(b, "axis-index-z", 2)
+        idx_z(b, "axis-index-z", 2),
+        add_yaw(b, "add-yaw-degrees", 0),
+        add_pitch(b, "add-pitch-degrees", 0),
+        add_roll(b, "add-roll-degrees", 0)
     {}
 };
 
diff --git a/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp_dialog.cpp b/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp_dialog.cpp
index 4840a56b..1f17355f 100644
--- a/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp_dialog.cpp
+++ b/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp_dialog.cpp
@@ -12,6 +12,10 @@ TrackerDialog::TrackerDialog()
     tie_setting(s.idx_x, ui.input_x);
     tie_setting(s.idx_y, ui.input_y);
     tie_setting(s.idx_z, ui.input_z);
+
+    tie_setting(s.add_yaw, ui.add_yaw);
+    tie_setting(s.add_pitch, ui.add_pitch);
+    tie_setting(s.add_roll, ui.add_roll);
 }
 
 void TrackerDialog::doOK() {