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]);
if (calibDone)
{
ApplyCalibs(ref currentYaw, ref currentPitch, ref currentRoll, ref AccelX, ref AccelY, ref AccelZ);
}
if (AccelX != 0 || AccelY != 0 || AccelZ != 0)
{
if (SixAccelMoved != null)
{
Previous.Copy(Current);
Current.Populate(currentYaw, currentPitch, currentRoll,
AccelX, AccelY, AccelZ, elapsedDelta, Previous);
SixAxisEventArgs args = new SixAxisEventArgs(state.ReportTimeStamp, Current);
state.Motion = Current;
SixAccelMoved(this, args);
}
}
}