STM32F4无人机6轴运动处理组件MPU6050

        学习MPU6050的原理、MPU6050初始化方法。

        MPU6050是9轴运动处理传感器。

它集成了3轴MEMS陀螺仪，3轴MEMS加速度计，以及一个可扩展的数字运动处理器DMP（Digital Motion Processor），可用I2C接口连接一个第三方的数字传感器，比如磁力计。

扩展之后就可以通过其I2C或SPI接口输出一个9轴的信号（SPI接口仅在MPU-6000可用）。

MPU-6050也可以通过其I2C接口连接非惯性的数字传感器，比如压力传感器。

MPU-6050对陀螺仪和加速度计分别用了三个16位的ADC，将其测量的模拟量转化为可输出的数字量。

为了精确跟踪快速和慢速的运动，传感器的测量范围都是用户可控的，陀螺仪可测范围为±250，±500，±1000，±2000°/秒（dps），加速度计可测范围为±2，±4，±8，±16g。

利用MPU6050芯片内部的DMP模块（Digital Motion Processor 数字运动处理器），可对传感器数据进行滤波、融合处理，它直接通过I2C接口向主控器输出姿态解算后的姿态数据，降低主控器的运算量。

其姿态解算频率最高可达200Hz，非常适合用于对姿态控制实时要求较高的领域。

常见应用于手机、智能手环、四轴飞行器及计步器等的姿态检测。

 传感器通过I2C接口进行控制，并且内部含有第二I2C接口，可用于连接外部从设备，传感器内部结构如下所示：

         其中，SCL和SDA是连接MCU的I2C接口，MCU通过这个I2C接口来控制MPU6050，另外还有一个I2C接口：AUX_CL和AUX_DA，这个接口可用于连接外部从设备，比如磁传感器，这样就可以组成一个九轴传感器。

VLOGIC是IO口电压，该引脚最低可以到1.8V，我们一般直接接VDD即可。

AD0是从I2C接口（接MCU）的地址控制引脚，该引脚控制I2C地址的最低位。

如果接GND，则MPU6050的I2C地址是0x68，如果接VDD，则是0x69，注意：这里的地址是不包含数据传输的最低位的（最低位用来表示读写）。

        接下来介绍一下MPU6050传感器的寄存器：

        电源管理寄存器1，地址0x6B。

DEVICE_RESET位：用来控制复位，设置为1，复位MPU6050，复位结束后，MPU硬件自动清零该位。

SLEEP位：用于控制MPU6050的工作模式，复位后，该位为1，即进入了睡眠模式（低功耗），所以我们要清零该位，以进入正常工作模式。

TEMP_DIS位：用于设置是否使能温度传感器，设置为0，则使能。

CLKSEL[2:0]：用于选择系统时钟源，选择关系如下所示：

 默认是使用内部8M RC晶振的，精度不高，所以我们一般选择X/Y/Z轴陀螺作为参考的PLL作为时钟源。

陀螺仪采样率分频寄存器，寄存器地址为：0x19。

 该寄存器用于设置MPU6050的陀螺仪采样频率，计算公式为：

采样频率 = 陀螺仪输出频率 /（1 + SMPLRT_DIV）

这里陀螺仪的输出频率，是1Khz或者8Khz，与数字低通滤波器（DLPF）的设置有关，当DLPF_CFG=0/7的时候，频率为8Khz，其他情况是1Khz。

配置寄存器，寄存器地址为0x1A。

 DLPF_CFG[2:0]：数字低通滤波器（DLPF的设置位），加速度计和陀螺仪，都是根据这三个位的配置进行过滤的。

这里的加速度传感器，输出频率（Fs）固定是1Khz，而角速度传感器的输出速率（Fs），则根据DLPF_CFG的配置有所不同。

陀螺仪配置寄存器，该寄存器地址为0x1B。

 FS_SEL[1:0]：这两个位，用于设置陀螺仪的满量程范围，如下所示：

 加速度传感器配置寄存器，寄存器地址为0x1C。

 AFS_SEL[1:0]：这两个位，用于设置加速度传感器的满量程范围，如下所示：

 加速度传感器数据输出寄存器（0X3B~0X40）

说明：

这个寄存器存储最近加速度计的测量值。

加速度计根据采样频率（由寄存器 25 定义）写入到这些寄存器。

加速度计测量值寄存器和温度测量值寄存器，陀螺仪测量值寄存器，外部传感器数据寄存器都是由 2 个寄存器集合组成：一个内部寄存器集合和一个面向用户的读取寄存器集合。

加速度计传感器的内部寄存器集合里的数据根据采样频率更新。

