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

只看该作者 · 2024-1-31 14:12

今日主要学习一款倾角传感器——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 数据的实时性。

只看该作者 · 2024-1-31 14:12

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

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

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

只看该作者 · 2024-1-31 14:12

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

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

只看该作者 · 2024-1-31 14:12

移植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;
}

只看该作者 · 2024-1-31 14:13

#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

只看该作者 · 2024-1-31 14:15

编写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)
{

只看该作者 · 2024-1-31 14:15

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

只看该作者 · 2024-1-31 14: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;
}

}

只看该作者 · 2024-1-31 14:16

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;
}

只看该作者 · 2024-1-31 14:17

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;
}

只看该作者 · 2024-1-31 14:17

方位角计算 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)。

只看该作者 · 2024-1-31 14:17

主函数代码：
#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);//清出中断寄存器标志位，用于退出中断
}
}