ArduPilot开源飞控之AP_AHRS

本文主要是介绍ArduPilot开源飞控之AP_AHRS，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

1. 源由

AHRS(Attitude Heading Reference System): 飞控最为重要的一个任务就是姿态、位置、方向计算。

本章节，将从代码层面研读下AP_AHRS的整体框架和设计逻辑。

其中AHRS可以分成两块内容：

1. 依赖内部sensor的AHRS，详见ArduPilot开源代码之AP_InertialSensor
2. 外部AHRS传感器

但是最终汇聚到数据算法处理的是AP_AHRS。

2. 框架设计

2.1 启动代码

Copter::init_ardupilot└──> Copter::startup_INS_ground└──> AP_AHRS::init

2.2 任务代码

FAST_TASK(read_AHRS)└──> Copter::read_AHRS└──> AP_AHRS::update

3. 重要例程

3.1 init

模块初始化例程：

1. EKF算法类型选择(目前不再支持EKF1)；
2. DCM & EXTERNAL_AHRS初始化；
3. 自定义板子方向初始化；

// init sets up INS board orientation
void AP_AHRS::init()││  /********************************************************************************│   * EKF1 is no longer supported - handle case where it is selected               *│   ********************************************************************************/├──> <_ekf_type.get() == 1>│   └──> AP_BoardConfig::config_error("EKF1 not available")├──> <_ekf_type.get() == 2>  //!HAL_NAVEKF2_AVAILABLE && HAL_NAVEKF3_AVAILABLE│   ├──> _ekf_type.set(3)│   └──> EKF3.set_enable(true)├──> <_ekf_type.get() == 3>  //!HAL_NAVEKF3_AVAILABLE && HAL_NAVEKF2_AVAILABLE│   ├──> _ekf_type.set(2)│   └──> EKF2.set_enable(true)├──> <HAL_NAVEKF2_AVAILABLE && HAL_NAVEKF3_AVAILABLE> <_ekf_type.get() == 2 && !EKF2.get_enable() && EKF3.get_enable()>│  // a special case to catch users who had AHRS_EKF_TYPE=2 saved and│  // updated to a version where EK2_ENABLE=0│   └──> _ekf_type.set(3)││  /********************************************************************************│   * DCM & external AHRS init                                                     *│   ********************************************************************************/├──> last_active_ekf_type = (EKFType)_ekf_type.get()├──> <AP_AHRS_DCM_ENABLED>│   └──> _dcm.init() // init backends├──> <HAL_EXTERNAL_AHRS_ENABLED>│   └──> external.init()││  /********************************************************************************│   * convert to new custom rotaton, PARAMETER_CONVERSION - Added: Nov-2021        *│   ********************************************************************************/└──> <!APM_BUILD_TYPE(APM_BUILD_AP_Periph><_board_orientation == ROTATION_CUSTOM_OLD>├──> _board_orientation.set_and_save(ROTATION_CUSTOM_1)├──> AP_Param::ConversionInfo info├──> <AP_Param::find_top_level_key_by_pointer(this, info.old_key)>│   ├──> info.type = AP_PARAM_FLOAT│   └──> <for (info.old_group_element=15 info.old_group_element<=17 info.old_group_element++)> <AP_Param::find_old_parameter(&info, &rpy_param)>│        └──> rpy[info.old_group_element-15] = rpy_param.get()└──> AP::custom_rotations().convert(ROTATION_CUSTOM_1, rpy[0], rpy[1], rpy[2])

3.2 update

AHRS更新过程：

1. 配置及传感数据更新；
2. EKF算法运算更新；

