|
我的MPU6050用了互补滤波法得到x， y，z轴的角度但是很不准，平放模块时不为x轴角度不为0，程序如下 //********************************************************* // 倾角计算（卡尔曼融合） //********************************************************* void Angle_Calcu(unsigned char A,unsigned char G) { //------加速度-------------------------- //范围为2g时，换算关系：16384 LSB/g //角度较小时，x=sinx得到角度（弧度）, deg = rad*180/3.14 //因为x>=sinx,故乘以1.3适当放大 Accel_x = GetData(A); //读取X轴加速度 Angle_ax = (Accel_x - 1100) /16384; //去除零点偏移,计算得到角度（弧度） Angle_ax = Angle_ax*1.2*180/3.14; //弧度转换为度, //-------角速度------------------------- //范围为2000deg/s时，换算关系：16.4 LSB/(deg/s) Gyro_y = GetData(G); //静止时角速度Y轴输出为-30左右 Gyro_y = -(Gyro_y + 30)/16.4; //去除零点偏移，计算角速度值,负号为方向处理 Angle_gy = Angle_gy + Gyro_y*0.01; //角速度积分得到倾斜角度. if(A==ACCEL_XOUT_H) ComplementFilterX(Angle_ax,Gyro_y); if(A==ACCEL_YOUT_H) ComplementFilterY(Angle_ax,Gyro_y); if(A==ACCEL_ZOUT_H) ComplementFilterZ(Angle_ax,Gyro_y); } /*//-------卡尔曼滤波融合----------------------- Kalman_Filter(Angle_ax,Gyro_y); //卡尔曼滤波计算倾角 //-------互补滤波----------------------- //补偿原理是取当前倾角和加速度获得倾角差值进行放大，然后与 //陀螺仪角速度叠加后再积分，从而使倾角最跟踪为加速度获得的角度 //0.5为放大倍数，可调节补偿度；0.01为系统周期10ms Angle = Angle + (((Angle_ax-Angle)*0.5 + Gyro_y)*0.01); */ /** * 互补滤波参数 */ float tau = 0.065, p = 0.0; // 用于计算比例 float dtc = 0.01; // 抽样时间10ms static float bias_gx = 0.0,bias_gy = 0.0,bias_gz = 0.0,gy_temp; // 零点漂移误差消除 static float fix_angle,fiy_angle,fiz_angle; /** * 互补滤波算法 * angle 为重力感应获得的角度值 * gy_speed 为角速度 */ float ComplementFilterX(float angle, float gy_sped) { bias_gx = bias_gx*0.998+gy_sped*0.002; // 消除零点误差 gy_temp = gy_sped - bias_gx; // 减去零点误差 p = tau/(tau+dtc); // 对角速度gy_temp进行高通滤波,对重力感应得到的角度angle进行低通滤波 fix_angle = p*(fix_angle+gy_temp*dtc)+(1-p)*angle; return fix_angle; } float ComplementFilterY(float angle, float gy_sped) { bias_gz = bias_gz*0.998+gy_sped*0.002; // 消除零点误差 gy_temp = gy_sped - bias_gz; // 减去零点误差 p = tau/(tau+dtc); // 对角速度gy_temp进行高通滤波,对重力感应得到的角度angle进行低通滤波 fiy_angle = p*(fiy_angle+gy_temp*dtc)+(1-p)*angle; return fiy_angle; } float ComplementFilterZ(float angle, float gy_sped) { bias_gx = bias_gx*0.998+gy_sped*0.002; // 消除零点误差 gy_temp = gy_sped - bias_gx; // 减去零点误差 p = tau/(tau+dtc); // 对角速度gy_temp进行高通滤波,对重力感应得到的角度angle进行低通滤波 fiz_angle = p*(fiz_angle+gy_temp*dtc)+(1-p)*angle; return fiz_angle; } int main(void) { Stm32_Clock_Init(9);//系统时钟设置 /*IIC接口初始化*/ I2C_MPU6050_Init(); /*陀螺仪传感器初始化*/ InitMPU6050(); //hw_tick_start(); // 定时器周期性中断，用于提供系统脉搏 while(1) { Angle_Calcu(ACCEL_XOUT_H,GYRO_YOUT_H); //Display10BitData(fix_angle); Angle_Calcu(ACCEL_YOUT_H,GYRO_ZOUT_H); //Display10BitData(fiy_angle); Angle_Calcu(ACCEL_ZOUT_H,GYRO_XOUT_H); //Display10BitData(fiz_angle); } } |
|
|
