public Tuple <short[], short[]> ApplyCalToInReport(byte[] inputReport, int startOffs = 0)
{
int idx = startOffs + 13; //first byte of gyro is here
var prevVals = new short[6];
var currVals = new short[6];
for (int i = 0; i < 6; i++)
{
short axisVal = (short)(((ushort)inputReport[idx + 1] << 8) | (ushort)inputReport[idx + 0]);
prevVals[i] = axisVal;
if (i == 0)
{
axisVal = Pitch.apply(axisVal);
}
else if (i == 1)
{
axisVal = Yaw.apply(axisVal);
}
else if (i == 2)
{
axisVal = Roll.apply(axisVal);
}
else if (i == 3)
{
axisVal = X.apply(axisVal);
}
else if (i == 4)
{
axisVal = Y.apply(axisVal);
}
else if (i == 5)
{
axisVal = Z.apply(axisVal);
}
currVals[i] = axisVal;
//put it back into the input report (little endian short)
inputReport[idx++] = (byte)(((ushort)axisVal >> 0) & 0xFF);
inputReport[idx++] = (byte)(((ushort)axisVal >> 8) & 0xFF);
}
return(new Tuple <short[], short[]>(prevVals, currVals));
}
public void ApplyCalToInReport(byte[] inputReport, int startOffs = 0)
{
int idx = startOffs + 13; //first byte of gyro is here
for (int i = 0; i < 6; i++)
{
short axisVal = (short)(((ushort)inputReport[idx + 1] << 8) | (ushort)inputReport[idx + 0]);
if (i == 0)
{
axisVal = Pitch.apply(axisVal);
}
else if (i == 1)
{
axisVal = Yaw.apply(axisVal);
}
else if (i == 2)
{
axisVal = Roll.apply(axisVal);
}
else if (i == 3)
{
axisVal = X.apply(axisVal);
}
else if (i == 4)
{
axisVal = Y.apply(axisVal);
}
else if (i == 5)
{
axisVal = Z.apply(axisVal);
}
//put it back into the input report (little endian short)
inputReport[idx++] = (byte)(((ushort)axisVal >> 0) & 0xFF);
inputReport[idx++] = (byte)(((ushort)axisVal >> 8) & 0xFF);
}
}