APM学习（4）：ArduPilot程序运行流程
AruduPilot飞控程序包含硬件抽象层（HAL）和应用层（ArduCopter，ArduPlane等），硬件抽象层负责硬件设备的驱动，应用层则负责具体飞行器的控制和功能实现。
ArduPilot采用任务调度的方式执行程序，其中包含多种任务的执行，如飞行控制、电机驱动、通讯、命令相应等等。
一、 程序入口（main）
以ArduPlane为例，ArduPilot的程序入口的定义在Plane.cpp文件中，如下
//Plane.cpp
Plane plane;
AP_Vehicle& vehicle = plane;AP_HAL_MAIN_CALLBACKS(&plane);
其中AP_HAL_MAIN_CALLBACKS是一个宏定义，如下
//AP_HAL_Main.h
#ifndef AP_MAIN
#define AP_MAIN main
#endif...#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; \} \}
定义了main函数，在main函数中运行hal.run()函数，并且把飞行器对象作为参数传递给hal.run()函数。hal是硬件层对象，run是它的成员函数，是一个虚函数，对应不同硬件平台有对应的run函数，以ChibiOS为例，它的run函数定义如下
static AP_HAL::HAL::Callbacks* g_callbacks;void HAL_ChibiOS::run(int argc, char * const argv[], Callbacks* callbacks) const
{...g_callbacks = callbacks;main_loop();
}static void main_loop()
{...hal.scheduler->init();...g_callbacks->setup();...while (true) {g_callbacks->loop();...}...
}
在run()函数中调用main_loop()函数，main_loop()为程序的主流程，其中最主要的为在初始化阶段进行scheduler的初始化（init）和回调对象的初始化（setup），这里的回调对象（g_callbacks）就是指向具体飞行器对象的指针，如Copter，Plane等，在run函数中作为参数（callbacks）赋值给g_callbacks。初始化之后，进入程序的循环，循环调用回调对象的loop()函数。
二、 任务管理（scheduler）
ArduPilot通过任务管理器（AP_Scheduler）来管理多任务的运行。目前飞行器对象的setup()函数和loop()函数已经标准化，在它们的公共基类AP_Vehicle中定义。其中在AP_Vehicle::setup()函数中，主要包含了任务管理器（AP_Scheduler）的初始化，如下
void AP_Vehicle::setup()
{...const AP_Scheduler::Task *tasks;uint8_t task_count;uint32_t log_bit;get_scheduler_tasks(tasks, task_count, log_bit);AP::scheduler().init(tasks, task_count, log_bit);...
}
其中调用了AP_Scheduler::init()函数，其中AP::scheduler()返回一个全局的AP_Scheduler对象。
在AP_Vehicle的loop()函数中，则调用了AP_Scheduler的loop()函数，如下
void AP_Vehicle::loop()
{
#if AP_SCHEDULER_ENABLEDscheduler.loop();...
#endif...
}
通过AP_Scheduler的init()函数和loop()函数，ArduPilot实现对多任务的运行管理。
在Plane.cpp中开始位置，定义了程序需要运行的任务，如下
#define SCHED_TASK(func, rate_hz, max_time_micros, priority) SCHED_TASK_CLASS(Plane, &plane, func, rate_hz, max_time_micros, priority)
#define FAST_TASK(func) FAST_TASK_CLASS(Plane, &plane, func)
...const AP_Scheduler::Task Plane::scheduler_tasks[] = {// Units: Hz usFAST_TASK(ahrs_update),FAST_TASK(update_control_mode),FAST_TASK(stabilize),FAST_TASK(set_servos),SCHED_TASK(read_radio, 50, 100, 6),SCHED_TASK(check_short_rc_failsafe, 50, 100, 9),SCHED_TASK(update_speed_height, 50, 200, 12),SCHED_TASK(update_throttle_hover, 100, 90, 24),SCHED_TASK_CLASS(RC_Channels, (RC_Channels*)&plane.g2.rc_channels, read_mode_switch, 7, 100, 27),SCHED_TASK(update_GPS_50Hz, 50, 300, 30),SCHED_TASK(update_GPS_10Hz, 10, 400, 33),SCHED_TASK(navigate, 10, 150, 36),SCHED_TASK(update_compass, 10, 200, 39),SCHED_TASK(calc_airspeed_errors, 10, 100, 42),SCHED_TASK(update_alt, 10, 200, 45),...
是一个任务列表，每一项指明了任务运行的函数、频率、最大时延、优先级等，其中FAST_TASK定义表示最高频率和最高优先级运行。前面的四项分别为飞行控制的最核心任务，姿态解算（ahrs_update）、控制模式更新（update_control_mode）、稳态控制（stabilize ）和电机/伺服输出（set_servos）。
AP_Scheduler在运行过程中（loop函数）对这些任务进行管理，调度它们按照预定的频率运行。
三、 飞行控制程序流程
飞行控制相关的任务大致可以分为几类，以最高频率400Hz为例
1 高频任务（400Hz）
- 读取IMU数据：从陀螺仪和加速度计获取最新数据。
- 姿态估计（AHRS/EKF）：通过EKF算法，根据IMU、磁力计等数据，计算出飞行器姿态、位置、速度等状态信息。
- 运行姿态控制：控制飞行器达到目标的状态（姿态、速度、偏航角等）。
- 电机/伺服输出：输出滚转、俯仰、油门等通道信号，转化为每个电机/伺服的具体控制指令，并发送给控制器。
2 中速循环（100Hz）
- 读取其它传感器：如气压计、GPS、磁力计等数据。
- 获取接收机输入：读取遥控器控制信号。
3 中低频任务（50Hz以下）
- 导航任务（50Hz）：基于飞行器的位置和速度，根据任务（如航点）计算下一段路径，并执行位置和速度控制。
- 数据记录：将飞行数据写入SD卡（.bin文件）。
- 数据回传：通过数传电台向地面站（如Mission Planner）发送状态信息。
- 电池监测： 读取电压和电流。
- 故障安全检测： 检查传感器是否失效、遥控器是否失联、电池是否电量过低等，并触发相应的安全措施（如降落或返航）。
- 参数保存： 如果参数被修改，将其保存到存储区。
- LED控制： 更新LED状态，指示飞行模式、GPS锁定状态等。
总体的代码流程如下
上电启动|
setup() - 硬件和应用层初始化|
loop() - 任务循环 <----------------------------------------------| || 调度不同的任务 || ||--- 快速循环（400Hz） || |--- 读取IMU || |--- EKF姿态/位置解算 || |--- 姿态控制 || |--- 电机/伺服输出 || ||--- 中速循环（100Hz） || |--- 读取GPS/气压计 || |--- 读取RC输入 || ||--- 导航任务（50Hz） || |--- 位置控制 || |--- 计算目标姿态 || ||--- 其它任务（<10Hz） || |--- 数据日志 || |--- 数据回传 || |--- 安全检测 || ||------------------------------------------------------------