MPU6050資料讀取出來後，經過一個星期的努力，姿態解算和在matlab上的實時顯示姿態終於完成了。

1：完成matlab的串列埠，並且實時通過波形顯示資料

2：新增RTT檢視CPU使用率的擴充套件功能，MPU6050讀取資料的優化

3：四元素表示的座標變化，四元素與尤拉角的關係和Madgwick的IMUupdate演算法

4：飛控資料採集執行緒和資料處理執行緒的安排，類似於生產者與消費者的關係。

 先放個效果視訊。。。如果看不了視訊，請開啟視屏網址：http://v.youku.com/v_show/id_XNzU3MTk0MTAw.html

1：matlab串列埠初始化還是比較簡單的，網上的資料也很多，這裡就直接貼初始化程式碼了。

 1 % --- Executes on button press in pb_OpenSerialPort.
 2 function pb_OpenSerialPort_Callback(hObject, eventdata, handles)
 3 % hObject    handle to pb_OpenSerialPort (see GCBO)
 4 % eventdata  reserved - to be defined in a future version of MATLAB
 5 % handles    structure with handles and user data (see GUIDATA)
 
 6 %
 7 global o_SerialPort;
 8 %______________________________________________
 9 %GUI全域性變數
10 
11 
12 %---------------------串列埠初始化-----------------------
13 %%%COM埠初始化
14 int_Index_COM=get(handles.pop_SerialPort,'Value');
15 string_COM=get(handles.pop_SerialPort,'String');
16 string_Select_COM=string_COM{int_Index_COM};
 
17 o_SerialPort=serial(string_Select_COM);
18 %%%Baud初始化
19 int_Index_Baud=get(handles.pop_BaudRate,'Value');
20 string_Baud=get(handles.pop_BaudRate,'String');
21 string_Select_Baud=string_Baud{int_Index_Baud};
22 double_Baud=str2double(string_Select_Baud);
23 set(o_SerialPort,'BaudRate',double_Baud);
24 %%%設定資料長度
25 int_Index_DataBit=get(handles.pop_DataBit,'Value');
26 string_DataBit=get(handles.pop_DataBit,'String');
27 string_Select_DataBit=string_DataBit(int_Index_DataBit);
28 double_DataBit=str2double(string_Select_DataBit);
29 set(o_SerialPort,'DataBits',double_DataBit);
30 %%%設定停止位長度
31 int_Index_StopBits=get(handles.pop_StopBits,'Value');
32 string_StopBits=get(handles.pop_StopBits,'String');
33 string_Select_StopBits=string_StopBits(int_Index_StopBits);
34 double_StopBits=str2double(string_Select_StopBits);
35 set(o_SerialPort,'StopBits',double_StopBits);
36 %%%設定輸入緩衝區大小為1M
37 set(o_SerialPort,'InputBufferSize',1024000);
38 %%%串列埠事件回撥設定
39         
40         set(o_SerialPort,'BytesAvailableFcnMode','terminator');
41         set(o_SerialPort,'terminator','!');     %!標識結束符結束，方便處理和讀取資料
42 
43         o_SerialPort.BytesAvailableFcn={@EveBytesAvailableFcn,handles};
44 % ----------------------開啟串列埠-----------------------
45 fopen(o_SerialPort);

      matlab串列埠我們採用回撥函式，類似於中斷方式哈，但是mtalb的串列埠十分的不好用哈，沒有多執行緒，而我們在中斷裡面需要進行波形顯示，四元素旋轉等各種資料操作，是需要花費點時間的，這就導致我們的資料平率不能很高。。當上傳的速率達到100hz以後，就會出錯了。。50hz也不穩定。。這個實在是有點。。。擔心以後的系統辨識和慣性導航的資料處理了。。頭疼。。。

      matlab採用符號‘！’為結束符，碰到這個符號matlab就會呼叫回撥函式，中間的資料都是逗號隔開的，資料順序一次為accex,accey,accez,temp,gyrox,gyroy,gyroz,cpu_major,q0,q1,q2,q3傳送，資料通過sprintf進行格式化，然後通過rt_kprintf函式傳送，