AP_AHRS::update││  // periodically checks to see if we should update the AHRS│  // orientation (e.g. based on the AHRS_ORIENTATION parameter)│  // allow for runtime change of orientation│  // this makes initial config easier│  /********************************************************************************│   * Configuration and sensor update                                              *│   ********************************************************************************/├──> update_orientation()├──> <!skip_ins_update>│   └──> AP::ins().update()  // tell the IMU to grab some data├──> WITH_SEMAPHORE(_rsem) // support locked access functions to AHRS data├──> <!_checked_watchdog_home>│   ├──> load_watchdog_home()  // see if we have to restore home after a watchdog reset:│   └──> _checked_watchdog_home = true││  // drop back to normal priority if we were boosted by the INS│  // calling delay_microseconds_boost()├──> hal.scheduler->boost_end()││  // update autopilot-body-to-vehicle-body from _trim parameters:├──> update_trim_rotation_matrices()│├──> <AP_AHRS_DCM_ENABLED>│   └──> update_DCM()││  // update takeoff/touchdown flags├──> update_flags()│├──> <AP_AHRS_SIM_ENABLED>│   └──> update_SITL()│├──> <HAL_EXTERNAL_AHRS_ENABLED>│   └──> update_external()││  /********************************************************************************│   * EKFx update                                                                  *│   ********************************************************************************/├──> <_ekf_type == 2>  // if EK2 is primary then run EKF2 first to give it CPU priority│   ├──> <HAL_NAVEKF2_AVAILABLE> update_EKF2()│   └──> <HAL_NAVEKF3_AVAILABLE> update_EKF3()├──> < else > // otherwise run EKF3 first│   ├──> HAL_NAVEKF3_AVAILABLE> update_EKF3()│   └──> <HAL_NAVEKF2_AVAILABLE> update_EKF2()├──> <AP_MODULE_SUPPORTED> // call AHRS_update hook if any│   └──> AP_Module::call_hook_AHRS_update(*this)│├──> <hal.opticalflow> // push gyros if optical flow present│   ├──> const Vector3f &exported_gyro_bias = get_gyro_drift()│   └──> hal.opticalflow->push_gyro_bias(exported_gyro_bias.x, exported_gyro_bias.y)│├──> <_view != nullptr> // update optional alternative attitude view│   └──> _view->update()│├──> update_AOA_SSA()  // update AOA and SSA││  /********************************************************************************│   * GCS notification                                                             *│   ********************************************************************************/├──> <HAL_GCS_ENABLED>│   ├──> state.active_EKF = _active_EKF_type()│   ├──> <state.active_EKF != last_active_ekf_type>│   │   ├──> last_active_ekf_type = state.active_EKF│   │   ├──> const char *shortname = "???"│   │   ├──> <case EKFType::DCM> <AP_AHRS_DCM_ENABLED>│   │   │   └──> shortname = "DCM"│   │   ├──> <case EKFType::SIM> <AP_AHRS_SIM_ENABLED>│   │   │   └──> shortname = "SIM"│   │   ├──> <case EKFType::EXTERNAL> <HAL_EXTERNAL_AHRS_ENABLED>│   │   │   └──> shortname = "External"│   │   ├──> <case EKFType::THREE> <HAL_NAVEKF3_AVAILABLE>│   │   │   └──> shortname = "EKF3"│   │   └──> <case EKFType::TWO> <HAL_NAVEKF2_AVAILABLE>│   │       └──> shortname = "EKF2"│   └──> GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AHRS: %s active", shortname)│├──> update_state()  // update published state││  /********************************************************************************│   * add timing jitter to simulate slow EKF response                              *│   ********************************************************************************/└──> <CONFIG_HAL_BOARD == HAL_BOARD_SITL> //├──> const auto *sitl = AP::sitl()└──> <sitl->loop_time_jitter_us > 0>└──> hal.scheduler->delay_microseconds(random() % sitl->loop_time_jitter_us)

4. 外部AHRS传感模块

启动调用关系：

Copter::init_ardupilot└──> Copter::startup_INS_ground└──> AP_AHRS::init└──> AP_ExternalAHRS::init

循环更新关系

FAST_TASK(read_AHRS)└──> Copter::read_AHRS└──> AP_AHRS::update└──> AP_ExternalAHRS::update

4.1 init

支持以下两种类型：

• AP_ExternalAHRS_VectorNav
• AP_ExternalAHRS_MicroStrain5

AP_ExternalAHRS::init├──> <rate.get() < 50>│   └──> rate.set(50)  // min 50Hz├──> <case DevType::None>│   └──> return  // nothing to do├──> <case DevType::VecNav> <AP_EXTERNAL_AHRS_VECTORNAV_ENABLED>│   ├──> backend = new AP_ExternalAHRS_VectorNav(this, state)│   └──> return├──> <case DevType::MicroStrain5> <AP_EXTERNAL_AHRS_MICROSTRAIN5_ENABLED>│   ├──> backend = new AP_ExternalAHRS_MicroStrain5(this, state)│   └──> return└──> GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Unsupported ExternalAHRS type %u", unsigned(devtype))

4.2 update

接下去又是front-end / back-end分层设计，本章节不再展开。

void AP_ExternalAHRS::update(void)└──> <backend>└──> backend->update()

5. 参考资料

【1】ArduPilot开源飞控系统之简单介绍
【2】ArduPilot之开源代码Task介绍
【3】ArduPilot飞控启动&运行过程简介
【4】ArduPilot之开源代码Library&Sketches设计
【5】ArduPilot之开源代码Sensor Drivers设计

这篇关于ArduPilot开源飞控之AP_AHRS的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！

---