ksvbka/mpu6050.c

## mpu6050.c
/********************************************************************************

Product: MPU6050
Module:
Created: 4/6/2015, by KienLTb

Description: Driver MPU6050 for MSP430

********************************************************************************/
/*-----------------------------------------------------------------------------*/
/* Header inclusions */
/*-----------------------------------------------------------------------------*/
#include "msp430g2553.h"
#include "mpu6050.h"
#include "math.h" // For atan();
/*-----------------------------------------------------------------------------*/
/* Local Constant definitions */
/*-----------------------------------------------------------------------------*/
/* AFS_SEL | Full Scale Range | LSB Sensitivity
* --------+------------------+----------------
* 0       | +/- 2g           | 16384 LSB/mg
* 1       | +/- 4g           | 8192 LSB/mg
* 2       | +/- 8g           | 4096 LSB/mg
* 3       | +/- 16g          | 2043 LSB/mg
*/
#define SCALED_ACC_2G       16384
#define SCALED_ACC_4G       8192
#define SCALED_ACC_8G       4096
#define SCALED_ACC_16G      2043

/* FS_SEL | Full Scale Range   | LSB Sensitivity
* -------+--------------------+----------------
* 0      | +/- 250 degrees/s  | 131 LSB/deg/s
* 1      | +/- 500 degrees/s  | 65.5 LSB/deg/s
* 2      | +/- 1000 degrees/s | 32.8 LSB/deg/s
* 3      | +/- 2000 degrees/s | 16.4 LSB/deg/s
*/
#define SCALED_GYRO_250	    131.0
#define SCALED_GYRO_500 	65.5
#define SCALED_GYRO_1000	32.8
#define SCALED_GYRO_2000	16.4

/*
*   Note:
*          |   ACCELEROMETER    |           GYROSCOPE
* DLPF_CFG | Bandwidth | Delay  | Bandwidth | Delay  | Sample Rate
* ---------+-----------+--------+-----------+--------+-------------
* 0        | 260Hz     | 0ms    | 256Hz     | 0.98ms | 8kHz
* 1        | 184Hz     | 2.0ms  | 188Hz     | 1.9ms  | 1kHz
* 2        | 94Hz      | 3.0ms  | 98Hz      | 2.8ms  | 1kHz
* 3        | 44Hz      | 4.9ms  | 42Hz      | 4.8ms  | 1kHz
* 4        | 21Hz      | 8.5ms  | 20Hz      | 8.3ms  | 1kHz
* 5        | 10Hz      | 13.8ms | 10Hz      | 13.4ms | 1kHz
* 6        | 5Hz       | 19.0ms | 5Hz       | 18.6ms | 1kHz
* 7        |   -- Reserved --   |   -- Reserved --   | Reserved
*/


/*-----------------------------------------------------------------------------*/
/* Local Macro definitions */
/*-----------------------------------------------------------------------------*/
// Note: This Macro use for Bigendian but MSP430 is little endien => revert
#define CONVERT_TO_16BIT(MSB, LSB) 		(((WORD)(MSB) << 8) | (WORD)(LSB))
#define ABS(x) (x < 0 ? -x : x)
/*-----------------------------------------------------------------------------*/
/* Local Data type definitions */
/*-----------------------------------------------------------------------------*/
#define dt 0.015f
/*-----------------------------------------------------------------------------*/
/* Global variables */
/*-----------------------------------------------------------------------------*/

/* Offset value to calibrate Gyro */
static int16 Gyro_OffsetValueX = 0;
static int16 Gyro_OffsetValueY = 0;
static int16 Gyro_OffsetValueZ = 0;

/* Off set value to calibrate Acc*/
static int16 Acc_OffsetValueX = 0;
static int16 Acc_OffsetValueY = 0;
static int16 Acc_OffsetValueZ = 0;

/* Scale Value config for ACC - default is 2G*/
static float Acc_scaleValue  = SCALED_ACC_2G;

/* Scale Value config for GYRO - default is 2G*/
static float Gyro_scaleValue = SCALED_GYRO_250;

/*-----------------------------------------------------------------------------*/
/* Function prototypes */
/*-----------------------------------------------------------------------------*/

VOID MPU6050_Init(BYTE ACC_SCALE_CONFIG, BYTE GYRO_SCALE_CONFIG);

BYTE MPU6050_CheckI2C(VOID);

/* MPU6050 test configure of register*/
BYTE MPU6050_TestRegConfig(VOID);

VOID MPU6050_Calibrate_Gyro(VOID);
/* Raw Acc Value*/
VOID MPU6050_GetAccValueRaw(PVOID pValue);

/* Value in degree/s */
VOID PMU6050_AccConvertData(ACC_DATA_RAW rawValue, PVOID scaledData);

/* Raw Gyro Value to m/s^2*/
VOID MPU6050_GetGyroValueRaw(PVOID pValue);

/* Convert to m/s^2*/
VOID PMU6050_GyroConvertData(GYRO_DATA_RAW rawValue, PVOID scaledData);

/*-----------------------------------------------------------------------------*/
/* Function implementations 												   */
/*-----------------------------------------------------------------------------*/

/*--------------------------------------------------------------------------------
Function	: MPU6050_Init
Purpose		: Init and configure for MPU6050
Parameters	: None
Return		: Note
--------------------------------------------------------------------------------*/
VOID MPU6050_Init(BYTE ACC_SCALE_CONFIG, BYTE GYRO_SCALE_CONFIG)
{

        I2C_WriteByte(0x07, MPU6050_ADDRESS, MPU6050_SIGNAL_PATH_RESET);

        // Reset MPU6050;
        I2C_WriteByte(DEVICE_RESET, MPU6050_ADDRESS, MPU6050_PWR_MGMT_1);

        //Set sample rate
        I2C_WriteByte(SET_SAMPLE_RATE_1000HZ, MPU6050_ADDRESS, MPU6050_SMPLRT_DIV);

        //Config Low pass filter
        I2C_WriteByte(EXT_SYNC_SET_INPUT_DISABLE + DLPF_CFG_BAND_WIDTH_10HZ, MPU6050_ADDRESS, MPU6050_CONFIG);

        //Config Accel
        BYTE AccConfig;
        switch(ACC_SCALE_CONFIG)
        {
            case  ACC_CONFIG_2G:
                Acc_scaleValue =   SCALED_ACC_2G;
                AccConfig      =   AFS_SEL_SCALE_2G;
                break;
            case  ACC_CONFIG_4G:
                Acc_scaleValue =   SCALED_ACC_4G;
                AccConfig      =   AFS_SEL_SCALE_4G;
                break;
            case  ACC_CONFIG_8G:
                Acc_scaleValue =   SCALED_ACC_8G;
                AccConfig      =   AFS_SEL_SCALE_8G;
                break;
            case  ACC_CONFIG_16G:
                Acc_scaleValue =   SCALED_ACC_16G;
                AccConfig      =   AFS_SEL_SCALE_16G;
                break;
        }
        I2C_WriteByte(AccConfig, MPU6050_ADDRESS, MPU6050_ACCEL_CONFIG);

        //Congfig Gyro
        BYTE GyroConfig;
        switch(GYRO_SCALE_CONFIG)
        {
            case GYRO_CONFIG_250:
                Gyro_scaleValue =  SCALED_GYRO_250;
                GyroConfig      =   PS_SEL_SCALE_250;
                break;
            case GYRO_CONFIG_500:
                Gyro_scaleValue =  SCALED_GYRO_500;
                GyroConfig      =   PS_SEL_SCALE_500;
                break;
            case GYRO_CONFIG_1000:
                Gyro_scaleValue =  SCALED_GYRO_1000;
                GyroConfig      =   PS_SEL_SCALE_1000;
                break;
            case GYRO_CONFIG_2000:
                Gyro_scaleValue =  SCALED_GYRO_2000;
                GyroConfig      =   PS_SEL_SCALE_2000;
                break;
        }
        I2C_WriteByte(GyroConfig, MPU6050_ADDRESS, MPU6050_GYRO_CONFIG);

        //Enable Interrupt
        I2C_WriteByte(0x01, MPU6050_ADDRESS,MPU6050_INT_ENABLE);

        // Enable MPU6050;
        I2C_WriteByte(CLKSEL_0, MPU6050_ADDRESS, MPU6050_PWR_MGMT_1);

        /*	Config another Register*/
//        	I2C_WriteByte(0x01, MPU6050_ADDRESS,MPU6050_INT_ENABLE);
//        	I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_TEMP_OUT_H);
//        	I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_TEMP_OUT_L);
//        	I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_GYRO_XOUT_H);
//        	I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_GYRO_XOUT_L);
//        	I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_GYRO_YOUT_H);
//        	I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_GYRO_YOUT_L);
//        	I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_GYRO_ZOUT_H);
//        	I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_GYRO_ZOUT_L);
//        	I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_ACCEL_XOUT_H);
//        	I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_ACCEL_XOUT_L);
//        	I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_ACCEL_YOUT_H);
//        	I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_ACCEL_YOUT_L);
//        	I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_ACCEL_ZOUT_H);
//        	I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_ACCEL_ZOUT_L);
//        	I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_FF_THR);
//        	I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_FF_DUR);
//        	I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_MOT_THR);
//        	I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_MOT_DUR);
//        	I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_ZRMOT_THR);
//        	I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_ZRMOT_DUR);
//        	I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_TEMP_OUT_H);
//
//        	I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_FIFO_EN);
//        	I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV0_ADDR);
//        	I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV0_REG);
//        	I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_MST_CTRL);
//        	I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV0_ADDR);
//        	I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV0_REG);
//        	I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV0_CTRL);
//        	I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV1_ADDR);
//        	I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV1_REG);
//        	I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV1_CTRL);
//        	I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV2_ADDR);
//        	I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV2_REG);
//        	I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV2_CTRL);
//        	I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV3_ADDR);
//        	I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV3_REG);
//        	I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV3_CTRL);
//        	I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV4_ADDR);
//        	I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV4_REG);
//        	I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV4_DO);
//        	I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV4_CTRL);
//        	I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV4_DI);
//        	I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_INT_PIN_CFG);

        __delay_cycles(100000);
}
/*--------------------------------------------------------------------------------
Function    : MPU6050_CheckI2C
Purpose     : Check I2C communication
Parameters  : None
Return      : Value of WHO_AM_I registor (0x68)
-------------------------------------------------------------------------------*/
BYTE MPU6050_CheckI2C(VOID)
{
        BYTE byBuff;
        I2C_ReadData(&byBuff, MPU6050_ADDRESS, MPU6050_WHO_AM_I,1);
        return (byBuff);
}

