/*
 * bsp_IMU.c
 *
 *  Created on: Oct 26, 2023
 *      Author: shiya
 */
#include "BSP/bsp_IMU.h"
#include "BSP/bsp_include.h"

#define NNN 4
#define NNN_1 4
float Factor_G= 1;
//#define MPU_ACCEL_OFFS_REG		0X06	//accel_offs寄存器,可读取版本号,寄存器手册未提到
//#define MPU_PROD_ID_REG			0X0C	//prod id寄存器,在寄存器手册未提到
#define MPU_SELF_TESTX_REG		0X0D	//自检寄存器X
#define MPU_SELF_TESTY_REG		0X0E	//自检寄存器Y
#define MPU_SELF_TESTZ_REG		0X0F	//自检寄存器Z
#define MPU_SELF_TESTA_REG		0X10	//自检寄存器A
#define MPU_SAMPLE_RATE_REG		0X19	//采样频率分频器
#define MPU_CFG_REG				0X1A	//配置寄存器
#define MPU_GYRO_CFG_REG		0X1B	//陀螺仪配置寄存器
#define MPU_ACCEL_CFG_REG		0X1C	//加速度计配置寄存器
#define MPU_MOTION_DET_REG		0X1F	//运动检测阀值设置寄存器
#define MPU_FIFO_EN_REG			0X23	//FIFO使能寄存器
#define MPU_I2CMST_CTRL_REG		0X24	//IIC主机控制寄存器
#define MPU_I2CSLV0_ADDR_REG	0X25	//IIC从机0器件地址寄存器
#define MPU_I2CSLV0_REG			0X26	//IIC从机0数据地址寄存器
#define MPU_I2CSLV0_CTRL_REG	0X27	//IIC从机0控制寄存器
#define MPU_I2CSLV1_ADDR_REG	0X28	//IIC从机1器件地址寄存器
#define MPU_I2CSLV1_REG			0X29	//IIC从机1数据地址寄存器
#define MPU_I2CSLV1_CTRL_REG	0X2A	//IIC从机1控制寄存器
#define MPU_I2CSLV2_ADDR_REG	0X2B	//IIC从机2器件地址寄存器
#define MPU_I2CSLV2_REG			0X2C	//IIC从机2数据地址寄存器
#define MPU_I2CSLV2_CTRL_REG	0X2D	//IIC从机2控制寄存器
#define MPU_I2CSLV3_ADDR_REG	0X2E	//IIC从机3器件地址寄存器
#define MPU_I2CSLV3_REG			0X2F	//IIC从机3数据地址寄存器
#define MPU_I2CSLV3_CTRL_REG	0X30	//IIC从机3控制寄存器
#define MPU_I2CSLV4_ADDR_REG	0X31	//IIC从机4器件地址寄存器
#define MPU_I2CSLV4_REG			0X32	//IIC从机4数据地址寄存器
#define MPU_I2CSLV4_DO_REG		0X33	//IIC从机4写数据寄存器
#define MPU_I2CSLV4_CTRL_REG	0X34	//IIC从机4控制寄存器
#define MPU_I2CSLV4_DI_REG		0X35	//IIC从机4读数据寄存器

#define MPU_I2CMST_STA_REG		0X36	//IIC主机状态寄存器
#define MPU_INTBP_CFG_REG		0X37	//中断/旁路设置寄存器
#define MPU_INT_EN_REG			0X38	//中断使能寄存器
#define MPU_INT_STA_REG			0X3A	//中断状态寄存器

#define MPU_ACCEL_XOUTH_REG		0X3B	//加速度值,X轴高8位寄存器
#define MPU_ACCEL_XOUTL_REG		0X3C	//加速度值,X轴低8位寄存器
#define MPU_ACCEL_YOUTH_REG		0X3D	//加速度值,Y轴高8位寄存器
#define MPU_ACCEL_YOUTL_REG		0X3E	//加速度值,Y轴低8位寄存器
#define MPU_ACCEL_ZOUTH_REG		0X3F	//加速度值,Z轴高8位寄存器
#define MPU_ACCEL_ZOUTL_REG		0X40	//加速度值,Z轴低8位寄存器

#define MPU_TEMP_OUTH_REG		0X41	//温度值高八位寄存器
#define MPU_TEMP_OUTL_REG		0X42	//温度值低8位寄存器

#define MPU_GYRO_XOUTH_REG		0X43	//陀螺仪值,X轴高8位寄存器
#define MPU_GYRO_XOUTL_REG		0X44	//陀螺仪值,X轴低8位寄存器
#define MPU_GYRO_YOUTH_REG		0X45	//陀螺仪值,Y轴高8位寄存器
#define MPU_GYRO_YOUTL_REG		0X46	//陀螺仪值,Y轴低8位寄存器
#define MPU_GYRO_ZOUTH_REG		0X47	//陀螺仪值,Z轴高8位寄存器
#define MPU_GYRO_ZOUTL_REG		0X48	//陀螺仪值,Z轴低8位寄存器

#define MPU_I2CSLV0_DO_REG		0X63	//IIC从机0数据寄存器
#define MPU_I2CSLV1_DO_REG		0X64	//IIC从机1数据寄存器
#define MPU_I2CSLV2_DO_REG		0X65	//IIC从机2数据寄存器
#define MPU_I2CSLV3_DO_REG		0X66	//IIC从机3数据寄存器

#define MPU_I2CMST_DELAY_REG	0X67	//IIC主机延时管理寄存器
#define MPU_SIGPATH_RST_REG	0X68	//信号通道复位寄存器
#define MPU_MDETECT_CTRL_REG	0X69	//运动检测控制寄存器
#define MPU_USER_CTRL_REG		0X6A	//用户控制寄存器
#define MPU_PWR_MGMT1_REG		0X6B	//电源管理寄存器1
#define MPU_PWR_MGMT2_REG		0X6C	//电源管理寄存器2
#define MPU_FIFO_CNTH_REG		0X72	//FIFO计数寄存器高八位
#define MPU_FIFO_CNTL_REG		0X73	//FIFO计数寄存器低八位
#define MPU_FIFO_RW_REG			0X74	//FIFO读写寄存器
#define MPU_DEVICE_ID_REG		0X75	//器件ID寄存器
 
