ArduPilot开源飞控之Copter任务

1. 源由

之前研读ArduPilot开源飞控代码更多是从静态角度：工程结构、代码框架、front/backend分层结构等。

上述方法也是最为基本的一种逻辑理解方式，但也缺少一种动态方面的整体设计。

为此，本章节将从另一个动态任务的角度切入，整体上看下系统是如何运行的。

2. 整体分析

分析的总体切入点，从ArduPilot之开源代码Task介绍开始。

ArduCopter/Copter.cppconst AP_Scheduler::Task Copter::scheduler_tasks[]数组包含了所用运行时的任务。

/*
  scheduler table - all tasks should be listed here.

  All entries in this table must be ordered by priority.

  This table is interleaved with the table in AP_Vehicle to determine
  the order in which tasks are run.  Convenience methods SCHED_TASK
  and SCHED_TASK_CLASS are provided to build entries in this structure:

SCHED_TASK arguments:
 - name of static function to call
 - rate (in Hertz) at which the function should be called
 - expected time (in MicroSeconds) that the function should take to run
 - priority (0 through 255, lower number meaning higher priority)

SCHED_TASK_CLASS arguments:
 - class name of method to be called
 - instance on which to call the method
 - method to call on that instance
 - rate (in Hertz) at which the method should be called
 - expected time (in MicroSeconds) that the method should take to run
 - priority (0 through 255, lower number meaning higher priority)

 */
