四旋翼飛行器論文

1、 四旋翼自主飛行器摘要：四旋翼飛行器是一種結構新穎、性能優(yōu)越的垂直起降飛行器，具有操作靈活、帶負載能力強等特點，具有重要的軍事和民用價值，以及研究價值。在深入了解四旋翼飛行器的研究現狀、關鍵技術與應用前景的基礎之上，根據四旋翼飛行器飛行原理，建立系統(tǒng)動力學模型，確定了系統(tǒng)組成和總體設計方案。首先根據設計方案采購了簡單飛行器機體模型，選擇合適的直流無刷電機作為系統(tǒng)動力裝置，設計線性度良好、功率滿足一定要求的無刷電機驅動器，以滿足四旋翼飛行器帶載飛行需要。選取了功能強大且容易開發(fā)的微處理器、傳感器和相關電子元器件滿足系統(tǒng)需要，并做了大量的系統(tǒng)軟硬件調試工作，最終完成了整體設計。飛行器運動姿態(tài)測量單

2、元主要由慣性測量單元(IMU)和三軸磁傳感器組成，根據其傳感器的性能指標，構建數學模型，從而得到載體準確的航向信息和相應姿勢，為飛行器系統(tǒng)穩(wěn)定飛行提供重要的保障。 四旋翼無人飛行器要實現穩(wěn)定飛行是以平衡控制為前提的，由于該飛行器具有六自由度而只有四個控制量的欠驅動的控制系統(tǒng)，通過調節(jié)四個電機轉速實現飛行器平衡穩(wěn)定飛行，因此平衡控制是四旋翼飛行器運動中的關鍵。根據系統(tǒng)動力學模型設計控制算法，設計四旋翼飛行器控制系統(tǒng)控制規(guī)律，主要包括兩個控制回路：姿態(tài)控制回路、位置控制回路。在仿真軟件平臺上，通過仿真驗證后將算法移植到處理器中，進行控制算法驗證及實驗研究，優(yōu)化飛行控制算法參數。最后，為了滿足四旋翼

3、無人飛行器的穩(wěn)定飛行控制的要求，設計實時性高的控制系統(tǒng)軟件程序，進行相關實驗調試工作。最終設計出能夠實現一鍵飛行、高效的四旋翼自主飛行器。關鍵詞：四旋翼飛行器 瑞薩R5F100LEA PWM 陀螺儀 超聲傳感器Abstract: Four rotor aircraft is a kind of novel structure, superior performance of vertical take-off and landing aircraft, has many characteristics, such as flexible operation and load ability,

4、has the important value of military and civilian, and research value. In understanding the research status of four rotor aircraft, the key technology and the application prospect, according to the principle of four rotor aircraft flight, establish a system dynamics model, the system composition and

5、the overall design scheme is determined. First according to the design plan to purchase the simple vehicle body model, select the appropriate brushless dc motor as the power unit system, design good linearity and power meet certain requirements of brushless motor drive, to meet the needs of the four

6、 rotor aircraft flying on load. Selected the powerful and easy development of microprocessors, sensors and related electronic components meet the needs of the system, and made a lot of system hardware and software debugging, finally completed the overall design. Aircraft motion measurement unit is m

7、ainly composed of inertial measurement unit (IMU) and a three-axis magnetic sensor, according to the sensor performance index, build the mathematical model of the carrier to get the exact course information and the corresponding position, provide important guarantee for aircraft flying system stable

8、. Four rotor unmanned aerial vehicles to achieve stable flight is the premise of the balance control, due to the aircraft has six degrees of freedom only four control of underactuated control system, by adjusting the motor speed to achieve four aircraft flying balanced and stable, so the balance con

9、trol is key in the four rotor aircraft movement. On the simulation software platform, through the simulation verify the algorithm after transplantation into the processor, verifies the control algorithm and experimental research, optimization of flight control algorithm parameters. Finally, in order

