public GyroscopeSensorEventArgs(YawPitchRoll value, GravityVector gravity, Quaternion _Quaternion, int simpleRate)
{
Value = value;
Gravity = gravity;
this._Quaternion = _Quaternion;
SimpleRate = simpleRate;
}
private void Device_OnInterrupt(GpioPin sender, GpioPinValueChangedEventArgs args)
{
if (Device == null)
{
return;
}
int interruptStatus = Device.getIntStatus();
if ((interruptStatus & 0x10) != 0)
{
Device.resetFIFO();
}
else if ((interruptStatus & 0x02) != 0)
{
int count = Device.getFIFOCount();
while (count >= Constants.DMPPacketSize)
{
byte[] buffer = Device.getFIFOBytes(Constants.DMPPacketSize);
count -= Constants.DMPPacketSize;
Quaternion q = Device.dmpGetQuaternion(buffer);
GravityVector g = Device.dmpGetGravity(q);
YawPitchRoll y = Device.dmpGetYawPitchRoll(q, g);
GyroscopeSensorEventArgs argv = new GyroscopeSensorEventArgs(y, g, q, SimpleRate);
OnInterrupt?.Invoke(this, argv);
}
}
}
public GravityVector dmpGetGravity(Quaternion q)
{
GravityVector v = new GravityVector();
v.X = 2 * (q.X * q.Z - q.W * q.Y);
v.Y = 2 * (q.W * q.X + q.Y * q.Z);
v.Z = q.W * q.W - q.X * q.X - q.Y * q.Y + q.Z * q.Z;
return(v);
}
public YawPitchRoll dmpGetYawPitchRoll(Quaternion q, GravityVector v)
{
YawPitchRoll ypw = new YawPitchRoll()
{
Yaw = Math.Atan2(2 * q.X * q.Y - 2 * q.W * q.Z, 2 * q.W * q.W + 2 * q.X * q.X - 1),
Pitch = Math.Atan(v.X / Math.Sqrt(v.Y * v.Y + v.Z * v.Z)),
Roll = Math.Atan(v.Y / Math.Sqrt(v.X * v.X + v.Z * v.Z))
};
return(ypw);
}