From b1cd3a76e94bfc88116e72333aa82b5ea857be74 Mon Sep 17 00:00:00 2001 From: Travis Nickles Date: Tue, 10 Oct 2017 17:45:42 -0500 Subject: [PATCH] First acceptable version of gyro calibration support Related to issue #103. --- DS4Windows/DS4Library/DS4Device.cs | 7 +- DS4Windows/DS4Library/DS4Sixaxis.cs | 109 ++++++++++++++++++++++++++++ 2 files changed, 115 insertions(+), 1 deletion(-) diff --git a/DS4Windows/DS4Library/DS4Device.cs b/DS4Windows/DS4Library/DS4Device.cs index 2916611..9043b3a 100644 --- a/DS4Windows/DS4Library/DS4Device.cs +++ b/DS4Windows/DS4Library/DS4Device.cs @@ -407,7 +407,7 @@ namespace DS4Windows private bool timeoutEvent = false; public DS4Device(HidDevice hidDevice) - { + { hDevice = hidDevice; conType = HidConnectionType(hDevice); Mac = hDevice.readSerial(); @@ -448,6 +448,11 @@ namespace DS4Windows touchpad = new DS4Touchpad(); sixAxis = new DS4SixAxis(); + + byte[] calibration = new byte[41]; + calibration[0] = conType == ConnectionType.BT ? (byte)0x02 : (byte)0x05; + hDevice.readFeatureData(calibration); + sixAxis.setCalibrationData(ref calibration); } private void timeoutTestThread() diff --git a/DS4Windows/DS4Library/DS4Sixaxis.cs b/DS4Windows/DS4Library/DS4Sixaxis.cs index 43e2b01..028fdfa 100644 --- a/DS4Windows/DS4Library/DS4Sixaxis.cs +++ b/DS4Windows/DS4Library/DS4Sixaxis.cs @@ -100,10 +100,22 @@ namespace DS4Windows } } + internal class CalibData + { + public int bias; + public int sensNumer; + public int sensDenom; + public const int GyroPitchIdx = 0, GyroYawIdx = 1, GyroRollIdx = 2, + AccelXIdx = 3, AccelYIdx = 4, AccelZIdx = 5; + } + public class DS4SixAxis { public event EventHandler SixAccelMoved = null; private SixAxis sPrev = null, now = null; + private CalibData[] calibrationData = new CalibData[6] { new CalibData(), new CalibData(), + new CalibData(), new CalibData(), new CalibData(), new CalibData() + }; public DS4SixAxis() { @@ -111,6 +123,101 @@ namespace DS4Windows now = new SixAxis(0, 0, 0, 0, 0, 0, 0.0); } + int temInt = 0; + public void setCalibrationData(ref byte[] calibData) + { + int pitchPlus, pitchMinus, yawPlus, yawMinus, rollPlus, rollMinus, + accelXPlus, accelXMinus, accelYPlus, accelYMinus, accelZPlus, accelZMinus, + gyroSpeedPlus, gyroSpeedMinus; + + calibrationData[0].bias = (short)((ushort)(calibData[2] << 8) | calibData[1]); + calibrationData[1].bias = (short)((ushort)(calibData[4] << 8) | calibData[3]); + calibrationData[2].bias = (short)((ushort)(calibData[6] << 8) | calibData[5]); + + if (calibData[0] == 5) + { + pitchPlus = temInt = (short)((ushort)(calibData[8] << 8) | calibData[7]); + yawPlus = temInt = (short)((ushort)(calibData[10] << 8) | calibData[9]); + rollPlus = temInt = (short)((ushort)(calibData[12] << 8) | calibData[11]); + pitchMinus = temInt = (short)((ushort)(calibData[14] << 8) | calibData[13]); + yawMinus = temInt = (short)((ushort)(calibData[16] << 8) | calibData[15]); + rollMinus = temInt = (short)((ushort)(calibData[18] << 8) | calibData[17]); + } + else + { + pitchPlus = temInt = (short)((ushort)(calibData[8] << 8) | calibData[7]); + yawPlus = temInt = (short)((ushort)(calibData[10] << 8) | calibData[9]); + rollPlus = temInt = (short)((ushort)(calibData[12] << 8) | calibData[11]); + pitchMinus = temInt = (short)((ushort)(calibData[14] << 8) | calibData[13]); + yawMinus = temInt = (short)((ushort)(calibData[16] << 8) | calibData[15]); + rollMinus = temInt = (short)((ushort)(calibData[18] << 8) | calibData[17]); + } + + gyroSpeedPlus = temInt = (short)((ushort)(calibData[20] << 8) | calibData[19]); + gyroSpeedMinus = temInt = (short)((ushort)(calibData[22] << 8) | calibData[21]); + accelXPlus = temInt = (short)((ushort)(calibData[24] << 8) | calibData[23]); + accelXMinus = temInt = (short)((ushort)(calibData[26] << 8) | calibData[25]); + + accelYPlus = temInt = (short)((ushort)(calibData[28] << 8) | calibData[27]); + accelYMinus = temInt = (short)((ushort)(calibData[30] << 8) | calibData[29]); + + accelZPlus = temInt = (short)((ushort)(calibData[32] << 8) | calibData[31]); + accelZMinus = temInt = (short)((ushort)(calibData[34] << 8) | calibData[33]); + + int gyroSpeed2x = temInt = (gyroSpeedPlus + gyroSpeedMinus); + calibrationData[0].sensNumer = gyroSpeed2x* SixAxis.GYRO_RES_IN_DEG_SEC; + calibrationData[0].sensDenom = pitchPlus - pitchMinus; + + calibrationData[1].sensNumer = gyroSpeed2x* SixAxis.GYRO_RES_IN_DEG_SEC; + calibrationData[1].sensDenom = yawPlus - yawMinus; + + calibrationData[2].sensNumer = gyroSpeed2x* SixAxis.GYRO_RES_IN_DEG_SEC; + calibrationData[2].sensDenom = rollPlus - rollMinus; + + int accelRange = temInt = accelXPlus - accelXMinus; + calibrationData[3].bias = accelXPlus - accelRange / 2; + calibrationData[3].sensNumer = 2 * SixAxis.ACC_RES_PER_G; + calibrationData[3].sensDenom = accelRange; + + accelRange = temInt = accelYPlus - accelYMinus; + calibrationData[4].bias = accelYPlus - accelRange / 2; + calibrationData[4].sensNumer = 2 * SixAxis.ACC_RES_PER_G; + calibrationData[4].sensDenom = accelRange; + + accelRange = temInt = accelZPlus - accelZMinus; + calibrationData[5].bias = accelZPlus - accelRange / 2; + calibrationData[5].sensNumer = 2 * SixAxis.ACC_RES_PER_G; + calibrationData[5].sensDenom = accelRange; + } + + private void applyCalibs(ref int yaw, ref int pitch, ref int roll, + ref int accelX, ref int accelY, ref int accelZ) + { + CalibData current = calibrationData[0]; + temInt = pitch - current.bias; + pitch = temInt = (int)(temInt * (current.sensNumer / (float)current.sensDenom)); + + current = calibrationData[1]; + temInt = yaw - current.bias; + yaw = temInt = (int)(temInt * (current.sensNumer / (float)current.sensDenom)); + + current = calibrationData[2]; + temInt = roll - current.bias; + roll = temInt = (int)(temInt * (current.sensNumer / (float)current.sensDenom)); + + current = calibrationData[3]; + temInt = accelX - current.bias; + accelX = temInt = (int)(temInt * (current.sensNumer / (float)current.sensDenom)); + + current = calibrationData[4]; + temInt = accelY - current.bias; + accelY = temInt = (int)(temInt * (current.sensNumer / (float)current.sensDenom)); + + current = calibrationData[5]; + temInt = accelZ - current.bias; + accelZ = temInt = (int)(temInt * (current.sensNumer / (float)current.sensDenom)); + } + public void handleSixaxis(byte[] gyro, byte[] accel, DS4State state, double elapsedDelta) { @@ -121,6 +228,8 @@ namespace DS4Windows int AccelY = (short)((ushort)(accel[3] << 8) | accel[2]); int AccelZ = (short)((ushort)(accel[5] << 8) | accel[4]); + applyCalibs(ref currentYaw, ref currentPitch, ref currentRoll, ref AccelX, ref AccelY, ref AccelZ); + SixAxisEventArgs args = null; if (AccelX != 0 || AccelY != 0 || AccelZ != 0) {