// ---------------------------------------------------------------------------
// Receive all measurements from an 9 DOF sensor board. 
// ---------------------------------------------------------------------------
//
// Structure:
//                                         
//                                           Sub I2C
//                                        ______^ ______
//                                       |              |
// ----------
//  Arduino  |               ----------------          -------------
//  2560     |- 3.3 V ------ | MPU 6050     |          |  HMC5883  |
//           |- GND ---------| Acceleration,|---SDA ---|  Compass  |
//           |- SDA ---------| Gyro, Temp   |---SCL ---|           |
//           |- SCL ---------|              |          |           |
//           |               ----------------          -------------
//-----------
//                       |___________________ _______________________|
//                                           V
//                                   Integrated IMU sensor 
//
// Pull-up resistors are integrated in the sensor board.
//
// IMPORTANT: When I connect the sensor board to a 5V power supply, it was 
//            not possible to realize a I2C connection in this case. I made
//            some experiments with additional pull-upps on the I2C but 
//            without any results.
//
// ---------------------------------------------------------------------------
//
// It exists a very good library for I2C communication based on Arduino "Wire"
// provided by Jeff Rowberg. It integrates specific controllers as MPU 6050 
// and HMC 5883. Take a view on https://github.com/jrowberg/i2cdevlib
//
// The example was implement with i2cdevlib Version and extends the existing 
// MPU_6050_raw // example. It uses the code proposed by @muzhig on i2cdevlib 
// https://github.com/jrowberg/i2cdevlib/issues/18
// ---------------------------------------------------------------------------

#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
#include "HMC5883L.h"

// The default I2C address is defined 
// MPU 6050 - 0x68 - MPU6050_DEFAULT_ADDRESS
// HMC5883L - 0x1E - HMC5883L_DEFAULT_ADDRESS    
MPU6050 mpu6050;
HMC5883L hmc5883l;

int16_t ax, ay, az;
int16_t gx, gy, gz;
int16_t mx, my, mz;
double temp;

#define LED_PIN 13
bool blinkState = false;

// this method is just used to collect different setSlave operations
void setSlaveControl(uint8_t slaveID){
    mpu6050.setSlaveEnabled(slaveID, true);
    mpu6050.setSlaveWordByteSwap(slaveID, false);
    mpu6050.setSlaveWriteMode(slaveID, false);
    mpu6050.setSlaveWordGroupOffset(slaveID, false);
    mpu6050.setSlaveDataLength(slaveID, 2);
}

void setup() {
    // join I2C bus (I2Cdev library doesn't do this automatically)
    Wire.begin();
    // initialize serial communication
    Serial.begin(9600);

    Serial.println("Initializing I2C devices...");

    mpu6050.initialize();
    if (mpu6050.testConnection()){
      Serial.println("MPU6050 connection successful");
    }
    else {
      Serial.println("MPU6050 connection failed");
    }

    // configuration of the compass module
    // activate the I2C bypass to directly access the Sub I2C 
    mpu6050.setI2CMasterModeEnabled(0);
    mpu6050.setI2CBypassEnabled(1);

    if (hmc5883l.testConnection()) {
        Serial.println("HMC5883l connection successful");
        hmc5883l.initialize();

        // unfourtunally 
        // hmc5883l.setMode(HMC5883L_MODE_CONTINUOUS); 
        // does not work correctly. I used the following command to 
        // "manually" switch on continouse measurements
        I2Cdev::writeByte(HMC5883L_DEFAULT_ADDRESS,
                          HMC5883L_RA_MODE,
                          HMC5883L_MODE_CONTINUOUS);

        // the HMC5883l is configured now, we switch back to the MPU 6050
        mpu6050.setI2CBypassEnabled(0);

        // X axis word
        mpu6050.setSlaveAddress(0, HMC5883L_DEFAULT_ADDRESS | 0x80); 
        mpu6050.setSlaveRegister(0, HMC5883L_RA_DATAX_H);
        setSlaveControl(0);

        // Y axis word
        mpu6050.setSlaveAddress(1, HMC5883L_DEFAULT_ADDRESS | 0x80);
        mpu6050.setSlaveRegister(1, HMC5883L_RA_DATAY_H);
        setSlaveControl(1);

        // Z axis word
        mpu6050.setSlaveAddress(2, HMC5883L_DEFAULT_ADDRESS | 0x80);
        mpu6050.setSlaveRegister(2, HMC5883L_RA_DATAZ_H);
        setSlaveControl(2);

        mpu6050.setI2CMasterModeEnabled(1);
    } else {
        Serial.println("HMC5883l connection failed");
    }

    // activate temperature MPU 6050 sensor
    mpu6050.setTempSensorEnabled(true);

    // configure Arduino LED for
    pinMode(LED_PIN, OUTPUT);
}

void loop() {

    // read raw heading measurements from device
    mx=mpu6050.getExternalSensorWord(0);
    my=mpu6050.getExternalSensorWord(2);
    mz=mpu6050.getExternalSensorWord(4);

   // To calculate heading in degrees. 0 degree indicates North
    float heading = atan2(my, mx);
    if(heading < 0)
      heading += 2 * M_PI;

   // read raw accel/gyro measurements from device
   mpu6050.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

   // see MPU 6050 datasheet page 31 of 47
   temp=((double) mpu6050.getTemperature()) /340.0 + 36.53;

   Serial.print(ax); Serial.print("\t");
   Serial.print(ay); Serial.print("\t");
   Serial.print(az); Serial.print("|\t");
   Serial.print(gx); Serial.print("\t");
   Serial.print(gy); Serial.print("\t");
   Serial.print(gz); Serial.print("|\t");
   Serial.print(heading * 180/M_PI); Serial.print("|\t");
   Serial.println(temp,3);

    // blink LED to indicate activity
    blinkState = !blinkState;
    digitalWrite(LED_PIN, blinkState);
}