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); }