mpu6050俯仰角和横滚角解算

来源：互联网发布：数据分析的职业规划编辑：程序博客网时间：2024/05/01 20:48

keil中的工程中的所有文件
rp.c

#include <REG51.H>#include <math.h>    //Keil library  #include <stdio.h>   //Keil library#include <INTRINS.H>#include "i2c.h"#include "mpu6050.h"//****************************************//定义类型及变量//****************************************float ax,ay,az;int pitch,roll;uchar dis[6];intdis_data;//变量//****************************************//函数声明//****************************************void  delay(unsigned int k);//延时void  lcd_printf(uchar *s,int temp_data);//MPU6050操作函数void  InitMPU6050();//初始化MPU6050//****************************************//整数转字符串//****************************************void lcd_printf(char *s,int temp_data){temp_data-=180;//改动一下这里就可以了，不知道为什么？if(temp_data<0){temp_data=-temp_data;*s='-';}else *s=' ';*++s =temp_data/10000+0x30;temp_data=temp_data%10000;     //取余运算*++s =temp_data/1000+0x30;temp_data=temp_data%1000;    *++s =temp_data/100+0x30;temp_data=temp_data%100;     *++s =temp_data/10+0x30;temp_data=temp_data%10;      *++s =temp_data+0x30;}//****************************************void  SeriPushSend(uchar send_data){    SBUF=send_data;  while(!TI);TI=0;  }//**************************************//延时5微秒(STC90C52RC@12M),但是11.0592也能用//不同的工作环境,需要调整此函数//当改用1T的MCU时,请调整此延时函数//**************************************void Delay5us(){_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();}//**************************************//初始化MPU6050//**************************************void InitMPU6050(){Single_WriteI2C(PWR_MGMT_1, 0x00);//解除休眠状态Single_WriteI2C(SMPLRT_DIV, 0x07);Single_WriteI2C(CONFIG, 0x06);Single_WriteI2C(ACCEL_CONFIG, 0x01);}//**************************************//合成数据//**************************************int GetData(uchar REG_Address){uchar H,L;H=Single_ReadI2C(REG_Address);L=Single_ReadI2C(REG_Address+1);return (H<<8)+L;   //合成数据}void Display10BitData(int value){  char i;lcd_printf(dis, value);//转换数据显示for(i=0;i<6;i++){    SeriPushSend(dis[i]); }}void init_uart(){TMOD=0x21;TH1=0xfd;TL1=0xfd;SCON=0x50;PS=1;      //串口中断设为高优先级别TR0=1;   //启动定时器TR1=1;ET0=1;     //打开定时器0中断ES=1;EA=1;}//*********************************************************//主程序//*********************************************************void main(){ float rr,pr;delay(500);//上电延时init_uart();InitMPU6050();//初始化MPU6050delay(150);while(1){ax=(float)GetData(ACCEL_XOUT_H);ay=(float)GetData(ACCEL_YOUT_H);az=(float)GetData(ACCEL_ZOUT_H);pr=-ax/sqrt(az*az+ay*ay);rr=ay/sqrt(az*az+ax*ax);pitch=(int)(((atan(pr)*180)/3.1415926)+180); //为什么不加180就不行呢？roll=(int)((((atan(rr)*180)/3.1415926)+180));Display10BitData(pitch);Display10BitData(roll);SeriPushSend(0x0d); SeriPushSend(0x0a);//换行，回车delay(100);}}

i2c.c

