void initialize_imu() {
// join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
// **************************************************************
// It is best to configure I2C to 400 kHz.
// If you are using an Arduino DUE, modify the variable TWI_CLOCK to 400000, defined in the file:
// c:/Program Files/Arduino/hardware/arduino/sam/libraries/Wire/Wire.h
// If you are using any other Arduino instead of the DUE, uncomment the following line:
//TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz) //This line should be commented if you are using Arduino DUE
// **************************************************************
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
// initialize serial communication
Serial.begin(250000);
// initialize device
Serial.println(F("Initializing I2C devices..."));
mpu.initialize();
// verify connection
Serial.println(F("Testing device connections..."));
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
// TODO: Compute these parameters
// mpu.setXAccelOffset(-1600);
// mpu.setYAccelOffset(-180);
// mpu.setZAccelOffset(650);
// mpu.setXGyroOffset(0);
// mpu.setYGyroOffset(0);
// mpu.setZGyroOffset(0);
mpu.setFullScaleGyroRange(0);
calibrate_imu();
// Magnetometer configuration
mpu.setI2CMasterModeEnabled(0);
mpu.setI2CBypassEnabled(1);
Wire.beginTransmission(HMC5883L_DEFAULT_ADDRESS);
Wire.write(0x02);
Wire.write(0x00); // Set continuous mode
Wire.endTransmission();
delay(5);
Wire.beginTransmission(HMC5883L_DEFAULT_ADDRESS);
Wire.write(0x00);
Wire.write(B00011000); // 75Hz
Wire.endTransmission();
delay(5);
mpu.setI2CBypassEnabled(0);
// X axis word
mpu.setSlaveAddress(0, HMC5883L_DEFAULT_ADDRESS | 0x80); // 0x80 turns 7th bit ON, according to datasheet, 7th bit controls Read/Write direction
mpu.setSlaveRegister(0, HMC5883L_RA_DATAX_H);
mpu.setSlaveEnabled(0, true);
mpu.setSlaveWordByteSwap(0, false);
mpu.setSlaveWriteMode(0, false);
mpu.setSlaveWordGroupOffset(0, false);
mpu.setSlaveDataLength(0, 2);
// Y axis word
mpu.setSlaveAddress(1, HMC5883L_DEFAULT_ADDRESS | 0x80);
mpu.setSlaveRegister(1, HMC5883L_RA_DATAY_H);
mpu.setSlaveEnabled(1, true);
mpu.setSlaveWordByteSwap(1, false);
mpu.setSlaveWriteMode(1, false);
mpu.setSlaveWordGroupOffset(1, false);
mpu.setSlaveDataLength(1, 2);
// Z axis word
mpu.setSlaveAddress(2, HMC5883L_DEFAULT_ADDRESS | 0x80);
mpu.setSlaveRegister(2, HMC5883L_RA_DATAZ_H);
mpu.setSlaveEnabled(2, true);
mpu.setSlaveWordByteSwap(2, false);
mpu.setSlaveWriteMode(2, false);
mpu.setSlaveWordGroupOffset(2, false);
mpu.setSlaveDataLength(2, 2);
mpu.setI2CMasterModeEnabled(1);
mpu.setDLPFMode(6);
}