//如果AD0脚(9脚)接地,IIC地址为0X68(不包含最低位).
//如果接V3.3,则IIC地址为0X69(不包含最低位).
#define MPU_ADDR				0X69
#define MPU_WHO_AM_I_ADDR		0X12

////因为模块AD0默认接GND,所以转为读写地址后,为0XD1和0XD0(如果接VCC,则为0XD3和0XD2)
//#define MPU_READ    0XD1
//#define MPU_WRITE   0XD0

SP_BSP_MPU6050 *MPU_Angle;



typedef uint8_t u8;
typedef uint16_t u16;

float Kalman_filter_pitch(float newAngle, float newRate, float dt);

float Kalman_BM_pitch(float newAngle_pitch, float newRate_pitch, float dt, float  Tol_Acc);

float Kalman_BM_roll(float newAngle_roll, float newRate_roll, float dt, float  Tol_Acc);

float FirstOrder_pitch(float newAngle, float newRate, float dt);
float SecondOrder_pitch(float newAngle, float newRate, float dt);

float Kalman_filter_roll(float newAngle, float newRate, float dt);

float FirstOrder_roll(float newAngle, float newRate, float dt);
float SecondOrder_roll(float newAngle, float newRate, float dt);


/* Kalman filter variables */
float Q_angle_pitch = 0.001; // Process noise variance for the accelerometer
float Q_bias_pitch = 0.003; // Process noise variance for the gyro bias
float R_measure_pitch = 0.03; // Measurement noise variance - this is actually the variance of the measurement noise

float angle_pitch = 0.0; // The angle calculated by the Kalman filter - part of the 2x1 state vector
float angle_roll = 0.0;
float bias_pitch = 0.0; // The gyro bias calculated by the Kalman filter - part of the 2x1 state vector
float rate_pitch; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate

float P_pitch[2][2]={{0.0,0.0},{0.0,0.0}}; // Error covariance matrix - This is a 2x2 matrix
///////////////////////////////////////////////////////////////////////////////////
float K1_pitch =0.1; 	// 对加速度计取值的权重
float FirstOrder_angle_pitch;

float K2_pitch =0.2; // 对加速度计取值的权重
float kx1_pitch,kx2_pitch,ky1_pitch;
float SecondOrder_angle_pitch;


float FirstOrder_angle_roll;
float SecondOrder_angle_roll;
								//初始化MPU6050
u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf);//IIC连续写
u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf); //IIC连续读
u8 MPU_Write_Byte(u8 devaddr,u8 reg,u8 data);
u8 MPU_Read_Byte(u8 devaddr,u8 reg);

u8 MPU_Set_Gyro_Fsr(u8 fsr);
u8 MPU_Set_Accel_Fsr(u8 fsr);
u8 MPU_Set_LPF(u16 lpf);
u8 MPU_Set_Rate(u16 rate);
u8 MPU_Set_Fifo(u8 sens);

short MPU_Get_Temperature(void);
u8 MPU_Get_Gyroscope(short *gx,short *gy,short *gz);
u8 MPU_Get_Accelerometer(short *ax,short *ay,short *az);
void Mean_filtering(double G_XX, double G_YY, double G_ZZ,double A_XX, double A_YY, double A_ZZ,double *XYZ_Val);
void Mean_filtering_1(double G_XX_1, double G_YY_1, double G_ZZ_1,double A_XX_1, double A_YY_1, double A_ZZ_1,double *XYZ_Val_1);
void Mean_filter_Angle(double G_XX_1, int Conversion_Flag,double *XY_Angle) ;//double G_YY_1,
void GetAngle();
void GetPitch();
//void Judging_Symbols(double x,double y,double z,int *Symbol_Val);



// The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds
float Kalman_filter_pitch(float newAngle_pitch, float newRate_pitch, float dt) {
	//Pitch_Kalman = Kalman_filter(Pitch, GyroX, dt);
    // KasBot V2  -  Kalman filter module - http://www.x-firm.com/?page_id=145
    // Modified by Kristian Lauszus
    // See my blog post for more information: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it

    // Discrete Kalman filter time update equations - Time Update ("Predict")
    // Update xhat - Project the state ahead
    /* Step 1 */
    rate_pitch = newRate_pitch - bias_pitch;
    angle_pitch += dt * rate_pitch;

    // Update estimation error covariance - Project the error covariance ahead
    /* Step 2 */
    P_pitch[0][0] += dt * (dt*P_pitch[1][1] - P_pitch[0][1] - P_pitch[1][0] + Q_angle_pitch);
    P_pitch[0][1] -= dt * P_pitch[1][1];
    P_pitch[1][0] -= dt * P_pitch[1][1];
    P_pitch[1][1] += Q_bias_pitch * dt;

    // Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
    // Calculate Kalman gain - Compute the Kalman gain
    /* Step 4 */
    float S_pitch = P_pitch[0][0] + R_measure_pitch; // Estimate error
    /* Step 5 */
    float K_pitch[2]; // Kalman gain - This is a 2x1 vector
    K_pitch[0] = P_pitch[0][0] / S_pitch;
    K_pitch[1] = P_pitch[1][0] / S_pitch;

    // Calculate angle and bias - Update estimate with measurement zk (newAngle)
    /* Step 3 */
    float y_pitch = newAngle_pitch - angle_pitch; // Angle difference
    /* Step 6 */
    angle_pitch += K_pitch[0] * y_pitch;
    bias_pitch += K_pitch[1] * y_pitch;

    // Calculate estimation error covariance - Update the error covariance
    /* Step 7 */
    float P00_temp_pitch = P_pitch[0][0];
    float P01_temp_pitch = P_pitch[0][1];

    P_pitch[0][0] -= K_pitch[0] * P00_temp_pitch;
    P_pitch[0][1] -= K_pitch[0] * P01_temp_pitch;
    P_pitch[1][0] -= K_pitch[1] * P00_temp_pitch;
    P_pitch[1][1] -= K_pitch[1] * P01_temp_pitch;
    return angle_pitch;
};
float angle_pitch_BM = 0.0;
float bias_pitch_BM  =0.0;
float Q_angle_pitch_BM = 0.001; // Process noise variance for the accelerometer
float PP_pitch_BM[2][2]={{1.0,0.0},{0.0,1.0}}; // Error covariance matrix - This is a 2x2 matrix
float R_angle_Pit_mea_BM= 0.1;
float Pitch_kal_BM = 0.0;

