【正文】 第 30 頁(yè) 西華大學(xué)畢業(yè)設(shè)計(jì)。 LATCH_INT_EN 等于 1， INT 引腳保持有效電平直到中斷被清除。 經(jīng)過(guò)分析后，程序里是這樣配置的： INT_LEVEL 等于 1， INT 引腳的邏輯電平為低電平有效。 INT_RD_CLEAR 該位等于 0，只有讀取 INT_STATUS (寄存器 58) 會(huì)導(dǎo)致中斷狀態(tài)位清除。 LATCH_INT_EN 該位等于 0， INT 引腳產(chǎn)生 50us 脈沖。 INT_LEVEL 該位等于 0， INT 引腳的邏輯電平為高電平。讀取寄存器該位清 0。這個(gè)寄存器我們需要注意 DATA_RDY_INT 位。 表 寄存器 58中斷標(biāo)志寄存器 Register (Hex) Register (Decimal) Bit7 Bit6 Bit5 Bit4 Bit3 Bit2 Bit1 Bit0 3A 58 MOT_INT FIFO _OFLOW _INT I2C _MST _INT DATA _RDY _INT 這個(gè)寄存器展示各個(gè)中斷源的中斷標(biāo)志。 為了解決這個(gè)問(wèn)題 ， 使用了 MPU6050 芯片的數(shù)據(jù)就緒中斷模式，再利用單片機(jī)的外部中斷 ， 我們就可以在 MPU6050 數(shù)據(jù)就緒時(shí)立刻獲得飛行器的姿態(tài) 。如果讀取晚了，最直觀的影響就是飛行器姿態(tài)調(diào)節(jié)緩慢，無(wú)法正常飛行。當(dāng) FIFO 數(shù)據(jù)不足時(shí)，會(huì)延時(shí)等待。 數(shù)據(jù)中斷 雖然我們可以通過(guò) DMP 模式獲取正確的歐拉角，但是應(yīng)該在什么時(shí)候去讀取 DMP的四元數(shù)呢？因?yàn)?DMP 模式下這要的數(shù)據(jù)都是存儲(chǔ)在 MPU6050 里面的 FIFO， FIFO 里面存的包括 GYRO， ACC，四元數(shù)等參數(shù)。即繞 Z 軸順時(shí)針轉(zhuǎn)動(dòng)，所以 yaw 為 30度。即繞 X 軸順時(shí)針轉(zhuǎn)動(dòng)，所以 roll 為 60度。即繞 Y 軸逆時(shí)針轉(zhuǎn)動(dòng)，所以 pitch 為30 度。 圖 模塊連接圖 將 GY86 模塊按不同傾角擺放，具體如下表 所示。 同樣的，我們 用杜邦線 把 GY86 模塊（模塊上載有 MPU6050 芯片）接入單片機(jī)，先讓單片機(jī)接收 MPU6050 原始數(shù)據(jù)，并把數(shù)據(jù)做一系列的處理并送入 DMP，再根據(jù)以上兩個(gè)公式對(duì) DMP 的數(shù)據(jù)做進(jìn)一步的處理，最后將計(jì)算出的歐拉角 yaw、 roll 和 pitch顯示在 TFT 屏幕上，方便觀察 和分析。 roll = atan2(2 * q2 * q3 + 2 * q0 * q1, 2 * q1 * q1 2 * q2* q2 + 1)* 。 在 得到四元數(shù)之后，還需要根據(jù)四元數(shù)到歐拉角的轉(zhuǎn)換公式計(jì)算出歐拉角，即得到y(tǒng)aw、 roll 和 pitch。 q2 = quat[2] / q30。必須先將其轉(zhuǎn)換為浮 點(diǎn)數(shù)，再除以 2 的30 次方，計(jì)算公式為： q0 = quat[0] / q30。四元數(shù)其實(shí)是旋轉(zhuǎn)另一種的表示方式，這里不做詳細(xì)介紹。 還有一種方法，利用 MPU6050 的數(shù)字運(yùn)動(dòng)處理器，即 DMP。如何消除這個(gè)累積誤差呢？ 一種方法就是通過(guò)上面的加速度獲得的角度信息對(duì)此進(jìn)行校正。 圖 角速度積分漂移現(xiàn)象曲線圖 圖中實(shí)線表示由角速度積分后的角度值，虛線表示真實(shí)的角度值 。這個(gè)誤差會(huì)隨著時(shí)間延長(zhǎng)逐步增加，最終導(dǎo)致無(wú)法形成正確的角度信號(hào)。但是這存在一個(gè)問(wèn)題，由于從陀螺儀角速度獲得角度信息，需要經(jīng)過(guò)積分運(yùn)算。上述兩方面的濾波效果使得飛行器保持平衡的難度加大。但是平滑濾波一方面會(huì)使得信號(hào)無(wú)法實(shí)時(shí)反映飛行器的傾角變化，進(jìn)而減緩了程序?qū)τ陲w行器控制。 圖 單軸加速度測(cè)量值曲線圖 圖中實(shí)線表示 MPU6050 單軸測(cè)得加速度變化量，而虛線表示重力加速度在該軸上 第 26 頁(yè) 西華大學(xué)畢業(yè)設(shè)計(jì)說(shuō)明書(shū) 的分量，可以看出，是有干擾疊加在重力加速度分量信號(hào)上的。 Azr = arccos(RZ / R) = 0 度。 Axr = arccos(RX / R) = 90 度。對(duì)應(yīng)的 ADC值就是 16384 (2g 的量程 )。很多時(shí)候，只有慣性矢量方向才是真正有用的，對(duì)矢量進(jìn)行規(guī)范化對(duì)簡(jiǎn)化程序運(yùn)算很有意義。 第 25 頁(yè) 西華大學(xué)畢業(yè)設(shè)計(jì)說(shuō)明書(shū) COS（ Axr） = RX / R 依此類(lèi)推： COS（ Ayr） = RY / R COS（ Azr） = RZ / R 當(dāng)使用 arccos()反余弦 : Axr = arccos(RX / R) Ayr = arccos(RY / R) Azr = arccos(RZ / R) 具體而言，就是： cosX = cos(Axr) = Rx / R cosY = cos(Ayr) = Ry / R cosZ = cos(Azr) = Rz / R 這 三個(gè)公式通常被稱為方向余弦。 可以看到由 R 和 Rx 組成的直角三角形。 我們感興趣的是向量 R 和 X、 Y、 Z 軸之間的角度，將它們定義為 Axr、 Ayr、 Azr。 Rx， Ry， Rz， 分別 R 在 X， Y， Z 軸上的矢量投影。 那能否使用加速度來(lái)計(jì)算姿態(tài) 角呢？加速度坐標(biāo)模型如圖 所示。有了這三個(gè)角，我們就可以得到當(dāng)前四軸的姿態(tài)，這才是我們想要的結(jié)果。 姿態(tài)解算 在 ， 四旋翼飛行器的飛行方向與速度都是由飛行器的傾角決定的，并且飛行器傾斜的角度越大，飛行速度也就越快。 Z 軸上，瞬時(shí)針轉(zhuǎn)動(dòng)角速度為正，逆時(shí)針轉(zhuǎn)動(dòng)角速度為 負(fù)。 當(dāng) 傳感器 繞 Z 軸轉(zhuǎn)動(dòng) 時(shí) , X、 Y 軸上的角速度因?yàn)殪o止幾乎為 0176。而加速度因?yàn)檗D(zhuǎn)動(dòng)的原因，Y 軸上的加速度絕對(duì)值在減小，而 X 軸上的加速度絕對(duì)值在增大， 而水平上的 Y 坐標(biāo)軸加速度幾乎為 0g。 /sec。而加速度因?yàn)橹亓Φ脑?，豎直向上的軸為 100（放大了 100 倍，方便觀察），即 1g，豎直向下的坐標(biāo)軸為 1g，而水平的坐標(biāo)軸加速度幾乎為 0g。 表 靜止時(shí) MPU6050 原始數(shù)據(jù) 狀態(tài) GYRO_X GYRO_Y GYRO_Z ACCEL_X ACCEL_Y ACCEL_Z MPU6050靜止 Z 軸垂直向上 1 0 0 1 0 100 1 0 0 1 0 100 1 0 0 1 0 100 1 1 0 1 0 99 1 0 0 1 0 99 1 0 0 1 0 100 MPU6050靜止 X 軸垂直向下 1 0 0 100 0 3 1 1 0 100 0 4 1 0 0 100 0 4 1 1 0 100 0 4 1 0 0 100 0 3 1 0 0 100 0 4 MPU6050靜止 Y 軸垂直向下 1 0 0 2 99 3 1 0 0 2 100 3 1 0 0 2 100 4 1 0 0 1 100 4 1 0 0 2 99 4 1 0 0 1 99 4 分析表格可知，當(dāng)靜止時(shí)： X、 Y、 Z 三個(gè)軸上的角速度因?yàn)殪o止幾乎為 0176。采集數(shù)據(jù)并取 100 次平均值后， 把ACCEL_OUT 放大 100 倍，再將全部數(shù)據(jù)發(fā)送到串口助手 6 次，這樣便于觀察和分析。 數(shù)據(jù)為 FF00，原碼是8100，即 256 先讓單片機(jī)接收 MPU6050 原始數(shù)據(jù)，并把數(shù)據(jù)除以對(duì)應(yīng)的分辨率，可將數(shù)據(jù)加速度值單位轉(zhuǎn)換為 g，角速度單位轉(zhuǎn)換為 176。負(fù) 數(shù)是 32768 1 對(duì)應(yīng) 16 進(jìn)制 即是 8000FFFF。/s MPU6050 的數(shù)據(jù)是為 int16 型，數(shù)據(jù)范圍是 32768 到 +32767。2021176。/s LSB/176。/s 2 177。500176。/s 131 LSB/176。 第 22 頁(yè) 西華大學(xué)畢業(yè)設(shè)計(jì)說(shuō)明書(shū) 表 陀螺儀靈敏度最低分辨率 FS_SEL Full Scale Range LSB Sensitivity 0 177。 表 陀螺儀測(cè)量值存儲(chǔ)寄存器 Register(Hex) Register(Decimal) Bit7 Bit6 Bit5 Bit4 Bit3 Bit2 Bit1 Bit0 43 67 GYRO_XOUT[15:8] 44 68 GYRO_XOUT[7:0] 45 69 GYRO_YOUT[15:8] 46 70 GYRO_YOUT[7:0] 47 71 GYRO_ZOUT[15:8] 48 72 GYRO_ZOUT[7:0] 每個(gè) 16 位陀螺儀測(cè)量值的滿量程定義在 MPU6050 的 FS_SEL 寄存器 （寄存器 27）。8 g 4096 LSB/g 3 177。2 g 16384 LSB/g 1 177。 表 加速度計(jì)測(cè)量值存儲(chǔ)寄存器 Register(Hex) Register(Decimal) Bit7 Bit6 Bit5 Bit4 Bit3 Bit2 Bit1 Bit0 3B 59 ACCEL_XOUT[15:8] 3C 60 ACCEL_XOUT[7:0] 3D 61 ACCEL_YOUT[15:8] 3E 62 ACCEL_YOUT[7:0] 3F 63 ACCEL_ZOUT[15:8] 40 64 ACCEL_ZOUT[7:0] 每個(gè) 16 位加速度計(jì) 測(cè)量值的滿量程定義在 MPU6050 的 CCEL_FS 寄存器 （寄存器28）。首先，通過(guò)操作 MPU6050 的寄存器可以獲得飛行時(shí)的線加速度和轉(zhuǎn)動(dòng)角速度原始值。并且在接收到重要的數(shù)據(jù)時(shí)，飛控系統(tǒng)還會(huì)返回一模一樣的數(shù)據(jù)給上位機(jī) ， 表示已經(jīng)正確的收到了這些重要數(shù)據(jù) ，大大增加了飛控系統(tǒng)的安全性 。 圖 上位機(jī)界面圖 因?yàn)槭褂玫?PID 參數(shù)有小數(shù)，所以協(xié)議傳遞的 PID 參數(shù)都是先放大了 1000 倍在進(jìn)行傳輸?shù)?。為保證通信的準(zhǔn)確 性，及時(shí)性，對(duì)于數(shù)據(jù)傳輸編寫(xiě)一個(gè)通信協(xié)議很有必要 。 用 上位機(jī)對(duì)飛行器實(shí)時(shí)控制 ， 上位機(jī) 發(fā)送到飛控系統(tǒng)的數(shù)據(jù)很多，比如想要加減油門(mén)，讓飛行器往哪飛，在調(diào)試的時(shí)候需要隨時(shí)更改 PID 參數(shù)，這也 是 上位機(jī) 要 發(fā)送命令。 飛控系統(tǒng)初始 化 初始化系統(tǒng)參數(shù) 初始化傳感器 通信數(shù)據(jù)處理 串級(jí) PID 控制 調(diào)節(jié)電機(jī)PWM 波輸出 第 20 頁(yè) 西華大學(xué)畢業(yè)設(shè)計(jì)說(shuō)明書(shū) 6 系統(tǒng)調(diào)試 通信 系統(tǒng) 飛行器不同于一般的控制系統(tǒng) ， 由于帶有四個(gè)螺旋槳，并且電機(jī)轉(zhuǎn)速很高 ， 近距離調(diào)試意味著伴隨著安全隱患 。 (8) 獨(dú)立看門(mén)狗： 溢出時(shí)間為 2s，防止系統(tǒng)死機(jī)停止工作。 (6) TIM 定時(shí)器的 PWM 通道：使用 TIM3 和 TIM4 的 PWM 功能輸出 4 路不同占空比的PWM 波給電子調(diào)速器從而控制無(wú)刷電機(jī)的轉(zhuǎn)動(dòng)。 (4) GPIO：用于控制 LED，通過(guò) 不同的閃爍以及亮滅提示飛行器的運(yùn)行狀態(tài)。 (2) USART2： 用于調(diào)試程序、發(fā)送和接收上位機(jī)命令的通信。主程序流程圖如 圖 所示。外部中斷流程圖如圖 所示。串口中斷流程圖如圖 所示。操作界面如圖 所示。 圖 Lab Windows 界面圖 STM32F407 芯片使用 C 語(yǔ)言軟件開(kāi)發(fā)系統(tǒng) KEIL 開(kāi)發(fā)。用于數(shù)據(jù)采集分析和顯示。 Lab windows 是 美國(guó)國(guó)家儀器公司推出的交互式 C語(yǔ)言開(kāi)發(fā)平臺(tái)。 圖 MPU6050 芯片坐標(biāo)軸分布圖 關(guān)于 加速度的測(cè)量其實(shí)是 當(dāng)物體在加速過(guò)程中作用在物體上的力，這里需要注意的是在地球上任何物體都會(huì)受到地球給的重力，芯片也不例外，所以 X,Y,Z 坐標(biāo)軸上 測(cè)得的加速度包含了重力在三個(gè)坐標(biāo)軸上的重力加速度分量。經(jīng)過(guò)芯片內(nèi)部處理后，會(huì)輸出一個(gè)繞坐標(biāo)軸旋轉(zhuǎn)的角速度。具體的講， 陀螺儀 的測(cè)量物理量是偏轉(zhuǎn)、傾斜時(shí)的轉(zhuǎn)動(dòng)角速度和坐標(biāo)軸上的線加速度。 MPU6050 芯片工作電路圖如圖 所示。 ? 高達(dá) 400Khz 的 IIC 通信接口。 ? 陀螺儀工作電流： 5mA，陀螺儀待機(jī)電流： 5uA；加速器工作電流： 500uA，加速 器省電模式電流： 40uA10Hz。 5%； VLOGIC 可低至 177。 5%、 177。 ⑨