单片机解密mpu6050卡尔曼滤波STM32单片机程序
- 单片机解密#include "kalman.h"
- 单片机解密#include "mpu6050.h"
- 单片机解密#include "math.h"
- short aacx,aacy,aacz; /*加速度传感器原始数据*/
- short gyrox,gyroy,gyroz; /*陀螺仪原始数据*/
- short temperature; /*陀螺仪温度数据*/
- float Accel_x; /*X轴加速度值暂存*/
- float Accel_y; /*Y轴加速度值暂存*/
- float Accel_z; /*Z轴加速度值暂存*/
- float Gyro_x; /*X轴陀螺仪数据暂存*/
- float Gyro_y; /*Y轴陀螺仪数据暂存*/
- float Gyro_z; /*Z轴陀螺仪数据暂存*/
- float Angle_x_temp; /*由加速度计算的x倾斜角度*/
- float Angle_y_temp; /*由加速度计算的y倾斜角度*/
- float Angle_X_Final; /*X最终倾斜角度*/
- float Angle_Y_Final; /*Y最终倾斜角度*/
- /**
- * @brief 读取数据预处理
- *
- * @param NULL
- *
- * @retval NULL
- */
- void Angle_Calcu(void)
- {
- /*1.原始数据读取*/
- float accx,accy,accz; /*三方向角加速度值*/
- MPU_Get_Accelerometer(&aacx,&aacy,&aacz); /*得到加速度传感器数据*/
- MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); /*得到陀螺仪数据*/
- temperature = MPU_Get_Temperature(); /*得到温度值*/
- Accel_x = aacx;
- Accel_y = aacy;
- Accel_z = aacz;
- Gyro_x = gyrox;
- Gyro_y = gyroy;
- Gyro_z = gyroz;
-
- /*2.角加速度原始值处理过程*/
- /*加速度传感器配置寄存器0X1C内写入0x01,设置范围为±2g。换算关系：2^16/4 = 16384LSB/g*/
- if(Accel_x<32764) accx=Accel_x/16384;
- else accx=1-(Accel_x-49152)/16384;
- if(Accel_y<32764) accy=Accel_y/16384;
- else accy=1-(Accel_y-49152)/16384;
- if(Accel_z<32764) accz=Accel_z/16384;
- else accz=(Accel_z-49152)/16384;
- /*加速度反正切公式计算三个轴和水平面坐标系之间的夹角*/
- Angle_x_temp=(atan(accy/accz))*180/3.14;
- Angle_y_temp=(atan(accx/accz))*180/3.14;
- /*判断计算后角度的正负号*/
- if(Accel_x<32764) Angle_y_temp = +Angle_y_temp;
- if(Accel_x>32764) Angle_y_temp = -Angle_y_temp;
- if(Accel_y<32764) Angle_x_temp = +Angle_x_temp;
- if(Accel_y>32764) Angle_x_temp = -Angle_x_temp;
-
- /*3.角速度原始值处理过程*/
- /*陀螺仪配置寄存器0X1B内写入0x18，设置范围为±2000deg/s。换算关系：2^16/4000=16.4LSB/(°/S)*/
- /*计算角速度*/
- if(Gyro_x<32768) Gyro_x=-(Gyro_x/16.4);
- if(Gyro_x>32768) Gyro_x=+(65535-Gyro_x)/16.4;
- if(Gyro_y<32768) Gyro_y=-(Gyro_y/16.4);
- if(Gyro_y>32768) Gyro_y=+(65535-Gyro_y)/16.4;
- if(Gyro_z<32768) Gyro_z=-(Gyro_z/16.4);
- if(Gyro_z>32768) Gyro_z=+(65535-Gyro_z)/16.4;
-
- /*4.调用卡尔曼函数*/
- Kalman_Filter_X(Angle_x_temp,Gyro_x); /*卡尔曼滤波计算X倾角*/
- Kalman_Filter_Y(Angle_y_temp,Gyro_y); /*卡尔曼滤波计算Y倾角*/
- }
芯片解密
