#include "mpu6050.h"
#include "twi_master.h"
#include "nrf_delay.h"

//#include "twi_master.h"
//#include "mltypes.h"
//#include "inv_mpu_dmp_motion_driver.h"
//#include "log.h"
//#include "invensense.h"
//#include "invensense_adv.h"
//#include "eMPL_outputs.h"
//#include "mpu.h"
//#include "inv_mpu.h"
//#include "log.h"
//#include "data_builder.h"

//unsigned char *mpl_key = (unsigned char*)"eMPL 5.1";

//volatile uint32_t hal_timestamp = 0;
//#define ACCEL_ON        (0x01)
//#define GYRO_ON         (0x02)
//#define COMPASS_ON      (0x04)

//#define MOTION          (0)
//#define NO_MOTION       (1)

///* Starting sampling rate. */
//#define DEFAULT_MPU_HZ  (100)

//#define FLASH_SIZE      (512)
//#define FLASH_MEM_START ((void*)0x1800)

//#define PEDO_READ_MS    (1000)
//#define TEMP_READ_MS    (500)
//#define COMPASS_READ_MS (100)

////struct rx_s {
////    unsigned char header[3];
////    unsigned char cmd;
////};

////struct hal_s {
////    unsigned char lp_accel_mode;
////    unsigned char sensors;
////    unsigned char dmp_on;
////    unsigned char wait_for_tap;
////    volatile unsigned char new_gyro;
////    unsigned char motion_int_mode;
////    unsigned long no_dmp_hz;
////    unsigned long next_pedo_ms;
////    unsigned long next_temp_ms;
////    unsigned long next_compass_ms;
////    unsigned int report;
////    unsigned short dmp_features;
////    struct rx_s rx;
////};
////static struct hal_s hal = {0};

///* Platform-specific information. Kinda like a boardfile. */
//struct platform_data_s {
//    signed char orientation[9];
//};
//static struct platform_data_s gyro_pdata = {
//		 .orientation = { 1, 0, 0,
//                     0, 1, 0,
//                     0, 0, 1}
//};

int mpu6050_register_write_len(uint8_t addr,uint8_t register_address, uint8_t len,uint8_t *buf)
{
	uint8_t w2_data[50],i;

//	printf("\r\n");
//	printf("write addr=0x%2X,",addr);
//	printf("register_address=0x%2X,",register_address);
	
	w2_data[0] = register_address;
	for(i=0;i<len;i++){
		w2_data[i+1] = *(buf+i);
//		printf("buf[%d]=%2X,",i,buf[i]);
	}
//	printf("\r\n");

	
	
	if(twi_master_transfer(addr, w2_data, len+1, TWI_ISSUE_STOP) == true) return 0;
	else return -1;
}

int mpu6050_register_read_len(uint8_t addr,uint8_t register_address, uint8_t number_of_bytes,uint8_t * destination )
{
    bool transfer_succeeded;
    transfer_succeeded  = twi_master_transfer(addr, &register_address, 1, TWI_DONT_ISSUE_STOP);
    transfer_succeeded &= twi_master_transfer(addr|TWI_READ_BIT, destination, number_of_bytes, TWI_ISSUE_STOP);
    
	  if(transfer_succeeded == true)return 0;
	  else return -1;
}

//volatile uint32_t g_ul_ms_ticks=0;
//int get_tick_count(unsigned long *count)
//{
//	count[0] = g_ul_ms_ticks;
//	return 0;
//}

//void mdelay(unsigned long nTime)
//{
//	nrf_delay_ms(nTime);
//}

//int _MLPrintLog (int priority, const char* tag, const char* fmt, ...)
//{
//	return 0;
//}

//static void tap_cb(unsigned char direction, unsigned char count)
//{
////	uint8_t buf[2];
////	uint16_t L=0;
//	printf("tap_cb:direction=%d,count=%d\n",direction,count);
//    return;
//}

//static void android_orient_cb(unsigned char orientation)
//{
//	printf("android_orient_cb:orientation=%d\n",orientation);
//	return;
//}

