#include "stm32f10x.h"                  // Device header
#include "MPU6050_Reg.h"
#include "MPU6050.h"
#include "kalman.h"
#include "alldata.h"


#define MPU6050_ADDRESS		0xD0


//成功返回0 其他失败
uint8_t MPU6050_WaitEvent(I2C_TypeDef* I2Cx, uint32_t I2C_EVENT)
{
	uint32_t Timeout;
	Timeout = 10000;
	while (I2C_CheckEvent(I2Cx, I2C_EVENT) != SUCCESS)
	{
		Timeout --;
		if (Timeout == 0)
		{
			return 1;
		}
	}
	return 0;
}

void MPU6050_WriteReg(uint8_t RegAddress, uint8_t Data)
{
	I2C_GenerateSTART(I2C1, ENABLE);
	MPU6050_WaitEvent(I2C1, I2C_EVENT_MASTER_MODE_SELECT);
	
	I2C_Send7bitAddress(I2C1, MPU6050_ADDRESS, I2C_Direction_Transmitter);
	MPU6050_WaitEvent(I2C1, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED);
	
	I2C_SendData(I2C1, RegAddress);
	MPU6050_WaitEvent(I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTING);
	
	I2C_SendData(I2C1, Data);
	MPU6050_WaitEvent(I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED);
	
	I2C_GenerateSTOP(I2C1, ENABLE);
}

uint8_t MPU6050_ReadReg(uint8_t RegAddress,uint8_t *Data)
{
	I2C_GenerateSTART(I2C1, ENABLE);
	if(MPU6050_WaitEvent(I2C1, I2C_EVENT_MASTER_MODE_SELECT) != 0) return 1;
	
	I2C_Send7bitAddress(I2C1, MPU6050_ADDRESS, I2C_Direction_Transmitter);
	if(MPU6050_WaitEvent(I2C1, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED)!= 0)  return 1;
	
	I2C_SendData(I2C1, RegAddress);
	if(MPU6050_WaitEvent(I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED)!= 0)  return 1;
	
	I2C_GenerateSTART(I2C1, ENABLE);
	if(MPU6050_WaitEvent(I2C1, I2C_EVENT_MASTER_MODE_SELECT)!= 0)    return 1;
	
	I2C_Send7bitAddress(I2C1, MPU6050_ADDRESS, I2C_Direction_Receiver);
	if(MPU6050_WaitEvent(I2C1, I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED)!= 0)   return 1;
	
	I2C_AcknowledgeConfig(I2C1, DISABLE);
	I2C_GenerateSTOP(I2C1, ENABLE);
	
	if(MPU6050_WaitEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED)!= 0)   return 1;
	*Data = I2C_ReceiveData(I2C1);
	
	I2C_AcknowledgeConfig(I2C1,ENABLE);
	return 0;
}

void MPU6050_Init(void)
{
	GPIO_InitTypeDef GPIO_InitStructure;
	I2C_InitTypeDef I2C_InitStructure;
	
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);//GPIOB
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE);
	
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7; 
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOB, &GPIO_InitStructure);
	
	I2C_DeInit(I2C1);
	I2C_InitStructure.I2C_Mode = I2C_Mode_I2C;
	I2C_InitStructure.I2C_DutyCycle = I2C_DutyCycle_2;
	I2C_InitStructure.I2C_OwnAddress1 = 0;     //I2C_OAR1
	I2C_InitStructure.I2C_Ack = I2C_Ack_Enable;
	I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
	I2C_InitStructure.I2C_ClockSpeed = 400000;    //400KHz
	I2C_Init(I2C1, &I2C_InitStructure);
	I2C_Cmd(I2C1, ENABLE);
	
	MPU6050_WriteReg(MPU6050_PWR_MGMT_1, 0x01);
	MPU6050_WriteReg(MPU6050_PWR_MGMT_2, 0x00);
	MPU6050_WriteReg(MPU6050_SMPLRT_DIV, 0x09);
	MPU6050_WriteReg(MPU6050_CONFIG, 0x06);
	MPU6050_WriteReg(MPU6050_GYRO_CONFIG, 0x18);
	MPU6050_WriteReg(MPU6050_ACCEL_CONFIG, 0x18);
}

uint8_t MPU6050_GetID(void)
{
	uint8_t data;
	MPU6050_ReadReg(MPU6050_WHO_AM_I,&data);
	return data;
}


uint8_t buffer[12];

uint8_t MPU6050_GetData()
{
	for(u8 i=0;i<6;i++)
	{
		if(MPU6050_ReadReg(MPU6050_ACCEL_XOUT_H + i,&buffer[i]) !=0)  return 1;
	}
	for(u8 i=0;i<6;i++)
	{
		if(MPU6050_ReadReg(MPU6050_GYRO_XOUT_H + i,&buffer[6+i]) !=0) return 1;;
	}
	return 0;
}


_st_Mpu MPU6050;   //MPU6050原始数据
static volatile int16_t *pMpu = (int16_t *)&MPU6050;
int16_t MpuOffset[6] = {0};		//MPU6050补偿数值

/* 读取MPU6050数据并加滤波 */
//返回值:0,正常
//其他,错误代码


uint8_t MpuGetData(void)
{
	uint8_t i;
	int res;
	
	if( MPU6050_GetData() != 0) return 1;
	
	for(i=0;i<6;i++)
	{
		pMpu[i] = (((int16_t)buffer[i<<1] << 8) | buffer[(i<<1)+1])-MpuOffset[i];							/* 将数据整为16bit，并减去水平校准值 */
		
		
		if(i < 3)		/* 角加速度卡尔曼滤波 */
		{
			{
				static struct KalmanFilter 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}};	
				kalmanfiter(&EKF[i],(float)pMpu[i]);  
				pMpu[i] = (int16_t)EKF[i].Out;
//				printf("EKF:%f\r\n",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;                
		}
	}
	return 0;
}