const AP_Scheduler::Task Copter::scheduler_tasks[] = {
    // update INS immediately to get current gyro data populated
    FAST_TASK_CLASS(AP_InertialSensor, &copter.ins, update),
    // run low level rate controllers that only require IMU data
    FAST_TASK(run_rate_controller),
#if AC_CUSTOMCONTROL_MULTI_ENABLED == ENABLED
    FAST_TASK(run_custom_controller),
#endif
#if FRAME_CONFIG == HELI_FRAME
    FAST_TASK(heli_update_autorotation),
#endif //HELI_FRAME
    // send outputs to the motors library immediately
    FAST_TASK(motors_output),
     // run EKF state estimator (expensive)
    FAST_TASK(read_AHRS),
#if FRAME_CONFIG == HELI_FRAME
    FAST_TASK(update_heli_control_dynamics),
#endif //HELI_FRAME
    // Inertial Nav
    FAST_TASK(read_inertia),
    // check if ekf has reset target heading or position
    FAST_TASK(check_ekf_reset),
    // run the attitude controllers
    FAST_TASK(update_flight_mode),
    // update home from EKF if necessary
    FAST_TASK(update_home_from_EKF),
    // check if we've landed or crashed
    FAST_TASK(update_land_and_crash_detectors),
    // surface tracking update
    FAST_TASK(update_rangefinder_terrain_offset),
#if HAL_MOUNT_ENABLED
    // camera mount's fast update
    FAST_TASK_CLASS(AP_Mount, &copter.camera_mount, update_fast),
#endif
    FAST_TASK(Log_Video_Stabilisation),

    SCHED_TASK(rc_loop,              250,    130,  3),
    SCHED_TASK(throttle_loop,         50,     75,  6),
    SCHED_TASK_CLASS(AP_GPS,               &copter.gps,                 update,          50, 200,   9),
#if AP_OPTICALFLOW_ENABLED
    SCHED_TASK_CLASS(AP_OpticalFlow,          &copter.optflow,             update,         200, 160,  12),
#endif
    SCHED_TASK(update_batt_compass,   10,    120, 15),
    SCHED_TASK_CLASS(RC_Channels, (RC_Channels*)&copter.g2.rc_channels, read_aux_all,    10,  50,  18),
    SCHED_TASK(arm_motors_check,      10,     50, 21),
#if TOY_MODE_ENABLED == ENABLED
    SCHED_TASK_CLASS(ToyMode,              &copter.g2.toy_mode,         update,          10,  50,  24),
#endif
    SCHED_TASK(auto_disarm_check,     10,     50,  27),
    SCHED_TASK(auto_trim,             10,     75,  30),
#if RANGEFINDER_ENABLED == ENABLED
    SCHED_TASK(read_rangefinder,      20,    100,  33),
#endif
#if HAL_PROXIMITY_ENABLED
    SCHED_TASK_CLASS(AP_Proximity,         &copter.g2.proximity,        update,         200,  50,  36),
#endif
#if BEACON_ENABLED == ENABLED
    SCHED_TASK_CLASS(AP_Beacon,            &copter.g2.beacon,           update,         400,  50,  39),
#endif
    SCHED_TASK(update_altitude,       10,    100,  42),
    SCHED_TASK(run_nav_updates,       50,    100,  45),
    SCHED_TASK(update_throttle_hover,100,     90,  48),
#if MODE_SMARTRTL_ENABLED == ENABLED
    SCHED_TASK_CLASS(ModeSmartRTL,         &copter.mode_smartrtl,       save_position,    3, 100,  51),
#endif
#if HAL_SPRAYER_ENABLED
    SCHED_TASK_CLASS(AC_Sprayer,           &copter.sprayer,               update,         3,  90,  54),
#endif
    SCHED_TASK(three_hz_loop,          3,     75, 57),
    SCHED_TASK_CLASS(AP_ServoRelayEvents,  &copter.ServoRelayEvents,      update_events, 50,  75,  60),
    SCHED_TASK_CLASS(AP_Baro,              &copter.barometer,             accumulate,    50,  90,  63),
#if PRECISION_LANDING == ENABLED
    SCHED_TASK(update_precland,      400,     50,  69),
#endif
#if FRAME_CONFIG == HELI_FRAME
    SCHED_TASK(check_dynamic_flight,  50,     75,  72),
#endif
#if LOGGING_ENABLED == ENABLED
    SCHED_TASK(loop_rate_logging, LOOP_RATE,    50,  75),
#endif
    SCHED_TASK_CLASS(AP_Notify,            &copter.notify,              update,          50,  90,  78),
    SCHED_TASK(one_hz_loop,            1,    100,  81),
    SCHED_TASK(ekf_check,             10,     75,  84),
    SCHED_TASK(check_vibration,       10,     50,  87),
    SCHED_TASK(gpsglitch_check,       10,     50,  90),
    SCHED_TASK(takeoff_check,         50,     50,  91),
#if AP_LANDINGGEAR_ENABLED
    SCHED_TASK(landinggear_update,    10,     75,  93),
#endif
    SCHED_TASK(standby_update,        100,    75,  96),
    SCHED_TASK(lost_vehicle_check,    10,     50,  99),
    SCHED_TASK_CLASS(GCS,                  (GCS*)&copter._gcs,          update_receive, 400, 180, 102),
    SCHED_TASK_CLASS(GCS,                  (GCS*)&copter._gcs,          update_send,    400, 550, 105),
#if HAL_MOUNT_ENABLED
    SCHED_TASK_CLASS(AP_Mount,             &copter.camera_mount,        update,          50,  75, 108),
#endif
#if AP_CAMERA_ENABLED
    SCHED_TASK_CLASS(AP_Camera,            &copter.camera,              update,          50,  75, 111),
#endif
#if LOGGING_ENABLED == ENABLED
    SCHED_TASK(ten_hz_logging_loop,   10,    350, 114),
    SCHED_TASK(twentyfive_hz_logging, 25,    110, 117),
    SCHED_TASK_CLASS(AP_Logger,            &copter.logger,              periodic_tasks, 400, 300, 120),
#endif
    SCHED_TASK_CLASS(AP_InertialSensor,    &copter.ins,                 periodic,       400,  50, 123),

    SCHED_TASK_CLASS(AP_Scheduler,         &copter.scheduler,           update_logging, 0.1,  75, 126),
#if AP_RPM_ENABLED
    SCHED_TASK_CLASS(AP_RPM,               &copter.rpm_sensor,          update,          40, 200, 129),
#endif
    SCHED_TASK_CLASS(AP_TempCalibration,   &copter.g2.temp_calibration, update,          10, 100, 135),
#if HAL_ADSB_ENABLED
    SCHED_TASK(avoidance_adsb_update, 10,    100, 138),
#endif
#if ADVANCED_FAILSAFE == ENABLED
    SCHED_TASK(afs_fs_check,          10,    100, 141),
#endif
#if AP_TERRAIN_AVAILABLE
    SCHED_TASK(terrain_update,        10,    100, 144),
#endif
#if AP_GRIPPER_ENABLED
    SCHED_TASK_CLASS(AP_Gripper,           &copter.g2.gripper,          update,          10,  75, 147),
#endif
#if AP_WINCH_ENABLED
    SCHED_TASK_CLASS(AP_Winch,             &copter.g2.winch,            update,          50,  50, 150),
#endif
#ifdef USERHOOK_FASTLOOP
    SCHED_TASK(userhook_FastLoop,    100,     75, 153),
#endif
#ifdef USERHOOK_50HZLOOP
    SCHED_TASK(userhook_50Hz,         50,     75, 156),
#endif
#ifdef USERHOOK_MEDIUMLOOP
    SCHED_TASK(userhook_MediumLoop,   10,     75, 159),
#endif
#ifdef USERHOOK_SLOWLOOP
    SCHED_TASK(userhook_SlowLoop,      3.3,   75, 162),
#endif
#ifdef USERHOOK_SUPERSLOWLOOP
    SCHED_TASK(userhook_SuperSlowLoop, 1,     75, 165),
#endif
#if HAL_BUTTON_ENABLED
    SCHED_TASK_CLASS(AP_Button,            &copter.button,              update,           5, 100, 168),
#endif
#if STATS_ENABLED == ENABLED
    SCHED_TASK_CLASS(AP_Stats,             &copter.g2.stats,            update,           1, 100, 171),
#endif
};
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
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172

