基于mspf的MPU测角度.docx

基于mspf的MPU测角度.docx

《基于mspf的MPU测角度.docx》由会员分享，可在线阅读，更多相关《基于mspf的MPU测角度.docx（18页珍藏版）》请在冰豆网上搜索。

基于mspf的MPU测角度

　　这个程序完成的功能为：

使用msp430f5529在12864上串行显示GY-521，MPU6050所测量的角度。

　　在IAR亲测成功。

　　注意：

我只给出了C文件，h文件自己去建立就好了。

　　////*******主函数******///

　　#include

　　#include"stdio.h"

　　#include"math.h"

　　#include"6050.h"

　　#include"LCD12864.h"

　　voidDelays（uchari）

　　{

　　unsignedintj;

　　while（i--）

　　{

　　j=2000;

　　while（j--）;

　　}

　　}

　　voidmain（void）

　　{

　　WDTCTL=WDTPW+WDTHOLD;//关闭看门狗

　　charsum1[10],sum2[10],sum3[10];//串口发送缓存

　　floata_x,a_y,a_z;

　　int_port（）;//管脚初始化

　　lcdinit（）;

　　InitMPU6050（）;//初始化模块

　　display（1,1,"角度X:

"）;

　　display（2,1,"角度Y:

"）;

　　display（3,1,"角度Z:

"）;

　　while

（1）

　　{

　　//Delays

（2）;

　　a_x=mpu6050_Angle

（2）;

　　a_y=mpu6050_Angle

（1）;

　　a_z=mpu6050_Angle（0）;

　　sprintf（sum1,"%.2f",a_x）;//将测量倾角值转换为字符串

　　sprintf（sum2,"%.2f",a_y）;

　　sprintf（sum3,"%.2f",a_z）;

　　display（1,4,sum1）;

　　display（2,4,sum2）;

　　display（3,4,sum3）;

　　}

　　}

　　////************6050IIC******////

　　/*

　　***************************************************************************

　　**文件名：

Mpu-6050.c

　　**编写者：

黄建军

　　**描述：

三轴加速度，三轴陀螺仪传感器Mpu-6050的驱动程序，此处用于149系列。

　　**注意-此处MCLK：

8Mhz

　　**版本：

2013-6V1.0

　　****************************************************************************

　　*/

　　#include"msp430f5529.h"

　　//#include"mytype.h"

　　#include"6050.h"

　　staticvoidI2C_Start（）;

　　staticvoidI2C_Stop（）;

　　staticvoidI2C_SendACK（ucharack）;

　　staticucharI2C_RecvACK（）;

　　staticvoidI2C_SendByte（uchardat）;

　　staticucharI2C_RecvACK（）;

　　shortaccData[3]={0};

　　//**************************************

　　//I2C起始信号

　　//**************************************

　　voidI2C_Start（）

　　{

　　MPU_SCL_OUT（）;//SCL设置为输出

　　MPU_SDA_OUT（）;//SDA设置为输出

　　MPU_SDA_H（）;//拉高数据线

　　MPU_SCL_H（）;//拉高时钟线

　　DELAY_US（5）;//延时

　　MPU_SDA_L（）;//产生下降沿

　　DELAY_US（5）;//延时

　　MPU_SCL_L（）;//拉低时钟线

　　}

　　//**************************************

　　//I2C停止信号

　　//**************************************

　　voidI2C_Stop（）

　　{

　　MPU_SCL_OUT（）;//SCL设置为输出

　　MPU_SDA_OUT（）;//SDA设置为输出

　　MPU_SDA_L（）;//拉低数据线

　　MPU_SCL_H（）;//拉高时钟线

　　DELAY_US（5）;//延时

　　MPU_SDA_H（）;//产生上升沿

　　DELAY_US（5）;//延时

　　}

　　//**************************************

　　//I2C发送应答信号

　　//入口参数:

ack（0:

ACK1:

NAK）

　　//**************************************

　　voidI2C_SendACK（ucharack）

　　{

　　MPU_SCL_OUT（）;//SCL设置为输出

　　MPU_SDA_OUT（）;//SDA设置为输出

　　if（ack）

　　MPU_SDA_H（）;

　　else

　　MPU_SDA_L（）;

　　//SDA=ack;//写应答信号

　　MPU_SCL_H（）;//拉高时钟线

　　DELAY_US（5）;//延时

　　MPU_SCL_L（）;//拉低时钟线

　　DELAY_US（5）;//延时

　　}

