磁力计校正和数据处理

在飞控涉及到的几个传感器中磁力计因该是属于最简单的传感器。在飞控上一般使用的磁力计传感器芯片是HMC5883L、HMC5983、MPU9250、MAG3110等。这些芯片利用的物理原理都相同，只是在精度和数据接口上有些区别。下面简单说说磁力计的原理。
我们的地球都是有磁场存在的，地理的南极是地磁的北极，在地球的每一个区域都是受地磁影响的，可以说地磁无处不在。而且地磁的方向都是一定的，除了在南北极地磁场是从上指到下（或者从下指到上）的，在其他区域基本都是由南指向北的（暂不考虑磁偏角）。这样就给人类的远行活动带了极大的便利。由于地磁场的这个特性我们可以使用罗盘（指南针）很容易的在地球的任何角落辨别出南北，随之另外两个方向东西也能确定下来。磁力计的原理就是使用霍尔元件的原理测量磁场的传感器。在实际的空间中只要不在赤道上空地磁场的方向有从南指向北的分量，还有从上指向下（或者从下指向上的分量）。霍尔元件只能测量一个方向的磁场，所以磁力计要在空间中测量磁场的话要使用三个方向的霍尔传感器测量到三个正交的方向的磁场后合成一个合磁场，再使用这个磁场的方向和地磁场的方向做对比从而可以获得传感器自身的方向和姿态。这些是基于一个比较普遍的假设：在很小的区域可以认为地磁场的方向和大小都是不变的。很小的区域相对于地球来说可能是一个或者几个国家的大小。
下面使用stm32芯片读取hmc5883的磁场数据然后使用matlab对数据进行处理。需要工程代码的可以在留言中留下邮箱号向我要，我看到就会发给你。下面是工程主循环中的代码：

while(1)
{
Delay(200);
X_Data = Single_Read(HMC5983_Write_Addr,0x03);
X_Data <<= 8;
X_Data += Single_Read(HMC5983_Write_Addr,0x04);
Z_Data = Single_Read(HMC5983_Write_Addr,0x05);
Z_Data <<= 8;
Z_Data += Single_Read(HMC5983_Write_Addr,0x06);
Y_Data = Single_Read(HMC5983_Write_Addr,0x07);
Y_Data <<= 8;
Y_Data += Single_Read(HMC5983_Write_Addr,0x08);
printf(“%d %d %d “,X_Data,Y_Data,Z_Data);
}

读取三轴磁场的数据后按顺序输出数据。在PC端使用串口终端接收数据。在接收数据的过程中不断变化传感器的方向，由三个分量组成的空间向量其实就是地磁场相对于传感器的位置向量。我们在各个位置旋转传感器，这样由每个时刻的向量的端点组成的一个图应该是一个以原点为圆心的球。下面我们采集数据。

在MATLAB中数据全部放在数组A中，下面是MATLAB代码：

与我们想象的不同，画出来的图形并不是一个球而是一个椭球，中心也不在原点。

再看看两个轴X、Y之间的图形（三维空间点的投影）。

很好！是一个圆，Y轴的数据分布在0的上下，但是X轴的数据偏离了0点。由于传感器在磁场的正反两个方向测量到的数据应该是大小相同符号相反的数据，也就是它应该是一个以原点为圆心的圆。现在这种情况说明数据是有零点漂移的，校正的方法很简单，我们可以通过matlab的数组X可以看出X的最大值是676最小值为-499，对最大值和最小值作和然后除以2就是X需要减去的零点补偿值即：(676-499)/2 = 88.5。然后以后每次采集到的X轴数据减去一个零点漂移值88.5就能校正这个轴的数据。
MATLAB代码：

Y = Y+25;
再次画图如下:

对Z轴的数据没有在原点对齐所以也需要类似的处理

做了零点漂移校正后的数据如下：

matlab代码：
Z = Z-149.5
此时我们在代码中需要做的就是对每一次采集到的数据都减去相应的校正值。对单片机编程的C代码应该改成如下：
X_Data = Single_Read(HMC5983_Write_Addr,0x03);
X_Data <<= 8;
X_Data += Single_Read(HMC5983_Write_Addr,0x04);
X_Data += 25;
Z_Data = Single_Read(HMC5983_Write_Addr,0x05);
Z_Data <<= 8;
Z_Data += Single_Read(HMC5983_Write_Addr,0x06);
Z_Data -= 149;
Y_Data = Single_Read(HMC5983_Write_Addr,0x07);
Y_Data <<= 8;
Y_Data += Single_Read(HMC5983_Write_Addr,0x08);
Y_Data += 25;

从上面的数据我们也看到，对于零点漂移校正的三轴数据，仍旧不是一个中心在原点的球，而是一个椭球（半径不同）。X轴上是587，Y轴上是571，Z轴上是638。下面就需要进行椭球校正，这个原理也很简单。由于三个轴上的传感器的测量敏感度不同导致了数据的幅值不同，因此我们需要做的就是以一个轴的数据为标准对其余两个轴的数据进行加权。下面是具体操作：
以X轴为标准
Y = Y*(Xmax-Xmin)/(Ymax-Ymin);
Z = Z*(Xmax-Xmin)/(Zmax-Zmin);

这样就将图形转化成了一个半径为587.5的圆球了。

对应的单片机的C代码改成：

X_Data = Single_Read(HMC5983_Write_Addr,0x03);
X_Data <<= 8;
X_Data += Single_Read(HMC5983_Write_Addr,0x04);
X_Data += 25;
Z_Data = Single_Read(HMC5983_Write_Addr,0x05);
Z_Data <<= 8;
Z_Data += Single_Read(HMC5983_Write_Addr,0x06);
Z_Data -= 149;
Z_Data = Z_Data*(587.5)/638.5;
Y_Data = Single_Read(HMC5983_Write_Addr,0x07);
Y_Data <<= 8;
Y_Data += Single_Read(HMC5983_Write_Addr,0x08);
Y_Data += 25;

下面如果传感器是水平的，对应飞行器平衡的时候需要求解角度为
Angle = arctan(X_Data/Y_Data);

                                            山东大学机器人研究中心