void Interrupt(GpioPin sender, GpioPinValueChangedEventArgs args)
{
if (mpu6050 != null)
{
int interrupt_status = ReadByte(INT_STATUS);
if ((interrupt_status & 0x10) != 0)
{
WriteByte(USER_CTRL, 0x44); // reset and enable fifo
}
if ((interrupt_status & 0x1) != 0)
{
var ea = new MpuSensorEventArgs()
{
Status = (byte)interrupt_status,
SamplePeriod = 0.02f,
};
var values = new List <SensorData>();
int count = ReadWord(FIFO_COUNT);
while (count >= SensorBytes)
{
var data = ReadBytes(FIFO_R_W, (byte)SensorBytes);
count -= SensorBytes;
short xa = (short)(data[0] << 8 | data[1]);
short ya = (short)(data[2] << 8 | data[3]);
short za = (short)(data[4] << 8 | data[5]);
short xg = (short)(data[6] << 8 | data[7]);
short yg = (short)(data[8] << 8 | data[9]);
short zg = (short)(data[10] << 8 | data[11]);
var sensorValue = new SensorData();
sensorValue.AccelerationX = (float)xa / (float)16384;
sensorValue.AccelerationY = (float)ya / (float)16384;
sensorValue.AccelerationZ = (float)za / (float)16384;
sensorValue.GyroX = (float)xg / (float)131;
sensorValue.GyroY = (float)yg / (float)131;
sensorValue.GyroZ = (float)zg / (float)131;
values.Add(sensorValue);
}
ea.Values = values.ToArray();
if (SensorInterruptEvent != null && ea.Values.Length > 0)
{
SensorInterruptEvent(this, ea);
}
}
}
}
void MpuSensorInterruptEvent(object sender, MpuSensorEventArgs e)
{
Debug.WriteLine(e.ToString());
}