/// <summary> Gets the vector corresponding to the orientation of the magnetometer according to the type specified. </summary>
/// <returns>
/// The vector.
/// If VECTOR_ACCELEROMETER was passed, the vector is the 3-axis acceleration in mG.
/// If VECTOR_MAGNETOMETER was passed, the vector is the Magnetic Field Strength vector in uT.
/// If VECTOR_GYROSCOPE was passed, the vector is Angular Velocity Vector in Degrees / sec.
/// If VECTOR_EULER was passed, the vector is the orientation as (Roll, Pitch, Yaw) in degrees
/// If VECTOR_LINEARACCEL was passed, returns the linear acceleration vector mG.
/// If VECTOR_GRAVITY was passed, returns the gravitational acceleration vector in mG.
/// </returns>/
/// <param name="VectorType"> Type of vector to be read. </param>
public Tuple <float, float, float> GetVector(VectorType VectorType)
{
short X = 0;
short Y = 0;
short Z = 0;
byte[] buffer = I2C.ReadRegister(Address, (byte)VectorType, 6);
X = (short)((buffer[1] << 8) | buffer[0]);
Y = (short)((buffer[3] << 8) | buffer[2]);
Z = (short)((buffer[5] << 8) | buffer[4]);
switch (VectorType)
{
case VectorType.VECTOR_MAGNETOMETER:
case VectorType.VECTOR_EULER:
case VectorType.VECTOR_GYROSCOPE:
return(new Tuple <float, float, float>(X / 16.0f, Y / 16.0f, Z / 16.0f));
default:
return(new Tuple <float, float, float>(X / 100.0f, Y / 100.0f, Z / 100.0f));
}
}
private byte read8(byte addr)
{
return(i2c.ReadRegister(i2caddr, addr, 1)[0]);
}
public void Configure(MPU6050Configuration Config)
{
Bus.WriteRegister(Address, 0x19, new byte[] { Config.SampleRateDivider });
// Get Current Configs
byte ConfigRegister = Bus.ReadRegister(Address, 0x1A, 1)[0];
byte GyroConfig = Bus.ReadRegister(Address, 0x1B, 1)[0];
byte AccelConfig = Bus.ReadRegister(Address, 0x1C, 1)[0];
// Clean the configs
GyroConfig &= 0b11100111;
AccelConfig &= 0b11100111;
ConfigRegister &= 0b11111100;
// Set Config
GyroConfig |= (byte)(Config.GyroSensitivity << 3);
AccelConfig |= (byte)(Config.AccelSensitivity << 3);
ConfigRegister |= Config.DLPFSetup;
// Write the config
Bus.WriteRegister(Address, 0x1A, new byte[] { ConfigRegister });
Bus.WriteRegister(Address, 0x1B, new byte[] { GyroConfig });
Bus.WriteRegister(Address, 0x1C, new byte[] { AccelConfig });
// Ensure self test is disabled
EnableSelfTest(false);
}
private byte read8(byte reg)
{
return(i2c.ReadRegister(address, reg, 1)[0]);
}