#include "data.h"
#include "hal.h"

#define	SMPLRT_DIV		0x19	//陀螺仪采样率，典型值：0x07(125Hz)
#define	CONFIGL			0x1A	//低通滤波频率，典型值：0x06(5Hz)
#define	GYRO_CONFIG		0x1B	//陀螺仪自检及测量范围，典型值：0x18(不自检，2000deg/s)
#define	ACCEL_CONFIG	0x1C	//加速计自检、测量范围及高通滤波频率，典型值：0x01(不自检，2G，5Hz)
#define	ACCEL_ADDRESS	0x3B
#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_ADDRESS  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	PWR_MGMT_1		0x6B	//电源管理，典型值：0x00(正常启用)
#define	WHO_AM_I		  0x75	//IIC地址寄存器(默认数值0x68，只读)
#define MPU6050_PRODUCT_ID 0x68
#define MPU6052C_PRODUCT_ID 0x72
#define MPU9250_PRODUCT_ID 0x71  //
//#define   MPU6050_is_DRY()      GPIO_ReadOutBit(HT_GPIOC, GPIO_PIN_0)//IRQ主机数据输入
	#ifdef  	USE_I2C_HARDWARE
		
		#define MPU6050_ADDRESS 0xD0//0x68
	#else
		#define  MPU6050_ADDRESS 0xD0   //IIC写入时的地址字节数据，+1为读取
	#endif


void delay_ms(int x){
	
	for(x = 0; x < 1000; x++){
			volatile int z = 0;
			for (z = 0; z < 100;z++){
				
			}
		}
}

int16_t MpuOffset[6] = {0};

_st_Mpu MPU6050;   //MPU6050原始数据

static volatile int16_t *pMpu = (int16_t *)&MPU6050;



/****************************************************************************************
*@brief  
*@brief   
*@param[in]
*****************************************************************************************/
int8_t mpu6050_rest(void)
{
	if(MPUWriteReg( PWR_MGMT_1, 0x80) == FAILED)
		return FAILED;	//复位
		//delay_ms(20);
	return SUCCESS;
}
/****************************************************************************************
*@brief   
*@brief  
*@param[in]
*****************************************************************************************/
int8_t MpuInit(void) //初始化
{
	uint8_t date = SUCCESS;
	do
	{
		date = MPUWriteReg( PWR_MGMT_1, 0x80);	//复位
		delay_ms(30);
		date += MPUWriteReg( SMPLRT_DIV, 0x02); //陀螺仪采样率，0x00(500Hz)
		date += MPUWriteReg( PWR_MGMT_1, 0x03);	//设置设备时钟源，陀螺仪Z轴
		date += MPUWriteReg( CONFIGL, 0x03);   //低通滤波频率，0x03(42Hz)
		date += MPUWriteReg( GYRO_CONFIG, 0x18);//+-2000deg/s
		date += MPUWriteReg( ACCEL_CONFIG, 0x09);//+-4G
	}
	while(date != SUCCESS);
	date = MPUReadReg(0x75);
	if(date!= MPU6050_PRODUCT_ID)
		return FAILED;
	else 
		//MpuGetOffset();
		return SUCCESS;
}
/****************************************************************************************
*@brief    
*@brief   
*@param[in]
*****************************************************************************************/

#define  Gyro_Read() MPUReadBuf( 0X3B,buffer,6)
#define  Acc_Read() MPUReadBuf( 0x43,&buffer[6],6)

void MpuGetData(void) //读取陀螺仪数据加滤波
{
	  uint8_t i;
    int8_t buffer[12];

		Gyro_Read();
	  Acc_Read();

		for(i=0;i<6;i++)
		{
			pMpu[i] = (((int16_t)buffer[i<<1] << 8) | buffer[(i<<1)+1])-MpuOffset[i];		
			if(i < 3)
			{
				{
					//static struct _1_ekf_filter ekf[3] = {{0.02,0,0,0,0.001,0.543},{0.02,0,0,0,0.001,0.543},{0.02,0,0,0,0.001,0.543}};	
					//kalman_1(&ekf[i],(float)pMpu[i]);  //一维卡尔曼
					//pMpu[i] = (int16_t)ekf[i].out;
				}
		}
		if(i > 2)
		{	
			uint8_t k=i-3;
			const float factor = 0.15f;  //滤波因素			
			static float tBuff[3];		

			pMpu[i] = tBuff[k] = tBuff[k] * (1 - factor) + pMpu[i] * factor;                
		}
	}
}

/**************************************END OF FILE*************************************/