嵌入式综合设计

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 基于UCOSI

15、II操作系统部分程序是在UCOSIII操作系统中实现，建立了一个定时器，五个任务。定时器实现四元素更新的三子样算法和姿态解算，其优先级为2，堆栈大小为256；开始任务负责创建定时器和其他四个任务，其优先级为3，堆栈大小为128；任务一实现开关对数据上传控制，优先级为4，堆栈大小为256；任务二实现上传MPU6050输出数据和自己解算姿态到上位机，其优先级为5，堆栈大小为256；任务三实现MPU6050的四元素读取并显示到LCD上，其优先级为6，堆栈大小为128；任务四负责LCD数据和字符的显示，其优先级为7，堆栈大小为128。Main函数中主要是初始化硬件资源和UCOSIII操作系统以及创建开

16、始任务和开始UCOSIII操作系统。int main(void)OS_ERR err;CPU_SR_ALLOC();NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);/设置系统中断优先级分组2delay_init(168); /初始化延时函数uart_init(500000);/初始化串口波特率为500000LED_Init();/初始化LED KEY_Init();/初始化按键 LCD_Init();/LCD初始化MPU_Init();/初始化MPU6050 Show_Main_UI(); /在LCD上显式界面OSInit(&err); /初始化U

17、COSIIIOS_CRITICAL_ENTER();/进入临界区 /创建开始任务OSTaskCreate(OS_TCB * )&StartTaskTCB,/任务控制块 (CPU_CHAR* )start task, /任务名字 (OS_TASK_PTR )start_task, /任务函数 (void* )0,/传递给任务函数的参数 (OS_PRIO )START_TASK_PRIO, /任务优先级 (CPU_STK * )&START_TASK_STK0,/任务堆栈基地址 (CPU_STK_SIZE)START_STK_SIZE/10,/任务堆栈深度限位 (CPU_STK_SIZE)STAR

18、T_STK_SIZE,/任务堆栈大小 (OS_MSG_QTY )0,/任务内部消息队列能够接收的最大消息数目,为0时禁止接收消息 (OS_TICK )0,/当使能时间片轮转时的时间片长度，为0时为默认长度， (void * )0,/用户补充的存储区 (OS_OPT )OS_OPT_TASK_STK_CHK|OS_OPT_TASK_STK_CLR, /任务选项 (OS_ERR * )&err);/存放该函数错误时的返回值OS_CRITICAL_EXIT();/退出临界区 OSStart(&err); /开启UCOSIII开始任务start_task( ),在创建了一个时钟和四个任务后将自己删除。

