ArduPilot开源飞控之AP_AHRS
- 1. 源由
- 2. 框架设计
- 2.1 启动代码
- 2.2 任务代码
- 3. 重要例程
- 3.1 init
- 3.2 update
- 4. 外部AHRS传感模块
- 4.1 init
- 4.2 update
- 5. 参考资料
1. 源由
AHRS(Attitude Heading Reference System): 飞控最为重要的一个任务就是姿态、位置、方向计算。
本章节，将从代码层面研读下AP_AHRS的整体框架和设计逻辑。
其中AHRS可以分成两块内容：
- 依赖内部sensor的AHRS，详见ArduPilot开源代码之AP_InertialSensor
- 外部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
模块初始化例程：
- EKF算法类型选择(目前不再支持EKF1)；
- DCM & EXTERNAL_AHRS初始化；
- 自定义板子方向初始化；
// 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算法运算更新；
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设计
)
)
)
