啟動代碼及入口函數(shù)

基礎(chǔ)知識

• 關(guān)于坐標(biāo)系

1）GeographicCoordinate System

Represents position on earth with alongitude and latitude value (Geographic_coordinate_system). Additionally thealtitude may may be included. The altitude can be expressed as distance fromthe earth center or as altitude above the mean sea level. All in all, thisgives a spherical coordinate system.

2）Earth-Fixed frame

Cartesian coordinate system at the center of the earth. Thepositive z axis goes through the north pole. The x and y axes are on theequatorial plane.

3）Body Fixed Frame

The x axis points in forward (defined bygeometry and not by movement) direction. (= roll axis)The y axis points to the right(geometrically) (= pitch axis)The z axis points downwards (geometrically)(= yaw axis)

4）EulerAngles

Usually a conversion between a earth fixed“ground” frame and the body fixed “in-air” frame is described via Euler-Angles.There are multiple conventions of the Euler angles.The rotation order for the Tait-Bryan angles is Z Y’X” (see thefigure):

rotation of around Z (yaw)
rotation of around Y' (pitch)
rotation of around X” (roll)

• pixhawk的HAL
  

• 3.pixhawk的控制圖
  

代碼分析

1、關(guān)于兩個控制器和任務(wù)優(yōu)先級

在PX4Firmware/src/modules中的mc_att_control：姿態(tài)控制器和mc_pos_control位置控制器（mc：multicopter），整個系統(tǒng)都是圍繞著這兩個控制器。mc_att_control – Multirotor attitude controllermc_pos_control – Multirotor position controllerThe PX4 firmware is organized in priority bands:

(interrupt level) fast sensordrivers

watchdog/system state monitors

actuator outputs (PWM outputdriver thread, IO comms sender thread)

attitude controller(s)

slow/blocking sensor drivers(must not block attitude controllers)

destination/positioncontroller(s)

default priority - generic usercode, shell commands, random crap, all RR scheduled

logger, parameter syncer

idle

2、 關(guān)于.mk文件（必須深入理解）

了解整個代碼運行過程的最簡便方法就是通過其makefile了解文件的包含關(guān)系和調(diào)用關(guān)系。請仔細(xì)閱讀arducopter中的.mk和modules/PX4Firmware中的.mk文件。.mk文件就是在Linux下編寫的一些腳本文件（makefile），類似于編譯過程中的條件編譯，便于直接使用命令行進(jìn)行編譯鏈接。1）首先介紹上層應(yīng)用的.mk。在ardupilot/mk/PX4/Tool中px4_common.mk文件中是關(guān)于所有PX4平臺的通用編譯文件，config_px4fmu_v2_APM.mk中針對pixhawk的特有硬件的編譯文件。下面實例代碼是config_px4fmu_v2_APM.mk中的代碼，.mk腳本主要目的是為了選擇需要編譯的MODULES。# Makefile for the px4fmu-v2_APM configuration include $(SKETCHBOOK)/mk/PX4/px4_common.mk MODULES += drivers/lsm303d MODULES += drivers/l3gd20 MODULES += drivers/mpu9250 MODULES += drivers/boards/px4fmu-v2 MODULES += drivers/pwm_input MODULES += modules/uavcan MODULES += lib/mathlib MODULES += drivers/px4io MODULES += drivers/px4flow MODULES += drivers/oreoled

Makefile的文件夾，指定編譯的規(guī)則，編譯順序：

ardupilot/ArduCopter/Makefile àmk/apm.mkà environ.mk, targets.mk, sketch_sources.mk, board_px4.mkà find_tools.mk,px4_targets.mk。

尤其是上面的這個px4_targets.mk文件，該文件中涉及了所有的需要編譯的文件，最后使用命令行px4-v2編譯整個工程，它是最終的編譯過程的執(zhí)行者，注意其中px4-v2規(guī)則的詳細(xì)執(zhí)行情況，它會去調(diào)用PX4Firmware的makefile。下面針對pixhawk硬件平臺摘錄比較重要一部分。

