Ardupilot — EKF3使用TOF作为高度源代码梳理

// Main loop - 400hz
void IRAM_ATTR Copter::fast_loop()
{
    ...
    // run EKF state estimator (expensive)
    // --------------------
    read_AHRS();
    ...
}

1.2 void Copter::read_AHRS(void)

读取姿态航向参考系统的信息入口函数。

我们告诉 AHRS 跳过 INS 更新，因为我们已经在 fast_loop() 中进行了更新。

void Copter::read_AHRS(void)
{
    // Perform IMU calculations and get attitude info
    //-----------------------------------------------
#if HIL_MODE != HIL_MODE_DISABLED
    // update hil before ahrs update
    gcs().update_receive();
    gcs().update_send();
#endif

    // we tell AHRS to skip INS update as we have already done it in fast_loop()
    ahrs.update(true);
}

1.3 对象ahrs说明

在 Copter.h 中，我们用 AP_AHRS_NavEKF 类定义了 ahrs 对象。

AP_AHRS_NavEKF ahrs{EKF2, EKF3, AP_AHRS_NavEKF::FLAG_ALWAYS_USE_EKF};

2 AP_AHRS_NavEKF.cpp

2.1 void AP_AHRS_NavEKF::update(bool skip_ins_update)

所以，我们在跳转 update() 这个成员函数的时候，跳转到 AP_AHRS_NavEKF 类的 update() 函数。

void AP_AHRS_NavEKF::update(bool skip_ins_update)
{
    ...
    if (_ekf_type == 2) {
        // if EK2 is primary then run EKF2 first to give it CPU
        // priority
        update_EKF2();
        update_EKF3();
    } else {
        // otherwise run EKF3 first
        update_EKF3();
        update_EKF2();
    }
    ...
}

2.2 void AP_AHRS_NavEKF::update_EKF3(void)

更新 EKF3。

void AP_AHRS_NavEKF::update_EKF3(void)
{
    ...
    if (_ekf3_started) {
        EKF3.UpdateFilter();
    ...
    }
}

2.3 对象EKF3说明

在 AP_AHRS_NavEKF.h 中，我们用 NavEKF3 类定义了 EKF3 对象。

NavEKF3 &EKF3;

3 AP_NavEKF3.cpp

3.1 void IRAM_ATTR NavEKF3::UpdateFilter(void)

所以，我们在跳转 UpdateFilter() 这个成员函数的时候，跳转到 NavEKF3 类的 UpdateFilter() 函数。

更新滤波器状态 - 只要有新的 IMU 数据，就应调用该函数。

// Update Filter States - this should be called whenever new IMU data is available
void IRAM_ATTR NavEKF3::UpdateFilter(void)
{
    if (!core) {
        return;
    }

    imuSampleTime_us = AP_HAL::micros64();

    const AP_InertialSensor &ins = AP::ins();

    bool statePredictEnabled[num_cores];
    for (uint8_t i=0; i<num_cores; i++) {
        // if we have not overrun by more than 3 IMU frames, and we
        // have already used more than 1/3 of the CPU budget for this
        // loop then suppress the prediction step. This allows
        // multiple EKF instances to cooperate on scheduling
        if (core[i].getFramesSincePredict() < (_framesPerPrediction+3) &&
            (AP_HAL::micros() - ins.get_last_update_usec()) > _frameTimeUsec/3) {
            statePredictEnabled[i] = false;
        } else {
            statePredictEnabled[i] = true;
        }
        core[i].UpdateFilter(statePredictEnabled[i]);
    }
    ...
}

3.2 对象core说明

在 AP_NavEKF3.h 中，我们用 NavEKF3_core 类定义了 core 对象。

NavEKF3_core *core = nullptr;

4 AP_NavEKF3_core.cpp

4.1 void IRAM_ATTR NavEKF3_core::UpdateFilter(bool predict)

所以，我们在跳转 UpdateFilter() 这个成员函数的时候，跳转到 NavEKF3_core 类的 UpdateFilter() 函数。

如果缓冲区中有新的 IMU 数据，则运行 EKF 方程，在融合时间跨度上进行估算。

