/* * bsp_IMU.c * * Created on: Oct 26, 2023 * Author: shiya */ #include "bsp_IMU.h" #include "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) { 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; } }