public void copy(SixAxis src)
{
gyroYaw = src.gyroYaw;
gyroPitch = src.gyroPitch;
gyroRoll = src.gyroRoll;
gyroYawFull = src.gyroYawFull;
accelXFull = src.accelXFull; accelYFull = src.accelYFull; accelZFull = src.accelZFull;
angVelYaw = src.angVelYaw;
angVelPitch = src.angVelPitch;
angVelRoll = src.angVelRoll;
accelXG = src.accelXG;
accelYG = src.accelYG;
accelZG = src.accelZG;
// Put accel ranges between 0 - 128 abs
accelX = src.accelX;
accelY = src.accelY;
accelZ = src.accelZ;
outputAccelX = accelX;
outputAccelY = accelY;
outputAccelZ = accelZ;
elapsed = src.elapsed;
previousAxis = src.previousAxis;
}
public DS4State()
{
PacketCounter = 0;
Square = Triangle = Circle = Cross = false;
DpadUp = DpadDown = DpadLeft = DpadRight = false;
L1 = L2Btn = L3 = R1 = R2Btn = R3 = false;
Share = Options = PS = Touch1 = Touch2 = TouchButton = TouchRight = TouchLeft = false;
Touch1Finger = Touch2Fingers = false;
LX = RX = LY = RY = 127;
L2 = R2 = 0;
FrameCounter = 255; // only actually has 6 bits, so this is a null indicator
TouchPacketCounter = 255; // 8 bits, no great junk value
Battery = 0;
LSAngle = 0.0;
LSAngleRad = 0.0;
RSAngle = 0.0;
RSAngleRad = 0.0;
LXUnit = 0.0;
LYUnit = 0.0;
RXUnit = 0.0;
RYUnit = 0.0;
Motion = new SixAxis(0, 0, 0, 0, 0, 0, 0);
TrackPadTouch0.IsActive = false;
TrackPadTouch1.IsActive = false;
}
public void handleSixaxis(byte[] gyro, byte[] accel, DS4State state)
{
//bool touchPadIsDown = sensors.TouchButton;
/*if (!PacketChanged(data, touchPacketOffset) && touchPadIsDown == lastTouchPadIsDown)
* {
* if (SixAxisUnchanged != null)
* SixAxisUnchanged(this, EventArgs.Empty);
* return;
* }*/
/* 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[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)
{
/* if (SixAxisMoved != null)
* {
* SixAxis sPrev, now;
* sPrev = new SixAxis(lastGyroX, lastGyroY, lastGyroZ, lastAX,lastAY,lastAZ);
* now = new SixAxis(currentX, currentY, currentZ, AccelX, AccelY, AccelZ, sPrev);
* args = new SixAxisEventArgs(state.ReportTimeStamp, now);
* SixAxisMoved(this, args);
* }
*
* lastGyroX = currentX;
* lastGyroY = currentY;
* lastGyroZ = currentZ;
* lastAX = AccelX;
* lastAY = AccelY;
* lastAZ = AccelZ;*/
}
if (AccelX != 0 || AccelY != 0 || AccelZ != 0)
{
if (SixAccelMoved != null)
{
SixAxis sPrev, now;
sPrev = new SixAxis(lastGyroX, lastGyroY, lastGyroZ, lastAX, lastAY, lastAZ);
now = new SixAxis(currentX, currentY, currentZ, AccelX, AccelY, AccelZ, sPrev);
args = new SixAxisEventArgs(state.ReportTimeStamp, now);
SixAccelMoved(this, args);
}
lastGyroX = currentX;
lastGyroY = currentY;
lastGyroZ = currentZ;
lastAX = AccelX;
lastAY = AccelY;
lastAZ = AccelZ;
}
}
public DS4State(DS4State state)
{
PacketCounter = state.PacketCounter;
ReportTimeStamp = state.ReportTimeStamp;
Square = state.Square;
Triangle = state.Triangle;
Circle = state.Circle;
Cross = state.Cross;
DpadUp = state.DpadUp;
DpadDown = state.DpadDown;
DpadLeft = state.DpadLeft;
DpadRight = state.DpadRight;
L1 = state.L1;
L2 = state.L2;
L2Btn = state.L2Btn;
L3 = state.L3;
R1 = state.R1;
R2 = state.R2;
R2Btn = state.R2Btn;
R3 = state.R3;
Share = state.Share;
Options = state.Options;
PS = state.PS;
Mute = state.Mute;
Touch1 = state.Touch1;
TouchRight = state.TouchRight;
TouchLeft = state.TouchLeft;
Touch1Identifier = state.Touch1Identifier;
Touch2 = state.Touch2;
Touch2Identifier = state.Touch2Identifier;
TouchButton = state.TouchButton;
OutputTouchButton = state.OutputTouchButton;
TouchPacketCounter = state.TouchPacketCounter;
Touch1Finger = state.Touch1Finger;
Touch2Fingers = state.Touch2Fingers;
LX = state.LX;
RX = state.RX;
LY = state.LY;
RY = state.RY;
FrameCounter = state.FrameCounter;
Battery = state.Battery;
LSAngle = state.LSAngle;
LSAngleRad = state.LSAngleRad;
RSAngle = state.RSAngle;
RSAngleRad = state.RSAngleRad;
LXUnit = state.LXUnit;
LYUnit = state.LYUnit;
RXUnit = state.RXUnit;
RYUnit = state.RYUnit;
elapsedTime = state.elapsedTime;
totalMicroSec = state.totalMicroSec;
ds4Timestamp = state.ds4Timestamp;
Motion = state.Motion;
TrackPadTouch0 = state.TrackPadTouch0;
TrackPadTouch1 = state.TrackPadTouch1;
SASteeringWheelEmulationUnit = state.SASteeringWheelEmulationUnit;
}
public DS4SixAxis()
{
sPrev = new SixAxis(0, 0, 0, 0, 0, 0, 0.0);
now = new SixAxis(0, 0, 0, 0, 0, 0, 0.0);
for (int i = 0; i < gyro_average_window.Length; i++)
{
gyro_average_window[i] = new GyroAverageWindow();
}
gyroAverageTimer.Start();
}
public void handleSixaxis(byte[] gyro, byte[] accel, DS4State state)
{
//bool touchPadIsDown = sensors.TouchButton;
/*if (!PacketChanged(data, touchPacketOffset) && touchPadIsDown == lastTouchPadIsDown)
{
if (SixAxisUnchanged != null)
SixAxisUnchanged(this, EventArgs.Empty);
return;
}*/
/* byte touchID1 = (byte)(data[0 + TOUCHPAD_DATA_OFFSET + touchPacketOffset] & 0x7F);
byte touchID2 = (byte)(data[4 + TOUCHPAD_DATA_OFFSET + touchPacketOffset] & 0x7F);*/
int currentX = (Int16)((UInt16)(gyro[0] << 8) | gyro[1]) / 256;
int currentY = (Int16)((UInt16)(gyro[2] << 8) | gyro[3]) / 256;
int currentZ = (Int16)((UInt16)(gyro[4] << 8) | gyro[5]) / 256;
int AccelX = (Int16)((UInt16)(accel[2] << 8) | accel[3]) / 256;
int AccelY = (Int16)((UInt16)(accel[0] << 8) | accel[1]) / 256;
int AccelZ = (Int16)((UInt16)(accel[4] << 8) | accel[5]) / 256;
SixAxisEventArgs args;
//if (sensors.Touch1 || sensors.Touch2)
{
if (SixAxisMoved != null)
{
SixAxis sPrev, now;
sPrev = new SixAxis(lastGyroX, lastGyroY, lastGyroZ, lastAX, lastAY, lastAZ);
now = new SixAxis(currentX, currentY, currentZ, AccelX, AccelY, AccelZ, sPrev);
args = new SixAxisEventArgs(state.ReportTimeStamp, now);
SixAxisMoved(this, args);
}
lastGyroX = currentX;
lastGyroY = currentY;
lastGyroZ = currentZ;
lastAX = AccelX;
lastAY = AccelY;
lastAZ = AccelZ;
}
if (AccelX != 0 || AccelY != 0 || AccelZ != 0)
{
if (SixAccelMoved != null)
{
SixAxis sPrev, now;
sPrev = new SixAxis(lastGyroX, lastGyroY, lastGyroZ, lastAX, lastAY, lastAZ);
now = new SixAxis(currentX, currentY, currentZ, AccelX, AccelY, AccelZ, sPrev);
args = new SixAxisEventArgs(state.ReportTimeStamp, now);
SixAccelMoved(this, args);
}
lastGyroX = currentX;
lastGyroY = currentY;
lastGyroZ = currentZ;
lastAX = AccelX;
lastAY = AccelY;
lastAZ = AccelZ;
}
}
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;
previousAxis = prevAxis;
if (previousAxis != null)
{
deltaX = X - previousAxis.gyroX;
deltaY = Y - previousAxis.gyroY;
deltaZ = Z - previousAxis.gyroZ;
}
}
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;
previousAxis = prevAxis;
if (previousAxis != null)
{
deltaX = X - previousAxis.gyroX;
deltaY = Y - previousAxis.gyroY;
deltaZ = Z - previousAxis.gyroZ;
}
}
public DS4State(DS4State state)
{
ReportTimeStamp = state.ReportTimeStamp;
Square = state.Square;
Triangle = state.Triangle;
Circle = state.Circle;
Cross = state.Cross;
DpadUp = state.DpadUp;
DpadDown = state.DpadDown;
DpadLeft = state.DpadLeft;
DpadRight = state.DpadRight;
L1 = state.L1;
L2 = state.L2;
L3 = state.L3;
R1 = state.R1;
R2 = state.R2;
R3 = state.R3;
Share = state.Share;
Options = state.Options;
PS = state.PS;
Touch1 = state.Touch1;
TouchRight = state.TouchRight;
TouchLeft = state.TouchLeft;
Touch1Identifier = state.Touch1Identifier;
Touch2 = state.Touch2;
Touch2Identifier = state.Touch2Identifier;
TouchButton = state.TouchButton;
TouchPacketCounter = state.TouchPacketCounter;
Touch1Finger = state.Touch1Finger;
Touch2Fingers = state.Touch2Fingers;
LX = state.LX;
RX = state.RX;
LY = state.LY;
RY = state.RY;
FrameCounter = state.FrameCounter;
Battery = state.Battery;
LSAngle = state.LSAngle;
LSAngleRad = state.LSAngleRad;
RSAngle = state.RSAngle;
RSAngleRad = state.RSAngleRad;
LXUnit = state.LXUnit;
LYUnit = state.LYUnit;
RXUnit = state.RXUnit;
RYUnit = state.RYUnit;
elapsedTime = state.elapsedTime;
Motion = state.Motion;
}
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 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]);
SixAxisEventArgs args = null;
if (AccelX != 0 || AccelY != 0 || AccelZ != 0)
{
if (SixAccelMoved != null)
{
SixAxis sPrev = null, now = null;
sPrev = new SixAxis(lastGyroYaw, lastGyroPitch, lastGyroRoll,
lastAX, lastAY, lastAZ, lastElapsedDelta);
now = new SixAxis(currentYaw, currentPitch, currentRoll,
AccelX, AccelY, AccelZ, elapsedDelta, sPrev);
args = new SixAxisEventArgs(state.ReportTimeStamp, now);
state.Motion = now;
SixAccelMoved(this, args);
}
lastGyroYaw = currentYaw;
lastGyroPitch = currentPitch;
lastGyroRoll = currentRoll;
lastAX = AccelX;
lastAY = AccelY;
lastAZ = AccelZ;
lastElapsedDelta = elapsedDelta;
}
}
public SixAxis(int X, int Y, int Z, int aX, int aY, int aZ, SixAxis prevAxis = null)
{
gyroX = X / 256;
gyroY = Y / 256;
gyroZ = Z / 256;
gyroXFull = X;
gyroYFull = Y;
gyroZFull = Z;
accelX = aX / 64;
accelY = aY / 64;
accelZ = aZ / 64;
accelXFull = aX;
accelYFull = aY;
accelZFull = aZ;
previousAxis = prevAxis;
if (previousAxis != null)
{
deltaX = gyroX - previousAxis.gyroX;
deltaY = gyroY - previousAxis.gyroY;
deltaZ = gyroZ - previousAxis.gyroZ;
}
}
public SixAxisEventArgs(System.DateTime utcTimestamp, SixAxis sa)
{
sixAxis = sa;
this.timeStamp = utcTimestamp;
}
public SixAxisEventArgs(DateTime utcTimestamp, SixAxis sa)
{
sixAxis = sa;
timeStamp = utcTimestamp;
}
public SixAxis(int X, int Y, int Z,
int aX, int aY, int aZ,
double elapsedDelta, SixAxis prevAxis = null)
{
populate(X, Y, Z, aX, aY, aZ, elapsedDelta, prevAxis);
}
public DS4SixAxis()
{
sPrev = new SixAxis(0, 0, 0, 0, 0, 0, 0.0);
now = new SixAxis(0, 0, 0, 0, 0, 0, 0.0);
}
public DS4SixAxis()
{
sPrev = new SixAxis(0, 0, 0, 0, 0, 0, 0.0);
now = new SixAxis(0, 0, 0, 0, 0, 0, 0.0);
StartContinuousCalibration();
}
public void handleSixaxis(byte[] inputReport, DateTime receivedTimestamp, string MacAddress)
{
uint timestamp = (((uint)inputReport[11]) << 8) | ((uint)inputReport[10]);
ulong fullTimestamp = 0;
// convert wrapped time to absolute time
if (_prevReportTimestamp < 0) //first one, start from zero
{
fullTimestamp = ((uint)timestamp * 16) / 3;
}
else
{
ushort delta;
if (_prevReportTimestamp > timestamp) //wrapped around
{
delta = (ushort)(ushort.MaxValue - _prevReportTimestamp + timestamp + 1);
}
else
{
delta = (ushort)(timestamp - _prevReportTimestamp);
}
fullTimestamp = Values.timestampUs + (((uint)delta * 16) / 3);
}
_prevReportTimestamp = timestamp;
// if (_cal != null)
// {
// var calValues = _cal.ApplyCalToInReport(inputReport);
//#if DUMP_DS4_CALIBRATION
// short[] preCal = calValues.Item1;
// short[] posCal = calValues.Item2;
// short[] delta = new short[preCal.Length];
// for (int i=1; i<posCal.Length; i++)
// {
// preCal[i] = (short)-preCal[i];
// posCal[i] = (short)-posCal[i];
// delta[i] = (short)(posCal[i] - preCal[i]);
// }
// var fmt = "+00000;-00000";
// Console.WriteLine(MacAddress.ToString() + "> " +
// String.Format("Cal applied (ts: {0}) pre: ({1} {2} {3}) ({4} {5} {6}) post: ({7} {8} {9}) ({10} {11} {12}) delta: ({13} {14} {15}) ({16} {17} {18})",
// ((double)(fullTimestamp) / 1000).ToString("0.000"),
// preCal[0].ToString(fmt), preCal[1].ToString(fmt), preCal[2].ToString(fmt), preCal[3].ToString(fmt), preCal[4].ToString(fmt), preCal[5].ToString(fmt),
// posCal[0].ToString(fmt), posCal[1].ToString(fmt), posCal[2].ToString(fmt), posCal[3].ToString(fmt), posCal[4].ToString(fmt), posCal[5].ToString(fmt),
// delta[0].ToString(fmt), delta[1].ToString(fmt), delta[2].ToString(fmt), delta[3].ToString(fmt), delta[4].ToString(fmt), delta[5].ToString(fmt))
// );
//#endif
// }
byte[] accel = new byte[6];
byte[] gyro = new byte[6];
Array.Copy(inputReport, 14, accel, 0, 6);
Array.Copy(inputReport, 20, gyro, 0, 6);
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;
System.Diagnostics.Debug.Print("{0:d},{1:d},{2:d},{3:d},{4:d},{5:d}",
currentX, currentY, currentZ,
AccelX, AccelY, AccelZ);
int intPitch = (short)(((ushort)inputReport[14] << 8) | (ushort)inputReport[13]);
int intYaw = (short)-(((ushort)inputReport[16] << 8) | (ushort)inputReport[15]);
int intRoll = (short)-(((ushort)inputReport[18] << 8) | (ushort)inputReport[17]);
int intAccX = (short)-(((ushort)inputReport[20] << 8) | (ushort)inputReport[19]);
int intAccY = (short)-(((ushort)inputReport[22] << 8) | (ushort)inputReport[21]);
int intAccZ = (short)-(((ushort)inputReport[24] << 8) | (ushort)inputReport[23]);
float angVelPitch = (float)(intPitch) / DS4Cal.GYRO_RESOLUTION_IN_DEG_SEC; //in deg/s
float angVelYaw = (float)(intYaw) / DS4Cal.GYRO_RESOLUTION_IN_DEG_SEC; //in deg/s
float angVelRoll = (float)(intRoll) / DS4Cal.GYRO_RESOLUTION_IN_DEG_SEC; //in deg/s
float accelX = (float)(intAccX) / DS4Cal.ACC_RESOLUTION_PER_G; //in Gs/s^2
float accelY = (float)(intAccY) / DS4Cal.ACC_RESOLUTION_PER_G; //in Gs/s^2
float accelZ = (float)(intAccZ) / DS4Cal.ACC_RESOLUTION_PER_G; //in Gs/s^2
SixAxis sPrev;
sPrev = new SixAxis(Values.timestampUs, Values.gyroPitch, Values.gyroYaw, Values.gyroRoll, Values.accelX, Values.accelY, Values.accelZ);
_values = new SixAxis(fullTimestamp, angVelPitch, angVelYaw, angVelRoll, accelX, accelY, accelZ, sPrev);
SixAxisEventArgs args;
if (SixAccelMoved != null)
{
args = new SixAxisEventArgs(receivedTimestamp, Values);
SixAccelMoved(this, args);
}
}
public SixAxis(ulong microseconds, float gX, float gY, float gZ, float aX, float aY, float aZ, SixAxis prevAxis = null)
{
timestampUs = microseconds;
gyroPitch = gX;
gyroYaw = gY;
gyroRoll = gZ;
accelX = aX;
accelY = aY;
accelZ = aZ;
previous = prevAxis;
// Put accel ranges between 0 - 128 abs
fakeAccelX = (int)Math.Round(aX * (DS4Cal.ACC_RESOLUTION_PER_G / 64));
fakeAccelY = (int)Math.Round(aY * (DS4Cal.ACC_RESOLUTION_PER_G / 64));
fakeAccelZ = (int)Math.Round(-aZ * (DS4Cal.ACC_RESOLUTION_PER_G / 64));
outputAccelX = fakeAccelX;
outputAccelY = fakeAccelY;
outputAccelZ = fakeAccelZ;
// Legacy values
fakeGyroPitch = (int)Math.Round(gX / (256 / DS4Cal.GYRO_RESOLUTION_IN_DEG_SEC));
fakeGyroYaw = (int)Math.Round(gY / (256 / DS4Cal.GYRO_RESOLUTION_IN_DEG_SEC));
fakeGyroRoll = (int)Math.Round(gZ / (256 / DS4Cal.GYRO_RESOLUTION_IN_DEG_SEC));
}
public SixAxisEventArgs(System.DateTime utcTimestamp, SixAxis sa)
{
sixAxis = sa;
this.timeStamp = utcTimestamp;
}