/*指定PX4Firmware、PX4NuttX、uavcan */ # these can be overridden in developer.mk PX4FIRMWARE_DIRECTORY ?= $(SKETCHBOOK)/modules/PX4Firmware PX4NUTTX_DIRECTORY ?= $(SKETCHBOOK)/modules/PX4NuttX UAVCAN_DIRECTORY ?= $(SKETCHBOOK)/modules/uavcan PX4_ROOT := $(shell cd $(PX4FIRMWARE_DIRECTORY) && pwd) NUTTX_ROOT := $(shell cd $(PX4NUTTX_DIRECTORY) && pwd) NUTTX_SRC := $(NUTTX_ROOT)/nuttx/ UAVCAN_DIR=$(shell cd $(UAVCAN_DIRECTORY) && pwd)/ ……… /*選擇硬件平臺*/ # we have different config files for V1 and V2 PX4_V1_CONFIG_FILE=$(MK_DIR)/PX4/config_px4fmu-v1_APM.mk PX4_V2_CONFIG_FILE=$(MK_DIR)/PX4/config_px4fmu-v2_APM.mk PX4_V4_CONFIG_FILE=$(MK_DIR)/PX4/config_px4fmu-v4_APM.mk ……… /*針對pixhawk，請詳細(xì)了解該部分的實現(xiàn)*/ px4-v2: $(BUILDROOT)/make.flags CHECK_MODULES $(MAVLINK_HEADERS) $(PX4_ROOT)/Archives/px4fmu-v2.export $(SKETCHCPP) module_mk px4-io-v2 $(RULEHDR)/* PX4_ROOT = ardupilot/modules/PX4Firmware*/ $(v) rm -f $(PX4_ROOT)/makefiles/$(PX4_V2_CONFIG_FILE) $(v) cp $(PX4_V2_CONFIG_FILE) $(PX4_ROOT)/makefiles/nuttx/ $(PX4_MAKE) px4fmu-v2_APM $(v) /bin/rm -f $(SKETCH)-v2.px4 $(v) arm-none-eabi-size $(PX4_ROOT)/Build/px4fmu-v2_APM.build/firmware.elf $(v) cp $(PX4_ROOT)/Images/px4fmu-v2_APM.px4 $(SKETCH)-v2.px4 $(v) $(SKETCHBOOK)/Tools/scripts/add_git_hashes.py $(HASHADDER_FLAGS) "$(SKETCH)-v2.px4" "$(SKETCH)-v2.px4" $(v) echo "PX4 $(SKETCH) Firmware is in $(SKETCH)-v2.px4" /*最后編譯生成的*.px4文件，直接下載到飛控板中即可實現(xiàn)飛行控制*/ ……… px4-clean: clean CHECK_MODULES px4-archives-clean px4-cleandep $(v) /bin/rm -rf $(PX4_ROOT)/makefiles/build $(PX4_ROOT)/Build $(PX4_ROOT)/Images/*.px4 $(PX4_ROOT)/Images/*.bin $(v) /bin/rm -rf $(PX4_ROOT)/src/modules/uORB/topics $(PX4_ROOT)/src/platforms/nuttx/px4_messages px4-cleandep: clean $(v) find $(PX4_ROOT)/Build -type f -name '*.d' | xargs rm -f $(v) find $(UAVCAN_DIRECTORY) -type f -name '*.d' | xargs rm -f $(v) find $(SKETCHBOOK)/$(SKETCH) -type f -name '*.d' | xargs rm -f px4-v2-upload-solo: px4-v2 scp $(SKETCH)-v2.px4 root@10.1.1.10:/tmp/ ssh root@10.1.1.10 PYTHONUNBUFFERED=1 loadPixhawk.py /tmp/ArduCopter-v2.px4 ssh root@10.1.1.10 rm /tmp/ArduCopter-v2.px4; ……… px4-v2-upload: px4-v2 $(RULEHDR) $(v) $(PX4_MAKE) px4fmu-v2_APM upload px4-upload: px4-v1-upload px4-archives-clean: $(v) /bin/rm -rf $(PX4_ROOT)/Archives 2）接下來就是PX4Firmware中的.mk。如果仔細(xì)了解了上面所講述的，那么這部分就很easy了，不再細(xì)講，自己去看代碼吧。需要說明的是在PX4Firmware的makefile文件中有一個README.txt文本，該文本詳細(xì)的介紹了PX4Firmware中的.mk的調(diào)用關(guān)系和作用。補(bǔ)充一點，PX4Firmware/makefile/nuttx中的config_px4fmu-v2_default.mk包含了所有基本的模塊，從這里再一次驗證了開篇講述的ardupilot的代碼是基于pixhawk原生代碼PX4Firmware“閹割”而來的。所以在后期做開發(fā)的過程中編寫應(yīng)用程序只是按照ardupilot現(xiàn)有的子模塊添加自己的就可以，參考mc_att_control實現(xiàn)。