/*--------------------------------------------------------------------------------
Function 	: MPU6050_TestRegConfig
Purpose     : Check the config of some register:
- MPU6050_RA_SMPLRT_DIV == 0x01:
-  MPU6050_RA_CONFIG == 0x03;
- MPU6050_RA_GYRO_CONFIG == 0x01;
- MPU6050_RA_ACCEL_CONFIG == 0x00;
Parameters  : None
Return      : 0 if pass and 1 if failt
-------------------------------------------------------------------------------*/

BYTE MPU6050_TestRegConfig(VOID)
{
		BYTE byBuff, ret;
		I2C_ReadData(&byBuff, MPU6050_ADDRESS, MPU6050_SMPLRT_DIV,1);
		ret =  (byBuff == 0x01 ? 0: 1);
        __delay_cycles(10000);

		I2C_ReadData(&byBuff, MPU6050_ADDRESS, MPU6050_CONFIG,1);
		ret =  (byBuff == 0x03 ? 0: 1);
        __delay_cycles(10000);


		I2C_ReadData(&byBuff, MPU6050_ADDRESS, MPU6050_GYRO_CONFIG,1);
		ret =  (byBuff == 0x01 ? 0: 1);
        __delay_cycles(10000);

		I2C_ReadData(&byBuff, MPU6050_ADDRESS, MPU6050_ACCEL_CONFIG,1);
		ret =  (byBuff == 0x00 ? 0: 1);
        __delay_cycles(10000);

		I2C_ReadData(&byBuff, MPU6050_ADDRESS, MPU6050_PWR_MGMT_1,1);
		ret =  (byBuff == 0x00 ? 0: 1);
        __delay_cycles(10000);

		return ret;
}

/*--------------------------------------------------------------------------------
Function    : MPU6050_GetAccValueRaw
Purpose     : Get raw value x, y, z of accel
Parameters  : PACC_DATA_RAW - pointer to a struct store acc raw data
Return      : NULL
--------------------------------------------------------------------------------*/
VOID MPU6050_GetAccValueRaw(PVOID pValue)
{
        BYTE pBuff[6];
        I2C_ReadData(pBuff, MPU6050_ADDRESS, MPU6050_ACCEL_XOUT_H, 6);

        ((PACC_DATA_RAW)pValue)->x = (int16)(CONVERT_TO_16BIT(pBuff[1], pBuff[0]) - Acc_OffsetValueX);
        ((PACC_DATA_RAW)pValue)->y = (int16)(CONVERT_TO_16BIT(pBuff[3], pBuff[2]) - Acc_OffsetValueY);
        ((PACC_DATA_RAW)pValue)->z = (int16)(CONVERT_TO_16BIT(pBuff[5], pBuff[4]) - Acc_OffsetValueZ);
}

/*--------------------------------------------------------------------------------
Function    : PMU6050_AccConvertData
Purpose     : Scaled data to radian value
Parameters  : ACC_DATA_RAW , PVOID scaledData
Return      : NULL
--------------------------------------------------------------------------------*/

VOID PMU6050_AccConvertData(ACC_DATA_RAW rawValue, PVOID scaledData)
{
        ((PACC_DATA_SCALED)scaledData)->x = (float)rawValue.x / Acc_scaleValue;
        ((PACC_DATA_SCALED)scaledData)->y = (float)rawValue.y / Acc_scaleValue;
        ((PACC_DATA_SCALED)scaledData)->z = (float)rawValue.z / Acc_scaleValue;

}
/*--------------------------------------------------------------------------------
Function    : PMU6050_GetAccValueAngle
Purpose     : Get the rotation angle of sensor (compare with x, y, z axis) in degre
Parameters  : PACC_DATA_ANGLE - pointer to a struct store angle rotation
Return      : NULL
--------------------------------------------------------------------------------*/

VOID PMU6050_GetRotationAngle(ACC_DATA_RAW raw, PVOID pRotationAngle)
{

        /* Caculate the angle rotation */
        /* 180/PI = 57.296 */
        /*Fix: use atan2 -> result in -pi -> pi */
        float x_angle = 57.296 * atan2((float)raw.y, sqrt((float)raw.z*(float)raw.z + (float)raw.x*(float)raw.x));
        if(x_angle < 0) x_angle += 360.0;

        float y_angle = 57.296 * atan2((float)raw.x, sqrt((float)raw.z*(float)raw.z + (float)raw.y*(float)raw.y));
        if(y_angle < 0) y_angle += 360.0;

        float z_angle = 57.296 * atan2((float)raw.z, sqrt(((float)raw.x*(float)raw.x + (float)raw.y*(float)raw.y)));
        if(x_angle < 0) x_angle += 360.0;

        ((PANGLE)pRotationAngle)->x = x_angle;
        ((PANGLE)pRotationAngle)->y = y_angle;
        ((PANGLE)pRotationAngle)->z = z_angle;
}

/*--------------------------------------------------------------------------------
Function    : MPU6050_GetGyroValueRaw
Purpose     : Get raw value x, y, z of Gyro
Parameters  : PGYRO_DATA_RAW - pointer to struct store Gyro data
Return      : NULL
--------------------------------------------------------------------------------*/
VOID MPU6050_GetGyroValueRaw(PVOID pValue)
{
        BYTE pBuff[6];
        I2C_ReadData(pBuff, MPU6050_ADDRESS, MPU6050_GYRO_XOUT_H, 6);

        ((PGYRO_DATA_RAW)pValue)->x = (int16)(CONVERT_TO_16BIT(pBuff[1], pBuff[0]) - Gyro_OffsetValueX);
        ((PGYRO_DATA_RAW)pValue)->y = (int16)(CONVERT_TO_16BIT(pBuff[3], pBuff[2]) - Gyro_OffsetValueY);
        ((PGYRO_DATA_RAW)pValue)->z = (int16)(CONVERT_TO_16BIT(pBuff[5], pBuff[4]) - Gyro_OffsetValueZ);

}

/*--------------------------------------------------------------------------------
Function    : PMU6050_GetGyroValueAngle
Purpose     : Get value x, y, z of Gyro
Parameters  : PGYRO_DATA_ANGLE - pointer to struct store Gyro data
Return      : NULL
--------------------------------------------------------------------------------*/
VOID PMU6050_GetGyroValueAngle(PVOID pValue)
{
        ;
}

/*--------------------------------------------------------------------------------
Function    : PMU6050_GyroConvertData
Purpose     : Convert data to m/s^2 by divide to scale
Parameters  : GYRO_DATA_RAW rawValue, PVOID scaledData
Return      : NULL
--------------------------------------------------------------------------------*/
VOID PMU6050_GyroConvertData(GYRO_DATA_RAW rawValue, PVOID scaledData)
{
        ((PGYRO_DATA_SCALED)scaledData)->x = (float)rawValue.x / Gyro_scaleValue;
        ((PGYRO_DATA_SCALED)scaledData)->y = (float)rawValue.y / Gyro_scaleValue;
        ((PGYRO_DATA_SCALED)scaledData)->z = (float)rawValue.z / Gyro_scaleValue;
}

/*--------------------------------------------------------------------------------
Function    :  MPU6050_Calibrate_Gyro
Purpose     :  Get the value to calibrate Gyro
Parameters  : NULL
Return      : NULL
--------------------------------------------------------------------------------*/

