文/mike

一、總體流程

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

上面是源碼里面的注釋，他說用那些在降采樣之前的積分?jǐn)?shù)據(jù)是非常好的，因?yàn)樗麥p少了混跌誤差，修正了原始數(shù)據(jù)的比例因子的誤差還有由于溫度振蕩帶來的漂移。

二、驅(qū)動層

在驅(qū)動層，以mpu6000為例（mpu6000.cpp），進(jìn)入start函數(shù)，start在啟動腳本（rcs）根據(jù)不同的硬件版本進(jìn)行啟動，此處不贅述。進(jìn)入start函數(shù)后可以看到，傳感器數(shù)據(jù)是通過定時器定時回調(diào)進(jìn)行測量的。

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

在measure_trampoline里面，通過spi總線讀取了陀螺和加速度計(jì)的值，最后通過sensor_accel和sensor_gyro主題發(fā)布了讀到的數(shù)據(jù)。

三、中間應(yīng)用層

這層，主要是一個sensors.cpp，里面有陀螺，加速度計(jì)，氣壓，空速等傳感器的值。這個函數(shù)對所有的傳感器的原始值進(jìn)行了濾波，然后發(fā)布。發(fā)布在sensor_combine主題里面，供其他函數(shù)調(diào)用。這個sensors.cpp里面關(guān)鍵的幾個函數(shù)
_voted_sensors_update.sensors_poll(raw);
這個函數(shù)在拉取數(shù)據(jù)的同時，對數(shù)據(jù)進(jìn)行了濾波，濾波就是上面提到的那個注釋，其實(shí)這里還做了一個事情，就是機(jī)體坐標(biāo)系的調(diào)整，也就是你的飛控在飛機(jī)里面的安裝方向的問題。然后調(diào)用了下面的函數(shù)，選出幾組傳感器里面最好的數(shù)據(jù)。
_accel.voter.get_best(hrt_absolute_time(), &best_index);
最后通過
orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw);
將所有傳感器的數(shù)據(jù)發(fā)布出去了。

四、應(yīng)用層

為什么要把上面的sensors.cpp歸類為中間層，我的理解是，他還是在為核心應(yīng)用準(zhǔn)備數(shù)據(jù)。核心應(yīng)用層，這里以EKF2為例，進(jìn)了主函數(shù)后，首先訂閱了sensor_combined 數(shù)據(jù)。數(shù)據(jù)比較多，這里還是以陀螺和加速度計(jì)為例子，下面有一段很清晰的代碼，外國兄弟把注釋都寫的很清楚的。
// 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);
把陀螺和加速度的數(shù)據(jù)推入評估器。哈哈，推進(jìn)去干什么呢？推進(jìn)去當(dāng)然是融合，怎么融合呢？說起來很簡單，卡爾曼五步法，細(xì)節(jié)很復(fù)雜，我也沒有完全弄透，關(guān)于卡爾曼的，一本書可以推薦（《最優(yōu)狀態(tài)估計(jì)》）。下面我們關(guān)心我們更想要的東西。那就是用來做控制用的ctrl_state。后面的所有的控制都會訂閱這個的，程序在這里，最新發(fā)布版本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);
到這里，傳感器的數(shù)據(jù)到處理，到融合到發(fā)布，基本都說完了?，F(xiàn)在的控制狀態(tài)的數(shù)據(jù)是可以直接拿去做控制的了。
最后說一點(diǎn)，如果你是一般的無人機(jī)公司，一般研究到這個程度，側(cè)重于調(diào)試，以及結(jié)合實(shí)際的應(yīng)用場景更改完善飛機(jī)的邏輯，修改程序的bug，是完全夠用了。如果你是一家比較大的無人機(jī)公司，想加點(diǎn)自己的傳感器，也不是問題（直接uart，spi對接fmu，還不行，寫寫底層驅(qū)動也不是什么大事，各種協(xié)議實(shí)現(xiàn)都是通的），如果你想把傳感器的數(shù)據(jù)推入到卡爾曼里面，一起融合，那就要研究的更深入了，當(dāng)然了沒有什么過不去的坑，只是精力和時間的分配是否值得的問題。
下一篇，跟隨筆者一起研究下，pixhawk里面的幾個關(guān)鍵dt，直接影響控制精度的。關(guān)于這個dt的概念，通過筆者下一篇的介紹，大家就可以知道，pixhawk能不能用來做火箭，做導(dǎo)彈（導(dǎo)引頭），太空飛船……，我不太喜歡這些規(guī)則的約束，太累，說不準(zhǔn)下一篇，說點(diǎn)別的，后會有期。