姿態解算篇A

基本知識

1、如何實現控制

一個無人機系統的算法主要有兩類：姿態檢測算法、姿態控制算法。姿態控制、被控對象、姿態檢測三個部分構成一個閉環控制系統。被控對象的模型是由其物理系統決定，設計無人機的算法就是設計姿態控制算法、姿態檢測算法。

1）姿態檢測算法：姿態的表示可以用歐拉角，也可以用四元數。姿態檢測算法的作用就是將加速度計、陀螺儀等傳感器的測量值解算成姿態，進而作為系統的反饋量。在獲取sensors值之前需要對數據進行濾波，濾波算法主要是將獲取到的陀螺儀和加速度計的數據進行去噪聲及融合，得出正確的角度數據（歐拉角或四元數），主要采用互補濾波或者高大上的卡爾曼濾波。

2）姿態控制算法：控制無人機姿態的三個自由度，用給定姿態與姿態檢測算法得出的姿態偏差作為輸入，被控對象模型的輸入量作為輸出（例如姿態增量），從而達到控制無人機姿態的作用。最常用的就是PID控制及其各種PID擴展（分段、模糊等，我的畢設就是模糊PID控制算法，慘了，啥都不懂，還能畢業不，哎~~~），高端點的有自適應控制（自動壁障應該就用這個）。

當然，姿態控制算法里面比較常用角速度、角度雙閉環控制（所謂的兩級PID控制），所以常常有PID外環+PID內環等等，懂我搞懂了再細說吧。PID算法就是控制四個電機的轉速來糾正歐拉角，從而使機身保持平穩。

2、關于編譯環境的搭建

ardupilot的編譯環境搭建比較簡單，直接github下載或者clone到PC上就可以了。PX4Firmware的開發環境的搭建有點困難，結合幾人之力中搞定了，現在把大致過程寫下來以便幫助更多的人。

下面主要是講述關于PX4Firmware開發平臺的搭建，即所謂的pixhawk原生固件的開發環境。直接使用github下載PX4Firmware源碼到PC上，在toolchain中的console控制臺中使用make命令編譯即可，在使用命令是需要在Firmware的目錄下面才行，不然會出現無效命令的錯誤提示。編譯過程相當復雜耗時，所以慢慢的等吧，如果中途出現編譯到某處以后等待了10分鐘之久還是沒有往下運行，那么關掉控制臺重啟，重新make即可。

在eclipse中編譯有點難度，不僅需要安裝Cmake；還需要在別的地方配置一二，首先是修改Firmware中的兩個文件名，即template_.cproject和template_.project修改為.cproject.和.project.（注意名稱前后各一個點“.”），然后就是修改eclipse的環境變量，修改成如下。原本默認的是“E:\PX4\Firmware”，現修改為“E:/PX4/Firmware”,就是斜杠的問題。在eclipse下很容易出現問題，所以建議還是在控制臺編譯吧，而且有很炫的顏色。

如果是拷貝別人的源碼，可能會出現無法編譯的情況，原因不是文件丟失的問題，而是編譯一次以后eclipse會默認配置好一些路徑，所以，在拷貝的時候按照原本的源文件的目錄重新建一個一個的目錄就可以了，比如本來是E/PX4目錄，那么就在你的電腦中也建一個同樣的目錄，把拷貝的東西放進去就行了。

PS：普遍遇到的一個問題就是每次編譯都會git submodule update，主要就是因為在使用console控制臺命令行編譯時的一個執行過程就是Tools中的check_submodules.sh，很明顯是shell腳本，這個腳本首先檢測源碼路徑中是否有“.git”，沒有的話將無法實現git，然后檢查.sh中指定的submodule，沒有的這些submodule的話需要聯網git，有的話直接跳過，顯示Checked xxxx currect version found 。不聯網時可以通過不執行/修改這個shell腳本略過檢查更新固件，下面會詳細介紹這個。

Tools里面有很多個.sh腳本，也就是一些關于配置的，比如make_color.sh，自己去配置為喜歡的顏色吧，關于控制臺顏色的問題詳見：http://blog.chinaunix.net/uid-598887-id-4019034.html。

