上节分析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即可上锁。
如果您觉得此文对您的发展有用,请随意打赏。
您的鼓励将是笔者书写高质量文章的最大动力^_^!!