以此同时，每当串行接口处于闲置状态，面向用户的读取寄存器集合会复制内部寄存器集合的数据值。

这保证了突发读取时传感器寄存器可以读到相同的采样时刻的测量值。

请注意，如果没有突发读取，用户通过检测数据就绪中断（ Data Ready interrupt）确保一组单字节的读取在相应的采样时刻。

每个 16 位加速度计测量值的满量程定义在 ACCEL_FS（ 寄存器 28）。

对于每个满量程的设置， ACCEL_xOUT 里加速度计测量值的灵敏度最低分辨率（ LSB）如下表：

 加速度传感器数据输出寄存器总共由6个寄存器组成，输出X/Y/Z三个轴的加速度传感器值，高字节在前，低字节在后。

陀螺仪数据输出寄存器（0X43~0X48）

 这个寄存器存储最近陀螺仪的测量值。

陀螺仪根据采样频率（由寄存器 25 定义）写入到这些寄存器。

陀螺仪测量值寄存器和温度测量值寄存器，加速度计测量值寄存器，外部传感器数据寄存器都是由 2 个寄存器集合组成：一个内部寄存器集合和一个面向用户的读取寄存器集合。

陀螺仪传感器的内部寄存器集合里的数据根据采样频率更新。

以此同时，每当串行接口处于闲置状态，面向用户的读取寄存器集合会复制内部寄存器集合的数据值。

这保证了突发读取时传感器寄存器可以读到相同的采样时刻的测量值。

请注意，如果没有突发读取，用户通过检测数据就绪中断（ Data Ready interrupt）确保一组单字节的读取在相应的采样时刻。

每个 16 位陀螺仪测量值的满量程定义在 FS_SEL（寄存器 27）。

对于每个满量程的设置，GYRO_xOUT 里陀螺仪测量值的灵敏度最低分辨率（ LSB）如下表：

 陀螺仪数据输出寄存器总共由6个寄存器组成，输出X/Y/Z三个轴的陀螺仪传感器数据，高字节在前，低字节在后。

MDK5开发环境。

STM32F4xx标准外设库。

STM32F407飞控板。

STM32F4xx 参考手册。

飞控板电路原理图。

• 查看飞控板电路原理图，可以找到MPU6050传感器连接的I2C1以及I2C1的时钟线与数据线所对应的GPIO引脚。
  

  
  

• 定义读、写以及多字节读取MPU6050函数。
  

  
  

  新建文件bsp_mpu6050.c和bsp_mpu6050.h。
  

  
  

  将文件添加到工程BSP目录中，在文件bsp_mpu6050.c中编写读写MPU6050读、写以及多字节读取函数。
  

  
  

  具体代码如下。
  

  
  

/** MPU6050单字节写入
 *   REG_Address：要写入的寄存器地址
 *   REG_data：要写入的数据
 */
bool MPU6050_Write_Byte(uint8_t REG_Address, uint8_t REG_data)
{
	return I2C_Write_REG(I2C_MPU6050, MPU6050_ADDR, REG_Address, REG_data);
}

/** MPU6050单字节读取
 *   REG_Address：要读取的寄存器地址
 */
uint8_t MPU6050_Read_Byte(uint8_t REG_Address)
{
	return I2C_Read_REG(I2C_MPU6050, MPU6050_ADDR, REG_Address);
}


/** MPU6050读取n个字节
 *   REG_Address：要读取的寄存器地址
 *   buf：读取后存储数据的buf
 *   len：要读取数据长度
 */
