public MPU6050Device()
{
int exceptionCount = 0;
while (true)
{
try
{
Log.WriteLine("Initialising the MPU-6050 accelerometer and gyro package...");
// Check connectivity
byte[] data = new byte[] { new byte() };
_I2CBus.ReadRegister(_i2cConfig, MPU6050Registers.WHO_AM_I, data, _timeout);
Debug.Assert((data[0] & 0x68) == 0x68);
// PWR_MGMT_1 = Power Management 1
// 0xF9 = 11111001: Device Reset=true, Sleep=true, Cycle=true, Temp Sensor=On, Clock Select=PLL with X axis gyroscope reference
// TODO: Given the reset, half of these values are probably ignored, and the defaults are used instead.
_I2CBus.WriteRegister(_i2cConfig, MPU6050Registers.PWR_MGMT_1, 0xF9, _timeout);
Thread.Sleep(100); // allow to reset
// Exit sleep mode (CARE! Not sure why, but this resets the device too! - only set config after this line)
// TODO: Figure out what's going on here. Perhaps the reset at the line above was taking some time, hence why the Scale settings (below) were ignored
_I2CBus.WriteRegister(_i2cConfig, MPU6050Registers.PWR_MGMT_1, 0x01, _timeout);
Thread.Sleep(100); // allow to reset
// Gyro Scale
_gyroRange = GyroConfig.Range.plusMinus0500dps;
_I2CBus.WriteRegister(_i2cConfig, MPU6050Registers.GYRO_CONFIG, GyroConfig.Build(_gyroRange), _timeout);
// Accelerometer Scale
_accelRange = AccelConfig.Range.plusMinus08G;
_I2CBus.WriteRegister(_i2cConfig, MPU6050Registers.ACCEL_CONFIG, AccelConfig.Build(_accelRange), _timeout);
// Confirm I2C_MST_EN is disabled (required to connect to the HMC5883L)
data = new byte[] { new byte() };
_I2CBus.ReadRegister(_i2cConfig, 0x6A, data, 1000);
Debug.Assert((data[0] & 0x10) == 0);
// Turn on I2C Bypass (required to connect to the HMC5883L)
_I2CBus.WriteRegister(_i2cConfig, 0x37, 0x02, 1000);
data = new byte[] { new byte() };
_I2CBus.ReadRegister(_i2cConfig, 0x37, data, 1000);
Debug.Assert((data[0] & 0x02) == 0x02);
Log.WriteLine("MPU-6050 Ready\n");
break;
}
catch (System.Exception)
{
exceptionCount++;
if (exceptionCount != 3)
{
Log.WriteLine("Exception during initialisation! Retrying...\n");
//I2CBus.DestroySingleton();
//_I2CBus = I2CBus.GetInstance();
continue;
}
Log.WriteLine("Exception during initialisation! Throwing...");
throw;
}
}
}
public static AccelerationAndGyroData Build(byte[] results, GyroConfig.Range gyroRange, AccelConfig.Range accelRange)
{
// Result for the acceleration sensor, merged together by bit shifting
var RawAccelerationX = ToShort(results[0], results[1]);
var RawAccelerationY = ToShort(results[2], results[3]);
var RawAccelerationZ = ToShort(results[4], results[5]);
// Results for temperature (not tested)
var RawTemperature = ToShort(results[6], results[7]);
// Result for the gyro sensor, merged together by bit shifting
var RawGyroX = ToShort(results[8], results[9]);
var RawGyroY = ToShort(results[10], results[11]);
var RawGyroZ = ToShort(results[12], results[13]);
// Now convert to sensible numbers
var Temp = ParseTemp(RawTemperature);
var Ax = ConvertRawValueToUnits(RawAccelerationX, (short)accelRange);
var Ay = ConvertRawValueToUnits(RawAccelerationY, (short)accelRange);
var Az = ConvertRawValueToUnits(RawAccelerationZ, (short)accelRange);
var Rx = ConvertRawValueToUnits(RawGyroX, (short)gyroRange);
var Ry = ConvertRawValueToUnits(RawGyroY, (short)gyroRange);
var Rz = ConvertRawValueToUnits(RawGyroZ, (short)gyroRange);
return(new AccelerationAndGyroData(Ax, Ay, Az, Temp, Rx, Ry, Rz));
}