嵌入式综合设计.docx
《嵌入式综合设计.docx》由会员分享，可在线阅读，更多相关《嵌入式综合设计.docx（18页珍藏版）》请在冰豆网上搜索。
嵌入式综合设计
基于STM32F4的姿态解算系统
摘要：
本实验主要利用的基于STM32F4开发板上MUP6050陀螺仪加速度计来做四元素的更新和四元素的姿态解算系统，开发平台为keiluvision5。
主要实现MPU6050自带数字运动处理DMP的四元素与自己编写的三子样旋转矢量算法对比的目的。
将MPU6050输出的原始数据和姿态解算数据通过串口上传到四轴上位机进行3D仿真，和用LCD屏进行数据的实时显示，并且用一个LED灯显示系统的运行和一个开关控制数据是否上传上位机。
程序是在UCOSIII操作系统中实现，建立了一个定时器，五个任务。
定时器实现四元素更新的三子样算法和姿态解算；开始任务负责创建任务；任务一实现开关对数据上传控制；任务二实现上传MPU6050输出数据和自己解算姿态到上位机；任务三实现MPU6050的四元素读取并显示到LCD上；任务四负责LCD数据和字符的显示。
关键字：
MPU6050；四元素；姿态解算；UCOSIII
1引言
现在MEMS陀螺仪和加速的计价格便宜，精度虽不如光纤和激光陀螺，但是它以价格便宜体积重量小等优势占据一定的地位，它应用的领域主要在民用手机导航小型飞行器当中。
基于MEMS惯性元件的导航可应用于个人的导航和小型飞行器的导航，研究MEMS陀螺导航具有一定的实用价值。
本实验主要利用的基于STM32F4开发板上MUP6050陀螺仪加速度计来做四元素的更新和四元素的姿态解算，开发平台为keiluvision5，将数据通过串口上传到四轴上位机进行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-1SD0与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-3TFTLCD与STM32F4的连接原理
MPU6050：
MPU6050内部整合了3轴陀螺仪和3轴加速度传感器，并且含有一个第二IIC接口，可用于连接外部磁力传感器，并利用自带的数字运动处理器（DMP:
DigitalMotionProcessor）硬件加速引擎，通过主IIC接口，向应用端输出完整的9轴融合演算数据。
MPU6050与STM32F4的连接如图3-4所示。
图3-4MPU6050与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!
!
u8KEY_Scan（u8mode）
{
staticu8key_up=1;//按键按松开标志
if（mode）key_up=1;//支持连按
if（key_up&&（KEY0==0||KEY1==0||KEY2==0||WK_UP==1））
{
delay_ms（10）;//去抖动
key_up=0;
if（KEY0==0）return1;
elseif（KEY1==0）return2;
elseif（KEY2==0）return3;
elseif（WK_UP==1）return4;
}elseif（KEY0==1&&KEY1==1&&KEY2==1&&WK_UP==0）key_up=1;
return0;//无按键按下
}
在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）;
voidLCD_ShowNum（u16x,u16y,u32num,u8len,u8size）;//显示一个数字
//显示数字,高位为0,还是显示
//x,y:
起点坐标
//num:
数值（0~999999999）;
//len:
长度（即要显示的位数）
//size:
字体大小
//mode:
//[7]:
0,不填充;1,填充0.
//[6:
1]:
保留
//[0]:
0,非叠加显示;1,叠加显示.
voidLCD_ShowxNum（u16x,u16y,u32num,u8len,u8size,u8mode）;//显示数字
//显示字符串
//x,y:
起点坐标
//width,height:
区域大小
//size:
字体大小
//*p:
字符串起始地址
voidLCD_ShowString（u16x,u16y,u16width,u16height,u8size,u8*p）;//显示一个字符串,12/16字体
MPU6050中的文件mpu6050.c和mpu6050.h实现mpu6050的驱动。
只要对mpu6050的一些基本的设置和初始化以及数据的读取。
u8MPU_Init（void）
{
u8res;
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_EN_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,PLLX轴为参考
MPU_Write_Byte（MPU_PWR_MGMT2_REG,0X00）;//加速度与陀螺仪都工作
MPU_Set_Rate（50）;//设置采样率为50Hz
}elsereturn1;
return0;
}
//得到温度值
//返回值:
温度值（扩大了100倍）
shortMPU_Get_Temperature（void）
//得到陀螺仪值（原始值），16位AD采集的值
//gx,gy,gz:
陀螺仪x,y,z轴的原始读数（带符号）
//返回值:
0,成功/其他,错误代码
u8MPU_Get_Gyroscope（short*gx,short*gy,short*gz）
//得到加速度值（原始值）
//gx,gy,gz:
陀螺仪x,y,z轴的原始读数（带符号），16位AD采集的值
//返回值:
0,成功；其他,错误代码
u8MPU_Get_Accelerometer（short*ax,short*ay,short*az）
//获得四元素
u8MPU_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,正常；其他,失败
u8mpu_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数据和字符的显示，其优先级为7，堆栈大小为128。
Main函数中主要是初始化硬件资源和UCOSIII操作系统以及创建开始任务和开始UCOSIII操作系统。
intmain（void）
{
OS_ERRerr;
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（）;//初始化MPU6050
Show_Main_UI（）;//在LCD上显式界面
OSInit（&err）;//初始化UCOSIII
OS_CRITICAL_ENTER（）;//进入临界区
//创建开始任务
OSTaskCreate（（OS_TCB*）&StartTaskTCB,//任务控制块
（CPU_CHAR*）"starttask",//任务名字
（OS_TASK_PTR）start_task,//任务函数
（void*）0,//传递给任务函数的参数
（OS_PRIO）START_TASK_PRIO,//任务优先级
（CPU_STK*）&START_TASK_STK[0],//任务堆栈基地址
（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_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_report_imu（aacx,aacy,aacz,gyrox,gyroy,gyroz,（int）（roll*100）,（int）（pitch*100）,（int）（yaw*10））;
任务一的函数如下每隔100ms进行扫描一次。
voidtask1_task（void*p_arg）
{
OS_ERRerr;
u8key;
LED1=0;
while
（1）
{
key=KEY_Scan（0）;
if（key==KEY0_PRES）
{
report=!
report;
if（report）
{
LCD_ShowString（30,170,200,16,16,"UPLOADON"）;
LED1=0;
}
else
{
LCD_ShowString（30,170,200,16,16,"UPLOADOFF"）;
LED1=1;
}
}
OSTimeDlyHMSM（0,0,0,100,OS_OPT_TIME_HMSM_STRICT,&err）;//延时100ms
}
}
对于在LCD上姿态角的显示，结果只精确到小数点后的一位。
可以先定义一个短整型的temp，将要显示的姿态角负值给，它然后将其扩大10倍，分为整数部分和小数部分分别显示。
任务二的服务函数如下。
voidtask2_task（void*p_arg）
{
OS_ERRerr;
floatpitch,roll,yaw;//欧拉角
shortaacx,aacy,aacz;//加速度传感器原始数据
shortgyrox,gyroy,gyroz;//陀螺仪原始数据
shorttemp;//温度
u8t=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,aacz,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（temp<0）
{
LCD_ShowChar（30+48,200,'-',16,0）;//显示负号
temp=-temp;//转为正数
}elseLCD_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（temp<0）
{
LCD_ShowChar（30+48,220,'-',16,0）;//显示负号
temp=-temp;//转为正数
}elseLCD_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（temp<0）
{
LCD_ShowChar（30+48,240,'-',16,0）;//显示负号
temp=-temp;//转为正数
}elseLCD_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（temp<0）
{
LCD_ShowChar（30+48,260,'-',16,0）;//显示负号
temp=-temp;//转为正数
}elseLCD_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++;
OSTimeDlyHMSM（0,0,0,10,OS_OPT_TIME_HMSM_STRICT,&err）;//延时10ms
}
}
由于四元素的四个数都是大于0小于1，所以借鉴前面的经验，只需要显示小数部分。
将q扩大100000000倍后变成整数显示在LCD上。
任务3的函数如下：
voidtask3_task（void*p_arg）
{
floatquat[4]={0,0,0,0};
OS_ERRerr;
CPU_SR_ALLOC（）;
OS_CRITICAL_ENTER（）;
LCD_ShowString（30,300,200,16,16,"OrinalQuaternion:
"）;
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,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）;//读取MPU6050DMP处理后的数据
{
printf（"四元素为q0=%fq1=%fq2=%fq3=%f\r\n",*quat,*（quat+1）,*（quat+2）,*（quat+3））;
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上显示一些字符，只执行一次。
任务四的服务函数如下：
voidtask4_task（void*p_arg）
{
POINT_COLOR=RED;//设置字体为红色
LCD_ShowString（30,340,200,16,16,"Clculate
升级会员 