STM32F103C8t6驱动MPU6050+HMC5883L+BMP280进行三轴姿态解算

---

前言

做了一个星期的姿态解算，遇到了许许多多奇怪的问题，从硬件到代码，特此记录一下，希望能够帮助大家走出困境。

一、STM32F103c8T6的IIC外设驱动（软件IIC）：

1.IIC 简介

IIC使用两根信号线进行通信：一根时钟线SCL，一根数据线SDA。IIC将SCL处于高时SDA拉低的动作作为开始信号，SCL处于高时SDA拉高的动作作为结束信号；传输数据时，SDA在SCL低电平时改变数据，在SCL高电平时保持数据，每个SCL脉冲的高电平传递1位数据。所有IIC设备的SCL连在一起，SDA连在一起；设备的SCL和SDA要配置成开漏输出模式；SCL和SDA个添加一个上拉电阻，阻值一般为4.7K欧姆左右（由于MPU6050内置上拉电阻，故外部不再外接弱上拉电阻）。注：正是由于IIC的开漏输出导致高电平驱动能力不如硬上拉，在速度不断提高的过程中，高电平的保持时间越来越短，由于主从机都要在高电平的时候读取数据，这导致数据不能很好的读取，从而限制了IIC的传输速率。

PS： 这里要注意IIC是为了与低速设备通信而发明的，所以IIC的传输速率比不上SPI，最大为400kHZ。SPI具有四根通信线：SCK、MOSI（Master Output Slave Input）、MISO、SS(Slave Select)，主从根据数据交换来传输数据。SPI根据数据移入移出时刻的不同（SS下降沿移入，时钟的上升沿移入）以及SCK的时钟极性不同分为4种模式。IIC与SPI都是高位先行。

2.软件IIC配置

详细细节参考江科协的配置，本文与其完全一致。

void I2CC_Init(void)
{
	 
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
	
	GPIO_InitTypeDef GPIO_InitStructure;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD;
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10 | GPIO_Pin_11;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	
	GPIO_Init(GPIOB,&GPIO_InitStructure);
	
	GPIO_SetBits(GPIOB, GPIO_Pin_10 | GPIO_Pin_11);
}

二、传感器数据读取与校准

这部分读取的代码很多教程里都有，我这里就只说我校准的思路了。

1.MPU6050：陀螺仪+加速度计数据读取与校准

加速度计和陀螺仪的误差模型

各个轴近似为：Y = KX+B的模型。

（1）陀螺仪粗校准：静置一段时间，读取数据并求平均值当做零偏误差

记录：由于我的Y轴陀螺仪读数会有跳变（量值在250左右），所以取了1000次数据当做平均值，如果无这种错误数据的话其实50次就够了。

void calibrate(void)
{
	uint16_t i;
	int16_t gx,gy,gz,sumx=0,sumy=0,sumz=0;
	for (i=0; i < 1000; i++)
	{
		MPU_Get_GyroRaw(&gx, &gy, &gz);
		sumx += gx;
		sumy += gy;
		sumz += gz;
	}
	gyro_offsetx=-(sumx/1000);
	gyro_offsety=-(sumy/1000);
	gyro_offsetz=-(sumz/1000);
}

（2）加速度校准：LM算法求解y = kx + b的系数校准
我们校准加速度计的目的：校准出绝对水平面。当我们的加速度计不是水平放置的时候，如果用静置求平均来当做偏置量，那么起飞不控飞机，飞机也会朝一个方向飞去。校准加速度计一般有两种方法：六面校准法、重力参考法，由于六面校准需要借助高精度转台进行实验测试，所以此处选择重力参考法。

重力参考法原理：静态放置情况下，无论加速度计的位置在哪，所测的的加速度模值始终应该是当地重力加速度。
校准步骤：1)转动到任意姿态，保持静止3-5s；2)重复执行1，静止时的姿态保持均匀分布任意姿态，采取30个姿态的数据，理论上越多，校准越有效。优化目标如下，其中h近似为h= kx + b的形式：

使用Matlab的LM算法即可求解，代码如下：

clear all;
clc;

