mpu9250磁力计校准 mpl库数据校准

mpu9250磁力计校准 mpl库数据校准

      • 写在前面
      • 为什么磁力计需要校准
      • 官方的mpl库简介
      • 如何磁力计校准以及保存校准数据
      • 参考代码
      • 总结

写在前面

 前段时间弄了MPU9250,也就是9轴传感器,用的是官方的mpl库。但是读的欧拉角翻滚角和俯仰角都很准,就是航向角不准。快速的转动一下,再回到原来的角度,航向角就偏移了几十度,完全达不到预期。因为航向角需要融合磁力计数据但是磁力计需要校准,一开始不知道怎么校准磁力计数据所以这个问题困扰了我好久,直到最近才弄好,所以想把我的一些经验分享给大家,以免大家遇到同样的困扰。

为什么磁力计需要校准

 至于为什么磁力计需要校准,大家看下下面这几个图就应该清楚了,有需要椭圆拟合Matlab程序的可以直接百度云下载,链接:https://pan.baidu.com/s/1eQ_A84vkUf3B_sdN6DUvQg 提取码:mpyn
mpu9250磁力计校准 mpl库数据校准_第1张图片

图1 磁力计原始数据

 如图1所示是我通过串口助手获取的原始磁力计数据,然后再将数据导入excel表格,在matlab上面显示的结果。大家应该可以清楚的发现,磁力计读出的数据是个椭球。这是因为磁力计是三个互相垂直的霍尔元件组成的一个传感器,每个轴的数据都是地磁场在每个轴的分量,但是由于传感器之间有差异所以导致椭圆的轴并不相同,而且大家可以从图上看出磁力传感器和加速度传感器一样,磁力计的数据原点并不是0,所以我们还要进行椭圆拟合求出,椭球的原点和椭球的轴长。
mpu9250磁力计校准 mpl库数据校准_第2张图片

图2 通过matlab拟合后的图形显示

 上图2是我通过matlab拟合椭圆中心和轴长的数据,说到椭球拟合也是费了我不少功夫,我也是在一篇博客上看到一个博主推到的公式,以前没怎么弄过matlab,所以又花了时间把matlab看了下才写出的这个拟合函数。
mpu9250磁力计校准 mpl库数据校准_第3张图片

图3 matlab拟合后的结果

 图3是我通过matlab拟合后的结果,但是拟合好数据又要将数据融合到世界坐标系,然后又是各种资料,各种数学公式,又困扰了我好久,最后感觉自己实在弄不好,就又重新看了下mpl的库文档,发现库文档上面说mpl库会自动校准数据,让传感器做8字运动会加快磁力计校准,所以我又重新试了下,8字运动了一两分钟后发现果然可以了,数据很准。相比于我们网上很多的数据融合算法,官方的mpl库还是更强大一些,毕竟是大公司的产品。

官方的mpl库简介

 官方的mpl库具有以下特点:
 1.陀螺仪实时校准,只要检测到陀螺仪停止5s就回重新校准陀螺仪。
 2.陀螺仪温度补偿。
 3.磁力计校准,只要收集到足够的数据就会在算法中融合磁力计数据,不然就只融合6轴传感器数据,做8字运动会加快校准。
 4.抗强磁干扰,当有强磁干扰的时候会启动6轴融合,不融合磁力计数据。

如何磁力计校准以及保存校准数据

 但是我们怎么知道数据什么时候融合好呢?其中有一个这个函数,

int inv_get_sensor_type_euler(long *data, int8_t *accuracy, inv_time_t *timestamp)

大家可以获取accuracy的值,在没有磁力计没有校准钱accuracy为0,校准后值为3,大家可以一直把这个值显示出来,在校准好数据前一直让传感器做8字运动,加快校准。
但是每次校准都要不少时间每次重启都要校准的话岂不是很麻烦,当然这个问题官方也肯定想到了。
其中有三个函数是关于储存校准数据的:

inv_error_t inv_get_mpl_state_size(size_t *size);
inv_error_t inv_load_mpl_states(const unsigned char *data, size_t len);
inv_error_t inv_save_mpl_states(unsigned char *data, size_t len);

 inv_save_mpl_states()此函数保存校准的数据,大家只要把数据通过flash存下来就好了,然后下次上电的时候通过inv_load_mpl_states()函数重新读取下数据。至于存放数据的大小可以同此函数inv_get_mpl_state_size()来获取。
mpu9250磁力计校准 mpl库数据校准_第4张图片

图4 校准后的数据结果

 如图4所示为校准后的数据结果,我们可以发现在校准后第一个数据还是1.3,第二个数据才变成正在的校准后的数据,所以我们在使用的时候可以把第一个结果舍去。
 还要一点需要注意的是,我在测试过程中发现数据校准好了,也就是accuracy值为3了,但是一会accuracy又变为0了,这事大家可以把强磁干扰给关了,这样校准好数据后accuracy值就不会变为0了。至于怎么关,我用的是正点原子的程序改的,只要把mpu_dmp_init()函数中的inv_enable_magnetic_disturbance()使能抗强磁干扰给关了就行了。

