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