《于基stm32f4的姿态解算系统--大学毕设论文》由会员分享，可在线阅读，更多相关《于基stm32f4的姿态解算系统--大学毕设论文（13页珍藏版）》请在金锄头文库上搜索。
1、基于STM32F4的姿态解算系统摘要：本实验主要利用的基于STM32F4开发板上MUP6050陀螺仪加速度计来做四元素的更新和四元素的姿态解算系统，开发平台为keil uvision5。主要实现MPU6050自带数字运动处理DMP的四元素与自己编写的三子样旋转矢量算法对比的目的。将MPU6050输出的原始数据和姿态解算数据通过串口上传到四轴上位机进行3D仿真，和用LCD屏进行数据的实时显示，并且用一个LED灯显示系统的运行和一个开关控制数据是否上传上位机。程序是在UCOSIII操作系统中实现，建立了一个定时器，五个任务。定时器实现四元素更新的三子样算法和姿态解算；开始任务负责创建任务；任务一实
2、现开关对数据上传控制；任务二实现上传MPU6050输出数据和自己解算姿态到上位机；任务三实现MPU6050的四元素读取并显示到LCD上；任务四负责LCD数据和字符的显示。关键字：MPU6050；四元素；姿态解算；UCOSIII1 引言现在MEMS陀螺仪和加速的计价格便宜，精度虽不如光纤和激光陀螺，但是它以价格便宜体积重量小等优势占据一定的地位，它应用的领域主要在民用手机导航小型飞行器当中。基于MEMS惯性元件的导航可应用于个人的导航和小型飞行器的导航，研究MEMS陀螺导航具有一定的实用价值。本实验主要利用的基于STM32F4开发板上MUP6050陀螺仪加速度计来做四元素的更新和四元素的姿态解算
3、，开发平台为keil uvision5，将数据通过串口上传到四轴上位机进行3D仿真，并用LCD屏进行数据的实时显示。2 系统架构以STM32F4为处理器，通过I2C总线与MEMS陀螺仪加速速度计通信，16位FTFLCD液晶显示屏作为数据的实时显示，通过串口将MPU6050的数据和解算的姿态角上传上位机。姿态解算系统的构架如图2-1。2-1系统构架图系统的功能：（1） 将STM32F4读取的MPU6050的数据上传上位机（2） 解算姿态角，上传四轴上位机进行仿真，并在LCD上实时显示姿态角。（3） 读取MPU6050的四元素并在LCD上显示。（4） 解算使用三子样四元素更新算法解算姿态角，并在L
4、CD上显示。3 软硬件设计3.1 硬件设计本实验所要用到的硬件资源如下：1） 指示灯 DS02） 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
5、 写入数据。RD：从 TFTLCD 读取数据。D15：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与STM32F
6、4的连接原理图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
7、 工程文件LED文件夹中的led.c和led.h主要是对GPIO口和使能对应的GPIO时钟的操作。KEY文件夹中的key.c和key.h主要是先初始化对应的GPIO口和实现一个按键扫描函数KEY_Scan( ) 。/mode:0,不支持连续按;1,支持连续按;/返回值1表示,KEY0按下/注意此函数有响应优先级,KEY0KEY1KEY2WK_UP!u8 KEY_Scan(u8 mode) static u8 key_up=1;/按键按松开标志if(mode)key_up=1; /支持连按 if(key_up&(KEY0=0|KEY1=0|KEY2=0|WK_UP=1)delay_ms(10);
8、/去抖动 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;else if(KEY0=1&KEY1=1&KEY2=1&WK_UP=0)key_up=1; return 0;/ 无按键按下在IIC文件夹中的myiic.c和myiic.h主要实现I2C读和写一个字节， IIC_Read_Byte ( )和IIC_Send_Byte( )。由于大家对I2C总线比较熟悉就不做多说明。LCD文件夹中lcd.c和lcd.h主要实现一些对LCD屏的操作的函数
9、。用到的主要有LCD_ShowString/LCD_ShowxNum( )/LCD_ShowNum( )。/显示数字,高位为0,则不显示/x,y :起点坐标 /len :数字的位数/size:字体大小/color:颜色 /num:数值(04294967295); void LCD_ShowNum(u16 x,u16 y,u32 num,u8 len,u8 size); /显示一个数字/显示数字,高位为0,还是显示/x,y:起点坐标/num:数值(0999999999); /len:长度(即要显示的位数)/size:字体大小/mode:/7:0,不填充;1,填充0./6:1:保留/0:0,非叠加
10、显示;1,叠加显示.void LCD_ShowxNum(u16 x,u16 y,u32 num,u8 len,u8 size,u8 mode);/显示 数字/显示字符串/x,y:起点坐标/width,height:区域大小 /size:字体大小/*p:字符串起始地址void LCD_ShowString(u16 x,u16 y,u16 width,u16 height,u8 size,u8 *p);/显示一个字符串,12/16字体MPU6050中的文件mpu6050.c和mpu6050.h实现mpu6050的驱动。只要对mpu6050的一些基本的设置和初始化以及数据的读取。u8 MPU_Ini
11、t(void) u8 res;IIC_Init();/初始化IIC总线MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X80);/复位MPU6050 delay_ms(100);MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X00);/唤醒MPU6050 MPU_Set_Gyro_Fsr(3);/陀螺仪传感器,2000dpsMPU_Set_Accel_Fsr(0);/加速度传感器,2gMPU_Set_Rate(50);/设置采样率50HzMPU_Write_Byte(MPU_INT_EN_REG,0X00);/关闭所有中断MPU_Write_Byte(
12、MPU_USER_CTRL_REG,0X00);/I2C主模式关闭MPU_Write_Byte(MPU_FIFO_EN_REG,0X00);/关闭FIFOMPU_Write_Byte(MPU_INTBP_CFG_REG,0X80);/INT引脚低电平有效res=MPU_Read_Byte(MPU_DEVICE_ID_REG);if(res=MPU_ADDR)/器件ID正确MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X01);/设置CLKSEL,PLL X轴为参考MPU_Write_Byte(MPU_PWR_MGMT2_REG,0X00);/加速度与陀螺仪都工作MPU_S
13、et_Rate(50);/设置采样率为50Hz else return 1;return 0;/得到温度值/返回值:温度值(扩大了100倍)short MPU_Get_Temperature(void)/得到陀螺仪值(原始值)，16位AD采集的值/gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)/返回值:0,成功/其他,错误代码u8 MPU_Get_Gyroscope(short *gx,short *gy,short *gz)/得到加速度值(原始值)/gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)，16位AD采集的值/返回值:0,成功；其他,错误代码u8 MPU_Get_A
14、ccelerometer(short *ax,short *ay,short *az)/获得四元素u8 MPU_Get_Quaternion(float *quat)/得到dmp处理后的数据(注意,本函数需要比较多堆栈,局部变量有点多)/pitch:俯仰角 精度:0.1 范围:-90.0 +90.0/roll:横滚角 精度:0.1 范围:-180.0 +180.0/yaw:航向角 精度:0.1 范围:-180.0 +180.0/返回值:0,正常； 其他,失败u8 mpu_dmp_get_data(float *pitch,float *roll,float *yaw)3.2.2 基于UCOSIII操作系统部分程序是在UCOSIII操作系统中实现，建立了一个定时器，五个任务。定时器实现四元素更新的三子样算法和姿态解算，其优先级为2，堆栈大小为256；开始任务负责创建定时器和其他四个任务，其优先级为3，堆栈大小为128；任务一实现开关对数据上传控制，优先级为4，堆栈大小为256；任务二实现上传MPU6050输出数据和自己解算姿态到上位机，其优先级为5，堆栈大小为256；任务三实现MPU6050的四元素读取并显示到LCD上，其优先级为6，堆栈大小为128；任务四负责LCD数据和字符的显示，