在PX4Firmware/src/modules中添加一個新的文件夾，命名為summer
在summer文件夾中創(chuàng)建module.mk文件，并輸入以下內(nèi)容：

MODULE_COMMAND = summer SRCS = summer.cpp

在summer文件夾中創(chuàng)建summer.c文件，編譯需要實現(xiàn)的功能代碼。
注冊新添加的應(yīng)用到NuttShell中：PX4Firmware/makefiles/nuttx/config_px4fmu-v2_default.mk文件中添加如下內(nèi)容：

MODULES += modules/summer

任務(wù)的啟動就是靠NuttShell的內(nèi)建命令啟動的，包含rcS，特別是rcS一定要了解，下面會詳細(xì)介紹。

3、關(guān)于編譯鏈接庫（上層應(yīng)用庫 libraries）

在ardupilot/Arducopter中有一個make.inc文件，指定了編譯ArduCopter時需要編譯哪些依賴庫，它對系統(tǒng)及硬件進(jìn)行了抽象，以獲取更好的可移植性。

http://dev.ardupilot.com/wiki/apmcopter-programming-libraries/

如果想添加或者刪除編譯的文件，就在此處進(jìn)行修改。如下是官方默認(rèn)的。

LIBRARIES += AP_Common LIBRARIES += AP_Menu LIBRARIES += AP_Param LIBRARIES += StorageManager LIBRARIES += GCS LIBRARIES += GCS_MAVLink LIBRARIES += AP_SerialManager LIBRARIES += AP_GPS LIBRARIES += DataFlash LIBRARIES += AP_ADC LIBRARIES += AP_Baro LIBRARIES += AP_Compass LIBRARIES += AP_Math LIBRARIES += AP_InertialSensor LIBRARIES += AP_AccelCal LIBRARIES += AP_AHRS LIBRARIES += AP_NavEKF

4、啟動腳本（rcS：有兩個，比較重要，任務(wù)分工）

所謂啟動腳本其實是用shellscript寫的啟動文件以前寫過比較簡單的腳本文件，還是參考的鳥哥的那本書，還有就是Linux啟動文件也是用的這個寫的，有興趣的可以去Linux平臺下查看一下，會寫C的學(xué)這個很快，該死的C++什么時候能學(xué)會啊。

• 1）其中之一就是負(fù)責(zé)較為底層driver里面的（比如mpu9250的register map、驅(qū)動），在使用命令px4-v2-upload以后，會在modules/PX4Firmware目錄下生成build文件夾，內(nèi)部是編譯生成的文件（不編譯沒有），在modules/PX4Firmware/build/px4fmu-v2_APM.build/romfs_scratch/init.d文件下就是rcS和rc.APM，這個rcS是自啟動腳本（setMODE autostart）。rcS腳本會掛載SD卡（這就是沒有SD飛控板不能使用的原因所在），然后跳轉(zhuǎn)rc.APM中，該腳本會在pixhawk系統(tǒng)中創(chuàng)建binfs，判斷硬件類型，然后啟動orb（對，就是那個uORB），然后啟動各種傳感器，然后咔咔咔，咔咔咔，完了，自己去看腳本吧。摘一部分鑒賞，關(guān)于shell腳本的編寫自己百度吧，跟C語言的風(fēng)格很像。

