예제 #1
0
void AP_AHRS_NavEKF::update(bool skip_ins_update)
{
    // EKF1 is no longer supported - handle case where it is selected
    if (_ekf_type == 1) {
        _ekf_type.set(2);
    }
    update_DCM(skip_ins_update);
    update_EKF2();
    update_EKF3();
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    update_SITL();
#endif

    // call AHRS_update hook if any
    AP_Module::call_hook_AHRS_update(*this);

    // push gyros if optical flow present
    if (hal.opticalflow) {
        const Vector3f &exported_gyro_bias = get_gyro_drift();
        hal.opticalflow->push_gyro_bias(exported_gyro_bias.x, exported_gyro_bias.y);
    }

    if (_view != nullptr) {
        // update optional alternative attitude view
        _view->update(skip_ins_update);
    }
}
예제 #2
0
void AP_AHRS_NavEKF::update(bool skip_ins_update)
{
    // support locked access functions to AHRS data
    WITH_SEMAPHORE(_rsem);
    
    // drop back to normal priority if we were boosted by the INS
    // calling delay_microseconds_boost()
    hal.scheduler->boost_end();
    
    // EKF1 is no longer supported - handle case where it is selected
    if (_ekf_type == 1) {
        _ekf_type.set(2);
    }

    update_DCM(skip_ins_update);

#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    update_SITL();
#endif

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

#if AP_MODULE_SUPPORTED
    // call AHRS_update hook if any
    AP_Module::call_hook_AHRS_update(*this);
#endif

    // push gyros if optical flow present
    if (hal.opticalflow) {
        const Vector3f &exported_gyro_bias = get_gyro_drift();
        hal.opticalflow->push_gyro_bias(exported_gyro_bias.x, exported_gyro_bias.y);
    }

    if (_view != nullptr) {
        // update optional alternative attitude view
        _view->update(skip_ins_update);
    }
}
예제 #3
0
// return a smoothed and corrected gyro vector using the latest ins data (which may not have been consumed by the EKF yet)
Vector3f AP_AHRS::get_gyro_latest(void) const
{
    const uint8_t primary_gyro = get_primary_gyro_index();
    return get_ins().get_gyro(primary_gyro) + get_gyro_drift();
}