19、用一个全局的变量report来表示数据上传串口开关的状态，当report=1时表示开启上传，report=0是表示停止上传。开始默认是开启上传的，即report=1，当按键KEY0按下时翻转report的值，即可达到开启和关闭数据上传串口的目的。if(report)mpu6050_send_data(aacx,aacy,aacz,gyrox,gyroy,gyroz);/用自定义帧发送加速度和陀螺仪原始数据if(report)usart1_report_imu(aacx,aacy,aacz,gyrox,gyroy,gyroz,(int)(roll*100),(int)(pitch*100),(i

20、nt)(yaw*10);任务一的函数如下每隔100ms进行扫描一次。void task1_task(void *p_arg)OS_ERR err;u8 key;LED1=0;while(1)key=KEY_Scan(0);if(key=KEY0_PRES)report=!report;if(report)LCD_ShowString(30,170,200,16,16,UPLOAD ON );LED1=0;else LCD_ShowString(30,170,200,16,16,UPLOAD OFF);LED1=1;OSTimeDlyHMSM(0,0,0,100,OS_OPT_TIME_HMSM

21、_STRICT,&err); /延时100ms对于在LCD上姿态角的显示，结果只精确到小数点后的一位。可以先定义一个短整型的temp，将要显示的姿态角负值给，它然后将其扩大10倍，分为整数部分和小数部分分别显示。任务二的服务函数如下。void task2_task(void *p_arg)OS_ERR err;float pitch,roll,yaw; /欧拉角short aacx,aacy,aacz;/加速度传感器原始数据short gyrox,gyroy,gyroz;/陀螺仪原始数据short temp;/温度u8 t=0;while(1)if(mpu_dmp_get_data(&pitc

22、h,&roll,&yaw)=0) temp=MPU_Get_Temperature();/得到温度值MPU_Get_Accelerometer(&aacx,&aacy,&aacz);/得到加速度传感器数据MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);/得到陀螺仪数据if(report)mpu6050_send_data(aacx,aacy,aacz,gyrox,gyroy,gyroz);/用自定义帧发送加速度和陀螺仪原始数据if(report)usart1_report_imu(aacx,aacy,aacz,gyrox,gyroy,gyroz,(int)(ro

23、ll*100),(int)(pitch*100),(int)(yaw*10);if(t%10)=0) if(temp0)LCD_ShowChar(30+48,200,-,16,0);/显示负号temp=-temp;/转为正数else LCD_ShowChar(30+48,200, ,16,0);/去掉负号 LCD_ShowNum(30+48+8,200,temp/100,3,16);/显示整数部分 LCD_ShowNum(30+48+40,200,temp%10,1,16);/显示小数部分 temp=pitch*10;if(temp0)LCD_ShowChar(30+48,220,-,16,0

24、);/显示负号temp=-temp;/转为正数else LCD_ShowChar(30+48,220, ,16,0);/去掉负号 LCD_ShowNum(30+48+8,220,temp/10,3,16);/显示整数部分 LCD_ShowNum(30+48+40,220,temp%10,1,16);/显示小数部分 temp=roll*10;if(temp0)LCD_ShowChar(30+48,240,-,16,0);/显示负号temp=-temp;/转为正数else LCD_ShowChar(30+48,240, ,16,0);/去掉负号 LCD_ShowNum(30+48+8,240,te

25、mp/10,3,16);/显示整数部分 LCD_ShowNum(30+48+40,240,temp%10,1,16);/显示小数部分 temp=yaw*10;if(temp0)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%10,1,16);/显示小数部分 t=0;LED0=!LED0;/LED闪烁t+;O

26、STimeDlyHMSM(0,0,0,10,OS_OPT_TIME_HMSM_STRICT,&err); /延时10ms由于四元素的四个数都是大于0小于1，所以借鉴前面的经验，只需要显示小数部分。将q扩大100000000倍后变成整数显示在LCD上。任务3的函数如下：void task3_task(void *p_arg)float quat4=0,0,0,0;OS_ERR err;CPU_SR_ALLOC();OS_CRITICAL_ENTER();LCD_ShowString(30,300,200,16,16,Orinal Quaternion:); POINT_COLOR=BLACK;/

27、设置字体为黑色/POINT_COLOR=BLUE;/设置字体为蓝色 LCD_ShowString(30,320,200,16,16,q0=0.); LCD_ShowString(30+100,320,200,16,16,q1=0.); LCD_ShowString(30+100*2,320,200,16,16,q2=0.); LCD_ShowString(30+100*3,320,200,16,16,q3=0.); OS_CRITICAL_EXIT();while(1)if(MPU_Get_Quaternion(quat)=0); /读取MPU6050 DMP处理后的数据printf(四元素为

28、q0=%f q1=%f q2=%f q3=%frn,*quat,*(quat+1),*(quat+2),*(quat+3);LCD_ShowxNum(70,320,quat0*10000000,7,16,0x80);/显示小数部分 LCD_ShowxNum(70+100,320,quat1*10000000,7,16,0x80);/显示小数部分LCD_ShowxNum(70+100*2,320,quat2*10000000,7,16,0x80);/显示小数部分 LCD_ShowxNum(70+100*3,320,quat3*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,Clculate Quaternion:); POINT_COLOR=BLACK;/设置字体为黑色 LCD_ShowString(30,360,200,16,16,