/********************************************************
*                 UPDATE FUNCTIONS                      *
********************************************************/
// Update Filter States - this should be called whenever new IMU data is available
void IRAM_ATTR NavEKF3_core::UpdateFilter(bool predict)
{
    ...
    // Run the EKF equations to estimate at the fusion time horizon if new IMU data is available in the buffer
    if (runUpdates) {
        // Predict states using IMU data from the delayed time horizon
        UpdateStrapdownEquationsNED();

        // Predict the covariance growth
        CovariancePrediction();

        // Update states using  magnetometer or external yaw sensor data
        SelectMagFusion();

        // Update states using GPS and altimeter data
        SelectVelPosFusion();

        // Update states using range beacon data
        SelectRngBcnFusion();

        // Update states using optical flow data
        SelectFlowFusion();

        // Update states using body frame odometry data
        SelectBodyOdomFusion();

        // Update states using airspeed data
        SelectTasFusion();

        // Update states using sideslip constraint assumption for fly-forward vehicles
        SelectBetaFusion();

        // Update the filter status
        updateFilterStatus();
    }
    ...
}

关于 EKF3 的位置和速度融合，Ardupilot 单独分出一个文件。

5 AP_NavEKF3_PosVelFusion.cpp

5.1 void NavEKF3_core::SelectVelPosFusion()

选择融合速度、位置和高度测量值。

/********************************************************
*                   FUSE MEASURED_DATA                  *
********************************************************/
// select fusion of velocity, position and height measurements
void NavEKF3_core::SelectVelPosFusion()
{
    ...
    // Select height data to be fused from the available baro, range finder and GPS sources
    selectHeightForFusion();
    ...
}

5.2 void NavEKF3_core::selectHeightForFusion()

从可用的气压计、测距仪和 GPS 信号源中选择要融合的高度测量值。

/********************************************************
*                   MISC FUNCTIONS                      *
********************************************************/

// select the height measurement to be fused from the available baro, range finder and GPS sources
void NavEKF3_core::selectHeightForFusion()
{
    // Read range finder data and check for new data in the buffer
    // This data is used by both height and optical flow fusion processing
    readRangeFinder();
    rangeDataToFuse = storedRange.recall(rangeDataDelayed,imuDataDelayed.time_ms);

    // correct range data for the body frame position offset relative to the IMU
    // the corrected reading is the reading that would have been taken if the sensor was
    // co-located with the IMU
    if (rangeDataToFuse) {
        AP_RangeFinder_Backend *sensor = frontend->_rng.get_backend(rangeDataDelayed.sensor_idx);
        if (sensor != nullptr) {
            Vector3f posOffsetBody = sensor->get_pos_offset() - accelPosOffset;
            if (!posOffsetBody.is_zero()) {
                Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
                rangeDataDelayed.rng += posOffsetEarth.z / prevTnb.c.z;
            }
        }
    }

    // read baro height data from the sensor and check for new data in the buffer
    readBaroData();
    baroDataToFuse = storedBaro.recall(baroDataDelayed, imuDataDelayed.time_ms);

    // select height source
    if (((frontend->_useRngSwHgt > 0) && (frontend->_altSource == 1)) && (imuSampleTime_ms - rngValidMeaTime_ms < 500)) {
        if (frontend->_altSource == 1) {
            // always use range finder
            activeHgtSource = HGT_SOURCE_RNG;
        }
        ...
    }
    ...
}

5.3 参数_altSource说明

在 AP_NavEKF3.cpp 文件中，我们定义了参数 EK3_ALT_SOURCE 的内容（element）为_altSource。

    // @Param: ALT_SOURCE
    // @DisplayName: Primary altitude sensor source
    // @Description: Primary height sensor used by the EKF. If the selected option cannot be used, baro is used. 1 uses the range finder and only with optical flow navigation (EK2_GPS_TYPE = 3), Do not use "1" for terrain following. NOTE: the EK3_RNG_USE_HGT parameter can be used to switch to range-finder when close to the ground.
    // @Values: 0:Use Baro, 1:Use Range Finder, 2:Use GPS, 3:Use Range Beacon
    // @User: Advanced
    // @RebootRequired: True
    AP_GROUPINFO("ALT_SOURCE", 9, NavEKF3, _altSource, 0),

至此，我们如果将参数 EK3_ALT_SOURCE 设置为 1，那么在此代码处（activeHgtSource = HGT_SOURCE_RNG;），测距仪（TOF）的高度将传入 EKF3 进行运算。