//int mpu6050_Init(void)
//{
//	inv_error_t result;
//	unsigned char accel_fsr;
//	unsigned short gyro_rate, gyro_fsr;
////	unsigned long timestamp;
//	static struct int_param_s int_param;
//	
//	
//	if(mpu_init(&int_param)){ printf("mpu_init fail! \r\n");
//		return -1;
//	}
//	if(inv_init_mpl()){ printf("inv_init_mpl fail! \r\n");
//		return -1;
//	}
//	
//	/* Compute 6-axis and 9-axis quaternions. */
//    inv_enable_quaternion();
//    inv_enable_9x_sensor_fusion();
//	
//	inv_enable_fast_nomot();
//	inv_enable_gyro_tc();
//	
//#ifdef COMPASS_ENABLED
//    /* Compass calibration algorithms. */
//    inv_enable_vector_compass_cal();
//    inv_enable_magnetic_disturbance();
//#endif
//	
//	inv_enable_eMPL_outputs();
//	
//	result = inv_start_mpl();
//	if (result == INV_ERROR_NOT_AUTHORIZED) {
//		printf("Not authorized.\n");
//	}
//	if (result) {
//		printf("Could not start the MPL.\n");
//	}
//	
//	
//	/* Get/set hardware configuration. Start gyro. */
//    /* Wake up all sensors. */
//#ifdef COMPASS_ENABLED
//    mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS);
//#else
//    mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
//#endif
//    /* Push both gyro and accel data into the FIFO. */
//    mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
//    mpu_set_sample_rate(DEFAULT_MPU_HZ);
//	
//#ifdef COMPASS_ENABLED
//    /* The compass sampling rate can be less than the gyro/accel sampling rate.
//     * Use this function for proper power management.
//     */
//    mpu_set_compass_sample_rate(1000 / COMPASS_READ_MS);
//#endif
//    /* Read back configuration in case it was set improperly. */
//    mpu_get_sample_rate(&gyro_rate);
//    mpu_get_gyro_fsr(&gyro_fsr);
//    mpu_get_accel_fsr(&accel_fsr);
//#ifdef COMPASS_ENABLED
//    mpu_get_compass_fsr(&compass_fsr);
//#endif
//    /* Sync driver configuration with MPL. */
//    /* Sample rate expected in microseconds. */
//    inv_set_gyro_sample_rate(1000000L / gyro_rate);
//    inv_set_accel_sample_rate(1000000L / gyro_rate);
//	
//#ifdef COMPASS_ENABLED
//    /* The compass rate is independent of the gyro and accel rates. As long as
//     * inv_set_compass_sample_rate is called with the correct value, the 9-axis
//     * fusion algorithm's compass correction gain will work properly.
//     */
//    inv_set_compass_sample_rate(COMPASS_READ_MS * 1000L);
//#endif
//    /* Set chip-to-body orientation matrix.
//     * Set hardware units to dps/g's/degrees scaling factor.
//     */
//    inv_set_gyro_orientation_and_scale(
//            inv_orientation_matrix_to_scalar(gyro_pdata.orientation),
//            (long)gyro_fsr<<15);
//    inv_set_accel_orientation_and_scale(
//            inv_orientation_matrix_to_scalar(gyro_pdata.orientation),
//            (long)accel_fsr<<15);
//#ifdef COMPASS_ENABLED
//    inv_set_compass_orientation_and_scale(
//            inv_orientation_matrix_to_scalar(compass_pdata.orientation),
//            (long)compass_fsr<<15);
//#endif

// /* Initialize HAL state variables. */
////#ifdef COMPASS_ENABLED
////    hal.sensors = ACCEL_ON | GYRO_ON | COMPASS_ON;
////#else
////    hal.sensors = ACCEL_ON | GYRO_ON;
////#endif
////    hal.dmp_on = 0;
////    hal.report = 0;
////    hal.rx.cmd = 0;
////    hal.next_pedo_ms = 0;
////    hal.next_compass_ms = 0;
////    hal.next_temp_ms = 0;
//	
//	/* Compass reads are handled by scheduler. */
////  get_tick_count(&timestamp);

