pixhawk ArduPilot_main启动与运行分析

上节分析2个系统启动脚本，一个是ardupilot/mk/PX4/ROMFS/init.d里的rcS，另一个是rc.APM，这个脚本在rcS里得到了调用，也就是说，rcS就是为Nuttx的启动文件。

查看rc.APM的最低端，调用ArduPilot_main

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

其中ArduPilot是一个内嵌的应用程序，由编译生成的ardupilot/modules/PX4Firmware/Build/px4fmu-v2_APM.build/builtin_commands.c可知，这个应用程序的入口地址就是ArduPilot_main。其实也只有这个ArduPilot_main应用是APM编写的，其余的应用基本上都是px4底层自带的。

{"ArduPilot",SCHED_PRIORITY_DEFAULT,4096,ArduPilot_main},
{"px4flow",SCHED_PRIORITY_DEFAULT,CONFIG_PTHREAD_STACK_DEFAULT,px4flow_main},

ArduPilot_main的内容是什么呢？ArduCopter.cpp实际上是多旋翼飞行器的主程序，那么该文件中找ArduPilot_main。

我们看源代码的时候，特别喜欢从main函数开始，顺着思路开始往下理。下面我就以ArduCopter工程里的px4-v2编译目标为例子，一步一步剖析main函数。
总的来说，这里的main函数就是ArduCopter.cpp里的AP_HAL_MAIN_CALLBACKS(&copter);，它实际上是一个宏定义，传进来的参数为类对象的引用，通过在AP_HAL_Main.h里的定义可知原型为：

#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_MAIN也是一个宏，如下：

#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 ||CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#define AP_MAIN __EXPORT ArduPilot_main
#endif

所以它实际上是这样的：

#define AP_HAL_MAIN_CALLBACKS(CALLBACKS)extern "C" { \
    int__EXPORT ArduPilot_main(int argc, char* const argv[]); \
   int __EXPORT ArduPilot_main(int argc, char* const argv[]) { \
       hal.run(argc, argv, CALLBACKS); \
       return 0; \
    }\
    }

将这个宏替换到ArduCopter.cpp里的AP_HAL_MAIN_CALLBACKS(&copter);它就变成了：

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

因此实际上这个工程的main函数就是ArduCopter.cpp里的ArduPilot_main函数。在这个main函数所在的CPP文件创建了不同的线程以供调用。

 

题外话builtin_commands.c是怎么生成的？这样的命令有很多，在rcS里就开始调用的比如rgbled就是的。至于这些内置的命令是怎么生成的，就要了解PX4原生的编译过程了，查看px4_targes.mk。

PX4_MAKE =$(v)+ GIT_SUBMODULES_ARE_EVIL=1 ARDUPILOT_BUILD=1 $(MAKE) -C $(SKETCHBOOK) -f $(PX4_ROOT)/Makefile.make EXTRADEFINES="$(SKETCHFLAGS)$(WARNFLAGS)$(OPTFLAGS) "'$(EXTRAFLAGS)'APM_MODULE_DIR=$(SKETCHBOOK)SKETCHBOOK=$(SKETCHBOOK)CCACHE=$(CCACHE)PX4_ROOT=$(PX4_ROOT)NUTTX_SRC=$(NUTTX_SRC)MAXOPTIMIZATION="-Os"UAVCAN_DIR=$(UAVCAN_DIR)

其中-f $(PX4_ROOT)/Makefile.make显示了makefile使用了PX4项目根目录的Makefile.make文件，拜读这里即可查出真相，真相在根目录下makefiles文件夹里的firmware.mk里。其实px4的代码使用的是Cmake，所以通过查看根目录下的CMakeLists.txt可知，真正产生builtin_commands.c的是px4_impl_nuttx.cmake里的px4_nuttx_generate_builtin_commands函数。同理，nuttx操作系统的ROMFS是由px4_nuttx_add_romfs函数产生的

 

接着继续分析main函数里的一些特征及其所做的事情。

由上面分析可知，main函数及为：ardupilot/Arducopter/Arducopter.cpp

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

其中hal定义为const AP_HAL::HAL& hal =AP_HAL::get_HAL();

ardupilot/Arducopter/Copter.cpp，而：

