MPU6050

一.姿态检测

1.姿态是载体坐标系与地理坐标系的转换关系。

地球坐标系：地球球心为原点，Z沿地球自转方向，x和y轴在赤道平面内。

地理坐标系：原点在地球表面，Z轴垂直于地面朝天，X,Y方向是相切于地球的经纬线。

载体坐标系：以运载体的质心为原点，一般根据运载体自身结构方向构成坐标系。

• 偏航角：绕载体Z轴旋转后，Y轴与北轴的夹角。
• 横滚角：绕载体Y轴旋转后，X轴与东轴的夹角。
• 俯仰角：绕载体X轴旋转后，Z轴与天轴的夹角。

2.相关的传感器

（1）陀螺仪（角度检测器）：可以检测物体绕坐标轴转动的角速度。将角速度对时间积分就可以计算出旋转的角度。在四轴飞行器，两轮平衡小车等项目中经常用到。

陀螺仪的缺点：测量角度时使用积分，会存在积分误差。这是由于积分时间比较大，误差也比较大。不断提高采样频率，就可以减小积分时间，减少误差。还有就是器件本身的误差。

（2）加速度传感器：检测角度。由于陀螺仪测量角度在长时间测量时会产生误差，又引入了检测倾角的传感器。加速度传感器可以检测俯仰角和横滚角，无法检测偏航角。由加速度来计算角度。

加速度传感器的缺点：无法区分重力加速度和外力加速度，在震动情况下，传感器数据会有非常大的变化，难以反映重力实际值。

（3）磁场检测：弥补加速度传感器无法检测偏航角的问题，加入磁场检测传感器。可以检测各个方向上的磁场大小，可以计算出偏航角。缺陷就是受到外部磁场的干扰。

（4）GPS检测：直接检测载体在地球上的坐标，只有在大范围移动时才有效。

3.姿态融合与四元数

陀螺仪和加速度传感器都可以测量角度，但是各有缺陷。陀螺仪在静止状态下会产生难以消除的误差，加速度传感器在运动时存在缺陷，两者刚好互补。为了较少数据的误差，我们同时使用这两个传感器，并且设计一个滤波算法。当物体处于静止状态时，增大加速度传感器的数据权重；当处于运动状态时，加大陀螺仪的数据权重。

同理，在检测偏航角时，在静止状态下增大磁场检测的权重，运动状态下增大陀螺仪和GPS的权重。

这种采用多种传感器来检测姿态的算法称为姿态融合。

在姿态融合解算时常常使用四元数来表示姿态。四元数是姿态角的另一种表示方式，它由三个实数和一个虚数组成。在数据处理时使用四元数，处理完之后再转换成欧拉角。

二.MPU6050

MPU6050是六轴传感器，可以同时检测三轴加速度，三轴角速度的运动数据和温度数据。

可以使用STM32控制器将这些数据都读取出来之后进行姿态融合解算，求出传感器当前的偏航角，横滚角，俯仰角。如果使用传感器内部的DMP单元进行解算，它可以直接对采样得到的加速度及角速度进行姿态解算，解算得到的结果再输出给STM32，STM32不需要自己解算。

DMP模块（Digital Motion Processor数字运动处理器），可对传感器数据进行滤波，融合处理，它直接通过I2C接口向主控器输出姿态结算后的姿态数据。非常适合对姿态控制实时要求比较高的领域。常应用于手机，智能手环，四轴飞行器等的姿态检测。

传感器分辨率是如何计算出来的？与ADC分辨率的关系？

传感器的最高分辨率一定是最小量程下，使用ADC分辨率计算出来的。ADC分辨率表示有多少个最小单位。传感器分辨率=ADC分辨率/最小量程。比如：ADC分辨率为12位，最小量程为+-2g，那么加速度传感器的最高分辨率就是2^12/4=16384LSB/g。

三.MPU6050模块

MPU6050模块内部有一个LDO，这决定了MPU6050也可以使用5V的电源输入。一般情况下是3.3V。那么我们可以看到有SCL,SDA（已经连接了上拉电阻，可以直接导线连接）是连接I2C接口的，使用硬件I2C则需要查表确定。XDA,XCL是MPU6050模块提供的第二I2C，一般用来和其他I2C传感器通讯，但实际上效率低，性能较差（使用时要注意连接上拉电阻）。除此之外，还有AD0和INT引脚。AD0决定了MPU6050的从机地址。当AD0接地或者悬空时，地址为0X68（7位地址，最后一位表示读写）,接VCC时，地址是0X69（7位地址）。INT是中断输出引脚，当MPU6050有新数据时，通过INT引脚通知主控，一般悬空或者接地。

