原創文章，歡迎轉載，轉載請注明出處
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。。。。互斥量的存在，可以保證數據操作的同步和安全性。最后，我們是采用第三種方法。