一、總體流程

先由驅動層drive，再到中間層sensor，再到應用層ekf2,最後釋出資料給其他應用。控制系統最重要的是頻寬，位置環的頻寬，到速度環的頻寬，再到感測器的更新率，所以，做控制，看程式的時候一定要注意幾個時間的概念。舉個例子，位置環50hz，速度環200hz，感測器資料的更新率一定要高於200hz。如果自己寫程式，裡面的這幾個時間一定要打印出來看一看，或者翻轉一個電平用示波器觀察以下。這是一個控制系統的基本要素，頻寬的概念。
為什麼位置環是50hz呢，速度環是200hz呢？這個問題，我們可以再開一篇文章進行討論，這裡說個大概，具體的控制頻率是取決於應用環境的環境變化速率。比如飛機在空中懸停著，我們就需要對自然環境給飛機繞動進行建模。比如飛機的機體低頻震動（高頻需要濾波，頻寬做高成本較大），比如一陣風吹過來，比如手動打舵的更新率等。一般控制頻寬要高於使用環境繞動或給定頻寬的3到5倍。不要問我為什麼，這是經驗公式，一定要有理論出處，那就是取樣定理，取樣定理說，只要2倍於一個訊號頻寬就可以還原這個訊號，但是我們一般都在做工程，一是要留一點冗餘量，二是取樣頻率越高，對訊號還原的失真度越低。大家一定還記得我們開始學控制原理的時候，是用一個pid去跟蹤幾個基本訊號，正弦訊號，斜坡訊號，階越訊號等。這裡可以反向看作是對這幾種訊號的取樣還原。
如果再需要理論化一點，一般一套控制系統，開始需要建模，對各種擾動訊號進行建模，建立了系統的控制模型後，在matlab裡面有個工具，叫simulink，裡面有一些基礎模型，另外，這個工具可以對建立的模型進行分析，自控裡面的兩大分析方法，根軌跡法和頻率分析法。一般做大型工程是需要按套路從理論到軟仿，到半實物模擬，到原理樣機實測，一步步來的。但是消費電子，為快不破，特別是現在很多開源的工程，前面的基礎工作已經有人做過了，論證過了，更何況有些專案已經被無數愛好者試驗過N多次了。我們就可以直接站在巨人的肩膀上了。有些年輕人，一進來就要去研究高深的理論演算法，系統模擬。彷彿不說幾個牛逼的理論，不足以證明自己也很牛逼似的，結果扎如了浩瀚的推導和公司，久久不能出局。就像筆者開始研究linux的時候，一頭扎進了系統核心的各種函式，彷彿進入了一片浩瀚的海洋，找不到方向，久而久之消磨了熱情。
能夠按照控制系統工程，從頭開始幹一遍的，不是軍工企業就是國有企業，如果都不是，一定是牛逼的上市公司，大型公司。他們有足夠的資金來搭建這些系統，軟體系統，硬體系統，半實物模擬系統，人員系統，人員系統尤其昂貴，做模擬的可能就是一個科室幾十號人，另外做研究做到這個層面基本都是碩士以上了，並且普通高校培養的人員可能技術層面上還略有欠缺，所以可以按照整個控制系統工程來做一遍的企業一定不簡單。有點扯遠了，還是來說我們的pixhawk。對於這個世界上的大多數工程和應用，人類憑藉自己的經驗就可以成功，比如生產一把菜刀，絕壁沒有人來對菜刀進行有限元應力分析，在多大削砍速度下，砍在什麼樣的材料上，刀身各處的強度分佈是怎麼樣的……，什麼情況下會卷韌會折斷……，有功夫算這些東西的，別人已經基於人類菜刀幾千年的經驗傳承，做了一把刀把生活中要砍之物全部砍了個遍。說到這裡，好像講到了哲學裡面的兩個派系，經驗派，邏輯派，哈哈，也有點像笑傲江湖裡面劍宗和氣宗。又扯遠了，這裡我其實想說的是，對於大多數的工程應用級別的，我們做好應用就行了，側重點放在框架，流程，介面，全貌，弄清楚了這些，還有精力的話，可以深入研究下里面的部分演算法，這就叫行有餘力而學文。讓哪些研究所，開源機構，大型公司去做理論分析。你看，google弄好的的牛逼的AI引擎tensorflow不也開源了嗎？保持對這個世界好奇，找準自己的定位，什麼人做什麼事，爭取到不惑之年真達到孔夫子講的那個境界。本來想講個感測器資料的流向問題（一個飛控用感測器的資料，怎麼產生，然後它經歷了什麼，最後到了哪裡，發揮了什麼作用，這個資料的整個生命週期多長，有哪幾個關鍵的階段，每個階段大概多久），結果手指隨心所欲，想到哪裡鍵盤就敲到了哪裡，各位湊合這看吧。有問題，我們可以在交流。
陀螺的資料，定時器回撥函式自動的採集，採集之後會進行積分，積分之後在sensor.cpp 裡面會根據積分時間在進行平均，又得到了角速度，然後有一個get_best的函式，會從幾組陀螺儀裡面，選出最好的一組速據，然後釋出在sensor_combine主題裡面。

                          /*
             * Using data that has been integrated in the driver before downsampling is preferred
             * becasue it reduces aliasing errors. Correct the raw sensor data for scale factor errors
             * and offsets due to temperature variation. It is assumed that any filtering of input
             * data required is
 
 performed in the sensor driver, preferably before downsampling.
            */