3. 必须任务

3.1 FAST_TASK_CLASS(AP_InertialSensor, &copter.ins, update)

详见：ArduPilot开源代码之AP_InertialSensor

3.2 FAST_TASK(run_rate_controller)

待补充

3.3 FAST_TASK(motors_output)

待补充

3.4 FAST_TASK(read_AHRS)

详见：ArduPilot开源飞控之AP_AHRS

void Copter::read_AHRS(void)
{
    // we tell AHRS to skip INS update as we have already done it in FAST_TASK.
    ahrs.update(true);
}
1
2
3
4
5

3.5 FAST_TASK(read_inertia)

详见：ArduPilot开源飞控之AP_InertialNav

3.6 FAST_TASK(check_ekf_reset)

待补充

3.7 FAST_TASK(update_flight_mode)

待补充

3.8 FAST_TASK(update_home_from_EKF)

待补充

3.9 FAST_TASK(update_land_and_crash_detectors)

待补充

3.10 FAST_TASK(update_rangefinder_terrain_offset)

待补充

3.11 FAST_TASK(Log_Video_Stabilisation)

待补充

3.12 SCHED_TASK(rc_loop, 250, 130, 3)

待补充

3.13 SCHED_TASK(throttle_loop, 50, 75, 6)

待补充

3.14 SCHED_TASK_CLASS(AP_GPS, &copter.gps, update, 50, 200, 9)

详见：ArduPilot开源代码之AP_GPS

3.15 SCHED_TASK(update_batt_compass, 10, 120, 15)

待补充

3.16 SCHED_TASK_CLASS(RC_Channels, (RC_Channels*)&copter.g2.rc_channels, read_aux_all, 10, 50, 18)

待补充

3.17 SCHED_TASK(arm_motors_check, 10, 50, 21)

待补充

3.18 SCHED_TASK(auto_disarm_check, 10, 50, 27)

待补充

3.19 SCHED_TASK(auto_trim, 10, 75, 30)

待补充

3.20 SCHED_TASK(update_altitude, 10, 100, 42)

待补充

3.21 SCHED_TASK(run_nav_updates, 50, 100, 45)

待补充

3.22 SCHED_TASK(update_throttle_hover,100, 90, 48)

待补充

3.23 SCHED_TASK(three_hz_loop, 3, 75, 57)

待补充

3.24 SCHED_TASK_CLASS(AP_Baro, &copter.barometer, accumulate, 50, 90, 63)

详见：ArduPilot开源飞控之AP_Baro

3.25 SCHED_TASK(one_hz_loop, 1, 100, 81)

待补充

3.26 SCHED_TASK(ekf_check, 10, 75, 84)

待补充

3.27 SCHED_TASK(check_vibration, 10, 50, 87),

待补充

3.28 SCHED_TASK(gpsglitch_check, 10, 50, 90),

待补充

3.29 SCHED_TASK(takeoff_check, 50, 50, 91),

待补充

3.30 SCHED_TASK(standby_update, 100, 75, 96),

待补充

3.31 SCHED_TASK(lost_vehicle_check, 10, 50, 99),

待补充

3.32 SCHED_TASK_CLASS(GCS, (GCS*)&copter._gcs, update_receive, 400, 180, 102),

待补充

3.33 SCHED_TASK_CLASS(GCS, (GCS*)&copter._gcs, update_send, 400, 550, 105),

待补充

3.34 SCHED_TASK_CLASS(AP_InertialSensor, &copter.ins, periodic, 400, 50, 123),

详见：ArduPilot开源代码之AP_InertialSensor

3.35 SCHED_TASK_CLASS(AP_Scheduler, &copter.scheduler, update_logging, 0.1, 75, 126),

待补充

3.36 SCHED_TASK_CLASS(AP_TempCalibration, &copter.g2.temp_calibration, update, 10, 100, 135),

待补充

4. 可选任务

4.1 AC_CUSTOMCONTROL_MULTI_ENABLED

FAST_TASK(run_custom_controller)
1

待补充

4.2 HAL_MOUNT_ENABLED

// camera mount's fast update
FAST_TASK_CLASS(AP_Mount, &copter.camera_mount, update_fast)
1
2

待补充

4.3 AP_OPTICALFLOW_ENABLED

SCHED_TASK_CLASS(AP_OpticalFlow,          &copter.optflow,             update,         200, 160,  12),
1

待补充

4.4 TOY_MODE_ENABLED

SCHED_TASK_CLASS(ToyMode,              &copter.g2.toy_mode,         update,          10,  50,  24),
1

