第四章MPU6000感測器驅動解析

         Mpu6000是一個3軸加速度和3軸陀螺儀感測器，這一章節我們將對MPU6000這個感測器進行解析，依照這個解析步驟同樣可以對L3GD20（3軸陀螺儀）、HMC5883（3軸磁力計）、MS5611（氣壓高度計）程式進行閱讀理解，應為這幾個感測器同樣都是採用SPI協議，這個可以檢視硬體圖，待會我們也會貼出來。

 首先我們當然是找到驅動的入口，這個我們很容易找到驅動的入口就是

這個函式。至於這個函式我們可以檢視到初始化這個幾個引數是這個，這裡我們對他進行分析一下，busid就是說明是哪個匯流排協議，可以看到這個列舉型是有5個數的都是關於內部SPI，外部SPI，內部

然後就是這個函數了

這個其實就是解析了啟動感測器的判斷了，還記得

這些就是在這裡解析了，這裡說明一下前面是對照找這個引數，然後接個冒號就是找這個引數值，比如我們來對照mpu6000 –X –R 4 start這個就是首先解析到-X就是

然後找到-R 然後找到這個引數後是4就是rotation是4也就是yaw旋轉180°，接著獲取start

引數

，這個介紹完了，我們來查詢我們V2硬體的mpu6000的啟動吧。我們找到rc.sensors中有

這裡就是啟動了，如果不知道怎麼找，那就去仔細閱讀RCS那一章節了，然後再仔細閱讀rc.sensors了。這裡可以看得出不會進入該函數了。

接著進入到函式

這裡的引數是，這裡我不妨貼出來這個函式實際執行的就是進入到start中這裡就會對驅動去迴圈查詢然後執行

，這裡我們不對這個做詳細說明，有興趣的可以去看看，這裡我們知道V2的硬體板就是使用的內部板載感測器。然後我們對start_bus說明，這個函式是啟動匯流排協議了。這些個引數是進入函式

這裡就是對函式協議的控制了。這裡我們先找到裝置匯流排的選擇，針對

我們選擇的是

看到沒這裡我們選擇的是

這個函式哦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檔案只是一個過渡檔案，最終會通過系統工具轉換為.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);

資料流程使用如下圖：