无人机项目跟踪记录五十一----高度融合模块详解

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
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
//timeStamp in us. Thread should be executed every 2~20ms
//MS5611_Altitude  , should be in m. (can be fixed to abs, not relative). positive above ground
//accFilted  ,should be filted 
//高度融合模块
void AltitudeCombineThread(void)
{
    static uint32_t tPre=0;
    uint32_t t;
    float dt;    //时间差值

    /* accelerometer bias correction */
    float accel_bias_corr[3] = { 0.0f, 0.0f, 0.0f };  //加速计偏移矫正
    uint8_t i,j;

    t=micros();                                       //当前的计时
    dt = (tPre>0)?((t-tPre)/1000000.0f):0;            //时间间隔
    tPre=t;

    if(!paOffsetInited)	//wait baro to init its offset
    	return;

    if(!imu.ready)
        return;

    //store err when sensor update
    if(Baro_ALT_Updated)	//后面应该在sensor数值后加一个timeStamp，判断是否更新
    {
#ifndef FBM320
        corr_baro = 0 - MS5611_Altitude - z_est[0];		// MS5611_Altitude baro alt, is postive above offset level. not in NED. z_est is in NED frame.
#else
			corr_baro = relPressData*0.1 - z_est[0];  //z方向上的气压高度差
#endif
        Baro_ALT_Updated=0;
    }

    if(accUpdated)
    {
        imu.accb[0] -= acc_bias[0];    //获得角加速度真实值
        imu.accb[1] -= acc_bias[1];
        imu.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]+=imu.DCMgb[j][i]* imu.accb[j]; //将机体系角加速度转换到地理系
            }
        }

        accel_NED[2]=-accel_NED[2];   //地理系z方向的加速度
        corr_acc[2] = accel_NED[2] + CONSTANTS_ONE_G - z_est[2];          //实际测量的加速度与期望的加速度差值，z方向有重力影响
        accUpdated=0;
    }

    //correct accelerometer bias every time step
    accel_bias_corr[2] -= corr_baro * w_z_baro * w_z_baro;     //通过位置来校准z方向加速度偏差    为何如此计算没看懂

    //transform error vector from NED frame to body frame
    for (i = 0; i < 3; i++)
    {
        float c = 0.0f;

        for (j = 0; j < 3; j++) {
					c += imu.DCMgb[i][j] * accel_bias_corr[j];                  //将地理系校准的加速度偏差转换到机体系
        }

        acc_bias[i] += c * w_acc_bias * dt;		//accumulate bias       //将加速度偏差做Pid运算求得期望与实际的加速度偏差值
    }

    acc_bias[2]=-acc_bias[2];  //坐标系方向朝下


    /* inertial filter prediction for altitude */
    inertial_filter_predict(dt, z_est);     //物理定律距离与加速度方程算出期望的位置、速度、加速度
    /* inertial filter correction for altitude */
    inertial_filter_correct(corr_baro, dt, z_est, 0, w_z_baro);	//0.5f    //通过高度位置来校准
    inertial_filter_correct(corr_acc[2], dt, z_est, 2, w_z_acc);		//20.0f  //想通过加速度偏差来校准？

    nav.z=z_est[0];                                 //z方向上期望的位置
    nav.vz=z_est[1];                                //z方向上期望的速度                                
    nav.az=z_est[2];                                //z方向上期望的加速度
}