10、 to meet the requirements of the stability of the four rotor unmanned spacecraft flight control, high real-time performance of control system software design, carries on the related experimental debugging. A key to achieve ultimately designed to fly, high efficient and automatic four rotor aircraft.

11、Keywords: four rotor aircraft renesas R5F100LEA PWM gyroscope ultrasonic sensors 目錄一、系統(tǒng)方案比較與設計6二、單元電路設計與計算7 1. 四旋翼飛行器飛行模式72. 系統(tǒng)硬件芯片的選擇133、飛行姿態(tài)的測量154、超聲波傳感器高度控制155、電源模塊166、硬件電路圖16三、程序設計17四、系統(tǒng)測試181、基本部分測試182、測試方法183、測試結果18五、結論 19六、參考文獻19七、附錄201、儀器設備清單202、程序清單201主函數203、子函數21一、系統(tǒng)方案比較與設計 1.方案一：以瑞薩R5F100L

12、EA為核心控制超聲波測距系統(tǒng)，通過主控芯片記錄超聲波發(fā)射的時間和收到反射波的時間，當收到超聲波的反射波時，接受電路利用PWM調制法，使飛行器根據不同高度，控制脈沖占空比，從而控制速度。該方法較易實現，僅由電機直接驅動，電路簡單，利于一鍵起飛。 2.方案二：考慮以上方案，受外力影響大，稍有傾斜，飛行器將失去平衡。為了達到控制飛行器平衡穩(wěn)定飛行，外加 MPU6050模塊，MPU-6050中陀螺儀和加速度計分別用了三個16位的ADC，將其測量的模擬量轉化為數字量，這些數字量輸入到主控芯片，結合精密算法，調節(jié)脈沖占空比，調節(jié)四個電機轉速來改變旋翼轉速，實現升力的變化，從而控制飛行器的姿態(tài)和位置。 通過

13、比較兩種方案，確定了使用方案二，雖然算法復雜，但困難是可能被克服的，能夠完成基本任務；而方案一，算法粗糙，一旦采用，面臨的各種問題是難以想象的，所以確立為方案二。二、單元電路設計與計算 1、帶防撞的四旋翼飛行器 1.1 四旋翼飛行器飛行模式 四旋翼飛行器是一種由固連在剛性十字交叉結構上的4個電機驅動的一種飛行器。飛行器動作依靠4個電機的轉速差進行控制，其機械結構相對簡單，可由電機直接驅動，無需復雜的傳動裝置，便于微型化。相較于典型的傳統(tǒng)直升機，僅僅裝配有一個主轉子和一個尾槳，通過控制舵機來改變螺旋槳的槳距角，從而控制直升機的姿態(tài)和位置的飛行模式，四旋翼飛行器是通過調節(jié)四個電機轉速來改變旋翼轉速

14、，實現升力的變化，從而控制飛行器的姿態(tài)和位置。由于飛行器是通過改變旋翼轉速實現升力變化，按照旋翼布置方式可分為十字模式和X模式，如圖l所示，四旋翼飛行器只有四個輸入力，同時卻有六個狀態(tài)輸出，是一種六自由度的垂直升降機，因此這種欠驅動系統(tǒng)非常適合靜態(tài)和準靜態(tài)條件下飛行。 對于姿態(tài)測量和控制的編程算法來說，十字模式較X模式簡單，前者的飛行模式是通過只改變單個機翼轉速的方法而完成前后左右四個方向的飛行；而后者的飛行模式是通過同時控制兩個機翼轉速的方法而完成前后左右四個方向的飛行。十字模式算法簡單，容易操作，飛行平穩(wěn)。X模式算法復雜，可操作性不強。綜上所述，采用了十字模式算法。前 圖1 十字模式算法

