基于mspf的MPU测角

1、这个程序完成的功能为：使用msp430f5529在12864上串行显示GY-521，MPU6050所测量的角度。在IAR亲测成功。注意：我只给出了C文件 ，h文件自己去建立就好了。/*主函数*/#include #include stdio.h#include math.h#include 6050.h#include LCD12864.hvoid Delays( uchar i) unsigned int j; while(i-) j=2000; while(j-); void main(void) WDTCTL = WDTPW + WDTHOLD; / 关闭看门狗 char sum110,

2、sum210,sum310; / 串口发送缓存 float a_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,%

3、.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-6 V1.0*/#include msp430f5529.h/#include mytype.h#include 6050.hstatic void I2C_Start();static void I2C_St

4、op();static void I2C_SendACK(uchar ack);static uchar I2C_RecvACK();static void I2C_SendByte(uchar dat);static uchar I2C_RecvACK();short accData3=0;/*/I2C起始信号/*void I2C_Start() MPU_SCL_OUT(); / SCL设置为输出 MPU_SDA_OUT(); / SDA设置为输出 MPU_SDA_H(); /拉高数据线 MPU_SCL_H(); /拉高时钟线 DELAY_US(5); /延时 MPU_SDA_L(); /产

5、生下降沿 DELAY_US(5); /延时 MPU_SCL_L(); /拉低时钟线/*/I2C停止信号/*void I2C_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:ACK 1:NAK)/*void I2C_SendACK(uchar ack) MPU_SCL_OUT(); / SCL设

6、置为输出 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接收应答信号/*uchar I2C_RecvACK() uchar cy; MPU_SCL_OUT(); / SCL设置为输出 MPU_SDA_IN(); / SDA设置为输入 MPU_SCL_H(); /拉高时钟线 DELAY_US(5); /延时 if(MPU

7、_SDA_DAT() cy=1; else cy=0; / cy = SDA; /读应答信号 MPU_SCL_L(); /拉低时钟线 DELAY_US(5); /延时 MPU_SDA_OUT(); / SDA设置为输出 return cy; /*/向I2C总线发送一个字节数据/*void I2C_SendByte(uchar dat) uchar i; MPU_SCL_OUT(); / SCL设置为输出 MPU_SDA_OUT(); / SDA设置为输出 for (i=0; i8; i+) /8位计数器 if(dati)&0x80) MPU_SDA_H(); else MPU_SDA_L();

8、 / SDA = cy; /送数据口 MPU_SCL_H(); /拉高时钟线 DELAY_US(5); /延时 MPU_SCL_L(); /拉低时钟线 DELAY_US(5); /延时 I2C_RecvACK();/*/从I2C总线接收一个字节数据/*uchar I2C_RecvByte() uchar i; uchar dat = 0,cy; MPU_SCL_OUT(); / SCL设置为输出 MPU_SDA_OUT(); / SDA设置为输出 MPU_SDA_H(); /使能内部上拉,准备读取数据, MPU_SDA_IN(); / SDA设置为输入，准备向主机输入数据 for (i=0;

9、i8; 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(); return dat;/*/向I2C设备写入一个字节数据/*void ByteWrite6050(uchar REG_Address,uchar REG_data) I2C_Start(); /起始信号 I2C_SendByte(SlaveAddress); /发送设备地址

10、+写信号 I2C_SendByte(REG_Address); /内部寄存器地址， I2C_SendByte(REG_data); /内部寄存器数据， I2C_Stop(); /发送停止信号/*/从I2C设备读取一个字节数据/*uchar ByteRead6050(uchar REG_Address)uchar REG_data;I2C_Start(); /起始信号I2C_SendByte(SlaveAddress); /发送设备地址+写信号I2C_SendByte(REG_Address); /发送存储单元地址，从0开始I2C_Start(); /起始信号I2C_SendByte(Slave

11、Address+1); /发送设备地址+读信号REG_data=I2C_RecvByte(); /读出寄存器数据I2C_SendACK(1); /接收应答信号I2C_Stop(); /停止信号return REG_data;/*/合成数据/*int Get6050Data(uchar REG_Address)char H,L;H=ByteRead6050(REG_Address);L=ByteRead6050(REG_Address+1);return (H=sinx,故乘以1.2适当放大*返回参数：测量的倾角值*传入参数：dir - 需要测量的方向* ACCEL_XOUT - X方向* AC

12、CEL_YOUT - Y方向* ACCEL_ZOUT - Z方向*/float Mpu6050AccelAngle(uchar dir) float accel_agle;/ 测量的倾角值 float result; / 测量值缓存变量 result = (float)Get6050Data(dir); / 测量当前方向的加速度值,转换为浮点数 accel_agle = (result + MPU6050_ZERO_ACCELL); / 去除零点偏移,计算得到角度（弧度）/ accel_agle = accel_agle*1.2*180/3.14; /弧度转换为度 return accel_a