　　//**************************************

　　//I2C接收应答信号

　　//**************************************

　　ucharI2C_RecvACK（）

　　{

　　ucharcy;

　　MPU_SCL_OUT（）;//SCL设置为输出

　　MPU_SDA_IN（）;//SDA设置为输入

　　MPU_SCL_H（）;//拉高时钟线

　　DELAY_US（5）;//延时

　　if（MPU_SDA_DAT（））

　　{

　　cy=1;

　　}

　　else

　　{

　　cy=0;

　　}

　　//cy=SDA;//读应答信号

　　MPU_SCL_L（）;//拉低时钟线

　　DELAY_US（5）;//延时

　　MPU_SDA_OUT（）;//SDA设置为输出

　　returncy;

　　}

　　//**************************************

　　//向I2C总线发送一个字节数据

　　//**************************************

　　voidI2C_SendByte（uchardat）

　　{

　　uchari;

　　MPU_SCL_OUT（）;//SCL设置为输出

　　MPU_SDA_OUT（）;//SDA设置为输出

　　for（i=0;i<8;i++）//8位计数器

　　{

　　if（（dat<

　　{

　　MPU_SDA_H（）;

　　}

　　else

　　{

　　MPU_SDA_L（）;

　　}

　　//SDA=cy;//送数据口

　　MPU_SCL_H（）;//拉高时钟线

　　DELAY_US（5）;//延时

　　MPU_SCL_L（）;//拉低时钟线

　　DELAY_US（5）;//延时

　　}

　　I2C_RecvACK（）;

　　}

　　//**************************************

　　//从I2C总线接收一个字节数据

　　//**************************************

　　ucharI2C_RecvByte（）

　　{

　　uchari;

　　uchardat=0,cy;

　　MPU_SCL_OUT（）;//SCL设置为输出

　　MPU_SDA_OUT（）;//SDA设置为输出

　　MPU_SDA_H（）;//使能内部上拉,准备读取数据,

　　MPU_SDA_IN（）;//SDA设置为输入，准备向主机输入数据

　　for（i=0;i<8;i++）//8位计数器

　　{

　　dat<<=1;

　　MPU_SCL_H（）;//拉高时钟线

　　DELAY_US（5）;//延时

　　if（MPU_SDA_DAT（））

　　{

　　cy=1;

　　}

　　else

　　{

　　cy=0;

　　}

　　dat|=cy;//读数据

　　MPU_SCL_L（）;//拉低时钟线

　　DELAY_US（5）;//延时

　　}

　　MPU_SDA_OUT（）;

　　returndat;

　　}

　　//**************************************

　　//向I2C设备写入一个字节数据

　　//**************************************

　　voidByteWrite6050（ucharREG_Address,ucharREG_data）

　　{

　　I2C_Start（）;//起始信号

　　I2C_SendByte（SlaveAddress）;//发送设备地址+写信号

　　I2C_SendByte（REG_Address）;//内部寄存器地址，

　　I2C_SendByte（REG_data）;//内部寄存器数据，

　　I2C_Stop（）;//发送停止信号

　　}

　　//**************************************

　　//从I2C设备读取一个字节数据

　　//**************************************

　　ucharByteRead6050（ucharREG_Address）

　　{

　　ucharREG_data;

　　I2C_Start（）;//起始信号

　　I2C_SendByte（SlaveAddress）;//发送设备地址+写信号

　　I2C_SendByte（REG_Address）;//发送存储单元地址，从0开始

　　I2C_Start（）;//起始信号

　　I2C_SendByte（SlaveAddress+1）;//发送设备地址+读信号

　　REG_data=I2C_RecvByte（）;//读出寄存器数据

　　I2C_SendACK

（1）;//接收应答信号

　　I2C_Stop（）;//停止信号

　　returnREG_data;

　　}

　　//**************************************

　　//合成数据

　　//**************************************

　　intGet6050Data（ucharREG_Address）

　　{

　　charH,L;

　　H=ByteRead6050（REG_Address）;

　　L=ByteRead6050（REG_Address+1）;

　　return（H<<8）+L;//合成数据

　　}

