From 8250f9be5cd0fed27d037d4e8fe5aafb4c99796e Mon Sep 17 00:00:00 2001
From: Travis Nickles <nickles.travis@gmail.com>
Date: Wed, 26 Jul 2017 04:30:50 -0500
Subject: [PATCH] Attempt to use timestamp from ds4. Preserve changes.

---
 DS4Windows/DS4Control/MouseCursor.cs |  3 ++-
 DS4Windows/DS4Library/DS4Device.cs   | 30 +++++++++++++++++++++++++++-
 DS4Windows/DS4Library/DS4State.cs    |  4 ++++
 3 files changed, 35 insertions(+), 2 deletions(-)

diff --git a/DS4Windows/DS4Control/MouseCursor.cs b/DS4Windows/DS4Control/MouseCursor.cs
index c049895..98ae266 100644
--- a/DS4Windows/DS4Control/MouseCursor.cs
+++ b/DS4Windows/DS4Control/MouseCursor.cs
@@ -42,7 +42,8 @@ namespace DS4Windows
             deltaX = Global.getGyroMouseHorizontalAxis(deviceNumber) == 0 ? arg.sixAxis.gyroYawFull :
                 arg.sixAxis.gyroRollFull;
             deltaY = -arg.sixAxis.gyroPitchFull;
-            tempDouble = arg.sixAxis.elapsed * 0.001 * 200.0; // Base default speed on 5 ms
+            //tempDouble = arg.sixAxis.elapsed * 0.001 * 200.0; // Base default speed on 5 ms
+            tempDouble = arg.sixAxis.elapsed * 200.0; // Base default speed on 5 ms
 
             gyroSmooth = Global.getGyroSmoothing(deviceNumber);
             double gyroSmoothWeight = 0.0;
diff --git a/DS4Windows/DS4Library/DS4Device.cs b/DS4Windows/DS4Library/DS4Device.cs
index abcbf64..5c61c30 100644
--- a/DS4Windows/DS4Library/DS4Device.cs
+++ b/DS4Windows/DS4Library/DS4Device.cs
@@ -644,6 +644,10 @@ namespace DS4Windows
         bool ds4InactiveFrame = true;
         bool idleInput = true;
 
+        bool timeStampInit = false;
+        uint timeStampPrevious = 0;
+        uint deltaTimeCurrent = 0;
+
         private void performDs4Input()
         {
             firstActive = DateTime.UtcNow;
@@ -661,6 +665,9 @@ namespace DS4Windows
 
             int maxBatteryValue = 0;
             int tempBattery = 0;
+            uint tempStamp = 0;
+            double elapsedDeltaTime = 0.0;
+            uint tempDelta = 0;
 
             while (!exitInputThread)
             {
@@ -848,10 +855,31 @@ namespace DS4Windows
                 }
                 catch { currerror = "Index out of bounds: touchpad"; }
 
+                tempStamp = (uint)((ushort)(inputReport[11] << 8) | inputReport[10]);
+                if (timeStampInit == false)
+                {
+                    timeStampInit = true;
+                    deltaTimeCurrent = tempStamp * 16u / 3u;
+                }
+                else if (timeStampPrevious > tempStamp)
+                {
+                    tempDelta = ushort.MaxValue - timeStampPrevious + tempStamp + 1u;
+                    deltaTimeCurrent = tempDelta * 16u / 3u;
+                }
+                else
+                {
+                    tempDelta = tempStamp - timeStampPrevious;
+                    deltaTimeCurrent = tempDelta * 16u / 3u;
+                }
+
+                cState.elapsedNanoSec = deltaTimeCurrent;
+                timeStampPrevious = tempStamp;
+                elapsedDeltaTime = 0.000001 * deltaTimeCurrent; // Convert from nanoseconds to seconds
+
                 // Store Gyro and Accel values
                 Array.Copy(inputReport, 13, gyro, 0, 6);
                 Array.Copy(inputReport, 19, accel, 0, 6);
-                sixAxis.handleSixaxis(gyro, accel, cState, lastTimeElapsedDouble);
+                sixAxis.handleSixaxis(gyro, accel, cState, elapsedDeltaTime);
 
                 /* Debug output of incoming HID data:
                 if (cState.L2 == 0xff && cState.R2 == 0xff)
diff --git a/DS4Windows/DS4Library/DS4State.cs b/DS4Windows/DS4Library/DS4State.cs
index d7fb3de..ac55dc8 100644
--- a/DS4Windows/DS4Library/DS4State.cs
+++ b/DS4Windows/DS4Library/DS4State.cs
@@ -23,6 +23,7 @@ namespace DS4Windows
         public double LYUnit;
         public double RXUnit;
         public double RYUnit;
+        public uint elapsedNanoSec = 0;
         public SixAxis Motion = null;
         public static readonly int DEFAULT_AXISDIR_VALUE = 127;
 
@@ -46,6 +47,7 @@ namespace DS4Windows
             LYUnit = 0.0;
             RXUnit = 0.0;
             RYUnit = 0.0;
+            elapsedNanoSec = 0;
             Motion = new SixAxis(0, 0, 0, 0, 0, 0, 0.0);
         }
 
@@ -93,6 +95,7 @@ namespace DS4Windows
             LYUnit = state.LYUnit;
             RXUnit = state.RXUnit;
             RYUnit = state.RYUnit;
+            elapsedNanoSec = state.elapsedNanoSec;
             Motion = state.Motion;
         }
 
@@ -145,6 +148,7 @@ namespace DS4Windows
             state.LYUnit = LYUnit;
             state.RXUnit = RXUnit;
             state.RYUnit = RYUnit;
+            state.elapsedNanoSec = elapsedNanoSec;
             state.Motion = Motion;
         }