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); }