1   sprintf(buffer,"\n%7.2f,%7.2f,%7.2f,%7.2f,%7.2f,%7.2f,%7.2f,%7d,%7.2f,%7.2f,%7.2f,%7.2f!", \
2             mpu6050_data_tf->acce_x,mpu6050_data_tf->acce_y,mpu6050_data_tf->acce_z,\
3             mpu6050_data_tf->temp,mpu6050_data_tf->gyro_x,mpu6050_data_tf->gyro_y,\
4             mpu6050_data_tf->gyro_z,(s8)cpu_major,q0,q1,q2,q3);
5           rt_kprintf("%s",buffer);

 temp是MPU6050讀出的溫度數，cpu_major是CPU使用率，q0,q1,q2,q3分別對應四元素的四個引數，q0是實數，其他分別對應i,j,k的引數。

  matlab資料處理:收到資料後，其實標準的處理方式是用matlab的regexp函式，用正則表示式將資料讀取出來，我們沒有用這個，上傳資料格式我們自己可以控制，所以處理起來很簡單，沒必要用到複雜的正則表示式，而且正則表示式處理時間應該比我們自己簡單的處理方法的時間要長，所以採用簡單的處理方法。處理方法是先將資料中的空格去掉，然後去掉結束符感嘆號，最後把資料中的間隔福逗號去掉，去掉後呼叫str2num函式將字串轉換為數字就行了。

1     StringIn(StringIn==' ')=[];   %先去掉空格
2     StringIn(StringIn=='!')=[];   %去掉感嘆號
3     StringIn(StringIn==',')=' ';  %逗號程式設計空格    
4     SourceData=str2num(StringIn);

呼叫plot函式就可以繪製波形了。。這個比較簡單，不過還是解釋下四元素在這裡的用處。

話說我們最開始的時候寫了一個通過yaw pitch roll現實姿態的函式，寫著寫著我們發現了用方向餘弦的方法，而現在我們直接使用四元素進行座標變化，簡單暴力，幾行程式碼搞定。具體程式碼如下：

 1     q0=SourceData(9);
 2     q1=SourceData(10);
 3     q2=SourceData(11);
 4     q3=SourceData(12);
 5     
 6     %建立四元素矩陣
 7    q = [1-2*(q2^2+q3^2)        2*(q1*q2-q0*q3)        2*(q0*q2+q1*q3);
 8         2*(q1*q2+q0*q3)        1-2*(q1^2+q3^2)        2*(q2*q3-q0*q1); 
 9         2*(q1*q3-q0*q2)        2*(q2*q3+q0*q1)        1-2*(q1^2+q2^2)];