姿態解算分析

1、ardupilot到PX4Firmware的變化

首先說明一下，這兩套代碼我是結合著看的，反正ardupilot的底層也是直接調用的PX4Firmware，博文還是以ardupilot為主線展開。介紹一下ardupilot到PX4Firmware的變化吧，其實變化的不多，主要就是由原本的make轉變到了現在的Cmake，原來在ardupilot里面處處可以見到.mk，現在是徹底的沒有了，全部都是CmakeList.txt和.cmake了，它倆就是由Cmake寫的makefile，一看就知道里面是什么意思，不做解釋了。

另外一個比較重要的變化就是關于各種庫的選擇編譯的配置，搬移到了PX4-Firmware/cmake/config中，即nuttx_px4fmu-v2_default.cmake，可以自己修改這個cmake文件增加或刪除某個庫。該部分會在console控制臺中使用命令make編譯時調用，在PX4-Firmware/Makefile中有如下代碼：

px4fmu-v2_default: $(call cmake-build,nuttx_px4fmu-v2_default) cmake-build是宏定義，nuttx_px4fmu-v2_default作為參數傳入。 # Functions # -------------------------------------------------------------------- # describe how to build a cmake config define cmake-build +@if [ $(PX4_CMAKE_GENERATOR) = "Ninja" ] && [ -e $(PWD)/build_$@/Makefile ]; then rm -rf $(PWD)/build_$@; fi +@if [ ! -e $(PWD)/build_$@/CMakeCache.txt ]; then mkdir -p $(PWD)/build_$@ && cd $(PWD)/build_$@ && cmake .. -G$(PX4_CMAKE_GENERATOR) -DCONFIG=$(1); fi +Tools/check_submodules.sh/*該腳本檢查是否需要更新固件源碼，如不需可以直接屏蔽*/ +$(PX4_MAKE) -C $(PWD)/build_$@ $(PX4_MAKE_ARGS) $(ARGS) Endef 其中：PX4_MAKE = make PX4_MAKE_ARGS = -j$(j) --no-print-directory

解析以后的命令就是如下圖所示。

簡要介紹一下Cmake：其實也沒什么難的，我們不需要寫多高端的，只是會用它即可，幾乎每個文件里面都有.cmake，在PX4-Firmware/cmake/中的common或者nuttx中是一些基本的cmake函數（類似于C/C++語言的的庫一樣）以供在別處編寫makefile時使用（比如在PX4-Firmware/src/modules/commander的CMakeLists.txt）。

舉一例說明問題，在PX4-Firmware/cmake/common中的px4_base.cmake，該文件的最前端部分會介紹本file內部有哪些function以及介紹每個function的用法，使用時就按照這里面的example仿寫就行。在CMakeLists.txt中不需要使用include（xxx.cmake）就可以使用需要的function，但是在xxx.cmake中使用時需要include(common/px4_base) 以后才可以使用（和C/C++類似）。下面是Firmware/src/modules/commander的CMakeLists.txt代碼。

set(MODULE_CFLAGS -Os) if(${OS} STREQUAL "nuttx") /*判斷OS類型*/ list(APPEND MODULE_CFLAGS -Wframe-larger-than=2450) endif() px4_add_module( /*在px4_base.cmake中定義，里面會介紹用法*/ MODULE modules__commander /*標明路徑*/ MAIN commander /* 類似于.mk中的MODULE_COMMAND = commander*/ STACK 4096 COMPILE_FLAGS ${MODULE_CFLAGS} -Os SRCS /*source files*/ commander.cpp state_machine_helper.cpp commander_helper.cpp calibration_routines.cpp accelerometer_calibration.cpp gyro_calibration.cpp mag_calibration.cpp baro_calibration.cpp rc_calibration.cpp airspeed_calibration.cpp esc_calibration.cpp PreflightCheck.cpp DEPENDS platforms__common )

2、在分析代碼之前首先需要了解一下arducopter/copter.h文件，內部以class類的形式定義了幾乎所有的使用的函數。平時索引函數時可以先到該文件中查找一下。

