一、開篇
慢慢的、慢慢的、慢慢的就快要到飛控的主要部分了，飛控飛控就是所謂的飛行控制唄，一個是姿態解算一個是姿態控制，解算是解算，控制是控制，各自負責各自的任務。我也不懂。還在學習中~~~~
近期看姿態預計部分看的太累了，明顯發現基礎知識太薄弱，什么歐拉角、DCM、四元數、gyro誤差、矯正、正交化等各個概念。然后就是各種轉換公式。接下來結合代碼介紹一些主要的東西。太深入的還不了解~~~
一定要多看論文啊，英文版的論文（也沒有中文的。國人的悲哀啊）。盡管看着頭疼，看是看完了以后就會發現很多不了解的迷惑的就自然都解開了。
三、實驗平台
Software Version：ArduCopter（Ver_3.3）
Hardware Version：pixhawk
IDE：eclipse Juno （Windows）
四、基本知識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下非常easy出現故障，所以建議還是在控制台編譯吧，並且有非常炫的顏色。
假設是拷貝別人的源代碼，可能會出現無法編譯的情況，原因不是文件丟失的問題，而是編譯一次以后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
解析以后的命令就是例如以下圖所看到的。
舉一例說明問題，在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;
}
因此實際上這個project的main函數就是ArduCopter.cpp里的ArduPilot_main函數。那么這里可能又牽扯到了一個問題，ArduPilot_main函數又是怎么調用的呢？假設像曾經我們常常使用的單片機裸機系統，入口函數就是程序中函數名為main的函數，可是這個project里邊名字不叫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。首先了解一下幾個比較關鍵的概念。用下圖先有個感性認識吧
因為外干擾力矩（機械摩擦、振動等因素）引起的陀螺自轉軸的緩慢進動：陀螺漂移。通常，干擾力矩分為兩類，與之相應的陀螺漂移也分為兩類：一類干擾力矩是系統性的，規律已知。它引起規律性漂移，因而是能夠通過計算機加以補償的。還有一類是隨機因素造成的，它引起隨機漂移。
在實際應用中。除了要盡可能減小隨機因素的影響外。對實驗結果還要進行統計處理，以期對隨機漂移作出標定，並通過系統來進行補償。但因為它是無規律的，非常難達到。
平時所說的用加速計（靜態特性好）修正陀螺儀的漂移。事實上這個修正是利用加速度計修正陀螺儀計（動態特性好）算出的姿態。並預計出陀螺儀的漂移，在下一次做姿態解算時，陀螺儀的輸出減去預計出的漂移后再做捷聯姿態解算，以此不斷循環。
融合的方法一般用EKF。KF也是基於最優預計的。
2、誤差來源
在進行數值積分的過程中一定會引入數值誤差，數值誤差分為兩類。一類是積分誤差（來自於我們如果旋轉速率在短時間內不變引起的），還有一類是量化誤差（主要來自於模數轉換過程中引起的）。關於gyro誤差產生的具體分析見：http://www.crazepony.com/wiki/mpu6050.html
3、旋轉矩陣的約束
旋轉矩陣就是改變方向不改變大小。旋轉矩陣有9個元素，實際上是僅僅有三個是獨立的。因為旋轉矩陣的正交性，意味着不論什么一對行或者列都是相互垂直的，而且每一個行或者列的元素的平方和為1。
所以在9個元素中有6個限制條件。
旋轉矩陣的關鍵性能之中的一個就是正交性。即在某個參考坐標系下的三個方向的向量兩兩垂直，且向量長度在每一個參考系下都是等長的。
數值誤差會違背該性能，比方旋轉矩陣的行和列都是單位向量，其長度都是1，可是因為數值誤差的原因導致其長度邊長或變短。終於致使它們縮小到0或者增長到無窮大。最后的結果就是導致原本相互正交的如今變的傾斜了。
例如以下圖所看到的。
5、怎樣消除各種誤差（積累數值的舍入誤差、陀螺漂移誤差、偏移誤差、增益誤差）
其方法就是採樣DCMIMU:Theory中提出的理論—強制正交化（也稱為Renormalization）。
首先計算矩陣中X、Y行的點積（dotproduct）。理論上應該是0，可是因為各種errors的影響。所以點積的值不是0，而表示為error。
然后把這個誤差項分半交叉耦合到X、Y行上，例如以下公式。
最后一步就是放縮旋轉矩陣的每一行使其長度等於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() 好了。就是它了。慢慢研究吧。把這個里面的過程都研究透吧~~~~至少好幾天