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"

 

你可能感兴趣的:(APM启动流程及ArduPilot函数入口)