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);

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; \
    } \
    }
```C

## 2.2 启动(run)

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

通常情况来说，硬件采用[ChibiOS嵌入式操作系统](https://www.chibios.org/dokuwiki/doku.php)。

[libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp](https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp#L315)

```C
void HAL_ChibiOS::run(int argc, char * const argv[], Callbacks* callbacks) const
 ├──>  usb_initialise()
 ├──>  sdStart((SerialDriver*)&HAL_STDOUT_SERIAL, &stdoutcfg)  //STDOUT Initialisation
 ├──> g_callbacks = callbacks
 └──> main_loop()  //Takeover main

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

ArduCopter/Copter.h

class Copter : public AP_Vehicle {

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
 ├──>  ChibiOS::I2CBus::clear_all();
 ├──>  ChibiOS::Shared_DMA::init();
 ├──> peripheral_power_enable();
 ├──>  ChibiOS::SPIDevice::test_clock_freq();
 ├──> hal.analogin->init();
 ├──> hal.scheduler->init();
 ├──> hal_chibios_set_priority(APM_STARTUP_PRIORITY);
 ├──>  
 │   ├──> 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();
 ├──>  utilInstance.apply_persistent_params();
 ├──>  
 │   ├──>  stm32_flash_unprotect_flash();
 │   └──>  stm32_flash_protect_flash(false, AP_BoardConfig::protect_flash()); stm32_flash_protect_flash(true, AP_BoardConfig::protect_bootloader());
 ├──> 
 │   ├──>  stm32_watchdog_init();
 │   └──> 
 │       ├──>  stm32_watchdog_init();
 │       └──> 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
 ├──> 
 
 ##################################################
 # AP_Vehicle::loop                               #
 ##################################################
 
 │   ├──> g_callbacks->loop();
 │   ├──>   hal.scheduler->delay_microseconds(50);
 │   └──> schedulerInstance.watchdog_pat();
 └──> thread_running = false;

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());
 ├──>  check_firmware_print();
 ├──> AP_Param::check_var_info(); // validate the static parameter table,
 ├──> load_parameters();  // then load persistentvalues from storage:
 ├──>   // 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);

 ├──>  msp.init(); // call MSP init before init_ardupilot to allow for MSP sensors
 ├──>  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.
 ├──> 
 │   ├──> airspeed.init();
 │   ├──> 
 │   │   └──> airspeed.calibrate(true);
 │   └──>  GCS_SEND_TEXT(MAV_SEVERITY_WARNING,"No airspeed sensor present or enabled");
 ├──>  SRV_Channels::init(); 
 ├──>   // gyro FFT needs to be initialized really late
 │   └──> gyro_fft.init(AP::scheduler().get_loop_rate_hz());
 ├──>  runcam.init();
 ├──>  hott_telem.init();
 ├──>  visual_odom.init();  // init library used for visual position estimation
 ├──>  vtx.init();
 ├──>  smartaudio.init();
 ├──>  tramp.init();
 ├──>  AP_Param::show_all(hal.console, true);
 ├──> send_watchdog_reset_statustext();
 ├──>  generator.init();
 ├──>  opendroneid.init();
 ├──>  efi.init(); // init EFI monitoring
 ├──>  temperature_sensor.init();
 ├──>  ais.init();
 ├──>  nmea.init();
 ├──>  fence.init();
 ├──>  
 │   └──> for (uint8_t i = 0; 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");

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();
 ├──> 
 │   │  /*
 │   │    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];
 │   └──> 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;

4. ArduCopter任务

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

AP_Vehicle::loop
 └──> scheduler.loop
     └──> run
         └──> task.function

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
};

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使用