//    /* To initialize the DMP:
//     * 1. Call dmp_load_motion_driver_firmware(). This pushes the DMP image in
//     *    inv_mpu_dmp_motion_driver.h into the MPU memory.
//     * 2. Push the gyro and accel orientation matrix to the DMP.
//     * 3. Register gesture callbacks. Don't worry, these callbacks won't be
//     *    executed unless the corresponding feature is enabled.
//     * 4. Call dmp_enable_feature(mask) to enable different features.
//     * 5. Call dmp_set_fifo_rate(freq) to select a DMP output rate.
//     * 6. Call any feature-specific control functions.
//     *
//     * To enable the DMP, just call mpu_set_dmp_state(1). This function can
//     * be called repeatedly to enable and disable the DMP at runtime.
//     *
//     * The following is a short summary of the features supported in the DMP
//     * image provided in inv_mpu_dmp_motion_driver.c:
//     * DMP_FEATURE_LP_QUAT: Generate a gyro-only quaternion on the DMP at
//     * 200Hz. Integrating the gyro data at higher rates reduces numerical
//     * errors (compared to integration on the MCU at a lower sampling rate).
//     * DMP_FEATURE_6X_LP_QUAT: Generate a gyro/accel quaternion on the DMP at
//     * 200Hz. Cannot be used in combination with DMP_FEATURE_LP_QUAT.
//     * DMP_FEATURE_TAP: Detect taps along the X, Y, and Z axes.
//     * DMP_FEATURE_ANDROID_ORIENT: Google's screen rotation algorithm. Triggers
//     * an event at the four orientations where the screen should rotate.
//     * DMP_FEATURE_GYRO_CAL: Calibrates the gyro data after eight seconds of
//     * no motion.
//     * DMP_FEATURE_SEND_RAW_ACCEL: Add raw accelerometer data to the FIFO.
//     * DMP_FEATURE_SEND_RAW_GYRO: Add raw gyro data to the FIFO.
//     * DMP_FEATURE_SEND_CAL_GYRO: Add calibrated gyro data to the FIFO. Cannot
//     * be used in combination with DMP_FEATURE_SEND_RAW_GYRO.
//     */

//    dmp_load_motion_driver_firmware();
//    dmp_set_orientation(
//        inv_orientation_matrix_to_scalar(gyro_pdata.orientation));
//    dmp_register_tap_cb(tap_cb);

//    dmp_register_android_orient_cb(android_orient_cb);
//    /*
//     * Known Bug -
//     * DMP when enabled will sample sensor data at 200Hz and output to FIFO at the rate
//     * specified in the dmp_set_fifo_rate API. The DMP will then sent an interrupt once
//     * a sample has been put into the FIFO. Therefore if the dmp_set_fifo_rate is at 25Hz
//     * there will be a 25Hz interrupt from the MPU device.
//     *
//     * There is a known issue in which if you do not enable DMP_FEATURE_TAP
//     * then the interrupts will be at 200Hz even if fifo rate
//     * is set at a different rate. To avoid this issue include the DMP_FEATURE_TAP
//     *
//     * DMP sensor fusion works only with gyro at +-2000dps and accel +-2G
//     */
////    hal.dmp_features = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
////        DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
////        DMP_FEATURE_GYRO_CAL;
////    dmp_enable_feature(hal.dmp_features);
//	dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
//        DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
//        DMP_FEATURE_GYRO_CAL);
//    dmp_set_fifo_rate(DEFAULT_MPU_HZ);
//    mpu_set_dmp_state(1);
//	return 0;
//}

//int mpu6050_get_dmp_data(short *gyro, short *accel, float *quat)
//{
//	short gy[3], acc[3];
//	long q[4];
//	
//	unsigned long timestamp;
//	
//	short sensors;
//	unsigned char more;
//	float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
////	float pitch,roll,yaw;
//	
//	dmp_read_fifo(gy, acc, q, &timestamp, &sensors, &more);
//	if((sensors & INV_WXYZ_QUAT)){
//		/* DMP所得的四元数 */
//		q0=q[0] / 1073741824.0f;
//		q1=q[1] / 1073741824.0f;
//		q2=q[2] / 1073741824.0f;
//		q3=q[3] / 1073741824.0f;
//		quat[0] = q0;
//		quat[1] = q1;
//		quat[2] = q2;
//		quat[3] = q3;
//		/* 由四元数所得的欧拉角，单位度 */
////		pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;	                                // pitch
////		roll  = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3;	// roll
////		yaw   = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;	              //yaw
////		printf("pitch=%f,roll=%f,yaw=%f\n",pitch,roll,yaw);
//	}
//	if((sensors & INV_XYZ_GYRO)){
//		gyro[0] = gy[0];
//		gyro[1] = gy[1];
//		gyro[2] = gy[2];
//	}
//	if((sensors & INV_XYZ_ACCEL)){
//		accel[0] = acc[0];
//		accel[1] = acc[1];
//		accel[2] = acc[2];
//	}
//	
//	if( (sensors & INV_WXYZ_QUAT) && (sensors & INV_XYZ_GYRO) && (sensors & INV_XYZ_ACCEL)) return 0;
//	return -1;
//}

