ArduPilot开源飞控之AP_AHRS

1. 源由

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

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

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

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

但是最终汇聚到数据算法处理的是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. 后续研读

• ArduPilot开源代码之AP_AHRS_Backend
• ArduPilot开源代码之AP_AHRS_View
• ArduPilot开源代码之AP_AHRS - 扩展研读1
• TBD … …

6. 参考资料

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