APM启动流程及ArduPilot函数入口

PX4 (APM)的启动流程

1. 脚本运行阶段

PX4的软件主要可分为Bootloader，Nuttx内核，ROMFS文件系统的挂载，和ArduPilot程序的执行，我们首先讨论ROMFS文件系统挂载完成到ArduPilot执行的过程。

ROMFS挂载完成后，会先执行/etc/init.d/rcS脚本，在源代码中的位置为mk/PX4/ROMFS/init.d/rcS，其内容见附录一

在px4-v2上，正常运行时，此脚本可以简化为下：

set MODE autostart set USB autoconnect rgbled start set HAVE_RGBLED 1 rgbled rgb 16 16 16 echo "[init] looking for microSD..." mount -t vfat /dev/mmcsd0 /fs/microsd echo "[init] card mounted at /fs/microsd" set HAVE_MICROSD 1 tone_alarm 1 sercon echo "[init] USB interface connected" echo Running rc.APM sh /etc/init.d/rc.APM

可以看到此脚本的功能就是，挂载rgbled设备并置成白色，挂载SD卡，注册CDC/ACM驱动，执行rc.APM脚本。

rc.APM脚本位于源代码mk/PX4/ROMFS/init.d路径下，其内容见附录二。

在px4-v2上正常运行时，此脚本可以简化如下：

set deviceA /dev/ttyACM0 echo "Mounting binfs" mount -t binfs /dev/null /bin echo "binfs mounted OK" set sketch NONE rm /fs/microsd/APM/boot.log set logfile /fs/microsd/APM/BOOT.LOG mkdir /fs/microsd/APM > /dev/null echo "Detected FMUv2 board" set BOARD FMUv2 set deviceC /dev/ttyS1 set deviceD /dev/ttyS2 uorb start echo "uorb started OK" echo "Trying PX4IO board" set HAVE_PX4IO false px4io start norc set HAVE_PX4IO true echo "PX4IO board OK" px4io checkcrc /etc/px4io/px4io.bin echo "PX4IO CRC OK" fmu mode_pwm4 echo "Set FMU mode_pwm4" echo "Starting APM sensors" ms5611 start echo "ms5611 started OK" adc start echo "adc started OK" echo "Starting FMUv2 sensors" hmc5883 -C -T -X start echo "Have external hmc5883" hmc5883 -C -T -I -R 4 start echo "No internal hmc5883" mpu6000 -X -R 4 start mpu9250 -X -R 4 start echo "No MPU6000 or MPU9250 external" set HAVE_FMUV3 false mpu6000 start echo "Found MPU6000" l3gd20 start echo "l3gd20 started OK" lsm303d start echo "lsm303d started OK" ets_airspeed start meas_airspeed start meas_airspeed start -b 2 ll40ls -X start ll40ls -I start trone start mb12xx start px4flow start pwm_input start echo "started pwm_input driver" mtd start /fs/mtd echo "started mtd driver OK" mtd readtest /fs/mtd echo "mtd readtest OK" oreoled start autoupdate echo "oreoled started OK" batt_smbus -b 2 start echo "Found batt_smbus" irlock start mtd rwtest /fs/mtd echo "mtd rwtest OK" echo Starting ArduPilot $deviceA $deviceC $deviceD ArduPilot -d $deviceA -d2 $deviceC -d3 $deviceD start echo ArduPilot started OK echo "rc.APM finished"

可以看到，此脚本主要是启动px4io及各个传感器，最后启动了ArduPilot主程序。下面我们来来看看ArduPilot主程序。

2. ArduPilot程序的启动

在ArduCopter下的ArduCopter.cpp的最后一行为：

AP_HAL_MAIN_CALLBACKS(&copter);

而AP_HAL_MAIN_CALLBACKS定义在libraries/AP_HAL/ AP_HAL_Main.h中

#define AP_MAIN __EXPORT ArduPilot_main #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; \ } \ }

所以，这句可以修改为如下形式：

int __EXPORT ArduPilot_main(int argc, char* const argv[]); int __EXPORT ArduPilot_main(int argc, char* const argv[]) { hal.run(argc, argv, &copter); }

copter为全局变量，在ArduCopter/Copter.cpp文件中定义并初始化，全局变量hal也在该文件中定义。

在rc.APM中执行的命令ArduPilot-d $deviceA -d2 $deviceC -d3 $deviceD start，其实就是执行hal.run(argc,argv, &copter);

px4的hal类定义在libraries/ AP_HAL_PX4/ HAL_PX4_Class.cpp文件中，HAL_PX4::run相当于main函数。

通过此页的usage函数，可以看到此命令可以带4个参数，-d 第一个电台使用串口，-d2第二个电台使用串口，-d3 第三个电台使用串口，-d4第二个GPS使用串口。命令start后，通过px4_task_spawn_cmd创建了一个任务，其入口函数为main_loop，相当于单片机中的while(1)循环。

附录一rcS源码

