private async void _interruptPin_ValueChanged(GpioPin sender, GpioPinValueChangedEventArgs args) { var ad = ReadAccelData(); //Debug.WriteLine($"Accel {ad[0]} {ad[1]} {ad[2]}"); var gd = ReadGyroData(); //Debug.WriteLine($"Gyrol {gd[0]} {gd[1]} {gd[2]}"); var sv = new MpuSensorValue { AccelerationX = ad[0] / 16384d, AccelerationY = ad[1] / 16384d, AccelerationZ = ad[2] / 16384d, GyroX = gd[0] / 131d, GyroY = gd[1] / 131d, GyroZ = gd[2] / 131d }; var gain = 0.00875; var xRotationPerSecond = sv.GyroX * gain; //xRotationPerSecond is the rate of rotation per second. var loopPeriod = 0.025; //loop period - 0.02 _gyroXangle += xRotationPerSecond * loopPeriod; //var M_PI = 3.14159265358979323846; var radToDeg = 57.29578; var accXangle = (Math.Atan2(sv.AccelerationY, sv.AccelerationZ) + 3.14159265358979323846) * radToDeg; var complementaryFilterConstant = 0.98; _cFangleX = complementaryFilterConstant * (_cFangleX + xRotationPerSecond * loopPeriod) + (1 - complementaryFilterConstant) * accXangle; //Debug.WriteLine("X: " + sv.GyroX + ", Y: " + sv.GyroY + ", Z: " + sv.GyroZ); Debug.WriteLine("CFangleX: " + _cFangleX); //Debug.WriteLine("AccelX: " + sv.AccelerationX + ", AccelY: " + sv.AccelerationY + ", AccelZ: " + sv.AccelerationZ); Debug.WriteLine("---------------------------"); await Task.Delay(5); }
private async void _interruptPin_ValueChanged(GpioPin sender, GpioPinValueChangedEventArgs args) { var ad = ReadAccelData(); //Debug.WriteLine($"Accel {ad[0]} {ad[1]} {ad[2]}"); var gd = ReadGyroData(); //Debug.WriteLine($"Gyrol {gd[0]} {gd[1]} {gd[2]}"); var sv = new MpuSensorValue { AccelerationX = ad[0] / 16384d, AccelerationY = ad[1] / 16384d, AccelerationZ = ad[2] / 16384d, GyroX = gd[0] / 131d, GyroY = gd[1] / 131d, GyroZ = gd[2] / 131d }; var gain = 0.00875; var xRotationPerSecond = sv.GyroX * gain;//xRotationPerSecond is the rate of rotation per second. var loopPeriod = 0.025;//loop period - 0.02 _gyroXangle += xRotationPerSecond * loopPeriod; //var M_PI = 3.14159265358979323846; var radToDeg = 57.29578; var accXangle = (Math.Atan2(sv.AccelerationY, sv.AccelerationZ) + 3.14159265358979323846) * radToDeg; var complementaryFilterConstant = 0.98; _cFangleX = complementaryFilterConstant * (_cFangleX + xRotationPerSecond * loopPeriod) + (1 - complementaryFilterConstant) * accXangle; //Debug.WriteLine("X: " + sv.GyroX + ", Y: " + sv.GyroY + ", Z: " + sv.GyroZ); Debug.WriteLine("CFangleX: " + _cFangleX); //Debug.WriteLine("AccelX: " + sv.AccelerationX + ", AccelY: " + sv.AccelerationY + ", AccelZ: " + sv.AccelerationZ); Debug.WriteLine("---------------------------"); await Task.Delay(5); }
private async void Interrupt(GpioPin sender, GpioPinValueChangedEventArgs args) { await Task.Delay(10); if (_mpu9150 == null) { return; } int interruptStatus = _mpu9150.ReadRegisterSingle((byte)Mpu9150Setup.InterruptStatus); if ((interruptStatus & 0x10) != 0) { _mpu9150.Write((byte)Mpu9150Setup.UserCtrl, 0x44);// reset - enable fifo } if ((interruptStatus & 0x1) == 0) { return; } var ea = new MpuSensorEventArgs(); ea.Status = (byte)interruptStatus; ea.SamplePeriod = 0.02f; var l = new List <MpuSensorValue>(); int count = _mpu9150.ReadUshort((byte)Mpu9150Setup.FifoCount); while (count >= SensorBytes) { _mpu9150.Write((byte)Mpu9150Setup.FifoReadWrite); byte[] buffer; _mpu9150.Read(SensorBytes, out buffer); count -= SensorBytes; var xa = (short)(buffer[0] << 8 | buffer[1]); var ya = (short)(buffer[2] << 8 | buffer[3]); var za = (short)(buffer[4] << 8 | buffer[5]); var xg = (short)(buffer[6] << 8 | buffer[7]); var yg = (short)(buffer[8] << 8 | buffer[9]); var zg = (short)(buffer[10] << 8 | buffer[11]); var sv = new MpuSensorValue { AccelerationX = xa / 16384d, AccelerationY = ya / 16384d, AccelerationZ = za / 16384d, GyroX = xg / 131d, GyroY = yg / 131d, GyroZ = zg / 131d }; l.Add(sv); var gain = 0.00875; var xRotationPerSecond = sv.GyroX * gain; //xRotationPerSecond is the rate of rotation per second. var loopPeriod = 0.015; //loop period - 0.02 _gyroXangle += xRotationPerSecond * loopPeriod; var radToDeg = 57.29578; var accXangle = (Math.Atan2(sv.AccelerationY, sv.AccelerationZ) + Math.PI) * radToDeg; var complementaryFilterConstant = 0.98; _cFangleX = complementaryFilterConstant * (_cFangleX + xRotationPerSecond * loopPeriod) + (1 - complementaryFilterConstant) * accXangle; //Debug.WriteLine("X: " + sv.GyroX + ", Y: " + sv.GyroY + ", Z: " + sv.GyroZ); //Debug.WriteLine("CFangleX: " + _cFangleX); //Debug.WriteLine("AccelX: " + sv.AccelerationX + ", AccelY: " + sv.AccelerationY + ", AccelZ: " + sv.AccelerationZ); } ea.Values = l.ToArray(); if (SensorInterruptEvent == null) { return; } if (ea.Values.Length > 0) { SensorInterruptEvent(this, ea); } }
private async void Interrupt(GpioPin sender, GpioPinValueChangedEventArgs args) { await Task.Delay(10); if (_mpu9150 == null) return; int interruptStatus = _mpu9150.ReadRegisterSingle((byte)Mpu9150Setup.InterruptStatus); if ((interruptStatus & 0x10) != 0) _mpu9150.Write((byte)Mpu9150Setup.UserCtrl, 0x44);// reset - enable fifo if ((interruptStatus & 0x1) == 0) return; var ea = new MpuSensorEventArgs(); ea.Status = (byte)interruptStatus; ea.SamplePeriod = 0.02f; var l = new List<MpuSensorValue>(); int count = _mpu9150.ReadUshort((byte)Mpu9150Setup.FifoCount); while (count >= SensorBytes) { _mpu9150.Write((byte)Mpu9150Setup.FifoReadWrite); byte[] buffer; _mpu9150.Read(SensorBytes, out buffer); count -= SensorBytes; var xa = (short)(buffer[0] << 8 | buffer[1]); var ya = (short)(buffer[2] << 8 | buffer[3]); var za = (short)(buffer[4] << 8 | buffer[5]); var xg = (short)(buffer[6] << 8 | buffer[7]); var yg = (short)(buffer[8] << 8 | buffer[9]); var zg = (short)(buffer[10] << 8 | buffer[11]); var sv = new MpuSensorValue { AccelerationX = xa / 16384d, AccelerationY = ya / 16384d, AccelerationZ = za / 16384d, GyroX = xg / 131d, GyroY = yg / 131d, GyroZ = zg / 131d }; l.Add(sv); var gain = 0.00875; var xRotationPerSecond = sv.GyroX * gain;//xRotationPerSecond is the rate of rotation per second. var loopPeriod = 0.015;//loop period - 0.02 _gyroXangle += xRotationPerSecond * loopPeriod; var radToDeg = 57.29578; var accXangle = (Math.Atan2(sv.AccelerationY, sv.AccelerationZ) + Math.PI) * radToDeg; var complementaryFilterConstant = 0.98; _cFangleX = complementaryFilterConstant * (_cFangleX + xRotationPerSecond * loopPeriod) + (1 - complementaryFilterConstant) * accXangle; //Debug.WriteLine("X: " + sv.GyroX + ", Y: " + sv.GyroY + ", Z: " + sv.GyroZ); //Debug.WriteLine("CFangleX: " + _cFangleX); //Debug.WriteLine("AccelX: " + sv.AccelerationX + ", AccelY: " + sv.AccelerationY + ", AccelZ: " + sv.AccelerationZ); } ea.Values = l.ToArray(); if (SensorInterruptEvent == null) return; if (ea.Values.Length > 0) { SensorInterruptEvent(this, ea); } }