The-Homebrew-Cloud/cemu-DS4Windows
2
0
Fork 0
mirror of https://github.com/cemu-project/DS4Windows.git synced 2024-11-29 20:44:20 +01:00
Code Issues Packages Projects Releases Wiki Activity

First acceptable version of gyro calibration support

Browse Source 
Related to issue #103.
This commit is contained in:
Travis Nickles 2017-10-10 17:45:42 -05:00
parent 957ad3b1d5
commit b1cd3a76e9
2 changed files with 115 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
DS4Windows/DS4Library/DS4Device.cs
View File

@ -448,6 +448,11 @@ namespace DS4Windows
touchpad = new DS4Touchpad(); touchpad = new DS4Touchpad();
sixAxis = new DS4SixAxis(); 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() private void timeoutTestThread()

109
DS4Windows/DS4Library/DS4Sixaxis.cs
View File

@ -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 class DS4SixAxis
{ {
public event EventHandler<SixAxisEventArgs> SixAccelMoved = null; public event EventHandler<SixAxisEventArgs> SixAccelMoved = null;
private SixAxis sPrev = null, now = 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() public DS4SixAxis()
{ {
@ -111,6 +123,101 @@ namespace DS4Windows
now = new SixAxis(0, 0, 0, 0, 0, 0, 0.0); 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, public void handleSixaxis(byte[] gyro, byte[] accel, DS4State state,
double elapsedDelta) double elapsedDelta)
{ {
@ -121,6 +228,8 @@ namespace DS4Windows
int AccelY = (short)((ushort)(accel[3] << 8) | accel[2]); int AccelY = (short)((ushort)(accel[3] << 8) | accel[2]);
int AccelZ = (short)((ushort)(accel[5] << 8) | accel[4]); 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; SixAxisEventArgs args = null;
if (AccelX != 0 || AccelY != 0 || AccelZ != 0) if (AccelX != 0 || AccelY != 0 || AccelZ != 0)
{ {