VOID MPU6050_Calibrate_Gyro(VOID)
{
        int i;
        int x = 0;
        int y = 0;
        int z = 0;
        BYTE pBuff[6];

        Gyro_OffsetValueX = 0;
        Gyro_OffsetValueY = 0;
        Gyro_OffsetValueZ = 0;
        for(i = 0; i < 2000; i++)
        {

                I2C_ReadData(pBuff, MPU6050_ADDRESS, MPU6050_GYRO_XOUT_H, 6);

                x = (int16)(CONVERT_TO_16BIT(pBuff[1], pBuff[0]));
                y = (int16)(CONVERT_TO_16BIT(pBuff[3], pBuff[2]));
                z = (int16)(CONVERT_TO_16BIT(pBuff[5], pBuff[4]));

                Gyro_OffsetValueX = (Gyro_OffsetValueX + x) >> 1;
                Gyro_OffsetValueY = (Gyro_OffsetValueY + y) >> 1;
                Gyro_OffsetValueZ = (Gyro_OffsetValueZ + z) >> 1;
        }
//        Gyro_OffsetValueX = (int16)(SumGyroX/200);
//        Gyro_OffsetValueY = (int16)(SumGyroY/200);
//        Gyro_OffsetValueZ = (int16)(SumGyroZ/200);
}


/*--------------------------------------------------------------------------------
Function    : ComplementaryFilter
Purpose     : Calculate angle use both acc and gyro
Parameters  : Raw data (ADC 16bit) of acc and gyro
Return      : ANGLE
--------------------------------------------------------------------------------*/

VOID Complementary_Filter(ACC_DATA_RAW accData, GYRO_DATA_RAW gyroData, PVOID pAngle)
{
        float pitchAcc, rollAcc;

        /* Angle around the X axis */
        ((PANGLE)pAngle)->x += ((float)gyroData.x / Gyro_scaleValue)*dt; // dt define as 0.015s
        /* Angle around the Y axis*/
        ((PANGLE)pAngle)->y += ((float)gyroData.y / Gyro_scaleValue)*dt;

        // Compensate for drift with accelerometer data if !bullshit
        // Sensitivity = -2 to 2 G at 16Bit -> 2G = 32768 && 0.5G = 8192
        long forceMagnitudeApprox = ABS(accData.x) + ABS(accData.y) + ABS(accData.z);
        if (forceMagnitudeApprox > 8192 && forceMagnitudeApprox < 32768)
        {
                // Turning around the X axis results in a vector on the Y-axis
                pitchAcc = 57.296 * atan2((float)accData.y, sqrt((float)accData.z*(float)accData.z + (float)accData.x*(float)accData.x));
                ((PANGLE)pAngle)->x = ((PANGLE)pAngle)->x * 0.98 + pitchAcc * 0.02;

                // Turning around the Y axis results in a vector on the X-axis
                rollAcc = 57.296 * atan2((float)accData.x, sqrt((float)accData.z*(float)accData.z + (float)accData.y*(float)accData.y));
                ((PANGLE)pAngle)->y = ((PANGLE)pAngle)->y* 0.98 + rollAcc * 0.02;
        }

}

## mpu6050.h
/********************************************************************************
Module: MPU6050
Author: 5/4/2015, by KienLTb

Description: MPU6050 Lib for MSP430G2553

********************************************************************************/
#ifndef __MPU6050__H__
#define __MPU6050__H__
/*-----------------------------------------------------------------------------*/
/* Header inclusions */
/*-----------------------------------------------------------------------------*/
#include "msp430g2553.h"
//#include "i2c_sw.h"
#include "i2c_hw.h"
#include "types.h"
/*-----------------------------------------------------------------------------*/
/* Constant definitions  */
/*-----------------------------------------------------------------------------*/
#define MPU6050_ADDRESS 		0x68 // Address MPU6050	0b01101000	0b10100100
#define MPU6050_XG_OFFS_TC 		0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD
#define MPU6050_YG_OFFS_TC 		0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD
#define MPU6050_ZG_OFFS_TC 		0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD
#define MPU6050_X_FINE_GAIN 	0x03 //[7:0] X_FINE_GAIN
#define MPU6050_Y_FINE_GAIN 	0x04 //[7:0] Y_FINE_GAIN
#define MPU6050_Z_FINE_GAIN 	0x05 //[7:0] Z_FINE_GAIN
#define MPU6050_XA_OFFS_H 		0x06 //[15:0] XA_OFFS
#define MPU6050_XA_OFFS_L_TC	0x07
#define MPU6050_YA_OFFS_H 		0x08 //[15:0] YA_OFFS
#define MPU6050_YA_OFFS_L_TC 	0x09
#define MPU6050_ZA_OFFS_H 		0x0A //[15:0] ZA_OFFS
#define MPU6050_ZA_OFFS_L_TC 	0x0B
#define MPU6050_XG_OFFS_USRH 	0x13 //[15:0] XG_OFFS_USR
#define MPU6050_XG_OFFS_USRL 	0x14
#define MPU6050_YG_OFFS_USRH 	0x15 //[15:0] YG_OFFS_USR
#define MPU6050_YG_OFFS_USRL 	0x16
#define MPU6050_ZG_OFFS_USRH 	0x17 //[15:0] ZG_OFFS_USR
#define MPU6050_ZG_OFFS_USRL 	0x18
#define MPU6050_SMPLRT_DIV 		0x19
#define MPU6050_CONFIG 			0x1A
#define MPU6050_GYRO_CONFIG 	0x1B
#define MPU6050_ACCEL_CONFIG 	0x1C
#define MPU6050_FF_THR 			0x1D
#define MPU6050_FF_DUR 			0x1E
#define MPU6050_MOT_THR 		0x1F
#define MPU6050_MOT_DUR 		0x20
#define MPU6050_ZRMOT_THR 		0x21
#define MPU6050_ZRMOT_DUR 		0x22
#define MPU6050_FIFO_EN 		0x23
#define MPU6050_I2C_MST_CTRL 	0x24
#define MPU6050_I2C_SLV0_ADDR 	0x25
#define MPU6050_I2C_SLV0_REG 	0x26
#define MPU6050_I2C_SLV0_CTRL 	0x27
#define MPU6050_I2C_SLV1_ADDR 	0x28
#define MPU6050_I2C_SLV1_REG 	0x29
#define MPU6050_I2C_SLV1_CTRL 	0x2A
#define MPU6050_I2C_SLV2_ADDR 	0x2B
#define MPU6050_I2C_SLV2_REG 	0x2C
#define MPU6050_I2C_SLV2_CTRL 	0x2D
#define MPU6050_I2C_SLV3_ADDR 	0x2E
#define MPU6050_I2C_SLV3_REG 	0x2F
#define MPU6050_I2C_SLV3_CTRL 	0x30
#define MPU6050_I2C_SLV4_ADDR 	0x31
#define MPU6050_I2C_SLV4_REG 	0x32
#define MPU6050_I2C_SLV4_DO 	0x33
#define MPU6050_I2C_SLV4_CTRL 	0x34
#define MPU6050_I2C_SLV4_DI 	0x35
#define MPU6050_I2C_MST_STATUS 	0x36
#define MPU6050_INT_PIN_CFG 	0x37
#define MPU6050_INT_ENABLE 		0x38
#define MPU6050_DMP_INT_STATUS 	0x39
#define MPU6050_INT_STATUS 		0x3A
#define MPU6050_ACCEL_XOUT_H 	0x3B
#define MPU6050_ACCEL_XOUT_L 	0x3C
#define MPU6050_ACCEL_YOUT_H 	0x3D
#define MPU6050_ACCEL_YOUT_L 	0x3E
#define MPU6050_ACCEL_ZOUT_H 	0x3F
#define MPU6050_ACCEL_ZOUT_L 	0x40
#define MPU6050_TEMP_OUT_H 		0x41
#define MPU6050_TEMP_OUT_L 		0x42
#define MPU6050_GYRO_XOUT_H 	0x43
#define MPU6050_GYRO_XOUT_L 	0x44
#define MPU6050_GYRO_YOUT_H 	0x45
#define MPU6050_GYRO_YOUT_L 	0x46
#define MPU6050_GYRO_ZOUT_H 	0x47
#define MPU6050_GYRO_ZOUT_L 	0x48
#define MPU6050_EXT_SENS_DATA_00 0x49
#define MPU6050_EXT_SENS_DATA_01 0x4A
#define MPU6050_EXT_SENS_DATA_02 0x4B
#define MPU6050_EXT_SENS_DATA_03 0x4C
#define MPU6050_EXT_SENS_DATA_04 0x4D
#define MPU6050_EXT_SENS_DATA_05 0x4E
#define MPU6050_EXT_SENS_DATA_06 0x4F
#define MPU6050_EXT_SENS_DATA_07 0x50
#define MPU6050_EXT_SENS_DATA_08 0x51
#define MPU6050_EXT_SENS_DATA_09 0x52
#define MPU6050_EXT_SENS_DATA_10 0x53
#define MPU6050_EXT_SENS_DATA_11 0x54
#define MPU6050_EXT_SENS_DATA_12 0x55
#define MPU6050_EXT_SENS_DATA_13 0x56
#define MPU6050_EXT_SENS_DATA_14 0x57
#define MPU6050_EXT_SENS_DATA_15 0x58
#define MPU6050_EXT_SENS_DATA_16 0x59
#define MPU6050_EXT_SENS_DATA_17 0x5A
#define MPU6050_EXT_SENS_DATA_18 0x5B
#define MPU6050_EXT_SENS_DATA_19 0x5C
#define MPU6050_EXT_SENS_DATA_20 0x5D
#define MPU6050_EXT_SENS_DATA_21 0x5E
#define MPU6050_EXT_SENS_DATA_22 0x5F
#define MPU6050_EXT_SENS_DATA_23 0x60
#define MPU6050_MOT_DETECT_STATUS	0x61
#define MPU6050_I2C_SLV0_DO 	 	0x63
#define MPU6050_I2C_SLV1_DO 	 	0x64
#define MPU6050_I2C_SLV2_DO 	 	0x65
#define MPU6050_I2C_SLV3_DO 	 	0x66
#define MPU6050_I2C_MST_DELAY_CTRL 	0x67
#define MPU6050_SIGNAL_PATH_RESET 	0x68
#define MPU6050_MOT_DETECT_CTRL 	0x69
#define MPU6050_USER_CTRL 			0x6A
#define MPU6050_PWR_MGMT_1 			0x6B
#define MPU6050_PWR_MGMT_2 			0x6C
#define MPU6050_BANK_SEL 			0x6D
#define MPU6050_MEM_START_ADDR 		0x6E
#define MPU6050_MEM_R_W 			0x6F
#define MPU6050_DMP_CFG_1 			0x70
#define MPU6050_DMP_CFG_2 			0x71
#define MPU6050_FIFO_COUNTH 		0x72
#define MPU6050_FIFO_COUNTL			0x73
#define MPU6050_FIFO_R_W 			0x74
#define MPU6050_WHO_AM_I 			0x75
/*---------- CONFIG VALUE----------*/
/* MPU6050_PWR_MGMT_1 REG */
#define CLKSEL_0 0x00 //Internal 8MHz Osilator
#define CLKSEL_1 0x01 //PLL with X axis gyroscope reference
#define CLKSEL_2 0x02 //PLL with Y axis gyroscope reference
#define CLKSEL_3 0x03 //PLL with Z axis gyroscope reference
#define CLKSEL_4 0x04 //PLL with external 32.768kHz reference
#define CLKSEL_5 0x05 //PLL with external 19.2MHz reference
#define CLKSEL_6 0x06 //Reserved
#define CLKSEL_7 0x07 //Stops the clock and keeps the timing generator in reset

