ArduPilot飞控启动&运行过程简介

1. 源由

ArduPilot从整体的设计框架角度，感觉是更加容易上手，尤其是对一些相对熟悉C语言/嵌入式固件开发的兄弟们来说。

1. 基于Ardunio编程方式
2. 采用C++类方式进行抽象
3. 应用业务模块化
4. 模块考虑重复利用
5. 设备代码工程隔离
6. ArduPilot自研任务调度

注：飞控由于其历史发展以及时间同步因素，大量的使用了自研的任务调度，这个和常见的OS任务调度有很大的差异，请大家特别注意。

为了更好从一个整体来理解ArduPilot代码行为，我们直接从启动&运行过程入手，围绕这根主线，类似鱼骨图方式展开研读和学习。

2. Copter飞控

鉴于官网文档也指出，从设计文档的角度来说，Copter相对详细（尽快也已经很久没有更新了）。因此，我们就重点关注Copter的启动&运行过程。

2.1 入口

ArduCopter/Copter.cpp

AP_HAL_MAIN_CALLBACKS(&copter);
1

libraries/AP_HAL/AP_HAL_Main.h

#ifndef AP_MAIN
#define AP_MAIN main
#endif

#define AP_HAL_MAIN() \
    AP_HAL::HAL::FunCallbacks callbacks(setup, loop); \
    extern "C" {                               \
    int AP_MAIN(int argc, char* const argv[]); \
    int AP_MAIN(int argc, char* const argv[]) { \
        hal.run(argc, argv, &callbacks); \
        return 0; \
    } \
    }

#define AP_HAL_MAIN_CALLBACKS(CALLBACKS) extern "C" { \
    int AP_MAIN(int argc, char* const argv[]); \
    int AP_MAIN(int argc, char* const argv[]) { \
        hal.run(argc, argv, CALLBACKS); \
        return 0; \
    } \
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

2.2 启动(run)

根据Copter硬件配置情况，可能使用不同的OS操作系统。

通常情况来说，硬件采用ChibiOS嵌入式操作系统。

libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp

void HAL_ChibiOS::run(int argc, char * const argv[], Callbacks* callbacks) const
 ├──> <HAL_USE_SERIAL_USB == TRUE> usb_initialise()
 ├──> <HAL_STDOUT_SERIAL> sdStart((SerialDriver*)&HAL_STDOUT_SERIAL, &stdoutcfg)  //STDOUT Initialisation
 ├──> g_callbacks = callbacks
 └──> main_loop()  //Takeover main
1
2
3
4
5

这里的callbacks=&copter，而Copter对象继承自AP_Vehicle。所以可以知道g_callbacks里面所带的setup/loop是AP_Vehicle::setup/AP_Vehicle::loop。

ArduCopter/Copter.h

class Copter : public AP_Vehicle {
1

2.3 运行(main_loop)

libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp

static void main_loop()
 ├──> daemon_task = chThdGetSelfX();
 ├──> chThdSetPriority(APM_MAIN_PRIORITY);  //switch to high priority for main loop
 ├──> <HAL_I2C_CLEAR_BUS> ChibiOS::I2CBus::clear_all();
 ├──> <AP_HAL_SHARED_DMA_ENABLED> ChibiOS::Shared_DMA::init();
 ├──> peripheral_power_enable();
 ├──> <HAL_SPI_CHECK_CLOCK_FREQ> ChibiOS::SPIDevice::test_clock_freq();
 ├──> hal.analogin->init();

 ##################################################
 # hal.scheduler                                  #
 ##################################################
 ├──> hal.scheduler->init();
 
 ├──> hal_chibios_set_priority(APM_STARTUP_PRIORITY);
 ├──> <stm32_was_watchdog_reset()> 
 │   ├──> stm32_watchdog_load((uint32_t *)&utilInstance.persistent_data, (sizeof(utilInstance.persistent_data)+3)/4);
 │   └──> utilInstance.last_persistent_data = utilInstance.persistent_data;
 ├──> schedulerInstance.hal_initialized();
 
 ##################################################
 # AP_Vehicle::setup                              #
 ##################################################
 
