예제 #1
0
        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);
        }
예제 #2
0
 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);
     }
 }