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

#include "twi_master.h"
#include "rec.h"

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



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





/****************************************** 寄存器版 ****************************************************/
//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
	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 , ACCEL_CONFIG);	  //配置加速度传感器工作在2G模式，不自检   正常我 用0x08
	MPU6050_WriteReg(MPU6050_RA_GYRO_CONFIG, GYRO_CONFIG);     //陀螺仪自检及测量范围，典型值：0x18(不自检，2000deg/s)   0x00   2502000deg/s    正常我 用0x08
	
	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;
}