set MODE autostart set USB autoconnect if rgbled start then set HAVE_RGBLED 1 rgbled rgb 16 16 16 else set HAVE_RGBLED 0 fi echo "[init] looking for microSD..." if mount -t vfat /dev/mmcsd0 /fs/microsd then echo "[init] card mounted at /fs/microsd" set HAVE_MICROSD 1 tone_alarm 1 else set HAVE_MICROSD 0 echo "Trying format of microSD" tone_alarm MBAGP if mkfatfs /dev/mmcsd0 then echo "microSD card formatted" if mount -t vfat /dev/mmcsd0 /fs/microsd then echo "format succeeded" set HAVE_MICROSD 1 tone_alarm 1 else echo "mount failed" tone_alarm MNBG if [ $HAVE_RGBLED == 1 ] then rgbled rgb 16 0 0 fi fi else echo "format failed" tone_alarm MNBGG if [ $HAVE_RGBLED == 1 ] then rgbled rgb 16 0 0 fi fi fi if [ -f /fs/microsd/etc/rc ] then echo "[init] reading /fs/microsd/etc/rc" sh /fs/microsd/etc/rc fi if [ -f /fs/microsd/etc/rc.txt ] then echo "[init] reading /fs/microsd/etc/rc.txt" sh /fs/microsd/etc/rc.txt fi if [ $USB != autoconnect ] then echo "[init] not connecting USB" else if sercon then echo "[init] USB interface connected" else echo "[init] No USB connected" fi fi if [ $HAVE_MICROSD == 0 ] then if usb_connected then echo "Opening USB nsh" else echo "booting with no microSD" set HAVE_MICROSD 1 fi fi if [ -f /etc/init.d/rc.APM -a $HAVE_MICROSD == 1 -a ! -f /fs/microsd/APM/nostart ] then echo Running rc.APM sh /etc/init.d/rc.APM else nshterm /dev/ttyACM0 & fi

附录二 rc.APM源码

