ArduPilot飞控启动运行过程简介
ArduPilot飞控启动&运行过程简介
- 1. 源由
- 2. Copter飞控
- 2.1 入口
- 2.3 运行(main_loop)
- 3. Ardunio编程
- 3.1 setup - AP_Vehicle::setup
- 3.2 loop - AP_Vehicle::loop
- 4. ArduCopter任务
- 5. 参考资料
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; \} \}
```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 ###################################################├──> 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 determinethe order in which tasks are run. Convenience methods SCHED_TASKand 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 populatedFAST_TASK_CLASS(AP_InertialSensor, &copter.ins, update),// run low level rate controllers that only require IMU dataFAST_TASK(run_rate_controller),
#if AC_CUSTOMCONTROL_MULTI_ENABLED == ENABLEDFAST_TASK(run_custom_controller),
#endif
#if FRAME_CONFIG == HELI_FRAMEFAST_TASK(heli_update_autorotation),
#endif //HELI_FRAME// send outputs to the motors library immediatelyFAST_TASK(motors_output),// run EKF state estimator (expensive)FAST_TASK(read_AHRS),
#if FRAME_CONFIG == HELI_FRAMEFAST_TASK(update_heli_control_dynamics),
#endif //HELI_FRAME// Inertial NavFAST_TASK(read_inertia),// check if ekf has reset target heading or positionFAST_TASK(check_ekf_reset),// run the attitude controllersFAST_TASK(update_flight_mode),// update home from EKF if necessaryFAST_TASK(update_home_from_EKF),// check if we've landed or crashedFAST_TASK(update_land_and_crash_detectors),// surface tracking updateFAST_TASK(update_rangefinder_terrain_offset),
#if HAL_MOUNT_ENABLED// camera mount's fast updateFAST_TASK_CLASS(AP_Mount, &copter.camera_mount, update_fast),
#endifFAST_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_ENABLEDSCHED_TASK_CLASS(AP_OpticalFlow, &copter.optflow, update, 200, 160, 12),
#endifSCHED_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 == ENABLEDSCHED_TASK_CLASS(ToyMode, &copter.g2.toy_mode, update, 10, 50, 24),
#endifSCHED_TASK(auto_disarm_check, 10, 50, 27),SCHED_TASK(auto_trim, 10, 75, 30),
#if RANGEFINDER_ENABLED == ENABLEDSCHED_TASK(read_rangefinder, 20, 100, 33),
#endif
#if HAL_PROXIMITY_ENABLEDSCHED_TASK_CLASS(AP_Proximity, &copter.g2.proximity, update, 200, 50, 36),
#endif
#if BEACON_ENABLED == ENABLEDSCHED_TASK_CLASS(AP_Beacon, &copter.g2.beacon, update, 400, 50, 39),
#endifSCHED_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 == ENABLEDSCHED_TASK_CLASS(ModeSmartRTL, &copter.mode_smartrtl, save_position, 3, 100, 51),
#endif
#if HAL_SPRAYER_ENABLEDSCHED_TASK_CLASS(AC_Sprayer, &copter.sprayer, update, 3, 90, 54),
#endifSCHED_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 == ENABLEDSCHED_TASK(update_precland, 400, 50, 69),
#endif
#if FRAME_CONFIG == HELI_FRAMESCHED_TASK(check_dynamic_flight, 50, 75, 72),
#endif
#if LOGGING_ENABLED == ENABLEDSCHED_TASK(loop_rate_logging, LOOP_RATE, 50, 75),
#endifSCHED_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_ENABLEDSCHED_TASK(landinggear_update, 10, 75, 93),
#endifSCHED_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_ENABLEDSCHED_TASK_CLASS(AP_Mount, &copter.camera_mount, update, 50, 75, 108),
#endif
#if AP_CAMERA_ENABLEDSCHED_TASK_CLASS(AP_Camera, &copter.camera, update, 50, 75, 111),
#endif
#if LOGGING_ENABLED == ENABLEDSCHED_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),
#endifSCHED_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_ENABLEDSCHED_TASK_CLASS(AP_RPM, &copter.rpm_sensor, update, 40, 200, 129),
#endifSCHED_TASK_CLASS(AP_TempCalibration, &copter.g2.temp_calibration, update, 10, 100, 135),
#if HAL_ADSB_ENABLEDSCHED_TASK(avoidance_adsb_update, 10, 100, 138),
#endif
#if ADVANCED_FAILSAFE == ENABLEDSCHED_TASK(afs_fs_check, 10, 100, 141),
#endif
#if AP_TERRAIN_AVAILABLESCHED_TASK(terrain_update, 10, 100, 144),
#endif
#if AP_GRIPPER_ENABLEDSCHED_TASK_CLASS(AP_Gripper, &copter.g2.gripper, update, 10, 75, 147),
#endif
#if AP_WINCH_ENABLEDSCHED_TASK_CLASS(AP_Winch, &copter.g2.winch, update, 50, 50, 150),
#endif
#ifdef USERHOOK_FASTLOOPSCHED_TASK(userhook_FastLoop, 100, 75, 153),
#endif
#ifdef USERHOOK_50HZLOOPSCHED_TASK(userhook_50Hz, 50, 75, 156),
#endif
#ifdef USERHOOK_MEDIUMLOOPSCHED_TASK(userhook_MediumLoop, 10, 75, 159),
#endif
#ifdef USERHOOK_SLOWLOOPSCHED_TASK(userhook_SlowLoop, 3.3, 75, 162),
#endif
#ifdef USERHOOK_SUPERSLOWLOOPSCHED_TASK(userhook_SuperSlowLoop, 1, 75, 165),
#endif
#if HAL_BUTTON_ENABLEDSCHED_TASK_CLASS(AP_Button, &copter.button, update, 5, 100, 168),
#endif
#if STATS_ENABLED == ENABLEDSCHED_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使用
本文来自互联网用户投稿，文章观点仅代表作者本人，不代表本站立场，不承担相关法律责任。如若转载，请注明出处。 如若内容造成侵权/违法违规/事实不符，请点击【内容举报】进行投诉反馈！