15、1.2四旋翼飛行器產生基本動作的原理 四旋翼飛行器結構形式如圖2所示，電機1 和電機3 逆時針旋轉的同時，電機2 和電機4 順時針旋轉，因此當飛行器平衡飛行時，陀螺效應和空氣動力扭矩效應均被抵消。與傳統(tǒng)的直升機相比，四旋翼飛行器有下列優(yōu)勢：各個旋翼對機身所施加的反扭矩與旋翼的旋轉方向相反，因此當電機1 和電機3逆時針旋轉的同時，電機2和電機4順時針旋轉，可以平衡旋翼對機身的反扭矩。 四旋翼飛行器在空間共有6個自由度（分別沿3個坐標軸作平移和旋轉動作），這6個自由度的控制都可以通過調節(jié)不同電機的轉速來實現。圖2 四旋翼飛行器的結構形式 基本運動狀態(tài)分別是：<1>垂直運動；<2&

16、gt;俯仰運動；<3>滾轉運動；<4>偏航運動；<5>前后運動；<6>側向運動。在圖3中，電機1和電機3作逆時針旋轉，電機2和電機4作順時針旋轉，規(guī)定沿x軸正方向運動稱為向前運動，箭頭在旋翼的運動平面上方表示此電機轉速提高，在下方表示此電機轉速下降。圖3（a）垂直運動：垂直運動相對來說比較容易。在圖中，因有兩對電機轉向相反，可以平衡其對機身的反扭矩，當同時增加四個電機的輸出功率，旋翼轉速增加使得總的拉力增大，當總拉力足以克服整機的重量時，四旋翼飛行器便離地垂直上升；反之，同時減小四個電機的輸出功率，四旋翼飛行器則垂直下降，直至平衡落地，實現了沿z

17、軸的垂直運動。當外界擾動量為零時，在旋翼產生的升力等于飛行器的自重時，飛行器便保持懸停狀態(tài)。保證四個旋翼轉速同步增加或減小是垂直運動的關鍵。 圖3（b）俯仰運動：電機1的轉速上升，電機3的轉速下降，電機2、電機4的轉速保持不變。為了不因為旋翼轉速的改變引起四旋翼飛行器整體扭矩及總拉力改變，旋翼1與旋翼3轉速該變量的大小應相等。由于旋翼1的升力上升，旋翼3的升力下降，產生的不平衡力矩使機身繞y軸旋轉（方向如圖所示），同理，當電機1的轉速下降，電機3的轉速上升，機身便繞y軸向另一個方向旋轉，實現飛行器的俯仰運動。 圖3（c）滾轉運動：與圖b的原理相同，在圖c中，改變電機2和電機4的轉速，保持電機1

18、和電機3的轉速不變，則可使機身繞x軸旋轉（正向和反向），實現飛行器的滾轉運動。 圖3（d）偏航運動：四旋翼飛行器偏航運動可以借助旋翼產生的反扭矩來實現。旋翼轉動過程中由于空氣阻力作用會形成與轉動方向相反的反扭矩，為了克服反扭矩影響，可使四個旋翼中的兩個正轉，兩個反轉，且對角線上的來年各個旋翼轉動方向相同。反扭矩的大小與旋翼轉速有關，當四個電機轉速相同時，四個旋翼產生的反扭矩相互平衡，四旋翼飛行器不發(fā)生轉動；當四個電機轉速不完全相同時，不平衡的反扭矩會引起四旋翼飛行器轉動。在圖d中，當電機1和電機3的轉速上升，電機2和電機4的轉速下降時，旋翼1和旋翼3對機身的反扭矩大于旋翼2和旋翼4對機身的反扭

19、矩，機身便在富余反扭矩的作用下繞z軸轉動，實現飛行器的偏航運動，轉向與電機1、電機3的轉向相反。 圖3（e）前后運動：要想實現飛行器在水平面內前后、左右的運動，必須在水平面內對飛行器施加一定的力。在圖e中，增加電機3轉速，使拉力增大，相應減小電機1轉速，使拉力減小，同時保持其它兩個電機轉速不變，反扭矩仍然要保持平衡。按圖b的理論，飛行器首先發(fā)生一定程度的傾斜，從而使旋翼拉力產生水平分量，因此可以實現飛行器的前飛運動。向后飛行與向前飛行正好相反。當然在圖b圖c中，飛行器在產生俯仰、翻滾運動的同時也會產生沿x、y軸的水平運動。 圖3（f）傾向運動：由于結構對稱，所以傾向飛行的工作原理與前后運動完全