其中類似于mpu9250 start就啟動了mpu9250模塊的驅(qū)動，理解該部分需要結(jié)合modules/PX4Firmware/src/drivers/mpu9250中的CMakeLists.txt和module.mk這兩個，其中CMakeLists.txt是使用Cmake寫的，和module.mk類似，在最新的pixhawk原生代碼中刪除了make，使用清一色的Cmake。

上面標(biāo)紅的MODULE_COMMAND = mpu9250就是為了以后可以直接在.mk文件直接使用的命令mpu9250。理解了上述以后，其他的driver中的所有的傳感器模塊、各種總線和modules/PX4Firmware/src/modules中的各種算法（mc_att_control、land_detector等等）都是按照此類方法實現(xiàn)的，不在贅述，但是這個rcS只是負(fù)責(zé)啟動driver中的，modules/PX4Firmware/src/modules需要靠下面的一個rcS啟動，兩個rcS各自分工。全部都是.mk啊，.mk，.mk~~~~

下一階段主要應(yīng)該就是放在這個modules/PX4Firmware/src/modules內(nèi)部了，整個控制響應(yīng)過程全靠它了，尤其是commander，校磁什么的都在里面，各種calibration，見過2000+行的函數(shù)么？！怕么？ ~~~ 168MHZ的主頻呢，怕啥

PS：需要注意的是，在modules/PX4Firmware/src/drivers/boards/px4fmu-v2中有幾個.c文件，其中px4fmu_spi.c和board_config.h。

px4fmu_spi.c 會在入口函數(shù)__start()中被調(diào)用初始化SPI，下面會提到。

* Name: stm32_spiinitialize * Description: * Called to configure SPI chip select GPIO pins for the PX4FMU board.

board_config.h：如下的代碼是不是很熟悉，用過單片機(jī)的肯定都見過，尤其是pixhawk使用的又是STM32，再加上ST公司的芯片在各個高校中的普及程度，雖然我沒用過，我是菜鳥。

• 2）另外一個rcS就是modules/PX4Firmware/ROMFS/px4fmu_common/init.d中的rcS，PX4FMU的自啟動文件，它主要是負(fù)責(zé)啟動modules/PX4Firmware/modules里面的關(guān)于各自控制算法的模塊。同時，該rcS會以腳本的形式啟動同一級目錄中的rc.mc_apps、rc_io等等。詳細(xì)實現(xiàn)過程參見源代碼。

地址：https://github.com/PX4/Firmware/tree/master/ROMFS/px4fmu_common/init.d

舉一例說明問題，rc.mc_apps：關(guān)于姿態(tài)估計、位置估計和姿態(tài)控制和位置控制的，源代碼如下。

#!nsh # Standard apps for multirotors: # att & pos estimator, att & pos control. # The system is defaulting to INAV_ENABLED = 1 # but users can alternatively try the EKF-based # filter by setting INAV_ENABLED = 0 if param compare INAV_ENABLED 1 then attitude_estimator_q start //姿態(tài)估計 position_estimator_inav start//位置估計 else if param compare LPE_ENABLED 1 then attitude_estimator_q start local_position_estimator start else ekf2 start fi fi if mc_att_control start //姿態(tài)控制 then else # try the multiplatform version mc_att_control_m start fi if mc_pos_control start //位置控制 then else # try the multiplatform version mc_pos_control_m start fi # Start Land Detector land_detector start multicopter //啟動落地檢測

應(yīng)該能了解代碼中的xxxxx start的含義了吧~~~~還不了解的話請撥打114，有專人為您解答
六、再深入一點

1、主控STM32F4的選擇（肯定很多人不知道這個）

在ardupilot/modules/PX4NuttX/nuttx/arch/arm/src/stm32中，主要是通過在stm32_start.c中包含頭文件<arch/board/board.h>，board.h中包含<stm32.h>并且配置了STM32的時鐘（Clock），stm32.h中包含<chip.h>。另外，stm32.h中還包含了關(guān)于stm32的各種Peripherals的頭文件，即各種外設(shè)（spi、can、uart、i2c等等）的驅(qū)動。

