// MPU6050 Register Map
#define XG_OFFS_TC         0x00
#define YG_OFFS_TC         0x01
#define ZG_OFFS_TC         0x02
#define X_FINE_GAIN        0x03
#define Y_FINE_GAIN        0x04
#define Z_FINE_GAIN        0x05
#define XA_OFFS_H          0x06
#define XA_OFFS_L_TC       0x07
#define YA_OFFS_H          0x08
#define YA_OFFS_L_TC       0x09
#define ZA_OFFS_H          0x0A
#define ZA_OFFS_L_TC       0x0B
#define XG_OFFS_USRH       0x13
#define XG_OFFS_USRL       0x14
#define YG_OFFS_USRH       0x15
#define YG_OFFS_USRL       0x16
#define ZG_OFFS_USRH       0x17
#define ZG_OFFS_USRL       0x18
#define SMPLRT_DIV         0x19
#define CONFIG             0x1A
#define GYRO_CONFIG        0x1B
#define ACCEL_CONFIG       0x1C
#define FF_THR             0x1D
#define FF_DUR             0x1E
#define MOT_THR            0x1F
#define MOT_DUR            0x20
#define ZRMOT_THR          0x21
#define ZRMOT_DUR          0x22
#define FIFO_EN            0x23
#define I2C_MST_CTRL       0x24
#define I2C_SLV0_ADDR      0x25
#define I2C_SLV0_REG       0x26
#define I2C_SLV0_CTRL      0x27
#define I2C_SLV1_ADDR      0x28
#define I2C_SLV1_REG       0x29
#define I2C_SLV1_CTRL      0x2A
#define I2C_SLV2_ADDR      0x2B
#define I2C_SLV2_REG       0x2C
#define I2C_SLV2_CTRL      0x2D
#define I2C_SLV3_ADDR      0x2E
#define I2C_SLV3_REG       0x2F
#define I2C_SLV3_CTRL      0x30
#define I2C_SLV4_ADDR      0x31
#define I2C_SLV4_REG       0x32
#define I2C_SLV4_DO        0x33
#define I2C_SLV4_CTRL      0x34
#define I2C_SLV4_DI        0x35
#define I2C_MST_STATUS     0x36
#define INT_PIN_CFG        0x37
#define INT_ENABLE         0x38
#define DMP_INT_STATUS     0x39
#define INT_STATUS         0x3A
#define ACCEL_XOUT_H       0x3B
#define ACCEL_XOUT_L       0x3C
#define ACCEL_YOUT_H       0x3D
#define ACCEL_YOUT_L       0x3E
#define ACCEL_ZOUT_H       0x3F
#define ACCEL_ZOUT_L       0x40
#define TEMP_OUT_H         0x41
#define TEMP_OUT_L         0x42
#define GYRO_XOUT_H        0x43
#define GYRO_XOUT_L        0x44
#define GYRO_YOUT_H        0x45
#define GYRO_YOUT_L        0x46
#define GYRO_ZOUT_H        0x47
#define GYRO_ZOUT_L        0x48
#define EXT_SENS_DATA_00   0x49
#define EXT_SENS_DATA_01   0x4A
#define EXT_SENS_DATA_02   0x4B
#define EXT_SENS_DATA_03   0x4C
#define EXT_SENS_DATA_04   0x4D
#define EXT_SENS_DATA_05   0x4E
#define EXT_SENS_DATA_06   0x4F
#define EXT_SENS_DATA_07   0x50
#define EXT_SENS_DATA_08   0x51
#define EXT_SENS_DATA_09   0x52
#define EXT_SENS_DATA_10   0x53
#define EXT_SENS_DATA_11   0x54
#define EXT_SENS_DATA_12   0x55
#define EXT_SENS_DATA_13   0x56
#define EXT_SENS_DATA_14   0x57
#define EXT_SENS_DATA_15   0x58
#define EXT_SENS_DATA_16   0x59
#define EXT_SENS_DATA_17   0x5A
#define EXT_SENS_DATA_18   0x5B
#define EXT_SENS_DATA_19   0x5C
#define EXT_SENS_DATA_20   0x5D
#define EXT_SENS_DATA_21   0x5E
#define EXT_SENS_DATA_22   0x5F
#define EXT_SENS_DATA_23   0x60
#define MOT_DETECT_STATUS  0x61
#define I2C_SLV0_DO        0x63
#define I2C_SLV1_DO        0x64
#define I2C_SLV2_DO        0x65
#define I2C_SLV3_DO        0x66
#define I2C_MST_DELAY_CTRL 0x67
#define SIGNAL_PATH_RESET  0x68
#define MOT_DETECT_CTRL    0x69
#define USER_CTRL          0x6A
#define PWR_MGMT_1         0x6B
#define PWR_MGMT_2         0x6C
#define BANK_SEL           0x6D
#define MEM_START_ADDR     0x6E
#define MEM_R_W            0x6F
#define DMP_CFG_1          0x70
#define DMP_CFG_2          0x71
#define FIFO_COUNTH        0x72
#define FIFO_COUNTL        0x73
#define FIFO_R_W           0x74
#define WHO_AM_I           0x75

