Take poll time into account for gyro mouse

DS4Windows/DS4Control/MouseCursor.cs

@@ -31,8 +31,9 @@ namespace DS4Windows
         double coefficient = 0.0;
         double verticalScale = 0.0;
         bool gyroSmooth = false;
-        //double gyroSmoothWeight = 0.0;
+
         int tempInt = 0;
+        double tempDouble = 0.0;
 
         public virtual void sixaxisMoved(SixAxisEventArgs arg)
         {
@@ -40,7 +41,7 @@ namespace DS4Windows
             deltaX = Global.getGyroMouseHorizontalAxis(deviceNumber) == 0 ? arg.sixAxis.gyroYawFull :
                 arg.sixAxis.gyroRollFull;
             deltaY = -arg.sixAxis.gyroPitchFull;
-            //Console.WriteLine(arg.sixAxis.deltaX);
+            tempDouble = arg.sixAxis.elapsed * 0.001 * 250.0;
 
             gyroSmooth = Global.getGyroSmoothing(deviceNumber);
             double gyroSmoothWeight = 0.0;
@@ -95,32 +96,23 @@ namespace DS4Windows
                 deltaY = 0;
             }
 
-            double xMotion = deltaX != 0 ? coefficient * deltaX + (normX * (offset * signX)) : 0;
+            double xMotion = deltaX != 0 ? coefficient * (deltaX * tempDouble)
+                + (normX * (offset * signX)) : 0;
+
             int xAction = 0;
             if (xMotion != 0.0)
             {
                 xMotion += hRemainder;
-                //xAction = (int)xMotion;
-                //hRemainder = xMotion - xAction;
-            }
-            else
-            {
-                //hRemainder = 0.0;
             }
 
-            //hRemainder -= (int)hRemainder;
             verticalScale = Global.getGyroSensVerticalScale(deviceNumber) * 0.01;
-            double yMotion = deltaY != 0 ? (coefficient * verticalScale) * deltaY + (normY * (offset * signY)) : 0;
+            double yMotion = deltaY != 0 ? (coefficient * verticalScale) * (deltaY * tempDouble)
+                + (normY * (offset * signY)) : 0;
+
             int yAction = 0;
             if (yMotion != 0.0)
             {
                 yMotion += vRemainder;
-                //yAction = (int)yMotion;
-                //vRemainder = yMotion - yAction;
-            }
-            else
-            {
-                //vRemainder = 0.0;
             }
 
             if (gyroSmooth)
@@ -133,9 +125,10 @@ namespace DS4Windows
                 double currentWeight = 1.0;
                 double finalWeight = 0.0;
                 double x_out = 0.0, y_out = 0.0;
+                int idx = 0;
                 for (int i = 0; i < SMOOTH_BUFFER_LEN; i++)
                 {
-                    int idx = System.Math.Abs(smoothBufferTail - i - 1) % SMOOTH_BUFFER_LEN;
+                    idx = System.Math.Abs(smoothBufferTail - i - 1) % SMOOTH_BUFFER_LEN;
                     x_out += xSmoothBuffer[idx] * currentWeight;
                     y_out += ySmoothBuffer[idx] * currentWeight;
                     finalWeight += currentWeight;
@@ -168,8 +161,6 @@ namespace DS4Windows
                 vRemainder = 0.0;
             }
 
-            //vRemainder -= (int)vRemainder;
-
             int gyroInvert = Global.getGyroInvert(deviceNumber);
             if ((gyroInvert & 0x02) == 2)
                 xAction *= -1;

DS4Windows/DS4Library/DS4Device.cs

@@ -851,7 +851,7 @@ namespace DS4Windows
                 // Store Gyro and Accel values
                 Array.Copy(inputReport, 13, gyro, 0, 6);
                 Array.Copy(inputReport, 19, accel, 0, 6);
-                sixAxis.handleSixaxis(gyro, accel, cState);
+                sixAxis.handleSixaxis(gyro, accel, cState, lastTimeElapsedDouble);
 
                 /* Debug output of incoming HID data:
                 if (cState.L2 == 0xff && cState.R2 == 0xff)

DS4Windows/DS4Library/DS4Sixaxis.cs

@@ -19,8 +19,9 @@ namespace DS4Windows
         public readonly int gyroYawFull, gyroPitchFull, gyroRollFull;
         public readonly int accelXFull, accelYFull, accelZFull;
         public readonly byte touchID;
-        public readonly SixAxis previousAxis;
+        public readonly double elapsed;
-        public SixAxis(int X, int Y, int Z, int aX, int aY, int aZ, SixAxis prevAxis = null)
+        public readonly SixAxis previousAxis = null;
+        public SixAxis(int X, int Y, int Z, int aX, int aY, int aZ, double milliseconds, SixAxis prevAxis = null)
         {
             gyroX = X / 256;
             gyroY = Y / 256;
@@ -35,6 +36,7 @@ namespace DS4Windows
             accelXFull = aX;
             accelYFull = aY;
             accelZFull = aZ;
+            elapsed = milliseconds;
 
             previousAxis = prevAxis;
             if (previousAxis != null)
@@ -51,9 +53,10 @@ namespace DS4Windows
         public event EventHandler<SixAxisEventArgs> SixAccelMoved = null; // no status change for the touchpad itself... but other sensors may have changed, or you may just want to do some processing
 
         internal int lastGyroX, lastGyroY, lastGyroZ, lastAX, lastAY, lastAZ; // tracks 0, 1 or 2 touches; we maintain touch 1 and 2 separately
+        internal double lastMilliseconds;
         internal byte[] previousPacket = new byte[8];
 
-        public void handleSixaxis(byte[] gyro, byte[] accel, DS4State state)
+        public void handleSixaxis(byte[] gyro, byte[] accel, DS4State state, double milliseconds)
         {
             int currentX = (short)((ushort)(gyro[3] << 8) | gyro[2]); // Gyro Yaw
             int currentY = (short)((ushort)(gyro[1] << 8) | gyro[0]); // Gyro Pitch
@@ -62,14 +65,14 @@ namespace DS4Windows
             int AccelY = (short)((ushort)(accel[3] << 8) | accel[2]);
             int AccelZ = (short)((ushort)(accel[5] << 8) | accel[4]);
 
-            SixAxisEventArgs args;
+            SixAxisEventArgs args = null;
             if (AccelX != 0 || AccelY != 0 || AccelZ != 0)
             {
                 if (SixAccelMoved != null)
                 {
-                    SixAxis sPrev, now;
+                    SixAxis sPrev = null, now = null;
-                    sPrev = new SixAxis(lastGyroX, lastGyroY, lastGyroZ, lastAX, lastAY, lastAZ);
+                    sPrev = new SixAxis(lastGyroX, lastGyroY, lastGyroZ, lastAX, lastAY, lastAZ, lastMilliseconds);
-                    now = new SixAxis(currentX, currentY, currentZ, AccelX, AccelY, AccelZ, sPrev);
+                    now = new SixAxis(currentX, currentY, currentZ, AccelX, AccelY, AccelZ, milliseconds, sPrev);
                     args = new SixAxisEventArgs(state.ReportTimeStamp, now);
                     SixAccelMoved(this, args);
                 }
@@ -80,6 +83,7 @@ namespace DS4Windows
                 lastAX = AccelX;
                 lastAY = AccelY;
                 lastAZ = AccelZ;
+                lastMilliseconds = milliseconds;
             }
         }
     }