#include"i2c.h"//****************************************//延时//****************************************void delay(unsigned int k){unsigned int i,j;for(i=0;i<k;i++){for(j=0;j<121;j++);}}//**************************************//I2C起始信号//**************************************void I2C_Start(){    SDA = 1;                    //拉高数据线    SCL = 1;                    //拉高时钟线    Delay5us();                 //延时    SDA = 0;                    //产生下降沿    Delay5us();                 //延时    SCL = 0;                    //拉低时钟线}//**************************************//I2C停止信号//**************************************void I2C_Stop(){    SDA = 0;                    //拉低数据线    SCL = 1;                    //拉高时钟线    Delay5us();                 //延时    SDA = 1;                    //产生上升沿    Delay5us();                 //延时}//**************************************//I2C发送应答信号//入口参数:ack (0:ACK 1:NAK)//**************************************void I2C_SendACK(bit ack){    SDA = ack;                  //写应答信号    SCL = 1;                    //拉高时钟线    Delay5us();                 //延时    SCL = 0;                    //拉低时钟线    Delay5us();                 //延时}//**************************************//I2C接收应答信号//**************************************bit I2C_RecvACK(){    SCL = 1;                    //拉高时钟线    Delay5us();                 //延时    CY = SDA;                   //读应答信号    SCL = 0;                    //拉低时钟线    Delay5us();                 //延时    return CY;}//**************************************//向I2C总线发送一个字节数据//**************************************void I2C_SendByte(uchar dat){    uchar i;    for (i=0; i<8; i++)         //8位计数器    {        dat <<= 1;              //移出数据的最高位        SDA = CY;               //送数据口        SCL = 1;                //拉高时钟线        Delay5us();             //延时        SCL = 0;                //拉低时钟线        Delay5us();             //延时    }    I2C_RecvACK();}//**************************************//从I2C总线接收一个字节数据//**************************************uchar I2C_RecvByte(){    uchar i;    uchar dat = 0;    SDA = 1;                    //使能内部上拉,准备读取数据,    for (i=0; i<8; i++)         //8位计数器    {        dat <<= 1;        SCL = 1;                //拉高时钟线        Delay5us();             //延时        dat |= SDA;             //读数据                       SCL = 0;                //拉低时钟线        Delay5us();             //延时    }    return dat;}//**************************************//向I2C设备写入一个字节数据//**************************************void Single_WriteI2C(uchar REG_Address,uchar REG_data){    I2C_Start();                  //起始信号    I2C_SendByte(SlaveAddress);   //发送设备地址+写信号    I2C_SendByte(REG_Address);    //内部寄存器地址，    I2C_SendByte(REG_data);       //内部寄存器数据，    I2C_Stop();                   //发送停止信号}//**************************************//从I2C设备读取一个字节数据//**************************************uchar Single_ReadI2C(uchar REG_Address){uchar REG_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();                    //停止信号return REG_data;}

i2c.h

#include<reg51.h>#ifndef __I2C_H_#define __I2C_H_typedef  unsigned char uchar;typedef  unsigned short ushort;typedef unsigned int uint;#define SlaveAddress 0xD0//IIC写入时的地址字节数据，+1为读取sbit    SCL=P2^2;//IIC时钟引脚定义sbit    SDA=P2^3;//IIC数据引脚定义void  I2C_Start();void  I2C_Stop();void  I2C_SendACK(bit ack);bit   I2C_RecvACK();void  I2C_SendByte(uchar dat);uchar I2C_RecvByte();void  I2C_ReadPage();void  I2C_WritePage();uchar Single_ReadI2C(uchar REG_Address);//读取I2C数据void  Delay5us();void  Single_WriteI2C(uchar REG_Address,uchar REG_data);//向I2C写入数据 #endif

mpu6050.h

#ifndef __MPU6050_H#define __MPU6050_H//****************************************// 定义MPU6050内部地址//****************************************#define SMPLRT_DIV 0x19//陀螺仪采样率，典型值：0x07(125Hz)#define CONFIG 0x1A//低通滤波频率，典型值：0x06(5Hz)#define ACCEL_CONFIG 0x1C//加速计自检、测量范围及高通滤波频率，典型值：0x01(不自检，2G，5Hz)#define ACCEL_XOUT_H    0x3B#define ACCEL_XOUT_L    0x3C#define ACCEL_YOUT_H 0x3D#define ACCEL_YOUT_L 0x3E#define ACCEL_ZOUT_H 0x3F#define ACCEL_ZOUT_L 0x40#define PWR_MGMT_1 0x6B//电源管理，典型值：0x00(正常启用)#define WHO_AM_I 0x75//IIC地址寄存器(默认数值0x68，只读)#endif

0 0