MPU6050与51单片机通信并计算俯仰角，反转角，航向角的计算代码

以下是MPU6050与51单片机通信并计算俯仰角，反转角，航向角的计算代码：

#include <reg52.h>
#include <intrins.h>

#define MPU6050_ADDRESS 0xD0
#define MPU6050_SMPLRT_DIV 0x19
#define MPU6050_CONFIG 0x1A
#define MPU6050_GYRO_CONFIG 0x1B
#define MPU6050_ACCEL_CONFIG 0x1C
#define MPU6050_ACCEL_XOUT_H 0x3B
#define MPU6050_ACCEL_XOUT_L 0x3C
#define MPU6050_ACCEL_YOUT_H 0x3D
#define MPU6050_ACCEL_YOUT_L 0x3E
#define MPU6050_ACCEL_ZOUT_H 0x3F
#define MPU6050_ACCEL_ZOUT_L 0x40
#define MPU6050_TEMP_OUT_H 0x41
#define MPU6050_TEMP_OUT_L 0x42
#define MPU6050_GYRO_XOUT_H 0x43
#define MPU6050_GYRO_XOUT_L 0x44
#define MPU6050_GYRO_YOUT_H 0x45
#define MPU6050_GYRO_YOUT_L 0x46
#define MPU6050_GYRO_ZOUT_H 0x47
#define MPU6050_GYRO_ZOUT_L 0x48

sbit SCL=P1^1;
sbit SDA=P1^0;

void delay(unsigned int t)
{
    while(t--);
}

void IIC_Start()
{
    SDA=1;
    SCL=1;
    _nop_();
    _nop_();
    _nop_();
    _nop_();
    SDA=0;
    _nop_();
    _nop_();
    _nop_();
    _nop_();
    SCL=0;
}

void IIC_Stop()
{
    SDA=0;
    SCL=1;
    _nop_();
    _nop_();
    _nop_();
    _nop_();
    SDA=1;
    _nop_();
    _nop_();
    _nop_();
    _nop_();
}

void IIC_SendByte(unsigned char dat)
{
    unsigned char i;
    for(i=0;i<8;i++)
    {
        SDA=(dat&0x80)>>7;
        dat<<=1;
        SCL=1;
        _nop_();
        _nop_();
        _nop_();
        _nop_();
        SCL=0;
    }
}

unsigned char IIC_RecvByte()
{
    unsigned char i,dat=0;
    SDA=1;
    for(i=0;i<8;i++)
    {
        dat<<=1;
        SCL=1;
        _nop_();
        _nop_();
        _nop_();
        _nop_();
        if(SDA) dat|=0x01;
        SCL=0;
    }
    return dat;
}

void IIC_WriteByte(unsigned char addr,unsigned char dat)
{
    IIC_Start();
    IIC_SendByte(MPU6050_ADDRESS);
    IIC_SendByte(addr);
    IIC_SendByte(dat);
    IIC_Stop();
}

unsigned char IIC_ReadByte(unsigned char addr)
{
    unsigned char dat;
    IIC_Start();
    IIC_SendByte(MPU6050_ADDRESS);
    IIC_SendByte(addr);
    IIC_Start();
    IIC_SendByte(MPU6050_ADDRESS+1);
    dat=IIC_RecvByte();
    IIC_Stop();
    return dat;
}

void Init_MPU6050()
{
    IIC_WriteByte(MPU6050_SMPLRT_DIV,0x07);
    IIC_WriteByte(MPU6050_CONFIG,0x06);
    IIC_WriteByte(MPU6050_GYRO_CONFIG,0x18);
    IIC_WriteByte(MPU6050_ACCEL_CONFIG,0x01);
}

short GetData(unsigned char addr_H,unsigned char addr_L)
{
    short dat;
    dat=IIC_ReadByte(addr_H);
    dat=dat<<8;
    dat|=IIC_ReadByte(addr_L);
    return dat;
}

void main()
{
    float Acc_x,Acc_y,Acc_z,Gyro_x,Gyro_y,Gyro_z,Temperature;
    float Pitch,Roll,Yaw;
    Init_MPU6050();
    while(1)
    {
        Acc_x=GetData(MPU6050_ACCEL_XOUT_H,MPU6050_ACCEL_XOUT_L);
        Acc_y=GetData(MPU6050_ACCEL_YOUT_H,MPU6050_ACCEL_YOUT_L);
        Acc_z=GetData(MPU6050_ACCEL_ZOUT_H,MPU6050_ACCEL_ZOUT_L);
        Gyro_x=GetData(MPU6050_GYRO_XOUT_H,MPU6050_GYRO_XOUT_L);
        Gyro_y=GetData(MPU6050_GYRO_YOUT_H,MPU6050_GYRO_YOUT_L);
        Gyro_z=GetData(MPU6050_GYRO_ZOUT_H,MPU6050_GYRO_ZOUT_L);
        Temperature=GetData(MPU6050_TEMP_OUT_H,MPU6050_TEMP_OUT_L)/340.0+36.53;
        Pitch=atan(Acc_x/sqrt(Acc_y*Acc_y+Acc_z*Acc_z))*57.3;
        Roll=atan(Acc_y/sqrt(Acc_x*Acc_x+Acc_z*Acc_z))*57.3;
        Yaw=Gyro_z*0.061;
        printf("Pitch=%.2f Roll=%.2f Yaw=%.2f Temperature=%.2f\r\n",Pitch,Roll,Yaw,Temperature);
        delay(1000);
    }
}

需要注意的是，以上代码使用了printf函数输出数据，需要在单片机中配置串口通信。另外，该代码只计算了俯仰角、反转角和航向角，如果需要计算其他姿态参数，需要根据相应的公式进行计算。