参考代码

 这是我写的校准代码,大家可以参考下,功能是将mpu获取的欧拉角数据和accuracy值通过串口打印出来,如果accuracy为3,说明数据校准完成,当按下按键时就把校准数据保存到flash中,下次再启动则直接读取上传的校准数据。

//MAIN函数
int main(void)
{
	u8 res;
	u32 size;
	u8 key;
	float pitch,roll,yaw;
	Cache_Enable();                 //打开L1-Cache
	HAL_Init();				        //初始化HAL库
	Stm32_Clock_Init(160,5,2,4);    //设置时钟,400Mhz 
	delay_init(400);				//延时初始化
	uart_init(256000);				//串口初始化
	LED_Init();						//初始化LED
	KEY_Init();						//初始化按键
	while(MPU9250_Init())
	{
		printf("MPU9250_Init Error\r\n");
		delay_ms(1000);
	}  
	LCD_Init();	   
	res=mpu_dmp_init();
	printf("res:%d\r\n",res);
	LCD_Clear(WHITE);      //清屏  
	Draw_Font16B(16,16,RED, (u8 *)"MPU TEST");
	NORFLASH_Init();				//NORFLASH(W25Q64)初始化
	if(NORFLASH_ReadID()!=W25Q64)
	{
		printf("W25Q64 error\r\n");
		delay_ms(500);
		LED0_Toggle;
	}
	else 
	{
		printf("W25Q64 ok\r\n");
	}
	NORFLASH_Read(mpudateBuff,MpuSaveDateAddr,sizeof(mpudateBuff));//读取mpu9250校准数据
	if(inv_load_mpl_states(mpudateBuff,sizeof(mpudateBuff))==INV_SUCCESS)
	{
		printf("=======inv_load_mpl_states ok====================\r\n");
	}
	else 
	{
		printf("=======inv_load_mpl_states error====================\r\n");
	}
	while(1)
	{
		LED0_Toggle;
		key=KEY_Scan(0);
		if(key==KEY1_PRES)
		{
			printf("key1_press====================\r\n");
			
			if(inv_get_mpl_state_size(&size)==INV_SUCCESS)
			{
				printf("=======inv_get_mpl_state_size ok====================\r\n");
				printf("lensize:%d\r\n",size);
				if(inv_save_mpl_states(mpudateBuff,sizeof(mpudateBuff))==INV_SUCCESS)
				{
					printf("=======inv_save_mpl_states ok====================\r\n");
					NORFLASH_Write((u8*)mpudateBuff,MpuSaveDateAddr,sizeof(mpudateBuff));
				}
				else
				{
					printf("=======inv_save_mpl_states error====================\r\n");
				}
				
			}
		}
		if(mpu_mpl_get_data(&pitch,&roll,&yaw)==0)
		{
			printf("pitch:%.1f,roll:%.1f,yaw:%.1f\r\n",pitch,roll,yaw);
		}
        delay_us(10000);                           //延时1s,也就是1000个时钟节拍	
	}
}
//mpu_mpl_get_data欧拉角获取函数
u8 mpu_mpl_get_data(float *pitch,float *roll,float *yaw)
{
	unsigned long sensor_timestamp=0,timestamp=0;
	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))return 1;	 
    if(sensors&INV_XYZ_GYRO)
    {
        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)
    {
        accel[0] = (long)accel_short[0];
        accel[1] = (long)accel_short[1];
        accel[2] = (long)accel_short[2];
        inv_build_accel(accel,0,sensor_timestamp);      //把加速度值发给MPL
    }
    if (!mpu_get_compass_reg(compass_short, &sensor_timestamp)) 
    {
        compass[0]=(long)compass_short[0];
        compass[1]=(long)compass_short[1];
        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);

    *roll  = (data[0]/q16);
    *pitch = -(data[1]/q16);
    *yaw   = -data[2] / q16;
	printf("accuracy %d\r\n",accuracy);
	return 0;
}

总结

 总结一下我遇到的问题:
 1.如果用杜邦线连mpu9250的话杜邦线要尽量短,不然读出来的设备地址可能就是0xff或者0xd1等等了,我是把杜邦线剪短再重新用热缩管接好才解决这个问题的。
 2.mpu9250要正这放,不然自检通不过。
 3.通过inv_get_sensor_type_euler()函数来获取accuracy值来确定磁力计数据是否校准好,为3则校准好,为0没校准好。
 4.做8字校准会加快校准速度,如果如果2分钟也没校准好也不要急慢慢来。
 5.通过inv_save_mpl_states()等函数来保存校准数据。

你可能感兴趣的:(九轴传感器,传感器,c语言,mpl,matlab,编程语言)