///*
// * 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);
//
//double 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;
//	GF_BSP_Interrupt_Add_CallBack(
//			DF_BSP_InterCall_TIM1_100ms_PeriodElapsedCallback,
//						GF_BSP_GetAngle);
//	return 1;
//}
//
////void 	GF_BSP_GetAngle();//如，数据有问题，请分开调用
//
//
//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倍)
//double MPU_Get_Temperature(void)
//{
//	u8 buf[2];
//	short raw;
//	double temp;
//	MPU_Read_Len(MPU_ADDR, MPU_TEMP_OUTH_REG, 2, buf);
//	raw = ((u16)buf[0] << 8) | buf[1];
//	temp = 25 + ((double)raw) / 326.8;
//	return temp;
//}
////得到陀螺仪值(原始值)
////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 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;
//	}
//}
//
//
//
//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;
//	}
//
//}
//
//