MPU9250内部DMP及MPL评测与JY901角度数据对比

最近在网上看到关于MPU9250内部有自带9轴MPL角度解算的功能，因为刚好工作需要，于是在网上查找了一番资料，经过琢磨，目前基本把MPU9250内部的四元数和角度都取出来了；因为之前开发一直使用的是维特智能的JY901模块，模块也是内置MPU9250，只不过是角度什么都处理好，直接输出，而且他们还加入了自己的算法，当时懒得折腾，直接买了几块，省去我大半的时间去琢磨一些算法什么的；刚好看到网上有MPL的技术，就拿来和JY901模块做下对比，具体操作流程及使用效果怎么样，我这边会在下文提及到。

1、前期准备

1.1、材料准备：

材料准备	备注
STM32F103RCT6开发板	在网上搜索都可以买到这种开发板，推荐使用STM32F103系列的，不需要去移植程序，毕竟移植程序也是非常耗时间的一件事，这里注意了，为什么要用RC系列的，因为MPU9250的MPL官方库至少有70K的Flash，选太小Flash芯片，不够存放代码量
MPU9250模块	直接在网上购买现成模块即可，没太多要求，只要I2C和SPI管脚有引出即可，这里我们需要用到I2C，需要用到4个引脚
胶带	用来固定线和MPU9250模块，避免8字校准时扯到线，导致接触不良
杜邦线及护套线	杜邦线用来连接MPU9250模块和开发板；护套线用来串口输出，在条件允许的情况下，最好别用杜邦线，因为整个过程中，会涉及到航向角8字校准，传感器要绕圈画8字，这样只要线稍微不牢固，容易断电，调试效果非常差
外壳	要测量角度，就必须使用外壳固定，千万不要用手拿着，这样测试角度是不标准的
串口USB-TTL	用来打印角度数据，可用串口助手查看数据
J-Link	用于下载程序
JY901模块	用来对比角度数据

1.2、软件准备

软件准备	备注
Keil5	开发环境，以下代码都是在此环境调试开发
9轴MPL示例代码	请在上面的链接下载
串口助手	用于显示角度数据

2、硬件连接

2.1、实物接线

在这里插入图片描述
这里先附上图片，主要两个部分，MPU9250模块（左边紫色模块）和STM32开发板，这里为什么不把MPU9250模块装进外壳里面呢，主要是因为磁场下面不能有电路布线，为了避免对比加入其他不可控因素，所以把他固定在外壳的耳朵；接下来直接讲接线，因为每个人手上的开发板不一样，附上图片也未必能讲清楚，这里直接用芯片和模块的管脚做讲解；

2.2、管脚接线图

3、程序讲解

这里先讲下main函数的整体结构：
在这里插入图片描述
打开程序工程“Template.uvprojx”（请下载1.2章节的MPL示例程序链接，具体的Keil5安装这里就不讲解了）；找到main.c文件，结合以上的程序流程框图，就能了解整个程序的工作流程了；
先附上主函数：


int main(void)
{
	unsigned char ucMPU9250Str[100];
	short sCaliFlag = 0;   //磁场校准标志位
	short sAcc[3],sGyro[3],sMag[3];
	float pitch,roll,yaw; 	        //欧拉角     
	delay_init();                //初始化延时函数
	uart_init(115200);              //初始化USART
	LED_Init();                     //初始化LED 
 //  MPU9250_Init();                     //这个函数可注释
	while(mpu_dmp_init()){;}

    while(1)
    {
			if(mpu_mpl_get_data(sAcc,sGyro,sMag,&pitch,&roll,&yaw,&sCaliFlag)==0)
			{	
				memset(ucMPU9250Str,0,100);
				sprintf(ucMPU9250Str,"pitch：%.3f，roll：%.3f，yaw：%.3f，CaliFlag：%d\r\n", pitch, roll, yaw,sCaliFlag);
				usart1_send_str(ucMPU9250Str);
				LED1=!LED1;//delay_ms(200);
			}
			else {LED0=!LED0;delay_ms(200);}
	  }
}

这里主要讲几个关于MPL的函数，其余的像串口初始化，串口数据打印，I2C读写等一些，就不细讲了，因为上面的示例程序工程，打开后，直接编译，下载就可以直接输出数据了；
首先是 mpu_dmp_init(void)函数：

