嵌入式综合设计.docx

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）解算使用三子样四元素更新算法解算姿态角，

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

5、：向 TFTLCD 写入数据。 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 M

6、PU6050与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文件夹为UCOSII

7、I操作系统文件。 图4-1 工程文件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

8、) 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; 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和lc

9、d.h主要实现一些对LCD屏的操作的函数。用到的主要有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

10、,填充0./6:1:保留/0:0,非叠加显示;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的一些基本的设

11、置和初始化以及数据的读取。u8 MPU_Init(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); /陀螺仪传感器,2000dps MPU_Set_Accel_Fsr(0); /加速度传感器,2g MPU_Set_Rate(50); /设置采样率50Hz MPU_Write_Byte(MPU_INT_E

12、N_REG,0X00); /关闭所有中断 MPU_Write_Byte(MPU_USER_CTRL_REG,0X00); /I2C主模式关闭 MPU_Write_Byte(MPU_FIFO_EN_REG,0X00); /关闭FIFO MPU_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_

13、Byte(MPU_PWR_MGMT2_REG,0X00); /加速度与陀螺仪都工作 MPU_Set_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轴

14、的原始读数(带符号)，16位AD采集的值/返回值:0,成功；其他,错误代码u8 MPU_Get_Accelerometer(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(

15、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数据和字符的显示，其

16、优先级为7，堆栈大小为128。Main函数中主要是初始化硬件资源和UCOSIII操作系统以及创建开始任务和开始UCOSIII操作系统。int main(void) OS_ERR err; CPU_SR_ALLOC(); NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);/设置系统中断优先级分组2 delay_init(168); /初始化延时函数 uart_init(500000); /初始化串口波特率为500000 LED_Init(); /初始化LED KEY_Init(); /初始化按键 LCD_Init(); /LCD初始化 MPU_Init

17、(); /初始化MPU6050 Show_Main_UI(); /在LCD上显式界面 OSInit(&err); /初始化UCOSIII OS_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,

18、 /任务堆栈基地址 (CPU_STK_SIZE)START_STK_SIZE/10, /任务堆栈深度限位 (CPU_STK_SIZE)START_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_

19、EXIT(); /退出临界区 OSStart(&err); /开启UCOSIII 开始任务start_task( ),在创建了一个时钟和四个任务后将自己删除。用一个全局的变量report来表示数据上传串口开关的状态，当report=1时表示开启上传，report=0是表示停止上传。开始默认是开启上传的，即report=1，当按键KEY0按下时翻转report的值，即可达到开启和关闭数据上传串口的目的。if(report)mpu6050_send_data(aacx,aacy,aacz,gyrox,gyroy,gyroz);/用自定义帧发送加速度和陀螺仪原始数据if(report)usart1_

20、report_imu(aacx,aacy,aacz,gyrox,gyroy,gyroz,(int)(roll*100),(int)(pitch*100),(int)(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 L

21、CD_ShowString(30,170,200,16,16,UPLOAD OFF); LED1=1; OSTimeDlyHMSM(0,0,0,100,OS_OPT_TIME_HMSM_STRICT,&err); /延时100ms 对于在LCD上姿态角的显示，结果只精确到小数点后的一位。可以先定义一个短整型的temp，将要显示的姿态角负值给，它然后将其扩大10倍，分为整数部分和小数部分分别显示。任务二的服务函数如下。void task2_task(void *p_arg) OS_ERR err; float pitch,roll,yaw; /欧拉角 short aacx,aacy,aacz;

22、/加速度传感器原始数据 short gyrox,gyroy,gyroz; /陀螺仪原始数据 short temp; /温度 u8 t=0; while(1) if(mpu_dmp_get_data(&pitch,&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,aa

23、cz,gyrox,gyroy,gyroz);/用自定义帧发送加速度和陀螺仪原始数据 if(report)usart1_report_imu(aacx,aacy,aacz,gyrox,gyroy,gyroz,(int)(roll*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

24、/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); /显示负号 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;

25、 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,temp/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_ShowCha

26、r(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+; OSTimeDlyHMSM(0,0,0,10,OS_OPT_TIME_HMSM_STRICT,&err); /延时10ms 由于四元素的四个数都是大于0小于1，所以借鉴前面的经验，只需要显示小数部分。将q扩大100000000倍后变成整数显示在LCD上。任务3的函数如下：void task3_task

27、(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;/设置字体为黑色 /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,3

28、20,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(四元素为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,32

29、0,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