第四章 PX4-Pixhawk-MPU6000传感器驱动解析
第四章MPU6000傳感器驅動解析
?????????Mpu6000是一個3軸加速度和3軸陀螺儀傳感器，這一章節我們將對MPU6000這個傳感器進行解析，依照這個解析步驟同樣可以對L3GD20（3軸陀螺儀）、HMC5883（3軸磁力計）、MS5611（氣壓高度計）程序進行閱讀理解，應為這幾個傳感器同樣都是采用SPI協議，這個可以查看硬件圖，待會我們也會貼出來。
?????????首先我們當然是找到驅動的入口，這個我們很容易找到驅動的入口就是
這個函數。至于這個函數我們可以查看到初始化這個幾個參數是這個，這里我們對他進行分析一下，busid就是說明是哪個總線協議，可以看到這個枚舉型是有5個數的都是關于內部SPI，外部SPI，內部IIC，外部IIC，這里的外部內部的意思是板上的傳感器和自己外加的傳感器，這個要理解清楚哦。Device_type是設備類型，external就是定義是內部V2硬件的傳感器還自己外接的傳感器，旋轉角度其實就是跟傳感器的擺放位置相關，加速度范圍是8G，即為+-4G。
然后就是這個函數了
這個其實就是解析了啟動傳感器的判斷了，還記得
這些就是在這里解析了，這里說明一下前面是對照找這個參數，然后接個冒號就是找這個參數值，比如我們來對照mpu6000 –X –R 4 start這個就是首先解析到-X就是
然后找到-R?然后找到這個參數后是4就是rotation是4也就是yaw旋轉180°，接著獲取start參數
，這個介紹完了，我們來查找我們V2硬件的mpu6000的啟動吧。我們找到rc.sensors中有
這里就是啟動了，如果不知道怎么找，那就去仔細閱讀RCS那一章節了，然后再仔細閱讀rc.sensors了。這里可以看得出不會進入該函數了。
接著進入到函數
這里的參數是，這里我不妨貼出來這個函數實際運行的就是進入到start中這里就會對驅動去循環查詢然后運行
，這里我們不對這個做詳細說明，有興趣的可以去看看，這里我們知道V2的硬件板就是使用的內部板載傳感器。然后我們對start_bus說明，這個函數是啟動總線協議了。這些個參數是進入函數
這里就是對函數協議的控制了。這里我們先找到設備總線的選擇，針對MPU6000我們選擇的是
看到沒這里我們選擇的是
這個函數哦bus.busnum這個參數我們到
中去找發現是
，device_type參數是6000，external還是0。
?????????進入到函數MUP6000_SPI_interface中external=0我們進入到else中，由于我們在板子的配置中并沒有定義PX4_SPIDEV_ICM，所以cs=PX4_SPIDEV_MPU=4。現在進入到
注：，這里是一個構造函數，C++中的new其實就是類似于C語言中的初始化。
看到沒這里就開始對SPI的配置進行說明了，這里SPI的進行選擇了哦，這里的bus=1了哦注意。Device變成了cs=4哦，然后就是模式和速率了。
這里對SPI的選擇模式和配置就完成了。然后回到mpu6000.cpp中的下一步就是
初始化協議了，這里用到了虛函數的概念哦，這里不懂也沒關系，最好是了解一下這些概念，應為這個系統會有很多都用到了虛函數，這里告訴你這個虛函數的就是SPI的初始化了。這里我們要找的就是SPI.ccp了，你可以查看到SPI的初始化了。這里我們就與剛才協議的驅動對接上了，
剛才說到這里的_bus=1，返回的值_dev=1,_device=cs=4
追蹤到這里就是這個函數了，至于為什么開始是SPI_SELECT到后面變成了STM32_spi1select到spi驅動篇去看看。這里選擇的執行時
，好了我們在來看看硬件圖，看到沒現在都對應起來了吧，spi選擇的是SPI1，cs選擇的是PC2引腳
。
再回到mpu6000.cpp中接下來就該
這里就開始對mpu6000的相關設置和配置，這里我們不對它進行細說這里還是一個構造函數，只是對相關數據進行初始化。這里面有很多，就有對路徑注冊，濾波器初始化，參數的初始化等等。這里開始進入到驅動的數據讀取端了。這里的初始化是重載了器件的init函數，也就是
這個函數就需要仔細去閱讀了，這里主要是去MPU的ID?
，然后設備初始化開辟數據空間
進行復位相關配置等等，這里需要配合MPU6000的寄存器手冊去看，這里不詳細說明，如果有需要大家提出來，本人也是可以出一個文檔進行說明，來教大家如何查看數據手冊和編寫底層驅動的。
這里的重頭函數是，這里面就有了數據的讀取了。進入該函數，首先進行定義數據，真正的讀取是函數
這個函數是read，至于為什么是read函數就需要有一點linux的常識了，這里的read是一個虛函數。這里后面到時候在做解釋。
這里就開始組合數據幀了，得到原始的加速度，溫度和陀螺儀數據。
得到數據后對這些數據的處理也比價多，有數據非0判斷、數據交換、旋轉、數據偏移糾正濾波。這些里面就涉及到了算法哦，比如原始數據的濾波器就用到了二階濾波。算法這塊我們暫時不去細講，目前的文檔比較適合應用，到時候也會出一些文檔適合真正的高手，就是算法這塊了。
數據相關處理完了之后就是nuttx的?機制出馬了需要發布數據。就是通過
這里也說一下，sensor_accel、sensor_gyro這兩個就是主題的名字，至于這個主題就是
這個文件夾下的.msg文件的名字了。打開這些文件你可以看到這里面就有對數據的說明了，這里說一下.msg文件只是一個過渡文件，最終會通過系統工具轉換為.h文件。如果自己要寫新的主題數據也是一樣的編寫一個.msg文件然后再目錄下的Cmakelists文件加入新的.msg文件就Ok了。在使用的時候加入頭文件（對應的文件.h）>就行了。這個生成的h文件是在目錄下。發布數據后就差不多完事了。然后就是啟動自動收集數據了。
。這樣MPU6000的數據和驅動就打通了哦。
這里對nuttx的相關函數進行說明一下，自己去理解哦，這個并不難，也沒必要知道函數怎么來的，只需要知道函數功能和使用就行了。
int poll(struct pollfd fds[], nfds_t nfds, int timeout)
功能：監控文件描述符（多個）；
說明：timemout=0,poll()函數立即返回而不阻塞；timeout=INFTIM(-1),poll()會一直阻塞下去，直到檢測到return > 0；
參數：
????fds:struct pollfd結構類型的數組；
????nfds:用于標記數組fds中的結構體元素的總數量；
????timeout:是poll函數調用阻塞的時間，單位：毫秒；
返回值：
????>0：數組fds中準備好讀、寫或出錯狀態的那些socket描述符的總數量；
????==0:poll()函數會阻塞timeout所指定的毫秒時間長度之后返回;
????-1:poll函數調用失敗；同時會自動設置全局變量errno；
int orb_subscribe(const struct orb_metadata *meta)
功能：訂閱主題（topic）;
說明：即使訂閱的主題沒有被公告，但是也能訂閱成功；但是在這種情況下，卻得不到數據，直到主題被公告；
參數：
????meta:uORB元對象，可以認為是主題id，一般是通過ORB_ID(主題名)來賦值；
返回值：
????錯誤則返回ERROR;成功則返回一個可以讀取數據、更新話題的句柄；如果待訂閱的主題沒有定義或聲明則會返回-1，然后會將errno賦值為ENOENT;
eg:
????int fd = orb_subscribe(ORB_ID(topicName));
int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
功能：從訂閱的主題中獲取數據并將數據保存到buffer中；
參數：
????meta:uORB元對象，可以認為是主題id，一般是通過ORB_ID(主題名)來賦值;
????handle:訂閱主題返回的句柄；
????buffer:從主題中獲取的數據；
返回值：
????返回OK表示獲取數據成功，錯誤返回ERROR;否則則有根據的去設置errno;
eg:
????struct sensor_combined_s raw;
????orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw);
orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data)
功能：公告發布者的主題；
說明：在發布主題之前是必須的；否則訂閱者雖然能訂閱，但是得不到數據；
參數：
????meta:uORB元對象，可以認為是主題id，一般是通過ORB_ID(主題名)來賦值;
????data:指向一個已被初始化，發布者要發布的數據存儲變量的指針；
返回值：錯誤則返回ERROR;成功則返回一個可以發布主題的句柄；如果待發布的主題沒有定義或聲明則會返回-1，然后會將errno賦值為ENOENT;
eg:
????struct vehicle_attitude_s att;
????memset(&att, 0, sizeof(att));
????int att_pub_fd = orb_advertise(ORB_ID(vehicle_attitude), &att);
int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
功能：發布新數據到主題；
參數：
????meta:uORB元對象，可以認為是主題id，一般是通過ORB_ID(主題名)來賦值;
????handle:orb_advertise函數返回的句柄；
????data:指向待發布數據的指針；
返回值：OK表示成功；錯誤返回ERROR；否則則有根據的去設置errno;
eg:
????orb_publish(ORB_ID(vehicle_attitude), att_pub_fd, &att);
int orb_set_interval(int handle, unsigned interval)
功能：設置訂閱的最小時間間隔；
說明：如果設置了，則在這間隔內發布的數據將訂閱不到；需要注意的是，設置后，第一次的數據訂閱還是由起初設置的頻率來獲取，
參數：
????handle:orb_subscribe函數返回的句柄；
????interval:間隔時間，單位ms;
返回值：OK表示成功；錯誤返回ERROR；否則則有根據的去設置errno;
eg:
????orb_set_interval(sensor_sub_fd, 1000);
orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority)
功能：設備/驅動器的多個實例實現公告，利用此函數可以注冊多個類似的驅動程序；
說明：例如在飛行器中有多個相同的傳感器，那他們的數據類型則類似，不必要注冊幾個不同的話題；
參數：
????meta:uORB元對象，可以認為是主題id，一般是通過ORB_ID(主題名)來賦值;
????data:指向一個已被初始化，發布者要發布的數據存儲變量的指針；
????instance:整型指針，指向實例的ID（從0開始）；
????priority:實例的優先級。如果用戶訂閱多個實例，優先級的設定可以使用戶使用優先級高的最優數據源；
返回值：
????錯誤則返回ERROR;成功則返回一個可以發布主題的句柄；如果待發布的主題沒有定義或聲明則會返回-1，然后會將errno賦值為ENOENT;
eg:
????struct orb_test t;
????t.val = 0;
????int instance0;
????orb_advert_t pfd0 = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance0, ORB_PRIO_MAX);
int orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance)
功能：訂閱主題（topic）;
說明：通過實例的ID索引來確定是主題的哪個實例；
參數：
????meta:uORB元對象，可以認為是主題id，一般是通過ORB_ID(主題名)來賦值;
????instance:主題實例ID;實例ID=0與orb_subscribe()實現相同；
返回值：
????錯誤則返回ERROR;成功則返回一個可以讀取數據、更新話題的句柄；如果待訂閱的主題沒有定義或聲明則會返回-1，然后會將errno賦值為ENOENT;
eg:
????int sfd1 = orb_subscribe_multi(ORB_ID(orb_multitest), 1);
int orb_unsubscribe(int handle)
功能：取消訂閱主題；
參數：
????handle:主題句柄；
返回值：
????OK表示成功；錯誤返回ERROR;否則則有根據的去設置errno;
eg:
????ret = orb_unsubscribe(handle);
int orb_check(int handle, bool *updated)
功能：訂閱者可以用來檢查一個主題在發布者上一次更新數據后，有沒有訂閱者調用過ob_copy來接收、處理過；
說明：如果主題在在被公告前就有人訂閱，那么這個API將返回“not-updated”直到主題被公告。可以不用poll，只用這個函數實現數據的獲取。
參數：
????handle:主題句柄；
????updated:如果當最后一次更新的數據被獲取了，檢測到并設置updated為ture;
返回值：
????OK表示檢測成功；錯誤返回ERROR;否則則有根據的去設置errno;
eg:
????if (PX4_OK != orb_check(sfd, &updated)) {
????????return printf("check(1) failed");
????}
????if (updated) {
????????return printf("spurious updated flag");
????}
????//or
????bool updated;
????struct random_integer_data rd;
???
????orb_check(topic_handle, &updated);
????if (updated) {
???????
????????orb_copy(ORB_ID(random_integer), topic_handle, &rd);
????????printf("Random integer is now %d\n", rd.r);
????}
int orb_stat(int handle, uint64_t *time)
功能：訂閱者可以用來檢查一個主題最后的發布時間；
參數：
????handle:主題句柄；
????time:存放主題最后發布的時間；0表示該主題沒有發布或公告；
返回值：
????OK表示檢測成功；錯誤返回ERROR;否則則有根據的去設置errno;
eg:
????ret = orb_stat(handle,time);
int orb_exists(const struct orb_metadata *meta, int instance)
功能：檢測一個主題是否存在；
參數：
????meta:uORB元對象，可以認為是主題id，一般是通過ORB_ID(主題名)來賦值;
????instance:ORB?實例ID;
返回值：
????OK表示檢測成功；錯誤返回ERROR;否則則有根據的去設置errno;
eg:
????ret = orb_exists(ORB_ID(vehicle_attitude),0);
int orb_priority(int handle, int *priority)
功能：獲取主題優先級別；
參數：
????handle:主題句柄；
????priority:存放獲取的優先級別；
返回值：
????OK表示檢測成功；錯誤返回ERROR;否則則有根據的去設置errno;
eg:
????ret = orb_priority(handle,&priority);
數據流程使用如下圖：
總結
以上是生活随笔為你收集整理的第四章 PX4-Pixhawk-MPU6000传感器驱动解析的全部內容，希望文章能夠幫你解決所遇到的問題。
- 上一篇： 第三章 PX4-Pixhawk-SPI底
- 下一篇： 第五章 PX4-Pixhawk-GPS解