IIC通信__驱动MPU6050 6轴运动处理组件—一阶互补滤波

今日主要学习一款倾角传感器——MPU6050,往后对单片机原理基础讲的会比较少，更倾向于简单粗暴地贴代码，因为经过前些日子对MSP432的学习，对原理方面也有些熟络了，除了在新接触它时会对其引脚、时钟、总线等进行仔细一些的研究之外，其余驱动方面便是照搬经验了~~

本文尝试使用STM32 F103C8T6通过IIC通信驱动MPU6050，文章提供源码、原理讲解、实践操作与结果截图，测试工程下载。MPU6050
下图为MPU6050:


MPU6050是由三个陀螺仪和三个加速度传感器组成的6轴运动处理组件，是一款六轴（三轴加速度+三轴角速度（陀螺仪））传感器

1. VCC：构成8位的IIC地址模块自带了 3.3V 超低压差稳压芯片，给 MPU6050 供电，因此外部供电可以选择：3.3V / 5V 都可以

2.GND：不多说

3.SCL、SDA：IIC通讯引脚

另外，IIC_SDA 和 IIC_SCL 带了 4.7K上拉电阻，外部可以不用再加上拉电阻了

4.XDA、XCL：XDA 跟 XCl是用来外接电磁传感器，即使模块不动，数值也会有波动。而外加电磁传感器就是更好的解决这一问题。

5.AD0: AD0 是从 IIC 接口（接 MCU）的地址控制引脚，该引脚控制IIC 地址的最低位。如果接 GND，则 MPU6050 的 IIC 地址是：0X68，如果接 VDD，则是0X69。另外，MPU_AD0 自带了 10K 下拉电阻，当 AD0悬空时，默认 IIC 地址为（0X68）。

6.INT:在MPU6050 中有个INT 引脚，每当MPU6050 有数据输出时，引脚INT 有相应的电平变化。可以将其触发外部中断作为控制周期。当MPU6050 的读取一次数据，就控制一次，可以很好地保持MPU6050 数据的实时性。

使用注意点：
1.不用触碰MPU6050芯片的表面，实测会使其卡住，我遇到这问题是需要给单片机重新下载程序才能解决。

2.AD0脚可以悬空也可接VCC\GND，这三种接法情况会导致MPU6050的地址变化，我是悬空的。

3.本文用到include "math.h"的库函数： double atan2  (double y     , double x);编译报错警告时注意添加头文件~

程序设计目标：
有关于MPU6050地址，寄存器读取，寄存器写入方面的一些知识，我们就在程序设计实践中一起讲了~~

此次程序设计目标简单，是IIC 读取MPU6050的角度值，用串口1每隔1S向上位机反馈：

移植IIC通信：
首先是移植IIC软件模拟时序、IIC引脚初始化的代码，这里直接贴出：此处我初始化了PA2 和 PA3为通信端口：该IIC代码是从他处移植的~  IIC基本原理不多加介绍：#include "bsp_i2c.h"

/* 定义I2C总线连接的GPIO端口, 用户只需要修改下面4行代码即可任意改变SCL和SDA的引脚 */
#define GPIO_PORT_I2C        GPIOA                        /* GPIO端口 */
#define RCC_I2C_PORT         RCC_APB2Periph_GPIOA                /* GPIO端口时钟 */
#define I2C_SCL_PIN                GPIO_Pin_2                        /* 连接到SCL时钟线的GPIO */
#define I2C_SDA_PIN                GPIO_Pin_3                        /* 连接到SDA数据线的GPIO */




/* 定义读写SCL和SDA的宏，已增加代码的可移植性和可阅读性 */
#if 0        /* 条件编译： 1 选择GPIO的库函数实现IO读写 */
        #define I2C_SCL_1()  GPIO_SetBits(GPIO_PORT_I2C, I2C_SCL_PIN)                /* SCL = 1 */
        #define I2C_SCL_0()  GPIO_ResetBits(GPIO_PORT_I2C, I2C_SCL_PIN)                /* SCL = 0 */
       
        #define I2C_SDA_1()  GPIO_SetBits(GPIO_PORT_I2C, I2C_SDA_PIN)                /* SDA = 1 */
        #define I2C_SDA_0()  GPIO_ResetBits(GPIO_PORT_I2C, I2C_SDA_PIN)                /* SDA = 0 */
       
        #define I2C_SDA_READ()  GPIO_ReadInputDataBit(GPIO_PORT_I2C, I2C_SDA_PIN)        /* 读SDA口线状态 */
#else        /* 这个分支选择直接寄存器操作实现IO读写 */
    /*　注意：如下写法，在IAR最高级别优化时，会被编译器错误优化 */
        #define I2C_SCL_1()  GPIO_PORT_I2C->BSRR = I2C_SCL_PIN                                /* SCL = 1 */
        #define I2C_SCL_0()  GPIO_PORT_I2C->BRR = I2C_SCL_PIN                                /* SCL = 0 */
       
        #define I2C_SDA_1()  GPIO_PORT_I2C->BSRR = I2C_SDA_PIN                                /* SDA = 1 */
        #define I2C_SDA_0()  GPIO_PORT_I2C->BRR = I2C_SDA_PIN                                /* SDA = 0 */
       
        #define I2C_SDA_READ()  ((GPIO_PORT_I2C->IDR & I2C_SDA_PIN) != 0)        /* 读SDA口线状态 */
#endif

void i2c_GPIO_Config(void);

/*
*********************************************************************************************************
*        函 数 名: i2c_Delay
*        功能说明: I2C总线位延迟，最快400KHz
*        形    参：无
*        返 回 值: 无
*********************************************************************************************************
*/
static void i2c_Delay(void)
{
        uint8_t i;

        /*　
                 下面的时间是通过安富莱AX-Pro逻辑分析仪测试得到的。
                CPU主频72MHz时，在内部Flash运行, MDK工程不优化
                循环次数为10时，SCL频率 = 205KHz
                循环次数为7时，SCL频率 = 347KHz， SCL高电平时间1.5us，SCL低电平时间2.87us
                 循环次数为5时，SCL频率 = 421KHz， SCL高电平时间1.25us，SCL低电平时间2.375us
        
    IAR工程编译效率高，不能设置为7
        */
        for (i = 0; i < 10; i++);
}