待补充

4.5 RANGEFINDER_ENABLED

SCHED_TASK(read_rangefinder,      20,    100,  33),
1

待补充

4.6 HAL_PROXIMITY_ENABLED

SCHED_TASK_CLASS(AP_Proximity,         &copter.g2.proximity,        update,         200,  50,  36),
1

待补充

4.7 AP_BEACON_ENABLED

SCHED_TASK_CLASS(AP_Beacon,            &copter.g2.beacon,           update,         400,  50,  39),
1

待补充

4.8 MODE_SMARTRTL_ENABLED

SCHED_TASK_CLASS(ModeSmartRTL,         &copter.mode_smartrtl,       save_position,    3, 100,  51),
1

待补充

4.9 HAL_SPRAYER_ENABLED

SCHED_TASK_CLASS(AC_Sprayer,           &copter.sprayer,               update,         3,  90,  54),
1

待补充

4.10 AP_SERVORELAYEVENTS_ENABLED

SCHED_TASK_CLASS(AP_ServoRelayEvents,  &copter.ServoRelayEvents,      update_events, 50,  75,  60),
1

待补充

4.11 AC_PRECLAND_ENABLED

SCHED_TASK(update_precland,      400,     50,  69),
1

待补充

4.12 LOGGING_ENABLED

SCHED_TASK(loop_rate_logging, LOOP_RATE,    50,  75),
SCHED_TASK(ten_hz_logging_loop,   10,    350, 114),
SCHED_TASK(twentyfive_hz_logging, 25,    110, 117),
SCHED_TASK_CLASS(AP_Logger,            &copter.logger,              periodic_tasks, 400, 300, 120),
1
2
3
4

待补充

4.13 AP_LANDINGGEAR_ENABLED

SCHED_TASK(landinggear_update,    10,     75,  93),
1

待补充

4.14 HAL_MOUNT_ENABLED

SCHED_TASK_CLASS(AP_Mount,             &copter.camera_mount,        update,          50,  75, 108),
1

待补充

4.15 AP_CAMERA_ENABLED

SCHED_TASK_CLASS(AP_Camera,            &copter.camera,              update,          50,  75, 111),
1

待补充

4.16 AP_RPM_ENABLED

SCHED_TASK_CLASS(AP_RPM,               &copter.rpm_sensor,          update,          40, 200, 129),
1

待补充

4.17 HAL_ADSB_ENABLED

SCHED_TASK(avoidance_adsb_update, 10,    100, 138),
1

待补充

4.18 ADVANCED_FAILSAFE

SCHED_TASK(afs_fs_check,          10,    100, 141),
1

待补充

4.19 AP_TERRAIN_AVAILABLE

SCHED_TASK(terrain_update,        10,    100, 144),
1

待补充

4.20 AP_GRIPPER_ENABLED

SCHED_TASK_CLASS(AP_Gripper,           &copter.g2.gripper,          update,          10,  75, 147),
1

待补充

4.21 AP_WINCH_ENABLED

SCHED_TASK_CLASS(AP_Winch,             &copter.g2.winch,            update,          50,  50, 150),
1

待补充

4.22 USERHOOK_FASTLOOP

SCHED_TASK(userhook_FastLoop,    100,     75, 153),
1

待补充

4.23 USERHOOK_50HZLOOP

SCHED_TASK(userhook_50Hz,         50,     75, 156),
1

待补充

4.24 USERHOOK_MEDIUMLOOP

SCHED_TASK(userhook_MediumLoop,   10,     75, 159),
1

待补充

4.25 USERHOOK_SLOWLOOP

SCHED_TASK(userhook_SlowLoop,      3.3,   75, 162),
1

待补充

4.26 USERHOOK_SUPERSLOWLOOP

SCHED_TASK(userhook_SuperSlowLoop, 1,     75, 165),
1

待补充

4.27 HAL_BUTTON_ENABLED

SCHED_TASK_CLASS(AP_Button,            &copter.button,              update,           5, 100, 168),
1

待补充

4.28 STATS_ENABLED

SCHED_TASK_CLASS(AP_Stats,             &copter.g2.stats,            update,           1, 100, 171),
1

待补充

5. HELI Copter任务

FRAME_CONFIG == HELI_FRAME

FAST_TASK(update_heli_control_dynamics)
FAST_TASK(heli_update_autorotation)
SCHED_TASK(check_dynamic_flight,  50,     75,  72),
1
2
3

注：暂不展开。

6. 总结

这里主要从业务逻辑角度整理的任务，其中还有驱动任务。

对比ArduPilot开源飞控系统之简单介绍，就能看出为什么很多业务逻辑面尚无法很好的关联，串联起来理解的原因。

后续将会逐步补充全前面关于【待补充】的相关研读内容。

7. 参考资料

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