public ModuleAngles(RPY center, RPY leftUpper, RPY leftLower, RPY rightUpper, RPY rightLower)
{
Center = center;
LeftUpper = leftUpper;
LeftLower = leftLower;
RightUpper = rightUpper;
RightLower = rightLower;
}
public static ModuleAngles ParseDataForOrientationAngles(byte[] bleData)
{
const float bleScaling = 325.94932f;
short temp;
float tempRoll, tempPitch, tempYaw;
RPY postCenter;
RPY postLeftUpper;
RPY postLeftLower;
RPY postRightUpper;
RPY postRightLower;
//*********************************
// Author: Elijah Schuldt
// Ported: Matt Brown
//Center
//*********************************
temp = (short)((bleData[3] & 0xE0) << 3);
temp |= (short)bleData[0];
if (temp > 1024)
{
temp -= 2048;
}
tempRoll = (float)temp / bleScaling;
temp = (short)((bleData[3] & 0x18) << 5);
temp |= (short)bleData[1];
if (temp > 512)
{
temp -= 1024;
}
tempPitch = (float)temp / bleScaling;
temp = (short)((bleData[3] & 0x07) << 8);
temp |= (short)bleData[2];
if (temp > 1024)
{
temp -= 2048;
}
tempYaw = (float)temp / bleScaling;
postCenter = new RPY(tempRoll, tempPitch, tempYaw);
//*********************************
//leftUpper
//*********************************
temp = (short)((bleData[7] & 0xE0) << 3);
temp |= (short)bleData[4];
if (temp > 1024)
{
temp -= 2048;
}
tempRoll = (float)temp / bleScaling;
temp = (short)((bleData[7] & 0x18) << 5);
temp |= (short)bleData[5];
if (temp > 512)
{
temp -= 1024;
}
tempPitch = (float)temp / bleScaling;
temp = (short)((bleData[7] & 0x07) << 8);
temp |= (short)bleData[6];
if (temp > 1024)
{
temp -= 2048;
}
tempYaw = (float)temp / bleScaling;
postLeftUpper = new RPY(tempRoll, tempPitch, tempYaw);
//*********************************
//leftLower
//*********************************
temp = (short)((bleData[11] & 0xE0) << 3);
temp |= (short)bleData[8];
if (temp > 1024)
{
temp -= 2048;
}
tempRoll = (float)temp / bleScaling;
temp = (short)((bleData[11] & 0x18) << 5);
temp |= (short)bleData[9];
if (temp > 512)
{
temp -= 1024;
}
tempPitch = (float)temp / bleScaling;
temp = (short)((bleData[11] & 0x07) << 8);
temp |= (short)bleData[10];
if (temp > 1024)
{
temp -= 2048;
}
tempYaw = (float)temp / bleScaling;
postLeftLower = new RPY(tempRoll, tempPitch, tempYaw);
//*********************************
//rightUpper
//*********************************
temp = (short)((bleData[15] & 0xE0) << 3);
temp |= (short)bleData[12];
if (temp > 1024)
{
temp -= 2048;
}
tempRoll = (float)temp / bleScaling;
temp = (short)((bleData[15] & 0x18) << 5);
temp |= (short)bleData[13];
if (temp > 512)
{
temp -= 1024;
}
tempPitch = (float)temp / bleScaling;
temp = (short)((bleData[15] & 0x07) << 8);
temp |= (short)bleData[14];
if (temp > 1024)
{
temp -= 2048;
}
tempYaw = (float)temp / bleScaling;
postRightUpper = new RPY(tempRoll, tempPitch, tempYaw);
//*********************************
//rightLower
//*********************************
temp = (short)((bleData[19] & 0xE0) << 3);
temp |= (short)bleData[16];
if (temp > 1024)
{
temp -= 2048;
}
tempRoll = (float)temp / bleScaling;
temp = (short)((bleData[19] & 0x18) << 5);
temp |= (short)bleData[17];
if (temp > 512)
{
temp -= 1024;
}
tempPitch = (float)temp / bleScaling;
temp = (short)((bleData[19] & 0x07) << 8);
temp |= (short)bleData[18];
if (temp > 1024)
{
temp -= 2048;
}
tempYaw = (float)temp / bleScaling;
postRightLower = new RPY(tempRoll, tempPitch, tempYaw);
var result = new ModuleAngles(
postCenter,
postLeftUpper,
postLeftLower,
postRightUpper,
postRightLower);
return(result);
}
