一、開篇

        首先告訴大家一個壞訊息，DJI提供了SDK~~~~

        然後再來一個好訊息，本篇博文很多幹貨哦~~~~~

        最近比較糾結的一個問題ardupilot和pixhawk原生程式碼到底有什麼區別和聯絡。經過和群友的討論，最終方向一致認為單獨的pixhawk原生程式碼是可以正常飛行的（群友Mr一直在用這套試飛的），其上電以後在rcS（nsh的啟動指令碼，其儲存在ROMFS中，後面會詳細介紹）中開啟相應的程序對無人機進行姿態等複雜的控制，關於這個大家最先認識到的應該是各種飛航模式的確定，關於flight_mode後續再結合ardupilot介紹；ardupilot這套原始碼前一篇博文已經分析過了，而且前一篇博文裡面涉及的網站連結都是關於這個ardupilot的，很多人稱之為APM，但是這個APM的HAL層也可以使用pixhawk硬體，此APM非彼apm，官網有介紹。Ardupilot是基於pixhawk原生程式碼開發的上層應用，即有了常見的mian和loop函式，正如前一篇博文中給出的一幅圖，ardupilot把底層的PX4Firmware和NuttX等都抽象為HAL，developer只需要關心上層應用即可，基於pixhawk原生設計上層控制邏輯。這個非常類似於CC26xx的stack和application部分，而TI關於這個做的更好。ardupilot其實是開源的、玩具級飛控，它為了相容各種亂七八糟的板子、機型，有很多冗餘的程式碼，這些冗餘的程式碼讓我們理解飛控本身增加了難度和學習成本。如果我們需要一款專業的飛控，完全可以去掉這些冗餘的程式碼，只保留我們需要的精簡過的功能，這樣也就能大大降低它程式碼的複雜程度。從程式設計的方法論來看（面向物件）：就是要把一個個物件確立好，把問題的複雜程度控制住，才能設計出邏輯清晰的程式出來。但是我的博文寫的沒什麼邏輯，水平有限，大家湊合看吧。

        也就是說，假如你打算開一家無人機公司，不需要從最底層一行一行的程式碼寫起，可以直接把這套開源的pixhawk原生程式碼搞透徹（這套可以不是一己之力可以完成的，這個開源專案N人N年了），然後基於它進行上層開發，加入自己NB的演算法，聽說進DJI的第一年就是給你一個pixhawk，玩一年。非常懷疑DJI現在是不是也是基於pixhawk（雖然DJI成立2006，ETH Zurichpixhawk專案成立2009年），有待考證或者有在DJI工作的給大家普及一下唄。

        好吧，用哪套原始碼分析都行，我用的ardupilot，最後還是需要到底層的PX4Firmware。感謝群友彩虹小羊、灰灰、Mr等提供的幫助（排名按字母順序，哈哈）。

三、實驗平臺

Software Version：ArduCopter（Ver_3.3）

Hardware Version：pixhawk

IDE：eclipse Juno （Windows）

四、基本知識介紹

1、關於座標系

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 ZY’X” (see thefigure):
rotation of  around Z (yaw)
rotation of  around Y' (pitch)
rotation of  around X” (roll) 

2、pixhawk的HAL

3、pixhawk的控制圖

五、正文

1、關於兩個控制器和任務優先順序

        在PX4Firmware/src/modules中的mc_att_control：姿態控制器和mc_pos_control位置控制器（mc：multicopter），整個系統都是圍繞著這兩個控制器。

        mc_att_control – Multirotor attitude controller

        mc_pos_control – Multirotor position controller

        The PX4 firmware is organized in priority bands:

1) (interrupt level) fast sensordrivers

2) watchdog/system state monitors

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

4) attitude controller(s)

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

6) destination/positioncontroller(s)

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

8) logger, parameter syncer

9) idle

        瞭解整個程式碼執行過程的最簡便方法就是通過其makefile瞭解檔案的包含關係和呼叫關係。請仔細閱讀arducopter中的.mk和modules/PX4Firmware中的.mk檔案。.mk檔案就是在Linux下編寫的一些指令碼檔案（makefile），類似於編譯過程中的條件編譯，便於直接使用命令列進行編譯連結。