上面是原始碼裡面的註釋，他說用那些在降取樣之前的積分資料是非常好的，因為他減少了混跌誤差，修正了原始資料的比例因子的誤差還有由於溫度振盪帶來的漂移。

二、驅動層

在驅動層，以mpu6000為例（mpu6000.cpp），進入start函式，start在啟動指令碼（rcs）根據不同的硬體版本進行啟動，此處不贅述。進入start函式後可以看到，感測器資料是通過定時器定時回撥進行測量的。

    hrt_call_every(&_call,
               1000
 
,
               _call_interval - MPU6000_TIMER_REDUCTION,
               (hrt_callout)&MPU6000::measure_trampoline, this);

在measure_trampoline裡面，通過spi匯流排讀取了陀螺和加速度計的值，最後通過sensor_accel和sensor_gyro主題釋出了讀到的資料。

三、中間應用層

這層，主要是一個sensors.cpp，裡面有陀螺，加速度計，氣壓，空速等感測器的值。這個函式對所有的感測器的原始值進行了濾波，然後釋出。釋出在sensor_combine主題裡面，供其他函式呼叫。這個sensors.cpp裡面關鍵的幾個函式
_voted_sensors_update.sensors_poll(raw);
這個函式在拉取資料的同時，對資料進行了濾波，濾波就是上面提到的那個註釋，其實這裡還做了一個事情，就是機體座標系的調整，也就是你的飛控在飛機裡面的安裝方向的問題。然後呼叫了下面的函式，選出幾組感測器裡面最好的資料。
_accel.voter.get_best(hrt_absolute_time(), &best_index);
最後通過
orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw);
將所有感測器的資料釋出出去了。

四、應用層

為什麼要把上面的sensors.cpp歸類為中間層，我的理解是，他還是在為核心應用準備資料。核心應用層，這裡以EKF2為例，進了主函式後，首先訂閱了sensor_combined 資料。資料比較多，這裡還是以陀螺和加速度計為例子，下面有一段很清晰的程式碼，外國兄弟把註釋都寫的很清楚的。
// push imu data into estimator float gyro_integral[3]; gyro_integral[0] = sensors.gyro_rad[0] * sensors.gyro_integral_dt; gyro_integral[1] = sensors.gyro_rad[1] * sensors.gyro_integral_dt; gyro_integral[2] = sensors.gyro_rad[2] * sensors.gyro_integral_dt; float accel_integral[3]; accel_integral[0] = sensors.accelerometer_m_s2[0] * sensors.accelerometer_integral_dt; accel_integral[1] = sensors.accelerometer_m_s2[1] * sensors.accelerometer_integral_dt; accel_integral[2] = sensors.accelerometer_m_s2[2] * sensors.accelerometer_integral_dt; _ekf.setIMUData(now, sensors.gyro_integral_dt * 1.e6f, sensors.accelerometer_integral_dt * 1.e6f, gyro_integral, accel_integral);
把陀螺和加速度的資料推入評估器。哈哈，推進去幹什麼呢？推進去當然是融合，怎麼融合呢？說起來很簡單，卡爾曼五步法，細節很複雜，我也沒有完全弄透，關於卡爾曼的，一本書可以推薦（《最優狀態估計》）。下面我們關心我們更想要的東西。那就是用來做控制用的ctrl_state。後面的所有的控制都會訂閱這個的，程式在這裡，最新發布版本1.6.3裡面 ekf2_main.cpp的第775行。

if (_ekf.update()) { // integrate time to monitor time slippage if (start_time_us == 0) { start_time_us = now; } else if (start_time_us > 0) { integrated_time_us += (uint64_t)((double)sensors.gyro_integral_dt * 1.0e6); } matrix::Quaternion<float> q; _ekf.copy_quaternion(q.data()); ……………… orb_publish(ORB_ID(control_state), _control_state_pub, &ctrl_state);
到這裡，感測器的資料到處理，到融合到釋出，基本都說完了。現在的控制狀態的資料是可以直接拿去做控制的了。
最後說一點，如果你是一般的無人機公司，一般研究到這個程度，側重於除錯，以及結合實際的應用場景更改完善飛機的邏輯，修改程式的bug，是完全夠用了。如果你是一家比較大的無人機公司，想加點自己的感測器，也不是問題（直接uart，spi對接fmu，還不行，寫寫底層驅動也不是什麼大事，各種協議實現都是通的），如果你想把感測器的資料推入到卡爾曼裡面，一起融合，那就要研究的更深入了，當然了沒有什麼過不去的坑，只是精力和時間的分配是否值得的問題。
下一篇，跟隨筆者一起研究下，pixhawk裡面的幾個關鍵dt，直接影響控制精度的。關於這個dt的概念，通過筆者下一篇的介紹，大家就可以知道，pixhawk能不能用來做火箭，做導彈（導引頭），太空飛船……，我不太喜歡這些規則的約束，太累，說不準下一篇，說點別的，後會有期。