小马哥robofly四轴气压计定高的不完全解读

我看过正点原子的定高算法，好像是直接用气压计测量得到的高度作为反馈进行，而小马哥的robofly是结合气压计获得的高度和运动情况下获得Z轴的位移2者结合来获取高度，但是2者结合的过程看得一知半解，下面仅提供思考的思路，仅做参考，互相学习。（其实robofly并没有定高功能，或者说定高仅是通过单级PID进行控制）

气压计定高基本的思路是：多传感器融合定高

• 通过传感器检测飞行器或无人机当前的气压和加速度数据
• 利用当前的气压数据计算高度变化，利用当前加速度数据计算高度变化
• 根据所述气压数据计算的高度变化对加速度数据计算得出的位移大小进行修正，修正加速度计算的高度中时间累积的影响，根据加速度数据计算的高度修正气流波动对气压计算高度变化的影响，将利用气压和加速度数据计算的高度变化进行相互校正获得飞行器高度数据进行融合
• 根据融合的数据确定飞行器的当前高度

当气压大幅度波动但加速度变化不明显时，此时对位移的计算以加速度为主要参考量，气压只是将新测量到的气压值作为目标值，逐步逼近，如果后面测量到的气压又恢复到原来值附近，目标值会变回原来值，让气压波动的影响变得非常小，如果新测量到的气压值一直维持大的变化，没有回复到原来值附近，气压结果就趋向新测量到的值，而且趋近速度会越来越快，保证良好的响应速度

如果气压值恒定平稳，加速度值平均变化明显，对速度和位移的计算以气压值为主； 如果加速度平均值相对恒定，气压值变化明显，对速度和位移的计算以加速度值为主； 如果气压值恒定平稳，加速度平均值相对恒定，对速度和位移的计算长期以气压结果为准、瞬时以加速度结果为准，两者相互融合； 如果气压值和加速度值平均值变化均明显，当两者变化一致时认为飞机在运动，当两者不一致时认为传感器异常，进入保护状态，引入经验值辅助修正计算结果

根据加速度计算的速度和位移随着温升的在一个方向上快速变化，对加速度计的温漂进行修正，加速度计修正后的结果和气压结果进行融合，减小气压计温漂的影响

nav_t nav;		  //NED frame in earth
extern uint8_t AccbUpdate;
uint8_t Altitude_mode = 0;
float Altitude = 2;   //进行定高控制的高度，2米
float z_est[3];	  // estimate z Vz  Az    通过加速度和速度预估无人机在Z轴上的位移
static float w_z_baro=10.0f;   //这3个不知道是啥，可能是常量
static float w_z_acc=30.0f;
static float w_acc_bias=0.05f;

/* acceleration in NED frame */  
float accel_NED[3] = { 0.0f, 0.0f, -G };   //地球坐标性下的加速度，Z轴选垂直向上为正方向
/* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */
float corr_acc[] = { 0.0f, 0.0f, 0.0f };	// N E D ,  m/s2
float acc_bias[] = { 0.0f, 0.0f, 0.0f };	// body frame , 这是机体坐标的加速度，需要乘以转化矩阵=地球坐标系
float corr_baro = 0.0f;					    //m（气压计矫正系数）

static void inertial_filter_predict(float dt, float x[2],float acc);  //得到机体的位移和速度
static void inertial_filter_correct(float e, float dt, float x[3], int i, float w);  //

/* 惯性滤波器的位移与速度预测 */
static void inertial_filter_predict(float dt, float x[2],float acc)
{
    x[0] += x[1] * dt + acc * dt * dt / 2.0f; // 位移 S = Vt + At^2/2 
    x[1] += acc * dt;                         // 速度 V = At
}

static void inertial_filter_correct(float e, float dt, float x[3], int i, float w)
{
    float ewdt = e * w * dt;
    x[i] += ewdt;

    if (i == 0) 
	{
		x[1] += w * ewdt;
		x[2] += w * w * ewdt / 3.0f;
	}else if (i == 1) 
	{
        x[2] += w * ewdt;
    }
}