3、程序的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函數。那么這里可能又牽扯到了一個問題，ArduPilot_main函數又是怎么調用的呢？如果像以前我們經常使用的單片機裸機系統，入口函數就是程序中函數名為main的函數，但是這個工程里邊名字不叫main，而是ArduPilot_main，所以這個也不像裸機系統那樣去運行ArduPilot_main那么簡單。區別在于這是跑的Nuttx操作系統，這是一個類Unix的操作系統，它的初始化過程是由腳本去完成的。注意一個重要的詞——腳本，如果你對Nuttx的啟動過程不是很熟悉，可以查看我先前寫的一些文章。而在這里需要注意兩個腳本，一個是ardupilot/mk/PX4/ROMFS/init.d里的rcS，另一個是rc.APM，這個腳本在rcS里得到了調用，也就是說，rcS就是為Nuttx的啟動文件。

那么到底調用ArduPilot_main的地方在哪里呢？查看rc.APM的最低端：

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是一個內嵌的應用程序，由編譯生成的builtin_commands.c可知，這個應用程序的入口地址就是ArduPilot_main。

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

這樣的命令有很多，在rcS里就開始調用的。至于這些內置的命令是怎么生成的，就要了解PX4Firmware的編譯過程了。查看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)

4、arducopter.cpp

分析ardupilot這套代碼首先就是拿arducopter.cpp開刀，

Loop函數的設計框架既要準確，又要高效。總體設計是這樣的：

其一用一個計時器定時觸發測量；其二所有測量過程都靠中斷推進；其三在main函數里不斷檢查測量是否完成，完成就進行解算。測量過程還是比較耗時間的，基于這樣的設計，測量和解算可以同時進行，不會浪費CPU時間在（等待）測量上。而通過計時器觸發測量，最大限度保證積分間隔的準確。

整個控制過程主要就集中在arducopter.cpp里面，首先就是scheduler_tasks[]中是需要創建的task線程，如下圖中，接收機的rc_loop、arm_motors_check等，還記得上篇博文中介紹的解鎖和上鎖的操作么，就是在arm_motors_check函數中實現的，以固定的頻率去采集遙控器信號。

1）setup函數

然后就是setup函數，在該函數中做的事情還是比較全面的，其中內部調用了一個比較重要的函數init_ardupilot()，該函數做了一系列的初始化，該初始化和上一篇博文介紹的不一樣，上一篇主要是配置底層硬件的（如STM32、sensors），而此處的主要是飛行前的檢測比那個初始化的晚；比如檢測是否有USB連接、初始化UART、打印固件版本、初始化電源檢測、初始化RSSI、注冊mavlink服務、初始化log、初始化rc_in/out（內部含有電調校準）、初始化姿態/位置控制器、初始化飛行模式、初始化aux_switch、使能失控保護、配置dataflash最后打印"Ready to FLY "。接下來就幾個比較重要的函數（上述標紅）進行深入分析。