MPU6050内部有16位的ADC（数模转换），因此MPU6050传输出去的数据是经过模数转换的数字信息，发送到STM32上时，是不再需要STM32上的ADC进行模数转换了。所以，并不是所有的传感器传输的数据发送到STM32都需要模数转换，要看清楚传感器模块上是否已经具备了ADC。

链接了解更多：

 2. MPU6050模块 — [野火]STM32模块例程介绍 文档 (embedfire.com)

四.写代码

1.MPU6050获得原始数据

I2C初始化(软件模拟)--->MPU6050初始化--->MPU6050_I2C读写函数。

在这个阶段，没有DMP解算，获得的数据只是MPU6050的原始数据，想要获得欧拉角，必须要使用DMP模块。

2.使用DMP解算，获得欧拉角

这一部分的代码较为庞大复杂，主要依靠代码移植来完成。需要使用官方提供的MPL软件库。使用这个软件库需要为它提供I2C读写接口，定时服务以及MPU6050的数据更新标志。如果需要把欧拉角输出到上位机，那么还需要提供串口接口。

MPL库的I2C读写接口的格式是固定的。定时服务同样在inv_mpu.c文件中给出了接口格式。与第一步写的I2C读写函数一致的话，直接使用宏定义就可以替换。

MPL代码库将调试输出函数放到了log_stm32.c文件中，可以为这些函数提供串口输出接口，一边把信息输出到上位机。

MPL代码使用了MPU6050的INT中断信号，为此，需要提供中断接口在.it.c文件中。中断里调用了MPL的某一变量，可以通知MPL库有新的数据。

具体的DMP解算过程，不作详细说明。

程序流程对比：

1.MPU6050在软件模拟I2C时，STM32读取原始数据：

初始化GPIO,I2C,MPU6050--->I2C读写函数--->判断传感器ID是否正确--->使用定时器，一定时间之后，循环读取MPU6050的原始数据

2.MPU6050在软件模拟I2C时，使用DMP模块获得欧拉角：

在1的基础上，需要提供MPL库的各种接口（接口函数需要与其规定的格式相同），设置运算参数，检查是否有数据更新，对数据进行处理。最后，调用read_from_mpl函数向主机输出解算数据。

MPU6050初始化：

MPU6050的初始化主要是通过I2C给MPU6050的寄存器发送数据，设置它的参数。

 
void MPU6050_Init(void)
{
	int i=0,j=0;
	for(j=0;j<1000;i++)
	{
		for(j=0;j<1000;j++)
		{
			;
		}
	}
//此处循环，防止断电之后数据出错
	I2C_GPIO_Config();
		
	/* 电源管理 */
	I2C_WriteData( SLAVEADRESS , PWR_MGMT_1   , 0x00 );
	
	/* 陀螺仪采样率,典型值:0x07(125Hz) */
	I2C_WriteData( SLAVEADRESS , SMPLRT_DIV   , 0x07 );
	
	/* 低通滤波器频率,(5Hz-256Hz) */
	I2C_WriteData( SLAVEADRESS , CONFIG       , 0x06 );	//
	
	/* 陀螺仪自检和测量范围,典型值:0x18(不自检,2000deg/s) */
	I2C_WriteData( SLAVEADRESS , GYRO_CONFIG  , 0x18);
	
	/*加速度自检和测量范围和高通滤波器频率,典型值0x01(不自检2G,5Hz) */
	I2C_WriteData( SLAVEADRESS , ACCEL_CONFIG , 0x01 );
}

包括电源管理，陀螺仪采样率，低通滤波器频率，陀螺仪自检和测量范围，加速度自检和测量范围，高通滤波器频率。

其中，陀螺采样率设置的是分频，陀螺仪输出频率/（分频+1）=采样率。输出频率为1000hz.

电源管理是解除休眠状态。低通滤波器是通低频阻高频的，高通滤波器则相反。