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

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

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);
}

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

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();
    }
    ...
}

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

NavEKF3 &EKF3;

// 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]);
    }
    ...
}

NavEKF3_core *core = nullptr;

/********************************************************
*                 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();
    }
    ...
}

/********************************************************
*                   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();
    ...
}

/********************************************************
*                   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;
        }
        ...
    }
    ...
}

    // @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),