private GyroData ParseCompleteMessage(byte[] message)
{
// According to https://habrahabr.ru/post/174115/
// MSB first
var data = new GyroData
{
GyroId = message[1]
};
// skip start byte and GyroId
var currentIndex = 2;
// need to correctly assign values
var iterations = 0;
while (currentIndex < message.Length)
{
var value = 0;
if (message[currentIndex] == StartByte)
{
switch (message[currentIndex + 1])
{
case StartByte:
value = MakeIntFromBytes(StartByte, message[currentIndex + 2]);
break;
case AdditionalByte:
value = MakeIntFromBytes(StopByte, message[currentIndex + 2]);
break;
}
currentIndex += 3;
}
else
{
value = MakeIntFromBytes(message[currentIndex], message[currentIndex + 1]);
currentIndex += 2;
}
AssignValue(data, value, iterations);
iterations++;
}
return(data);
}
private void AssignValue(GyroData data, int value, int iteration)
{
// we need 3 iterations for one device.
// sequence in received data is: Mag, Acc, Gyro
// 0 is X, 1 is Y, 3 is Z
if (iteration > 5)
{
data.SetGyro(iteration - 6, value);
}
else if (iteration > 2)
{
data.SetAcc(iteration - 3, value);
}
else
{
data.SetMag(iteration, value);
}
}