　　//**************************************

　　//初始化MPU6050

　　//**************************************

　　voidInitMPU6050（）

　　{

　　ByteWrite6050（PWR_MGMT_1,0x00）;//解除休眠状态

　　ByteWrite6050（SMPLRT_DIV,0x07）;//陀螺仪采样率设置（125HZ）

　　ByteWrite6050（CONFIG,0x06）;//低通滤波器频率设置（5HZ）

　　ByteWrite6050（GYRO_CONFIG,0x18）;//陀螺仪自检及检测范围设置（不自检,16.4LSB/DBS/S）

　　ByteWrite6050（ACCEL_CONFIG,0x01）;//加速计自检、测量范围及高通滤波频率（不自检，2G（16384LSB/G），5Hz）

　　}

　　/*

　　**********************************************

　　**函数名：

floatMpu6050AccelAngle（int8dir）

　　**函数功能：

输出加速度传感器测量的倾角值

　　**范围为2g时，换算关系：

16384LSB/g

　　**角度较小时，x=sinx得到角度（弧度）,deg=rad*180/3.14

　　**因为x>=sinx,故乘以1.2适当放大

　　**返回参数：

测量的倾角值

　　**传入参数：

dir-需要测量的方向

　　**ACCEL_XOUT-X方向

　　**ACCEL_YOUT-Y方向

　　**ACCEL_ZOUT-Z方向

　　**********************************************

　　*/

　　floatMpu6050AccelAngle（uchardir）

　　{

　　floataccel_agle;//测量的倾角值

　　floatresult;//测量值缓存变量

　　result=（float）Get6050Data（dir）;//测量当前方向的加速度值,转换为浮点数

　　accel_agle=（result+MPU6050_ZERO_ACCELL）;//去除零点偏移,计算得到角度（弧度）

　　//accel_agle=accel_agle*1.2*180/3.14;//弧度转换为度

　　returnaccel_agle;//返回测量值

　　}

　　/*

　　**********************************************

　　**函数名：

floatMpu6050GyroAngle（int8dir）

　　**函数功能：

输出陀螺仪测量的倾角加速度

　　**范围为2000deg/s时，换算关系：

16.4LSB/（deg/s）

　　**返回参数：

测量的倾角加速度值

　　**传入参数：

dir-需要测量的方向

　　**GYRO_XOUT-X轴方向

　　**GYRO_YOUT-Y轴方向

　　**GYRO_ZOUT-Z轴方向

　　**********************************************

　　*/

　　floatMpu6050GyroAngle（uchardir）

　　{

　　floatgyro_angle;

　　//floatAngle_gy;

　　gyro_angle=（float）Get6050Data（dir）;//检测陀螺仪的当前值

　　gyro_angle=-（gyro_angle+MPU6050_ZERO_GYRO）/16.4;//去除零点偏移，计算角速度值,负号为方向处理

　　//Angle_gy+=gyro_angle*0.005;

　　returngyro_angle;//返回测量值

　　}

　　//采样10次去掉两个最大最小值求平均

　　voidMPU6050ReadAcc（）

　　{

　　inti=0,j=0;

　　intx_buf[10];

　　inty_buf[10];

　　intz_buf[10];

　　inttemp=0;

　　longtemp2=0;

　　for（i=0;i<10;i++）

　　{

　　x_buf[i]=Get6050Data（ACCEL_XOUT）;

　　y_buf[i]=Get6050Data（ACCEL_YOUT）;

　　z_buf[i]=Get6050Data（ACCEL_ZOUT）;

　　}

　　for（i=0;i<9;i++）

　　{

　　for（j=i+1;j<10;j++）

　　{

　　if（x_buf[j]>x_buf[i]）

　　{

　　temp=x_buf[j];

　　x_buf[j]=x_buf[i];

　　x_buf[i]=temp;

　　}

　　if（y_buf[j]>y_buf[i]）

　　{

　　temp=y_buf[j];

　　y_buf[j]=y_buf[i];

　　y_buf[i]=temp;

　　}

　　if（z_buf[j]>z_buf[i]）

　　{

　　temp=z_buf[j];

　　z_buf[j]=z_buf[i];

　　z_buf[i]=temp;

　　}

　　}

　　}

　　temp2=0;

　　for（i=2;i<8;i++）

　　{

　　temp2=temp2+x_buf[i];

　　}

　　accData[0]=temp2/6;

　　temp2=0;

　　for（i=2;i<8;i++）

　　{

　　temp2=temp2+y_buf[i];

　　}

　　accData[1]=temp2/6;

　　temp2=0;

　　for（i=2;i<8;i++）

　　{

　　temp2=temp2+z_buf[i];

　　}

　　accData[2]=temp2/6;

　　}

