| private ReadRegisterSingle ( byte register ) : byte | ||
| register | byte | |
| return | byte | |
//This works
// Accelerometer and gyroscope self test; check calibration wrt factory settings
// Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass
internal bool Mpu9150SelfTest(out double[] destination)
{
destination = new double[7];
byte[] rawData = { 0, 0, 0, 0 };
byte[] selfTest = { 0, 0, 0, 0, 0, 0 };
double[] factoryTrim = { 0, 0, 0, 0, 0, 0 };
// Configure the accelerometer for self-test
_mpu9150.Write(ACCEL_CONFIG, 0xF0);
// Enable self test on all three axes and set accelerometer range to +/- 8 g
_mpu9150.Write(GYRO_CONFIG, 0xE0);
// Enable self test on all three axes and set gyro range to +/- 250 degrees/s
Task.Delay(250); // Delay a while to let the device execute the self-test
rawData[0] = _mpu9150.ReadRegisterSingle(SELF_TEST_X); // X-axis self-test results
rawData[1] = _mpu9150.ReadRegisterSingle(SELF_TEST_Y); // Y-axis self-test results
rawData[2] = _mpu9150.ReadRegisterSingle(SELF_TEST_Z); // Z-axis self-test results
rawData[3] = _mpu9150.ReadRegisterSingle(SELF_TEST_A); // Mixed-axis self-test results
// Extract the acceleration test results first
selfTest[0] = (byte)((rawData[0] >> 3) | (rawData[3] & 0x30) >> 4);
// XA_TEST result is a five-bit unsigned integer
selfTest[1] = (byte)((rawData[1] >> 3) | (rawData[3] & 0x0C) >> 4);
// YA_TEST result is a five-bit unsigned integer
selfTest[2] = (byte)((rawData[2] >> 3) | (rawData[3] & 0x03) >> 4);
// ZA_TEST result is a five-bit unsigned integer
// Extract the gyration test results first
selfTest[3] = (byte)(rawData[0] & 0x1F); // XG_TEST result is a five-bit unsigned integer
selfTest[4] = (byte)(rawData[1] & 0x1F); // YG_TEST result is a five-bit unsigned integer
selfTest[5] = (byte)(rawData[2] & 0x1F); // ZG_TEST result is a five-bit unsigned integer
// Process results to allow final comparison with factory set values
factoryTrim[0] = (4096.0f * 0.34f) * (Math.Pow((0.92f / 0.34f), ((selfTest[0] - 1.0f) / 30.0f)));
// FT[Xa] factory trim calculation
factoryTrim[1] = (4096.0f * 0.34f) * (Math.Pow((0.92f / 0.34f), ((selfTest[1] - 1.0f) / 30.0f)));
// FT[Ya] factory trim calculation
factoryTrim[2] = (4096.0f * 0.34f) * (Math.Pow((0.92f / 0.34f), ((selfTest[2] - 1.0f) / 30.0f)));
// FT[Za] factory trim calculation
factoryTrim[3] = (25.0f * 131.0f) * (Math.Pow(1.046f, (selfTest[3] - 1.0f))); // FT[Xg] factory trim calculation
factoryTrim[4] = (-25.0f * 131.0f) * (Math.Pow(1.046f, (selfTest[4] - 1.0f)));
// FT[Yg] factory trim calculation
factoryTrim[5] = (25.0f * 131.0f) * (Math.Pow(1.046f, (selfTest[5] - 1.0f))); // FT[Zg] factory trim calculation
// Output self-test results and factory trim calculation if desired
// Serial.println(selfTest[0]); Serial.println(selfTest[1]); Serial.println(selfTest[2]);
// Serial.println(selfTest[3]); Serial.println(selfTest[4]); Serial.println(selfTest[5]);
// Serial.println(factoryTrim[0]); Serial.println(factoryTrim[1]); Serial.println(factoryTrim[2]);
// Serial.println(factoryTrim[3]); Serial.println(factoryTrim[4]); Serial.println(factoryTrim[5]);
// Report results as a ratio of (STR - FT)/FT; the change from Factory Trim of the Self-Test Response
// To get to percent, must multiply by 100 and subtract result from 100
for (var i = 0; i < 6; i++)
{
destination[i] = (100.0f + 100.0f * (selfTest[i] - factoryTrim[i]) / factoryTrim[i]);
// Report percent differences
}
if (destination[0] < 1.0f && destination[1] < 1.0f && destination[2] < 1.0f && destination[3] < 1.0f && destination[4] < 1.0f && destination[5] < 1.0f)
{
Debug.WriteLine("MPU Self test passed.");
return(true);
}
return(false);
}
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);
}
}