20、一樣。圖3 四旋翼飛行器沿各自由度的運動 1.3 四旋翼直升機的動力學原理 不像普通的直升機有可變螺距角度，四旋翼直升機由固定俯仰角轉子和轉子速度控制，以產生預期的升力。四旋翼的基本運動可以用圖4來描述。圖4 四旋翼三維受力圖 直升機的垂直運動可以在同一時間通過改變所有的轉子速度來實現。沿著X軸的運動與在Y軸方向上傾斜有關。這種傾斜可通過降低旋翼1、2的速度，增加旋翼3、4的速度來實現。這種傾斜也可以沿X軸的加速度。類似的，沿著Y軸的運動與在X軸方向上傾斜有關。偏航運動是利用旋翼產生的力矩來實現的。常規(guī)直升機有尾槳，以平衡由主旋翼產生的力矩。但在四旋翼的情況下，旋翼的旋轉方向是用來平衡和減少這

21、些力矩的。這也可以被用來產生預期的偏航運動。為了在順時針方向上轉向，必須增加旋翼2、4的速度以克服旋翼1和3產生的力矩。因此一個好的控制器應能達到預期的偏航角，同時保持固定的傾斜角度和高度。假設一個固定在支架上的結構在該直升機重心處，此處Z軸是指向上的，它的身體軸心是與慣性系的位置矢量（x,y,z)和三個分別代表了俯仰滾轉與偏航的歐拉角（，），有關的式1用來表示旋 轉在上式中和分別表示了和每個旋翼產生的力矩相當于縱向的力。這些力矩已通過實驗觀察到在低速情況下與力是呈線性的。這里有4個輸入的力與6個輸出的參數（）因此直升機是一個欠驅動系統(tǒng)。兩個旋翼的旋轉方向是順時針方向，另外兩個是逆時針的，為了

22、平衡力矩并且實現所需要的偏航運動。根據力和力矩平衡所得方程如下：上面的是阻尼系數，接下來我們假設阻力為0，因為阻力在低速時可以忽略的。為了簡單起見，我們定義輸入為：是對于軸的轉動慣量，是力與力矩的比例因子。表示了在Z軸方向上的加速度，、表示the roll and pitch inputs表示了一個偏航力矩。因此運動方程變成了：假設重心在對角線的交點處，若重心向上（或向下）移動d單位，則角速度對力更不敏感，從而穩(wěn)定性增加了。使旋翼的力指向中心同樣增加穩(wěn)定性，同時減少 The roll and pitch moments和垂直方向上的總推力。2. 系統(tǒng)硬件芯片的選擇2.1 飛行器控制板主控芯片R

23、5F100LEA，如圖5所示。圖5 主控芯片R5F100LEA引腳圖 主控芯片R5F100LEA是瑞薩公司生產的一款單片機芯片，采用64引腳LQFP封裝，64KB的ROM容量，擁有Flash存儲器。本產品相較于C8051有以下特點：擁有15個端口，采用引腳復用方式，既節(jié)省了引腳資源，又拓展了功能。適合在復雜應用且微型飛行器上應用。2.2 MPU 6050 如圖6圖6 MPU6050引腳圖MPU-6050為全球首例整合性6軸運動處理組件，采用最小最薄包裝 (4x4x0.9mm QFN) 符合RoHS及環(huán)境標準，相較于多組件方案，免除了組合陀螺儀與加速器時之軸間差的問題，減少了大量的包裝空間。MP

