当前位置:   article > 正文

ArduPilot开源飞控之AP_AHRS_飞控里的ahrs

飞控里的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
  • 1
  • 2
  • 3

2.2 任务代码

FAST_TASK(read_AHRS)
 └──> Copter::read_AHRS
     └──> AP_AHRS::update
  • 1
  • 2
  • 3

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])
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39

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)
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76
  • 77
  • 78
  • 79
  • 80
  • 81
  • 82
  • 83
  • 84
  • 85
  • 86

4. 外部AHRS传感模块

启动调用关系:

Copter::init_ardupilot
 └──> Copter::startup_INS_ground
     └──> AP_AHRS::init
         └──> AP_ExternalAHRS::init
  • 1
  • 2
  • 3
  • 4

循环更新关系

FAST_TASK(read_AHRS)
 └──> Copter::read_AHRS
     └──> AP_AHRS::update
         └──> AP_ExternalAHRS::update
  • 1
  • 2
  • 3
  • 4

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))
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12

4.2 update

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

void AP_ExternalAHRS::update(void)
 └──> <backend>
     └──> backend->update()
  • 1
  • 2
  • 3

5. 后续研读

6. 参考资料

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

声明:本文内容由网友自发贡献,不代表【wpsshop博客】立场,版权归原作者所有,本站不承担相应法律责任。如您发现有侵权的内容,请联系我们。转载请注明出处:https://www.wpsshop.cn/w/知新_RL/article/detail/849600
推荐阅读
相关标签
  

闽ICP备14008679号