bool MPU6050_Read_NByte(uint8_t REG_Address, uint8_t* buf, uint8_t len)
{
	if(!I2C_Start(I2C_MPU6050))return false;
	I2C_SendByte(I2C_MPU6050, MPU6050_ADDR);  //发送设备地址+写信号
	if(!I2C_WaitAck(I2C_MPU6050)){I2C_Stop(I2C_MPU6050); return false;}
	I2C_SendByte(I2C_MPU6050, REG_Address);   
	I2C_WaitAck(I2C_MPU6050);
	I2C_Start(I2C_MPU6050);
	I2C_SendByte(I2C_MPU6050, MPU6050_ADDR | 1); // 读 *** 作
	I2C_WaitAck(I2C_MPU6050);
	for(uint8_t i=0; i

//初始化
void MPU6050_Init(void)
{
	uint8_t data=0,counter =0;
	
	IIC_init();	
//复位设备
	do
	{
		vTaskDelay(1 / portTICK_RATE_MS);
		MPU6050_Write_Byte(MPU6050_RA_PWR_MGMT_1 , 0x80);  
		vTaskDelay(1 / portTICK_RATE_MS);
		data = MPU6050_Read_Byte(MPU6050_RA_PWR_MGMT_1);
	}	while(data !=MPU6050_PWR1_SLEEP && counter++ <200);	//复位后处于sleep模式
	
	vTaskDelay(100/portTICK_RATE_MS);
	
//关闭睡眠模式，设定Z轴的时钟为参考时钟
	do
	{
		vTaskDelay(1 / portTICK_RATE_MS);
		MPU6050_Write_Byte(MPU6050_RA_PWR_MGMT_1 , MPU6050_CLOCK_PLL_ZGYRO);		
		vTaskDelay(1 / portTICK_RATE_MS);
		data = MPU6050_Read_Byte(MPU6050_RA_PWR_MGMT_1);
	}	while(data !=MPU6050_CLOCK_PLL_ZGYRO && counter++ <200);
	
	vTaskDelay(1/portTICK_RATE_MS);
	
	do
	{
		vTaskDelay(1 / portTICK_RATE_MS);
		/*采样频率 = 陀螺仪输出频率/（1+SMPLRT_DIV）*/
		MPU6050_Write_Byte(MPU6050_RA_SMPLRT_DIV , 1000/(1000)-1);
		vTaskDelay(1 / portTICK_RATE_MS);
		data = MPU6050_Read_Byte(MPU6050_RA_SMPLRT_DIV);
	}while(data != 1000/(1000)-1 && counter++ <200);
	
	vTaskDelay(5/portTICK_RATE_MS);

//设置低通滤波频率，当滤波频率超过90Hz时无明显滤波效果
	do
	{
		vTaskDelay(1 / portTICK_RATE_MS);
		MPU6050_Write_Byte(MPU6050_RA_CONFIG , MPU6050_DLPF_BW_42); 
		vTaskDelay(1 / portTICK_RATE_MS);
		data = MPU6050_Read_Byte(MPU6050_RA_CONFIG);
	}while(data != MPU6050_DLPF_BW_42 && counter++ <200);
	
	vTaskDelay(1/portTICK_RATE_MS);
	
// 陀螺仪最大量程 +-2000度每秒
	do
	{
	vTaskDelay(1 / portTICK_RATE_MS);
	MPU6050_Write_Byte(MPU6050_RA_GYRO_CONFIG , MPU6050_GYRO_FS);
	vTaskDelay(1 / portTICK_RATE_MS);
	data = MPU6050_Read_Byte(MPU6050_GYRO_FS);
	}while(data != MPU6050_GYRO_FS && counter++ <200);
	
	vTaskDelay(1/portTICK_RATE_MS);
// 加速度度最大量程 +-8G;
	do
	{
	vTaskDelay(1 / portTICK_RATE_MS);
	MPU6050_Write_Byte(MPU6050_RA_ACCEL_CONFIG , MPU6050_ACCEL_AFS);
	vTaskDelay(1 / portTICK_RATE_MS);
	data = MPU6050_Read_Byte(MPU6050_RA_ACCEL_CONFIG);
	}while(data != MPU6050_ACCEL_AFS && counter++ <200);

}

/**
 * 读取MPU6050原始数据
 */
bool MPU6050_Read(Vector3i* acc, Vector3i* gyro)
{
  MPU6050_Read_NByte(MPU6050_RA_ACCEL_XOUT_H, mpu6050_buffer, 14);
	if(mpu6050_buffer[0] | mpu6050_buffer[1] | mpu6050_buffer[2] | mpu6050_buffer[3] | mpu6050_buffer[4] | mpu6050_buffer[5])
	{
		acc->x  =  (int16_t)(mpu6050_buffer[0]  << 8 | mpu6050_buffer[1]);
		acc->y  =  (int16_t)(mpu6050_buffer[2]  << 8 | mpu6050_buffer[3]);	
		acc->z  =  (int16_t)(mpu6050_buffer[4]  << 8 | mpu6050_buffer[5]);
		gyro->x =  (int16_t)(mpu6050_buffer[8]  << 8 | mpu6050_buffer[9]);
		gyro->y	=	 (int16_t)(mpu6050_buffer[10] << 8 | mpu6050_buffer[11]);
		gyro->z =  (int16_t)(mpu6050_buffer[12] << 8 | mpu6050_buffer[13]);
		
		if(acc->z ==0)return false;
		return true;
	}
	else
	{
		return false;
	}
}

void vTask1(void * pvParameters)
{
	portTickType xLastWakeTime;
    const portTickType xFrequency = 2;
    xLastWakeTime = xTaskGetTickCount();
    while(1)
    {
			mems_data();
      vTaskDelayUntil( &xLastWakeTime, xFrequency );
    }
}