一、總體流程

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

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

二、驅動層

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

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

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

三、中間應用層

這層，主要是一個sensors.cpp，里面有陀螺，加速度計，氣壓，空速等傳感器的值。這個函數(shù)對所有的傳感器的原始值進行了濾波，然后發(fā)布。發(fā)布在sensor_combine主題里面，供其他函數(shù)調(diào)用。這個sensors.cpp里面關鍵的幾個函數(shù)
_voted_sensors_update.sensors_poll(raw);
這個函數(shù)在拉取數(shù)據(jù)的同時，對數(shù)據(jù)進行了濾波，濾波就是上面提到的那個注釋，其實這里還做了一個事情，就是機體坐標系的調(diào)整，也就是你的飛控在飛機里面的安裝方向的問題。然后調(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ā)布出去了。

四、應用層

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

作者：周大有
鏈接：https://www.jianshu.com/p/5f50a90c6922
來源：簡書
著作權歸作者所有。商業(yè)轉載請聯(lián)系作者獲得授權，非商業(yè)轉載請注明出處。

總結

以上是生活随笔為你收集整理的Pixhawk的传感器数据（陀螺、加计等）流程的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。