//int mpu6050_get_linear_data(float *gyro, float *accel, float *quat)
//{
//	short gy[3], acc[3];
//	long q[4];
//	
//	float acc_f[4];
//	unsigned long timestamp;
//	long acc_long[3];
//	
//	int8_t accuracy = 0;
//	
//	short sensors;
//	unsigned char more;
//	float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
////	float pitch,roll,yaw;
//	
//	dmp_read_fifo(gy, acc, q, &timestamp, &sensors, &more);
//	if((sensors & INV_WXYZ_QUAT)){
//		/* DMP所得的四元数 */
//		q0=q[0] / 1073741824.0f;
//		q1=q[1] / 1073741824.0f;
//		q2=q[2] / 1073741824.0f;
//		q3=q[3] / 1073741824.0f;
//		quat[0] = q0;
//		quat[1] = q1;
//		quat[2] = q2;
//		quat[3] = q3;
//		inv_build_quat(q, 0, timestamp);
//		/* 由四元数所得的欧拉角，单位度 */
////		pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;	                                // pitch
////		roll  = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3;	// roll
////		yaw   = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;	              //yaw
////		printf("pitch=%f,roll=%f,yaw=%f\n",pitch,roll,yaw);
//	}
//	if((sensors & INV_XYZ_GYRO)){
//		gyro[0] = (float)(gy[0]/16.4f);
//		gyro[1] = (float)(gy[1]/16.4f);
//		gyro[2] = (float)(gy[2]/16.4f);
//		inv_build_gyro(gy, timestamp);
//	}
//	if((sensors & INV_XYZ_ACCEL)){
////		accel[0] = acc[0];
////		accel[1] = acc[1];
////		accel[2] = acc[2];
//		acc_long[0] = (long)acc[0];
//		acc_long[1] = (long)acc[1];
//		acc_long[2] = (long)acc[2];
//		inv_build_accel(acc_long, 0, timestamp);
//		inv_execute_on_data();
//		inv_get_sensor_type_linear_acceleration(acc_f, &accuracy, (inv_time_t*)&timestamp);
////		printf("%f,%f,%f \n",acc_f[0],acc_f[1],acc_f[2]); 
//		accel[0] = acc_f[0];
//		accel[1] = acc_f[1];
//		accel[2] = acc_f[2];
//		
//		
//		return 0;
//	}
//	
////	if( sensors & (INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_WXYZ_QUAT)) return 0;
//	return -1;
//}

