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

写在前面

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

为什么磁力计需要校准

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

图1 磁力计原始数据

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

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

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

图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()来获取。

图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,