Experimental Gyro Calibration for DS4Windows

ds4windows_1_4_92_gyrocalibtest.diff

diff --git a/DS4Windows/DS4Library/DS4Device.cs b/DS4Windows/DS4Library/DS4Device.cs
index 82919d2..aaf38a6 100644
--- a/DS4Windows/DS4Library/DS4Device.cs
+++ b/DS4Windows/DS4Library/DS4Device.cs
@@ -441,6 +441,11 @@ namespace DS4Windows
 
             touchpad = new DS4Touchpad();
             sixAxis = new DS4SixAxis();
+
+            byte[] calibration = new byte[41];
+            calibration[0] = 0x05;
+            hDevice.readFeatureData(calibration);
+            sixAxis.setCalibrationData(ref calibration);
         }
 
         private void timeoutTestThread()
diff --git a/DS4Windows/DS4Library/DS4Sixaxis.cs b/DS4Windows/DS4Library/DS4Sixaxis.cs
index ed20635..422e572 100644
--- a/DS4Windows/DS4Library/DS4Sixaxis.cs
+++ b/DS4Windows/DS4Library/DS4Sixaxis.cs
@@ -6,7 +6,7 @@ namespace DS4Windows
     {
         public readonly SixAxis sixAxis;
         public readonly DateTime timeStamp;
-        public SixAxisEventArgs(DateTime utcTimestamp, SixAxis sa)
+        public SixAxisEventArgs(ref DateTime utcTimestamp, SixAxis sa)
         {
             sixAxis = sa;
             timeStamp = utcTimestamp;
@@ -30,7 +30,6 @@ namespace DS4Windows
         public readonly double elapsed;
         public readonly SixAxis previousAxis = null;
 
-        double recip = 1d / 8192d;
         double tempDouble = 0d;
 
         public SixAxis(int X, int Y, int Z,
@@ -74,17 +73,102 @@ namespace DS4Windows
 
         private double lastElapsedDelta;
         private byte[] previousPacket = new byte[8];
+        private CalibData[] calibrationData = new CalibData[6];
+        
+
+        public DS4SixAxis()
+        {
+        }
+
+        int temInt = 0;
+        public void setCalibrationData(ref byte[] calibData)
+        {
+            int pitchPlus, pitchMinus, yawPlus, yawMinus, rollPlus, rollMinus,
+                accelXPlus, accelXMinus, accelYPlus, accelYMinus, accelZPlus, accelZMinus,
+                gyroSpeedPlus, gyroSpeedMinus;
+
+            calibrationData[0].bias = (short)((ushort)(calibData[2] << 8) | calibData[1]);
+            calibrationData[1].bias = (short)((ushort)(calibData[4] << 8) | calibData[3]);
+            calibrationData[2].bias = (short)((ushort)(calibData[6] << 8) | calibData[5]);
+
+            pitchPlus = temInt = (short)((ushort)(calibData[8] << 8) | calibData[7]);
+            yawPlus = temInt = (short)((ushort)(calibData[10] << 8) | calibData[9]);
+            rollPlus = temInt = (short)((ushort)(calibData[12] << 8) | calibData[11]);
+            pitchMinus = temInt = (short)((ushort)(calibData[14] << 8) | calibData[13]);
+            yawMinus = temInt = (short)((ushort)(calibData[16] << 8) | calibData[15]);
+            rollMinus = temInt = (short)((ushort)(calibData[18] << 8) | calibData[17]);
+
+            gyroSpeedPlus = temInt = (short)((ushort)(calibData[20] << 8) | calibData[19]);
+            gyroSpeedMinus = temInt = (short)((ushort)(calibData[22] << 8) | calibData[21]);
+            accelXPlus = temInt = (short)((ushort)(calibData[24] << 8) | calibData[23]);
+            accelXMinus = temInt = (short)((ushort)(calibData[26] << 8) | calibData[25]);
+
+            accelYPlus = temInt = (short)((ushort)(calibData[28] << 8) | calibData[27]);
+            accelYMinus = temInt = (short)((ushort)(calibData[30] << 8) | calibData[29]);
+
+            accelZPlus = temInt = (short)((ushort)(calibData[32] << 8) | calibData[31]);
+            accelZMinus = temInt = (short)((ushort)(calibData[34] << 8) | calibData[33]);
+
+            int gyroSpeed2x = temInt = (gyroSpeedPlus + gyroSpeedMinus);
+            calibrationData[0].sensNumer = gyroSpeed2x * SixAxis.GYRO_RES_IN_DEG_SEC;
+            calibrationData[0].sensDenom = pitchPlus - pitchMinus;
+
+            calibrationData[1].sensNumer = gyroSpeed2x * SixAxis.GYRO_RES_IN_DEG_SEC;
+            calibrationData[1].sensDenom = pitchPlus - pitchMinus;
+
+            calibrationData[2].sensNumer = gyroSpeed2x * SixAxis.GYRO_RES_IN_DEG_SEC;
+            calibrationData[2].sensDenom = pitchPlus - pitchMinus;
+
+            int accelRange = temInt = accelXPlus - accelXMinus;
+            calibrationData[3].bias = accelXPlus - accelRange / 2;
+            calibrationData[3].sensNumer = 2 * SixAxis.ACC_RES_PER_G;
+            calibrationData[3].sensDenom = accelRange;
+
+            accelRange = temInt = accelYPlus - accelYMinus;
+            calibrationData[4].bias = accelYPlus - accelRange / 2;
+            calibrationData[4].sensNumer = 2 * SixAxis.ACC_RES_PER_G;
+            calibrationData[4].sensDenom = accelRange;
+
+            accelRange = temInt = accelZPlus - accelZMinus;
+            calibrationData[5].bias = accelZPlus - accelRange / 2;
+            calibrationData[5].sensNumer = 2 * SixAxis.ACC_RES_PER_G;
+            calibrationData[5].sensDenom = accelRange;
+        }
+
+        internal struct CalibData
+        {
+            public int bias;
+            public int sensNumer;
+            public int sensDenom;
+            public const int GyroPitchIdx = 0, GyroYawIdx = 1, GyroRollIdx = 2,
+            AccelXIdx = 3, AccelYIdx = 4, AccelZIdx = 5;
+            private int tempInt;
+
+            public int apply(int value)
+            {
+                tempInt = value - bias;
+                int result = tempInt = (int)(tempInt * (sensNumer / (float)sensDenom));
+                return result;
+            }
+        }
 
         public void handleSixaxis(byte[] gyro, byte[] accel, DS4State state,
             double elapsedDelta)
         {
-            int currentYaw = (short)((ushort)(gyro[3] << 8) | gyro[2]);
             int currentPitch = (short)((ushort)(gyro[1] << 8) | gyro[0]);
+            int currentYaw = (short)((ushort)(gyro[3] << 8) | gyro[2]);
             int currentRoll = (short)((ushort)(gyro[5] << 8) | gyro[4]);
             int AccelX = (short)((ushort)(accel[1] << 8) | accel[0]);
             int AccelY = (short)((ushort)(accel[3] << 8) | accel[2]);
             int AccelZ = (short)((ushort)(accel[5] << 8) | accel[4]);
 
+            currentPitch = calibrationData[CalibData.GyroPitchIdx].apply(currentPitch);
+            currentYaw = calibrationData[CalibData.GyroYawIdx].apply(currentYaw);
+            currentRoll = calibrationData[CalibData.GyroRollIdx].apply(currentRoll);
+            AccelX = calibrationData[CalibData.AccelXIdx].apply(AccelX);
+            AccelY = calibrationData[CalibData.AccelYIdx].apply(AccelY);
+            AccelZ = calibrationData[CalibData.AccelZIdx].apply(AccelZ);
+
             SixAxisEventArgs args = null;
             if (AccelX != 0 || AccelY != 0 || AccelZ != 0)
             {
@@ -97,7 +181,7 @@ namespace DS4Windows
                     now = new SixAxis(currentYaw, currentPitch, currentRoll,
                         AccelX, AccelY, AccelZ, elapsedDelta, sPrev);
 
-                    args = new SixAxisEventArgs(state.ReportTimeStamp, now);
+                    args = new SixAxisEventArgs(ref state.ReportTimeStamp, now);
                     state.Motion = now;
                     SixAccelMoved(this, args);
                 }