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 virtual void sixaxisMoved(SixAxisEventArgs arg)
{
int deltaX = 0, deltaY = 0;
deltaX = Global.getGyroMouseHorizontalAxis(deviceNumber) == 0 ? arg.sixAxis.gyroYawFull :
arg.sixAxis.gyroRollFull;
deltaY = -arg.sixAxis.gyroPitchFull;
//tempDouble = arg.sixAxis.elapsed * 0.001 * 200.0; // Base default speed on 5 ms
tempDouble = arg.sixAxis.elapsed * 200.0; // Base default speed on 5 ms
gyroSmooth = Global.getGyroSmoothing(deviceNumber);
double gyroSmoothWeight = 0.0;
coefficient = (Global.getGyroSensitivity(deviceNumber) * 0.01) * GYRO_MOUSE_COEFFICIENT;
double offset = GYRO_MOUSE_OFFSET;
if (gyroSmooth)
{
gyroSmoothWeight = Global.getGyroSmoothingWeight(deviceNumber);
if (gyroSmoothWeight > 0.0)
{
offset = GYRO_SMOOTH_MOUSE_OFFSET;
}
}
double tempAngle = Math.Atan2(-deltaY, deltaX);
double normX = Math.Abs(Math.Cos(tempAngle));
double normY = Math.Abs(Math.Sin(tempAngle));
int signX = Math.Sign(deltaX);
int signY = Math.Sign(deltaY);
if (deltaX == 0 || (hRemainder > 0 != deltaX > 0))
{
hRemainder = 0.0;
}
if (deltaY == 0 || (vRemainder > 0 != deltaY > 0))
{
vRemainder = 0.0;
}
int deadzoneX = (int)Math.Abs(normX * GYRO_MOUSE_DEADZONE);
int deadzoneY = (int)Math.Abs(normY * GYRO_MOUSE_DEADZONE);
if (Math.Abs(deltaX) > deadzoneX)
{
deltaX -= signX * deadzoneX;
}
else
{
deltaX = 0;
}
if (Math.Abs(deltaY) > deadzoneY)
{
deltaY -= signY * deadzoneY;
}
else
{
deltaY = 0;
}
double xMotion = deltaX != 0 ? coefficient * (deltaX * tempDouble)
+ (normX * (offset * signX)) : 0;
int xAction = 0;
if (xMotion != 0.0)
{
xMotion += hRemainder;
}
else
{
hRemainder = 0.0;
}
verticalScale = Global.getGyroSensVerticalScale(deviceNumber) * 0.01;
double yMotion = deltaY != 0 ? (coefficient * verticalScale) * (deltaY * tempDouble)
+ (normY * (offset * signY)) : 0;
int yAction = 0;
if (yMotion != 0.0)
{
yMotion += vRemainder;
}
else
{
vRemainder = 0.0;
}
if (gyroSmooth)
{
int iIndex = smoothBufferTail % SMOOTH_BUFFER_LEN;
xSmoothBuffer[iIndex] = xMotion;
ySmoothBuffer[iIndex] = yMotion;
smoothBufferTail = iIndex + 1;
double currentWeight = 1.0;
double finalWeight = 0.0;
double x_out = 0.0, y_out = 0.0;
int idx = 0;
for (int i = 0; i < SMOOTH_BUFFER_LEN; i++)
{
idx = (smoothBufferTail - i - 1 + SMOOTH_BUFFER_LEN) % SMOOTH_BUFFER_LEN;
x_out += xSmoothBuffer[idx] * currentWeight;
y_out += ySmoothBuffer[idx] * currentWeight;
finalWeight += currentWeight;
currentWeight *= gyroSmoothWeight;
}
x_out /= finalWeight;
xMotion = x_out;
y_out /= finalWeight;
yMotion = y_out;
}
hRemainder = vRemainder = 0.0;
if (xMotion != 0.0)
{
xAction = (int)xMotion;
hRemainder = xMotion - xAction;
}
if (yMotion != 0.0)
{
yAction = (int)yMotion;
vRemainder = yMotion - yAction;
}
int gyroInvert = Global.getGyroInvert(deviceNumber);
if ((gyroInvert & 0x02) == 2)
{
xAction *= -1;
}
if ((gyroInvert & 0x01) == 1)
{
yAction *= -1;
}
if (yAction != 0 || xAction != 0)
{
InputMethods.MoveCursorBy(xAction, yAction);
}
hDirection = xMotion > 0.0 ? Direction.Positive : xMotion < 0.0 ? Direction.Negative : Direction.Neutral;
vDirection = yMotion > 0.0 ? Direction.Positive : yMotion < 0.0 ? Direction.Negative : Direction.Neutral;
}
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
}
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 void FireSixAxisEvent(SixAxisEventArgs args)
{
SixAccelMoved?.Invoke(this, args);
}