void Copter::init_rc_in() { //rc_map 結合AC_RCMapper.h channel_roll = RC_Channel::rc_channel(rcmap.roll()-1); channel_pitch = RC_Channel::rc_channel(rcmap.pitch()-1); channel_throttle = RC_Channel::rc_channel(rcmap.throttle()-1); channel_yaw = RC_Channel::rc_channel(rcmap.yaw()-1); // set rc channel ranges channel_roll->set_angle(ROLL_PITCH_INPUT_MAX);//4500 channel_pitch->set_angle(ROLL_PITCH_INPUT_MAX); channel_yaw->set_angle(4500); channel_throttle->set_range(g.throttle_min, THR_MAX);//1000 channel_roll->set_type(RC_CHANNEL_TYPE_ANGLE_RAW); channel_pitch->set_type(RC_CHANNEL_TYPE_ANGLE_RAW); channel_yaw->set_type(RC_CHANNEL_TYPE_ANGLE_RAW); //set auxiliary servo ranges g.rc_5.set_range_in(0,1000); g.rc_6.set_range_in(0,1000); g.rc_7.set_range_in(0,1000); g.rc_8.set_range_in(0,1000); // set default dead zones default_dead_zones(); // initialise throttle_zero flag ap.throttle_zero = true;//注意這個，rc_map好以后把油門配置為0 // true if the throttle stick is at zero, debounced, determines if pilot intends shut-down when not using motor interlock } // init_rc_out -- initialise motors and check if pilot wants to perform ESC calibration void Copter::init_rc_out() { motors.set_update_rate(g.rc_speed); motors.set_frame_orientation(g.frame_orientation);//配置機體類型（+/x） motors.Init(); // motor initialisation #if FRAME_CONFIG != HELI_FRAME motors.set_throttle_range(g.throttle_min, channel_throttle->radio_min, channel_throttle->radio_max);//配置油門最大值和最小值 motors.set_hover_throttle(g.throttle_mid); #endif for(uint8_t i = 0; i < 5; i++) { delay(20); read_radio(); } // we want the input to be scaled correctly channel_throttle->set_range_out(0,1000); // check if we should enter esc calibration mode esc_calibration_startup_check();//電調校準，進入以后首先判斷是否有pre-arm，如果沒有則直接退出校準。校準過飛機的都知道這個 // enable output to motors pre_arm_rc_checks(); if (ap.pre_arm_rc_check) { enable_motor_output();//內部會調用motors.output_min()函數sends minimum values out to the motors，待會介紹該函數 } // refresh auxiliary channel to function map RC_Channel_aux::update_aux_servo_function(); // setup correct scaling for ESCs like the UAVCAN PX4ESC which // take a proportion of speed. hal.rcout->set_esc_scaling(channel_throttle->radio_min, channel_throttle->radio_max); }

簡單介紹如何如何控制電機轉動以及cork() and push()，并在此基礎上測試了關于scheduler_tasks[] 中的任務的執行頻率是否可以達到要求。測試方法：在scheduler_tasks[] 任務列表的throttle_task中添加一個累加標志位counterflag，每執行一次throttle_task任務就對齊加1，加到100時使電機轉動，測試結果約為5S時電機轉動，理論是50HZ的周期（不加執行時間是需要2S轉動）再加上每次需要的執行時間以后還是比較理想的。

// output_min - sends minimum values out to the motors void AP_MotorsMatrix::output_min() { int8_t i; // set limits flags limit.roll_pitch = true; limit.yaw = true; limit.throttle_lower = true; limit.throttle_upper = false; // fill the motor_out[] array for HIL use and send minimum value to each motor hal.rcout->cork(); /*Delay subsequent calls to write() going to the underlying hardware in order to group related writes together. When all the needed writes are done, call push() to commit the changes. This method is optional: if the subclass doesn't implement it all calls to write() are synchronous.*/ for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) { // AP_MOTORS_MAX_NUM_MOTORS=8 if( motor_enabled[i] ) { rc_write(i, _throttle_radio_min); //下面會解析rc_write(uint8_t chan, uint16_t pwm) } } hal.rcout->push(); /*Push pending changes to the underlying hardware. All changes between a call to cork() and push() are pushed together in a single transaction.This method is optional: if the subclass doesn't implement it all calls to write() are synchronous.*/ } //由上述可知在通過HAL層配置數據時使用cork() and push()函數包裝需要單次傳輸的數據。該方法就是為了實現對四個電機同時配置，避免由for語句產生的延時，也是強調同步（synchronous）。 如下解析rc_write(uint8_t chan, uint16_t pwm) void AP_Motors::rc_write(uint8_t chan, uint16_t pwm) { if (_motor_map_mask & (1U<<chan)) { // we have a mapped motor number for this channel chan = _motor_map[chan];// mapping to output channels } hal.rcout->write(chan, pwm);//通過HAL層直接輸出配置電調 }

Setup()函數的最后一句是fast_loopTimer = AP_HAL::micros()，獲取micros()通過層層封裝最終就是上篇博文中介紹的hrt，該時間會在下面的loop函數中使用。
2）loop函數

該函數比較重要，fast_loop和schedule_task都是封裝在該函數中的，下面主要講述fast_loop。

// Main loop - 400hz void Copter::fast_loop() { // IMU DCM Algorithm 里面有個update函數，這個函數就是讀取解算好的角度信息 read_AHRS(); // run low level rate controllers that only require IMU data attitude_control.rate_controller_run(); // 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(); // log sensor health if (should_log(MASK_LOG_ANY)) { Log_Sensor_Health(); } }

read_AHRS()內部使用ahrs.update更新AHRS數據。

// run a full DCM update round Void AP_AHRS_DCM::update(void) { // tell the IMU to grab some data _ins.update();//update gyro and accel values from backends具體實現過程詳見源碼 // ask the IMU how much time this sensor reading represents delta_t = _ins.get_delta_time(); // Integrate the DCM matrix using gyro inputs //使用陀螺儀數據更新DCM矩陣（方向余弦矩陣：direction-cosine-matrix ），使用剛剛測量出來的陀螺儀值、以及上個周期對陀螺儀的補償值進行角度更新 matrix_update(delta_t); // Normalize the DCM matrix 歸一化處理 normalize(); // Perform drift correction drift_correction(delta_t); // paranoid check for bad values in the DCM matrix check_matrix(); // Calculate pitch, roll, yaw for stabilization and navigation euler_angles(); // update trig values including _cos_roll, cos_pitch update_trig(); }

到此為止，卡住了。不是因為不懂C++的緣故，而是因為理 論 知 識 太 欠 缺 了~~~~
所以還是好好的準備理論知識吧，大把大把的論文要看。我是看Mahony的和Paul的，現在主要是Paul的《Direction Cosine Matrix IMU: Theory》，講的實在是太好了，搞無人機必看啊，這篇文章的最后給出了Mahony論文的下載地址。

高大上的理論

關于上面的這個Normalize the DCM matrix（ 歸一化處理）很有深度，值得了解一下其原理，在使用DCM控制和導航時，肯定存在積累數值的舍入誤差、陀螺漂移誤差、偏移誤差、增益誤差。它主要就是補償抵消這幾種誤差（注意這幾種誤差 error）的；使用PI負反饋控制器檢測這些誤差，并在下一次產生之前就抵消這些誤差（GPS檢測yaw error，加速度計檢測pitch和roll error）。在ardupilot源碼中使用的Normalize算法就是來自2009年Paul的研究成果--Direction Cosine Matrix IMU: Theory。首先了解一下幾個比較關鍵的概念。用下圖先有個感性認識吧

1、陀螺漂移（Gyro drift）

由于外干擾力矩（機械摩擦、振動等因素）引起的陀螺自轉軸的緩慢進動：陀螺漂移。通常，干擾力矩分為兩類，與之對應的陀螺漂移也分為兩類：一類干擾力矩是系統性的，規律已知，它引起規律性漂移，因而是可以通過計算機加以補償的；另一類是隨機因素造成的，它引起隨機漂移。在實際應用中，除了要盡可能減小隨機因素的影響外，對實驗結果還要進行統計處理，以期對隨機漂移作出標定，并通過系統來進行補償。但由于它是無規律的，很難達到。平時所說的用加速計（靜態特性好）修正陀螺儀的漂移，其實這個修正是利用加速度計修正陀螺儀計（動態特性好）算出的姿態，并估計出陀螺儀的漂移，在下一次做姿態解算時，陀螺儀的輸出減去估計出的漂移后再做捷聯姿態解算，以此不斷循環。融合的方法一般用EKF，KF也是基于最優估計的。

2、誤差來源

在進行數值積分的過程中一定會引入數值誤差，數值誤差分為兩類；一類是積分誤差（來自于我們假設旋轉速率在短時間內不變引起的），另一類是量化誤差（主要來自于模數轉換過程中引起的）。關于gyro誤差產生的詳細分析見：http://www.crazepony.com/wiki/mpu6050.html

3、旋轉矩陣的約束

旋轉矩陣就是改變方向不改變大小；旋轉矩陣有9個元素，實際上是只有三個是獨立的。由于旋轉矩陣的正交性，意味著任何一對行或者列都是相互垂直的，并且每個行或者列的元素的平方和為1。所以在9個元素中有6個限制條件。

4、誤差導致的結果

旋轉矩陣的關鍵性能之一就是正交性，即在某個參考坐標系下的三個方向的向量兩兩垂直，且向量長度在每個參考系下都是等長的。數值誤差會違背該性能，比如旋轉矩陣的行和列都是單位向量，其長度都是1，但是由于數值誤差的原因導致其長度邊長或變短；最終致使它們縮小到0或者增長到無窮大，最后的結果就是導致原本相互正交的現在變的傾斜了。如下圖所示。

那么問題來了，如何修正這個問題呢？可以使用如下方法（ Ardupilot源碼中就是利用如下算法處理的errors）。

5、如何消除各種誤差（積累數值的舍入誤差、陀螺漂移誤差、偏移誤差、增益誤差）

其方法就是采樣DCMIMU:Theory中提出的理論—強制正交化（也稱為Renormalization）。首先計算矩陣中X、Y行的點積（dotproduct），理論上應該是0，但是由于各種errors的影響，所以點積的值不是0，而表示為error。

然后把這個誤差項分半交叉耦合到X、Y行上，如下公式。

通過上述兩個公式處理過以后，正交誤差明顯減小了很多，即R旋轉矩陣的每個行和列的長度都非常接近1。接下來就是調整旋轉矩陣的Z行，使其與X、Y正交，方法比較簡單，直接使用X、Y的叉積（cross product）即可。

最后一步就是放縮旋轉矩陣的每一行使其長度等于1，現在用一種比較簡單的方法實現，即使用泰勒展開。

ardupilot中的源碼實現如下：

Void AP_AHRS_DCM::normalize(void) { float error; Vector3f t0, t1, t2; error = _dcm_matrix.a * _dcm_matrix.b; // eq.18 t0 = _dcm_matrix.a - (_dcm_matrix.b * (0.5f * error)); // eq.19 t1 = _dcm_matrix.b - (_dcm_matrix.a * (0.5f * error)); //eq.19 t2 = t0 % t1; // c= a x b // eq.20 if (!renorm(t0, _dcm_matrix.a) || !renorm(t1, _dcm_matrix.b) || !renorm(t2, _dcm_matrix.c)) { // Our solution is blowing up and we will force back // to last euler angles _last_failure_ms = AP_HAL::millis(); AP_AHRS_DCM::reset(true); } }

結論

本篇博文沒有介紹多少關于源代碼的，主要就時覺得理論太欠缺了，接下來還有四元數，控制中主要還是用它做運算，還有各種濾波。接下來就是主要看關于姿態估計的了，姿態估計算法實現主要就是在PX4Firmware/src/modules/attitude_estimator_q中（q：quaternion），首先介紹一些代碼執行順序，方便后期有個良好的邏輯框架閱讀代碼和習慣。

1） 首先就是該文件中需要的頭文件的包含；
2） 然后是一個C++的引用定義extern"C" __EXPORT int attitude_estimator_q_main(int argc, char *argv[])，可以根據這個attitude_estimator_q_main進行追蹤到函數原型（754）；
3） 在attitude_estimator_q_main函數中調用姿態估計的啟動函數start()（775）；
4） 詳細介紹一下該啟動函數，比較重要，源碼中很多都是靠這種方法啟動的，還記上次講的sensor初始化么。源碼如下。

int AttitudeEstimatorQ::start() { ASSERT(_control_task == -1); /* start the task *//*POSIX接口的任務啟動函數*/ _control_task = px4_task_spawn_cmd("attitude_estimator_q", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2100, (px4_main_t)&AttitudeEstimatorQ::task_main_trampoline, nullptr); if (_control_task < 0) { warn("task start failed"); return -errno; } return OK; }

5） 然后是task_main_trampoline函數，內部跳轉到task_main()

好了，就是它了，慢慢研究吧，把這個里面的過程都研究透吧

總結

以上是生活随笔為你收集整理的Pixhawk代码分析-姿态解算篇A的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。