//【算法】基于STM32的MPU6050卡尔曼滤波算法（入门级）

//在该算法中对角度噪声进行处理，根据加速度的值，重新调整角度计算权值
float Kalman_BM_pitch(float newAngle_pitch, float newRate_pitch, float dt, float Tol_Acc) {

	Pitch_kal_BM += dt * (newRate_pitch-bias_pitch_BM);  //过程计算值
    //设置权重
//    if((Tol_Acc>1.001)&&(Tol_Acc<0.999))
//    {
//    	Factor_G=1;
//    }
//    else
//    {
//    	//Factor_G= 1/(fabs((Tol_Acc-1)+0.02))     +0.1 ;
//    	Factor_G= 20 ;
//    }
  //  Factor_G=1;
    //预测协方差矩阵
	PP_pitch_BM[0][0] = Factor_G*(PP_pitch_BM[0][0]  - (PP_pitch_BM[0][1] + PP_pitch_BM[1][0]-PP_pitch_BM[1][1]*dt)*dt)+ Q_angle_pitch_BM;
	PP_pitch_BM[0][1] = Factor_G*(PP_pitch_BM[0][1] - PP_pitch_BM[1][1]*dt);
	PP_pitch_BM[1][0] = Factor_G*(PP_pitch_BM[1][0] - PP_pitch_BM[1][1]*dt);
	PP_pitch_BM[1][1] = Factor_G*(PP_pitch_BM[1][1] )+Q_bias_pitch;

	float K_deno_Pitch=PP_pitch_BM[0][0]+R_angle_Pit_mea_BM;

	//卡尔曼增益计算0
    float K_pitch_BM[2]; // Kalman gain - This is a 2x1 vector
    K_pitch_BM[0] = PP_pitch_BM[0][0] / K_deno_Pitch;
    K_pitch_BM[1] = PP_pitch_BM[1][0] / K_deno_Pitch;

    bias_pitch_BM= bias_pitch_BM+ K_pitch_BM[1] * (newAngle_pitch - Pitch_kal_BM);
    Pitch_kal_BM = Pitch_kal_BM + K_pitch_BM[0] * (newAngle_pitch - Pitch_kal_BM);
//    float PP_pitch_BM
    float PP_pitch_BM_L[2][2];
    PP_pitch_BM_L[0][0]=PP_pitch_BM[0][0];PP_pitch_BM_L[0][1]=PP_pitch_BM[0][1];PP_pitch_BM_L[1][0]=PP_pitch_BM[1][0];PP_pitch_BM_L[1][1]=PP_pitch_BM[1][1];

    PP_pitch_BM[0][0] = PP_pitch_BM_L[0][0] -K_pitch_BM[0] * PP_pitch_BM_L[0][0];
    PP_pitch_BM[0][1] = PP_pitch_BM_L[0][1] -K_pitch_BM[0] * PP_pitch_BM_L[0][1];
    PP_pitch_BM[1][0] = PP_pitch_BM_L[1][0] -K_pitch_BM[1] * PP_pitch_BM_L[0][0];
    PP_pitch_BM[1][1] = PP_pitch_BM_L[1][1] -K_pitch_BM[1] * PP_pitch_BM_L[0][1];
    return  Pitch_kal_BM;
}

float PP_roll_BM[2][2]={{1.0,0.0},{0.0,1.0}};
float bias_roll_BM  =0.0;
float Q_bias_roll = 0.003; // Process noise variance for the gyro bias
float Q_angle_roll_BM = 0.001; // Process noise variance for the accelerometer
float Roll_kal_BM = 0.0;
float R_angle_Roll_mea_BM= 0.1;
float Factor_G;

float Kalman_BM_roll(float newAngle_roll, float newRate_roll, float dt, float Tol_Acc) {

	Roll_kal_BM += dt * (newRate_roll-bias_roll_BM);  //过程计算值
    //设置权重
  //  float Factor_G= 1/fabs((Tol_Acc-1+0.01))     +0.1 ;

    if((Tol_Acc>1.001)&&(Tol_Acc<0.999))
    {
    	Factor_G=1;
    }
    else
    {
    	//Factor_G= 1/(fabs((Tol_Acc-1)+0.02))     +0.1 ;
    	Factor_G= 20 ;
    }
  //  Factor_G=1;

    //预测协方差矩阵
	PP_roll_BM[0][0] = Factor_G*(PP_roll_BM[0][0]  - (PP_roll_BM[0][1] + PP_roll_BM[1][0]-PP_roll_BM[1][1]*dt)*dt)+ Q_angle_roll_BM;
	PP_roll_BM[0][1] = Factor_G*(PP_roll_BM[0][1] - PP_roll_BM[1][1]*dt);
	PP_roll_BM[1][0] = Factor_G*(PP_roll_BM[1][0] - PP_roll_BM[1][1]*dt);
	PP_roll_BM[1][1] = Factor_G*(PP_roll_BM[1][1] )+Q_bias_roll;

	float K_deno_Roll=PP_roll_BM[0][0]+R_angle_Roll_mea_BM;

	//卡尔曼增益计算0
    float K_roll_BM[2]; // Kalman gain - This is a 2x1 vector
    K_roll_BM[0] = PP_roll_BM[0][0] / K_deno_Roll;
    K_roll_BM[1] = PP_roll_BM[1][0] / K_deno_Roll;

    bias_roll_BM= bias_roll_BM+ K_roll_BM[1] * (newAngle_roll - Roll_kal_BM);
    Roll_kal_BM = Roll_kal_BM + K_roll_BM[0] * (newAngle_roll - Roll_kal_BM);
//    float PP_roll_BM
    float PP_roll_BM_L[2][2];
    PP_roll_BM_L[0][0]=PP_roll_BM[0][0];PP_roll_BM_L[0][1]=PP_roll_BM[0][1];PP_roll_BM_L[1][0]=PP_roll_BM[1][0];PP_roll_BM_L[1][1]=PP_roll_BM[1][1];

    PP_roll_BM[0][0] = PP_roll_BM_L[0][0] -K_roll_BM[0] * PP_roll_BM_L[0][0];
    PP_roll_BM[0][1] = PP_roll_BM_L[0][1] -K_roll_BM[0] * PP_roll_BM_L[0][1];
    PP_roll_BM[1][0] = PP_roll_BM_L[1][0] -K_roll_BM[1] * PP_roll_BM_L[0][0];
    PP_roll_BM[1][1] = PP_roll_BM_L[1][1] -K_roll_BM[1] * PP_roll_BM_L[0][1];
    return  Roll_kal_BM;
}