在ardupilot/modules/PX4NuttX/nuttx/arch/arm/src/stm32文件下有頭文件chip.h，內(nèi)部通過條件編譯執(zhí)行進(jìn)行主控MCU的選擇。實例代碼如下。

/* STM32 F2 Family ******************************************************************/ #elif defined(CONFIG_STM32_STM32F20XX) # include "chip/stm32f20xxx_pinmap.h" /* STM32 F3 Family ******************************************************************/ #elif defined(CONFIG_STM32_STM32F30XX) # include "chip/stm32f30xxx_pinmap.h" /* STM32 F4 Family ******************************************************************/ #elif defined(CONFIG_STM32_STM32F40XX) # include "chip/stm32f40xxx_pinmap.h" #else # error "No pinmap file for this STM32 chip" #endif

2、入口函數(shù)（__start()）（這個就更不知道了吧）

在ardupilot/modules/PX4NuttX/nuttx/arch/arm/src/stm32文件下有定義文件stm32_start.c，內(nèi)部有個上電/重啟的函數(shù)入口__start(void)。仔細(xì)看，一行行的看，別偷懶，代碼附加列出了幾個小問題，檢查自己一下都會么？！！！/**************************************************************************** * Public Functions ****************************************************************************/ /**************************************************************************** * Name: _start * * Description: * This is the reset entry point. * ****************************************************************************/ void __start(void) { const uint32_t *src; uint32_t *dest; #ifdef CONFIG_ARMV7M_STACKCHECK /* Set the stack limit before we attempt to call any functions */ __asm__ volatile ("sub r10, sp, %0" : : "r" (CONFIG_IDLETHREAD_STACKSIZE - 64) : ); /*限制棧大小，可以防止遞歸把棧內(nèi)存浪費完了，知道原因么？！*/ #endif /* Configure the uart so that we can get debug output as soon as possible */ stm32_clockconfig(); stm32_fpuconfig(); stm32_lowsetup(); stm32_gpioinit(); showprogress('A'); /* Clear .bss. We'll do this inline (vs. calling memset) just to be * certain that there are no issues with the state of global variables. inline的好處。 */ for (dest = &_sbss; dest < &_ebss; ) { *dest++ = 0; } showprogress('B'); /* Move the intialized data section from his temporary holding spot in * FLASH into the correct place in SRAM. The correct place in SRAM is * give by _sdata and _edata. The temporary location is in FLASH at the * end of all of the other read-only data (.text, .rodata) at _eronly. * 知道上述所講的幾個segments的分布么？！*/ for (src = &_eronly, dest = &_sdata; dest < &_edata; ) { *dest++ = *src++; } showprogress('C'); /* Perform early serial initialization */ #ifdef USE_EARLYSERIALINIT up_earlyserialinit(); #endif showprogress('D'); /* For the case of the separate user-/kernel-space（知道幾比幾么？） build, perform whatever * platform specific initialization of the user memory is required. * Normally this just means initializing the user space .data and .bss * segments. */ #ifdef CONFIG_NUTTX_KERNEL stm32_userspace(); showprogress('E'); #endif /* Initialize onboard resources */ stm32_boardinitialize();//初始化SPI（飛控板上的傳感器都是通過SPI通信）和LEDs。 // 還記上面提到的px4fmu_spi.c么？！ showprogress('F'); /* Then start NuttX */ showprogress('\r'); showprogress('\n'); os_start();//還記得上一篇博文的這個么？ /* Shoulnd't get here */ for(;;); }

輕松一下：飛控板上為什么使用SPI而不用I2C?如下是官方原版解釋，說白了就是速度問題，如下權(quán)當(dāng)練練English~~~

I2C was not intended by Philips as a high rate sensor bus - this is what SPI has been invented for. Running multiple sensors at a suitably high rateover I2C is not recommended. Our hardware is designed to rely on SPI for allcritical sensors, and is available globally. Given the individual cost of thediscovery kit and sensor boards and power supply, one of the available Pixhawkkits is actually not more expensive, and will save a lot of trouble duringdevelopment and operation.

3、 Init sensors

上面那么多傳感器必須得初始化以后才能使用啊，所以需要先初始化用到的所有的傳感器，在哪配置呢，你找到?jīng)]?! 在ardupilot/modules/PX4Firmware/src/modules/sensors文件中有定義文件sensor.c（關(guān)于飛控板中的各個sensor的初始化函數(shù)，以task的方式啟動，初始化完成以后kill掉這個task，都是POSIX接口的API）。