13、gle; / 返回测量值/*函数名 ：float Mpu6050GyroAngle(int8 dir)*函数功能：输出陀螺仪测量的倾角加速度* 范围为2000deg/s时，换算关系：16.4 LSB/(deg/s)*返回参数：测量的倾角加速度值*传入参数：dir - 需要测量的方向* GYRO_XOUT - X轴方向* GYRO_YOUT - Y轴方向* GYRO_ZOUT - Z轴方向*/float Mpu6050GyroAngle(uchar dir) float gyro_angle;/ float Angle_gy; gyro_angle = (float)Get6050Data(di

14、r); / 检测陀螺仪的当前值 gyro_angle = -(gyro_angle + MPU6050_ZERO_GYRO)/16.4; /去除零点偏移，计算角速度值,负号为方向处理 / Angle_gy += gyro_angle*0.005; return gyro_angle; / 返回测量值/采样10次去掉两个最大最小值求平均void MPU6050ReadAcc() int i=0,j=0;int x_buf10;int y_buf10;int z_buf10;int temp=0;long temp2=0;for(i=0;i10;i+)x_bufi = Get6050Data(AC

15、CEL_XOUT);y_bufi = Get6050Data(ACCEL_YOUT);z_bufi = Get6050Data(ACCEL_ZOUT);for(i=0;i9;i+)for(j=i+1;jx_bufi)temp=x_bufj;x_bufj=x_bufi;x_bufi=temp;if(y_bufjy_bufi)temp=y_bufj;y_bufj=y_bufi;y_bufi=temp;if(z_bufjz_bufi)temp=z_bufj;z_bufj=z_bufi;z_bufi=temp;temp2=0;for(i=2;i8;i+)temp2=temp2+x_bufi;accDa

16、ta0=temp2/6;temp2=0;for(i=2;i8;i+)temp2=temp2+y_bufi;accData1=temp2/6;temp2=0;for(i=2;i8;i+)temp2=temp2+z_bufi;accData2=temp2/6;/得到角度/x,y,z:x,y,z方向的重力加速度分量(不需要单位,直接数值即可)/dir:要获得的角度.0,与x轴的角度;1,与Y轴的角度;2,与Z轴的角度./返回值:角度值.单位0.1.float mpu6050_Angle(uchar dir)float temp,x,y,z; float res=0; MPU6050ReadAcc()

17、; /得到accData值 x = accData0; y = accData1; z = accData2; switch(dir)case 0:/与自然Z轴的角度 temp=sqrt(x*x+y*y)/z; res=atan(temp); break;case 1:/与自然X轴的角度 temp=x/sqrt(y*y+z*z); res=atan(temp); break; case 2:/与自然Y轴的角度 temp=y/sqrt(x*x+z*z); res=atan(temp); break; return (res*180/3.14)+0;/设置平放的基础角度值/*LCD12864*/#

18、include #include LCD12864.h#define uchar unsigned char#define uint unsigned intextern unsigned char ADS1256_buf9;void int_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* 输出 : 无*/vo

19、id delay(uint t) /延时函数 /粗略延时，满足时序要求 uint i,j; for(i=0; it; i+) for(j=0; j10; j+);/* 名称 : sendbyte()* 功能 : 按照液晶的串口通信协议，发送数据* 输入 : zdata* 输出 : 无*/void sendbyte(uchar zdata) /数据传送函数 uchar code_seg7; uchar serial_clk; uchar serial_shift; code_seg7=zdata; serial_shift=0x80; for(serial_clk=0; serial_clk1;

20、 /准备发送下一位数据 /* 名称 : write_com()* 功能 : 写串口指令* 输入 : cmdcode* 输出 : 无*/void write_com(unsigned char cmdcode) /写命令函数 /串口控制格式（11111AB0） /A数据方向控制，A=H时读，A=L时写 /B数据类型选择，B=H时为显示数据，B=L时为命令 sendbyte(0xf8); /MCU向LCD发命令 sendbyte(cmdcode & 0xf0); /发高四位数据(数据格式D7D6D5D4_0000) sendbyte(cmdcode 4) & 0xf0);/发低四位数据(数据格式D

21、3D2D1D0_0000) delay(2); /延时等待/* 名称 : write_data()* 功能 : 写串口数据* 输入 : Dispdata* 输出 : 无*/void write_data(uchar Dispdata)/写数据函数 sendbyte(0xfa); /MCU向LCD发数据 sendbyte(Dispdata & 0xf0); /发高四位数据 sendbyte(Dispdata 4) & 0xf0);/发低四位数据 delay(2);/* 名称 : lcdinit()* 功能 : 初始化函数* 输入 : cmdcode* 输出 : 无*/void lcdinit() /LCD初始化 delay(20000); /复位等待（内部自带上电复位电路），时间较长 write_com(0x30); /功能设定：基本指令集操作 delay(50); /延时等待 write_com(0x0c); /整体显示，关游标 delay(50); write_com(0x01); /清屏 delay(50); write_com(0x06); /入口设置 delay(50);/* 函数名称 : LCD_12864_address_s