public void handleSixaxis(byte[] gyro, byte[] accel, State 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);*/
var currentX = (short)((ushort)(gyro[0] << 8) | gyro[1]) / 64;
var currentY = (short)((ushort)(gyro[2] << 8) | gyro[3]) / 64;
var currentZ = (short)((ushort)(gyro[4] << 8) | gyro[5]) / 64;
var AccelX = (short)((ushort)(accel[2] << 8) | accel[3]) / 256;
var AccelY = (short)((ushort)(accel[0] << 8) | accel[1]) / 256;
var AccelZ = (short)((ushort)(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)
{
var sPrev = new SixAxisReadings(lastGyroX, lastGyroY, lastGyroZ, lastAX, lastAY, lastAZ);
var now = new SixAxisReadings(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 SixAxisReadings(int X, int Y, int Z, int aX, int aY, int aZ, SixAxisReadings prevAxisReadings = null)
{
gyroX = X;
gyroY = Y;
gyroZ = Z;
accelX = aX;
accelY = aY;
accelZ = aZ;
PreviousAxisReadings = prevAxisReadings;
if (PreviousAxisReadings != null)
{
deltaX = X - PreviousAxisReadings.gyroX;
deltaY = Y - PreviousAxisReadings.gyroY;
deltaZ = Z - PreviousAxisReadings.gyroZ;
}
}
public SixAxisEventArgs(DateTime utcTimestamp, SixAxisReadings sa)
{
SixAxisReadings = sa;
timeStamp = utcTimestamp;
}