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);
}
// Uses a default configuration
public void Configure()
{
MPU6050Configuration Configuration = new MPU6050Configuration
{
AccelSensitivity = 0,
GyroSensitivity = 0,
SampleRateDivider = 0,
DLPFSetup = 0
};
Configure(Configuration);
}