// Device Address
unsigned short MPU6050_ADDRESS=0xD0;

// Offset Values
int accelX_Offset=0,accelY_Offset=0,accelZ_Offset=0,gyroX_Offset=0,gyroY_Offset=0,gyroZ_Offset=0;

// MPU6050 Write Data to Selected Data Address
void MPU6050_Write(unsigned short address, unsigned short value){
  I2C1_Start();                   // Start I2C Communication
  I2C1_Wr(MPU6050_ADDRESS);       // Send Device Address
  I2C1_Wr(address);               // Select Register Addres
  I2C1_Wr(value);                 // Write Register Value
  I2C1_Stop();                    // Stop I2C Communication
}

// MPU6050 Initialize Func.
void MPU6050_Init(){
  // Power-up Delay
  delay_ms(100);

  // Setting The Clock Source
  MPU6050_Write(PWR_MGMT_1, 0x00);        // Internal 8MHz

  // Configure The DLPF
  MPU6050_Write(CONFIG, 0x06);            // Accelerometer and Gyroscope Bandwidth

  // Configure The GYRO
  MPU6050_Write(GYRO_CONFIG, 0x18);       // FS_SEL=±2000°/s

  // Configure The ACCEL
  MPU6050_Write(ACCEL_CONFIG, 0x18);      // AFS_SEL=±2g
}

// MPU6050 Offset Func.
void MPU6050_SetOffset(int accelX, int accelY, int accelZ, int gyroX, int gyroY, int gyroZ){
  accelX_Offset=accelX;
  accelY_Offset=accelY;
  accelZ_Offset=accelZ;
  gyroX_Offset=gyroX;
  gyroY_Offset=gyroY;
  gyroZ_Offset=gyroZ;
}

// MPU6050 Read Func.
void MPU6050_Read(int *accelX, int *accelY, int *accelZ, int *temp, int *gyroX, int *gyroY, int *gyroZ){
  // Prepare For Reading, Starting From ACCEL_XOUT_H
  I2C1_Start();                   // Start I2C Communication
  I2C1_Wr(MPU6050_ADDRESS);       // Send Device Address
  I2C1_Wr(ACCEL_XOUT_H);          // Select ACCEL_XOUT_H Register Addres
  I2C1_Stop();                    // Stop I2C Communication
  I2C1_Start();                   // Start I2C Communication
  I2C1_Wr(MPU6050_ADDRESS+1);     // Ready to Read

  *accelX = (I2C1_Rd(1)<<8) | I2C1_Rd(1);   // Read accelX Data
  *accelY = (I2C1_Rd(1)<<8) | I2C1_Rd(1);   // Read accelY Data
  *accelZ = (I2C1_Rd(1)<<8) | I2C1_Rd(1);   // Read accelZ Data
  *temp   = (I2C1_Rd(1)<<8) | I2C1_Rd(1);   // Read temp Data
  *gyroX  = (I2C1_Rd(1)<<8) | I2C1_Rd(1);   // Read gyroX Data
  *gyroY  = (I2C1_Rd(1)<<8) | I2C1_Rd(1);   // Read gyroY Data
  *gyroZ  = (I2C1_Rd(1)<<8) | I2C1_Rd(0);   // Read gyroZ Data
  
  I2C1_Stop();                              // Stop I2C Communication
  *accelX-=accelX_Offset;                   // Decrease accelX_Offset
  *accelY-=accelY_Offset;                   // Decrease accelY_Offset
  *accelZ-=accelZ_Offset;                   // Decrease accelZ_Offset
  *temp = (*temp/340)+36;                   // Convert Temperature °C
  *gyroX-=gyroX_Offset;                     // Decrease gyroX_Offset
  *gyroY-=gyroY_Offset;                     // Decrease gyroY_Offset
  *gyroZ-=gyroZ_Offset;                     // Decrease gyroZ_Offset
}