uint8_t GF_MSP_IMU_6050_Init(SP_BSP_MPU6050 *_IMU6050_Angle)
{
	MPU_Angle = _IMU6050_Angle;
	return 1;
}

float FirstOrder_pitch(float newAngle_pitch, float newRate_pitch, float dt)//采集后计算的角度和角加速度
{
  FirstOrder_angle_pitch = K1_pitch * newAngle_pitch + (1-K1_pitch) * (angle_pitch + newRate_pitch * dt);
  return FirstOrder_angle_pitch;
}

float SecondOrder_pitch(float newAngle_pitch, float newRate_pitch, float dt)//采集后计算的角度和角加速度
{
	kx1_pitch=(newAngle_pitch-SecondOrder_angle_pitch)*(1-K2_pitch)*(1-K2_pitch);
	ky1_pitch=ky1_pitch+kx1_pitch*dt;
	kx2_pitch=ky1_pitch+2*(1-K2_pitch)*(newAngle_pitch-SecondOrder_angle_pitch)+newRate_pitch;
	SecondOrder_angle_pitch=SecondOrder_angle_pitch+ kx2_pitch*dt;
	return SecondOrder_angle_pitch;
}

float Pitch_A,Roll_A,Pitch_A_1,Roll_A_1,Yaw_A,Pitch_A_2,Roll_A_2;					//滤波前

float ROLL_END,ROLL_END_1,PITCH_END,PITCH_END_1;
//float ROll_KAL_END,	PITCH_KAL_END;


float Pitch_Kalman,Pitch_first,Pitch_second;
float Roll_Kalman,Roll_first,Roll_second;

u8 bufG[6],bufA[6];			//角速度、加速度数据缓冲区
int16_t GyroX,GyroY,GyroZ;				//角速度

float GyroX_D,GyroY_D,GyroZ_D;				//角速度

int16_t AccX,AccY,AccZ;		//加速度
double AccX_D,AccY_D,AccZ_D;		//加速度
uint32_t tem0,tem1;			//存放系统时间，us

//获取系统时间，用于卡尔曼滤波
__STATIC_INLINE uint32_t GXT_SYSTICK_IsActiveCounterFlag(void)
{
  return ((SysTick->CTRL & SysTick_CTRL_COUNTFLAG_Msk) == (SysTick_CTRL_COUNTFLAG_Msk));
}
static uint32_t getCurrentMicros(void)
{
  /* Ensure COUNTFLAG is reset by reading SysTick control and status register */
  GXT_SYSTICK_IsActiveCounterFlag();
  uint32_t m = HAL_GetTick();
  const uint32_t tms = SysTick->LOAD + 1;
  __IO uint32_t u = tms - SysTick->VAL;
  if (GXT_SYSTICK_IsActiveCounterFlag()) {
    m = HAL_GetTick();
    u = tms - SysTick->VAL;
  }
  return (m * 1000 + (u * 1000) / tms);
}
//获取系统时间，单位us
uint32_t micros(void)
{
  return getCurrentMicros();
}

//初始化MPU6050
//返回值:1,成功
//    其他,错误代码
uint8_t GF_BSP_MPU_Init(void)
{
	u8 res;
	MPU_Write_Byte(MPU_ADDR, MPU_PWR_MGMT1_REG, 0X80); //复位MPU6050
	HAL_Delay(500);									   //延时500ms
	MPU_Write_Byte(MPU_ADDR, MPU_PWR_MGMT1_REG, 0X00); //唤醒MPU6050
	HAL_Delay(500);

	tem1 = micros();

	MPU_Set_LPF(50);									//设置MPU6050的数字低通滤波器
	MPU_Set_Gyro_Fsr(2);							   //陀螺仪传感器,±1000dps
	MPU_Set_Accel_Fsr(2);							   //加速度传感器,±8g
	MPU_Set_Rate(1000);								   //设置采样率1000Hz
	
	MPU_Write_Byte(MPU_ADDR, MPU_INT_EN_REG, 0X00);	   //关闭所有中断
	MPU_Write_Byte(MPU_ADDR, MPU_USER_CTRL_REG, 0X00); //I2C主模式关闭
	MPU_Write_Byte(MPU_ADDR, MPU_FIFO_EN_REG, 0X00);   //关闭FIFO
	MPU_Write_Byte(MPU_ADDR, MPU_INTBP_CFG_REG, 0X80); //INT引脚低电平有效
	res = MPU_Read_Byte(MPU_ADDR, MPU_DEVICE_ID_REG);
	if (res == MPU_WHO_AM_I_ADDR) //器件ID正确
	{
		MPU_Write_Byte(MPU_ADDR, MPU_PWR_MGMT1_REG, 0X01); //设置CLKSEL,PLL 1：X，2：Y，3：Z轴为参考
		MPU_Write_Byte(MPU_ADDR, MPU_PWR_MGMT2_REG, 0X00); //加速度与陀螺仪都工作
		MPU_Set_Rate(1000);								   //设置采样率为1000Hz
		return 1;
	}
	else
		return 0;
}

