基于pixhawk2.4.6硬件和NUTTX系统的ardupilot启动流程：从上电到ArduCopter应用层代码
原文: http://www.amovauto.com/portal.php?mod=view&aid=132 ,在此基础上进行了细化分析
px4从上电到执行rcS脚本的过程两块控制板（即fmu和io）上电后分别独立启动并运行各自的code。运行过程为：
(1) 2套stm32_start()分别初始化fmu和io
- fmu链接脚本: /ardupilot/modules/PX4Firmware/nuttx-configs/px4fmu-v2/scripts/ld.script，中由ENTRY_POINT(__start)指定入口方法为符号 __start；
- io链接脚本: /ardupilot/modules/PX4Firmware/nuttx-configs/px4io-v2/scripts/ld.script，中由ENTRY_POINT(__start)指定入口方法为符号 __start；
px4使用的是stm32f100(IO)和stm32f427（fmu），该入口函数在stm32_start.c中定义：
stm32_start.c位于ardupilot/modules/PX4NuttX/nuttx/arch/arm/src/stm32/
这里两个soc公用同一个 __start 函数；
__start-- #处理器执行的第一条指令
|
v
stm32_clockconfig()------ #初始化时钟 80003d2: f000 f827 bl 8000424
|
v
rcc_reset() #复位rcc
stm32_stdclockconfig() #初始化标准时钟
rcc_enableperipherals() #使能外设时钟
|
--------------------
|
v
stm32_fpuconfig() #配置fpu，
stm32_lowsetup() #基本初始化串口，之后可以使用up_lowputc()
stm32_gpioinit() #初始化gpio，只是调用stm32_gpioremap()设置重映射
clear bss # 清bss段
up_earlyserialinit() #初始化串口，之后可以使用up_putc()
stm32_boardinitialize()-- #板级初始化
|
v
stm32_spiinitialize() #初始化spi，只是调用stm32_configgpio()设置gpio
stm32_usbinitialize() #初始化usb，只是调用stm32_configgpio()设置gpio
board_led_initialize() #初始化led，只是调用stm32_configgpio()设置gpio
|
--------------------
|
v
上面__start 中调用的这些方法，在具体的芯片系列中有不同的定义，比如这里stm32f100 和stm32f427, 均在其芯片具体配置代码中实现了 stm32_clockconfig(），其称之为public方法，
stm32_start.c 的头部通过#include *.c 包含进来，如下所示，在编译时会按当前的芯片类型，走具体芯片的 stm32_clockconfig(）方法；
/****************************************************************************
* Private Functions
****************************************************************************/
/* Include chip-specific clocking initialization logic */
#if defined(CONFIG_STM32_STM32L15XX)
# include "stm32l15xxx_rcc.c"
#elif defined(CONFIG_STM32_STM32F10XX)
# include "stm32f10xxx_rcc.c"
#elif defined(CONFIG_STM32_STM32F20XX)
# include "stm32f20xxx_rcc.c"
#elif defined(CONFIG_STM32_STM32F30XX)
# include "stm32f30xxx_rcc.c"
#elif defined(CONFIG_STM32_STM32F40XX)
# include "stm32f40xxx_rcc.c"
#else
# error "Unsupported STM32 chip"
#endif
(2) 2套os_start()分别初始化nuttx系统
28. os_start()-------------#初始化操作系统 ardupilot\modules\PX4NuttX\nuttx\sched\os_start.c
29. |
30. v
31. dq_init() #初始化各种状态的任务列表（置为null）
32. g_pidhash[i]= #初始化唯一可以确定的元素--进程ID
33. g_pidhash[PIDHASH(0)]= #分配空闲任务的进程ID为0
34. g_idletcb= #初始化空闲任务的任务控制块
35. sem_initialize()-- #初始化信号量
36. |
37. v
38. dq_init() #将信号量队列置为null
39. sem_initholders() #初始化持有者结构以支持优先级继承，
40. |
41. --------
42. |
43. v
44. up_allocate_heap() #分配用户模式的堆（设置堆的起点和大小）
45. kumm_initialize() #初始化用户模式的堆
46. up_allocate_kheap() #分配内核模式的堆，
47. kmm_initialize() #初始化内核模式的堆，
48. task_initialize() #初始化任务数据结构，
49. irq_initialize() #将所有中断向量都指向同一个异常中断处理程序
50. wd_initialize() #初始化看门狗数据结构
51. clock_initialize() #初始化rtc
52. timer_initialize() #配置POSIX定时器
53. sig_initialize() #初始化信号
54. mq_initialize() #初始化命名消息队列
55. pthread_initialize() #初始化线程特定的数据，空函数
56. fs_initialize()--- #初始化文件系统
57. |
58. v
59. sem_init() #初始化节点信号量为1
60. files_initialize() #初始化文件数组，空函数
61. |
62. --------
63. |
64. v
65. net_initialize()-- #初始化网络
66. |
67. v
68. uip_initialize() #初始化uIP层
69. net_initroute() #初始化路由表，
70. netdev_seminit() #初始化网络设备信号量
71. arptimer_init() #初始化ARP定时器
72. |
73. --------
74. |
75. v
76. up_initialize()--- #处理器特定的初始化
77. |
78. v
79. up_calibratedelay()#校准定时器
80. up_addregion() #增加额外的内存段
81. up_irqinitialize() #设置中断优先级，关联硬件异常处理函数
82. up_pminitialize() #初始化电源管理，
83. up_dmainitialize() #初始化DMA，
84. up_timerinit() #初始化定时器中断
85. devnull_register() #注册/dev/null
86. devzero_register() #注册/dev/zero，
87. up_serialinit() #注册串口控制台/dev/console和串口/dev/ttyS0
88. up_rnginitialize() #初始化并注册随机数生成器
89. up_netinitialize() #初始化网络，是arch/arm/src/chip/stm32_eth.c中的
90. up_usbinitialize() #初始化usb驱动，
91. board_led_on() #打开中断使能led，
92. |
93. --------
94. |
95. v
96. lib_initialize() #初始化c库，空函数
97. group_allocate() #分配空闲组
98. group_setupidlefiles() #在空闲任务上创建stdout、stderr、stdin
99. group_initialize() #完全初始化空闲组
100. os_bringup()------ #创建初始任务 os_bringup.c
101. |
102. v
103. KEKERNEL_THREAD() #启动内核工作者线程
104. board_initialize() #最后一刻的板级初始化，nsh未调用
105. TASK_CREATE() #启动默认应用程序 os_bringup.c
106. |
107. --------
108. |
109. v
110. for up_idle() #空闲任务循环
111. |
112. --------------------
113. |
114. v
115. for(;;)
(3) 2套os_bringup()函数分别创建进程并传入进程指针CONFIG_USER_ENTRYPOINT
#ifdef CONFIG_NUTTX_KERNEL
DEBUGASSERT(USERSPACE->us_entrypoint != NULL);
taskid = TASK_CREATE("init", SCHED_PRIORITY_DEFAULT,
CONFIG_USERMAIN_STACKSIZE, USERSPACE->us_entrypoint,
(FAR char * const *)NULL);
#else
taskid = TASK_CREATE("init", SCHED_PRIORITY_DEFAULT,
CONFIG_USERMAIN_STACKSIZE,
(main_t)CONFIG_USER_ENTRYPOINT,
(FAR char * const *)NULL);
(4) fmu中的config.h文件宏定义
#define CONFIG_USER_ENTRYPOINT nsh_main
io中的config.h文件宏定义
#define CONFIG_USER_ENTRYPOINT user_start
(5) fmu开启nshshell
int nsh_main(int argc, char *argv[])
{
int exitval = 0;
int ret;
/* Call all C++ static constructors */
#if defined(CONFIG_HAVE_CXX) && defined(CONFIG_HAVE_CXXINITIALIZE)
up_cxxinitialize();
#endif
/* Make sure that we are using our symbol take */
#if defined(CONFIG_LIBC_EXECFUNCS) && defined(CONFIG_EXECFUNCS_SYMTAB)
exec_setsymtab(CONFIG_EXECFUNCS_SYMTAB, 0);
#endif
/* Register the BINFS file system */
#if defined(CONFIG_FS_BINFS) && (CONFIG_BUILTIN)
ret = builtin_initialize();
if (ret < 0)
{
fprintf(stderr, "ERROR: builtin_initialize failed: %d\n", ret);
exitval = 1;
}
#endif
/* Initialize the NSH library */
nsh_initialize();
/* If the Telnet console is selected as a front-end, then start the
* Telnet daemon.
*/
#ifdef CONFIG_NSH_TELNET
ret = nsh_telnetstart();
if (ret < 0)
{
/* The daemon is NOT running. Report the the error then fail...
* either with the serial console up or just exiting.
*/
fprintf(stderr, "ERROR: Failed to start TELNET daemon: %d\n", ret);
exitval = 1;
}
#endif
/* If the serial console front end is selected, then run it on this thread */
#ifdef CONFIG_NSH_CONSOLE
ret = nsh_consolemain(0, NULL);
/* nsh_consolemain() should not return. So if we get here, something
* is wrong.
*/
fprintf(stderr, "ERROR: nsh_consolemain() returned: %d\n", ret);
exitval = 1;
#endif
return exitval;
}
io
user_start 负责px4io 基础环境的初始化,包括PWM,串口,ADC 等资源的初始化,最后运行一个死循环,用于处理遥控器输入,与PX4FMU 通信的内容
controls_tick 负责处理遥控器的输入内容,包括SBUS 的处理sbus_input、 SPKT/DSM 的处理dsm_port_input、 PPM 的处理ppm_input
int
user_start(int argc, char *argv[])
{
/* configure the first 8 PWM outputs (i.e. all of them) */
up_pwm_servo_init(0xff);
/* run C++ ctors before we go any further */
up_cxxinitialize();
/* reset all to zero */
memset(&system_state, 0, sizeof(system_state));
/* configure the high-resolution time/callout interface */
hrt_init();
/* calculate our fw CRC so FMU can decide if we need to update */
calculate_fw_crc();
/*
* Poll at 1ms intervals for received bytes that have not triggered
* a DMA event.
*/
#ifdef CONFIG_ARCH_DMA
hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL);
#endif
/* print some startup info */
lowsyslog("\nPX4IO: starting\n");
/* default all the LEDs to off while we start */
LED_AMBER(false);
LED_BLUE(false);
LED_SAFETY(false);
#ifdef GPIO_LED4
LED_RING(false);
#endif
/* turn on servo power (if supported) */
#ifdef POWER_SERVO
POWER_SERVO(true);
#endif
/* turn off S.Bus out (if supported) */
#ifdef ENABLE_SBUS_OUT
ENABLE_SBUS_OUT(false);
#endif
/* start the safety switch handler */
safety_init();
/* initialise the control inputs */
controls_init();
/* set up the ADC */
adc_init();
/* start the FMU interface */
interface_init();
/* add a performance counter for mixing */
perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix");
/* add a performance counter for controls */
perf_counter_t controls_perf = perf_alloc(PC_ELAPSED, "controls");
/* and one for measuring the loop rate */
perf_counter_t loop_perf = perf_alloc(PC_INTERVAL, "loop");
struct mallinfo minfo = mallinfo();
lowsyslog("MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks);
/* initialize PWM limit lib */
pwm_limit_init(&pwm_limit);
/*
* P O L I C E L I G H T S
*
* Not enough memory, lock down.
*
* We might need to allocate mixers later, and this will
* ensure that a developer doing a change will notice
* that he just burned the remaining RAM with static
* allocations. We don't want him to be able to
* get past that point. This needs to be clearly
* documented in the dev guide.
*
*/
if (minfo.mxordblk < 600) {
lowsyslog("ERR: not enough MEM");
bool phase = false;
while (true) {
if (phase) {
LED_AMBER(true);
LED_BLUE(false);
} else {
LED_AMBER(false);
LED_BLUE(true);
}
up_udelay(250000);
phase = !phase;
}
}
/* Start the failsafe led init */
failsafe_led_init();
/*
* Run everything in a tight loop.
*/
uint64_t last_debug_time = 0;
for (;;) {
/* track the rate at which the loop is running */
perf_count(loop_perf);
/* kick the mixer */
perf_begin(mixer_perf);
if (!(r_setup_features & PX4IO_P_SETUP_FEATURES_ONESHOT)) {
/*
when in oneshot we don't trigger output
until we get new data from the FMU
*/
mixer_tick();
}
perf_end(mixer_perf);
/* kick the control inputs */
perf_begin(controls_perf);
controls_tick();
perf_end(controls_perf);
check_reboot();
/* check for debug activity (default: none) */
show_debug_messages();
/* post debug state at ~1Hz - this is via an auxiliary serial port
* DEFAULTS TO OFF!
*/
if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) {
isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x m=%u",
(unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG],
(unsigned)r_status_flags,
(unsigned)r_setup_arming,
(unsigned)r_setup_features,
(unsigned)mallinfo().mxordblk);
last_debug_time = hrt_absolute_time();
}
if (r_page_setup[PX4IO_P_SETUP_HEATER_DUTY_CYCLE] <= PX4IO_HEATER_MAX) {
control_IMU_heater(r_page_setup[PX4IO_P_SETUP_HEATER_DUTY_CYCLE]);
} else {
control_heartbeat_LED();
}
}
}
至此io板启动分析完了，fmu继续分析
(6)nsh_main中
nsh_initialize()初始化nshshell，
nsh_consolemain()执行nshshell
进入nsh_consolemain()
int nsh_consolemain(int argc, char *argv[])
{
FAR struct console_stdio_s *pstate = nsh_newconsole();
int ret;
DEBUGASSERT(pstate);
/* Execute the start-up script */
#ifdef CONFIG_NSH_ROMFSETC
(void)nsh_initscript(&pstate->cn_vtbl);
#endif
/* Initialize any USB tracing options that were requested */
#ifdef CONFIG_NSH_USBDEV_TRACE
usbtrace_enable(TRACE_BITSET);
#endif
/* Execute the session */
ret = nsh_session(pstate);
/* Exit upon return */
nsh_exit(&pstate->cn_vtbl, ret);
return ret;
}
(void)nsh_initscript(&pstate->cn_vtbl);初始化nsh脚本
ret = nsh_session(pstate);执行nsh脚本
至此nsh脚本还没有引入，所以需要去寻找nsh脚本是什么，进入nsh_initscript()
#ifdef CONFIG_NSH_ROMFSETC
int nsh_initscript(FAR struct nsh_vtbl_s *vtbl)
{
static bool initialized;
bool already;
int ret = OK;
/* Atomic test and set of the initialized flag */
sched_lock();
already = initialized;
initialized = true;
sched_unlock();
/* If we have not already executed the init script, then do so now */
if (!already)
{
ret = nsh_script(vtbl, "init", NSH_INITPATH);
}
return ret;
}
ret = nsh_script(vtbl, “init”, NSH_INITPATH); //Execute the NSH script at path.
#define NSH_INITPATH CONFIG_NSH_ROMFSMOUNTPT “/” CONFIG_NSH_INITSCRIPT
#define CONFIG_NSH_ROMFSMOUNTPT “/etc”
#define CONFIG_NSH_INITSCRIPT “init.d/rcS”
“/etc”是挂载目录
“init.d/rcS”是rcS路径
因此将”/etc”和”init.d/rcS”路径传入nsh_script()
int nsh_script(FAR struct nsh_vtbl_s *vtbl, FAR const char *cmd,
FAR const char *path)
{
FAR char *fullpath;
FAR FILE *savestream;
FAR char *buffer;
FAR char *pret;
int ret = ERROR;
/* The path to the script may be relative to the current working directory */
fullpath = nsh_getfullpath(vtbl, path);
if (!fullpath)
{
return ERROR;
}
/* Get a reference to the common input buffer */
buffer = nsh_linebuffer(vtbl);
if (buffer)
{
/* Save the parent stream in case of nested script processing */
savestream = vtbl->np.np_stream;
/* Open the file containing the script */
vtbl->np.np_stream = fopen(fullpath, "r");
if (!vtbl->np.np_stream)
{
nsh_output(vtbl, g_fmtcmdfailed, cmd, "fopen", NSH_ERRNO);
/* Free the allocated path */
nsh_freefullpath(fullpath);
/* Restore the parent script stream */
vtbl->np.np_stream = savestream;
return ERROR;
}
/* Loop, processing each command line in the script file (or
* until an error occurs)
*/
do
{
/* Flush any output generated by the previous line */
fflush(stdout);
#ifndef CONFIG_NSH_DISABLE_LOOPS
/* Get the current file position. This is used to control
* looping. If a loop begins in the next line, then this file
* offset will be needed to locate the top of the loop in the
* script file. Note that ftell will return -1 on failure.
*/
vtbl->np.np_foffs = ftell(vtbl->np.np_stream);
vtbl->np.np_loffs = 0;
if (vtbl->np.np_foffs < 0)
{
nsh_output(vtbl, g_fmtcmdfailed, "loop", "ftell", NSH_ERRNO);
}
#endif
/* Now read the next line from the script file */
pret = fgets(buffer, CONFIG_NSH_LINELEN, vtbl->np.np_stream);
if (pret)
{
/* Parse process the command. NOTE: this is recursive...
* we got to cmd_sh via a call to nsh_parse. So some
* considerable amount of stack may be used.
*/
if ((vtbl->np.np_flags & NSH_PFLAG_SILENT) == 0)
{
nsh_output(vtbl,"%s", buffer);
}
ret = nsh_parse(vtbl, buffer);
}
}
while (pret && (ret == OK || (vtbl->np.np_flags & NSH_PFLAG_IGNORE)));
/* Close the script file */
fclose(vtbl->np.np_stream);
/* Restore the parent script stream */
vtbl->np.np_stream = savestream;
}
/* Free the allocated path */
nsh_freefullpath(fullpath);
return ret;
}
其中nsh_output(vtbl, g_fmtcmdfailed, cmd, “fopen”, NSH_ERRNO);是将脚本打开
ret = nsh_parse(vtbl, buffer);解析脚本
至此分析完了fmu从上电到执行rcS脚本的过程。
从rcS启动到ArduCopter执行setup()、loop()
rcS的命令是nshshell的命令，比如
if rgbled start
先执行rgbled start，再作判断。
rgbled 可以对应到modules/PX4Firmware/Build/px4fmu-v2_APM.build/builtin_commands.c中
该文件在固件编译过程中自动生成，记录着一个名为g_builtin[]的数组，数组所有成员即为飞控上电之后，后台运行的所有应用程序，每个成员均有４个参数，分别为程序名、程序调度优先级、堆栈大小以及程序入口函数。
/* builtin command list - automatically generated, do not edit */
#include <nuttx/config.h>
#include <nuttx/binfmt/builtin.h>
extern int sercon_main(int argc, char *argv[]);
extern int serdis_main(int argc, char *argv[]);
extern int px4io_main(int argc, char *argv[]);
extern int mtd_main(int argc, char *argv[]);
extern int adc_main(int argc, char *argv[]);
extern int perf_main(int argc, char *argv[]);
extern int fmu_main(int argc, char *argv[]);
extern int tone_alarm_main(int argc, char *argv[]);
extern int ArduPilot_main(int argc, char *argv[]); ///< --------------------
extern int pwm_input_main(int argc, char *argv[]);
extern int nshterm_main(int argc, char *argv[]);
extern int mixer_main(int argc, char *argv[]);
extern int uorb_main(int argc, char *argv[]);
extern int ver_main(int argc, char *argv[]);
extern int usb_connected_main(int argc, char *argv[]);
extern int bl_update_main(int argc, char *argv[]);
extern int top_main(int argc, char *argv[]);
extern int reboot_main(int argc, char *argv[]);
const struct builtin_s g_builtins[] = {
{"sercon", SCHED_PRIORITY_DEFAULT, 2048, sercon_main},
{"serdis", SCHED_PRIORITY_DEFAULT, 2048, serdis_main},
{"px4io", SCHED_PRIORITY_DEFAULT, 1200, px4io_main},
{"mtd", SCHED_PRIORITY_DEFAULT, CONFIG_PTHREAD_STACK_DEFAULT, mtd_main},
{"adc", SCHED_PRIORITY_DEFAULT, CONFIG_PTHREAD_STACK_DEFAULT, adc_main},
{"perf", SCHED_PRIORITY_DEFAULT, 1800, perf_main},
{"fmu", SCHED_PRIORITY_DEFAULT, 1200, fmu_main},
{"tone_alarm", SCHED_PRIORITY_DEFAULT, CONFIG_PTHREAD_STACK_DEFAULT, tone_alarm_main},
///< ----------------------------------------------------------- >///
{"ArduPilot", SCHED_PRIORITY_DEFAULT, 4096, ArduPilot_main},
///< ----------------------------------------------------------- >///
{"pwm_input", SCHED_PRIORITY_DEFAULT, CONFIG_PTHREAD_STACK_DEFAULT, pwm_input_main},
{"nshterm", SCHED_PRIORITY_DEFAULT-30, 1500, nshterm_main},
{"mixer", SCHED_PRIORITY_DEFAULT, 4096, mixer_main},
{"uorb", SCHED_PRIORITY_DEFAULT, 2048, uorb_main},
{"ver", SCHED_PRIORITY_DEFAULT, 1024, ver_main},
{"usb_connected", SCHED_PRIORITY_DEFAULT, CONFIG_PTHREAD_STACK_DEFAULT, usb_connected_main},
{"bl_update", SCHED_PRIORITY_DEFAULT, 4096, bl_update_main},
{"top", SCHED_PRIORITY_DEFAULT, 1700, top_main},
{"reboot", SCHED_PRIORITY_DEFAULT, 800, reboot_main},
{NULL, 0, 0, NULL}
};
const int g_builtin_count = 18;
送些应用程序可分为系统应用、驱动程序、飞行控制程序等几类。系统应用如top可以显示当前运行的任务及CPU占用率；uORB在应用么间分发数据；nshterm处理nsh终端数据；pref用于打印任务状态信息等。驱动程序如gps，接收并处理gps数据；pwm，可启动、停止各PWM通道，显示并设置各通道的工作频率及最小、最大pwm值；px4io用于处理与协处理器的通信等；飞行控制程序如ArduPilot，这便是飞行控制找的入口。
{"rgbled", SCHED_PRIORITY_DEFAULT, CONFIG_PTHREAD_STACK_DEFAULT, rgbled_main}
也就对应到rgbled _main，所以就可以跳转到rgbled.cpp文件中的rgbled _main()函数
然后argc是参数数量，*argv[]是参数数组。对应到rcS
if rgbled start
rgbled是第一个参数，start是第二个参数，所以在rgbled _main()函数中，执行
if (!strcmp(verb, "start")) {
......
}
其他命令也是如此，于是跳过中间步骤，直接运行
sh /etc/init.d/rc.APM
---------------------------->>>
#!nsh
# APM startup script for NuttX on PX4
# To disable APM startup add a /fs/microsd/APM/nostart file
# check for an old file called APM, caused by
# a bug in an earlier firmware release
if [ -f /fs/microsd/APM ]
then
echo "APM file found - renaming"
mv /fs/microsd/APM /fs/microsd/APM.old
fi
if [ -f /fs/microsd/APM/nostart ]
then
echo "APM/nostart found - skipping APM startup"
sh /etc/init.d/rc.error
fi
set sketch NONE
if rm /fs/microsd/APM/boot.log
then
echo "removed old boot.log"
fi
set logfile /fs/microsd/APM/BOOT.LOG
if mkdir /fs/microsd/APM > /dev/null
then
echo "Created APM directory"
fi
if uorb start
then
echo "uorb started OK"
else
sh /etc/init.d/rc.error
fi
echo Starting ArduPilot
if ArduPilot start
then
echo ArduPilot started OK
else
sh /etc/init.d/rc.error
fi
echo "rc.APM finished"
再跳到rc.APM最后，执行
if ArduPilot start
那么一定会有ArduPilot_main和ArduPilot对应
extern int ArduPilot_main(int argc, char *argv[]);
{"ArduPilot", SCHED_PRIORITY_DEFAULT, 4096, ArduPilot_main},
ArduPilot_main则为ＡＰＭ飞行控制找的入口函数，但整个飞行控制找中并没有这个入口函数的实现。这是由于APM飞行控制软件可以应用在多种平台（如固定翼、多旋翼、地面车等）和不同的飞控板（如APM、Pixhawk、Navio+等）上，而对于不同的平台和飞控板，固件编译和启动有对应的流程。针对不同的飞控板，APM飞行控制找使用硬件抽象层隔离不同飞控板的差异化部分，将不同飞控板的入口函数（ArduPilot_main）统一封装到AP_MAIN()。对于不同的控制平台，ＡＰＭ飞行控制巧将AP_MAIN()再次封装为AP_HAL_MAIN_CALLBACKS(CALLBACKS)，最后将hal.run(argc, argv, CALLBACKS);函数解析到相应硬件抽象层类方法，如HAL_Linux.run()、HAL_PX4.run()等。
#pragma once
#include "HAL.h"
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
//将AP_MAIN替换成ArduPilot_main
#define AP_MAIN __EXPORT ArduPilot_main
#endif
#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; \
} \
}
//针对不同的机型，AP_MAIN再次封装为AP_HAL_MAIN_CALLBACKS(CALLBACKS)
#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; \
} \
}
这样，将不同机型、不同控制板的飞控程序入口函数统一到AP_HAL_MAIN_CALLBACKS(CALLBACKS)，只需改变机型参数就可以，如固定翼机调用AP_HAL_MAIN_CALLBACKS(&plane)，地面车调用AP_HAL_MAIN_CALLBACKS(&rover)，多旋翼则调用AP_HAL_MAIN_CALLBACKS(&copter)。 （AuduPilot.cpp末尾，copter对象在Copter.cpp末尾定义）
确定Pixhawk飞行控制栈的入口函数后，可看到函数体为hal.run()，最终会解析成Pixhawk的硬件抽象层类的方法HAL_PX4.run()。首先，在Copter.cpp中定义hal为硬件抽象层类AP_HAL::HAL的引用，
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
并且在Pixhawk固件的Makefile文件px4_target.mk中有
SKETCHFLAGS=$(SKETCHLIBINCLUDES) -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAVCAN_NO_ASSERTIONS -DUAVCAN_NULLPTR=nullptr -DARDUPILOT_BUILD -DTESTS_MATHLIB_DISABLE -DCONFIG_HAL_BOARD=HAL_BOARD_PX4 -DSKETCHNAME="\\\"$(SKETCH)\\\"" -DSKETCH_MAIN=ArduPilot_main -DAPM_BUILD_DIRECTORY=APM_BUILD_$(SKETCH)
继而，将hal解析为HAL_PX4。HAL_PX4_Class.cpp中代码如下：
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
......
const AP_HAL::HAL& AP_HAL::get_HAL() {
static const HAL_PX4 hal_px4;
return hal_px4;
}
#endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4
}
void HAL_PX4::run(int argc, char * const argv[], Callbacks* callbacks)实际上是一个守护任务，可Ｗ启动、停止飞行控制栈。Pixhawk飞控板在上电后，Nuttx操作系统经由rc.APM启动ArduPilot_main ，即这里的HAL_PX4::run(），继而由
static AP_HAL::HAL::Callbacks* g_callbacks;
void HAL_PX4::run(int argc, char * const argv[], Callbacks* callbacks) const
{
......
g_callbacks = callbacks;
......
daemon_task = px4_task_spawn_cmd(SKETCHNAME,
SCHED_FIFO,
APM_MAIN_PRIORITY,
APM_MAIN_THREAD_STACK_SIZE,
main_loop,
nullptr);
......
}
创建main_loop()线程(libraries/AP_HAL_PX4/HAL_PX4_Class.cpp)
static int main_loop(int argc, char **argv)
{
hal.uartA->begin(115200);
hal.uartB->begin(38400);
hal.uartC->begin(57600);
hal.uartD->begin(57600);
hal.uartE->begin(57600);
hal.scheduler->init();
// init the I2C wrapper class
PX4_I2C::init_lock();
/*
run setup() at low priority to ensure CLI doesn't hang the
system, and to allow initial sensor read loops to run
*/
hal_px4_set_priority(APM_STARTUP_PRIORITY);
schedulerInstance.hal_initialized();
g_callbacks->setup(); ///> copter->setup()
hal.scheduler->system_initialized();
perf_counter_t perf_loop = perf_alloc(PC_ELAPSED, "APM_loop");
perf_counter_t perf_overrun = perf_alloc(PC_COUNT, "APM_overrun");
struct hrt_call loop_overtime_call;
thread_running = true;
/*
switch to high priority for main loop
*/
hal_px4_set_priority(APM_MAIN_PRIORITY);
while (!_px4_thread_should_exit) {
perf_begin(perf_loop);
/*
this ensures a tight loop waiting on a lower priority driver
will eventually give up some time for the driver to run. It
will only ever be called if a loop() call runs for more than
0.1 second
*/
hrt_call_after(&loop_overtime_call, 100000, (hrt_callout)loop_overtime, nullptr);
g_callbacks->loop(); ///> copter->loop()
if (px4_ran_overtime) {
/*
we ran over 1s in loop(), and our priority was lowered
to let a driver run. Set it back to high priority now.
*/
hal_px4_set_priority(APM_MAIN_PRIORITY);
perf_count(perf_overrun);
px4_ran_overtime = false;
}
perf_end(perf_loop);
/*
give up 250 microseconds of time, to ensure drivers get a
chance to run. This relies on the accurate semaphore wait
using hrt in semaphore.cpp
*/
hal.scheduler->delay_microseconds(250);
}
thread_running = false;
return 0;
}
copter对象的setup和loop方法：
void Copter::setup()
{
// Load the default values of variables listed in var_info[]s
AP_Param::setup_sketch_defaults();
// setup storage layout for copter
StorageManager::set_layout_copter();
init_ardupilot();
// initialise the main loop scheduler
scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks), MASK_LOG_PM);
}
void Copter::loop()
{
scheduler.loop();
G_Dt = scheduler.get_last_loop_time_s();
}
scheduler对象的init和loop方法：
// initialise the scheduler
void AP_Scheduler::init(const AP_Scheduler::Task *tasks, uint8_t num_tasks, uint32_t log_performance_bit)
{
_tasks = tasks;
_num_tasks = num_tasks;
_last_run = new uint16_t[_num_tasks];
memset(_last_run, 0, sizeof(_last_run[0]) * _num_tasks);
_tick_counter = 0;
// setup initial performance counters
perf_info.set_loop_rate(get_loop_rate_hz());
perf_info.reset();
_log_performance_bit = log_performance_bit;
}
void AP_Scheduler::loop()
{
// wait for an INS sample
AP::ins().wait_for_sample();
const uint32_t sample_time_us = AP_HAL::micros();
if (_loop_timer_start_us == 0) {
_loop_timer_start_us = sample_time_us;
_last_loop_time_s = get_loop_period_s();
} else {
_last_loop_time_s = (sample_time_us - _loop_timer_start_us) * 1.0e-6;
}
// Execute the fast loop
// ---------------------
if (_fastloop_fn) {
_fastloop_fn();
}
// tell the scheduler one tick has passed
tick();
// run all the tasks that are due to run. Note that we only
// have to call this once per loop, as the tasks are scheduled
// in multiples of the main loop tick. So if they don't run on
// the first call to the scheduler they won't run on a later
// call until scheduler.tick() is called again
const uint32_t loop_us = get_loop_period_us();
const uint32_t time_available = (sample_time_us + loop_us) - AP_HAL::micros();
run(time_available > loop_us ? 0u : time_available);
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
// move result of AP_HAL::micros() forward:
hal.scheduler->delay_microseconds(1);
#endif
// check loop time
perf_info.check_loop_time(sample_time_us - _loop_timer_start_us);
_loop_timer_start_us = sample_time_us;
}
scheduler对象的run方法：
/*
run one tick
this will run as many scheduler tasks as we can in the specified time
*/
void AP_Scheduler::run(uint32_t time_available)
{
uint32_t run_started_usec = AP_HAL::micros();
uint32_t now = run_started_usec;
if (_debug > 1 && _perf_counters == nullptr) {
_perf_counters = new AP_HAL::Util::perf_counter_t[_num_tasks];
if (_perf_counters != nullptr) {
for (uint8_t i=0; i<_num_tasks; i++) {
_perf_counters[i] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, _tasks[i].name);
}
}
}
for (uint8_t i=0; i<_num_tasks; i++) {
uint16_t dt = _tick_counter - _last_run[i];
uint16_t interval_ticks = _loop_rate_hz / _tasks[i].rate_hz;
if (interval_ticks < 1) {
interval_ticks = 1;
}
if (dt < interval_ticks) {
// this task is not yet scheduled to run again
continue;
}
// this task is due to run. Do we have enough time to run it?
_task_time_allowed = _tasks[i].max_time_micros;
if (dt >= interval_ticks*2) {
// we've slipped a whole run of this task!
debug(2, "Scheduler slip task[%u-%s] (%u/%u/%u)\n",
(unsigned)i,
_tasks[i].name,
(unsigned)dt,
(unsigned)interval_ticks,
(unsigned)_task_time_allowed);
}
if (_task_time_allowed > time_available) {
// not enough time to run this task. Continue loop -
// maybe another task will fit into time remaining
continue;
}
// run it
_task_time_started = now;
current_task = i;
if (_debug > 1 && _perf_counters && _perf_counters[i]) {
hal.util->perf_begin(_perf_counters[i]);
}
_tasks[i].function();
if (_debug > 1 && _perf_counters && _perf_counters[i]) {
hal.util->perf_end(_perf_counters[i]);
}
current_task = -1;
// record the tick counter when we ran. This drives
// when we next run the event
_last_run[i] = _tick_counter;
// work out how long the event actually took
now = AP_HAL::micros();
uint32_t time_taken = now - _task_time_started;
if (time_taken > _task_time_allowed) {
// the event overran!
debug(3, "Scheduler overrun task[%u-%s] (%u/%u)\n",
(unsigned)i,
_tasks[i].name,
(unsigned)time_taken,
(unsigned)_task_time_allowed);
}
if (time_taken >= time_available) {
time_available = 0;
break;
}
time_available -= time_taken;
}
// update number of spare microseconds
_spare_micros += time_available;
_spare_ticks++;
if (_spare_ticks == 32) {
_spare_ticks /= 2;
_spare_micros /= 2;
}
}