# 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的資料夾，指定編譯的規則，編譯順序：

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編譯整個工程，它是最終的編譯過程的執行者，注意其中px4-v2規則的詳細執行情況，它會去呼叫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，請詳細瞭解該部分的實現*/
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檔案，直接下載到飛控板中即可實現飛行控制*/
………
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 [email protected]:/tmp/
	ssh [email protected] PYTHONUNBUFFERED=1 loadPixhawk.py /tmp/ArduCopter-v2.px4
	ssh [email protected] 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。如果仔細瞭解了上面所講述的，那麼這部分就很easy了，不再細講，自己去看程式碼吧。需要說明的是在PX4Firmware的makefile檔案中有一個README.txt文字，該文字詳細的介紹了PX4Firmware中的.mk的呼叫關係和作用。補充一點，PX4Firmware/makefile/nuttx中的config_px4fmu-v2_default.mk包含了所有基本的模組，從這裡再一次驗證了開篇講述的ardupilot的程式碼是基於pixhawk原生程式碼PX4Firmware“閹割”而來的。所以在後期做開發的過程中編寫應用程式只是按照ardupilot現有的子模組新增自己的就可以，參考mc_att_control實現。

        在PX4Firmware/src/modules中新增一個新的資料夾，命名為summer

        在summer資料夾中建立module.mk檔案，並輸入以下內容：

MODULE_COMMAND = summer
SRCS = summer.cpp

        在summer資料夾中建立summer.c檔案，編譯需要實現的功能程式碼。

        註冊新新增的應用到NuttShell中：PX4Firmware/makefiles/nuttx/config_px4fmu-v2_default.mk檔案中新增如下內容：

MODULES += modules/summer

        任務的啟動就是靠NuttShell的內建命令啟動的，包含rcS，特別是rcS一定要了解，下面會詳細介紹。

        在ardupilot/Arducopter中有一個make.inc檔案，指定了編譯ArduCopter時需要編譯哪些依賴庫，它對系統及硬體進行了抽象，以獲取更好的可移植性。

        如果想新增或者刪除編譯的檔案，就在此處進行修改。如下是官方預設的。

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：有兩個，比較重要，任務分工）

        所謂啟動指令碼其實是用shellscript寫的啟動檔案以前寫過比較簡單的指令碼檔案，還是參考的鳥哥的那本書，還有就是Linux啟動檔案也是用的這個寫的，有興趣的可以去Linux平臺下檢視一下，會寫C的學這個很快，該死的C++什麼時候能學會啊。

        1）其中之一就是負責較為底層driver裡面的（比如mpu9250的register map、驅動），在使用命令px4-v2-upload以後，會在modules/PX4Firmware目錄下生成build資料夾，內部是編譯生成的檔案（不編譯沒有），在modules/PX4Firmware/build/px4fmu-v2_APM.build/romfs_scratch/init.d檔案下就是rcS和rc.APM，這個rcS是自啟動指令碼（setMODE autostart）。rcS指令碼會掛載SD卡（這就是沒有SD飛控板不能使用的原因所在），然後跳轉rc.APM中，該指令碼會在pixhawk系統中建立binfs，判斷硬體型別，然後啟動orb（對，就是那個uORB），然後啟動各種感測器，然後咔咔咔，咔咔咔，完了，自己去看指令碼吧。摘一部分鑑賞，關於shell指令碼的編寫自己百度吧，跟C語言的風格很像。

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

        上面標紅的MODULE_COMMAND = mpu9250就是為了以後可以直接在.mk檔案直接使用的命令mpu9250。理解了上述以後，其他的driver中的所有的感測器模組、各種匯流排和modules/PX4Firmware/src/modules中的各種演算法（mc_att_control、land_detector等等）都是按照此類方法實現的，不在贅述，但是這個rcS只是負責啟動driver中的，modules/PX4Firmware/src/modules需要靠下面的一個rcS啟動，兩個rcS各自分工。全部都是.mk啊，.mk，.mk~~~~

下一階段主要應該就是放在這個modules/PX4Firmware/src/modules內部了，整個控制響應過程全靠它了，尤其是commander，校磁什麼的都在裡面，各種calibration，見過2000+行的函式麼？！怕麼？ ~~~   168MHZ的主頻呢，怕啥

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

