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

STM32 MPU6050数据获取、数据处理_第1张图片

具体线路连接:

这里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库的移植

主要就是移植下面的几个文件,文件会和源码工程文件一起发给大家,网上自己下载的需要更改,这里给的已经更改好了。

STM32 MPU6050数据获取、数据处理_第2张图片

移植步骤:

(1)将存放上面文件的DMP文件夹复制到STM32工程文件的HAREWARE(存放配置文件的目录,不同的工程可能名字不一样)文件夹下

STM32 MPU6050数据获取、数据处理_第3张图片

(2)设置路径(按序号依次点击)

STM32 MPU6050数据获取、数据处理_第4张图片
关于IIC的文件和MPU6050的文件,按照步骤(1)(2)同样操作既可。确保都完成路径添加。

STM32 MPU6050数据获取、数据处理_第5张图片

(3)引入文件

STM32 MPU6050数据获取、数据处理_第6张图片
STM32 MPU6050数据获取、数据处理_第7张图片

IIC和MPU6050的文件也是上面同样的操作,确保所有的文件都被引入如下图所示

STM32 MPU6050数据获取、数据处理_第8张图片

(4)到此,DMP移植的相关文件都完成了,下面就是引用头文件开始使用了。

2.4.5 MPU6050硬件初始化

(1)首先,主函数引入头文件

STM32 MPU6050数据获取、数据处理_第9张图片

(2)初始化

STM32 MPU6050数据获取、数据处理_第10张图片

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速度控制
在这里插入图片描述

你可能感兴趣的:(ROS小车搭建,MPU6050,STM32,GY521,零偏,温漂)