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
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)
 ├──>  <_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()
 ├──> 
 │   └──> _dcm.init() // init backends
 ├──> 
 │   └──> external.init()
 │
 │  /********************************************************************************
 │   * convert to new custom rotaton, PARAMETER_CONVERSION - Added: Nov-2021        *
 │   ********************************************************************************/
 └──> <_board_orientation == ROTATION_CUSTOM_OLD>
     ├──> _board_orientation.set_and_save(ROTATION_CUSTOM_1)
     ├──> AP_Param::ConversionInfo info
     ├──> 
     │   ├──> info.type = AP_PARAM_FLOAT
     │   └──>  
     │        └──> 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()
 ├──> 
 │   └──> AP::ins().update()  // tell the IMU to grab some data
 ├──> WITH_SEMAPHORE(_rsem) // support locked access functions to AHRS data
 ├──> 
 │   ├──> 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()
 │
 ├──> 
 │   └──> update_DCM()
 │
 │  // update takeoff/touchdown flags
 ├──> update_flags()
 │
 ├──> 
 │   └──> update_SITL()
 │
 ├──> 
 │   └──> update_external()
 │
 │  /********************************************************************************
 │   * EKFx update                                                                  *
 │   ********************************************************************************/
 ├──> <_ekf_type == 2>  // if EK2 is primary then run EKF2 first to give it CPU priority
 │   ├──>  update_EKF2()
 │   └──>  update_EKF3()
 ├──> < else > // otherwise run EKF3 first
 │   ├──> HAL_NAVEKF3_AVAILABLE> update_EKF3()
 │   └──>  update_EKF2()
 ├──>  // call AHRS_update hook if any
 │   └──> AP_Module::call_hook_AHRS_update(*this)
 │
 ├──>  // 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                                                             *
 │   ********************************************************************************/
 ├──> 
 │   ├──> state.active_EKF = _active_EKF_type()
 │   ├──> 
 │   │   ├──> last_active_ekf_type = state.active_EKF
 │   │   ├──> const char *shortname = "???"
 │   │   ├──>  
 │   │   │   └──> shortname = "DCM"
 │   │   ├──>  
 │   │   │   └──> shortname = "SIM"
 │   │   ├──>  
 │   │   │   └──> shortname = "External"
 │   │   ├──>  
 │   │   │   └──> shortname = "EKF3"
 │   │   └──>  
 │   │       └──> 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                              *
 │   ********************************************************************************/
 └──>  //
     ├──> const auto *sitl = AP::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.set(50)  // min 50Hz
 ├──> 
 │   └──> return  // nothing to do
 ├──>  
 │   ├──> backend = new AP_ExternalAHRS_VectorNav(this, state)
 │   └──> return
 ├──>  
 │   ├──> 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->update()
1
2
3

5. 参考资料

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