 ├──> g_callbacks->setup();
 ├──> <HAL_ENABLE_SAVE_PERSISTENT_PARAMS> utilInstance.apply_persistent_params();
 ├──> <HAL_FLASH_PROTECTION> 
 │   ├──> <AP_BoardConfig::unlock_flash()> stm32_flash_unprotect_flash();
 │   └──> <else> stm32_flash_protect_flash(false, AP_BoardConfig::protect_flash()); stm32_flash_protect_flash(true, AP_BoardConfig::protect_bootloader());
 ├──> <!defined(DISABLE_WATCHDOG)>
 │   ├──> <IOMCU_FW> stm32_watchdog_init();
 │   └──> <!IOMCU_FW>
 │       ├──> <AP_BoardConfig::watchdog_enabled()> stm32_watchdog_init();
 │       └──> <hal.util->was_watchdog_reset()> INTERNAL_ERROR(AP_InternalError::error_t::watchdog_reset);
 ├──> schedulerInstance.watchdog_pat();
 ├──> hal.scheduler->set_system_initialized();
 ├──> thread_running = true;
 ├──> chRegSetThreadName(SKETCHNAME);
 ├──> chThdSetPriority(APM_MAIN_PRIORITY); //switch to high priority for main loop
 ├──> <while (true)>
 
 ##################################################
 # AP_Vehicle::loop                               #
 ##################################################
 
 │   ├──> g_callbacks->loop();
 │   ├──> <!defined(HAL_DISABLE_LOOP_DELAY) && !APM_BUILD_TYPE(APM_BUILD_Replay)> <!schedulerInstance.check_called_boost()> hal.scheduler->delay_microseconds(50);
 │   └──> schedulerInstance.watchdog_pat();
 └──> thread_running = false;
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

3. Ardunio编程

因为基于Ardunio编程方式，所以在启动&运行过程中，先调用setup进行初始化设备，在主线程中进行loop运行。

3.1 setup - AP_Vehicle::setup

libraries/AP_Vehicle/AP_Vehicle.cpp

void AP_Vehicle::setup()
 ├──> AP_Param::setup_sketch_defaults(); // load the default values of variables listed in var_info[]
 ├──> serial_manager.init_console();  // initialise serial port
 ├──> DEV_PRINTF("\n\nInit %s"
 │                      "\n\nFree RAM: %u\n",
 │                      AP::fwversion().fw_string,
 │                      (unsigned)hal.util->available_memory());
 ├──> <AP_CHECK_FIRMWARE_ENABLED> check_firmware_print();
 ├──> AP_Param::check_var_info(); // validate the static parameter table,
 ├──> load_parameters();  // then load persistentvalues from storage:
 ├──> <CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS> <AP_BoardConfig::get_sdcard_slowdown() != 0> // user wants the SDcard slower, we need to remount
 │   ├──> sdcard_stop();
 │   └──> sdcard_retry();
 ├──> get_scheduler_tasks(tasks, task_count, log_bit);
 ├──> AP::scheduler().init(tasks, task_count, log_bit);
 ├──> G_Dt = scheduler.get_loop_period_s();  // time per loop - this gets updated in the main loop() based on actual loop rate
 
 ##################################################
 # this is here for Plane; its failsafe_check method requires the
 # RC channels to be set as early as possible for maximum
 # survivability.
 ##################################################
 
 ├──> set_control_channels();

 ##################################################
 # initialise serial manager as early as sensible to get
 # diagnostic output during boot process.  We have to initialise
 # the GCS singleton first as it sets the global mavlink system ID
 # which may get used very early on.
 ##################################################

 ├──> gcs().init();

 ##################################################
 # initialise serial ports                        #
 ##################################################
 
 ├──> serial_manager.init();
 ├──> gcs().setup_console();

 ##################################################
 # Register scheduler_delay_cb, which will run anytime you have
 # more than 5ms remaining in your call to hal.scheduler->delay
 ##################################################
 
 ├──> hal.scheduler->register_delay_callback(scheduler_delay_callback, 5);

