ArduPilot开源飞控之AP_AHRS
- [1. 源由](#1. 源由)
- [2. 框架设计](#2. 框架设计)
-
- [2.1 启动代码](#2.1 启动代码)
- [2.2 任务代码](#2.2 任务代码)
- [3. 重要例程](#3. 重要例程)
-
- [3.1 init](#3.1 init)
- [3.2 update](#3.2 update)
- [4. 外部AHRS传感模块](#4. 外部AHRS传感模块)
-
- [4.1 init](#4.1 init)
- [4.2 update](#4.2 update)
- [5. 参考资料](#5. 参考资料)
1. 源由
AHRS(Attitude Heading Reference System): 飞控最为重要的一个任务就是姿态、位置、方向计算。
本章节，将从代码层面研读下AP_AHRS的整体框架和设计逻辑。
其中AHRS可以分成两块内容：
- 依赖内部sensor的AHRS，详见ArduPilot开源代码之AP_InertialSensor
- 外部AHRS传感器
但是最终汇聚到数据算法处理的是AP_AHRS。
2. 框架设计
2.1 启动代码
C
Copter::init_ardupilot
└──> Copter::startup_INS_ground
└──> AP_AHRS::init2.2 任务代码
C
FAST_TASK(read_AHRS)
└──> Copter::read_AHRS
└──> AP_AHRS::update3. 重要例程
3.1 init
模块初始化例程：
- EKF算法类型选择(目前不再支持EKF1)；
- DCM & EXTERNAL_AHRS初始化；
- 自定义板子方向初始化；
C
// 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更新过程：
- 配置及传感数据更新；
- EKF算法运算更新；
C
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传感模块
启动调用关系：
C
Copter::init_ardupilot
└──> Copter::startup_INS_ground
└──> AP_AHRS::init
└──> AP_ExternalAHRS::init循环更新关系
C
FAST_TASK(read_AHRS)
└──> Copter::read_AHRS
└──> AP_AHRS::update
└──> AP_ExternalAHRS::update4.1 init
支持以下两种类型：
- AP_ExternalAHRS_VectorNav
- AP_ExternalAHRS_MicroStrain5
C
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分层设计，本章节不再展开。
C
void AP_ExternalAHRS::update(void)
└──> <backend>
└──> backend->update()