px4fmu_spi.c 會在入口函式__start()中被呼叫初始化SPI，下面會提到。

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

        board_config.h：如下的程式碼是不是很熟悉，用過微控制器的肯定都見過，尤其是pixhawk使用的又是STM32，再加上ST公司的晶片在各個高校中的普及程度，雖然我沒用過，我是菜鳥。

        2）另外一個rcS就是modules/PX4Firmware/ROMFS/px4fmu_common/init.d中的rcS，PX4FMU的自啟動檔案，它主要是負責啟動modules/PX4Firmware/modules裡面的關於各自控制演算法的模組。同時，該rcS會以指令碼的形式啟動同一級目錄中的rc.mc_apps、rc_io等等。詳細實現過程參見原始碼。

        舉一例說明問題，rc.mc_apps：關於姿態估計、位置估計和姿態控制和位置控制的，原始碼如下。

#!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  //姿態估計
	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 //姿態控制
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 //啟動落地檢測

應該能瞭解程式碼中的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中還包含了關於stm32的各種Peripherals的標頭檔案，即各種外設（spi、can、uart、i2c等等）的驅動。

        在ardupilot/modules/PX4NuttX/nuttx/arch/arm/src/stm32檔案下有標頭檔案chip.h，內部通過條件編譯執行進行主控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、入口函式（__start()）（這個就更不知道了吧）

        在ardupilot/modules/PX4NuttX/nuttx/arch/arm/src/stm32檔案下有定義檔案stm32_start.c，內部有個上電/重啟的函式入口__start(void)。仔細看，一行行的看，別偷懶，程式碼附加列出了幾個小問題，檢查自己一下都會麼？！！！

/****************************************************************************
 * 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) : ); /*限制棧大小，可以防止遞迴把棧記憶體浪費完了，知道原因麼？！*/
#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(;;);
}

        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.

        飛控板是幹嘛的？

        控制無人機飛行滴（逼格高點就是通過各種感測器檢測無人機的實時姿態資訊和位置資訊並結合MCU輸出控制資訊）。

        那sensor使用前要幹嘛呢？

        ！#￥@￥！@%￥！#%

        初始化，初始化，初始化~~~~

        上面那麼多感測器必須得初始化以後才能使用啊，所以需要先初始化用到的所有的感測器，在哪配置呢，你找到沒?! 在ardupilot/modules/PX4Firmware/src/modules/sensors檔案中有定義檔案sensor.c（關於飛控板中的各個sensor的初始化函式，以task的方式啟動，初始化完成以後kill掉這個task，都是POSIX介面的API）。

        首先是sensors_main函式

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;
}
………

Int Sensors::start()
{
ASSERT(_sensors_task == -1);
/* start the task */
/*建立任務程序對sensors進行初始化以後，在task_main()函式執行的最後會呼叫px4_task_exit()函式退出該任務程序*//* px4_task_spawn_cmd()建立任務的介面函式是posix介面的，具體實現參見原始碼*/
_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()函式

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

        再然後捏：task_main()函式，大牌終於出場了

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 ()終止任務的介面函式也是posix介面的*/
}

PS：關於IPC使用的 uORB的簡單介紹（摘自官網）。

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_attitudetopic transports a message containing the attitudestruct (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.

        在控制過程中多數環節都是使用經典的PID控制演算法，為了獲取較為實時的響應最重要的時間變數，這就涉及如何獲取高精度的時間問題。

        Pixhawk主控使用ST公司的STM32F4系列處理器，其主頻達168MHZ，內部有高精度RTC，以RTC為基準獲取精確計時。根據分析原始碼發現，在modules/PX4Firmware/src/drivers中有一個頭檔案drv_hrt.h（High-resolutiontimer with callouts and timekeeping），內部對其作出了一部分的介紹，微妙（us）級的精確計時，在中斷上下文中呼叫，與其他函式並行執行不會被堵塞。摘抄幾個典型函式如下。

/** 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)函式做闡述，其他的都類似，它的原型在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;
}

        然後捏：直接進入作業系統（NuttX），輪到作業系統上場了，modules/PX4NuttX/nuttx/schedclock_gettimeofday.c，自己跟蹤進去看吧，不在贅述。

        一句話兩句話也寫清楚，那就不寫了吧~~~其實也不知道寫啥了。

        反正就是這套程式碼不簡單，不簡單。

        接下來可能是繼續研究ardupilot這套程式碼，這套的話就是到上層應用了，即關於loop函式和scheduler_task的問題了；或者轉變到pixhawk的原生程式碼上，這套就是直接到控制演算法上，commander。。。。。