From 7f2e427584c469c47e071da5a7351210a3bca7fc Mon Sep 17 00:00:00 2001
From: Anthony Thibault <tony@highfidelity.io>
Date: Mon, 3 Dec 2018 18:56:45 -0800
Subject: [PATCH] Added DropAfterDelay strategy to Vive Tracker Calibration
 Dialog

This is a hybrid of None and Drop strategies.
Basically, if the puck is out of range for less then 1/2 a second we still use it, however any longer then that and we mark it as invalid.
---
 .../qml/hifi/tablet/OpenVrConfiguration.qml   |  2 +-
 plugins/openvr/src/ViveControllerManager.cpp  | 23 ++++++++++++++++++-
 plugins/openvr/src/ViveControllerManager.h    |  5 +++-
 3 files changed, 27 insertions(+), 3 deletions(-)

diff --git a/interface/resources/qml/hifi/tablet/OpenVrConfiguration.qml b/interface/resources/qml/hifi/tablet/OpenVrConfiguration.qml
index e18fdea444..4ce9fe7c84 100644
--- a/interface/resources/qml/hifi/tablet/OpenVrConfiguration.qml
+++ b/interface/resources/qml/hifi/tablet/OpenVrConfiguration.qml
@@ -871,7 +871,7 @@ Flickable {
 
                     editable: true
                     colorScheme: hifi.colorSchemes.dark
-                    model: ["None", "Freeze", "Drop"]
+                    model: ["None", "Freeze", "Drop", "DropAfterDelay"]
                     label: ""
 
                     onCurrentIndexChanged: {
diff --git a/plugins/openvr/src/ViveControllerManager.cpp b/plugins/openvr/src/ViveControllerManager.cpp
index af4f4da18c..ff1f10f8ed 100644
--- a/plugins/openvr/src/ViveControllerManager.cpp
+++ b/plugins/openvr/src/ViveControllerManager.cpp
@@ -138,6 +138,8 @@ static QString outOfRangeDataStrategyToString(ViveControllerManager::OutOfRangeD
         return "Freeze";
     case ViveControllerManager::OutOfRangeDataStrategy::Drop:
         return "Drop";
+    case ViveControllerManager::OutOfRangeDataStrategy::DropAfterDelay:
+        return "DropAfterDelay";
     }
 }
 
@@ -146,6 +148,8 @@ static ViveControllerManager::OutOfRangeDataStrategy stringToOutOfRangeDataStrat
         return ViveControllerManager::OutOfRangeDataStrategy::Drop;
     } else if (string == "Freeze") {
         return ViveControllerManager::OutOfRangeDataStrategy::Freeze;
+    } else if (string == "DropAfterDelay") {
+        return ViveControllerManager::OutOfRangeDataStrategy::DropAfterDelay;
     } else {
         return ViveControllerManager::OutOfRangeDataStrategy::None;
     }
@@ -302,7 +306,7 @@ void ViveControllerManager::loadSettings() {
         if (_inputDevice) {
             const double DEFAULT_ARM_CIRCUMFERENCE = 0.33;
             const double DEFAULT_SHOULDER_WIDTH = 0.48;
-            const QString DEFAULT_OUT_OF_RANGE_STRATEGY = "Drop";
+            const QString DEFAULT_OUT_OF_RANGE_STRATEGY = "DropAfterDelay";
             _inputDevice->_armCircumference = settings.value("armCircumference", QVariant(DEFAULT_ARM_CIRCUMFERENCE)).toDouble();
             _inputDevice->_shoulderWidth = settings.value("shoulderWidth", QVariant(DEFAULT_SHOULDER_WIDTH)).toDouble();
             _inputDevice->_outOfRangeDataStrategy = stringToOutOfRangeDataStrategy(settings.value("outOfRangeDataStrategy", QVariant(DEFAULT_OUT_OF_RANGE_STRATEGY)).toString());
@@ -516,6 +520,7 @@ void ViveControllerManager::InputDevice::handleTrackedObject(uint32_t deviceInde
         _nextSimPoseData.vrPoses[deviceIndex].bPoseIsValid &&
         poseIndex <= controller::TRACKED_OBJECT_15) {
 
+        uint64_t now = usecTimestampNow();
         controller::Pose pose;
         switch (_outOfRangeDataStrategy) {
         case OutOfRangeDataStrategy::Drop:
@@ -544,6 +549,22 @@ void ViveControllerManager::InputDevice::handleTrackedObject(uint32_t deviceInde
                 _nextSimPoseData.angularVelocities[deviceIndex] = _lastSimPoseData.angularVelocities[deviceIndex];
             }
             break;
+        case OutOfRangeDataStrategy::DropAfterDelay:
+            const uint64_t DROP_DELAY_TIME = 500 * USECS_PER_MSEC;
+
+            // All Running_OK results are valid.
+            if (_nextSimPoseData.vrPoses[deviceIndex].eTrackingResult == vr::TrackingResult_Running_OK) {
+                pose = buildPose(_nextSimPoseData.poses[deviceIndex], _nextSimPoseData.linearVelocities[deviceIndex], _nextSimPoseData.angularVelocities[deviceIndex]);
+                // update the timer
+                _simDataRunningOkTimestampMap[deviceIndex] = now;
+            } else if (now - _simDataRunningOkTimestampMap[deviceIndex] < DROP_DELAY_TIME) {
+                // report the pose, even though pose is out-of-range
+                pose = buildPose(_nextSimPoseData.poses[deviceIndex], _nextSimPoseData.linearVelocities[deviceIndex], _nextSimPoseData.angularVelocities[deviceIndex]);
+            } else {
+                // this pose has been out-of-range for too long.
+                pose.valid = false;
+            }
+            break;
         }
 
         if (pose.valid) {
diff --git a/plugins/openvr/src/ViveControllerManager.h b/plugins/openvr/src/ViveControllerManager.h
index 647702ae77..dbd248dc53 100644
--- a/plugins/openvr/src/ViveControllerManager.h
+++ b/plugins/openvr/src/ViveControllerManager.h
@@ -63,7 +63,8 @@ public:
     enum class OutOfRangeDataStrategy {
         None,
         Freeze,
-        Drop
+        Drop,
+        DropAfterDelay
     };
 
 private:
@@ -205,6 +206,8 @@ private:
 
         bool _hmdTrackingEnabled { true };
 
+        std::map<uint32_t, uint64_t> _simDataRunningOkTimestampMap;
+
         QString configToString(Config config);
         friend class ViveControllerManager;
     };