//设置MPU6050陀螺仪传感器满量程范围
//fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
//返回值:0,设置成功
//    其他,设置失败
u8 MPU_Set_Gyro_Fsr(u8 fsr)
{
	return MPU_Write_Byte(MPU_ADDR, MPU_GYRO_CFG_REG, fsr << 3); //设置陀螺仪满量程范围
}


//设置MPU6050加速度传感器满量程范围
//fsr:0,±2g;1,±4g;2,±8g;3,±16g
//返回值:0,设置成功
//    其他,设置失败
u8 MPU_Set_Accel_Fsr(u8 fsr)
{
	return MPU_Write_Byte(MPU_ADDR, MPU_ACCEL_CFG_REG, fsr << 3); //设置加速度传感器满量程范围
}


//设置MPU6050的数字低通滤波器
//lpf:数字低通滤波频率(Hz)
//返回值:0,设置成功
//    其他,设置失败
u8 MPU_Set_LPF(u16 lpf)
{
	u8 data = 0;
	if (lpf >= 188) data = 1;
	else if (lpf >= 98) data = 2;
	else if (lpf >= 42) data = 3;
	else if (lpf >= 20) data = 4;
	else if (lpf >= 10) data = 5;
	else data = 6;
	return MPU_Write_Byte(MPU_ADDR, MPU_CFG_REG, data); //设置数字低通滤波器
}
//设置MPU6050的采样率(假定Fs=1KHz)
//rate:4~1000(Hz)
//返回值:0,设置成功
//    其他,设置失败
u8 MPU_Set_Rate(u16 rate)
{
	u8 data;
	if (rate > 1000) rate = 1000;
	if (rate < 4) 	  rate = 4;
	data = 1000 / rate - 1;
	data = MPU_Write_Byte(MPU_ADDR, MPU_SAMPLE_RATE_REG, data); //设置数字低通滤波器
	return MPU_Set_LPF(rate / 2);								//自动设置LPF为采样率的一半
}

//得到温度值
//返回值:温度值(扩大了100倍)
short MPU_Get_Temperature(void)
{
	u8 buf[2];
	short raw;
	float temp;
	MPU_Read_Len(MPU_ADDR, MPU_TEMP_OUTH_REG, 2, buf);
	raw = ((u16)buf[0] << 8) | buf[1];
	temp = 36.53 + ((double)raw) / 340;
	return temp * 100;
}
//得到陀螺仪值(原始值)
//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
//返回值:0,成功
//    其他,错误代码
u8 MPU_Get_Gyroscope(short *gx, short *gy, short *gz)
{
	u8 buf[6], res;
	res = MPU_Read_Len(MPU_ADDR, MPU_GYRO_XOUTH_REG, 6, buf);
	if (res == 0)
	{
		*gx = ((u16)buf[0] << 8) | buf[1];
		*gy = ((u16)buf[2] << 8) | buf[3];
		*gz = ((u16)buf[4] << 8) | buf[5];
	}
	return res;
}
//得到加速度值(原始值)
//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
//返回值:0,成功
//    	其他,错误代码
u8 MPU_Get_Accelerometer(short *ax, short *ay, short *az)
{
	u8 buf[6], res;
	res = MPU_Read_Len(MPU_ADDR, MPU_ACCEL_XOUTH_REG, 6, buf);
	if (res == 0)
	{
		*ax = ((u16)buf[0] << 8) | buf[1];
		*ay = ((u16)buf[2] << 8) | buf[3];
		*az = ((u16)buf[4] << 8) | buf[5];
	}
	return res;
}

//IIC连续写
//addr:器件地址
//reg:寄存器地址
//len:写入长度
//buf:数据区
//返回值:0,正常
//    	其他,错误代码
u8 MPU_Write_Len(u8 addr, u8 reg, u8 len, u8 *buf)
{
	u8 res;
	res = HAL_I2C_Mem_Write(&hi2c4, (addr << 1), reg, I2C_MEMADD_SIZE_8BIT, buf, len, 0xfff);
	return res;
}

//IIC连续读
//addr:器件地址
//reg:要读取的寄存器地址
//len:要读取的长度
//buf:读取到的数据存储区
//返回值:0,正常
//    其他,错误代码
u8 MPU_Read_Len(u8 addr, u8 reg, u8 len, u8 *buf)
{
	u8 res;
	res = HAL_I2C_Mem_Read(&hi2c4, (addr << 1), reg, I2C_MEMADD_SIZE_8BIT, buf, len, 0xfff);
	return res;
}

//IIC写一个字节
//devaddr:器件IIC地址
//reg:寄存器地址
//data:数据
//返回值:0,正常
//    	其他,错误代码
u8 MPU_Write_Byte(u8 addr, u8 reg, u8 data)
{
	u8 res;
	res = HAL_I2C_Mem_Write(&hi2c4, (addr << 1), reg, I2C_MEMADD_SIZE_8BIT, &data, 1, 0xfff);
	return res;
}

//IIC读一个字节
//reg:寄存器地址
//返回值:读到的数据
double Angle_End[2];float dt;
u8 MPU_Read_Byte(u8 addr, u8 reg)
{
	u8 res;
	HAL_I2C_Mem_Read(&hi2c4, (addr << 1), reg, I2C_MEMADD_SIZE_8BIT, &res, 1, 0xfff);
	return res;
}

float Angle_X_G, Angle_Y_G;
double ACC_TOTAL;