//返回值:0,正常
//    其他,失败
u8 mpu_dmp_init(void)
{
	u8 res=0;
    struct int_param_s int_param;
    unsigned char accel_fsr;
    unsigned short gyro_rate, gyro_fsr;
    unsigned short compass_fsr;
    
	IIC_Init(); 		        //初始化IIC总线
	if(mpu_init(&int_param)==0)	//初始化MPU9250
	{	 
        res=inv_init_mpl();     //初始化MPL
        if(res)return 1;
        inv_enable_quaternion();
        inv_enable_9x_sensor_fusion();
        inv_enable_fast_nomot();
        inv_enable_gyro_tc();
        inv_enable_vector_compass_cal();
        inv_enable_magnetic_disturbance();
        inv_enable_eMPL_outputs();
        res=inv_start_mpl();    //开启MPL
        if(res)return 1;
		res=mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL|INV_XYZ_COMPASS);//设置所需要的传感器
		if(res)return 2; 
		res=mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);   //设置FIFO
		if(res)return 3; 
		res=mpu_set_sample_rate(DEFAULT_MPU_HZ);	            //设置采样率
		if(res)return 4; 
        res=mpu_set_compass_sample_rate(1000/COMPASS_READ_MS);  //设置磁力计采样率
        if(res)return 5;
        mpu_get_sample_rate(&gyro_rate);
        mpu_get_gyro_fsr(&gyro_fsr);
        mpu_get_accel_fsr(&accel_fsr);
        mpu_get_compass_fsr(&compass_fsr);
        inv_set_gyro_sample_rate(1000000L/gyro_rate);
        inv_set_accel_sample_rate(1000000L/gyro_rate);
        inv_set_compass_sample_rate(COMPASS_READ_MS*1000L);
        inv_set_gyro_orientation_and_scale(
            inv_orientation_matrix_to_scalar(gyro_orientation),(long)gyro_fsr<<15);
        inv_set_accel_orientation_and_scale(
            inv_orientation_matrix_to_scalar(gyro_orientation),(long)accel_fsr<<15);
        inv_set_compass_orientation_and_scale(
            inv_orientation_matrix_to_scalar(comp_orientation),(long)compass_fsr<<15);
            
            
		res=dmp_load_motion_driver_firmware();		             //加载dmp固件
		if(res)return 6; 
		res=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));//设置陀螺仪方向
		if(res)return 7; 
		res=dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_TAP|	            //设置dmp功能
		    DMP_FEATURE_ANDROID_ORIENT|DMP_FEATURE_SEND_RAW_ACCEL|DMP_FEATURE_SEND_CAL_GYRO|
		    DMP_FEATURE_GYRO_CAL);
		if(res)return 8; 
		res=dmp_set_fifo_rate(DEFAULT_MPU_HZ);	//设置DMP输出速率(最大不超过200Hz)
		if(res)return 9;   
		res=run_self_test();		//自检
//		if(res)return 10;    
		res=mpu_set_dmp_state(1);	//使能DMP
		if(res)return 11;     
	}
	return 0;
}

这个函数主要3大功能：
I2C初始化：模拟I2C，只配置IO口就行了；
MPU9250初始化：打开加速度、角速度、磁场，设置各种参数，比如量程，带宽、采样频率等等；
MPL配置：让航向角数据融合磁场；
这个初始化函数里面需要把inv_enable_magnetic_disturbance();注释，否则校准是非常困难的，反正这个函数不注释，我就没校准成功过，画8字几十次都过不了，后面在网上找到资料，要把这个函数注释才行，具体什么原因，后续我继续琢磨；去掉后果然校准可以通过了，这时MPU9250会从6轴算法切换到9轴算法，航向角就是融合了磁场的；默认上电是6轴模式，必须画8字校准才能切换到9轴模式。

接下来讲mpu_mpl_get_data(…)函数：