/*
*********************************************************************************************************
*        函 数 名: i2c_Start
*        功能说明: CPU发起I2C总线启动信号
*        形    参：无
*        返 回 值: 无
*********************************************************************************************************
*/
void i2c_Start(void)
{
        /* 当SCL高电平时，SDA出现一个下跳沿表示I2C总线启动信号 */
        I2C_SDA_1();
        I2C_SCL_1();
        i2c_Delay();
        I2C_SDA_0();
        i2c_Delay();
        I2C_SCL_0();
        i2c_Delay();
}

/*
*********************************************************************************************************
*        函 数 名: i2c_Start
*        功能说明: CPU发起I2C总线停止信号
*        形    参：无
*        返 回 值: 无
*********************************************************************************************************
*/
void i2c_Stop(void)
{
        /* 当SCL高电平时，SDA出现一个上跳沿表示I2C总线停止信号 */
        I2C_SDA_0();
        I2C_SCL_1();
        i2c_Delay();
        I2C_SDA_1();
}

/*
*********************************************************************************************************
*        函 数 名: i2c_SendByte
*        功能说明: CPU向I2C总线设备发送8bit数据
*        形    参：_ucByte ： 等待发送的字节
*        返 回 值: 无
*********************************************************************************************************
*/
void i2c_SendByte(uint8_t _ucByte)
{
        uint8_t i;

        /* 先发送字节的高位bit7 */
        for (i = 0; i < 8; i++)
        {               
                if (_ucByte & 0x80)
                {
                        I2C_SDA_1();
                }
                else
                {
                        I2C_SDA_0();
                }
                i2c_Delay();
                I2C_SCL_1();
                i2c_Delay();       
                I2C_SCL_0();
                if (i == 7)
                {
                         I2C_SDA_1(); // 释放总线
                }
                _ucByte <<= 1;        /* 左移一个bit */
                i2c_Delay();
        }
}

/*
*********************************************************************************************************
*        函 数 名: i2c_ReadByte
*        功能说明: CPU从I2C总线设备读取8bit数据
*        形    参：无
*        返 回 值: 读到的数据
*********************************************************************************************************
*/
uint8_t i2c_ReadByte(u8 ack)
{
        uint8_t i;
        uint8_t value;

        /* 读到第1个bit为数据的bit7 */
        value = 0;
        for (i = 0; i < 8; i++)
        {
                value <<= 1;
                I2C_SCL_1();
                i2c_Delay();
                if (I2C_SDA_READ())
                {
                        value++;
                }
                I2C_SCL_0();
                i2c_Delay();
        }
        if(ack==0)
                i2c_NAck();
        else
                i2c_Ack();
        return value;
}

/*
*********************************************************************************************************
*        函 数 名: i2c_WaitAck
*        功能说明: CPU产生一个时钟，并读取器件的ACK应答信号
*        形    参：无
*        返 回 值: 返回0表示正确应答，1表示无器件响应
*********************************************************************************************************
*/
uint8_t i2c_WaitAck(void)
{
        uint8_t re;

        I2C_SDA_1();        /* CPU释放SDA总线 */
        i2c_Delay();
        I2C_SCL_1();        /* CPU驱动SCL = 1, 此时器件会返回ACK应答 */
        i2c_Delay();
        if (I2C_SDA_READ())        /* CPU读取SDA口线状态 */
        {
                re = 1;
        }
        else
        {
                re = 0;
        }
        I2C_SCL_0();
        i2c_Delay();
        return re;
}

/*
*********************************************************************************************************
*        函 数 名: i2c_Ack
*        功能说明: CPU产生一个ACK信号
*        形    参：无
*        返 回 值: 无
*********************************************************************************************************
*/
void i2c_Ack(void)
{
        I2C_SDA_0();        /* CPU驱动SDA = 0 */
        i2c_Delay();
        I2C_SCL_1();        /* CPU产生1个时钟 */
        i2c_Delay();
        I2C_SCL_0();
        i2c_Delay();
        I2C_SDA_1();        /* CPU释放SDA总线 */
}

/*
*********************************************************************************************************
*        函 数 名: i2c_NAck
*        功能说明: CPU产生1个NACK信号
*        形    参：无
*        返 回 值: 无
*********************************************************************************************************
*/
void i2c_NAck(void)
{
        I2C_SDA_1();        /* CPU驱动SDA = 1 */
        i2c_Delay();
        I2C_SCL_1();        /* CPU产生1个时钟 */
        i2c_Delay();
        I2C_SCL_0();
        i2c_Delay();       
}

/*
*********************************************************************************************************
*        函 数 名: i2c_GPIO_Config
*        功能说明: 配置I2C总线的GPIO，采用模拟IO的方式实现
*        形    参：无
*        返 回 值: 无
*********************************************************************************************************
*/
void i2c_GPIO_Config(void)
{
        GPIO_InitTypeDef GPIO_InitStructure;

        RCC_APB2PeriphClockCmd(RCC_I2C_PORT, ENABLE);        /* 打开GPIO时钟 */

        GPIO_InitStructure.GPIO_Pin = I2C_SCL_PIN | I2C_SDA_PIN;
        GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
        GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD;          /* 开漏输出 */
        GPIO_Init(GPIO_PORT_I2C, &GPIO_InitStructure);

        /* 给一个停止信号, 复位I2C总线上的所有设备到待机模式 */
        i2c_Stop();
}

