基于STM32F4的姿态解算系统

} }

} t++;

OSTimeDlyHMSM(0,0,0,10,OS_OPT_TIME_HMSM_STRICT,&err); //延时10ms

}

LCD_ShowNum(30+48+8,240,temp/10,3,16); LCD_ShowNum(30+48+40,240,temp,1,16); temp=yaw*10; if(temp<0) {

LCD_ShowChar(30+48,260,'-',16,0); temp=-temp;

//转为正数

//去掉负号 //显示整数部分

//显示小数部分

//显示负号

//显示整数部分

//显示小数部分

}else LCD_ShowChar(30+48,260,' ',16,0);

LCD_ShowNum(30+48+8,260,temp/10,3,16); LCD_ShowNum(30+48+40,260,temp,1,16); t=0;

LED0=!LED0;//LED闪烁

由于四元素的四个数都是大于0小于1，所以借鉴前面的经验，只需要显示小数部分。将q扩大100000000

倍后变成整数显示在LCD上。任务3的函数如下：

void task3_task(void *p_arg) {

float quat[4]={0,0,0,0}; OS_ERR err; CPU_SR_ALLOC(); OS_CRITICAL_ENTER();

LCD_ShowString(30,300,200,16,16,\ POINT_COLOR=BLACK;//设置字体为黑色 //POINT_COLOR=BLUE;//设置字体为蓝色

LCD_ShowString(30,320,200,16,16,\ LCD_ShowString(30+100,320,200,16,16,\ LCD_ShowString(30+100*2,320,200,16,16,\LCD_ShowString(30+100*3,320,200,16,16,\

OS_CRITICAL_EXIT(); while(1) {

if(MPU_Get_Quaternion(quat)==0); //读取MPU6050 DMP处理后的数据 {

printf(\四元素为q0=%f q1=%f q2=%f q3=%f\\r\\n\LCD_ShowxNum(70,320,quat[0]*10000000,7,16,0x80);

//显示小数部分

//显示小数部分

//显示小数部分 //显示小数部分

LCD_ShowxNum(70+100,320,quat[1]*10000000,7,16,0x80); LCD_ShowxNum(70+100*2,320,quat[2]*10000000,7,16,0x80); LCD_ShowxNum(70+100*3,320,quat[3]*10000000,7,16,0x80);

OSTimeDlyHMSM(0,0,0,10,OS_OPT_TIME_HMSM_STRICT,&err); //延时10ms

} }

}

任务四负责LCD上显示一些字符，只执行一次。任务四的服务函数如下：

void task4_task(void *p_arg) {

POINT_COLOR=RED;//设置字体为红色

LCD_ShowString(30,340,200,16,16,\ POINT_COLOR=BLACK;//设置字体为黑色

LCD_ShowString(30,380,200,16,16,\ . C\ }

LCD_ShowString(30,400,200,16,16,\ . C\

LCD_ShowString(30,360,200,16,16,\ LCD_ShowString(30+100,360,200,16,16,\ LCD_ShowString(30+100*2,360,200,16,16,\LCD_ShowString(30+100*3,360,200,16,16,\

LCD_ShowString(30,420,200,16,16,\ yaw= . C\

定时器定时进行50ms的周期定时，其优先级为2，是这几个任务中最高的，主要负责三子样旋转矢量算法。

三子样四元素更新算法的步骤为：

2h],[tk,tk?h]三个时间段的角增量，??1,??2,??3； 3927??1???2???2????3???1?； （2）计算旋转矢量Φ?h????1???2???3?2040（1）分别计算[tk,tk?],[tk,tk?（3）由旋转矢量构造增量四元素q?h?；

h3Φ??cos()??2?ΦΦ?x?sin()?2?其中Φ??Φ,Φ,Φ?， Φ?Φ2?Φ2?Φ2。 q?h???ΦxyzxyzΦ??Φy?Φsin(2)??ΦΦ?z?sin()?2??Φ（4）利用Q?tk?1??Q?tk?1??q?h?计算更新后的四元素Q?tk?1?； （5）利用更新后的四元素Q?tk?1?得到姿态变换矩阵Cb。

n?q12?q02?q22?q32?Cbn??2?q1q2?q0q3??2?qq?qq?1302?2?q1q2?q0q3?q2?q0?q3?q12?q2q3?q0q1?2222??T11T12T13???

2?q2q3?q0q1????TTT23??21222222?q3?q0?q2?q1???T31T32T33??2?q1q3?q0q2?

??T32)?pitch?arcsin(?T31roll?arctan(?) ?T33??T12yaw?arctan()?T22?为了实现上面的功能本实验做了如下的一些设计。MPU6050的陀螺仪输出是角速度，所以需要先

积分得到角度，再用角度计算角增量。选取h?120ms，计算角增量按下图4-2所示。

图4-2 角增量计算方法

由于定时器回调函数有点多，这里就不给出程序，具体程序可以见工程文件包中的main.c文件。

4实验

正对开机时刻的MPU6050的状态为初始状态。实验结果见下图4-1到4-5。

图4-1 开机后的状态

图4-2 LED灯的情况

图4-3 与四轴上位机连接