u8 mpu_mpl_get_data(short sAcc[3], short sGyro[3], short sMag[3], float *pitch,float *roll,float *yaw, short *sCaliFlag)
{
	unsigned long sensor_timestamp,timestamp;
	short gyro[3], accel_short[3],compass_short[3],sensors;
	unsigned char more;
	long compass[3],accel[3],quat[4],temperature; 
    long data[9];
    int8_t accuracy;
    
//	if(dmp_read_fifo(gyro, accel_short, quat, &sensor_timestamp, &sensors,&more)){
//		printf("%d\r\n",dmp_read_fifo(gyro, accel_short, quat, &sensor_timestamp, &sensors,&more));
//		return 1;	 
//	
//	}
	while(dmp_read_fifo(gyro, accel_short, quat, &sensor_timestamp, &sensors,&more)){};
    if(sensors&INV_XYZ_GYRO)
    {
    	sGyro[0] = gyro[0]; sGyro[1] = gyro[1]; sGyro[1] = gyro[1];
        inv_build_gyro(gyro,sensor_timestamp);          //把新数据发送给MPL
        mpu_get_temperature(&temperature,&sensor_timestamp);
        inv_build_temp(temperature,sensor_timestamp);   //把温度值发给MPL，只有陀螺仪需要温度值
    }
    
    if(sensors&INV_XYZ_ACCEL)
    {
        sAcc[0] = accel[0] = (long)accel_short[0];
        sAcc[1] = accel[1] = (long)accel_short[1];
        sAcc[2] = accel[2] = (long)accel_short[2];
        inv_build_accel(accel,0,sensor_timestamp);      //把加速度值发给MPL
    }
    
    if (!mpu_get_compass_reg(compass_short, &sensor_timestamp)) 
    {
        sMag[0] = compass[0]=(long)compass_short[0];
        sMag[1] = compass[1]=(long)compass_short[1];
        sMag[2] = compass[2]=(long)compass_short[2];
        inv_build_compass(compass,0,sensor_timestamp); //把磁力计值发给MPL
    }
    inv_execute_on_data();
    inv_get_sensor_type_euler(data,&accuracy,&timestamp);
    *sCaliFlag = accuracy;
    *roll  = (data[0]/q16);
    *pitch = -(data[1]/q16);
    *yaw   = -data[2] / q16;
	return 0;
}

这里我在官方提供的程序基础上加了4个参数，sAcc，sGyro，sMag，sCaliFlag，之前这个函数只能输出3个角度，然后又在主函数里面，在调函数读取这几个值，我干脆就直接放在通过函数里面了，这样一个函数就可以把加速度、角速度、磁场、角度全部读取出来了，还有一个sCaliFlag标志位，他就是磁场校准的标志位，当等于3时，表示磁场校准通过，否则不通过，这个值我需要用到，所以必须拿到数据，打印到电脑；

主要这几个地方注意到，拿到上面提供的官方示例程序，直接编译后，接上串口，就可以输出MPU9250的数据了。

注意事项

1、接线必须牢固，接完后，必须重复检查管脚是否接错；
2、每个开发板不一样，串口或者I2C管脚配置要根据自己的板子配置，换IO口的需要修改IO口管脚号；
3、MPU9250的MPL，上电默认时6轴，必须磁场画8校准后，才能进入9轴模式；
4、inv_enable_magnetic_disturbance();这个函数最好注释，不注释的画，磁场画8校准过不了；
5、程序移植，无论I2C还是SPI，只需要修改inv_mpu.c文件里面的#define i2c_write MPU_Write_Len
#define i2c_read MPU_Read_Len这两个宏定义，把这两个接口替换成你编写的函数接口即可，如果之前的函数一共有4个形参，你编写的函数最好也和他一样，不要去增加也不要去减少；我之前移植过DMP的SPI接口，可以用，但是MPL的没时间去移植，之前移过一点，然后报了很多错误，又没时间去弄，所以没整成功，这里也只能提供一些其他人的资料给你们了：移植可能需要的代码；

4、数据输出

上面的程序编译后，下载进开发板，串口就可以直接输出了，这里我用串口助手查看数据；
在这里插入图片描述
这里只输出了角度，如果想要加速度、角速度、磁场，可以把sAcc，sGyro，sMag里面的值也打印出来，解算公式：(float)加速度X = sAcc[0]/32768✖16; (float)角速度X = sGyro[0]/32768✖2000;
再次强调，如果想让sCaliFlag磁场校准标志位等于3，通过校准，那最好把inv_enable_magnetic_disturbance();函数注释，示例程序里面是没注释的。

5、效果对比

花了好大功夫去调了MPU9250的MPL，最后发现，效果一般般，只能用于对角度要求不高的场合，如果想做个实验品，作为演示，或者应付下老师的，可以用这种，但如果想做产品，还是别想了，效果完全不行，还是直接买维特的JY901模块吧，毕竟他们公司是专门做这类传感器的，我就是客户推荐我买他们的，本来想着直接用MPL，能降低成本，控制下板子的大小，用个便宜的MCU，不用自己写算法，看来是想多了，其他的不多说了，直接看图吧；
在这里插入图片描述
这里把XY和Z轴（yaw）角度分开讲，主要是yaw角融合了磁场，跟XY不一样的解算方法；