From 513f93b01ac22842746f493c23bb48eb36071dc5 Mon Sep 17 00:00:00 2001
From: Stanislaw Halik <sthalik@misaki.pl>
Date: Fri, 1 Nov 2013 23:16:11 +0100
Subject: spring v2: magic nums -> ui, squash uninit mem access

-sthalik&mm0zct
---
 ftnoir_tracker_rift/ftnoir_rift_clientcontrols.ui  | 110 +++++++++++++++++----
 ftnoir_tracker_rift/ftnoir_tracker_rift.cpp        |  17 +++-
 ftnoir_tracker_rift/ftnoir_tracker_rift.h          |   3 +-
 ftnoir_tracker_rift/ftnoir_tracker_rift_dialog.cpp |   8 ++
 4 files changed, 118 insertions(+), 20 deletions(-)

diff --git a/ftnoir_tracker_rift/ftnoir_rift_clientcontrols.ui b/ftnoir_tracker_rift/ftnoir_rift_clientcontrols.ui
index 9c3ec53a..38e745c9 100644
--- a/ftnoir_tracker_rift/ftnoir_rift_clientcontrols.ui
+++ b/ftnoir_tracker_rift/ftnoir_rift_clientcontrols.ui
@@ -9,10 +9,16 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>176</width>
-    <height>145</height>
+    <width>195</width>
+    <height>265</height>
    </rect>
   </property>
+  <property name="sizePolicy">
+   <sizepolicy hsizetype="Minimum" vsizetype="Minimum">
+    <horstretch>0</horstretch>
+    <verstretch>0</verstretch>
+   </sizepolicy>
+  </property>
   <property name="windowTitle">
    <string>Oculus Rift tracker settings FaceTrackNoIR</string>
   </property>
@@ -26,21 +32,9 @@
   <property name="autoFillBackground">
    <bool>false</bool>
   </property>
-  <layout class="QFormLayout" name="formLayout">
-   <item row="0" column="0" colspan="2">
+  <layout class="QVBoxLayout" name="verticalLayout">
+   <item>
     <widget class="QGroupBox" name="groupBox_3">
-     <property name="sizePolicy">
-      <sizepolicy hsizetype="Preferred" vsizetype="Expanding">
-       <horstretch>0</horstretch>
-       <verstretch>0</verstretch>
-      </sizepolicy>
-     </property>
-     <property name="minimumSize">
-      <size>
-       <width>0</width>
-       <height>0</height>
-      </size>
-     </property>
      <property name="title">
       <string>Enable Axis</string>
      </property>
@@ -159,7 +153,89 @@
      </layout>
     </widget>
    </item>
-   <item row="1" column="1">
+   <item>
+    <widget class="QGroupBox" name="groupBox">
+     <property name="title">
+      <string>Yaw spring</string>
+     </property>
+     <layout class="QFormLayout" name="formLayout">
+      <item row="0" column="0">
+       <widget class="QCheckBox" name="yawSpring">
+        <property name="text">
+         <string>Enable</string>
+        </property>
+       </widget>
+      </item>
+      <item row="1" column="0">
+       <widget class="QLabel" name="label">
+        <property name="text">
+         <string>Persistence</string>
+        </property>
+       </widget>
+      </item>
+      <item row="1" column="1">
+       <widget class="QDoubleSpinBox" name="persistence">
+        <property name="decimals">
+         <number>5</number>
+        </property>
+        <property name="minimum">
+         <double>0.900000000000000</double>
+        </property>
+        <property name="maximum">
+         <double>1.000000000000000</double>
+        </property>
+        <property name="singleStep">
+         <double>0.001000000000000</double>
+        </property>
+       </widget>
+      </item>
+      <item row="2" column="0">
+       <widget class="QLabel" name="label_2">
+        <property name="text">
+         <string>Constant drift</string>
+        </property>
+       </widget>
+      </item>
+      <item row="2" column="1">
+       <widget class="QDoubleSpinBox" name="constantDrift">
+        <property name="decimals">
+         <number>5</number>
+        </property>
+        <property name="minimum">
+         <double>0.000100000000000</double>
+        </property>
+        <property name="maximum">
+         <double>0.100000000000000</double>
+        </property>
+        <property name="singleStep">
+         <double>0.001000000000000</double>
+        </property>
+       </widget>
+      </item>
+      <item row="3" column="0">
+       <widget class="QLabel" name="label_3">
+        <property name="text">
+         <string>Deadzone</string>
+        </property>
+       </widget>
+      </item>
+      <item row="3" column="1">
+       <widget class="QDoubleSpinBox" name="deadzone">
+        <property name="decimals">
+         <number>5</number>
+        </property>
+        <property name="maximum">
+         <double>0.100000000000000</double>
+        </property>
+        <property name="singleStep">
+         <double>0.010000000000000</double>
+        </property>
+       </widget>
+      </item>
+     </layout>
+    </widget>
+   </item>
+   <item>
     <widget class="QDialogButtonBox" name="buttonBox">
      <property name="sizePolicy">
       <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