　　//得到角度

　　//x,y,z:

x,y,z方向的重力加速度分量（不需要单位,直接数值即可）

　　//dir:

要获得的角度.0,与x轴的角度;1,与Y轴的角度;2,与Z轴的角度.

　　//返回值:

角度值.单位0.1°.

　　floatmpu6050_Angle（uchardir）

　　{

　　floattemp,x,y,z;

　　floatres=0;

　　MPU6050ReadAcc（）;//得到accData[]值

　　x=accData[0];

　　y=accData[1];

　　z=accData[2];

　　switch（dir）

　　{

　　case0:

//与自然Z轴的角度

　　temp=sqrt（（x*x+y*y））/z;

　　res=atan（temp）;

　　break;

　　case1:

//与自然X轴的角度

　　temp=x/sqrt（（y*y+z*z））;

　　res=atan（temp）;

　　break;

　　case2:

//与自然Y轴的角度

　　temp=y/sqrt（（x*x+z*z））;

　　res=atan（temp）;

　　break;

　　}

　　return（res*180/3.14）+0;//设置平放的基础角度值

　　}

　　////******LCD12864*****///

　　#include

　　#include"LCD12864.h"

　　#defineucharunsignedchar

　　#defineuintunsignedint

　　externunsignedcharADS1256_buf[9];

　　voidint_port（void）//管脚初始化

　　{

　　P2SEL&=~BIT4;//P2.4模拟SID，设置为i/o口输出方向

　　P2DIR|=BIT4;

　　P2OUT&=~BIT4;

　　P2SEL&=~BIT5;//P2.5模拟SCLK，设置为i/o口输出方向

　　P2DIR|=BIT5;

　　P2OUT&=~BIT5;

　　}

　　/********************************************************************

　　*名称:

delay（）

　　*

　　*输入:

t

　　*输出:

无

　　***********************************************************************/

　　voiddelay（uintt）//延时函数

　　{//粗略延时，满足时序要求

　　uinti,j;

　　for（i=0;i

　　for（j=0;j<10;j++）;

　　}

　　/********************************************************************

　　*名称:

sendbyte（）

　　*功能:

按照液晶的串口通信协议，发送数据

　　*输入:

zdata

　　*输出:

无

　　***********************************************************************/

　　voidsendbyte（ucharzdata）//数据传送函数

　　{

　　ucharcode_seg7;

　　ucharserial_clk;

　　ucharserial_shift;

　　code_seg7=zdata;

　　serial_shift=0x80;

　　for（serial_clk=0;serial_clk<8;serial_clk++）

　　{

　　if（code_seg7&serial_shift）

　　{

　　P2OUT|=BIT4;//SID为1

　　}

　　else

　　{

　　P2OUT&=~BIT4;//SID为0

　　}

　　P2OUT&=~BIT5;//产生时钟信号下沿

　　P2OUT|=BIT5;//产生时钟信号上沿

　　serial_shift=serial_shift>>1;//准备发送下一位数据

　　}

　　}

　　/********************************************************************

　　*名称:

write_com（）

　　*功能:

写串口指令

　　*输入:

cmdcode

　　*输出:

无

　　***********************************************************************/

　　voidwrite_com（unsignedcharcmdcode）//写命令函数

　　{

　　//串口控制格式（11111AB0）

　　//A数据方向控制，A=H时读，A=L时写

　　//B数据类型选择，B=H时为显示数据，B=L时为命令

　　sendbyte（0xf8）;//MCU向LCD发命令

　　sendbyte（cmdcode&0xf0）;//发高四位数据（数据格式D7D6D5D4_0000）

　　sendbyte（（cmdcode<<4）&0xf0）;//发低四位数据（