基于STM32F407四旋翼无人机—AK8975磁力计（四）


在这里插入图片描述@TOC

1、磁力计简单介绍

1.1、磁力计简介

1.2、磁力计原理图

2、磁力计数据获取
3、磁力计椭球拟合校准

3.1、简单介绍椭球拟合

3.2、校准代码

磁力计基本介绍

该模块采用高灵敏度霍尔传感器技术，通过IIC读取X,Y,Z轴的磁力计数据，通过解算磁力计数据得出比较可靠的偏航角，由于只通过MPU6050解算出来的欧拉角，偏航角并不准确，则需要通过磁力计解算出来的偏航角去修正，得到准确的偏航。

1.2、磁力计原理图

磁力计原理图如下：由于IIC可以同时挂3个设备，就可以把MPU6050、MS5611和AK8975用两个普通IO模拟的IIC时序读取数据。
在这里插入图片描述

2、磁力计数据获取

首先定义AK8975的寄存器，通过IIC发送命令获取X,Y,Z轴的磁力计数据：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
//读取三轴磁力计数据
 IIC_Read_1Byte(AK8975_ADDR,AK8975_HXL,&AK8975_buffer[0]);
 IIC_Read_1Byte(AK8975_ADDR,AK8975_HXH,&AK8975_buffer[1]);
 IIC_Read_1Byte(AK8975_ADDR,AK8975_HYL,&AK8975_buffer[2]);
 IIC_Read_1Byte(AK8975_ADDR,AK8975_HYH,&AK8975_buffer[3]);
 IIC_Read_1Byte(AK8975_ADDR,AK8975_HZL,&AK8975_buffer[4]);
 IIC_Read_1Byte(AK8975_ADDR,AK8975_HZH,&AK8975_buffer[5]);

 AK8975_temp[1] = ((((int16_t)AK8975_buffer[1])<<8) | AK8975_buffer[0]);//磁力计X轴
 AK8975_temp[0] = ((((int16_t)AK8975_buffer[3])<<8) | AK8975_buffer[2]);//磁力计Y轴
 AK8975_temp[2] = -((((int16_t)AK8975_buffer[5])<<8) | AK8975_buffer[4]);//磁力计Z轴
 
 AK8975.Mag_Adc.x = AK8975_temp[0];
 AK8975.Mag_Adc.y = AK8975_temp[1];
 AK8975.Mag_Adc.z = AK8975_temp[2];

 AK8975.Mag_Val.x = AK8975.Mag_Adc.x - AK8975.Mag_Offset.x;
 AK8975.Mag_Val.y = AK8975.Mag_Adc.y - AK8975.Mag_Offset.y;
 AK8975.Mag_Val.z = AK8975.Mag_Adc.z - AK8975.Mag_Offset.z;
 
 WHY.MAGX = AK8975.Mag_Val.x;
 WHY.MAGY = AK8975.Mag_Val.y;
 WHY.MAGZ = AK8975.Mag_Val.z;

 AK8975_CalOffset();  //判断时候有磁力计校准
 /*由于AK8975只有单次测量模式，所以每次读出数据之后要重新设置模式以便下次读取*/
 IIC_Write_1Byte(AK8975_ADDR,AK8975_CNTL,0x01);

3、磁力计椭球拟合校准

3.1、简单介绍椭球拟合

传统的传感器标定补偿方法主要有四位置法、八位置法等， 存在计算量大、操作复杂、不易实现等不足， 尤其是三轴磁传感器的标定补偿， 对标定设备要求极高， 十分容易引入外界磁干扰， 从而导致结果不准确。加之， 现有多轴矢量传感器性能参数的标定补偿方法主要是针对各单轴传感器的安装误差进行补偿， 而忽略了对多轴矢量传感器中各单轴传感器的零偏误差、灵敏度误差的同时补偿。

三轴磁传感器轴间的不正交误差是指由于在制造过程中三个磁传感器的测量轴无法保证两两完全正交所引起， 其不正交角模型如图 1 所示。
在这里插入图片描述
如图 1 所示， ==X0、Y0、Z0 表示理想正交模型中三轴磁传感器的矢量指向; X、Y、Z 表示实际磁传感器三轴的指向; 假设 Z 轴与正交模型中的 Z0 轴重合，且 XOZ 面与 X0OZ0 面重合， α 为 X 轴在 XOZ 面与轴 X0 的夹角; β 为 Y 在 X0OY0 面的投影与 Y0 的夹角; γ 为 Y 与 X0OY0 面的夹角。==由此可建立三轴磁传感器不正交角的数学模型如下:
在这里插入图片描述
三轴加速度传感器坐标系 O-xbybzb 与地理坐标系 O-xgygzg 关系如图 2 所示重力矢量 g垂直向下分别表示载体的俯仰角和滚转角 某姿态下加速度传感器的测量值为 A=（ax，ay，az）T重力矢量在加速度传感器各轴的分量为 ：
在这里插入图片描述

理想的三轴加速度传感器在任意姿态下加速度模值,根号下(gx)^2 + (gy)2+(gz)2 是固定值 g遥测量值 A 的轨迹是一个以 1g 为半径的重力圆球面袁实际由于加速度传感器的三轴存在各种误差袁输出轨迹会是一个中心偏离坐标原点的椭球,所以要校准到原点在中心的椭球。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
static xyz_t MagMAX = { -100 , -100 , -100 }, MagMIN = { 100 , 100 , 100 }, MagSum;
 static uint16_t cnt_m=0;
  if(Mag_CALIBRATED)
  {
  if(ABS(AK8975.Mag_Adc.x)<400 && ABS(AK8975.Mag_Adc.y)<400 && ABS(AK8975.Mag_Adc.z)<400)
   {
    MagMAX.x = _MAX(AK8975.Mag_Adc.x, MagMAX.x);
    MagMAX.y = _MAX(AK8975.Mag_Adc.y, MagMAX.y);
    MagMAX.z = _MAX(AK8975.Mag_Adc.z, MagMAX.z);
 
    MagMIN.x = _MIN(AK8975.Mag_Adc.x, MagMIN.x);
    MagMIN.y = _MIN(AK8975.Mag_Adc.y, MagMIN.y);
    MagMIN.z = _MIN(AK8975.Mag_Adc.z, MagMIN.z);
    if(cnt_m == Calibration_time)
    {
     AK8975.Mag_Offset.x = (int16_t)((MagMAX.x + MagMIN.x) * 0.5f);
     AK8975.Mag_Offset.y = (int16_t)((MagMAX.y + MagMIN.y) * 0.5f);
     AK8975.Mag_Offset.z = (int16_t)((MagMAX.z + MagMIN.z) * 0.5f);

     MagSum.x = MagMAX.x - MagMIN.x;
     MagSum.y = MagMAX.y - MagMIN.y;
     MagSum.z = MagMAX.z - MagMIN.z;
     
     AK8975.Mag_Gain.y = MagSum.x / MagSum.y;
     AK8975.Mag_Gain.z = MagSum.x / MagSum.z;

     Param_SaveMagOffset(&AK8975.Mag_Offset);//保存数据
     cnt_m = 0;
     Mag_CALIBRATED = 0;
     }
    }
    cnt_m++;
  }

由于惯性元件都已经介绍完了，就要利用这些数据做姿态解算了。