 ├──> <HAL_MSP_ENABLED> msp.init(); // call MSP init before init_ardupilot to allow for MSP sensors
 ├──> <HAL_EXTERNAL_AHRS_ENABLED> externalAHRS.init(); // call externalAHRS init before init_ardupilot to allow for external sensors
 ├──> init_ardupilot();  // init_ardupilot is where the vehicle does most of its initialisation.
 ├──> <AP_AIRSPEED_ENABLED>
 │   ├──> airspeed.init();
 │   ├──> <airspeed.enabled()>
 │   │   └──> airspeed.calibrate(true);
 │   └──> <APM_BUILD_TYPE(APM_BUILD_ArduPlane)> GCS_SEND_TEXT(MAV_SEVERITY_WARNING,"No airspeed sensor present or enabled");
 ├──> <!APM_BUILD_TYPE(APM_BUILD_Replay)> SRV_Channels::init(); 
 ├──> <HAL_GYROFFT_ENABLED>  // gyro FFT needs to be initialized really late
 │   └──> gyro_fft.init(AP::scheduler().get_loop_rate_hz());
 ├──> <HAL_RUNCAM_ENABLED> runcam.init();
 ├──> <HAL_HOTT_TELEM_ENABLED> hott_telem.init();
 ├──> <HAL_VISUALODOM_ENABLED> visual_odom.init();  // init library used for visual position estimation
 ├──> <AP_VIDEOTX_ENABLED> vtx.init();
 ├──> <AP_SMARTAUDIO_ENABLED> smartaudio.init();
 ├──> <AP_TRAMP_ENABLED> tramp.init();
 ├──> <AP_PARAM_KEY_DUMP> AP_Param::show_all(hal.console, true);
 ├──> send_watchdog_reset_statustext();
 ├──> <HAL_GENERATOR_ENABLED> generator.init();
 ├──> <AP_OPENDRONEID_ENABLED> opendroneid.init();
 ├──> <HAL_EFI_ENABLED> efi.init(); // init EFI monitoring
 ├──> <AP_TEMPERATURE_SENSOR_ENABLED> temperature_sensor.init();
 ├──> <AP_AIS_ENABLED> ais.init();
 ├──> <HAL_NMEA_OUTPUT_ENABLED> nmea.init();
 ├──> <AP_FENCE_ENABLED> fence.init();
 ├──> <custom_rotations.init();
 ├──> <HAL_WITH_ESC_TELEM && HAL_GYROFFT_ENABLED>
 │   └──> for (uint8_t i = 0; i<ESC_TELEM_MAX_ESCS; i++) {
 │       └──> esc_noise[i].set_cutoff_frequency(2);
 ├──> AP_Param::invalidate_count(); // invalidate count in case an enable parameter changed during initialisation
 └──> gcs().send_text(MAV_SEVERITY_INFO, "ArduPilot Ready");
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

3.2 loop - AP_Vehicle::loop

libraries/AP_Vehicle/AP_Vehicle.cpp

void AP_Vehicle::loop()
 ├──> scheduler.loop();
 ├──> G_Dt = scheduler.get_loop_period_s();
 ├──> <!done_safety_init>
 │   │  /*
 │   │    disable safety if requested. This is delayed till after the
 │   │    first loop has run to ensure that all servos have received
 │   │    an update for their initial values. Otherwise we may end up
 │   │    briefly driving a servo to a position out of the configured
 │   │    range which could damage hardware
 │   │  */
 │   ├──> done_safety_init = true;
 │   ├──> BoardConfig.init_safety();
 │   ├──> char banner_msg[50];
 │   └──> <hal.rcout->get_output_mode_banner(banner_msg, sizeof(banner_msg))> GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s", banner_msg); // send RC output mode info if available
 ├──> const uint32_t new_internal_errors = AP::internalerror().errors();
 └──> _last_internal_errors != new_internal_errors>
     ├──> AP::logger().Write_Error(LogErrorSubsystem::INTERNAL_ERROR, LogErrorCode::INTERNAL_ERRORS_DETECTED);
     ├──> gcs().send_text(MAV_SEVERITY_CRITICAL, "Internal Errors 0x%x", (unsigned)new_internal_errors);
     └──> _last_internal_errors = new_internal_errors;
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

4. ArduCopter任务

ArduCopter任务的调用栈逻辑依次是：

AP_Vehicle::loop
 └──> scheduler.loop
     └──> run
         └──> task.function
1
2
3
4

task.function是ArduCopter/Copter.cpp中给出的任务列表对应的函数。这张表格给出了ArduCopter所有的任务。飞控运行时，将不断的通过表中任务的优先级进行切换运行。

注：关于每个任务的执行细节方面，我们后续抽时间将会逐一研究，也请大家持续关注，谢谢！

/*
  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

5. 参考资料

【1】ArduPilot开源飞控系统之简单介绍
【2】ArduPilot之开源代码框架
【3】ArduPilot飞控之ubuntu22.04-SITL安装
【4】ArduPilot飞控之ubuntu22.04-Gazebo模拟
【5】ArduPilot飞控之Mission Planner模拟
【6】ArduPilot飞控AOCODARC-H7DUAL固件编译
【7】ArduPilot之开源代码Library&Sketches设计
【8】ArduPilot之开源代码Sensor Drivers设计
【9】ArduPilot之开源代码基础知识&Threading概念
【10】ArduPilot之开源代码UARTs and the Console使用