#define TEMP_DIS	 (0x01 << 3) //disables the temperature sensor
#define CYCLE 	 	 (0x01 << 5) //
#define SLEEP 		 (0x01 << 6) //
#define DEVICE_RESET (0x01 << 7) //

/*----------MPU6050_CONFIG----------*/
#define EXT_SYNC_SET_INPUT_DISABLE 	(0x00 << 3)// Input disabled
#define EXT_SYNC_SET_TEMP_OUT 		(0x01 << 3)// TEMP_OUT_L[0]
#define EXT_SYNC_SET_GYRO_XOUT 		(0x02 << 3)// GYRO_XOUT_L[0]
#define EXT_SYNC_SET_GYRO_YOUT 		(0x03 << 3)// GYRO_YOUT_L[0]
#define EXT_SYNC_SET_GYRO_ZOUT 		(0x04 << 3)// GYRO_ZOUT_L[0]
#define EXT_SYNC_SET_ACCEL_XOUT 	(0x05 << 3)// ACCEL_XOUT_L[0]
#define EXT_SYNC_SET_ACCEL_YOUT 	(0x06 << 3)// ACCEL_YOUT_L[0]
#define EXT_SYNC_SET_ACCEL_ZOUT 	(0x07 << 3)// ACCEL_ZOUT_L[0]

#define DLPF_CFG_BAND_WIDTH_260HZ 		0x00// BandWidth 260Hz
#define DLPF_CFG_BAND_WIDTH_184HZ  		0x01// BandWidth 184Hz
#define DLPF_CFG_BAND_WIDTH_94HZ 		0x02// BandWidth 94Hz
#define DLPF_CFG_BAND_WIDTH_44HZ 		0x03// BandWidth 44Hz
#define DLPF_CFG_BAND_WIDTH_21HZ 		0x04// BandWidth 21Hz
#define DLPF_CFG_BAND_WIDTH_10HZ 		0x05// BandWidth 10Hz
#define DLPF_CFG_BAND_WIDTH_5HZ 		0x06// BandWidth 5Hz

/*---------MPU6050_GYRO_CONFIG-------*/
#define PS_SEL_SCALE_250	(0x00 << 3)
#define PS_SEL_SCALE_500	(0x01 << 3)
#define PS_SEL_SCALE_1000	(0x02 << 3)
#define PS_SEL_SCALE_2000 	(0x03 << 3)

#define ZG_ST	(0x01 << 5)
#define YG_ST	(0x01 << 6)
#define XG_ST	(0x01 << 7)

/*---------MPU6050_ACCEL_CONFIG------*/
#define AFS_SEL_SCALE_2G	(0x00 << 3)
#define AFS_SEL_SCALE_4G	(0x01 << 3)
#define AFS_SEL_SCALE_8G	(0x02 << 3)
#define AFS_SEL_SCALE_16G	(0x03 << 3)

#define ZA_ST	(0x01 << 5)
#define YA_ST	(0x01 << 6)
#define XA_ST	(0x01 << 7)
/*---------MPU6050_SMPLRT_DIV_CONFIG---*/
#define SET_SAMPLE_RATE_1000HZ	 0x07
/*-----------------------------------------------------------------------------*/
/* Macro definitions  */
/*-----------------------------------------------------------------------------*/

/*-----------------------------------------------------------------------------*/
/* Global variables  */
/*-----------------------------------------------------------------------------*/
#define GYRO_CONFIG_250      0
#define GYRO_CONFIG_500      1
#define GYRO_CONFIG_1000     2
#define GYRO_CONFIG_2000     3


#define ACC_CONFIG_2G 	    4
#define ACC_CONFIG_4G 	    5
#define ACC_CONFIG_8G	    6
#define ACC_CONFIG_16G	    7


/*-----------------------------------------------------------------------------*/
/* Data type definitions */
/*-----------------------------------------------------------------------------*/
//typedef unsigned char BYTE;
//typedef unsigned short WORD;
//typedef unsigned long DWORD;
//typedef unsigned int UINT;
//typedef BYTE BOOL;
//typedef unsigned char CHAR;
//typedef void VOID;
//
//typedef BYTE* PBYTE;
//typedef WORD* PWORD;
//typedef DWORD* PDWORD;
//typedef UINT* PUINT;
//typedef CHAR* PCHAR;
//typedef VOID* PVOID;
//
//typedef BYTE RESULT;
//
//typedef const BYTE* PCBYTE;
//
//#define FALSE   0
//#define TRUE    1
//#define NULL	0
/* Acc data raw value */
typedef struct _ACC_DATA_RAW
{
        int16 x;
        int16 y;
        int16 z;

}ACC_DATA_RAW, *PACC_DATA_RAW;

/* Acc data value in angle */
typedef struct _ACC_DATA_SCALED
{
        float x;
        float y;
        float z;
}ACC_DATA_SCALED, *PACC_DATA_SCALED;

/* Gyro data raw value */
typedef struct _GYRO_DATA_RAW
{
        int16 x;
        int16 y;
        int16 z;
}GYRO_DATA_RAW, *PGYRO_DATA_RAW;

/* Gyro data value in angle */
typedef struct _GYRO_DATA_SCALED
{
        float x;
        float y;
        float z;
}GYRO_DATA_SCALED, *PGYRO_DATA_SCALED;

/* Angle data caculate from raw value */
typedef struct _ANGLE
{
        float x;
        float y;
        float z;
}ANGLE, *PANGLE;
/*-----------------------------------------------------------------------------*/
/* Function prototypes  */
/*-----------------------------------------------------------------------------*/

VOID MPU6050_Init(BYTE ACC_SCALE_CONFIG, BYTE GYRO_SCALE_CONFIG);

/* MPU6050 test i2c connection */
BYTE MPU6050_CheckI2C(VOID);

/* MPU6050 test configure of register*/
BYTE MPU6050_TestRegConfig(VOID);

/* Get the calibrate value and store to offset*/
VOID MPU6050_Calibrate_Gyro(VOID);
/* Raw Acc Value*/
VOID MPU6050_GetAccValueRaw(PVOID pValue);

/* Value in degree/s */
VOID PMU6050_AccConvertData(ACC_DATA_RAW rawValue, PVOID scaledData);

/* Get rotation of sensor, value in degree*/
VOID PMU6050_GetRotationAngle(ACC_DATA_RAW raw, PVOID pRotationAngle);

/* Raw Gyro Value to m/s^2*/
VOID MPU6050_GetGyroValueRaw(PVOID pValue);

/* Convert to m/s^2*/
VOID PMU6050_GyroConvertData(GYRO_DATA_RAW rawValue, PVOID scaledData);

/* Complementrary Filter*/
VOID Complementary_Filter(ACC_DATA_RAW accData, GYRO_DATA_RAW gyroData, PVOID pAngle);