24、U-6000（6050）整合了3軸陀螺儀、3軸加速器，并含可藉由第二個I2C端口連接其他廠牌之加速器、磁力傳感器、或其他傳感器的數位運動處理(DMP: Digital Motion Processor)硬件加速引擎，由主要I2C端口以單一數據流的形式，具有向應用端輸出完整的9軸融合演算技術InvenSense的運動處理資料庫，可處理運動感測的復雜數據，降低了運動處理運算對操作系統(tǒng)的負荷，并為應用開發(fā)提供架構化的API等功能。MPU-6000（6050）的角速度全格感測范圍為±250、±500、±1000與±2000°/sec (dps)，可準確

25、追蹤快速與慢速動作，并且，用戶可程式控制的加速器全格感測范圍為±2g、±4g±8g與±16g。產品傳輸可透過最高至400kHz的IC或最高達20MHz的SPI（MPU-6050沒有SPI）。MPU-6000可在不同電壓下工作，VDD供電電壓介為2.5V±5%、3.0V±5%或3.3V±5%，邏輯接口VVDIO供電為1.8V± 5%（MPU6000僅用VDD）。MPU-6050中陀螺儀和加速度計分別用了三個16位的ADC，將其測量的模擬量轉化為數字量。 2.3 HC-SR04 如圖7圖7 HC-SR04實物圖 HC-

