Initial attempt at fixing gyro support. Related to issue #48.

It will be a mess for the forseeable future
This commit is contained in:
Travis Nickles 2017-06-21 18:07:21 -07:00
parent d6d6f63073
commit f4302a752d
4 changed files with 74 additions and 39 deletions

View File

@ -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)

View File

@ -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

View File

@ -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<SixAxisEventArgs> SixAxisMoved = null; // deltaX/deltaY are set because one or both fingers were already down on a prior sensor reading
//public event EventHandler<SixAxisEventArgs> SixAxisMoved = null; // deltaX/deltaY are set because one or both fingers were already down on a prior sensor reading
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 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)
{

View File

@ -58,38 +58,59 @@ namespace DS4Windows
/// <summary> Yaw leftward/counter-clockwise/turn to port or larboard side </summary>
/// <remarks> Add double the previous result to this delta and divide by three.</remarks>
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; } }
/// <summary> Pitch upward/backward </summary>
/// <remarks> Add double the previous result to this delta and divide by three.</remarks>
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; } }
/// <summary> roll left/L side of controller down/starboard raising up </summary>
/// <remarks> Add double the previous result to this delta and divide by three.</remarks>
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; } }
/// <summary> R side of controller upward </summary>
/// <remarks> Add double the previous result to this delta and divide by three.</remarks>
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;
}
/// <summary> touchpad and button face side of controller upward </summary>
/// <remarks> Add double the previous result to this delta and divide by three.</remarks>
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;
}
/// <summary> Audio/expansion ports upward and light bar/shoulders/bumpers/USB port downward </summary>
/// <remarks> Add double the previous result to this delta and divide by three.</remarks>
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;
}
}
}