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