26、SR04超聲波測距模塊包括超聲波發(fā)射器、接收器與控制電路，性能穩(wěn)定，測度距離精確，模塊高精度，盲區(qū)小。具有可提供2cm-400cm的非接觸式距離感測功能，測距精度可達高到3mm?；竟ぷ髟恚?1)采用IO口TRIG觸發(fā)測距，給至少10us的高電平信號;(2)模塊自動發(fā)送8個40khz的方波，自動檢測是否有信號返回；(3)有信號返回，通過IO口ECHO輸出一個高電平，高電平持續(xù)的時間就是超聲波從發(fā)射到返回的時間。測試距離=(高電平時間*聲速(340M/S)/2。3、飛行姿態(tài)的測量 對無人機的控制來說測量無人機的飛行姿態(tài)是勢必不可少。我們用一個慣性測量裝置（陀螺儀mpu6050）來執(zhí)行這一任務。

27、這架旋翼機的姿態(tài)是確定利用互補濾波器為每個旋轉軸。該過濾器的工程計算誤差信號之間的估計角（），并參考角（）在直接從加速度計算。在積分前，這個錯誤信號減去了初始角速度信號。應該指出的是，比率傳感器可以測量在體內固定軸，而加速度測量傾斜，在地球上的固定軸。4、超聲波傳感器高度控制 超聲波傳感器高度控制是通過PID閉環(huán)控制算法實現的，這種控制采用閉合反饋，通過超聲波傳感器發(fā)射的時間和收到反射波的時間差，計算距離，反饋給主控芯片，同時控制四個電機的轉速。PID是比例(P)、積分(I)、微分(D)控制算法。但并不是必須同時具備這三種算法，也可以是PD,PI,甚至可以只有P算法控制。采用了以上三種算法比例

28、(P)、積分(I)、微分(D)控制超聲波傳感器反饋的脈沖，以控制四個電機的轉速，越過障礙。積分，反應系統(tǒng)的累計偏差，使系統(tǒng)消除穩(wěn)態(tài)誤差，提高無差度，因為有差，積分調節(jié)就進行，直至無誤差；微分，具有預見性，能預見偏差變化的趨勢，產生超前的控制作用，在偏差還沒有形成之前，已被微分調節(jié)作用消除，因此可以改善系統(tǒng)的動態(tài)性能。但是微分對噪聲干擾有放大作用，加強微分對系統(tǒng)抗干擾不利積分和微分都不能單獨起作用，必須與比例控制配合。采用比例控制規(guī)律能較快地克服擾動的影響，它的作用于輸出值較快，但不能很好穩(wěn)定在一個理想的數值，不良的結果是雖較能有效的克服擾動的影響，但有余差出現。5、電源模塊 為四旋翼飛行器飛行

29、控制系統(tǒng)、無刷電機等提供電源。由于四旋翼飛行器載重量有限，所以電源系統(tǒng)應該盡可能 輕，能量密度大。考慮到線性穩(wěn)壓方式轉換效率不高，能量損耗大的缺點，本系統(tǒng)改用開關電源的方式來穩(wěn)壓。但開關電源會產生較強的電磁干擾，而且存在電源波紋，這樣對PPM的遙控方式影響就很大。6、硬件電路圖 整體連接電路如圖8圖8 硬件連接電路圖三、程序設計 程序流程圖 如圖9圖 9 流程圖 四、系統(tǒng)測試 1、基本部分測試 測試儀器：秒表、數字萬用表、示波器、R5F100LEA仿真機等。 2、測試方法： 數字萬用表主要用來測試分立元件的電阻、壓降、漏電流、截止/導通狀態(tài)等參數； 示波器用于測試各PWM波形； 秒表用于產品測

30、試，按照基本要求對制成的四旋翼飛行器進行飛行時間的測量； R5F100LEA仿真器用來測試軟件； 3、測試結果： 測試結果顯示本系統(tǒng)以實現需求中基本功能，能滿足在時間限度里飛行所要求的距離，不足之處在于系統(tǒng)功能相對不夠完善，在傳感器模塊還需要進行完善。此次測試目的在于發(fā)現系統(tǒng)缺陷與漏洞，從測試結果中沒有發(fā)現嚴重系統(tǒng)缺陷。五、結論 本系統(tǒng)完成了硬件電路的設計并結合了軟件開發(fā)程序，使四旋翼飛行器能夠一鍵啟動后，平衡穩(wěn)定的飛行任務。通過各種方案的討論和嘗試，在經過多次的整體軟硬件結合調試，不斷地對系統(tǒng)進行優(yōu)化，設計采用簡單的動力學原理結合軟件開發(fā)平臺，就能夠簡單高效的完成任務，若采用其他類型的功能傳

31、感器，功能會更加強大，且若將算法設計精準，功能完成會更加流暢。六、參考文獻黃智偉.全國大學生電子設計競賽訓練教程.電子工業(yè)出版社，2010李曉林.單片機原理與接口技術.電子工業(yè)出版社，2010黃智偉.全國大學生電子設計競賽訓練教程.電子工業(yè)出版社，2010黃智偉.全國大學生電子設計競賽訓練教程.電子工業(yè)出版社，2010J.C Zufferey, D. Floreano Evolving Vision-Based Flying Robots. Proceedings of the 2nd International Workshop on Biologically Motivated Compu

32、ter Vision, LNCS 2525, pp. 592-600, Berlin, Springer-Verlag,2002. A. Elfes, S.S. Bueno and al, Robotic Airship for Exploration of planetary Bodies with an Atmosphere Autonomy Challenges. Autonomous Robots Journal: Kluwer Academic Publishers, 2003. S. Sastry, A mathematical introduction to robotic ma

33、nipulation . Boca Raton, FL, 1994. P. Mllhaupt, Analysis and Control of Underactuated Mechanical Nonminimum-phase Systems. Phd thesis, Department of MechanicalEngineering, EPFL, 1999. 七、附錄 1、儀器設備清單序號名稱型號數量 備注1四旋翼飛行器12MCU R5F100LEA116位微處理器3陀螺儀 MPU-605014超聲傳感器 HC-SR0412、程序清單1主函數void main(void) R_MAIN_

34、UserInit(); /* Start user code. Do not edit comment generated here */ /*?*/ while(get_systemtimer()<5000);/wait 5 sec/clr_systemtimer(); curatt_init(); power=750; delaytime=0; while (1U) task_mpu6050(); if(OK=get_mpu6050_mode() atttitude_calculate(4000,fgyrox,fgyroy,fgyroz); if(flag!=0) flag=0; e

35、lse flag=1; q4_to_euler_rpy(&curattX,&currrpy); euler_control(&currrpy,&target);control_task(power);delaytime+;if(delaytime>500)delaytime=600;/power=0; ; /* End user code. Do not edit comment generated here */3、子函數1.#include "mytype.h"#include "math.h"Q4 curatt

36、X=1,0,0,0;RPY currrpy=0,0,0;float math_rsqrt(float number) long i; float x2, y; x2 = number * 0.5F; y = number; i = * ( long * ) &y; / evil floating point bit level hacking i = 0x5f3759df - ( i >> 1 ); / what the fuck y = * ( float * ) &i; y = y * ( 1.5F - ( x2 * y * y ) ); / 1st itera

37、tion return y;void curatt_init(void)curattX.w = 1;curattX.x = 0;curattX.y = 0;curattX.z = 0;void atttitude_calculate(unsigned int timer_us,float gx,float gy,float gz)float a_rsqrt; float qa,qb,qc;float timesec=timer_us*0.000001; gx *= (0.5f * timesec); / pre-multiply common factorsgy *= (0.5f * time

38、sec);gz *= (0.5f * timesec);qa = curattX.w;qb = curattX.x;qc = curattX.y;curattX.w += (-qb * gx - qc * gy - curattX.z * gz);curattX.x += (qa * gx + qc * gz - curattX.z * gy);curattX.y += (qa * gy - qb * gz + curattX.z * gx);curattX.z += (qa * gz + qb * gy - qc * gx); a_rsqrt = math_rsqrt(curattX.w*c

39、urattX.w + curattX.x*curattX.x + curattX.y*curattX.y + curattX.z*curattX.z); curattX.w *= a_rsqrt; curattX.x *= a_rsqrt; curattX.y *= a_rsqrt; curattX.z *= a_rsqrt;void q4_to_euler_rpy(Q4 *dat,RPY* ang) float gx, gy, gz,x_2,w_2; gx = 2 * (dat->x*dat->z - dat->w*dat->y); gy = 2 * (dat->

40、;w*dat->x + dat->y*dat->z); x_2 = dat->x*dat->x; w_2 = dat->w*dat->w; gz = w_2 -x_2 - dat->y*dat->y + dat->z*dat->z; gz*= gz; ang->yaw = -1*atan2(2*dat->x*dat->y - 2*dat->w*dat->z, 2*w_2 + 2*x_2 - 1);/ * 180 / PI; ang->roll =-1* atan(gx* math_rsqrt(

41、gy*gy + gz);/ * 180 / PI; ang->pitch = atan(gy* math_rsqrt(gx*gx + gz);/ * 180 / PI;2.static void updat_pid(PIDP* pid)float ilimit1=pid->ilimit*(-1);pid->istate+=(pid->errdat)*pid->iGain;if(pid->istate>pid->ilimit)pid->istate=pid->ilimit;if(pid->istate<ilimit1)pid

42、->istate=ilimit1;pid->pidout = pid->pGain*pid->errdat+pid->istate+(pid->errdat-pid->perr)*pid->dGain; pid->perr=pid->errdat;static void pid_init(PIDP* pid)pid->istate=0;pid->perr=0;pid->errdat=0;pid->pidout=0;void control_init(void)curatt_init();check_mpu605

43、0();pid_init(&Xaxespid);pid_init(&Yaxespid);pid_init(&Zaxespid);pid_init(&XGpid);pid_init(&YGpid);pid_init(&ZGpid);/*功能：X型 馬達分配驅動分配*/enumL_F,L_B,R_B,R_F;static void x_mode_updat_motodat(S16 basedat,S16 Xaxesdat,S16 Yaxesdat,S16 Zaxesdat)S16 pwm4;pwmL_F=basedat + Xaxesdat + Yaxesdat - Zaxesdat;pwmL_B=basedat - Xaxesdat + Yaxesdat + Zaxesdat;pwmR_B=basedat - Xaxesdat - Yaxesdat - Zaxesdat;pwmR_F=basedat +