10      
11     %c初始化三角形的三個座標點
12     xd=[3 -1.2735;3 -1.2735]; 
13     yd=[0  1.3474;0  -1.3474];
14     zd=[0 0;0 0];
15     
16     %座標變換
17     temp = [xd(1,1) yd(1,1) zd(1,1);
18             xd(1,2) yd(1,2) zd(1,2);
19             xd(2,2) yd(2,2) zd(2,2)];
20     temp = temp*q;
21     xd = [temp(1:2,1)';temp(1,1),temp(3,1)];
22     yd = [temp(1:2,2)';temp(1,2),temp(3,2)];
23     zd = [temp(1:2,3)';temp(1,3),temp(3,3)];
24         

   首先成功資料中提取四元素的四個引數，然後建立四元素的旋轉矩陣，最後對三角形的三個座標旋轉下就可以了。。真是暴汗。。之前那程式寫了一天。。。。。。

matlab介面如下，後期我們還需要新增控制四個電機的pwm數值現實和pid控制器中yaw pitch roll目標值的顯示，這樣就可以看到PID的控制效果和對齊進行調整了。

左邊中間兩個方框，左邊那個33.76是mpu6050讀出來的溫度數值，右邊的7是代表CPU使用率為7%。

2：為了觀察cpu的使用情況，我想找個像UCOS裡面的一個變數，可以檢視CPU使用率的引數，可是RTT並沒有包涵在標準的系統中，而需要單獨新增，在RTT系統目錄下的examples\kernel中，我們可以找到一個叫做cpuusage.c檔案，將這個檔案新增到我們自己的工程中，然後呼叫void cpu_usage_init()函式，這個函式是初始化RTT IDLE執行緒的一個鉤子函式，在空閒期間統計CPU的使用率。。函式程式碼如下：

1 void cpu_usage_init()
2 {
3     /* set idle thread hook */
4     rt_thread_idle_sethook(cpu_usage_idle_hook);
5 }

具體統計方式我們就不多說了哈。。這一說又要說一大段話。。初始化後怎麼獲取CPU的使用率，我們呼叫void cpu_usage_get(rt_uint8_t *major, rt_uint8_t *minor)函式就可以獲取CPU的使用率，分別為整數部分和小數部分，我們值用了整數部分。。哈。。懶得處理了。。整數部分已經可以反映CPU的使用率了。

使用率一出來，嚇了一跳，在1000hz取樣率和100hz姿態解算下，使用率高達43%，而且我們還沒有進行姿態解算。。指示簡單的上傳資料到matlab。。。先分析原因，我們在讀取MPU6050資料的時候，因為I2C是可能可以重入的函式，使用了RTT零界區的管理函式，rt_enter_critical(void)和rt_exit_critical(void) 進行處理，I2C讀取資料的時間也是相當可觀的，可能是因為這個導致CPU使用率很高，當時用零界區也是想偷懶，I2C雖然是一個可重入的函式，但是我們使用的是模擬I2C，也就是說I2C是可以被打斷的，所以其實我們是可以試用互斥量來處理的，本來共享資源就是應該使用互斥量來處理的哈。。額。。用互斥量，可以保證I2C不被重入，但是可以被打斷，這樣不回影響更高優先順序任務的執行。。。

MPU6050讀資料還有一個需要處理，上一章我們也說了，為了偷懶，避免大端小端的轉換，我們通過共用體的方式讀取處理從而避免轉換，之前我們也說了，這個會帶來讀取資料的時候效率底下的問題，我們來算算有多底下。。。我們需要讀取accex,accey,accez,temp,gyrox,gyroy,gyroz，7個變數，都是2位元組的，需要讀取14次，每次通過I2C讀取的過程是寫MPU6050的地址，然後的到ack，然後寫暫存器地址，然後寫MPU6050讀取的命令，然後讀資料，然後返回noack命令，返回stop命令，中間通訊過程中的應答幀需要的事件我們忽略掉，單看讀一個位元組，我們需要寫三個位元組讀取一個位元組，中間傳輸了4個位元組，14*4=56個位元組，也就是讀取14個位元組的資料我們中間傳輸了56個位元組。。相當的浪費，所以還是改成連讀吧。。如果我們使用硬體I2C或者使用DMA方式，這還不那麼明顯，讀取的時候CPU可以做其他事情，指示讀取獲得資料的時間長了點。。哎。。模擬的生不起啊。。看看連讀，連讀下我們只需要寫入MPU6050地址，開始暫存器，然後寫讀命令，然後讀啊讀，讀完14個位元組就可以了（這幾個暫存器在MPU6050裡面內部地址是連起來的哈），也就是3+14=17，也就是說讀14個位元組，我們中間只傳了17個位元組，效率相當的客觀。經過這樣處理,CPU的使用率可以降低15%左右。。。。

 3：四元素表示的座標變化和四元素與尤拉角的關係

    說起四元素，不懂的時候感覺那是相當的高深，這是個姿態表示式而已，就和尤拉角一樣的，換個表達方式。。

    其實回到最開始的地方，就是我們獲取MPU6050的資料後怎麼處理。一個是加速度，一個是角速度。。有了加速度我們就可以算出pitch和roll了，可是可是。。。。。可是灰機是動態的，會動的，灰機動的過程中自己本身機體也會產生加速度，那麼我們就需要分辨因為地球重力產生的重力加速度和機體的加速度才能算出集體的pitch和roll了，這個分辨的過程會比較麻煩，不過後面用慣性導航演算法的時候，這個是接觸，否則沒有分辨出機體的加速度怎麼可以計算出灰機的軌跡呢。。對吧。。不過這裡有個問題哈，加速度在震動情況下輸出的值是波動很大的，而陀螺在動態下輸出就要好很多。。。可是可是再可是。。。陀螺輸出的是角速度，我們用時間乘以這個角速度就可以的到角度，每次積分的角度和上次的角度相加，就可以達到集體的xyz三個軸的角度了，可是可是再可是。。。。你取樣頻率足夠高嗎？解算頻率足夠高嗎？在你解算的過程中，角速度是恆定的嗎？保證不了吧？那就意味著長時間的對陀螺儀積分出來的角度誤差會越來越大。。。雖然積分的時候可以使用龍格-庫塔積分方法，這個方法還是比較簡單的哈，但是角速度積分誤差還是無可避免的。。。這個時候加速度就派上用場了。。。用加速度校正陀螺儀角度積分的誤差啊。。。。對，沒錯，就是短期相信陀螺儀，然後長期相信加速度。Madgwick的IMUupdate演算法的總體思路就是這樣的哈。。。

   返回來說四元素。。。四元素其實不難的哈，我參照 鄧正隆的慣性技術的四元素部分學習了下，講的還是比較詳細的，看不懂？對，開始是看不怎麼太懂，，哈。怎麼辦？做題啊。。。按照書上講的，按照他的推導過程，自己推倒一遍。。瞬間明白了有木有？雖然理解深度可能還不深，但是我直到怎麼用了。。哈哈。。

推到過程自己用筆推下就懂了。。

四元素還可以參考這個網址http://www.cnblogs.com/Mrt-02/archive/2011/10/15/2213656.html

四元素的一些公式：

座標轉換公式，我們matlab裡面畫圖的座標轉化公式就是這個哈。。

其中w,x,y,z就是四元素的四個元素，W為實數部分，xyz對應ijk的三個變數。

四元素的微分方程：

這個公式在IMUupdate有用到哈，看了這個公式應該直到halfT是怎麼來的了吧。。四元素其實知識還挺多的哈。。還是要自己算算才會哈。。看沒用的。。

4：飛控資料採集執行緒和資料處理執行緒的安排，類似於生產者與消費者的關係。

    採集資料的頻率可以很高，採集足夠多的資料才好處理嗎。哈。。可是CPU的效能考慮，姿態解算頻率太高並不划算。。。

    我們有兩個執行緒，一個採集資料執行緒，採集頻率500hz，一個姿態解算執行緒，將採集過來的資料進行解算，通過解算的結果再控制電機。這其中資料採集執行緒就是生產者，姿態解算執行緒就是消費者。。。。他們之間需要通過怎樣的協調工作才能是效率高並且資料安全呢？這裡我們有三鍾方法：

（1） 生產者只管採集資料，消費者只管消費資料，這就有個麻煩，生產者不知道消費者什麼時候會消費資料，所以他要時刻將資料準備好，讓消費者隨時可以消費資料，並且資料是要最新的。那麼濾波怎麼辦？每個資料都要是最新的且濾波好的，那就要用平滑濾波了，應該是這麼叫，然後視窗多大呢？不知道，額。。希望是低耦合高內聚的程式碼哈。如果需要解決視窗大大小，有兩種方法，實現預定好，或者消費者消費的時候告知生產者，我已經消費，那麼生產者可以重新設定視窗。那麼，這中間不可避免的消費者和生產者是要交流的，而且消費資料的時候資料是共享資源，需要互斥量。。也就是說，要實現我們需要互斥量，然後要平滑濾波，生產者每次生產資料都要進行濾波運算。。。效率並不高

（2） 生產者採集N個數據，通知消費者消費。這裡有個問題，消費者處在阻塞狀態，等生產者生產好了通知消費者，消費者才消費，可是，系統執行會出現各種各樣的問題，無法保證100%生產者的task不出問題，所以這樣的系統怎麼說呢，安全不好保證，同是也不利於我們到時候統計各個任務的狀態，通過統計各個任務的狀態我們可以實現類似於硬體看門狗的功能，任務出問題了，可以對任務進行刪除再重啟的操作等。。

（3） 生產者只管採集資料，消費者消費資料，但是我們這中間增加一個協調員。我們有兩個變數來同步這兩個任務。。互斥量和計數。。。生產者只管統計資料，將資料進行累加，同時技術，消費者消費資料，但是需要做些處理，拿到資料後對資料均值濾波，就是除以統計的次數，同時將原來的數值請0。。。。互斥量的存在，可以保證資料操作的同步和安全性。最後，我們是採用第三種方法。