public bool Test()
{
// Read Self Test Registers
byte[] FactoryTrims = GetFactoryTrims(Bus.ReadRegister(Address, 0x0D, 4));
// Activate self test for accelerometer
EnableSelfTest(true);
UpdateState();
MPUData SelfTestResponse = GetReading();
// Disable self test
EnableSelfTest(false);
UpdateState();
MPUData RawTest = GetReading();
// Check each field for self test deviation
bool ReturnValue = true;
ReturnValue &= FieldOkay(SelfTestResponse.AccelX, RawTest.AccelX, FactoryTrims[0]);
ReturnValue &= FieldOkay(SelfTestResponse.AccelY, RawTest.AccelY, FactoryTrims[1]);
ReturnValue &= FieldOkay(SelfTestResponse.AccelZ, RawTest.AccelZ, FactoryTrims[3]);
ReturnValue &= FieldOkay(SelfTestResponse.GyroX, RawTest.GyroX, FactoryTrims[2]);
ReturnValue &= FieldOkay(SelfTestResponse.GyroY, RawTest.GyroY, FactoryTrims[4]);
ReturnValue &= FieldOkay(SelfTestResponse.GyroZ, RawTest.GyroZ, FactoryTrims[5]);
return(ReturnValue);
}
public void UpdateState()
{
LastReading = new MPUData();
byte[] AX = Bus.ReadRegister(Address, 0x3B, 2);
byte[] AY = Bus.ReadRegister(Address, 0x3D, 2);
byte[] AZ = Bus.ReadRegister(Address, 0x3F, 2);
byte[] GX = Bus.ReadRegister(Address, 0x43, 2);
byte[] GY = Bus.ReadRegister(Address, 0x45, 2);
byte[] GZ = Bus.ReadRegister(Address, 0x47, 2);
LastReading.AccelX = ((AX[0] << 8) | AX[1]) / AccelSensitivity[Configuration.AccelSensitivity];
LastReading.AccelY = ((AY[0] << 8) | AY[1]) / AccelSensitivity[Configuration.AccelSensitivity];
LastReading.AccelZ = ((AZ[0] << 8) | AZ[1]) / AccelSensitivity[Configuration.AccelSensitivity];
LastReading.GyroX = ((GX[0] << 8) | GX[1]) / GyroSensitivity[Configuration.GyroSensitivity];
LastReading.GyroY = ((GY[0] << 8) | GY[1]) / GyroSensitivity[Configuration.GyroSensitivity];
LastReading.GyroZ = ((GZ[0] << 8) | GZ[1]) / GyroSensitivity[Configuration.GyroSensitivity];
}