load("data.mat"); %自己采集的数据，形式转换成下面axm的样子

% axm=[0.007813   0.100098   0.066162   0.031982   1.070068    -0.939697];
% aym=[-0.061279  -0.019043  -1.013916  0.979248   -0.018555   -0.023438];
% azm=[0.929688   -1.088379  -0.096191  -0.079346  -0.172607   -0.054932];
% am=[axm',aym',azm']; %axm, aym, azm分别是采集的三轴加速度计数据，最好是6个面进行采集
n = size(data,1);
G=ones(n,1);
am = data;
f=@(a,am)(a(1)*am(:,1)+a(2)).^2+(a(3)*am(:,2)+a(4)).^2+(a(5)*am(:,3)+a(6)).^2; %误差构造
a0=[1 0 1 0 1 0];
a=lsqcurvefit(f,a0,am,G)

这里放两张我校准前后的对比图：

2.HMC5883L磁力计数据读取与校准

先简单解释一下磁力计的原理：我们所处的每一个地方，如何没有其他磁场干扰，那么磁场大小和方向是恒定不变的，当我们观察磁场与XYZ轴（与机体坐标系绑定）的夹角就能得到机体姿态的变化。我们校准的时候也用到了这个原理：我们拿着磁力计转圈时，想象一下，矢量不变坐标系转动，可以变换成坐标系不变，矢量绕着坐标系旋转，一个大小不变的矢量旋转就会得到一个球面，这是理想条件下。实际上，我们的周围充斥了很多软磁和硬磁的影响，这导致我们采集到的数据并不是一个球面，而是一个椭球面，我们的校准干的是什么事呢？就是把这个椭球一一映射到球面上的关系求出来。
网上关于原理教程推导的很多，小弟才疏学浅实在看不懂，好在MATLAB中有现成的函数，

[A,b,expmfs] = magcal(D)
校准后的数据CC = (D-b)*A

这里放几张我校准前后的对比图：

注：需要在磁场很干净的地方才能校准成功，之前在显示器之前校准了N次都没有成功（衰）。（后面要研究一下如何转换成C语言，然后在线校准）

3.BMP280气压计数据读取与处理

气压计的数据读取校准和前面两个比起来就简单多了，校准参数也是直接从寄存器里读，这里只放了校准的代码，其实这个代码手册里也有：

double BMP280_Get_Temperature(void)
{
	double var1, var2, temperature_raw, temperature;
	
	BMP280_Get_TemperatureRaw(&temperature_raw);
 
    var1 = (((double) temperature_raw) / 16384.0 - ((double) dig_T1) / 1024.0)* ((double) dig_T2);
    var2 = ((((double) temperature_raw) / 131072.0 - ((double) dig_T1) / 8192.0)
            * (((double) temperature_raw) / 131072.0 - ((double) dig_T1) / 8192.0))* ((double) dig_T3);
    t_fine = (int32_t) (var1 + var2);
    temperature = (var1 + var2) / 5120.0;
 
    return temperature;
}

double BMP280_Get_Pressure(void)
{
	double var1, var2, pressure_Raw, pressure;
	
	BMP280_Get_PressureRaw(&pressure_Raw);

    var1 = ((double) t_fine / 2.0) - 64000.0;
    var2 = var1 * var1 * ((double) dig_P6) / 32768.0;
    var2 = var2 + var1 * ((double) dig_P5) * 2.0;
    var2 = (var2 / 4.0) + (((double) dig_P4) * 65536.0);
    var1 = (((double) dig_P3) * var1 * var1 / 524288.0 + ((double) dig_P2) * var1) / 524288.0;
    var1 = (1.0 + var1 / 32768.0) * ((double) dig_P1);
 
    if (var1 == 0.0) {
        return 0; // avoid exception caused by division by zero
    }
 
    pressure = 1048576.0 - (double) pressure_Raw;
    pressure = (pressure - (var2 / 4096.0)) * 6250.0 / var1;
    var1 = ((double) dig_P9) * pressure * pressure / 2147483648.0;
    var2 = pressure * ((double) dig_P8) / 32768.0;
    pressure = pressure + (var1 + var2 + ((double) dig_P7)) / 16.0;
 
    return pressure;
}

这里气压计数据可以用来定高，但单个气压计的数据肯定是不够的，需要加速度的信息来融合，我这里暂且没研究位置，后面再学习。

三、Mahony互补滤波融合算法

了解这个代码需要先去学习一下无人机的坐标系、坐标系之间的旋转矩阵、姿态表示的欧拉角、四元数等等，这里推荐一个旋转矩阵的博客，我当时看到瞬间就醍醐灌顶了，讲的十分清晰：https://blog.csdn.net/weixin_45632220/article/details/117735223
Mahony互补滤波融合算法这块网上讲解的博客很多，我这里只概括了一个流程图：

部分代码如下，配合上图食用：

/*对加速度计和磁力计数据进行归一化*/
		   // 加速度数据，来源：实际加速度计测量得到(机体系下)
           norm = invSqrt(ax*ax + ay*ay + az*az);
           ax = ax * norm;
           ay = ay * norm;
           az = az * norm;
		   // 磁力计数据，来源：实际磁力计测量得到(机体系下)
           norm = invSqrt(mx*mx + my*my + mz*mz);
           mx = mx * norm;
           my = my * norm;
           mz = mz * norm;
          
		   // mx,my,mz--->hx,hy,hz 机体系--->惯性系
           hx = 2*mx*(0.50 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
           hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.50 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
           hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.50 - q1q1 -q2q2);    
	       //bx：大地坐标系下水平方向磁场；bz：大地坐标系下竖直方向磁场
           bx = sqrt((hx*hx) + (hy*hy));  // 近似计算理论地磁场，就像加速度的[0,0,g]，这里不过变成了[bx,0,bz]
           bz = hz;
		   
           //wx,wy,wz是地磁场在机体坐标系的表示，来源：磁场通过旋转矩阵得到
           wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
           wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
           wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2); 
          
		   //vx,vy,vz是重力加速度在机体坐标系下的表示，来源：[0,0,g]通过旋转矩阵得到
           vx = 2*(q1q3 - q0q2);
           vy = 2*(q0q1 + q2q3);
           vz = q0q0 - q1q1 - q2q2 + q3q3;
		  
		   //ex,ey,ez是加速度计与磁力计测量出的方向与实际重力加速度与地磁场方向的误差，误差用叉积来表示，且加速度计与磁力计的权重是一样的
           // ax-az是加速度测量的
		   ex = (ay*vz - az*vy) + (my*wz - mz*wy);
           ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
           ez = (ax*vy - ay*vx) + (mx*wy - my*wx);

		   //积分误差
           exInt = exInt + ex*Ki*dt;
           eyInt = eyInt + ey*Ki*dt;
           ezInt = ezInt + ez*Ki*dt;
		   
		   // printf("exInt=%0.1f eyInt=%0.1f ezInt=%0.1f ",exInt,eyInt,ezInt);
           // adjusted gyroscope measurements
		   //PI调节陀螺仪数据
           gx = gx + Kp*ex + exInt;
           gy = gy + Kp*ey + eyInt;
           gz = gz + Kp*ez + ezInt;

这里解释两个问题：
1）为什么选择叉乘作为误差项，因为随着我们不断地修正，当旋转矩阵很精确的时候，我们实际测量的和旋转矩阵求出来的就是一样的了，一样的两个向量叉乘就等于0；
2）其次是代码中磁力计的计算，其实磁力也应该有和加速度[0 0 g]这种一样的恒定不变的量，只是每个地方都不一样，我们不可能每个地方的都精确测量到，所以我们用测量的数据来近似得到这个数据，形式为[Mx 0 My]，这里还有一个知识点，我们一般定义大地坐标系为东北天，Y轴正方向指向东，而地磁是恒定指向北的，所以Y轴方向的分量为0。

我的理解就到这里了，我自己做个总结，希望能够帮到你们，有错误的话及时反馈给我，谢谢大家！

总结

硬件总是会让人很头疼，但还是需要耐心一块一块搭建找问题，慢慢的学到的也越来越多，加油吧！