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); }