STM32 MPU6050数据获取、数据处理
2.4 STM32 MPU6050数据获取（IIC + DMP）
本篇文章主要针对廉价的MPU6050模块。我们这里完成了MPU6050的数据获取、零偏自动设置、温漂抑制。这里提供源码工程文件，供大家下载，在公众号：小白学移动机器人，发送：MPU6050，即可获得。
2.4.1 解决的问题
DMP库的移植（文件已被更改过，更好的移植）
MPU6050数据的获取（通过DMP获取的四元数，做姿态解算）
零偏自动校准（实现DMP方式上电既是0角度）
有效的温漂抑制（针对yaw值无法滤波的情况，使用特殊的方法，实现原本4分钟漂1度，到30分钟漂一度的改变）
2.4.2 实现工具
STM32F103单片机、GY521六轴模块、keil5
具体线路连接：
这里MCU使用模拟IIC的方式和GY521连接在一起，具体线路连接如下：
GY521 MCU
VCC -----------> VCC 5v
GND -----------> GND
SCL -----------> SCL 模拟SCL的引脚
SDA -----------> SDA 模拟SDA的引脚
XDA -----------> 不接 外接地磁计SDA，这里不接
XCL -----------> 不接 外接地磁计SCL，这里不接
AD0 -----------> 不接
INT -----------> PA12 外部中断使用，这里使用PA12，可以做更改
2.4.3 GY521
MPU-60X0是世界上第一款集成 6 轴MotionTracking设备。它集成了3轴MEMS陀螺仪，3轴MEMS加速度计，以及一个可扩展的数字运动处理器 DMP（ DigitalMotion Processor），可用I2C接口连接一个第三方的数字传感器，比如磁力计。
关于MPU6050的工作原理、以及重要寄存器这里就不做介绍了。网上很多大神都介绍的很清楚，大家可以浏览一下。
参考：点击查看
就算不看也不影响大家解决上面的问题。
2.4.4 DMP库的移植
主要就是移植下面的几个文件，文件会和源码工程文件一起发给大家，网上自己下载的需要更改，这里给的已经更改好了。
移植步骤：
（1）将存放上面文件的DMP文件夹复制到STM32工程文件的HAREWARE（存放配置文件的目录，不同的工程可能名字不一样）文件夹下
（2）设置路径（按序号依次点击）
关于IIC的文件和MPU6050的文件，按照步骤（1）（2）同样操作既可。确保都完成路径添加。
（3）引入文件
IIC和MPU6050的文件也是上面同样的操作，确保所有的文件都被引入如下图所示
（4）到此，DMP移植的相关文件都完成了，下面就是引用头文件开始使用了。
2.4.5 MPU6050硬件初始化
（1）首先，主函数引入头文件
（2）初始化
2.4.6 IIC初始化
IIC串行总线一般有两根信号线，一根是双向的数据线SDA，另一根是时钟线SCL。所有接到I2C总线设备上的串行数据SDA都接到总线的SDA上，各设备的时钟线SCL接到总线的SCL上。
设备上的串行数据线SDA接口电路应该是双向的，输出电路用于向总线上发送数据，输入电路用于接收总线上的数据。而串行时钟线也应是双向的，作为控制总线数据传送的主机，一方面要通过SCL输出电路发送时钟信号，另一方面还要检测总线上的SCL电平，以决定什么时候发送下一个时钟脉冲电平；作为接受主机命令的从机，要按总线上的SCL信号发出或接收SDA上的信号，也可以向SCL线发出低电平信号以延长总线时钟信号周期。
总线空闲时，因各设备都是开漏输出，上拉电阻Rp使SDA和SCL线都保持高电平。任一设备输出的低电平都将使相应的总线信号线变低，也就是说：各设备的SDA是“与”关系，SCL也是“与”关系。
IIC.c中部分
/**************************实现函数********************************************
*函数原型: void IIC_Init(void)
*功 能: 初始化I2C对应的接口引脚。如果不能使用这两个引脚可以自行更改
*******************************************************************************/
void IIC_Init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); //使能PB端口时钟
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8|GPIO_Pin_9; //端口配置
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; //推挽输出
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //50M
GPIO_Init(GPIOB, &GPIO_InitStructure); //根据设定参数初始化GPIOB
}
IIC.h中部分
//IO方向设置
#define SDA_IN() {GPIOB->CRH&=0XFFFFFF0F;GPIOB->CRH|=8<<4;}
#define SDA_OUT() {GPIOB->CRH&=0XFFFFFF0F;GPIOB->CRH|=3<<4;}
//IO操作函数
#define IIC_SCL PBout(8) //SCL
#define IIC_SDA PBout(9) //SDA
#define READ_SDA PBin(9) //输入SDA
其他的IIC读取和写入函数在例程中已完成,我们主要的工作是将任意两端口配置为IIC的SDA和SCL.
2.4.7 MPU6050初始化
/**************************实现函数********************************************
*函数原型: void MPU6050_initialize(void)
*功 能: 初始化 MPU6050 以进入可用状态。
*******************************************************************************/
void MPU6050_initialize(void) {
MPU6050_setClockSource(MPU6050_CLOCK_PLL_YGYRO); //设置时钟
MPU6050_setFullScaleGyroRange(MPU6050_GYRO_FS_2000);//陀螺仪最大量程 +-2000度每秒
MPU6050_setFullScaleAccelRange(MPU6050_ACCEL_FS_2); //加速度度最大量程 +-2G
MPU6050_setSleepEnabled(0); //进入工作状态
MPU6050_setI2CMasterModeEnabled(0); //不让MPU6050 控制AUXI2C
MPU6050_setI2CBypassEnabled(0); //主控制器的I2C与MPU6050的AUXI2C 直通。控制器可以直接访问HMC5883L
}
2.4.8 DMP初始化
/**************************************************************************
函数功能：MPU6050内置DMP的初始化
入口参数：无
返回 值：无
**************************************************************************/
void DMP_Init(void)
{
u8 temp[1]={0};
i2cRead(0x68,0x75,1,temp);
printf("mpu_set_sensor complete ......\r\n");
if(temp[0]!=0x68)NVIC_SystemReset();
if(!mpu_init())
{
if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL))
printf("mpu_set_sensor complete ......\r\n");
if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL))
printf("mpu_configure_fifo complete ......\r\n");
if(!mpu_set_sample_rate(DEFAULT_MPU_HZ))
printf("mpu_set_sample_rate complete ......\r\n");
if(!dmp_load_motion_driver_firmware())
printf("dmp_load_motion_driver_firmware complete ......\r\n");
if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)))
printf("dmp_set_orientation complete ......\r\n");
if(!dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
DMP_FEATURE_GYRO_CAL))
printf("dmp_enable_feature complete ......\r\n");
if(!dmp_set_fifo_rate(DEFAULT_MPU_HZ))
printf("dmp_set_fifo_rate complete ......\r\n");
run_self_test(); //该函数决定了是否设置起始零偏
if(!mpu_set_dmp_state(1))
printf("mpu_set_dmp_state complete ......\r\n");
}
}
此函数主要是对DMP进行初始化,如果想要了解每个函数具体是初始化的什么内容,请看printf函数输出的内容即可。
我们主要讲陀螺仪是如何进行开机校准,或者设置开机不校准。将目光放到run_self_test();这个函数,这是用来设置校准的函数.
static void run_self_test(void)
{
int result;
long gyro[3], accel[3];
result = mpu_run_self_test(gyro, accel);
if (result == 0x03) { //返回0x03为MPU6050六轴，只要通过该if语句，就可以实现零偏自动校准
/* Test passed. We can trust the gyro data here, so let's push it down
* to the DMP.
*/
float sens;
unsigned short accel_sens;
mpu_get_gyro_sens(&sens); //读取当前陀螺仪的状态
gyro[0] = (long)(gyro[0] * sens);
gyro[1] = (long)(gyro[1] * sens);
gyro[2] = (long)(gyro[2] * sens);
dmp_set_gyro_bias(gyro); //根据读取的状态进行校准
mpu_get_accel_sens(&accel_sens); //读取当前加速度计的状态
accel[0] *= accel_sens;
accel[1] *= accel_sens;
accel[2] *= accel_sens;
dmp_set_accel_bias(accel); //根据读取的状态进行校准
printf("setting bias succesfully ......\r\n");
}
}
这样的话,mpu6050校准的过程就明确了.
如果不需要开机校准,可以是if(result==0x03)这个条件不满足,不进入处理,或者将陀螺仪 加速度计的基准设置函数set_bias不执行即可.
如果需要开机校准,保证整个校准流程正常执行就行.
2.4.9 MPU6050数据获取以及温漂抑制
温漂随时间近似线性，不做处理的时候，大概3-4分钟yaw就飘1度。根据采集一段时间数据，计算单位时间内yaw漂移的量，根据时间变化，将对应的漂移量减掉，这样我们就可以得到，半小时只偏差1度的效果，这样已经满足大部分场景了。
(1) main.c
/*===================================================================
程序功能：MPU6050 DMP数据读取
程序编写：公众号：小白学移动机器人
其他 ：如果对代码有任何疑问，可以私信小编，一定会回复的。
=====================================================================
------------------关注公众号，获得更多有趣的分享---------------------
===================================================================*/
int main(void)
{
GPIO_PinRemapConfig(GPIO_Remap_SWJ_Disable,ENABLE);
GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable,ENABLE);//禁用JTAG 启用 SWD
MY_NVIC_PriorityGroupConfig(2); //=====设置中断分组
delay_init(); //=====延时函数初始化
LED_Init(); //=====LED初始化 程序灯
usart3_init(9600); //=====串口3初始化 蓝牙
IIC_Init(); //=====IIC初始化 读取MPU6050数据
MPU6050_initialize(); //=====MPU6050初始化
DMP_Init(); //=====初始化DMP
MBOT_EXTI_Init(); //=====MPU6050 5ms定时中断初始化
while(1)
{
getAngle(&yaw,&yaw_acc_error); //数据获取，包含去除温漂
printf("%d\r\n",(int)yaw);
}
}
(2) 外部中断，根据时间累积温漂
#include "control.h"
#include "led.h"
float yaw =0; //转向陀螺仪
float yaw_acc_error =0; //yaw累积误差
#define FIVE_MS_ERROR 0.00002115 //yaw每5ms的向上漂移的度数，这里近似线性，可以做到半小时偏1度，每个人的这个值可能有所不同，可以自行计算
/**************************************************************************
函数功能：所有的控制代码都在这里面
5ms定时中断由MPU6050的INT引脚触发
**************************************************************************/
void EXTI15_10_IRQHandler(void)
{
EXTI_ClearITPendingBit(EXTI_Line12); //===清除LINE12线路挂起位
yaw_acc_error += FIVE_MS_ERROR; //===yaw漂移误差累加
Led_Flash(200); //===LED闪烁，证明程序正常运行
}
（3）getAngle函数
/**************************************************************************
函数功能：获取角度 0-359
入口参数：无
返回 值：无
**************************************************************************/
void getAngle(float *yaw,float *yaw_acc_error)
{
Read_DMP(); //===读取Yaw(-180 - 179)
if(Yaw < 0)
Yaw = Yaw + 360;
*yaw = Yaw; //===转换yaw( 0 - 359)
*yaw = *yaw - *yaw_acc_error; //===减去yaw随时间的向上漂移
if(*yaw < 0)
*yaw = *yaw+360;
}
2.4.10 总结
本篇文章基本上实现了（IIC+DMP+零偏校准+温漂大幅度抑制），对于ROS小车yaw的精度我们已经实现了。到这里，我们就已经完成了，ROS小车底层的大部分代码，下面剩下STM32与ROS通信。
参考：
MEMS陀螺仪如何进行信号温漂补偿：点击查看
MPU6050 6轴陀螺仪的使用与校准：点击查看
系列文章
搭建ROS小车真的难吗？
ROS小车软件结构以及控制流程
STM32电机PWM控制
STM32电机测速（正交\霍尔编码器）
STM32电机PID速度控制