首先是sensors_main函數(shù)

int sensors_main(int argc, char *argv[]) { ……… if (OK != sensors::g_sensors->start()) { //2293 line number delete sensors::g_sensors; sensors::g_sensors = nullptr; warnx("start failed"); return 1; } ………

然后捏：start()函數(shù)

Int Sensors::start() { ASSERT(_sensors_task == -1); /* start the task */ /*創(chuàng)建任務(wù)進(jìn)程對sensors進(jìn)行初始化以后，在task_main()函數(shù)執(zhí)行的最后會調(diào)用px4_task_exit()函數(shù)退出該任務(wù)進(jìn)程*//* px4_task_spawn_cmd()創(chuàng)建任務(wù)的接口函數(shù)是posix接口的，具體實現(xiàn)參見源碼*/ _sensors_task = px4_task_spawn_cmd("sensors", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2000, (px4_main_t)&Sensors::task_main_trampoline, nullptr); /* wait until the task is up and running or has failed */ while (_sensors_task > 0 && _task_should_exit) { usleep(100); } if (_sensors_task < 0) { return -ERROR; } return OK; }

然后捏：task_main_trampoline()函數(shù):

Void Sensors::task_main_trampoline(int argc, char *argv[]) { sensors::g_sensors->task_main(); }

再然后捏：task_main()函數(shù)，大牌終于出場了

Void Sensors::task_main() { /* start individual sensors */ int ret = 0; do { /* create a scope to handle exit with break *//*do_while用的精妙*/ ret = accel_init();/* return 0 */ if (ret) { break; } ret = gyro_init(); if (ret) { break; } ret = mag_init(); if (ret) { break; } ret = baro_init(); if (ret) { break; } ret = adc_init(); if (ret) { break; } break; } while (0); ………//2059 /* * do subscriptions 這就是所謂的IPC使用的uORB模型（ publish_subscribe）吧 ，我也不懂 */ unsigned gcount_prev = _gyro_count; ……… _gyro_count = init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0], &raw.gyro_priority[0], &raw.gyro_errcount[0]); ……… /* get a set of initial values */ accel_poll(raw);//通過uORB模型獲取acc值 gyro_poll(raw); mag_poll(raw); baro_poll(raw); diff_pres_poll(raw); parameter_update_poll(true /* forced */); rc_parameter_map_poll(true /* forced */); ……… /* check vehicle status for changes to publication state */ vehicle_control_mode_poll(); /* the timestamp of the raw struct is updated by the gyro_poll() method */ /* copy most recent sensor data */ gyro_poll(raw); accel_poll(raw); mag_poll(raw); baro_poll(raw); ……… /* check battery voltage */ adc_poll(raw); diff_pres_poll(raw); /* Inform other processes that new data is available to copy */ if (_publishing && raw.timestamp > 0) { orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw); } /* keep adding sensors as long as we are not armed, * when not adding sensors poll for param updates */ if (!_armed && hrt_elapsed_time(&_last_config_update) > 500 * 1000) { _gyro_count = init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0], &raw.gyro_priority[0], &raw.gyro_errcount[0]); _mag_count = init_sensor_class(ORB_ID(sensor_mag), &_mag_sub[0], &raw.magnetometer_priority[0], &raw.magnetometer_errcount[0]); _accel_count = init_sensor_class(ORB_ID(sensor_accel), &_accel_sub[0], &raw.accelerometer_priority[0], &raw.accelerometer_errcount[0]); _baro_count = init_sensor_class(ORB_ID(sensor_baro), &_baro_sub[0], &raw.baro_priority[0], &raw.baro_errcount[0]); _last_config_update = hrt_absolute_time(); } else { /* check parameters for updates */ parameter_update_poll(); /* check rc parameter map for updates */ rc_parameter_map_poll(); } /* Look for new r/c input data */ rc_poll(); perf_end(_loop_perf); } warnx("exiting."); _sensors_task = -1; px4_task_exit(ret);/* px4_task_exit ()終止任務(wù)的接口函數(shù)也是posix接口的*/ } PS：關(guān)于IPC使用的 uORB的簡單介紹（摘自官網(wǎng)）。The micro Object Request Broker (uORB) application is used toshare data structures between threads and applications,Communications between processes / applications (e.g. sendingsensor values from the sensors app to the attitude filter app) is a key part ofthe PX4 software architecture. Processes (often called nodes in this context) exchange messagesover named buses, called topics.In PX4, a topic contains only one message type, e.g. the vehicle_attitude topic transports a message containing the attitude struct (roll, pitch and yaw estimates). Nodes can publish a message on a bus/topic (“send” data)or subscribe toa bus/topic (“receive” data). They are not aware of who they are communicatingwith. There can be multiple publishers and multiple subscribers to a topic.This design pattern prevents locking issues and is very common in robotics. Tomake this efficient, there is always only one message on the bus and no queue iskept.

詳細(xì)介紹：https://pixhawk.org/dev/shared_object_communication

4、如何獲取精確時間（timestamp）

在控制過程中多數(shù)環(huán)節(jié)都是使用經(jīng)典的PID控制算法，為了獲取較為實時的響應(yīng)最重要的時間變量，這就涉及如何獲取高精度的時間問題。

Pixhawk主控使用ST公司的STM32F4系列處理器，其主頻達(dá)168MHZ，內(nèi)部有高精度RTC，以RTC為基準(zhǔn)獲取精確計時。根據(jù)分析源碼發(fā)現(xiàn)，在modules/PX4Firmware/src/drivers中有一個頭文件drv_hrt.h（High-resolutiontimer with callouts and timekeeping），內(nèi)部對其作出了一部分的介紹，微妙（us）級的精確計時，在中斷上下文中調(diào)用，與其他函數(shù)并行執(zhí)行不會被堵塞。摘抄幾個典型函數(shù)如下。

/** Get absolute time.*/ __EXPORT extern hrt_abstime hrt_absolute_time(void); /** Compute the delta between a timestamp taken in the past and now. * This function is safe to use even if the timestamp is updated by an interrupt during execution. */ __EXPORT extern hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then); /** Store the absolute time in an interrupt-safe fashion. * This function ensures that the timestamp cannot be seen half-written by an interrupt handler.*/ __EXPORT extern hrt_abstime hrt_store_absolute_time(volatile hrt_abstime *now); /* initialise a hrt_call structure */ __EXPORT extern void hrt_call_init(struct hrt_call *entry); /* Initialise the HRT. */ __EXPORT extern void hrt_init(void);

針對上述hrt_absolute_time(void)函數(shù)做闡述，其他的都類似，它的原型在modules/PX4Firmware/unittests中的hrt.cpp。

hrt_abstime hrt_absolute_time() { struct timeval te; gettimeofday(&te, NULL); // get current time hrt_abstime us = static_cast<uint64_t>(te.tv_sec) * 1e6 + te.tv_usec; // caculate us return us; }

然后捏：直接進(jìn)入操作系統(tǒng)（NuttX），輪到操作系統(tǒng)上場了，modules/PX4NuttX/nuttx/schedclock_gettimeofday.c，自己跟蹤進(jìn)去看吧，不在贅述。

接下來可能是繼續(xù)研究ardupilot這套代碼，這套的話就是到上層應(yīng)用了，即關(guān)于loop函數(shù)和scheduler_task的問題了；或者轉(zhuǎn)變到pixhawk的原生代碼上，這套就是直接到控制算法上

總結(jié)

以上是生活随笔為你收集整理的Pixhawk代码分析-启动代码及入口函数的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。