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

最新推荐文章于 2025-08-19 09:31:38 发布

原创

最新推荐文章于 2025-08-19 09:31:38 发布 · 712 阅读

收录于

本文介绍了一个高度融合模块的实现细节，该模块通过结合气压高度传感器数据与加速度计数据来估算无人机的高度、速度和加速度。具体包括时间戳同步、偏差修正、惯性导航系统预测与修正等关键步骤。

这个模块的函数是：void AltitudeCombineThread(void)。函数有点乱，部分没有看懂，加上了注释、以后再详细搞清楚吧！

//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_