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"