基于STM32F4的姿态解算系统

基于STM32F4的姿态解算系统

摘要：本实验主要利用的基于STM32F4开发板上MUP6050陀螺仪加速度计来做四元素的更新和四元素的姿态解算系统，开发平台为keil uvision5。主要实现MPU6050自带数字运动处理DMP的四元素与自己编写的三子样旋转矢量算法对比的目的。将MPU6050输出的原始数据和姿态解算数据通过串口上传到四轴上位机进行3D仿真，和用LCD屏进行数据的实时显示，并且用一个LED灯显示系统的运行和一个开关控制数据是否上传上位机。程序是在UCOSIII操作系统中实现，建立了一个定时器，五个任务。定时器实现四元素更新的三子样算法和姿态解算；开始任务负责创建任务；任务一实现开关对数据上传控制；任务二实现上传MPU6050输出数据和自己解算姿态到上位机；任务三实现MPU6050的四元素读取并显示到LCD上；任务四负责LCD数据和字符的显示。

关键字：MPU6050；四元素；姿态解算；UCOSIII

1 引言

现在MEMS陀螺仪和加速的计价格便宜，精度虽不如光纤和激光陀螺，但是它以价格便宜体积重量小等优势占据一定的地位，它应用的领域主要在民用手机导航小型飞行器当中。基于MEMS惯性元件的导航可应用于个人的导航和小型飞行器的导航，研究MEMS陀螺导航具有一定的实用价值。本实验主要利用的基于STM32F4开发板上MUP6050陀螺仪加速度计来做四元素的更新和四元素的姿态解算，开发平台为keil uvision5，将数据通过串口上传到四轴上位机进行3D仿真，并用LCD屏进行数据的实时显示。

2 系统架构

以STM32F4为处理器，通过I2C总线与MEMS陀螺仪加速速度计通信，16位FTFLCD液晶显示屏作为数据的实时显示，通过串口将MPU6050的数据和解算的姿态角上传上位机。姿态解算系统的构架如图2-1。

2-1系统构架图

系统的功能：

（1）将STM32F4读取的MPU6050的数据上传上位机

（2）解算姿态角，上传四轴上位机进行仿真，并在LCD上实时显示姿态角。 （3）读取MPU6050的四元素并在LCD上显示。

（4）解算使用三子样四元素更新算法解算姿态角，并在LCD上显示。

3 软硬件设计

3.1 硬件设计

本实验所要用到的硬件资源如下： 1） 指示灯 DS0 2） KEY0 按键 3） TFTLCD 模块 4） 串口 5） MPU6050 指示灯：

用到的指示灯DS0，位共阳型，连接到STM32F4的PF9引脚。如图3-1所示。

图3-1 SD0与STM32连接原理图

按键：

KEY0按下引脚PE4接上低电平。判断PE4上的电平就可以知道按键的状态。如图3-2所示。

3-2 按键与STM32F4的连接原理图

TFTLCD 模块：

4.3寸，分辨率为800*480，16真彩显示。其与STM32F4的连接如图3-3所示。 引脚含义：

CS： TFTLCD 片选信号。 WR：向 TFTLCD 写入数据。 RD：从 TFTLCD 读取数据。 D[15：0]：16 位双向数据线。 RST：硬复位 TFTLCD。 RS：命令/数据标志（ 0，读写命令； 1，读写数据）。

图3-3 TFTLCD与STM32F4的连接原理

MPU6050：

MPU6050内部整合了3轴陀螺仪和 3 轴加速度传感器，并且含有一个第二 IIC 接口，可用于连接外部磁力传感器，并利用自带的数字运动处理器（ DMP: Digital Motion Processor）硬件加速引擎，通过主 IIC 接口，向应用端输出完整的 9 轴融合演算数据。MPU6050与STM32F4的连接如图3-4所示。

图3-4 MPU6050与STM32F4的连接原理图

3.2 软件设计

3.2.1 底层驱动部分

程序的实现是在UCOSIII实时操作系统上实现，但是在使用操作系统前必须写好指示灯 DS0、KEY0 按键、TFTLCD 模块、串口以及MPU6050的驱动，还有系统的移植。本设计是在已经移植好的UCOSIII操作系统上进行的，所以就对UCOSIII移植到STM32F4上不做说明。

工程文件夹下的文件HARDWARE文件夹存放的是驱动文件，其中有IIC、KEY、LCD、LED、MPU6050

文件分别是I2C、按键KEY、LCD、LED和MPU6050的驱动文件，如图4-1所示。其中的UCOSIII文件夹为UCOSIII操作系统文件。

图4-1 工程文件

LED文件夹中的led.c和led.h主要是对GPIO口和使能对应的GPIO时钟的操作。KEY文件夹中的key.c和key.h主要是先初始化对应的GPIO口和实现一个按键扫描函数KEY_Scan( ) 。

//mode:0,不支持连续按;1,支持连续按; //返回值1表示,KEY0按下

//注意此函数有响应优先级,KEY0>KEY1>KEY2>WK_UP!! u8 KEY_Scan(u8 mode) {

static u8 key_up=1;//按键按松开标志 if(mode)key_up=1; //支持连按 { }

delay_ms(10);//去抖动 key_up=0;

if(KEY0==0)return 1; else if(KEY1==0)return 2; else if(KEY2==0)return 3; else if(WK_UP==1)return 4;

return 0;// 无按键按下

if(key_up&&(KEY0==0||KEY1==0||KEY2==0||WK_UP==1))

}else if(KEY0==1&&KEY1==1&&KEY2==1&&WK_UP==0)key_up=1;

在IIC文件夹中的myiic.c和myiic.h主要实现I2C读和写一个字节， IIC_Read_Byte ( )和IIC_Send_Byte( )。由于大家对I2C总线比较熟悉就不做多说明。

LCD文件夹中lcd.c和lcd.h主要实现一些对LCD屏的操作的函数。用到的主要有LCD_ShowString/LCD_ShowxNum( )/LCD_ShowNum( )。

//显示数字,高位为0,则不显示 //x,y :起点坐标 //len :数字的位数 //size:字体大小 //color:颜色

//num:数值(0~4294967295);

void LCD_ShowNum(u16 x,u16 y,u32 num,u8 len,u8 size); //显示数字,高位为0,还是显示 //x,y:起点坐标

//显示一个数字