set deviceA /dev/ttyACM0 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 if [ -f /bin/reboot ] then echo "binfs already mounted" else echo "Mounting binfs" if mount -t binfs /dev/null /bin then echo "binfs mounted OK" else sh /etc/init.d/rc.error fi 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 [ ! -f /bin/ArduPilot ] then echo "/bin/ardupilot not found" sh /etc/init.d/rc.error fi if mkdir /fs/microsd/APM > /dev/null then echo "Created APM directory" fi if [ -f /bin/px4io ] then if [ -f /bin/lsm303d ] then echo "Detected FMUv2 board" set BOARD FMUv2 else echo "Detected FMUv1 board" set BOARD FMUv1 fi else echo "Detected FMUv4 board" set BOARD FMUv4 fi if [ $BOARD == FMUv1 ] then set deviceC /dev/ttyS2 if [ -f /fs/microsd/APM/AUXPWM.en ] then set deviceD /dev/null else set deviceD /dev/ttyS1 fi else set deviceC /dev/ttyS1 set deviceD /dev/ttyS2 fi if uorb start then echo "uorb started OK" else sh /etc/init.d/rc.error fi if [ -f /fs/microsd/APM/mkblctrl ] then echo "Setting up mkblctrl driver" echo "Setting up mkblctrl driver" >> $logfile mkblctrl -d /dev/pwm_output fi if [ -f /fs/microsd/APM/mkblctrl_+ ] then echo "Setting up mkblctrl driver +" echo "Setting up mkblctrl driver +" >> $logfile mkblctrl -mkmode + -d /dev/pwm_output fi if [ -f /fs/microsd/APM/mkblctrl_x ] then echo "Setting up mkblctrl driver x" echo "Setting up mkblctrl driver x" >> $logfile mkblctrl -mkmode x -d /dev/pwm_output fi if [ -f /bin/px4io ] then echo "Trying PX4IO board" set HAVE_PX4IO false if px4io start norc then set HAVE_PX4IO true else echo Loading /etc/px4io/px4io.bin tone_alarm MBABGP if px4io update /etc/px4io/px4io.bin then echo "upgraded PX4IO firmware OK" tone_alarm MSPAA else echo "Failed to upgrade PX4IO firmware" tone_alarm MNGGG fi sleep 1 if px4io start norc then set HAVE_PX4IO true tone_alarm 1 fi fi else set HAVE_PX4IO false echo "No PX4IO support" fi if [ $HAVE_PX4IO == true ] then echo "PX4IO board OK" if px4io checkcrc /etc/px4io/px4io.bin then echo "PX4IO CRC OK" else echo "PX4IO CRC failure" echo "PX4IO CRC failure" >> $logfile tone_alarm MBABGP if px4io safety_on then echo "PX4IO disarm OK" else echo "PX4IO disarm failed" fi sleep 1 if px4io forceupdate 14662 /etc/px4io/px4io.bin then sleep 1 if px4io start norc then echo "PX4IO restart OK" echo "PX4IO restart OK" >> $logfile tone_alarm MSPAA else echo "PX4IO restart failed" echo "PX4IO restart failed" >> $logfile tone_alarm MNGGG sh /etc/init.d/rc.error fi else echo "PX4IO update failed" echo "PX4IO update failed" >> $logfile tone_alarm MNGGG fi fi else echo "No PX4IO board found" echo "No PX4IO board found" >> $logfile if [ $BOARD == FMUv2 ] then sh /etc/init.d/rc.error fi fi if [ $BOARD == FMUv1 -a $deviceD == /dev/ttyS1 ] then echo "Setting FMU mode_serial" fmu mode_serial else if fmu mode_pwm4 then echo "Set FMU mode_pwm4" fi fi echo "Starting APM sensors" if ms5611 start then echo "ms5611 started OK" else echo "no ms5611 found" echo "No ms5611 found" >> $logfile sh /etc/init.d/rc.error fi if adc start then echo "adc started OK" else echo "No adc" >> $logfile sh /etc/init.d/rc.error fi if [ $BOARD == FMUv1 ] then echo "Starting FMUv1 sensors" if hmc5883 -C -T -X start then echo "Have external hmc5883" else echo "No external hmc5883" fi if hmc5883 -C -T -I start then echo "Have internal hmc5883" else echo "No internal hmc5883" fi if mpu6000 start then echo "mpu6000 started OK" else sh /etc/init.d/rc.error fi if l3gd20 start then echo "l3gd20 started OK" else echo "No l3gd20" echo "No l3gd20" >> $logfile fi fi if [ $BOARD == FMUv2 ] then echo "Starting FMUv2 sensors" if hmc5883 -C -T -X start then echo "Have external hmc5883" else echo "No external hmc5883" fi if hmc5883 -C -T -I -R 4 start then echo "Have internal hmc5883" else echo "No internal hmc5883" fi if mpu6000 -X -R 4 start then echo "Found MPU6000 external" set HAVE_FMUV3 true else if mpu9250 -X -R 4 start then echo "Found MPU9250 external" set HAVE_FMUV3 true else echo "No MPU6000 or MPU9250 external" set HAVE_FMUV3 false fi fi if [ $HAVE_FMUV3 == true ] then if l3gd20 -X -R 4 start then echo "l3gd20 external started OK" else echo "No l3gd20" sh /etc/init.d/rc.error fi if lsm303d -X -R 6 start then echo "lsm303d external started OK" else echo "No lsm303d" sh /etc/init.d/rc.error fi if mpu6000 -R 14 start then echo "Found MPU6000 internal" else if mpu9250 -R 14 start then echo "Found MPU9250 internal" else echo "No MPU6000 or MPU9250" echo "No MPU6000 or MPU9250" >> $logfile sh /etc/init.d/rc.error fi fi if hmc5883 -C -T -S -R 8 start then echo "Found SPI hmc5883" fi else if mpu6000 start then echo "Found MPU6000" else if mpu9250 start then echo "Found MPU9250" else echo "No MPU6000 or MPU9250" echo "No MPU9250" >> $logfile fi fi if l3gd20 start then echo "l3gd20 started OK" else sh /etc/init.d/rc.error fi if lsm303d start then echo "lsm303d started OK" else sh /etc/init.d/rc.error fi fi fi if [ $BOARD == FMUv4 ] then echo "Starting FMUv4 sensors" if hmc5883 -C -T -X start then echo "Have external hmc5883" else echo "No external hmc5883" fi if hmc5883 -C -T -S -R 2 start then echo "Have SPI hmc5883" else echo "No SPI hmc5883" fi if mpu6000 -R 2 -T 20608 start then echo "Found ICM-20608 internal" fi if mpu9250 -R 2 start then echo "Found mpu9250 internal" fi fi if ets_airspeed start then echo "Found ETS airspeed sensor" fi if meas_airspeed start then echo "Found MEAS airspeed sensor" else if meas_airspeed start -b 2 then echo "Found MEAS airspeed sensor (bus2)" fi fi if ll40ls -X start then echo "Found external ll40ls sensor" fi if ll40ls -I start then echo "Found internal ll40ls sensor" fi if trone start then echo "Found trone sensor" fi if mb12xx start then echo "Found mb12xx sensor" fi if [ -f /bin/px4flow ] then if px4flow start then echo "Found px4flow sensor" fi fi if pwm_input start then echo "started pwm_input driver" fi if mtd start /fs/mtd then echo "started mtd driver OK" else echo "failed to start mtd driver" echo "failed to start mtd driver" >> $logfile sh /etc/init.d/rc.error fi if mtd readtest /fs/mtd then echo "mtd readtest OK" else echo "failed to read mtd" echo "failed to read mtd" >> $logfile sh /etc/init.d/rc.error fi if [ -f /bin/oreoled ] then if oreoled start autoupdate then echo "oreoled started OK" fi fi if batt_smbus -b 2 start then echo "Found batt_smbus" fi if irlock start then echo "irlock started" fi if [ $BOARD == FMUv2 -o $BOARD == FMUv4 ] then if mtd rwtest /fs/mtd then echo "mtd rwtest OK" else echo "failed to test mtd" echo "failed to test mtd" >> $logfile sh /etc/init.d/rc.error fi fi echo Starting ArduPilot $deviceA $deviceC $deviceD if ArduPilot -d $deviceA -d2 $deviceC -d3 $deviceD start then echo ArduPilot started OK else sh /etc/init.d/rc.error fi echo "rc.APM finished"