磁力计校订和数据处理

在飞控涉及到的几个传感器中磁力计因该是属于最简单的传感器。在飞控上通常使用的磁力计传感器芯片是HMC5883L、HMC598三、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);

山东大学机器人研究中心