#include "hal.h"
#include "mpu.h"


//#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

int i2c0Lock = 0;


#define  Gyro_Read() MPUReadBuf(MPU6050_ADDRESS,0x3b,buffer,6)
#define  Acc_Read() MPUReadBuf(MPU6050_ADDRESS, 0x43,&buffer[6],6)
#define  Mag_Read() MPUReadBuf(AK8963_I2C_ADDR,0x03,&buffer[12],6)

void delay_ms(int x){
	
	for(x = 0; x < 1000; x++){
			int z = 0;
			for (z = 0; z < 1020;){
				z  ++ ;
			}
		}
}
void I2C0_LCK (){
		if (i2c0Lock == 0){
			i2c0Lock = 1;
		}else{
			while(i2c0Lock == 0){
				i2c0Lock = 1;
			}
		}
}

void I2C0_UNLOCK(){
		if (i2c0Lock == 1){
			i2c0Lock = 0;
		}else{
			while(i2c0Lock == 1){
				i2c0Lock = 0;
			}
		}
}

int16_t MpuOffset[6] = {0};
_st_AngE Angle;
_st_Mpu MPU6050;   //MPU6050原始数据
_st_Mpu MPU6050UnFiltered;   //MPU6050原始数据

_st_Mpu_Angle MPU6050Angle;
static volatile int16_t *pMpu = (int16_t *)&MPU6050;
static volatile int16_t *pMpuUnFilter = (int16_t *)&MPU6050UnFiltered;



/****************************************************************************************
*@brief  
*@brief   
*@param[in]
*****************************************************************************************/
int8_t mpu6050_rest(void)
{
	if(MPUWriteReg(MPU6050_ADDRESS,PWR_MGMT_1, 0x80) == FAILED)
		return FAILED;	//复位
		//delay_ms(20);
	return SUCCESS;
}
/****************************************************************************************
*@brief   
*@brief  
*@param[in]
*****************************************************************************************/
int8_t MpuInit(void) //初始�?
{
	uint8_t date = SUCCESS;
	
	
		date = MPUWriteReg(MPU6050_ADDRESS, PWR_MGMT_1, 0x00);	// reset
		delay_ms(30);
		date += MPUWriteReg(MPU6050_ADDRESS,SMPLRT_DIV, 0x02); //sample rate
		date += MPUWriteReg( MPU6050_ADDRESS,PWR_MGMT_1, 0x03);	// clock source
		date += MPUWriteReg( MPU6050_ADDRESS,CONFIGL, 0x03);   // lpf

		date += MPUWriteReg( MPU6050_ADDRESS,GYRO_CONFIG, 0x18);//+-2000deg/s
		date += MPUWriteReg( MPU6050_ADDRESS,ACCEL_CONFIG, 0x09);//+-4G
		delay_ms(30);
		delay_ms(30);
    MPUWriteReg(MPU6050_ADDRESS,USER_CTRL,0x00); // 
		delay_ms(30);
		delay_ms(30);

		date += MPUWriteReg( MPU6050_ADDRESS,INT_PIN_CFG ,0X02);  //bypass mode
		delay_ms(30);
		delay_ms(30);
		delay_ms(30);
	  //MPUWriteReg(AK8963_I2C_ADDR,AK8963_CNTL1, 0x11);
		delay_ms(30);
		delay_ms(30);

		delay_ms(30);
		delay_ms(30);

	date += MPUWriteReg( AK8963_I2C_ADDR,0x0A,0x01);  //one-time messure

	date = MPUReadReg(MPU6050_ADDRESS,WHO_AM_I);
	//if(date!= MPU9250_PRODUCT_ID)
		//return FAILED;
	char us = MPUReadReg( AK8963_I2C_ADDR,0);//+-2000deg/s
	if (us != 0x48){
		return FAIL;
	}
}

void MpuAngle(){
	MPU6050Angle.accX = MPU6050.accX *360/32768;
	MPU6050Angle.accY = MPU6050.accY *360/32768;
	MPU6050Angle.accZ = MPU6050.accZ *360/32768;

	MPU6050Angle.gyroX = MPU6050.gyroX*3.1415926/180;
	MPU6050Angle.gyroY = MPU6050.gyroY*3.1415926/180;
	MPU6050Angle.gyroZ = MPU6050.gyroZ*3.1415926/180;

}

void MpuMag(){
	
}
/****************************************************************************************
*@brief    
*@brief   
*@param[in]
*****************************************************************************************/

void kalman_1(struct _1_ekf_filter *ekf,float input)  //一维卡尔曼
{
	ekf->Now_P = ekf->LastP + ekf->Q;
	ekf->Kg = ekf->Now_P / (ekf->Now_P + ekf->R);
	ekf->out = ekf->out + ekf->Kg * (input - ekf->out);
	ekf->LastP = (1-ekf->Kg) * ekf->Now_P ;
}


void MpuGetData(void) //读取陀螺仪数据加滤�?
{
	  uint8_t i;
    int8_t buffer[18];
	
		MPUWriteReg( AK8963_I2C_ADDR,0x0A,0x01);
		//Mag_Read();

		Gyro_Read();
	  Acc_Read();	
		MpuAngle();
		for(i = 0; i < 9;i++)
		{
			pMpu[i] = (((int16_t)buffer[2*i] *256)| buffer[2*i + 1]&0xff)-MpuOffset[i];		
			pMpuUnFilter[i] = (((int16_t)buffer[2*i] *256)| buffer[2*i + 1]&0xff)-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*************************************/