const AP_HAL::HAL& AP_HAL::get_HAL() {
    staticconst HAL_PX4 hal_px4;
    return hal_px4;
}

ardupilot/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp

classHAL_PX4:public AP_HAL::HAL {
public:
   HAL_PX4();
    void run(int argc,char*const argv[], Callbacks* callbacks)const override;
};

ardupilot/libraries/AP_HAL_PX4/HAL_PX4_Class.h

故hal.run函数即为HAL_PX4里面的run方法，这个main函数主要运行的是这个方法。这个方法能在HAL_PX4_Class.cpp中找到。在这个方法中有：

           

daemon_task =px4_task_spawn_cmd(SKETCHNAME,
                                SCHED_FIFO,
                                APM_MAIN_PRIORITY,
                                APM_MAIN_THREAD_STACK_SIZE,
                                main_loop,
                                NULL);

所以这里又调用了main_loop函数，在main_loop函数中，主要分析两点：

g_callbacks->setup();
while(!_px4_thread_should_exit){
g_callbacks->loop();
}

setup()函数在板子启动的时候被调用一次，它实际的调用来自每块板子的HAL，所有main函数是在HAL里的，其后就是loop()函数的调用，sketch的主要工作体现在loop()函数里。

setup、loop函数可以在ArduCopter.cpp中分别找到，其中setup函数里有scheduler.init(&scheduler_tasks[0],ARRAY_SIZE(scheduler_tasks));，这样在这个应用函数里又启动了调度任务:

constAP_Scheduler::TaskCopter::scheduler_tasks[]={
    SCHED_TASK(rc_loop,              100,    130),
    SCHED_TASK(throttle_loop,         50,     75),
    SCHED_TASK(update_GPS,            50,    200),
#if OPTFLOW == ENABLED
    SCHED_TASK(update_optical_flow,  200,    160),
#endif
    SCHED_TASK(update_batt_compass,   10,    120),
    SCHED_TASK(read_aux_switches,     10,     50),
    SCHED_TASK(arm_motors_check,      10,     50),
    SCHED_TASK(auto_disarm_check,     10,     50),
    SCHED_TASK(auto_trim,             10,     75),
    SCHED_TASK(update_altitude,       10,    140),
    SCHED_TASK(run_nav_updates,       50,    100),

所以apm的源码就是在px4的原生代码上以一个应用的接口加入了自己的调度任务，总的来说就是在px4上加了自己的应用。而在loop函数里开展了主要的循环fast_loop函数，如下：

// Main loop - 400hz
voidCopter::fast_loop()
{
 
    // IMU DCM Algorithm
    // --------------------
    read_AHRS();
 
    // run low level rate controllers that only require IMU data
    attitude_control.rate_controller_run();
 
#if FRAME_CONFIG == HELI_FRAME
    update_heli_control_dynamics();
#endif //HELI_FRAME
 
    // send outputs to the motors library
    motors_output();
 
    // Inertial Nav
    // --------------------
    read_inertia();
 
    // check if ekf has reset target heading
    check_ekf_yaw_reset();
 
    // run the attitude controllers
    update_flight_mode();
 
    // update home from EKF if necessary
    update_home_from_EKF();
 
    // check if we've landed or crashed
    update_land_and_crash_detectors();
 
#if MOUNT == ENABLED
    // camera mount's fast update
    camera_mount.update_fast();
#endif
 
    // log sensor health
    if(should_log(MASK_LOG_ANY)){
        Log_Sensor_Health();
    }
}

在这里边进行姿态解算，PID控制等等，那么分析到这里我想完过飞行器的同学就大致清楚了。
在这个工程里有一个重要的类叫Copter，大部分函数都是该类的方法，如voidCopter::arm_motors_check()，以后用的一些全局变量基本上都属于这个类里面的。

下面看一下一些简单的应用，以电机解锁为例子，玩过飞行器的同胞就知道，油门拉杆按右下角几秒就可以解锁，一般都是这样设置的，看看代码都是怎么实现的：
首先解锁的函数为arm_motors_check，调用次数为10hz。代码很简单，从中可以看出，2s后如果检查通过就可以解锁，当飞行器不属于手动控制模式时，拉杆打左下角2s即可上锁。