From f4302a752d92133c288290807dcd33d8a4ef13de Mon Sep 17 00:00:00 2001 From: Travis Nickles Date: Wed, 21 Jun 2017 18:07:21 -0700 Subject: [PATCH] Initial attempt at fixing gyro support. Related to issue #48. It will be a mess for the forseeable future --- DS4Windows/DS4Control/MouseCursor.cs | 26 +++++++++----- DS4Windows/DS4Library/DS4Device.cs | 4 +-- DS4Windows/DS4Library/DS4Sixaxis.cs | 44 +++++++++++++----------- DS4Windows/DS4Library/DS4StateExposed.cs | 39 ++++++++++++++++----- 4 files changed, 74 insertions(+), 39 deletions(-) diff --git a/DS4Windows/DS4Control/MouseCursor.cs b/DS4Windows/DS4Control/MouseCursor.cs index bb4010b..132ee52 100644 --- a/DS4Windows/DS4Control/MouseCursor.cs +++ b/DS4Windows/DS4Control/MouseCursor.cs @@ -21,22 +21,32 @@ namespace DS4Windows public virtual void sixaxisMoved(SixAxisEventArgs arg) { int deltaX = 0, deltaY = 0; - deltaX = -arg.sixAxis.accelX; - deltaY = -arg.sixAxis.accelY; + deltaX = -arg.sixAxis.gyroXFull; + deltaY = -arg.sixAxis.gyroYFull; //Console.WriteLine(arg.sixAxis.deltaX); - double coefficient = Global.GyroSensitivity[deviceNumber] / 100f; - //Collect rounding errors instead of losing motion. + double coefficient = Global.GyroSensitivity[deviceNumber] / 100f * 0.008; + + if ((hRemainder > 0) != (deltaX > 0)) + { + hRemainder = 0.0; + } + + if ((vRemainder > 0) != (deltaY > 0)) + { + vRemainder = 0.0; + } + double xMotion = coefficient * deltaX; xMotion += hRemainder; int xAction = (int)xMotion; - hRemainder += xMotion - xAction; - hRemainder -= (int)hRemainder; + hRemainder = xMotion - xAction; + //hRemainder -= (int)hRemainder; double yMotion = coefficient * deltaY; yMotion += vRemainder; int yAction = (int)yMotion; - vRemainder += yMotion - yAction; - vRemainder -= (int)vRemainder; + vRemainder = yMotion - yAction; + //vRemainder -= (int)vRemainder; int gyroInvert = Global.GyroInvert[deviceNumber]; if (gyroInvert == 2 || gyroInvert == 3) diff --git a/DS4Windows/DS4Library/DS4Device.cs b/DS4Windows/DS4Library/DS4Device.cs index 760a46b..696b2df 100644 --- a/DS4Windows/DS4Library/DS4Device.cs +++ b/DS4Windows/DS4Library/DS4Device.cs @@ -814,8 +814,8 @@ namespace DS4Windows cState.FrameCounter = (byte)(inputReport[7] >> 2); // Store Gyro and Accel values - Array.Copy(inputReport, 14, accel, 0, 6); - Array.Copy(inputReport, 20, gyro, 0, 6); + Array.Copy(inputReport, 13, gyro, 0, 6); + Array.Copy(inputReport, 19, accel, 0, 6); sixAxis.handleSixaxis(gyro, accel, cState); try diff --git a/DS4Windows/DS4Library/DS4Sixaxis.cs b/DS4Windows/DS4Library/DS4Sixaxis.cs index bed25c0..7df9480 100644 --- a/DS4Windows/DS4Library/DS4Sixaxis.cs +++ b/DS4Windows/DS4Library/DS4Sixaxis.cs @@ -5,45 +5,47 @@ namespace DS4Windows public class SixAxisEventArgs : EventArgs { public readonly SixAxis sixAxis; - public readonly System.DateTime timeStamp; - public SixAxisEventArgs(System.DateTime utcTimestamp, SixAxis sa) + public readonly DateTime timeStamp; + public SixAxisEventArgs(DateTime utcTimestamp, SixAxis sa) { sixAxis = sa; - this.timeStamp = utcTimestamp; + timeStamp = utcTimestamp; } } public class SixAxis { public readonly int gyroX, gyroY, gyroZ, deltaX, deltaY, deltaZ, accelX, accelY, accelZ; + public readonly int gyroXFull, gyroYFull; public readonly byte touchID; public readonly SixAxis previousAxis; public SixAxis(int X, int Y, int Z, int aX, int aY, int aZ, SixAxis prevAxis = null) { - gyroX = X; - gyroY = Y; - gyroZ = Z; - accelX = aX; - accelY = aY; - accelZ = aZ; + gyroX = X / 256; + gyroY = Y / 256; + gyroZ = Z / 256; + gyroXFull = X; + gyroYFull = Y; + accelX = aX / 64; + accelY = aY / 64; + accelZ = aZ / 64; previousAxis = prevAxis; if (previousAxis != null) { - deltaX = X - previousAxis.gyroX; - deltaY = Y - previousAxis.gyroY; - deltaZ = Z - previousAxis.gyroZ; + deltaX = aX - previousAxis.gyroX; + deltaY = aY - previousAxis.gyroY; + deltaZ = aZ - previousAxis.gyroZ; } } } public class DS4SixAxis { - public event EventHandler SixAxisMoved = null; // deltaX/deltaY are set because one or both fingers were already down on a prior sensor reading + //public event EventHandler SixAxisMoved = null; // deltaX/deltaY are set because one or both fingers were already down on a prior sensor reading public event EventHandler 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 byte[] previousPacket = new byte[8]; - public void handleSixaxis(byte[] gyro, byte[] accel, DS4State state) { @@ -56,12 +58,14 @@ namespace DS4Windows }*/ /* byte touchID1 = (byte)(data[0 + TOUCHPAD_DATA_OFFSET + touchPacketOffset] & 0x7F); byte touchID2 = (byte)(data[4 + TOUCHPAD_DATA_OFFSET + touchPacketOffset] & 0x7F);*/ - int currentX = (short)((ushort)(gyro[0] << 8) | gyro[1]) / 64; - int currentY = (short)((ushort)(gyro[2] << 8) | gyro[3]) / 64; - int currentZ = (short)((ushort)(gyro[4] << 8) | gyro[5]) / 64; - int AccelX = (short)((ushort)(accel[2] << 8) | accel[3]) / 256; - int AccelY = (short)((ushort)(accel[0] << 8) | accel[1]) / 256; - int AccelZ = (short)((ushort)(accel[4] << 8) | accel[5]) / 256; + + int currentX = (short)((ushort)(gyro[3] << 8) | gyro[2]); // Gyro Pitch + int currentY = (short)((ushort)(gyro[1] << 8) | gyro[0]); // Gyro Yaw + int currentZ = (short)((ushort)(gyro[5] << 8) | gyro[4]); // Gyro Roll + int AccelX = (short)((ushort)(accel[1] << 8) | accel[0]); // Accel Pitch + int AccelY = (short)((ushort)(accel[3] << 8) | accel[2]); // Accel Roll + int AccelZ = (short)((ushort)(accel[5] << 8) | accel[4]); // Accel Yaw + SixAxisEventArgs args; //if (sensors.Touch1 || sensors.Touch2) { diff --git a/DS4Windows/DS4Library/DS4StateExposed.cs b/DS4Windows/DS4Library/DS4StateExposed.cs index c51cf85..2826bf6 100644 --- a/DS4Windows/DS4Library/DS4StateExposed.cs +++ b/DS4Windows/DS4Library/DS4StateExposed.cs @@ -58,38 +58,59 @@ namespace DS4Windows /// Yaw leftward/counter-clockwise/turn to port or larboard side /// Add double the previous result to this delta and divide by three. - public int AccelX { get { return (short)((ushort)(accel[2] << 8) | accel[3]) / 256; } } + //public int AccelX { get { return (short)((ushort)(accel[2] << 8) | accel[3]) / 256; } } + //public int AccelX { get { return (short)((ushort)(accel[1] << 8) | accel[0]) / 64; } } + public int AccelX { get { return (short)((ushort)(gyro[3] << 8) | gyro[2]) / 256; } } + /// Pitch upward/backward /// Add double the previous result to this delta and divide by three. - public int AccelY { get { return (short)((ushort)(accel[0] << 8) | accel[1] ) / 256; } } + //public int AccelY { get { return (short)((ushort)(accel[0] << 8) | accel[1] ) / 256; } } + //public int AccelY { get { return (short)((ushort)(accel[3] << 8) | accel[2]) / 64; } } + public int AccelY { get { return (short)((ushort)(gyro[1] << 8) | gyro[0]) / 256; } } + /// roll left/L side of controller down/starboard raising up /// Add double the previous result to this delta and divide by three. - public int AccelZ { get { return (short)((ushort)(accel[4] << 8) | accel[5]) / 256; } } + //public int AccelZ { get { return (short)((ushort)(accel[4] << 8) | accel[5]) / 256; } } + //public int AccelZ { get { return (short)((ushort)(accel[5] << 8) | accel[4]) / 64; } } + public int AccelZ { get { return (short)((ushort)(gyro[5] << 8) | gyro[4]) / 256; } } + /// R side of controller upward /// Add double the previous result to this delta and divide by three. - public int GyroX { get { return (short)((ushort)(gyro[0] << 8) | gyro[1]) / 64; } } + //public int GyroX { get { return (short)((ushort)(gyro[0] << 8) | gyro[1]) / 64; } } + //public int GyroX { get { return (short)((ushort)(gyro[3] << 8) | gyro[2]) / 256; } } + public int GyroX { get { return (short)((ushort)(accel[1] << 8) | accel[0]) / 64; } } public int getGyroX() { - return (short)((ushort)(gyro[0] << 8) | gyro[1]) / 64; + //return (short)((ushort)(gyro[0] << 8) | gyro[1]) / 64; + //return (short)((ushort)(gyro[3] << 8) | gyro[2]) / 256; + return (short)((ushort)(accel[1] << 8) | accel[0]) / 64; } /// touchpad and button face side of controller upward /// Add double the previous result to this delta and divide by three. - public int GyroY { get { return (short)((ushort)(gyro[2] << 8) | gyro[3]) / 64; } } + //public int GyroY { get { return (short)((ushort)(gyro[2] << 8) | gyro[3]) / 64; } } + //public int GyroY { get { return (short)((ushort)(gyro[1] << 8) | gyro[0]) / 256; } } + public int GyroY { get { return (short)((ushort)(accel[3] << 8) | accel[2]) / 64; } } public int getGyroY() { - return (short)((ushort)(gyro[2] << 8) | gyro[3]) / 64; + //return (short)((ushort)(gyro[2] << 8) | gyro[3]) / 64; + //return (short)((ushort)(gyro[1] << 8) | gyro[0]) / 256; + return (short)((ushort)(accel[3] << 8) | accel[2]) / 64; } /// Audio/expansion ports upward and light bar/shoulders/bumpers/USB port downward /// Add double the previous result to this delta and divide by three. - public int GyroZ { get { return (short)((ushort)(gyro[4] << 8) | gyro[5]) / 64; } } + //public int GyroZ { get { return (short)((ushort)(gyro[4] << 8) | gyro[5]) / 64; } } + //public int GyroZ { get { return (short)((ushort)(gyro[5] << 8) | gyro[4]) / 256; } } + public int GyroZ { get { return (short)((ushort)(accel[5] << 8) | accel[4]) / 64; } } public int getGyroZ() { - return (short)((ushort)(gyro[4] << 8) | gyro[5]) / 64; + //return (short)((ushort)(gyro[4] << 8) | gyro[5]) / 64; + //return (short)((ushort)(gyro[5] << 8) | gyro[4]) / 256; + return (short)((ushort)(accel[5] << 8) | accel[4]) / 64; } } }