double XYZ_ACC[6];double XYZ_ACC_1[6];
void GF_BSP_GetAngle(){

	int GyroX_bias=29; int GyroY_bias=10;int GyroZ_bias=13;
	tem0 = micros();
	MPU_Read_Len(MPU_ADDR, MPU_GYRO_XOUTH_REG, 6, bufG);
	GyroX = (int16_t)(((int16_t)bufG[0] << 8) + bufG[1]);
	GyroY =	(int16_t)(((int16_t)bufG[2] << 8) + bufG[3]);
	GyroZ = (int16_t)(((int16_t)bufG[4] << 8) + bufG[5]);

	GyroX=GyroX-GyroX_bias;
	GyroY=GyroY-GyroY_bias;
	GyroZ=GyroZ-GyroZ_bias;
	GyroX_D=(float)GyroX*0.030487804878049;
	GyroY_D=(float)GyroY*0.030487804878049;
	GyroZ_D=(float)GyroZ*0.030487804878049;
//	GyroX = GyroX / 32.7 - 0 ;//gyroZ_OFF; // Convert to deg/s

	MPU_Read_Len(MPU_ADDR, MPU_ACCEL_XOUTH_REG, 6, bufA);
	AccX = (int16_t)(((int16_t)bufA[0] << 8) | bufA[1]);
	AccY = (int16_t)(((int16_t)bufA[2] << 8) | bufA[3]);
	AccZ = (int16_t)(((int16_t)bufA[4] << 8) | bufA[5]);
	int AccX_Bis=-40; int AccY_Bis=-22; int AccZ_Bis=29;
	AccX=AccX-AccX_Bis;
	AccY=AccY-AccY_Bis;
	AccZ=AccZ-AccZ_Bis;

	AccX_D =1.0*(double)AccX *0.000244140625;
	AccY_D =1.0*(double)AccY *0.000244140625;
	AccZ_D =(double)AccZ *0.000244140625;


	dt = (tem0-tem1)/1000000.0;

	Roll_A_1=atan2(-AccY_D,AccX_D)*180/3.1415926;//正确//正确//正确//正确//正确//正确//正确//正确//壁面安装计算方法，已和瑞芬对比
	ACC_TOTAL=sqrt(AccX_D*AccX_D+AccY_D*AccY_D+AccZ_D*AccZ_D);


	Mean_filtering(GyroX_D,GyroY_D,GyroZ_D,AccX_D, AccY_D, AccZ_D,XYZ_ACC);
//	Mean_filtering_1(GyroX_D,GyroY_D,GyroZ_D,AccX_D, AccY_D, AccZ_D,XYZ_ACC_1);
	double GyroX_D_F,GyroY_D_F,GyroZ_D_F,AccX_D_F,AccY_D_F,AccZ_D_F;				//角速度
	GyroX_D_F=XYZ_ACC[0];
	GyroY_D_F=XYZ_ACC[1];
	GyroZ_D_F=XYZ_ACC[2];

	AccX_D_F=XYZ_ACC[3];
	AccY_D_F=XYZ_ACC[4];
	AccZ_D_F=XYZ_ACC[5];

//	基于加速度计算欧拉角

	Roll_A_2=atan2(-AccY_D_F,AccX_D_F)*180/3.1415926;//正确//正确//正确//正确//正确//正确//正确//正确//壁面安装计算方法，已和瑞芬对比
	Pitch_A_2=-atan(AccZ_D_F/sqrt(AccY_D_F*AccY_D_F+AccX_D_F*AccX_D_F))*180/3.14159;//正确//正确//正确//正确//正确//壁面放置计算方法
	//基于陀螺仪计算欧拉角



	Kalman_BM_roll(Roll_A_2, GyroZ_D_F, dt, ACC_TOTAL);//修正后
	//Kalman_BM_pitch(Pitch_A_2, GyroY_D, dt, ACC_TOTAL);//修正后

	Mean_filter_Angle(Roll_kal_BM, 0,Angle_End);//,Pitch_kal_BM*100
	Angle_End[0]=Angle_End[0]-1*0;
	if(Angle_End[0]>180)
	{
		Angle_End[0]=Angle_End[0]-360;
	}
	else if(Angle_End[0]<=-180)
	{
		Angle_End[0]=Angle_End[0]+360;
	}

	Angle_End[0]=Angle_End[0]*100;
	//MPU_Angle->MPU_Roll=(int32_t)(fabs(Roll_kal_BM*100));


	MPU_Angle->MPU_Pitch=(int32_t)(Angle_End[0]);

	//MPU_Angle->MPU_Pitch=(int32_t)(fabs(Pitch_kal_BM*100));
	tem1= tem0;
}

// 自行处理预留端口
//void GF_BSP_GetAngle(){
//	tem0 = micros();
//	MPU_Read_Len(MPU_ADDR, MPU_GYRO_XOUTH_REG, 6, bufG);
//	GyroX = (int16_t)(((int16_t)bufG[0] << 8) + bufG[1]);
//	GyroY =	(int16_t)(((int16_t)bufG[2] << 8) + bufG[3]);
//	GyroZ = (int16_t)(((int16_t)bufG[4] << 8) + bufG[5]);
//
//
//	GyroX_D=(double)GyroX*0.030487804878049;
//	GyroY_D=(double)GyroY*0.030487804878049;
//	GyroZ_D=(double)GyroZ*0.030487804878049;
////	GyroX = GyroX / 32.7 - 0 ;//gyroZ_OFF; // Convert to deg/s
//
//	MPU_Read_Len(MPU_ADDR, MPU_ACCEL_XOUTH_REG, 6, bufA);
//	AccX = (int16_t)(((int16_t)bufA[0] << 8) | bufA[1]);
//	AccY = (int16_t)(((int16_t)bufA[2] << 8) | bufA[3]);
//	AccZ = (int16_t)(((int16_t)bufA[4] << 8) | bufA[5]);
//
//	AccX_D =(double)AccX *0.000244140625;
//	AccY_D =(double)AccY *0.000244140625;
//	AccZ_D =(double)AccZ *0.000244140625;
//	int SYM_XYZ[1];
//	Judging_Symbols(AccZ_D,SYM_XYZ);
//	Pitch_A=atan2(AccX_D,sqrt(AccY_D*AccY_D+AccZ_D*AccZ_D)*SYM_XYZ[0])*180/3.14159;
//	Roll_A=atan2(AccY_D,sqrt(AccX_D*AccX_D+AccZ_D*AccZ_D)*SYM_XYZ[0])*180/3.14159;
//	float dt = (tem0-tem1)/1000000.0;
//	Pitch_Kalman = Kalman_filter(Pitch, GyroX, dt);
//	Pitch_first = FirstOrder(Pitch, GyroX, dt);
//	Pitch_second = SecondOrder(Pitch, GyroX, dt);
//
//	tem1= tem0;
//}