#endif // __MPU6050__H__
	/********************************************************************************

	Product: MPU6050
	Module:
	Created: 4/6/2015, by KienLTb

	Description: Driver MPU6050 for MSP430

	********************************************************************************/
	/-----------------------------------------------------------------------------/
	/* Header inclusions */
	/-----------------------------------------------------------------------------/
	#include "msp430g2553.h"
	#include "mpu6050.h"
	#include "math.h" // For atan();
	/-----------------------------------------------------------------------------/
	/* Local Constant definitions */
	/-----------------------------------------------------------------------------/
	/* AFS_SEL \| Full Scale Range \| LSB Sensitivity
	* --------+------------------+----------------
	* 0 \| +/- 2g \| 16384 LSB/mg
	* 1 \| +/- 4g \| 8192 LSB/mg
	* 2 \| +/- 8g \| 4096 LSB/mg
	* 3 \| +/- 16g \| 2043 LSB/mg
	*/
	#define SCALED_ACC_2G 16384
	#define SCALED_ACC_4G 8192
	#define SCALED_ACC_8G 4096
	#define SCALED_ACC_16G 2043

	/* FS_SEL \| Full Scale Range \| LSB Sensitivity
	* -------+--------------------+----------------
	* 0 \| +/- 250 degrees/s \| 131 LSB/deg/s
	* 1 \| +/- 500 degrees/s \| 65.5 LSB/deg/s
	* 2 \| +/- 1000 degrees/s \| 32.8 LSB/deg/s
	* 3 \| +/- 2000 degrees/s \| 16.4 LSB/deg/s
	*/
	#define SCALED_GYRO_250 131.0
	#define SCALED_GYRO_500 65.5
	#define SCALED_GYRO_1000 32.8
	#define SCALED_GYRO_2000 16.4

	/*
	* Note:
	* \| ACCELEROMETER \| GYROSCOPE
	* DLPF_CFG \| Bandwidth \| Delay \| Bandwidth \| Delay \| Sample Rate
	* ---------+-----------+--------+-----------+--------+-------------
	* 0 \| 260Hz \| 0ms \| 256Hz \| 0.98ms \| 8kHz
	* 1 \| 184Hz \| 2.0ms \| 188Hz \| 1.9ms \| 1kHz
	* 2 \| 94Hz \| 3.0ms \| 98Hz \| 2.8ms \| 1kHz
	* 3 \| 44Hz \| 4.9ms \| 42Hz \| 4.8ms \| 1kHz
	* 4 \| 21Hz \| 8.5ms \| 20Hz \| 8.3ms \| 1kHz
	* 5 \| 10Hz \| 13.8ms \| 10Hz \| 13.4ms \| 1kHz
	* 6 \| 5Hz \| 19.0ms \| 5Hz \| 18.6ms \| 1kHz
	* 7 \| -- Reserved -- \| -- Reserved -- \| Reserved
	*/


	/-----------------------------------------------------------------------------/
	/* Local Macro definitions */
	/-----------------------------------------------------------------------------/
	// Note: This Macro use for Bigendian but MSP430 is little endien => revert
	#define CONVERT_TO_16BIT(MSB, LSB) (((WORD)(MSB) << 8) \| (WORD)(LSB))
	#define ABS(x) (x < 0 ? -x : x)
	/-----------------------------------------------------------------------------/
	/* Local Data type definitions */
	/-----------------------------------------------------------------------------/
	#define dt 0.015f
	/-----------------------------------------------------------------------------/
	/* Global variables */
	/-----------------------------------------------------------------------------/

	/* Offset value to calibrate Gyro */
	static int16 Gyro_OffsetValueX = 0;
	static int16 Gyro_OffsetValueY = 0;
	static int16 Gyro_OffsetValueZ = 0;

	/* Off set value to calibrate Acc*/
	static int16 Acc_OffsetValueX = 0;
	static int16 Acc_OffsetValueY = 0;
	static int16 Acc_OffsetValueZ = 0;

	/* Scale Value config for ACC - default is 2G*/
	static float Acc_scaleValue = SCALED_ACC_2G;

	/* Scale Value config for GYRO - default is 2G*/
	static float Gyro_scaleValue = SCALED_GYRO_250;

	/-----------------------------------------------------------------------------/
	/* Function prototypes */
	/-----------------------------------------------------------------------------/

	VOID MPU6050_Init(BYTE ACC_SCALE_CONFIG, BYTE GYRO_SCALE_CONFIG);

	BYTE MPU6050_CheckI2C(VOID);

	/* MPU6050 test configure of register*/
	BYTE MPU6050_TestRegConfig(VOID);

	VOID MPU6050_Calibrate_Gyro(VOID);
	/* Raw Acc Value*/
	VOID MPU6050_GetAccValueRaw(PVOID pValue);

	/* Value in degree/s */
	VOID PMU6050_AccConvertData(ACC_DATA_RAW rawValue, PVOID scaledData);

	/* Raw Gyro Value to m/s^2*/
	VOID MPU6050_GetGyroValueRaw(PVOID pValue);

	/* Convert to m/s^2*/
	VOID PMU6050_GyroConvertData(GYRO_DATA_RAW rawValue, PVOID scaledData);

	/-----------------------------------------------------------------------------/
	/* Function implementations */
	/-----------------------------------------------------------------------------/

	/*--------------------------------------------------------------------------------
	Function : MPU6050_Init
	Purpose : Init and configure for MPU6050
	Parameters : None
	Return : Note
	--------------------------------------------------------------------------------*/
	VOID MPU6050_Init(BYTE ACC_SCALE_CONFIG, BYTE GYRO_SCALE_CONFIG)
	{

	I2C_WriteByte(0x07, MPU6050_ADDRESS, MPU6050_SIGNAL_PATH_RESET);

	// Reset MPU6050;
	I2C_WriteByte(DEVICE_RESET, MPU6050_ADDRESS, MPU6050_PWR_MGMT_1);

	//Set sample rate
	I2C_WriteByte(SET_SAMPLE_RATE_1000HZ, MPU6050_ADDRESS, MPU6050_SMPLRT_DIV);

	//Config Low pass filter
	I2C_WriteByte(EXT_SYNC_SET_INPUT_DISABLE + DLPF_CFG_BAND_WIDTH_10HZ, MPU6050_ADDRESS, MPU6050_CONFIG);

	//Config Accel
	BYTE AccConfig;
	switch(ACC_SCALE_CONFIG)
	{
	case ACC_CONFIG_2G:
	Acc_scaleValue = SCALED_ACC_2G;
	AccConfig = AFS_SEL_SCALE_2G;
	break;
	case ACC_CONFIG_4G:
	Acc_scaleValue = SCALED_ACC_4G;
	AccConfig = AFS_SEL_SCALE_4G;
	break;
	case ACC_CONFIG_8G:
	Acc_scaleValue = SCALED_ACC_8G;
	AccConfig = AFS_SEL_SCALE_8G;
	break;
	case ACC_CONFIG_16G:
	Acc_scaleValue = SCALED_ACC_16G;
	AccConfig = AFS_SEL_SCALE_16G;
	break;
	}
	I2C_WriteByte(AccConfig, MPU6050_ADDRESS, MPU6050_ACCEL_CONFIG);

	//Congfig Gyro
	BYTE GyroConfig;
	switch(GYRO_SCALE_CONFIG)
	{
	case GYRO_CONFIG_250:
	Gyro_scaleValue = SCALED_GYRO_250;
	GyroConfig = PS_SEL_SCALE_250;
	break;
	case GYRO_CONFIG_500:
	Gyro_scaleValue = SCALED_GYRO_500;
	GyroConfig = PS_SEL_SCALE_500;
	break;
	case GYRO_CONFIG_1000:
	Gyro_scaleValue = SCALED_GYRO_1000;
	GyroConfig = PS_SEL_SCALE_1000;
	break;
	case GYRO_CONFIG_2000:
	Gyro_scaleValue = SCALED_GYRO_2000;
	GyroConfig = PS_SEL_SCALE_2000;
	break;
	}
	I2C_WriteByte(GyroConfig, MPU6050_ADDRESS, MPU6050_GYRO_CONFIG);

	//Enable Interrupt
	I2C_WriteByte(0x01, MPU6050_ADDRESS,MPU6050_INT_ENABLE);

	// Enable MPU6050;
	I2C_WriteByte(CLKSEL_0, MPU6050_ADDRESS, MPU6050_PWR_MGMT_1);

	/* Config another Register*/
	// I2C_WriteByte(0x01, MPU6050_ADDRESS,MPU6050_INT_ENABLE);
	// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_TEMP_OUT_H);
	// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_TEMP_OUT_L);
	// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_GYRO_XOUT_H);
	// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_GYRO_XOUT_L);
	// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_GYRO_YOUT_H);
	// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_GYRO_YOUT_L);
	// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_GYRO_ZOUT_H);
	// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_GYRO_ZOUT_L);
	// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_ACCEL_XOUT_H);
	// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_ACCEL_XOUT_L);
	// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_ACCEL_YOUT_H);
	// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_ACCEL_YOUT_L);
	// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_ACCEL_ZOUT_H);
	// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_ACCEL_ZOUT_L);
	// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_FF_THR);
	// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_FF_DUR);
	// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_MOT_THR);
	// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_MOT_DUR);
	// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_ZRMOT_THR);
	// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_ZRMOT_DUR);
	// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_TEMP_OUT_H);
	//
	// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_FIFO_EN);
	// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV0_ADDR);
	// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV0_REG);
	// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_MST_CTRL);
	// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV0_ADDR);
	// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV0_REG);
	// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV0_CTRL);
	// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV1_ADDR);
	// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV1_REG);
	// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV1_CTRL);
	// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV2_ADDR);
	// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV2_REG);
	// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV2_CTRL);
	// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV3_ADDR);
	// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV3_REG);
	// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV3_CTRL);
	// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV4_ADDR);
	// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV4_REG);
	// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV4_DO);
	// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV4_CTRL);
	// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_I2C_SLV4_DI);
	// I2C_WriteByte(0x00, MPU6050_ADDRESS,MPU6050_INT_PIN_CFG);

	__delay_cycles(100000);
	}
	/*--------------------------------------------------------------------------------
	Function : MPU6050_CheckI2C
	Purpose : Check I2C communication
	Parameters : None
	Return : Value of WHO_AM_I registor (0x68)
	-------------------------------------------------------------------------------*/
	BYTE MPU6050_CheckI2C(VOID)
	{
	BYTE byBuff;
	I2C_ReadData(&byBuff, MPU6050_ADDRESS, MPU6050_WHO_AM_I,1);
	return (byBuff);
	}

	/*--------------------------------------------------------------------------------
	Function : MPU6050_TestRegConfig
	Purpose : Check the config of some register:
	- MPU6050_RA_SMPLRT_DIV == 0x01:
	- MPU6050_RA_CONFIG == 0x03;
	- MPU6050_RA_GYRO_CONFIG == 0x01;
	- MPU6050_RA_ACCEL_CONFIG == 0x00;
	Parameters : None
	Return : 0 if pass and 1 if failt
	-------------------------------------------------------------------------------*/

	BYTE MPU6050_TestRegConfig(VOID)
	{
	BYTE byBuff, ret;
	I2C_ReadData(&byBuff, MPU6050_ADDRESS, MPU6050_SMPLRT_DIV,1);
	ret = (byBuff == 0x01 ? 0: 1);
	__delay_cycles(10000);

	I2C_ReadData(&byBuff, MPU6050_ADDRESS, MPU6050_CONFIG,1);
	ret = (byBuff == 0x03 ? 0: 1);
	__delay_cycles(10000);


	I2C_ReadData(&byBuff, MPU6050_ADDRESS, MPU6050_GYRO_CONFIG,1);
	ret = (byBuff == 0x01 ? 0: 1);
	__delay_cycles(10000);

	I2C_ReadData(&byBuff, MPU6050_ADDRESS, MPU6050_ACCEL_CONFIG,1);
	ret = (byBuff == 0x00 ? 0: 1);
	__delay_cycles(10000);

	I2C_ReadData(&byBuff, MPU6050_ADDRESS, MPU6050_PWR_MGMT_1,1);
	ret = (byBuff == 0x00 ? 0: 1);
	__delay_cycles(10000);

	return ret;
	}

	/*--------------------------------------------------------------------------------
	Function : MPU6050_GetAccValueRaw
	Purpose : Get raw value x, y, z of accel
	Parameters : PACC_DATA_RAW - pointer to a struct store acc raw data
	Return : NULL
	--------------------------------------------------------------------------------*/
	VOID MPU6050_GetAccValueRaw(PVOID pValue)
	{
	BYTE pBuff[6];
	I2C_ReadData(pBuff, MPU6050_ADDRESS, MPU6050_ACCEL_XOUT_H, 6);

	((PACC_DATA_RAW)pValue)->x = (int16)(CONVERT_TO_16BIT(pBuff[1], pBuff[0]) - Acc_OffsetValueX);
	((PACC_DATA_RAW)pValue)->y = (int16)(CONVERT_TO_16BIT(pBuff[3], pBuff[2]) - Acc_OffsetValueY);
	((PACC_DATA_RAW)pValue)->z = (int16)(CONVERT_TO_16BIT(pBuff[5], pBuff[4]) - Acc_OffsetValueZ);
	}

	/*--------------------------------------------------------------------------------
	Function : PMU6050_AccConvertData
	Purpose : Scaled data to radian value
	Parameters : ACC_DATA_RAW , PVOID scaledData
	Return : NULL
	--------------------------------------------------------------------------------*/

	VOID PMU6050_AccConvertData(ACC_DATA_RAW rawValue, PVOID scaledData)
	{
	((PACC_DATA_SCALED)scaledData)->x = (float)rawValue.x / Acc_scaleValue;
	((PACC_DATA_SCALED)scaledData)->y = (float)rawValue.y / Acc_scaleValue;
	((PACC_DATA_SCALED)scaledData)->z = (float)rawValue.z / Acc_scaleValue;

	}
	/*--------------------------------------------------------------------------------
	Function : PMU6050_GetAccValueAngle
	Purpose : Get the rotation angle of sensor (compare with x, y, z axis) in degre
	Parameters : PACC_DATA_ANGLE - pointer to a struct store angle rotation
	Return : NULL
	--------------------------------------------------------------------------------*/

	VOID PMU6050_GetRotationAngle(ACC_DATA_RAW raw, PVOID pRotationAngle)
	{

	/* Caculate the angle rotation */
	/* 180/PI = 57.296 */
	/Fix: use atan2 -> result in -pi -> pi /
	float x_angle = 57.296 * atan2((float)raw.y, sqrt((float)raw.z(float)raw.z + (float)raw.x(float)raw.x));
	if(x_angle < 0) x_angle += 360.0;

	float y_angle = 57.296 * atan2((float)raw.x, sqrt((float)raw.z(float)raw.z + (float)raw.y(float)raw.y));
	if(y_angle < 0) y_angle += 360.0;

	float z_angle = 57.296 * atan2((float)raw.z, sqrt(((float)raw.x(float)raw.x + (float)raw.y(float)raw.y)));
	if(x_angle < 0) x_angle += 360.0;

	((PANGLE)pRotationAngle)->x = x_angle;
	((PANGLE)pRotationAngle)->y = y_angle;
	((PANGLE)pRotationAngle)->z = z_angle;
	}

	/*--------------------------------------------------------------------------------
	Function : MPU6050_GetGyroValueRaw
	Purpose : Get raw value x, y, z of Gyro
	Parameters : PGYRO_DATA_RAW - pointer to struct store Gyro data
	Return : NULL
	--------------------------------------------------------------------------------*/
	VOID MPU6050_GetGyroValueRaw(PVOID pValue)
	{
	BYTE pBuff[6];
	I2C_ReadData(pBuff, MPU6050_ADDRESS, MPU6050_GYRO_XOUT_H, 6);

	((PGYRO_DATA_RAW)pValue)->x = (int16)(CONVERT_TO_16BIT(pBuff[1], pBuff[0]) - Gyro_OffsetValueX);
	((PGYRO_DATA_RAW)pValue)->y = (int16)(CONVERT_TO_16BIT(pBuff[3], pBuff[2]) - Gyro_OffsetValueY);
	((PGYRO_DATA_RAW)pValue)->z = (int16)(CONVERT_TO_16BIT(pBuff[5], pBuff[4]) - Gyro_OffsetValueZ);

	}

	/*--------------------------------------------------------------------------------
	Function : PMU6050_GetGyroValueAngle
	Purpose : Get value x, y, z of Gyro
	Parameters : PGYRO_DATA_ANGLE - pointer to struct store Gyro data
	Return : NULL
	--------------------------------------------------------------------------------*/
	VOID PMU6050_GetGyroValueAngle(PVOID pValue)
	{
	;
	}

	/*--------------------------------------------------------------------------------
	Function : PMU6050_GyroConvertData
	Purpose : Convert data to m/s^2 by divide to scale
	Parameters : GYRO_DATA_RAW rawValue, PVOID scaledData
	Return : NULL
	--------------------------------------------------------------------------------*/
	VOID PMU6050_GyroConvertData(GYRO_DATA_RAW rawValue, PVOID scaledData)
	{
	((PGYRO_DATA_SCALED)scaledData)->x = (float)rawValue.x / Gyro_scaleValue;
	((PGYRO_DATA_SCALED)scaledData)->y = (float)rawValue.y / Gyro_scaleValue;
	((PGYRO_DATA_SCALED)scaledData)->z = (float)rawValue.z / Gyro_scaleValue;
	}

	/*--------------------------------------------------------------------------------
	Function : MPU6050_Calibrate_Gyro
	Purpose : Get the value to calibrate Gyro
	Parameters : NULL
	Return : NULL
	--------------------------------------------------------------------------------*/

	VOID MPU6050_Calibrate_Gyro(VOID)
	{
	int i;
	int x = 0;
	int y = 0;
	int z = 0;
	BYTE pBuff[6];

	Gyro_OffsetValueX = 0;
	Gyro_OffsetValueY = 0;
	Gyro_OffsetValueZ = 0;
	for(i = 0; i < 2000; i++)
	{

	I2C_ReadData(pBuff, MPU6050_ADDRESS, MPU6050_GYRO_XOUT_H, 6);

	x = (int16)(CONVERT_TO_16BIT(pBuff[1], pBuff[0]));
	y = (int16)(CONVERT_TO_16BIT(pBuff[3], pBuff[2]));
	z = (int16)(CONVERT_TO_16BIT(pBuff[5], pBuff[4]));

	Gyro_OffsetValueX = (Gyro_OffsetValueX + x) >> 1;
	Gyro_OffsetValueY = (Gyro_OffsetValueY + y) >> 1;
	Gyro_OffsetValueZ = (Gyro_OffsetValueZ + z) >> 1;
	}
	// Gyro_OffsetValueX = (int16)(SumGyroX/200);
	// Gyro_OffsetValueY = (int16)(SumGyroY/200);
	// Gyro_OffsetValueZ = (int16)(SumGyroZ/200);
	}




	/*--------------------------------------------------------------------------------
	Function : ComplementaryFilter
	Purpose : Calculate angle use both acc and gyro
	Parameters : Raw data (ADC 16bit) of acc and gyro
	Return : ANGLE
	--------------------------------------------------------------------------------*/

	VOID Complementary_Filter(ACC_DATA_RAW accData, GYRO_DATA_RAW gyroData, PVOID pAngle)
	{
	float pitchAcc, rollAcc;

	/* Angle around the X axis */
	((PANGLE)pAngle)->x += ((float)gyroData.x / Gyro_scaleValue)*dt; // dt define as 0.015s
	/* Angle around the Y axis*/
	((PANGLE)pAngle)->y += ((float)gyroData.y / Gyro_scaleValue)*dt;

	// Compensate for drift with accelerometer data if !bullshit
	// Sensitivity = -2 to 2 G at 16Bit -> 2G = 32768 && 0.5G = 8192
	long forceMagnitudeApprox = ABS(accData.x) + ABS(accData.y) + ABS(accData.z);
	if (forceMagnitudeApprox > 8192 && forceMagnitudeApprox < 32768)
	{
	// Turning around the X axis results in a vector on the Y-axis
	pitchAcc = 57.296 * atan2((float)accData.y, sqrt((float)accData.z(float)accData.z + (float)accData.x(float)accData.x));
	((PANGLE)pAngle)->x = ((PANGLE)pAngle)->x * 0.98 + pitchAcc * 0.02;

	// Turning around the Y axis results in a vector on the X-axis
	rollAcc = 57.296 * atan2((float)accData.x, sqrt((float)accData.z(float)accData.z + (float)accData.y(float)accData.y));
	((PANGLE)pAngle)->y = ((PANGLE)pAngle)->y* 0.98 + rollAcc * 0.02;
	}

	}
	/********************************************************************************
	Module: MPU6050
	Author: 5/4/2015, by KienLTb

	Description: MPU6050 Lib for MSP430G2553

	********************************************************************************/
	#ifndef __MPU6050__H__
	#define __MPU6050__H__
	/-----------------------------------------------------------------------------/
	/* Header inclusions */
	/-----------------------------------------------------------------------------/
	#include "msp430g2553.h"
	//#include "i2c_sw.h"
	#include "i2c_hw.h"
	#include "types.h"
	/-----------------------------------------------------------------------------/
	/* Constant definitions */
	/-----------------------------------------------------------------------------/
	#define MPU6050_ADDRESS 0x68 // Address MPU6050 0b01101000 0b10100100
	#define MPU6050_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD
	#define MPU6050_YG_OFFS_TC 0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD
	#define MPU6050_ZG_OFFS_TC 0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD
	#define MPU6050_X_FINE_GAIN 0x03 //[7:0] X_FINE_GAIN
	#define MPU6050_Y_FINE_GAIN 0x04 //[7:0] Y_FINE_GAIN
	#define MPU6050_Z_FINE_GAIN 0x05 //[7:0] Z_FINE_GAIN
	#define MPU6050_XA_OFFS_H 0x06 //[15:0] XA_OFFS
	#define MPU6050_XA_OFFS_L_TC 0x07
	#define MPU6050_YA_OFFS_H 0x08 //[15:0] YA_OFFS
	#define MPU6050_YA_OFFS_L_TC 0x09
	#define MPU6050_ZA_OFFS_H 0x0A //[15:0] ZA_OFFS
	#define MPU6050_ZA_OFFS_L_TC 0x0B
	#define MPU6050_XG_OFFS_USRH 0x13 //[15:0] XG_OFFS_USR
	#define MPU6050_XG_OFFS_USRL 0x14
	#define MPU6050_YG_OFFS_USRH 0x15 //[15:0] YG_OFFS_USR
	#define MPU6050_YG_OFFS_USRL 0x16
	#define MPU6050_ZG_OFFS_USRH 0x17 //[15:0] ZG_OFFS_USR
	#define MPU6050_ZG_OFFS_USRL 0x18
	#define MPU6050_SMPLRT_DIV 0x19
	#define MPU6050_CONFIG 0x1A
	#define MPU6050_GYRO_CONFIG 0x1B
	#define MPU6050_ACCEL_CONFIG 0x1C
	#define MPU6050_FF_THR 0x1D
	#define MPU6050_FF_DUR 0x1E
	#define MPU6050_MOT_THR 0x1F
	#define MPU6050_MOT_DUR 0x20
	#define MPU6050_ZRMOT_THR 0x21
	#define MPU6050_ZRMOT_DUR 0x22
	#define MPU6050_FIFO_EN 0x23
	#define MPU6050_I2C_MST_CTRL 0x24
	#define MPU6050_I2C_SLV0_ADDR 0x25
	#define MPU6050_I2C_SLV0_REG 0x26
	#define MPU6050_I2C_SLV0_CTRL 0x27
	#define MPU6050_I2C_SLV1_ADDR 0x28
	#define MPU6050_I2C_SLV1_REG 0x29
	#define MPU6050_I2C_SLV1_CTRL 0x2A
	#define MPU6050_I2C_SLV2_ADDR 0x2B
	#define MPU6050_I2C_SLV2_REG 0x2C
	#define MPU6050_I2C_SLV2_CTRL 0x2D
	#define MPU6050_I2C_SLV3_ADDR 0x2E
	#define MPU6050_I2C_SLV3_REG 0x2F
	#define MPU6050_I2C_SLV3_CTRL 0x30
	#define MPU6050_I2C_SLV4_ADDR 0x31
	#define MPU6050_I2C_SLV4_REG 0x32
	#define MPU6050_I2C_SLV4_DO 0x33
	#define MPU6050_I2C_SLV4_CTRL 0x34
	#define MPU6050_I2C_SLV4_DI 0x35
	#define MPU6050_I2C_MST_STATUS 0x36
	#define MPU6050_INT_PIN_CFG 0x37
	#define MPU6050_INT_ENABLE 0x38
	#define MPU6050_DMP_INT_STATUS 0x39
	#define MPU6050_INT_STATUS 0x3A
	#define MPU6050_ACCEL_XOUT_H 0x3B
	#define MPU6050_ACCEL_XOUT_L 0x3C
	#define MPU6050_ACCEL_YOUT_H 0x3D
	#define MPU6050_ACCEL_YOUT_L 0x3E
	#define MPU6050_ACCEL_ZOUT_H 0x3F
	#define MPU6050_ACCEL_ZOUT_L 0x40
	#define MPU6050_TEMP_OUT_H 0x41
	#define MPU6050_TEMP_OUT_L 0x42
	#define MPU6050_GYRO_XOUT_H 0x43
	#define MPU6050_GYRO_XOUT_L 0x44
	#define MPU6050_GYRO_YOUT_H 0x45
	#define MPU6050_GYRO_YOUT_L 0x46
	#define MPU6050_GYRO_ZOUT_H 0x47
	#define MPU6050_GYRO_ZOUT_L 0x48
	#define MPU6050_EXT_SENS_DATA_00 0x49
	#define MPU6050_EXT_SENS_DATA_01 0x4A
	#define MPU6050_EXT_SENS_DATA_02 0x4B
	#define MPU6050_EXT_SENS_DATA_03 0x4C
	#define MPU6050_EXT_SENS_DATA_04 0x4D
	#define MPU6050_EXT_SENS_DATA_05 0x4E
	#define MPU6050_EXT_SENS_DATA_06 0x4F
	#define MPU6050_EXT_SENS_DATA_07 0x50
	#define MPU6050_EXT_SENS_DATA_08 0x51
	#define MPU6050_EXT_SENS_DATA_09 0x52
	#define MPU6050_EXT_SENS_DATA_10 0x53
	#define MPU6050_EXT_SENS_DATA_11 0x54
	#define MPU6050_EXT_SENS_DATA_12 0x55
	#define MPU6050_EXT_SENS_DATA_13 0x56
	#define MPU6050_EXT_SENS_DATA_14 0x57
	#define MPU6050_EXT_SENS_DATA_15 0x58
	#define MPU6050_EXT_SENS_DATA_16 0x59
	#define MPU6050_EXT_SENS_DATA_17 0x5A
	#define MPU6050_EXT_SENS_DATA_18 0x5B
	#define MPU6050_EXT_SENS_DATA_19 0x5C
	#define MPU6050_EXT_SENS_DATA_20 0x5D
	#define MPU6050_EXT_SENS_DATA_21 0x5E
	#define MPU6050_EXT_SENS_DATA_22 0x5F
	#define MPU6050_EXT_SENS_DATA_23 0x60
	#define MPU6050_MOT_DETECT_STATUS 0x61
	#define MPU6050_I2C_SLV0_DO 0x63
	#define MPU6050_I2C_SLV1_DO 0x64
	#define MPU6050_I2C_SLV2_DO 0x65
	#define MPU6050_I2C_SLV3_DO 0x66
	#define MPU6050_I2C_MST_DELAY_CTRL 0x67
	#define MPU6050_SIGNAL_PATH_RESET 0x68
	#define MPU6050_MOT_DETECT_CTRL 0x69
	#define MPU6050_USER_CTRL 0x6A
	#define MPU6050_PWR_MGMT_1 0x6B
	#define MPU6050_PWR_MGMT_2 0x6C
	#define MPU6050_BANK_SEL 0x6D
	#define MPU6050_MEM_START_ADDR 0x6E
	#define MPU6050_MEM_R_W 0x6F
	#define MPU6050_DMP_CFG_1 0x70
	#define MPU6050_DMP_CFG_2 0x71
	#define MPU6050_FIFO_COUNTH 0x72
	#define MPU6050_FIFO_COUNTL 0x73
	#define MPU6050_FIFO_R_W 0x74
	#define MPU6050_WHO_AM_I 0x75
	/---------- CONFIG VALUE----------/
	/* MPU6050_PWR_MGMT_1 REG */
	#define CLKSEL_0 0x00 //Internal 8MHz Osilator
	#define CLKSEL_1 0x01 //PLL with X axis gyroscope reference
	#define CLKSEL_2 0x02 //PLL with Y axis gyroscope reference
	#define CLKSEL_3 0x03 //PLL with Z axis gyroscope reference
	#define CLKSEL_4 0x04 //PLL with external 32.768kHz reference
	#define CLKSEL_5 0x05 //PLL with external 19.2MHz reference
	#define CLKSEL_6 0x06 //Reserved
	#define CLKSEL_7 0x07 //Stops the clock and keeps the timing generator in reset

	#define TEMP_DIS (0x01 << 3) //disables the temperature sensor
	#define CYCLE (0x01 << 5) //
	#define SLEEP (0x01 << 6) //
	#define DEVICE_RESET (0x01 << 7) //

	/----------MPU6050_CONFIG----------/
	#define EXT_SYNC_SET_INPUT_DISABLE (0x00 << 3)// Input disabled
	#define EXT_SYNC_SET_TEMP_OUT (0x01 << 3)// TEMP_OUT_L[0]
	#define EXT_SYNC_SET_GYRO_XOUT (0x02 << 3)// GYRO_XOUT_L[0]
	#define EXT_SYNC_SET_GYRO_YOUT (0x03 << 3)// GYRO_YOUT_L[0]
	#define EXT_SYNC_SET_GYRO_ZOUT (0x04 << 3)// GYRO_ZOUT_L[0]
	#define EXT_SYNC_SET_ACCEL_XOUT (0x05 << 3)// ACCEL_XOUT_L[0]
	#define EXT_SYNC_SET_ACCEL_YOUT (0x06 << 3)// ACCEL_YOUT_L[0]
	#define EXT_SYNC_SET_ACCEL_ZOUT (0x07 << 3)// ACCEL_ZOUT_L[0]

	#define DLPF_CFG_BAND_WIDTH_260HZ 0x00// BandWidth 260Hz
	#define DLPF_CFG_BAND_WIDTH_184HZ 0x01// BandWidth 184Hz
	#define DLPF_CFG_BAND_WIDTH_94HZ 0x02// BandWidth 94Hz
	#define DLPF_CFG_BAND_WIDTH_44HZ 0x03// BandWidth 44Hz
	#define DLPF_CFG_BAND_WIDTH_21HZ 0x04// BandWidth 21Hz
	#define DLPF_CFG_BAND_WIDTH_10HZ 0x05// BandWidth 10Hz
	#define DLPF_CFG_BAND_WIDTH_5HZ 0x06// BandWidth 5Hz

	/---------MPU6050_GYRO_CONFIG-------/
	#define PS_SEL_SCALE_250 (0x00 << 3)
	#define PS_SEL_SCALE_500 (0x01 << 3)
	#define PS_SEL_SCALE_1000 (0x02 << 3)
	#define PS_SEL_SCALE_2000 (0x03 << 3)

	#define ZG_ST (0x01 << 5)
	#define YG_ST (0x01 << 6)
	#define XG_ST (0x01 << 7)

	/---------MPU6050_ACCEL_CONFIG------/
	#define AFS_SEL_SCALE_2G (0x00 << 3)
	#define AFS_SEL_SCALE_4G (0x01 << 3)
	#define AFS_SEL_SCALE_8G (0x02 << 3)
	#define AFS_SEL_SCALE_16G (0x03 << 3)

	#define ZA_ST (0x01 << 5)
	#define YA_ST (0x01 << 6)
	#define XA_ST (0x01 << 7)
	/---------MPU6050_SMPLRT_DIV_CONFIG---/
	#define SET_SAMPLE_RATE_1000HZ 0x07
	/-----------------------------------------------------------------------------/
	/* Macro definitions */
	/-----------------------------------------------------------------------------/

	/-----------------------------------------------------------------------------/
	/* Global variables */
	/-----------------------------------------------------------------------------/
	#define GYRO_CONFIG_250 0
	#define GYRO_CONFIG_500 1
	#define GYRO_CONFIG_1000 2
	#define GYRO_CONFIG_2000 3


	#define ACC_CONFIG_2G 4
	#define ACC_CONFIG_4G 5
	#define ACC_CONFIG_8G 6
	#define ACC_CONFIG_16G 7


	/-----------------------------------------------------------------------------/
	/* Data type definitions */
	/-----------------------------------------------------------------------------/
	//typedef unsigned char BYTE;
	//typedef unsigned short WORD;
	//typedef unsigned long DWORD;
	//typedef unsigned int UINT;
	//typedef BYTE BOOL;
	//typedef unsigned char CHAR;
	//typedef void VOID;
	//
	//typedef BYTE* PBYTE;
	//typedef WORD* PWORD;
	//typedef DWORD* PDWORD;
	//typedef UINT* PUINT;
	//typedef CHAR* PCHAR;
	//typedef VOID* PVOID;
	//
	//typedef BYTE RESULT;
	//
	//typedef const BYTE* PCBYTE;
	//
	//#define FALSE 0
	//#define TRUE 1
	//#define NULL 0
	/* Acc data raw value */
	typedef struct _ACC_DATA_RAW
	{
	int16 x;
	int16 y;
	int16 z;

	}ACC_DATA_RAW, *PACC_DATA_RAW;

	/* Acc data value in angle */
	typedef struct _ACC_DATA_SCALED
	{
	float x;
	float y;
	float z;
	}ACC_DATA_SCALED, *PACC_DATA_SCALED;

	/* Gyro data raw value */
	typedef struct _GYRO_DATA_RAW
	{
	int16 x;
	int16 y;
	int16 z;
	}GYRO_DATA_RAW, *PGYRO_DATA_RAW;

	/* Gyro data value in angle */
	typedef struct _GYRO_DATA_SCALED
	{
	float x;
	float y;
	float z;
	}GYRO_DATA_SCALED, *PGYRO_DATA_SCALED;

	/* Angle data caculate from raw value */
	typedef struct _ANGLE
	{
	float x;
	float y;
	float z;
	}ANGLE, *PANGLE;
	/-----------------------------------------------------------------------------/
	/* Function prototypes */
	/-----------------------------------------------------------------------------/

	VOID MPU6050_Init(BYTE ACC_SCALE_CONFIG, BYTE GYRO_SCALE_CONFIG);

	/* MPU6050 test i2c connection */
	BYTE MPU6050_CheckI2C(VOID);

	/* MPU6050 test configure of register*/
	BYTE MPU6050_TestRegConfig(VOID);

	/* Get the calibrate value and store to offset*/
	VOID MPU6050_Calibrate_Gyro(VOID);
	/* Raw Acc Value*/
	VOID MPU6050_GetAccValueRaw(PVOID pValue);

	/* Value in degree/s */
	VOID PMU6050_AccConvertData(ACC_DATA_RAW rawValue, PVOID scaledData);

	/* Get rotation of sensor, value in degree*/
	VOID PMU6050_GetRotationAngle(ACC_DATA_RAW raw, PVOID pRotationAngle);

	/* Raw Gyro Value to m/s^2*/
	VOID MPU6050_GetGyroValueRaw(PVOID pValue);

	/* Convert to m/s^2*/
	VOID PMU6050_GyroConvertData(GYRO_DATA_RAW rawValue, PVOID scaledData);

	/* Complementrary Filter*/
	VOID Complementary_Filter(ACC_DATA_RAW accData, GYRO_DATA_RAW gyroData, PVOID pAngle);

	#endif // __MPU6050__H__