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