void GetPitch(){
//	pitch = &Pitch;
//	pitch_kalman = &Pitch_Kalman;
//	pitch_first = &Pitch_first;
//	pitch_second = &Pitch_second;

	//printf("%f,%f,%f,%f\r\n",Pitch,Pitch_Kalman,Pitch_first,Pitch_second);
}
void Judging_Symbols(double x,double y,double z,int *Symbol_Val)
{
	if(x>0)
	{
		Symbol_Val[0]=1;
	}
	else
	{
		Symbol_Val[0]=-1;
	}
	if(y>0)
	{
		Symbol_Val[1]=1;
	}
	else
	{
		Symbol_Val[1]=-1;
	}
	if(z>0)
	{
		Symbol_Val[2]=1;
	}
	else
	{
		Symbol_Val[2]=-1;
	}

}

float bias_roll = 0.0;
float rate_roll;
float P_roll[2][2]={{0.0,0.0},{0.0,0.0}};
float Q_angle_roll = 0.001;

float R_measure_roll = 0.03;

float Kalman_filter_roll(float newAngle_roll, float newRate_roll, float dt) {
	//Pitch_Kalman = Kalman_filter(Pitch, GyroX, dt);
    // KasBot V2  -  Kalman filter module - http://www.x-firm.com/?page_id=145
    // Modified by Kristian Lauszus
    // See my blog post for more information: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it

    // Discrete Kalman filter time update equations - Time Update ("Predict")
    // Update xhat - Project the state ahead
    /* Step 1 */
    rate_roll = newRate_roll - bias_roll;
    angle_roll += dt * rate_roll;

    // Update estimation error covariance - Project the error covariance ahead
    /* Step 2 */
    P_roll[0][0] += dt * (dt*P_roll[1][1] - P_roll[0][1] - P_roll[1][0] + Q_angle_roll);
    P_roll[0][1] -= dt * P_roll[1][1];
    P_roll[1][0] -= dt * P_roll[1][1];
    P_roll[1][1] += Q_bias_roll * dt;

    // Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
    // Calculate Kalman gain - Compute the Kalman gain
    /* Step 4 */
    float S_roll = P_roll[0][0] + R_measure_roll; // Estimate error
    /* Step 5 */
    float K_roll[2]; // Kalman gain - This is a 2x1 vector
    K_roll[0] = P_roll[0][0] / S_roll;
    K_roll[1] = P_roll[1][0] / S_roll;

    // Calculate angle and bias - Update estimate with measurement zk (newAngle)
    /* Step 3 */
    float y_roll = newAngle_roll - angle_roll; // Angle difference
    /* Step 6 */
    angle_roll += K_roll[0] * y_roll;
    bias_roll += K_roll[1] * y_roll;

    // Calculate estimation error covariance - Update the error covariance
    /* Step 7 */
    float P00_temp_roll = P_roll[0][0];
    float P01_temp_roll = P_roll[0][1];

    P_roll[0][0] -= K_roll[0] * P00_temp_roll;
    P_roll[0][1] -= K_roll[0] * P01_temp_roll;
    P_roll[1][0] -= K_roll[1] * P00_temp_roll;
    P_roll[1][1] -= K_roll[1] * P01_temp_roll;

    return angle_roll;
};
float K1_roll =0.1;
float FirstOrder_roll(float newAngle_roll, float newRate_roll, float dt)//采集后计算的角度和角加速度
{
  FirstOrder_angle_roll = K1_roll * newAngle_roll + (1-K1_roll) * (angle_roll + newRate_roll * dt);
  return FirstOrder_angle_roll;
}

float K2_roll =0.2; // 对加速度计取值的权重
float kx1_roll,kx2_roll,ky1_roll;
float SecondOrder_roll(float newAngle_roll, float newRate_roll, float dt)//采集后计算的角度和角加速度
{
	kx1_roll=(newAngle_roll-SecondOrder_angle_roll)*(1-K2_roll)*(1-K2_roll);
	ky1_roll=ky1_roll+kx1_roll*dt;
	kx2_roll=ky1_roll+2*(1-K2_roll)*(newAngle_roll-SecondOrder_angle_roll)+newRate_roll;
	SecondOrder_angle_roll=SecondOrder_angle_roll+ kx2_roll*dt;
	return SecondOrder_angle_roll;
}

double GG_XX[NNN];
double GG_YY[NNN];
double GG_ZZ[NNN];

double AA_XX[NNN];
double AA_YY[NNN];
double AA_ZZ[NNN];

int Mean_Index=0;
void Mean_filtering(double G_XX, double G_YY, double G_ZZ,double A_XX, double A_YY, double A_ZZ,double *XYZ_Val)
{
	GG_XX[Mean_Index]=G_XX;
	GG_YY[Mean_Index]=G_YY;
	GG_ZZ[Mean_Index]=G_ZZ;


	AA_XX[Mean_Index]=A_XX;
	AA_YY[Mean_Index]=A_YY;
	AA_ZZ[Mean_Index]=A_ZZ;

	double Sum_XYZ[7];

	for (int i = 0; i < NNN; i++)
	{
		Sum_XYZ[0] += GG_XX[i];
		Sum_XYZ[1] += GG_YY[i];
		Sum_XYZ[2] += GG_ZZ[i];

		Sum_XYZ[3] += AA_XX[i];
		Sum_XYZ[4] += AA_YY[i];
		Sum_XYZ[5] += AA_ZZ[i];
	}

	XYZ_Val[0]=Sum_XYZ[0]/NNN;
	XYZ_Val[1]=Sum_XYZ[1]/NNN;
	XYZ_Val[2]=Sum_XYZ[2]/NNN;
	XYZ_Val[3]=Sum_XYZ[3]/NNN;
	XYZ_Val[4]=Sum_XYZ[4]/NNN;
	XYZ_Val[5]=Sum_XYZ[5]/NNN;

	Mean_Index++;

	if(Mean_Index>=NNN)
	{
		Mean_Index=0;
	}
}

