public double ReadHeading()
{
WiringPiLib.WiringPiI2CWrite(fd, POST_HEADING);
Thread.Sleep(2);
byte[] response = new byte[6];
for (int byteCount = 0; byteCount < 6; byteCount++)
{
response [byteCount] = (byte)WiringPiLib.WiringPiI2CRead(fd);
}
short head = (short)((response [0] << 8) | response [1]);
//short pitch = (short)((response [2] << 8) | response [3]);
//short roll = (short)((response [4] << 8) | response [5]);
return(((double)head) / (10)); //Head in 10tel Grad
//Console.WriteLine("Head: " + head);
//Console.WriteLine("Pitch: " + pitch);
//Console.WriteLine("Roll: " + roll);
}