轉載出處：http://www.cnblogs.com/adfjhg/p/3918316.html
原創文章，歡迎轉載，轉載請註明出處
MPU6050數據讀取出來後，經過一個星期的努力，姿態解算和在matlab上的實時顯示姿態終於完成了。
1：完成matlab的串口，並且實時通過波形顯示數據
2：添加RTT查看CPU使用率的擴展功能，MPU6050讀取數據的優化
3：四元素表示的座標變化，四元素與歐拉角的關係和Madgwick的IMUupdate算法
4：飛控數據採集線程和數據處理線程的安排，類似於生產者與消費者的關係。
先放個效果視頻：http://v.youku.com/v_show/id_XNzU3MTk0MTAw.html
1：matlab串口初始化還是比較簡單的，網上的資料也很多，這裏就直接貼初始化代碼了。
% --- Executes on button press in pb_OpenSerialPort.
function pb_OpenSerialPort_Callback(hObject, eventdata, handles)
% hObject handle to pb_OpenSerialPort (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
%
global o_SerialPort;
%______________________________________________
%GUI全局變量
%---------------------串口初始化-----------------------
%%%COM端口初始化
int_Index_COM=get(handles.pop_SerialPort,'Value');
string_COM=get(handles.pop_SerialPort,'String');
string_Select_COM=string_COM{int_Index_COM};
o_SerialPort=serial(string_Select_COM);
%%%Baud初始化
int_Index_Baud=get(handles.pop_BaudRate,'Value');
string_Baud=get(handles.pop_BaudRate,'String');
string_Select_Baud=string_Baud{int_Index_Baud};
double_Baud=str2double(string_Select_Baud);
set(o_SerialPort,'BaudRate',double_Baud);
%%%設置數據長度
int_Index_DataBit=get(handles.pop_DataBit,'Value');
string_DataBit=get(handles.pop_DataBit,'String');
string_Select_DataBit=string_DataBit(int_Index_DataBit);
double_DataBit=str2double(string_Select_DataBit);
set(o_SerialPort,'DataBits',double_DataBit);
%%%設置停止位長度
int_Index_StopBits=get(handles.pop_StopBits,'Value');
string_StopBits=get(handles.pop_StopBits,'String');
string_Select_StopBits=string_StopBits(int_Index_StopBits);
double_StopBits=str2double(string_Select_StopBits);
set(o_SerialPort,'StopBits',double_StopBits);
%%%設置輸入緩衝區大小爲1M
set(o_SerialPort,'InputBufferSize',1024000);
%%%串口事件回調設置
set(o_SerialPort,'BytesAvailableFcnMode','terminator');
set(o_SerialPort,'terminator','!'); %!標識結束符結束，方便處理和讀取數據
o_SerialPort.BytesAvailableFcn={@EveBytesAvailableFcn,handles};
% ----------------------打開串口-----------------------
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現實姿態的函數，寫着寫着我們發現了用方向餘弦的方法，而現在我們直接使用四元素進行座標變化，簡單暴力，幾行代碼搞定。具體代碼如下：
q0=SourceData(9);
q1=SourceData(10);
q2=SourceData(11);
q3=SourceData(12);
%創建四元素矩陣
q = [1-2*(q2^2+q3^2) 2*(q1*q2-q0*q3) 2*(q0*q2+q1*q3);
2*(q1*q2+q0*q3) 1-2*(q1^2+q3^2) 2*(q2*q3-q0*q1);
2*(q1*q3-q0*q2) 2*(q2*q3+q0*q1) 1-2*(q1^2+q2^2)];
%c初始化三角形的三個座標點
xd=[3 -1.2735;3 -1.2735];
yd=[0 1.3474;0 -1.3474];
zd=[0 0;0 0];
%坐標變換
temp = [xd(1,1) yd(1,1) zd(1,1);
xd(1,2) yd(1,2) zd(1,2);
xd(2,2) yd(2,2) zd(2,2)];
temp = temp*q;
xd = [temp(1:2,1)';temp(1,1),temp(3,1)];
yd = [temp(1:2,2)';temp(1,2),temp(3,2)];
zd = [temp(1:2,3)';temp(1,3),temp(3,3)]; 首先成功數據中提取四元素的四個參數，然後創建四元素的旋轉矩陣，最後對三角形的三個座標旋轉下就可以了。。真是暴汗。。之前那程序寫了一天。。。。。。
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。。。。互斥量的存在，可以保證數據操作的同步和安全性。最後，我們是採用第三種方法。