double Angle_RROOLL[NNN_1]={0};
double Angle_PITCH[NNN_1];
int Angle_Index=0;
double Sum_Angle[2]={0,0};

void Mean_filter_Angle(double G_XX_1, int Conversion_Flag,double *XY_Angle) //double G_YY_1,
{
	double Old_GG_X= Angle_RROOLL[Angle_Index];
	Angle_RROOLL[Angle_Index]=G_XX_1;
	if(Sum_Angle[0]>135*NNN_1)
	{
		if(Angle_RROOLL[Angle_Index]<-160)
		{
			Angle_RROOLL[Angle_Index]=Angle_RROOLL[Angle_Index]+360;
		}
	}
	else if(Sum_Angle[0]<-135*NNN_1)
	{
		if(Angle_RROOLL[Angle_Index]>160)
		{
			Angle_RROOLL[Angle_Index]=Angle_RROOLL[Angle_Index]-360;
		}
	}//角度转换完成

	Sum_Angle[0]=Sum_Angle[0]+Angle_RROOLL[Angle_Index]- Old_GG_X;
	XY_Angle[0]=Sum_Angle[0]/NNN_1;
	if(XY_Angle[0]>180)
	{
		XY_Angle[0]=XY_Angle[0]-360;
		for(int i=0;i<NNN_1;i++)
		{
			Angle_RROOLL[i]=XY_Angle[0];
		}
		Sum_Angle[0]=XY_Angle[0]*NNN_1;
	}
	else if(XY_Angle[0]<-180)
	{
		XY_Angle[0]=XY_Angle[0]+360;
		for(int i=0;i<NNN_1;i++)
		{
			Angle_RROOLL[i]=XY_Angle[0];
		}
		Sum_Angle[0]=XY_Angle[0]*NNN_1;
	}

	Angle_Index++;
	if(Angle_Index>=NNN_1)
	{
		Angle_Index=0;
	}
}




//void Mean_filter_Angle(double G_XX_1, int Conversion_Flag,double *XY_Angle) //double G_YY_1,
//{
//
//
//
//	Angle_RROOLL[Angle_Index]=G_XX_1;
//	//Angle_PITCH[Angle_Index]=G_YY_1;
//
//	double Sum_Angle[2]={0,0};
//
//
//
//
//	for (int i = 0; i < NNN_1; i++)
//	{
//		Sum_Angle[0] += Angle_RROOLL[i];
//		//Sum_Angle[1] += Angle_PITCH[i];
//	}
//	XY_Angle[0]=Sum_Angle[0]/NNN_1;
//	//XY_Angle[1]=Sum_Angle[1]/NNN_1;
//	Angle_Index++;
//	if(Angle_Index>=NNN_1)
//	{
//		Angle_Index=0;
//	}
//}

double Sum_GX=0;
double Sum_GY=0;
double Sum_GZ=0;

double Sum_AX=0;
double Sum_AY=0;
double Sum_AZ=0;
int Mean_Index_0=0;

double GG_XX_X[NNN];
double GG_YY_X[NNN];
double GG_ZZ_X[NNN];

double AA_XX_X[NNN];
double AA_YY_X[NNN];
double AA_ZZ_X[NNN];

void Mean_filtering_1(double G_XX_1, double G_YY_1, double G_ZZ_1,double A_XX_1, double A_YY_1, double A_ZZ_1,double *XYZ_Val_1)
{
	double Old_GG_X= GG_XX_X[Mean_Index_0];
	double Old_GG_Y= GG_YY_X[Mean_Index_0];
	double Old_GG_Z= GG_ZZ_X[Mean_Index_0];

	double Old_AA_X= AA_XX_X[Mean_Index_0];
	double Old_AA_Y= AA_YY_X[Mean_Index_0];
	double Old_AA_Z= AA_ZZ_X[Mean_Index_0];


	GG_XX_X[Mean_Index_0]=G_XX_1;
	GG_YY_X[Mean_Index_0]=G_YY_1;
	GG_ZZ_X[Mean_Index_0]=G_ZZ_1;

	AA_XX_X[Mean_Index_0]=A_XX_1;
	AA_YY_X[Mean_Index_0]=A_YY_1;
	AA_ZZ_X[Mean_Index_0]=A_ZZ_1;
	//数据已更新

	Sum_GX=Sum_GX+GG_XX_X[Mean_Index_0]-Old_GG_X;//Sum Update
	Sum_GY=Sum_GY+GG_YY_X[Mean_Index_0]-Old_GG_Y;//Sum Update
	Sum_GZ=Sum_GZ+GG_ZZ_X[Mean_Index_0]-Old_GG_Z;//Sum Update

	Sum_AX=Sum_AX+AA_XX_X[Mean_Index_0]-Old_AA_X;//Sum Update
	Sum_AY=Sum_AY+AA_YY_X[Mean_Index_0]-Old_AA_Y;//Sum Update
	Sum_AZ=Sum_AZ+AA_ZZ_X[Mean_Index_0]-Old_AA_Z;//Sum Update

	XYZ_Val_1[0]=Sum_GX/NNN;
	XYZ_Val_1[1]=Sum_GY/NNN;
	XYZ_Val_1[2]=Sum_GZ/NNN;

	XYZ_Val_1[3]=Sum_AX/NNN;
	XYZ_Val_1[4]=Sum_AY/NNN;
	XYZ_Val_1[5]=Sum_AZ/NNN;

	Mean_Index_0++;
	if(Mean_Index_0>=NNN)
	{
		Mean_Index_0=0;
	}

}