/*
*********************************************************************************************************
*        函 数 名: i2c_CheckDevice
*        功能说明: 检测I2C总线设备，CPU向发送设备地址，然后读取设备应答来判断该设备是否存在
*        形    参：_Address：设备的I2C总线地址
*        返 回 值: 返回值 0 表示正确， 返回1表示未探测到
*********************************************************************************************************
*/
uint8_t i2c_CheckDevice(uint8_t _Address)
{
        uint8_t ucAck;

        i2c_GPIO_Config();                /* 配置GPIO */
       
        i2c_Start();                /* 发送启动信号 */

        /* 发送设备地址+读写控制bit（0 = w， 1 = r) bit7 先传 */
        i2c_SendByte(_Address|I2C_WR);
        ucAck = i2c_WaitAck();        /* 检测设备的ACK应答 */

        i2c_Stop();                        /* 发送停止信号 */

        return ucAck;
}

#ifndef _BSP_I2C_H
#define _BSP_I2C_H

#include <inttypes.h>
#include "stm32f10x.h"

#define I2C_WR        0                /* 写控制bit */
#define I2C_RD        1                /* 读控制bit */

void i2c_Start(void);
void i2c_Stop(void);
void i2c_SendByte(uint8_t _ucByte);
uint8_t i2c_ReadByte(uint8_t ack);
uint8_t i2c_WaitAck(void);
void i2c_Ack(void);
void i2c_NAck(void);
uint8_t i2c_CheckDevice(uint8_t _Address);
void i2c_GPIO_Config(void);

#endif

编写IIC与MPU6050的通信：
向MPU6050寄存器写数据：

//模块的A0引脚接GND，IIC的7位地址为0x68，若接到VCC，需要改为0x69
#define MPU6050_SLAVE_ADDRESS  (0x68<<1)      //MPU6050器件读地址