主要看下面进行融合高度的函数：

void Altitude_Combine(void)
{
	uint8_t i,j;
	float dt = 0.005; //次程序没次被调用的时间间隔
	float accel_bias_corr[3] = { 0.0f, 0.0f, 0.0f }; /* 加速度计的偏移矫正 */
	
	if(AccbUpdate)  //这个的判断条件在imu.c里面
	{
		accb[0] -= acc_bias[0];  //去零偏处理，这里无论是mpu6050还是其他传感器都要进行去零偏处理
		accb[1] -= acc_bias[1];
		accb[2] -= acc_bias[2];
		
		for(i=0; i<3; i++)
		{
			accel_NED[i]=0.0f;
			for(j=0; j<3; j++)
			{
				accel_NED[i] += DCMgb[j][i]* accb[j];  //二重循环进行矩阵的相乘（1 3 ）*（3 3）=（1 3）
			}
		}
		accel_NED[2]=-accel_NED[2];   //注意方向的正负
		corr_acc[2] = accel_NED[2] + G - z_est[2]; //corr_acc[] 机身坐标系 转换到 地理坐标系的加速度
		AccbUpdate = 0;
	} 
	if(ALT_Updated)
	{
		corr_baro = 0 - FBM.AltitudeFilter - z_est[0];    //初始化值为零 测量值与估计值求差
		ALT_Updated = 0;
	}
	 
	accel_bias_corr[2] -= corr_baro * w_z_baro * w_z_baro; //这里不知道为啥
	 
	if(accel_bias_corr[2])
	{
		for (i = 0; i < 3; i++)  //矩阵相乘，其实这里的c也可定义为数组c[3]
		{
			float c = 0.0f;
			for (j = 0; j < 3; j++) 
			{
				c += DCMgb[i][j] * accel_bias_corr[j];
			}
			acc_bias[i] += c * w_acc_bias * dt;		//accumulate bias偏差积分（acc_bias[i] 较正后的机体系下的加速度）
		}
		acc_bias[2]=-acc_bias[2];

		/* 惯性滤波器的位移与速度预测 */
		inertial_filter_predict(dt, z_est,z_est[2]);
		/* 高度惯性滤波器的较正 */
		inertial_filter_correct(corr_baro, dt, z_est, 0, w_z_baro);   	//0.5f (corr_baro气压计较正系数 ;  z_est预测的Z轴数据 z , vz , az)
		inertial_filter_correct(corr_acc[2], dt, z_est, 2, w_z_acc);		//20.0f(corr_acc[] 机身坐标系 转换到 地理坐标系的加速度值 ;  z_est预测的Z轴数据 z , vz , az)
	}
	nav.z =z_est[0];  //以上都是为了获取nav.z但是后面的程序中却没有用到
    nav.vz=z_est[1];
    nav.az=z_est[2];
	
//	printf("nav.z:%0.2f  nav.vz:%0.2f  nav.az:%0.2f  Altiude:%0.2f\r\n",nav.z,nav.vz,nav.az,FBM.Altitude);
}

//下面的其实就是简单的PID处理
void Altitude_Control(void)
{
	PID_Postion_Cal(&PID_ALT,Altitude,FBM.AltitudeFilter);
	
	if(Altitude_mode && RC_Control.THROTTLE>200)
	THROTTLE = 480 + PID_ALT.OutPut;
}

我并没有获取到FBM320的数据，但是它却在I2C的线路上，可能是有虚焊，讲一下焊接FBM320的技巧吧，先在焊盘上上锡，然后涂上助焊剂，用镊子夹住气压计，并固定（手抖症后期患者误解），用热吹风枪吹即可，或者在焊盘上涂上 焊锡膏 ，一定要用镊子固定住，用热吹风枪吹。因为我用的是BMP280，但还没移植成功，移植成功了再放到博客上。

互相学习批评指正！