static void Main(string[] args)
{
using (var bus = I2CBus.Open("/dev/i2c-1"))
{
var mpu6050 = new Mpu6050Api(bus);
Console.Write("Initialization...");
mpu6050.Init();
Console.WriteLine("OK");
Console.Write("Calibration...");
var baseReadings = mpu6050.ReadSensors(1000);
Console.WriteLine("OK");
Console.WriteLine("Base readings: " + baseReadings);
Console.WriteLine();
Console.WriteLine("Streaming... (Press CTRL+C to stop)");
Readings accumulator = new Readings();
for (;;)
{
var r = mpu6050.ReadSensors(100).DiffrenceTo(baseReadings);
accumulator.Accumulate(r);
DateTime now = DateTime.Now;
Console.WriteLine(now + "." + now.Millisecond.ToString("000") + ": " + accumulator);
}
}
}
public Readings Decode()
{
MemoryStream ms = new MemoryStream(_buf);
Func<byte> b = () => (byte)ms.ReadByte();
Func<short> s = () => (short)(((short)b() << 8) | b());
Func<double> sg = () => ((double)s())/(short.MaxValue/2) ;
var ret = new Readings
{
Acc = new double[3] { sg(), sg(), sg() },
Temp = s() / 340.0 + 36.53,
Gyro = new double[3] { sg(), sg(), sg() }
};
return ret;
}
public Readings ReadSensors(int samples)
{
Readings[] accumulator = new Readings[samples];
for (int i = 0; i < samples; i++)
{
accumulator[i] = ReadRawReadings().Decode();
System.Threading.Thread.Sleep(1);
}
Readings r = new Readings()
{
Acc = new double[3] { accumulator.Average(a => a.Acc[0]), accumulator.Average(a => a.Acc[1]), accumulator.Average(a => a.Acc[2]), },
Temp = accumulator.Average(a => a.Temp),
Gyro = new double[3] { accumulator.Average(a => a.Gyro[0]), accumulator.Average(a => a.Gyro[1]), accumulator.Average(a => a.Gyro[2]), }
};
return r;
}