//add 是地址
//dat 是数据
void MPU6050_WriteReg(u8 reg_add,u8 reg_dat)
{

MPU6050地址：
此处我翻译了部分地址，进行宏定义

//模块的A0引脚接GND，IIC的7位地址为0x68，若接到VCC，需要改为0x69
#define MPU6050_SLAVE_ADDRESS  (0x68<<1)      //MPU6050器件读地址



#define MPU6050_WHO_AM_I        0x75     //用于验证设备是否正常连接。
#define MPU6050_SMPLRT_DIV      0        //是采样率分频器的寄存器。对其进行配置可以设置采样率，这里设置为8000Hz。
#define MPU6050_DLPF_CFG        0        //数字低通滤波器的寄存器。对其进行配置可以设置陀螺仪和加速度计的滤波参数，这里设置为0表示不滤波。
#define MPU6050_GYRO_OUT        0x43     //MPU6050陀螺仪数据寄存器地址
#define MPU6050_ACC_OUT         0x3B     //MPU6050加速度数据寄存器地址


//MPU6050 的两个可能的器件地址。AD0 引脚的状态决定了使用哪个地址，低电平表示使用 `MPU6050_ADDRESS_AD0_LOW`。
#define MPU6050_ADDRESS_AD0_LOW     0x68 // address pin low (GND), default for InvenSense evaluation board
#define MPU6050_ADDRESS_AD0_HIGH    0x69 // address pin high (VCC)
#define MPU6050_DEFAULT_ADDRESS     MPU6050_ADDRESS_AD0_LOW

#define MPU6050_RA_XG_OFFS_TC       0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD X轴陀螺仪偏移温度补偿寄存器
#define MPU6050_RA_YG_OFFS_TC       0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD Y轴陀螺仪偏移温度补偿寄存器
#define MPU6050_RA_ZG_OFFS_TC       0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD Z轴陀螺仪偏移温度补偿寄存器

#define MPU6050_RA_X_FINE_GAIN      0x03 //[7:0] X_FINE_GAIN X轴加速度计校准增益寄存器
#define MPU6050_RA_Y_FINE_GAIN      0x04 //[7:0] Y_FINE_GAIN Y轴加速度计校准增益寄存器
#define MPU6050_RA_Z_FINE_GAIN      0x05 //[7:0] Z_FINE_GAIN Z轴加速度计校准增益寄存器

#define MPU6050_RA_XA_OFFS_H        0x06 //[15:0] XA_OFFS X轴加速度计偏移高位寄存器
#define MPU6050_RA_XA_OFFS_L_TC     0x07                //X轴加速度计偏移低位寄存器
#define MPU6050_RA_YA_OFFS_H        0x08 //[15:0] YA_OFFS Y轴加速度计偏移高位寄存器
#define MPU6050_RA_YA_OFFS_L_TC     0x09                //Y轴加速度计偏移低位寄存器
#define MPU6050_RA_ZA_OFFS_H        0x0A //[15:0] ZA_OFFS Z轴加速度计偏移高位寄存器
#define MPU6050_RA_ZA_OFFS_L_TC     0x0B                //Z轴加速度计偏移低位寄存器

#define MPU6050_RA_XG_OFFS_USRH     0x13 //[15:0] XG_OFFS_USR X轴陀螺仪用户偏移高位寄存器
#define MPU6050_RA_XG_OFFS_USRL     0x14                //X轴陀螺仪用户偏移低位寄存器
#define MPU6050_RA_YG_OFFS_USRH     0x15 //[15:0] YG_OFFS_USR Y轴陀螺仪用户偏移高位寄存器
#define MPU6050_RA_YG_OFFS_USRL     0x16                //Y轴陀螺仪用户偏移低位寄存器
#define MPU6050_RA_ZG_OFFS_USRH     0x17 //[15:0] ZG_OFFS_USR  Z轴陀螺仪用户偏移高位寄存器
#define MPU6050_RA_ZG_OFFS_USRL     0x18                //Z轴陀螺仪用户偏移低位寄存器。

#define MPU6050_RA_SMPLRT_DIV       0x19 //设置采样率除数，用于控制采样率的频率。
#define MPU6050_RA_CONFIG           0x1A //配置加速度计和陀螺仪的低通滤波器和同步采样率
#define MPU6050_RA_GYRO_CONFIG      0x1B //配置陀螺仪的测量范围和自检
#define MPU6050_RA_ACCEL_CONFIG     0x1C //配置加速度计的测量范围和自检
#define MPU6050_RA_FF_THR           0x1D //双向运动检测的自由落体阈值
#define MPU6050_RA_FF_DUR           0x1E //双向运动检测的自由落体持续时间
#define MPU6050_RA_MOT_THR          0x1F //单向运动检测的运动阈值
#define MPU6050_RA_MOT_DUR          0x20 //单向运动检测的运动持续时间
#define MPU6050_RA_ZRMOT_THR        0x21 //零运动检测的运动阈值
#define MPU6050_RA_ZRMOT_DUR        0x22 //零运动检测的运动持续时间
#define MPU6050_RA_FIFO_EN          0x23 //FIFO缓冲区的数据输出使能

#define MPU6050_RA_I2C_MST_CTRL     0x24 //I2C主控制器的配置
#define MPU6050_RA_I2C_SLV0_ADDR    0x25 //I2C从设备0的地址
#define MPU6050_RA_I2C_SLV0_REG     0x26 //I2C从设备0的寄存器地址
#define MPU6050_RA_I2C_SLV0_CTRL    0x27 //I2C从设备0的控制设置
#define MPU6050_RA_I2C_SLV1_ADDR    0x28 //I2C从设备1的地址
#define MPU6050_RA_I2C_SLV1_REG     0x29 //I2C从设备1的寄存器地址
#define MPU6050_RA_I2C_SLV1_CTRL    0x2A //I2C从设备1的控制设置
#define MPU6050_RA_I2C_SLV2_ADDR    0x2B //I2C从设备2的地址
#define MPU6050_RA_I2C_SLV2_REG     0x2C //I2C从设备2的寄存器地址
#define MPU6050_RA_I2C_SLV2_CTRL    0x2D //I2C从设备2的控制设置
#define MPU6050_RA_I2C_SLV3_ADDR    0x2E //I2C从设备3的地址
#define MPU6050_RA_I2C_SLV3_REG     0x2F //I2C从设备3的寄存器地址
#define MPU6050_RA_I2C_SLV3_CTRL    0x30 //I2C从设备3的控制设置
#define MPU6050_RA_I2C_SLV4_ADDR    0x31 //I2C从设备4的地址
#define MPU6050_RA_I2C_SLV4_REG     0x32 //I2C从设备4的寄存器地址
#define MPU6050_RA_I2C_SLV4_DO      0x33 //I2C从设备4的写数据
#define MPU6050_RA_I2C_SLV4_CTRL    0x34 //I2C从设备4的控制设置
#define MPU6050_RA_I2C_SLV4_DI      0x35 //I2C从设备4的读数据
#define MPU6050_RA_I2C_MST_STATUS   0x36 //I2C主机状态寄存器

#define MPU6050_RA_INT_PIN_CFG      0x37 //中断引脚配置寄存器
#define MPU6050_RA_INT_ENABLE       0x38 //中断使能寄存器
#define MPU6050_RA_DMP_INT_STATUS   0x39 //DMP中断状态寄存器
#define MPU6050_RA_INT_STATUS       0x3A //中断状态寄存器

#define MPU6050_RA_ACCEL_XOUT_H     0x3B //X轴加速度输出高位字节
#define MPU6050_RA_ACCEL_XOUT_L     0x3C //X轴加速度输出低位字节
#define MPU6050_RA_ACCEL_YOUT_H     0x3D //Y轴加速度输出高位字节
#define MPU6050_RA_ACCEL_YOUT_L     0x3E //Y轴加速度输出低位字节
#define MPU6050_RA_ACCEL_ZOUT_H     0x3F //Z轴加速度输出高位字节
#define MPU6050_RA_ACCEL_ZOUT_L     0x40 //Z轴加速度输出低位字节
#define MPU6050_RA_TEMP_OUT_H       0x41 //温度输出高位字节
#define MPU6050_RA_TEMP_OUT_L       0x42 //温度输出低位字节
#define MPU6050_RA_GYRO_XOUT_H      0x43 //X轴陀螺仪输出高位字节
#define MPU6050_RA_GYRO_XOUT_L      0x44 //X轴陀螺仪输出低位字节
#define MPU6050_RA_GYRO_YOUT_H      0x45 //Y轴陀螺仪输出高位字节
#define MPU6050_RA_GYRO_YOUT_L      0x46 //Y轴陀螺仪输出低位字节
#define MPU6050_RA_GYRO_ZOUT_H      0x47 //Z轴陀螺仪输出高位字节
#define MPU6050_RA_GYRO_ZOUT_L      0x48 //Z轴陀螺仪输出低位字节

//外部传感器数据寄存器
#define MPU6050_RA_EXT_SENS_DATA_00 0x49 //
#define MPU6050_RA_EXT_SENS_DATA_01 0x4A //
#define MPU6050_RA_EXT_SENS_DATA_02 0x4B //
#define MPU6050_RA_EXT_SENS_DATA_03 0x4C //
#define MPU6050_RA_EXT_SENS_DATA_04 0x4D //
#define MPU6050_RA_EXT_SENS_DATA_05 0x4E //
#define MPU6050_RA_EXT_SENS_DATA_06 0x4F //
#define MPU6050_RA_EXT_SENS_DATA_07 0x50 //
#define MPU6050_RA_EXT_SENS_DATA_08 0x51 //
#define MPU6050_RA_EXT_SENS_DATA_09 0x52 //
#define MPU6050_RA_EXT_SENS_DATA_10 0x53 //
#define MPU6050_RA_EXT_SENS_DATA_11 0x54 //
#define MPU6050_RA_EXT_SENS_DATA_12 0x55 //
#define MPU6050_RA_EXT_SENS_DATA_13 0x56 //
#define MPU6050_RA_EXT_SENS_DATA_14 0x57 //
#define MPU6050_RA_EXT_SENS_DATA_15 0x58 //
#define MPU6050_RA_EXT_SENS_DATA_16 0x59 //
#define MPU6050_RA_EXT_SENS_DATA_17 0x5A //
#define MPU6050_RA_EXT_SENS_DATA_18 0x5B //
#define MPU6050_RA_EXT_SENS_DATA_19 0x5C //
#define MPU6050_RA_EXT_SENS_DATA_20 0x5D //
#define MPU6050_RA_EXT_SENS_DATA_21 0x5E //
#define MPU6050_RA_EXT_SENS_DATA_22 0x5F //
#define MPU6050_RA_EXT_SENS_DATA_23 0x60 //


#define MPU6050_RA_MOT_DETECT_STATUS    0x61 //运动检测状态寄存器
#define MPU6050_RA_I2C_SLV0_DO      0x63 //I2C从机0数据输出寄存器
#define MPU6050_RA_I2C_SLV1_DO      0x64 //I2C从机1数据输出寄存器
#define MPU6050_RA_I2C_SLV2_DO      0x65 //I2C从机2数据输出寄存器
#define MPU6050_RA_I2C_SLV3_DO      0x66 //I2C从设备3的数据寄存器地址
#define MPU6050_RA_I2C_MST_DELAY_CTRL   0x67 //信号路径重置寄存器地址
#define MPU6050_RA_SIGNAL_PATH_RESET    0x68 //运动检测控制寄存器地址
#define MPU6050_RA_MOT_DETECT_CTRL      0x69 //用户控制寄存器地址
#define MPU6050_RA_USER_CTRL        0x6A //电源管理1寄存器地址
#define MPU6050_RA_PWR_MGMT_1       0x6B //电源管理2寄存器地址
#define MPU6050_RA_PWR_MGMT_2       0x6C //寄存器银行选择寄存器地址
#define MPU6050_RA_BANK_SEL         0x6D //内存起始地址寄存器地址
#define MPU6050_RA_MEM_START_ADDR   0x6E //内存读/写寄存器地址
#define MPU6050_RA_MEM_R_W          0x6F //DMP配置1寄存器地址
#define MPU6050_RA_DMP_CFG_1        0x70 //DMP配置2寄存器地址
#define MPU6050_RA_DMP_CFG_2        0x71 //FIFO计数高字节寄存器地址
#define MPU6050_RA_FIFO_COUNTH      0x72 //FIFO计数低字节寄存器地址
#define MPU6050_RA_FIFO_COUNTL      0x73 //FIFO读/写寄存器地址
#define MPU6050_RA_FIFO_R_W         0x74 //FIFO读/写寄存器地址
#define MPU6050_RA_WHO_AM_I         0x75 //器件ID寄存器地址

#define MPU6050_TC_PWR_MODE_BIT     7 //TC_PWR_MODE位，用于选择电源模式
#define MPU6050_TC_OFFSET_BIT       6 //TC_OFFSET位，用于设置温度传感器的偏移
#define MPU6050_TC_OFFSET_LENGTH    6 //TC_OFFSET字段的长度
#define MPU6050_TC_OTP_BNK_VLD_BIT  0 //TC_OTP_BNK_VLD位，用于验证OTP存储的摄氏度系数是否有效

#define MPU6050_VDDIO_LEVEL_VLOGIC  0 //VDDIO电平模式，表示VLOGIC级别
#define MPU6050_VDDIO_LEVEL_VDD     1 //VDDIO电平模式，表示VDD级别

#define MPU6050_CFG_EXT_SYNC_SET_BIT    5 //EXT_SYNC_SET位，用于选择外部同步信号
#define MPU6050_CFG_EXT_SYNC_SET_LENGTH 3 //EXT_SYNC_SET字段的长度
#define MPU6050_CFG_DLPF_CFG_BIT    2 //DLPF_CFG位，用于选择数字低通滤波器配置
#define MPU6050_CFG_DLPF_CFG_LENGTH 3 //DLPF_CFG字段的长度

#define MPU6050_EXT_SYNC_DISABLED       0x0 //禁用外部同步信号
#define MPU6050_EXT_SYNC_TEMP_OUT_L     0x1 //外部同步信号为温度输出的低字节
#define MPU6050_EXT_SYNC_GYRO_XOUT_L    0x2 //外部同步信号为陀螺仪X轴输出的低字节
#define MPU6050_EXT_SYNC_GYRO_YOUT_L    0x3 //外部同步信号为陀螺仪Y轴输出的低字节
#define MPU6050_EXT_SYNC_GYRO_ZOUT_L    0x4 //外部同步信号为陀螺仪Z轴输出的低字节
#define MPU6050_EXT_SYNC_ACCEL_XOUT_L   0x5 //外部同步信号为加速度计X轴输出的低字节
#define MPU6050_EXT_SYNC_ACCEL_YOUT_L   0x6 //外部同步信号为加速度计Y轴输出的低字节
#define MPU6050_EXT_SYNC_ACCEL_ZOUT_L   0x7 //外部同步信号为加速度计Z轴输出的低字节

#define MPU6050_DLPF_BW_256         0x00 //
#define MPU6050_DLPF_BW_188         0x01 //
#define MPU6050_DLPF_BW_98          0x02 //
#define MPU6050_DLPF_BW_42          0x03 //
#define MPU6050_DLPF_BW_20          0x04 //
#define MPU6050_DLPF_BW_10          0x05 //
#define MPU6050_DLPF_BW_5           0x06 //

#define MPU6050_GCONFIG_FS_SEL_BIT      4
#define MPU6050_GCONFIG_FS_SEL_LENGTH   2

#define MPU6050_GYRO_FS_250         0x00
#define MPU6050_GYRO_FS_500         0x01
#define MPU6050_GYRO_FS_1000        0x02
#define MPU6050_GYRO_FS_2000        0x03

#define MPU6050_ACONFIG_XA_ST_BIT           7
#define MPU6050_ACONFIG_YA_ST_BIT           6
#define MPU6050_ACONFIG_ZA_ST_BIT           5
#define MPU6050_ACONFIG_AFS_SEL_BIT         4
#define MPU6050_ACONFIG_AFS_SEL_LENGTH      2
#define MPU6050_ACONFIG_ACCEL_HPF_BIT       2
#define MPU6050_ACONFIG_ACCEL_HPF_LENGTH    3

#define MPU6050_ACCEL_FS_2          0x00
#define MPU6050_ACCEL_FS_4          0x01
#define MPU6050_ACCEL_FS_8          0x02
#define MPU6050_ACCEL_FS_16         0x03

#define MPU6050_DHPF_RESET          0x00
#define MPU6050_DHPF_5              0x01
#define MPU6050_DHPF_2P5            0x02
#define MPU6050_DHPF_1P25           0x03
#define MPU6050_DHPF_0P63           0x04
#define MPU6050_DHPF_HOLD           0x07

#define MPU6050_TEMP_FIFO_EN_BIT    7
#define MPU6050_XG_FIFO_EN_BIT      6
#define MPU6050_YG_FIFO_EN_BIT      5
#define MPU6050_ZG_FIFO_EN_BIT      4
#define MPU6050_ACCEL_FIFO_EN_BIT   3
#define MPU6050_SLV2_FIFO_EN_BIT    2
#define MPU6050_SLV1_FIFO_EN_BIT    1
#define MPU6050_SLV0_FIFO_EN_BIT    0

#define MPU6050_MULT_MST_EN_BIT     7
#define MPU6050_WAIT_FOR_ES_BIT     6
#define MPU6050_SLV_3_FIFO_EN_BIT   5
#define MPU6050_I2C_MST_P_NSR_BIT   4
#define MPU6050_I2C_MST_CLK_BIT     3
#define MPU6050_I2C_MST_CLK_LENGTH  4

#define MPU6050_CLOCK_DIV_348       0x0
#define MPU6050_CLOCK_DIV_333       0x1
#define MPU6050_CLOCK_DIV_320       0x2
#define MPU6050_CLOCK_DIV_308       0x3
#define MPU6050_CLOCK_DIV_296       0x4
#define MPU6050_CLOCK_DIV_286       0x5
#define MPU6050_CLOCK_DIV_276       0x6
#define MPU6050_CLOCK_DIV_267       0x7
#define MPU6050_CLOCK_DIV_258       0x8
#define MPU6050_CLOCK_DIV_500       0x9
#define MPU6050_CLOCK_DIV_471       0xA
#define MPU6050_CLOCK_DIV_444       0xB
#define MPU6050_CLOCK_DIV_421       0xC
#define MPU6050_CLOCK_DIV_400       0xD
#define MPU6050_CLOCK_DIV_381       0xE
#define MPU6050_CLOCK_DIV_364       0xF

#define MPU6050_I2C_SLV_RW_BIT      7
#define MPU6050_I2C_SLV_ADDR_BIT    6
#define MPU6050_I2C_SLV_ADDR_LENGTH 7
#define MPU6050_I2C_SLV_EN_BIT      7
#define MPU6050_I2C_SLV_BYTE_SW_BIT 6
#define MPU6050_I2C_SLV_REG_DIS_BIT 5
#define MPU6050_I2C_SLV_GRP_BIT     4
#define MPU6050_I2C_SLV_LEN_BIT     3
#define MPU6050_I2C_SLV_LEN_LENGTH  4

#define MPU6050_I2C_SLV4_RW_BIT         7
#define MPU6050_I2C_SLV4_ADDR_BIT       6
#define MPU6050_I2C_SLV4_ADDR_LENGTH    7
#define MPU6050_I2C_SLV4_EN_BIT         7
#define MPU6050_I2C_SLV4_INT_EN_BIT     6
#define MPU6050_I2C_SLV4_REG_DIS_BIT    5
#define MPU6050_I2C_SLV4_MST_DLY_BIT    4
#define MPU6050_I2C_SLV4_MST_DLY_LENGTH 5

#define MPU6050_MST_PASS_THROUGH_BIT    7
#define MPU6050_MST_I2C_SLV4_DONE_BIT   6
#define MPU6050_MST_I2C_LOST_ARB_BIT    5
#define MPU6050_MST_I2C_SLV4_NACK_BIT   4
#define MPU6050_MST_I2C_SLV3_NACK_BIT   3
#define MPU6050_MST_I2C_SLV2_NACK_BIT   2
#define MPU6050_MST_I2C_SLV1_NACK_BIT   1
#define MPU6050_MST_I2C_SLV0_NACK_BIT   0

#define MPU6050_INTCFG_INT_LEVEL_BIT        7
#define MPU6050_INTCFG_INT_OPEN_BIT         6
#define MPU6050_INTCFG_LATCH_INT_EN_BIT     5
#define MPU6050_INTCFG_INT_RD_CLEAR_BIT     4
#define MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT  3
#define MPU6050_INTCFG_FSYNC_INT_EN_BIT     2
#define MPU6050_INTCFG_I2C_BYPASS_EN_BIT    1
#define MPU6050_INTCFG_CLKOUT_EN_BIT        0

#define MPU6050_INTMODE_ACTIVEHIGH  0x00
#define MPU6050_INTMODE_ACTIVELOW   0x01

#define MPU6050_INTDRV_PUSHPULL     0x00
#define MPU6050_INTDRV_OPENDRAIN    0x01

#define MPU6050_INTLATCH_50USPULSE  0x00
#define MPU6050_INTLATCH_WAITCLEAR  0x01

#define MPU6050_INTCLEAR_STATUSREAD 0x00
#define MPU6050_INTCLEAR_ANYREAD    0x01

#define MPU6050_INTERRUPT_FF_BIT            7
#define MPU6050_INTERRUPT_MOT_BIT           6
#define MPU6050_INTERRUPT_ZMOT_BIT          5
#define MPU6050_INTERRUPT_FIFO_OFLOW_BIT    4
#define MPU6050_INTERRUPT_I2C_MST_INT_BIT   3
#define MPU6050_INTERRUPT_PLL_RDY_INT_BIT   2
#define MPU6050_INTERRUPT_DMP_INT_BIT       1
#define MPU6050_INTERRUPT_DATA_RDY_BIT      0

// TODO: figure out what these actually do
// UMPL source code is not very obivous
#define MPU6050_DMPINT_5_BIT            5
#define MPU6050_DMPINT_4_BIT            4
#define MPU6050_DMPINT_3_BIT            3
#define MPU6050_DMPINT_2_BIT            2
#define MPU6050_DMPINT_1_BIT            1
#define MPU6050_DMPINT_0_BIT            0

#define MPU6050_MOTION_MOT_XNEG_BIT     7
#define MPU6050_MOTION_MOT_XPOS_BIT     6
#define MPU6050_MOTION_MOT_YNEG_BIT     5
#define MPU6050_MOTION_MOT_YPOS_BIT     4
#define MPU6050_MOTION_MOT_ZNEG_BIT     3
#define MPU6050_MOTION_MOT_ZPOS_BIT     2
#define MPU6050_MOTION_MOT_ZRMOT_BIT    0

#define MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT   7
#define MPU6050_DELAYCTRL_I2C_SLV4_DLY_EN_BIT   4
#define MPU6050_DELAYCTRL_I2C_SLV3_DLY_EN_BIT   3
#define MPU6050_DELAYCTRL_I2C_SLV2_DLY_EN_BIT   2
#define MPU6050_DELAYCTRL_I2C_SLV1_DLY_EN_BIT   1
#define MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT   0

#define MPU6050_PATHRESET_GYRO_RESET_BIT    2
#define MPU6050_PATHRESET_ACCEL_RESET_BIT   1
#define MPU6050_PATHRESET_TEMP_RESET_BIT    0

#define MPU6050_DETECT_ACCEL_ON_DELAY_BIT       5
#define MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH    2
#define MPU6050_DETECT_FF_COUNT_BIT             3
#define MPU6050_DETECT_FF_COUNT_LENGTH          2
#define MPU6050_DETECT_MOT_COUNT_BIT            1
#define MPU6050_DETECT_MOT_COUNT_LENGTH         2

#define MPU6050_DETECT_DECREMENT_RESET  0x0
#define MPU6050_DETECT_DECREMENT_1      0x1
#define MPU6050_DETECT_DECREMENT_2      0x2
#define MPU6050_DETECT_DECREMENT_4      0x3

#define MPU6050_USERCTRL_DMP_EN_BIT             7
#define MPU6050_USERCTRL_FIFO_EN_BIT            6
#define MPU6050_USERCTRL_I2C_MST_EN_BIT         5
#define MPU6050_USERCTRL_I2C_IF_DIS_BIT         4
#define MPU6050_USERCTRL_DMP_RESET_BIT          3
#define MPU6050_USERCTRL_FIFO_RESET_BIT         2
#define MPU6050_USERCTRL_I2C_MST_RESET_BIT      1
#define MPU6050_USERCTRL_SIG_COND_RESET_BIT     0

#define MPU6050_PWR1_DEVICE_RESET_BIT   7
#define MPU6050_PWR1_SLEEP_BIT          6
#define MPU6050_PWR1_CYCLE_BIT          5
#define MPU6050_PWR1_TEMP_DIS_BIT       3
#define MPU6050_PWR1_CLKSEL_BIT         2
#define MPU6050_PWR1_CLKSEL_LENGTH      3

#define MPU6050_CLOCK_INTERNAL          0x00 // 内部振荡器作为时钟源
#define MPU6050_CLOCK_PLL_XGYRO         0x01 //X轴陀螺仪的相位锁定环（PLL）作为时钟源
#define MPU6050_CLOCK_PLL_YGYRO         0x02 //Y轴陀螺仪的相位锁定环（PLL）作为时钟源
#define MPU6050_CLOCK_PLL_ZGYRO         0x03 //Z轴陀螺仪的相位锁定环（PLL）作为时钟源
#define MPU6050_CLOCK_PLL_EXT32K        0x04 //外部32.768kHz晶振作为时钟源
#define MPU6050_CLOCK_PLL_EXT19M        0x05 //外部19.2MHz晶振作为时钟源
#define MPU6050_CLOCK_KEEP_RESET        0x07 //保持复位状态

#define MPU6050_PWR2_LP_WAKE_CTRL_BIT       7
#define MPU6050_PWR2_LP_WAKE_CTRL_LENGTH    2
#define MPU6050_PWR2_STBY_XA_BIT            5
#define MPU6050_PWR2_STBY_YA_BIT            4
#define MPU6050_PWR2_STBY_ZA_BIT            3
#define MPU6050_PWR2_STBY_XG_BIT            2
#define MPU6050_PWR2_STBY_YG_BIT            1
#define MPU6050_PWR2_STBY_ZG_BIT            0

#define MPU6050_WAKE_FREQ_1P25      0x0
#define MPU6050_WAKE_FREQ_2P5       0x1
#define MPU6050_WAKE_FREQ_5         0x2
#define MPU6050_WAKE_FREQ_10        0x3

#define MPU6050_BANKSEL_PRFTCH_EN_BIT       6
#define MPU6050_BANKSEL_CFG_USER_BANK_BIT   5
#define MPU6050_BANKSEL_MEM_SEL_BIT         4
#define MPU6050_BANKSEL_MEM_SEL_LENGTH      5

#define MPU6050_WHO_AM_I_BIT        6
#define MPU6050_WHO_AM_I_LENGTH     6

#define MPU6050_DMP_MEMORY_BANKS        8
#define MPU6050_DMP_MEMORY_BANK_SIZE    256
#define MPU6050_DMP_MEMORY_CHUNK_SIZE   16

MPU6050初始化：


//初始化MPU6050
void MPU6050_Init(void)
{
  delay_ms(100);
        MPU6050_WriteReg(MPU6050_RA_PWR_MGMT_1, 0x00);            //解除休眠状态
        MPU6050_WriteReg(MPU6050_RA_SMPLRT_DIV , 0x07);            //陀螺仪采样率，1KHz
        MPU6050_WriteReg(MPU6050_RA_CONFIG , 0x06);                //低通滤波器的设置，截止频率是1K，带宽是5K
        MPU6050_WriteReg(MPU6050_RA_ACCEL_CONFIG , 0x00);          //配置加速度传感器工作在2G模式，不自检
        MPU6050_WriteReg(MPU6050_RA_GYRO_CONFIG, 0x18);     //陀螺仪自检及测量范围，典型值：0x18(不自检，2000deg/s)
}


//读取MPU6050的ID
uint8_t MPU6050ReadID(void)
{
        unsigned char Re = 0;
    MPU6050_ReadData(MPU6050_RA_WHO_AM_I,&Re,1);    //读器件地址
        if(Re != 0x68)
        {
                printf("MPU6050 dectected error!\r\n");
                return 0;
        }
        else
        {
                printf("MPU6050 ID = %d\r\n",Re);
                return 1;
        }
               
}

MPU6050数据读取处理：


// 读取MPU6050的加速度数据
void MPU6050ReadAcc(short *accData)
{
    u8 buf[6];
    MPU6050_ReadData(MPU6050_ACC_OUT, buf, 6);
    accData[0] = ((buf[0] << 8) | buf[1]);
    accData[1] = ((buf[2] << 8) | buf[3]);
    accData[2] = ((buf[4] << 8) | buf[5]);
}

//读取MPU6050的角加速度数据
void MPU6050ReadGyro(short *gyroData)
{
    u8 buf[6];
    MPU6050_ReadData(MPU6050_GYRO_OUT,buf,6);
    gyroData[0] = ((buf[0] << 8) | buf[1]);
    gyroData[1] = ((buf[2] << 8) | buf[3]);
    gyroData[2] = ((buf[4] << 8) | buf[5]);
}

//读取MPU6050的原始温度数据
void MPU6050ReadTemp(short *tempData)
{
        u8 buf[2];
    MPU6050_ReadData(MPU6050_RA_TEMP_OUT_H,buf,2);     //读取温度值
    *tempData = (buf[0] << 8) | buf[1];
}

//读取MPU6050的温度数据，转化成摄氏度
void MPU6050_ReturnTemp(float *Temperature)
{
        short temp3;
        u8 buf[2];
       
        MPU6050_ReadData(MPU6050_RA_TEMP_OUT_H,buf,2);     //读取温度值
  temp3= (buf[0] << 8) | buf[1];       
        *Temperature=((double) temp3/340.0)+36.53;
}

MPU6050滤波一阶互补：

float yijiehubu_R(float angle_m, float gyro_m)//?????????????
{
     angle_R = K1 * angle_m+ (1-K1) * (angle_R + gyro_m * dt);
         return angle_R;
}
float yijiehubu_P(float angle_m, float gyro_m)//?????????????
{
     angle_P = K1 * angle_m+ (1-K1) * (angle_P + gyro_m * dt);
         return angle_P;
}

方位角计算  atan2（X,Y）：

坐标方位角是指一个点相对于另一个参考点的方向角度。下面是计算坐标方位角的公式：

假设要计算从点 A 到点 B 的坐标方位角，点 A 的坐标为 (x1, y1)，点 B 的坐标为 (x2, y2)。

1. 计算点 B 相对于点 A 的水平距离 dx = x2 - x1。

2. 计算点 B 相对于点 A 的竖直距离 dy = y2 - y1。

3. 计算坐标方位角 angle = atan2(dy, dx)。

4. 将 angle 的弧度值转化为角度值。

其中， atan2(dy, dx) 是求取反正切值的函数，返回值区间是(-π, π]。需要注意，因为 atan2函数的取值范围不包括负半轴，所以需要做特殊处理，即：

如果 atan2(dy, dx) < 0，则 angle = 360 + atan2(dy, dx)。

主函数代码：
#include "main.h"

//211

uint16_t t1,t2,t3;
        short Accel[3];
        short Gyro[3];
        float Temp;

        float Angle_Y,Angle_X,Angle_Z;
        int pitch,roll,yaw;

int main(void)
{       
        init_ALL();     //初始化所有函数
  while(1)
        {       
                        MPU6050ReadAcc(Accel);//获取加速度原始值
                        MPU6050ReadGyro(Gyro);//获取角速度原始值
                        MPU6050_ReturnTemp(&Temp);//获取温度值       
               
                                        Angle_X=atan2(Accel[0],Accel[2])*57.3-Gyro[0]/16.4*0.005;//换算
                                        Angle_Y=atan2(Accel[1],Accel[2])*57.3-Gyro[1]/16.4*0.005;
                                       
                                        pitch=(int)yijiehubu_P(Angle_X,Gyro[0]/16.4); //一阶互补滤波
                                        roll=-(int)yijiehubu_R(Angle_Y,Gyro[1]/16.4);       
        }
}


//初始化所有函数：
void init_ALL(void)
{
        Usart1_Init(115200);      //初始化串口1
        printf("HELLO \r\n");           //串口1 测试重定向Printf
        SysTick_Init(72);
        Timer2_Init();
//  Timer3_PWM_init(2000,719);
        i2c_GPIO_Config();              //IIC初始化
        MPU6050_Init();           //mpu6050初始化
        MPU6050ReadID();  //读取器件ID
}

void TIM2_IRQHandler(void)
{
        if (TIM_GetITStatus(TIM2, TIM_IT_Update) == SET)
        {               
//                printf("Angle_X=%lf",Angle_X);
//                printf("\r\n");
//                printf("Angle_Y=%lf",Angle_Y);               
//                printf("\r\n");
               
//                printf("\r\n");
//                printf("Temp=%lf",Temp);
               
                printf("pitch=%d",pitch);
                printf("\r\n");
                printf("roll=%d",roll);               
                printf("\r\n");
                printf("yaw=%d",yaw);
               
                TIM_ClearITPendingBit(TIM2, TIM_IT_Update);//清出中断寄存器标志位，用于退出中断
        }
}

楼主，你传的代码很详细，传感器的读写函数和滤波参数发一下呗
void MPU6050_WriteReg(u8 reg_add,u8 reg_dat)，MPU6050_ReadData(MPU6050_ACC_OUT, buf, 6);