//int mpu6050_get_linear_data_int(short *gyro, short *accel, long *quat)
//{
//	short gy[3], acc[3];
//	long q[4];
//	
//	float acc_f[4];
//	unsigned long timestamp;
//	long acc_long[3];
//	
//	int8_t accuracy = 0;
//	
//	short sensors;
//	unsigned char more;
////	float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
////	float pitch,roll,yaw;
//	
//	dmp_read_fifo(gy, acc, q, &timestamp, &sensors, &more);
//	if((sensors & INV_WXYZ_QUAT)){
//		/* DMP所得的四元数 */
////		q0=q[0] / 1073741824.0f;
////		q1=q[1] / 1073741824.0f;
////		q2=q[2] / 1073741824.0f;
////		q3=q[3] / 1073741824.0f;
//		quat[0] = q[0];
//		quat[1] = q[1];
//		quat[2] = q[2];
//		quat[3] = q[3];
//		inv_build_quat(q, 0, timestamp);
//		/* 由四元数所得的欧拉角，单位度 */
////		pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;	                                // pitch
////		roll  = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3;	// roll
////		yaw   = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;	              //yaw
////		printf("pitch=%f,roll=%f,yaw=%f\n",pitch,roll,yaw);
//	}
//	if((sensors & INV_XYZ_GYRO)){
//		gyro[0] = gy[0];
//		gyro[1] = gy[1];
//		gyro[2] = gy[2];
//		inv_build_gyro(gy, timestamp);
//	}
//	if((sensors & INV_XYZ_ACCEL)){
////		accel[0] = acc[0];
////		accel[1] = acc[1];
////		accel[2] = acc[2];
//		acc_long[0] = (long)acc[0];
//		acc_long[1] = (long)acc[1];
//		acc_long[2] = (long)acc[2];
//		inv_build_accel(acc_long, 0, timestamp);
//		inv_execute_on_data();
//		inv_get_sensor_type_linear_acceleration(acc_f, &accuracy, (inv_time_t*)&timestamp);
//		accel[0] = acc_f[0]*100;
//		accel[1] = acc_f[1]*100;
//		accel[2] = acc_f[2]*100;
//		return 0;
//	}
//	
////	if( sensors & (INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_WXYZ_QUAT)) return 0;
//	return -1;
//}



/****************************************** 寄存器版 ****************************************************/
//int mpu6050_register_write_len(uint8_t addr,uint8_t register_address, uint8_t len,uint8_t *buf)
//int mpu6050_register_read_len(uint8_t addr,uint8_t register_address, uint8_t number_of_bytes,uint8_t * destination )
/**
  * @brief   写数据到MPU6050寄存器
  * @param   
  * @retval  
  */
int MPU6050_WriteReg(uint8_t reg_add,uint8_t reg_dat)
{
	if (mpu6050_register_write_len(MPU6050_ADDR, reg_add, 1, &reg_dat))
        return -1;
	return 0;
}


/**
  * @brief   从MPU6050寄存器读取数据
  * @param   
  * @retval  
  */
int MPU6050_ReadData(uint8_t reg_add,unsigned char*Read,uint8_t num)
{
	if (mpu6050_register_read_len(MPU6050_ADDR,reg_add,num,Read))
        return -1;
	return 0;
}

/**
  * @brief   初始化MPU6050芯片
  * @param   
  * @retval  
  */
void MPU6050_Init_reg(void)
{
	MPU6050_WriteReg(MPU6050_RA_SIGNAL_PATH_RESET, 0x07);	    //Resets gyro, accelerometer and temperature sensor signal paths
	nrf_delay_ms(100);
	MPU6050_WriteReg(MPU6050_RA_PWR_MGMT_1, 0x00);	    //解除休眠状态
	nrf_delay_ms(100);
	MPU6050_WriteReg(MPU6050_RA_SMPLRT_DIV , 0x07);	    //陀螺仪采样率，1KHz
	MPU6050_WriteReg(MPU6050_RA_CONFIG , 0x06);	        //低通滤波器的设置，截止频率是1K，带宽是5K
	MPU6050_WriteReg(MPU6050_RA_ACCEL_CONFIG , 0x18);	//4g
	MPU6050_WriteReg(MPU6050_RA_GYRO_CONFIG, 0x18);     //2000deg/s
	MPU6050_WriteReg(MPU6050_RA_USER_CTRL, 0x00);
	MPU6050_WriteReg(MPU6050_RA_INT_PIN_CFG, 0x32);
	MPU6050_WriteReg(MPU6050_RA_INT_ENABLE, 0x01);
	nrf_delay_ms(100);
// MPU6050_WriteReg(MPU6050_RA_SIGNAL_PATH_RESET, 0x07);     //Resets gyro, accelerometer and temperature sensor signal paths
// MPU6050_WriteReg(MPU6050_RA_PWR_MGMT_1, 0x00);     //??????
// MPU6050_WriteReg(MPU6050_RA_SMPLRT_DIV , 0x07);     //??????,1KHz
// MPU6050_WriteReg(MPU6050_RA_CONFIG , 0x06);         //????????,?????1K,???5K
// 
// MPU6050_WriteReg(MPU6050_RA_ACCEL_CONFIG , 0x08);   //???????????2G??,???
// MPU6050_WriteReg(MPU6050_RA_GYRO_CONFIG, 0x08);     //??????????,???:0x18(???,2000deg/s)   0x00   2502000deg/s
// 
// MPU6050_WriteReg(MPU6050_RA_USER_CTRL, 0x00);
// MPU6050_WriteReg(MPU6050_RA_INT_PIN_CFG, 0x32);
// MPU6050_WriteReg(MPU6050_RA_INT_ENABLE, 0x01);
	
//	MPU6050_WriteReg(MPU6050_RA_SIGNAL_PATH_RESET, 0x04U | 0x02U | 0x01U);
//	MPU6050_WriteReg(MPU6050_RA_SMPLRT_DIV, 0x00);
//	MPU6050_WriteReg(MPU6050_RA_CONFIG, 0x00);
//	MPU6050_WriteReg(MPU6050_RA_PWR_MGMT_1, 0x03);
//	MPU6050_WriteReg(MPU6050_RA_GYRO_CONFIG, 0x10);
//	MPU6050_WriteReg(MPU6050_RA_USER_CTRL, 0x00);
//	MPU6050_WriteReg(MPU6050_RA_INT_PIN_CFG, 0x32);
//	MPU6050_WriteReg(MPU6050_RA_INT_ENABLE, 0x01);
//	MPU6050_WriteReg(MPU6050_RA_ACCEL_CONFIG, 0x00);
}

