初始化
CPU1初始化代码
#include "..\Driver\include.h"//各个模块的头文件
extern IfxCpu_mutexLock mutexCpu0InitIsOk; /** CPU0 初始化完成标志位 */
int core1_main (void)
{
IfxCpu_enableInterrupts();
/*
* 关闭看门狗
*/
IfxScuWdt_disableCpuWatchdog (IfxScuWdt_getCpuWatchdogPassword ());
//等待CPU0 初始化完成
while(!IfxCpu_acquireMutex(&mutexCpu0InitIsOk));
/* 用户代码 */
//OLED初始化
OLED_Init();
//MPU6050初始化
MPU6050Init();
//设置定时中断 5000us
CCU6_InitConfig(CCU61, CCU6_Channel1, 5000);
#pragma warning 557 // 屏蔽警告
while(1)//主循环
{
;
}
#pragma warning default // 打开警告
}
MPU6050初始化
void MPU6050Init(void)
{
//陀螺仪IIC初始化
IIC_Init();
if(MPU6050_Init())
{
OLED_P6x8Str(0,0,"6050 Test Fail \r\n");
#pragma warning 557 // 屏蔽警告
while (1);
#pragma warning default // 打开警告
}
}
主要函数
中断服务函数
void CCU61_CH1_IRQHandler(void)
{
/* 开启CPU中断 否则中断不可嵌套 */
IfxCpu_enableInterrupts();
//清除中断标志
IfxCcu6_clearInterruptStatusFlag(&MODULE_CCU61, IfxCcu6_InterruptSource_t13PeriodMatch);
/* 用户代码 */
c++;
if(c == 2)
{
MPU6050();
c = 0;
sprintf(txt,"Angle_X:%06f",Angle_X_Final);
OLED_P6x8Str(0,0,txt); //字符串
sprintf(txt,"Angle_Y:%06f",Angle_Y_Final);
OLED_P6x8Str(0,1,txt); //字符串
OLED_CLS();
}
}
MPU6050子函数代码
void MPU6050(void)
{
MPU_Get_Raw_data(&Accel_x,&Accel_y,&Accel_z,&Gyro_x,&Gyro_y,&Gyro_z); //得到加速度传感器数据
ax = (9.8*Accel_x)/8192;
ay = (9.8*Accel_y)/8192;
az = (9.8*Accel_z)/8192;
Gyro_x = (Gyro_x)/16.4;
Gyro_y = (Gyro_y)/16.4;
Gyro_z = (Gyro_z)/16.4;
//用加速度计算三个轴和水平面坐标系之间的夹角
Angle_x_temp=(atan(ay/az))*180/3.14;
Angle_y_temp=(atan(ax/az))*180/3.14;
Kalman_Filter_X(Angle_x_temp,Gyro_x); //卡尔曼滤波计算X倾角
Kalman_Filter_Y(Angle_y_temp,Gyro_y); //卡尔曼滤波计算Y倾角
}
卡尔曼滤波（这段代码网上不知道谁发的，直接拿来用了，感谢作者）
//卡尔曼参数
float Q_angle = 0.001;
float Q_gyro = 0.003;
float R_angle = 0.5;
float dt = 0.01;//dt为kalman滤波器采样时间
char C_0 = 1;
float Q_bias, Angle_err;
float PCt_0, PCt_1, E;
float K_0, K_1, t_0, t_1;
float Pdot[4] ={0,0,0,0};
float PP[2][2] = { { 1, 0 },{ 0, 1 } };
void Kalman_Filter_X(float Accel,float Gyro) //卡尔曼函数
{
Angle_X_Final += (Gyro - Q_bias) * dt; //先验估计
Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分
Pdot[1]= -PP[1][1];
Pdot[2]= -PP[1][1];
Pdot[3]= Q_gyro;
PP[0][0] += Pdot[0] * dt; // Pk-先验估计误差协方差微分的积分
PP[0][1] += Pdot[1] * dt; // =先验估计误差协方差
PP[1][0] += Pdot[2] * dt;
PP[1][1] += Pdot[3] * dt;
Angle_err = Accel - Angle_X_Final; //zk-先验估计
PCt_0 = C_0 * PP[0][0];
PCt_1 = C_0 * PP[1][0];
E = R_angle + C_0 * PCt_0;
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;
t_0 = PCt_0;
t_1 = C_0 * PP[0][1];
PP[0][0] -= K_0 * t_0; //后验估计误差协方差
PP[0][1] -= K_0 * t_1;
PP[1][0] -= K_1 * t_0;
PP[1][1] -= K_1 * t_1;
Angle_X_Final += K_0 * Angle_err; //后验估计
Q_bias += K_1 * Angle_err; //后验估计
Gyro_x = Gyro - Q_bias; //输出值（后验估计）的微分 = 角速度
}
void Kalman_Filter_Y(float Accel,float Gyro) //卡尔曼函数
{
Angle_Y_Final += (Gyro - Q_bias) * dt; //先验估计
Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分
Pdot[1]=- PP[1][1];
Pdot[2]=- PP[1][1];
Pdot[3]=Q_gyro;
PP[0][0] += Pdot[0] * dt; // Pk-先验估计误差协方差微分的积分
PP[0][1] += Pdot[1] * dt; // =先验估计误差协方差
PP[1][0] += Pdot[2] * dt;
PP[1][1] += Pdot[3] * dt;
Angle_err = Accel - Angle_Y_Final; //zk-先验估计
PCt_0 = C_0 * PP[0][0];
PCt_1 = C_0 * PP[1][0];
E = R_angle + C_0 * PCt_0;
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;
t_0 = PCt_0;
t_1 = C_0 * PP[0][1];
PP[0][0] -= K_0 * t_0; //后验估计误差协方差
PP[0][1] -= K_0 * t_1;
PP[1][0] -= K_1 * t_0;
PP[1][1] -= K_1 * t_1;
Angle_Y_Final += K_0 * Angle_err; //后验估计
Q_bias += K_1 * Angle_err; //后验估计
Gyro_y = Gyro - Q_bias; //输出值（后验估计）的微分 = 角速度
}
代码主要求解出pitch和roll，yaw没有求解
ICM20602/MPU6050实测有效，上张ICM的图，角度挺准，速度也很快，动态测算挺准的，视频不知道咋传，将就看一下图片吧
不过解算过程耗费时间，如果想要节省单片机的计算量，也可以选择直接从MPU6050的DMP中读出四元数然后解算出欧拉角。使用英飞凌TC264的可以参考文章，效果很不错。互补滤波求欧拉角也很实用。
基于英飞凌TC264D的MPU6050调用DMP的移值.
想要了解卡尔曼滤波数学原理的也可以跳转KalmanFilter.Net
或者跳转个人推荐视频链接，这个up主很厉害
1.【卡尔曼滤波器】1_递归算法_Recursive Processing.
2.【卡尔曼滤波器】2_数学基础_数据融合_协方差矩阵_状态空间方程_观测器问题.
3.【卡尔曼滤波器】3_卡尔曼增益超详细数学推导 ～全网最完整.
4.【卡尔曼滤波器】4_误差协方差矩阵数学推导_卡尔曼滤波器的五个公式.
5.【卡尔曼滤波器】5_直观理解与二维实例【包含完整的EXCEL代码】.
祝大家学习愉快-_-
版权声明：本文为CSDN博主「冷暖自知^」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。
原文链接：https://blog.csdn.net/qq_43745620/article/details/107834899
暂无评论