diff --git a/ftnoir_tracker_rift/ftnoir_tracker_rift.cpp b/ftnoir_tracker_rift/ftnoir_tracker_rift.cpp
index 7d567ca3..dc0892fe 100644
--- a/ftnoir_tracker_rift/ftnoir_tracker_rift.cpp
+++ b/ftnoir_tracker_rift/ftnoir_tracker_rift.cpp
@@ -21,6 +21,7 @@ Rift_Tracker::Rift_Tracker()
 	pHMD = NULL;
 	pSensor = NULL;
 	pSFusion = NULL;
+    old_yaw = 0;
 }
 
 Rift_Tracker::~Rift_Tracker()
@@ -87,9 +88,17 @@ bool Rift_Tracker::GiveHeadPoseData(double *data)
         float pitch = 0.0f;
         float roll = 0.0f;
         hmdOrient.GetEulerAngles<Axis_Y, Axis_X, Axis_Z>(&yaw, &pitch , &roll);
-        newHeadPose[Yaw] = yaw;
-        newHeadPose[Pitch] =pitch;
+        newHeadPose[Pitch] = pitch;
         newHeadPose[Roll] = roll;
+        newHeadPose[Yaw] = yaw;
+        if (useYawSpring)
+        {
+            newHeadPose[Yaw] = newHeadPose[Yaw]*persistence + (yaw-old_yaw);
+            if(newHeadPose[Yaw]>deadzone)newHeadPose[Yaw]-= constant_drift;
+            if(newHeadPose[Yaw]<-deadzone)newHeadPose[Yaw]+= constant_drift;
+            old_yaw=yaw;
+        }
+        else
 
 #if 0
         newHeadPose[TX] = acd.controllers[0].pos[0]/50.0f;
@@ -142,6 +151,10 @@ void Rift_Tracker::loadSettings() {
     bEnableY = iniFile.value ( "EnableY", 1 ).toBool();
     bEnableZ = iniFile.value ( "EnableZ", 1 ).toBool();
 #endif
+    useYawSpring = iniFile.value("yaw-spring", false).toBool();
+    constant_drift = iniFile.value("constant-drift", 0.000005).toDouble();
+    persistence = iniFile.value("persistence", 0.99999).toDouble();
+    deadzone = iniFile.value("deadzone", 0.02).toDouble();
     iniFile.endGroup ();
 }
 
diff --git a/ftnoir_tracker_rift/ftnoir_tracker_rift.h b/ftnoir_tracker_rift/ftnoir_tracker_rift.h
index 55fa99e3..bd295838 100644
--- a/ftnoir_tracker_rift/ftnoir_tracker_rift.h
+++ b/ftnoir_tracker_rift/ftnoir_tracker_rift.h
@@ -35,7 +35,8 @@ private:
 	bool bEnableY;
 	bool bEnableZ;
 #endif
-
+    bool useYawSpring;
+    double old_yaw, constant_drift, persistence, deadzone;
 };
 
 // Widget that has controls for FTNoIR protocol client-settings.
diff --git a/ftnoir_tracker_rift/ftnoir_tracker_rift_dialog.cpp b/ftnoir_tracker_rift/ftnoir_tracker_rift_dialog.cpp
index 6b1a559d..2efefbb3 100644
--- a/ftnoir_tracker_rift/ftnoir_tracker_rift_dialog.cpp
+++ b/ftnoir_tracker_rift/ftnoir_tracker_rift_dialog.cpp
@@ -117,6 +117,10 @@ void TrackerControls::loadSettings() {
 	ui.chkEnableY->setChecked(iniFile.value ( "EnableY", 1 ).toBool());
 	ui.chkEnableZ->setChecked(iniFile.value ( "EnableZ", 1 ).toBool());
 #endif
+    ui.yawSpring->setChecked(iniFile.value("yaw-spring", true).toBool());
+    ui.deadzone->setValue(iniFile.value("deadzone", 0.02).toDouble());
+    ui.constantDrift->setValue(iniFile.value("constant-drift", 0.000005).toDouble());
+    ui.persistence->setValue(iniFile.value("persistence", 0.9999).toDouble());
 	iniFile.endGroup ();
 
 	settingsDirty = false;
@@ -140,6 +144,10 @@ void TrackerControls::save() {
 	iniFile.setValue ( "EnableY", ui.chkEnableY->isChecked() );
 	iniFile.setValue ( "EnableZ", ui.chkEnableZ->isChecked() );
 #endif
+    iniFile.setValue("yaw-spring", ui.yawSpring->isChecked());
+    iniFile.setValue("deadzone", ui.deadzone->value());
+    iniFile.setValue("constant-drift", ui.constantDrift->value());
+    iniFile.setValue("persistence", ui.persistence->value());
 	iniFile.endGroup ();
 
 	settingsDirty = false;
-- 
cgit v1.2.3