/**
  * @brief   读取MPU6050的ID
  * @param   
  * @retval  
  */
uint8_t MPU6050ReadID(void)
{
	unsigned char Re = 0;
    MPU6050_ReadData(MPU6050_RA_WHO_AM_I,&Re,1);    //读器件地址
	if(Re != 0x68)
	{
		printf("MPU6050 dectected error!\r\n检测不到MPU6050模块，请检查模块与开发板的接线");
		return 0;
	}
	else
	{
		printf("MPU6050 ID = %d\r\n",Re);
		return 1;
	}
		
}
/**
  * @brief   读取MPU6050的加速度数据
  * @param   
  * @retval  
  */
int MPU6050ReadAcc(short *accData)
{
    uint8_t buf[6];
    if(MPU6050_ReadData(MPU6050_ACC_OUT, buf, 6))
		return -1;
    accData[0] = (buf[0] << 8) | buf[1];
    accData[1] = (buf[2] << 8) | buf[3];
    accData[2] = (buf[4] << 8) | buf[5];
	return 0;
}

/**
  * @brief   读取MPU6050的角加速度数据
  * @param   
  * @retval  
  */
int MPU6050ReadGyro(short *gyroData)
{
    uint8_t buf[6];
    if(MPU6050_ReadData(MPU6050_GYRO_OUT,buf,6))
		return -1;
    gyroData[0] = (buf[0] << 8) | buf[1];
    gyroData[1] = (buf[2] << 8) | buf[3];
    gyroData[2] = (buf[4] << 8) | buf[5];
	return 0;
}

int mpu6050_get_reg_data(short *gyro, short *accel)
{
	int ret = 0;
	if(MPU6050ReadGyro(gyro)) ret = -1;
	if(MPU6050ReadAcc(accel)) ret = -1;
	return ret;
}


/**
  * @brief   读取MPU6050的原始温度数据
  * @param   
  * @retval  
  */
int MPU6050ReadTemp(short *tempData)
{
	uint8_t buf[2];
    if(MPU6050_ReadData(MPU6050_RA_TEMP_OUT_H,buf,2))
		return -1;
    *tempData = (buf[0] << 8) | buf[1];
	return 0;
}


/**
  * @brief   读取MPU6050的温度数据，转化成摄氏度
  * @param   
  * @retval  
  */
int MPU6050_ReturnTemp(float *Temperature)
{
	short temp3;
	uint8_t buf[2];
	
	if(MPU6050_ReadData(MPU6050_RA_TEMP_OUT_H,buf,2))
		return -1;
	temp3= (buf[0] << 8) | buf[1];	
	*Temperature=((double) temp3/340.0)+36.53;
	return 0;
}