代码收藏家 ArduPilot飞控启动指南
ArduPilot飞控启动&运行过程简介
1. 源由
ArduPilot从整体的设计框架角度，感觉是更加容易上手，尤其是对一些相对熟悉C语言/嵌入式固件开发的兄弟们来说。
- 基于Ardunio编程方式
- 采用C++类方式进行抽象
- 应用业务模块化
- 模块考虑重复利用
- 设备代码工程隔离
- 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; \
} \
}
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
这里的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
├──> <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;
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");
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;
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使用
