久久久久久久999_99精品久久精品一区二区爱城_成人欧美一区二区三区在线播放_国产精品日本一区二区不卡视频_国产午夜视频_欧美精品在线观看免费

<dfn id="48mgm"></dfn>

<li id="48mgm"><option id="48mgm"></option></li>

<strike id="48mgm"></strike>

<tr id="48mgm"></tr><tr id="48mgm"></tr>

<noscript id="48mgm"></noscript>

<dfn id="48mgm"></dfn>

<li id="48mgm"><acronym id="48mgm"></acronym></li>

<kbd id="48mgm"></kbd>

標(biāo)題: STC51單片機(jī)四軸飛控開(kāi)源項(xiàng)目原理圖與源碼資料分享 [打印本頁(yè)]

---

作者: openswan    時(shí)間: 2017-11-26 11:19
標(biāo)題: STC51單片機(jī)四軸飛控開(kāi)源項(xiàng)目原理圖與源碼資料分享
四軸飛控-STC8A8K64S4A12-LQFP44-PPM-V10，來(lái)源于STC發(fā)布，按需取用
控制信號(hào)排針要靠在一起布線(xiàn)成3排針3*9*2.54
P1~P4對(duì)應(yīng)接收機(jī)的俯仰、橫滾、油門(mén)、航向PPM信號(hào)。M1~M4輸出到4個(gè)電調(diào)的信號(hào)。P5備用。
四個(gè)電機(jī)安排順序如下：

串口1: 下載、調(diào)試、控制信號(hào)輸入.
串口2: 備用為GPS接口或數(shù)傳接口.


原理圖：



飛控的stc單片機(jī)程序源碼：

1. /* 本程序經(jīng)過(guò)測(cè)試完全正常, 不提供電話(huà)技術(shù)支持, 如不能理解, 請(qǐng)自行補(bǔ)充相關(guān)基礎(chǔ).  */
   
2. 
   
3. /***  特別注意: 下載時(shí)選擇內(nèi)部時(shí)鐘24MHZ, 設(shè)置用戶(hù)EEPROM大小為2K或以上.  ****/
   
4. 
   
5. /*********************************************
   
6.   四軸飛控-V10.C
   
7. 
   
8. 使用遙控接收器型號(hào): MC6B六通道2.4G 100mW.
   
9. 
   
10. 四軸上電待機(jī)：上電后，航燈不亮，接收機(jī)LED閃爍，此時(shí)打開(kāi)遙控器，將左右油門(mén)下拉到最小，接收機(jī)收到信號(hào)LED常亮，
    
11.               表示RF通訊已連接。此時(shí)蜂鳴器"嗶"一聲，航燈閃爍，表示待機(jī)模式。
    
12. 
    
13. 四軸啟動(dòng)：將遙控器左右操縱桿掰成下內(nèi)八，啟動(dòng)四軸，四軸"嗶"一聲，4個(gè)螺旋槳開(kāi)始低速旋轉(zhuǎn)，航燈常亮。
    
14.           此后提升油門(mén)，就可以加速螺旋槳，直到起飛。
    
15. 
    
16. 四軸飛行：起飛后，可以操縱右手的俯仰、橫滾操縱桿，實(shí)現(xiàn)前后左右或任意方向的飛行。
    
17.           左手油門(mén)桿左掰是航向逆時(shí)針轉(zhuǎn)，右掰是航向順時(shí)鐘轉(zhuǎn)。
    
18. 
    
19. 四軸下降停止：收油門(mén)，四軸逐漸下降到地面，然后兩操縱桿掰成下外八，停止四軸，重新處于待機(jī)模式。
    
20. 
    
21. 四軸水平校準(zhǔn)：將四軸放置于水平地面，處于待機(jī)模式，然后兩操縱桿掰成上內(nèi)八，四軸"嗶"一聲進(jìn)入校準(zhǔn)，完成后"嗶嗶"兩聲完成校準(zhǔn)。
    
22. 
    
23. 四軸取消水平校準(zhǔn)：將四軸放置于水平地面，處于待機(jī)模式，然后兩操縱桿掰成上外八，四軸"嗶"一聲取消校準(zhǔn)。取消水平校準(zhǔn)或未進(jìn)行水平校準(zhǔn)過(guò)的四軸，起飛時(shí)即使無(wú)風(fēng)也可能會(huì)有明顯漂移。
    
24. 
    
25. 電池低壓報(bào)警：當(dāng)電池低壓時(shí)，蜂鳴器"嗶嗶"報(bào)警，同時(shí)航燈閃爍，此時(shí)請(qǐng)盡快回航降落。
    
26. 
    
27. 無(wú)遙控信號(hào)異常：當(dāng)四軸在空中突然收不到遙控信號(hào)時(shí)，四軸蜂鳴器發(fā)出"嗶嗶嗶"報(bào)警，同時(shí)航燈閃爍，四軸保持水平，逐漸自動(dòng)減小油門(mén)降落。
    
28. 
    
29. ***********************************************/
    
30. 
    
31. #define                Baudrate1                        115200UL
    
32. #define                TX1_LENGTH        128
    
33. #define                RX1_LENGTH        128
    
34. 
    
35. 
    
36. #include "config.h"
    
37. #include "STC8xxx_PWM.H"
    
38. #include "MPU6050.H"
    
39. #include "AD.H"       
    
40. #include "EEPROM.H"
    
41. #include "PCA.h"
    
42. #include <math.H>
    
43. 
    
44. sbit        P_Light  = P5^4;        //航燈
    
45. sbit        P_BUZZER = P5^5;        //蜂鳴器
    
46. 
    
47. 
    
48. int                xdata g_x=0,g_y=0,g_z=0;                                        //陀螺儀矯正參數(shù)
    
49. float        xdata a_x=0,a_y=0;                                                        //角度矯正參數(shù)
    
50. float        data  AngleX=0,AngleY=0;                                        //四元數(shù)解算出的歐拉角
    
51. float        xdata Angle_gx=0,Angle_gy=0,Angle_gz=0;                //由角速度計(jì)算的角速率(角度制)
    
52. float        xdata Angle_ax=0,Angle_ay=0,Angle_az=0;                //由加速度計(jì)算的加速度(弧度制)
    
53. float        xdata Ax=0,Ay=0,Az=0;                                                //加入遙控器控制量后的角度   
    
54. float        data PID_x=0,PID_y=0,PID_z=0;                                //PID最終輸出量
    
55. int                data  speed0=0,speed1=0,speed2=0,speed3=0;        //電機(jī)速度參數(shù)
    
56. int                data  PWM0=0,PWM1=0,PWM2=0,PWM3=0;//,PWM4=0,PWM5=0;                        //加載至PWM模塊的參數(shù)
    
57. 
    
58. int                int_tmp;
    
59. u8                YM=0,FRX=128,FRY=128,FRZ=128;                                //4通道遙控信號(hào).
    
60. u8                xdata        tp[16];                //讀MP6050緩沖
    
61. 
    
62. 
    
63. //****************姿態(tài)處理和PID*********************************************
    
64. 
    
65. float xdata Out_PID_X=0,Last_Angle_gx=0;                                        //外環(huán)PI輸出量  上一次陀螺儀數(shù)據(jù)
    
66. float xdata ERRORX_Out=0,ERRORX_In=0;                        //外環(huán)P  外環(huán)I  外環(huán)誤差積分
    
67. float xdata Out_PID_Y=0,Last_Angle_gy=0;
    
68. float xdata ERRORY_Out=0,ERRORY_In=0;            //規(guī)則1:內(nèi)外環(huán)P乘積等于10.5
    
69. 
    
70. float xdata Last_Ax=0,Last_Ay=0,Last_Az=0;
    
71. 
    
72. 
    
73. /******************************************************************************/
    
74. #define        Out_XP        6.65f        //ADC0        外環(huán)P        V1 / 10
    
75. #define        Out_XI        0.0074f        //ADC4        外環(huán)I        V2 / 10000
    
76. #define        Out_XD        6.0f        //ADC5        外環(huán)D        V3 / 10
    
77. 
    
78. #define        In_XP        0.8275f        //ADC6        內(nèi)環(huán)P        V4 / 100
    
79. #define        In_XI        0.0074f        //ADC4        內(nèi)環(huán)I        V2 / 10000
    
80. #define        In_XD        6.0f        //ADC5        內(nèi)環(huán)D        V3 / 10
    
81. 
    
82. 
    
83. #define        Out_YP        Out_XP
    
84. #define        Out_YI        Out_XI
    
85. #define        Out_YD        Out_XD
    
86. 
    
87. #define        In_YP        In_XP
    
88. #define        In_YI        In_XI
    
89. #define        In_YD        In_XD
    
90. 
    
91. 
    
92. #define        ZP        5.0f
    
93. #define        ZI        0.1f
    
94. #define        ZD        4.0f        //自旋控制的P D
    
95. float Z_integral=0;//Z軸積分
    
96. 
    
97. #define        ERR_MAX        500
    
98. //======================================================================
    
99. 
    
100. 
     
101. u8        data YM_LostCnt=0, Lost16S; //上一次RxBuf[0]數(shù)據(jù)(RxBuf[0]數(shù)據(jù)在不斷變動(dòng)的)   狀態(tài)標(biāo)識(shí)
     
102. u8        SW2_tmp;
     
103. 
     
104. 
     
105. //======================================================================
     
106. bit        B_8ms;        //8ms標(biāo)志
     
107. 
     
108. bit        B_rtn_ADC0;        //請(qǐng)求返回信息
     
109. bit        B_BAT_LOW;        //低電壓標(biāo)志
     
110. u8        xdata cnt_ms;                //時(shí)間計(jì)數(shù)
     
111. 
     
112. 
     
113. u8                xdata UART1_cmd=0;        //串口命令
     
114. u8                xdata TX1_Read=0;        //發(fā)送讀指針
     
115. u8                xdata TX1_Write=0;        //發(fā)送寫(xiě)指針
     
116. u8                xdata TX1_cnt=0;        //發(fā)送計(jì)數(shù)
     
117. u8                 xdata TX1_Buffer[TX1_LENGTH];        //發(fā)送緩沖
     
118. bit                B_TX1_Busy;                        //發(fā)送忙標(biāo)志
     
119. u8                 xdata RX1_Cnt,RX1_Timer;
     
120. u8                 xdata RX1_Buffer[RX1_LENGTH];
     
121. bit         B_RX1_OK;
     
122. 
     
123. 
     
124. u8                xdata Cal_Setp=0;                        //校準(zhǔn)步驟
     
125. u8                xdata Cal_cnt=0;                        //校準(zhǔn)平均值計(jì)數(shù)
     
126. int                xdata x_sum,y_sum,z_sum;        //校準(zhǔn)累加和
     
127. float        xdata float_x_sum,float_y_sum;        //校準(zhǔn)累加和
     
128. 
     
129. u8        xdata BuzzerOnTime,BuzzerOffTime,BuzzerRepeat,BuzzerOnCnt,BuzzerOffCnt;
     
130. u8        xdata cnt_100ms;
     
131. 
     
132. 
     
133. /* =================== PPM接收相關(guān)變量 ========================== */
     
134. u16        xdata CCAP0_RiseTime;                //捕捉到的上升沿時(shí)刻
     
135. u8        xdata PPM1_Rise_TimeOut;        //高電平限時(shí)
     
136. u8        xdata PPM1_Rx_TimerOut;                //接收超時(shí)計(jì)數(shù)
     
137. u8        xdata PPM1_RxCnt;                        //接收次數(shù)計(jì)數(shù)
     
138. u16        xdata PPM1_Cap;                                //捕捉到的PPM脈沖寬度
     
139. bit        B_PPM1_OK;                                        //接收到一個(gè)PPM脈沖寬度
     
140. 
     
141. u16        xdata CCAP1_RiseTime;
     
142. u8        xdata PPM2_Rise_TimeOut;        //高電平限時(shí)
     
143. u8        xdata PPM2_Rx_TimerOut;
     
144. u8        xdata PPM2_RxCnt;
     
145. u16        xdata PPM2_Cap;
     
146. bit        B_PPM2_OK;
     
147. 
     
148. u16        xdata CCAP2_RiseTime;
     
149. u8        xdata PPM3_Rise_TimeOut;        //高電平限時(shí)
     
150. u8        xdata PPM3_Rx_TimerOut;
     
151. u8        xdata PPM3_RxCnt;
     
152. u16        xdata PPM3_Cap;
     
153. bit        B_PPM3_OK;
     
154. 
     
155. u16        xdata CCAP3_RiseTime;
     
156. u8        xdata PPM4_Rise_TimeOut;        //高電平限時(shí)
     
157. u8        xdata PPM4_Rx_TimerOut;
     
158. u8        xdata PPM4_RxCnt;
     
159. u16        xdata PPM4_Cap;
     
160. bit        B_PPM4_OK;
     
161. 
     
162. u16        xdata CCAP_FallTime;
     
163. 
     
164. u8        PPM1,PPM2,PPM3,PPM4;
     
165. bit        B_Start;
     
166. u8        cnt_start;
     
167. 
     
168. /* ============================================= */
     
169. 
     
170. 
     
171. void        UART1_config(void);
     
172. void         PrintString1(u8 *puts);        //發(fā)送一個(gè)字符串
     
173. void        TX1_write2buff(u8 dat);        //寫(xiě)入發(fā)送緩沖，指針+1
     
174. void        TX1_int_value(int i);
     
175. void        delay_ms(u8 ms);
     
176. void        Return_Message(void);
     
177. u16         MODBUS_CRC16(u8 *p,u8 n);        //input:        *p--->First Data Address,n----->Data Number,        return:        CRC16
     
178. void        PCA_config(void);
     
179. void         Timer0_Config(void);
     
180. void         Timer1_Config(void);
     
181. void        return_TTMx(u8 id,PPMx);
     
182. void         Timer0_Config(void);
     
183. u16         MODBUS_CRC16(u8 *p,u8 n);        //input:        *p--->First Data Address,n----->Data Number,        return:        CRC16
     
184. 
     
185. extern xdata u16        adc0;
     
186. extern xdata int        Battery;
     
187. 
     
188. 
     
189. //*********************************************************************
     
190. //****************角度計(jì)算*********************************************
     
191. //*********************************************************************
     
192. #define        pi                3.14159265f                           
     
193. #define        Kp                0.8f                        
     
194. #define        Ki                0.001f                        
     
195. #define        halfT        0.004f           
     
196. 
     
197. float idata q0=1,q1=0,q2=0,q3=0;   
     
198. float idata exInt=0,eyInt=0,ezInt=0;  
     
199. 
     
200. 
     
201. void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
     
202. {
     
203.         float data norm;
     
204.         float idata vx, vy, vz;
     
205.         float idata ex, ey, ez;
     
206. 
     
207.         norm = sqrt(ax*ax + ay*ay + az*az);        //把加速度計(jì)的三維向量轉(zhuǎn)成單維向量   
     
208.         ax = ax / norm;
     
209.         ay = ay / norm;
     
210.         az = az / norm;
     
211. 
     
212.                 //        下面是把四元數(shù)換算成《方向余弦矩陣》中的第三列的三個(gè)元素。
     
213.                 //        根據(jù)余弦矩陣和歐拉角的定義，地理坐標(biāo)系的重力向量，轉(zhuǎn)到機(jī)體坐標(biāo)系，正好是這三個(gè)元素
     
214.                 //        所以這里的vx vy vz，其實(shí)就是當(dāng)前的歐拉角（即四元數(shù)）的機(jī)體坐標(biāo)參照系上，換算出來(lái)的
     
215.                 //        重力單位向量。
     
216.         vx = 2*(q1*q3 - q0*q2);
     
217.         vy = 2*(q0*q1 + q2*q3);
     
218.         vz = q0*q0 - q1*q1 - q2*q2 + q3*q3 ;
     
219. 
     
220.         ex = (ay*vz - az*vy) ;
     
221.         ey = (az*vx - ax*vz) ;
     
222.         ez = (ax*vy - ay*vx) ;
     
223. 
     
224.         exInt = exInt + ex * Ki;
     
225.         eyInt = eyInt + ey * Ki;
     
226.         ezInt = ezInt + ez * Ki;
     
227. 
     
228.         gx = gx + Kp*ex + exInt;
     
229.         gy = gy + Kp*ey + eyInt;
     
230.         gz = gz + Kp*ez + ezInt;
     
231. 
     
232.         q0 = q0 + (-q1*gx - q2*gy - q3*gz) * halfT;
     
233.         q1 = q1 + ( q0*gx + q2*gz - q3*gy) * halfT;
     
234.         q2 = q2 + ( q0*gy - q1*gz + q3*gx) * halfT;
     
235.         q3 = q3 + ( q0*gz + q1*gy - q2*gx) * halfT;
     
236. 
     
237.         norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
     
238.         q0 = q0 / norm;
     
239.         q1 = q1 / norm;
     
240.         q2 = q2 / norm;
     
241.         q3 = q3 / norm;
     
242. 
     
243.         AngleX = asin(2*(q0*q2 - q1*q3 )) * 57.2957795f; // 俯仰   換算成度
     
244.         AngleY = asin(2*(q0*q1 + q2*q3 )) * 57.2957795f; // 橫滾
     
245. }
     
246. 
     
247. 
     
248. 
     
249. //****************姿態(tài)計(jì)算*********************************************
     
250. void PWM_int (void) interrupt         22        //PWM中斷函數(shù)
     
251. {
     
252.         PWMCFG = 0;        //CBIF;        //清除中斷標(biāo)志
     
253. 
     
254.         B_8ms = 1;
     
255. 
     
256. //======================== 超時(shí)溢出處理 ==============================================
     
257.         PPM1_Rise_TimeOut++;        //高電平限時(shí)
     
258.         PPM2_Rise_TimeOut++;        //高電平限時(shí)
     
259.         PPM3_Rise_TimeOut++;        //高電平限時(shí)
     
260.         PPM4_Rise_TimeOut++;        //高電平限時(shí)
     
261. 
     
262.         if(--PPM1_Rx_TimerOut == 0)                //超過(guò)100ms收不到信號(hào)
     
263.         {
     
264.                 PPM1_RxCnt = 0;                        //一旦出現(xiàn)溢出, 則開(kāi)始的n個(gè)脈沖無(wú)效
     
265.                 PPM1 = 128;;                        //默認(rèn)中點(diǎn)
     
266.         }
     
267.         if(--PPM2_Rx_TimerOut == 0)                //超過(guò)100ms收不到信號(hào)
     
268.         {
     
269.                 PPM2_RxCnt = 0;                        //一旦出現(xiàn)溢出, 則開(kāi)始的n個(gè)脈沖無(wú)效
     
270.                 PPM2 = 128;;                        //默認(rèn)中點(diǎn)
     
271.         }
     
272.         if(--PPM3_Rx_TimerOut == 0)                //超過(guò)200ms收不到信號(hào)
     
273.         {
     
274.                 PPM3_RxCnt = 0;                        //一旦出現(xiàn)溢出, 則開(kāi)始的n個(gè)脈沖無(wú)效
     
275.         }
     
276.         if(--PPM4_Rx_TimerOut == 0)                //超過(guò)100ms收不到信號(hào)
     
277.         {
     
278.                 PPM4_RxCnt = 0;                        //一旦出現(xiàn)溢出, 則開(kāi)始的n個(gè)脈沖無(wú)效
     
279.                 PPM4 = 128;                                //默認(rèn)中點(diǎn)
     
280.         }
     
281. //======================================================================
     
282. 
     
283.         if(++YM_LostCnt >= 250)                //失聯(lián)2秒后
     
284.         {
     
285.                 YM_LostCnt = 200;                //重復(fù)0.4秒，失控保護(hù)
     
286.                 if(PPM3 > 80)        PPM3--;
     
287.                 else if(++Lost16S >= 40)
     
288.                 {
     
289.                         Lost16S = 250;
     
290.                         PPM3 = 0;
     
291.                         B_Start = 0;
     
292.                 }
     
293.         }
     
294.         if(YM_LostCnt  >= 25)        //失聯(lián)200ms
     
295.         {
     
296.                 PPM1 = 128;
     
297.                 PPM2 = 128;                //俯仰 橫滾 航向均歸0
     
298.                 PPM4 = 128;
     
299.         }
     
300. 
     
301.         FRX = PPM1;
     
302.         FRY = PPM2;
     
303.         YM  = PPM3;        //油門(mén)
     
304.         FRZ = PPM4;
     
305.        
     
306. 
     
307. //********************************************************************************************
     
308.         Read_MPU6050(tp);        //680us
     
309. 
     
310.         Angle_ax = ((float)(((int *)&tp)[0])) / 8192.0;        //加速度處理        結(jié)果單位是 +- g
     
311.         Angle_ay = ((float)(((int *)&tp)[1])) / 8192.0;        //轉(zhuǎn)換關(guān)系        8192 LSB/g, 1g對(duì)應(yīng)讀數(shù)8192
     
312.         Angle_az = ((float)(((int *)&tp)[2])) / 8192.0;        //加速度量程 +-4g/S
     
313.         Last_Angle_gx = Angle_gx;                //儲(chǔ)存上一次角速度數(shù)據(jù)
     
314.         Last_Angle_gy = Angle_gy;
     
315.         Angle_gx = ((float)(((int *)&tp)[4] - g_x)) / 65.5;        //陀螺儀處理        結(jié)果單位是 +-度
     
316.         Angle_gy = ((float)(((int *)&tp)[5] - g_y)) / 65.5;        //陀螺儀量程 +-500度/S, 1度/秒 對(duì)應(yīng)讀數(shù) 65.536
     
317.         Angle_gz = ((float)(((int *)&tp)[6] - g_z)) / 65.5;        //轉(zhuǎn)換關(guān)系65.5 LSB/度
     
318. 
     
319.         IMUupdate(Angle_gx*0.0174533f, Angle_gy*0.0174533f, Angle_gz*0.0174533f, Angle_ax,Angle_ay,Angle_az);
     
320. 
     
321. //**********************************X軸指向************************************************
     
322.         Ax  = AngleX - a_x - ((float)FRX - 128) / 4.0;                //角度控制量加載至角度
     
323. 
     
324.         if(YM > 35)        ERRORX_Out += Ax,        ERRORX_Out += Ax,        ERRORX_Out += Ax;        //外環(huán)積分(油門(mén)小于某個(gè)值時(shí)不積分)
     
325.         else                ERRORX_Out = 0; //油門(mén)小于定值時(shí)清除積分值
     
326.                  if(ERRORX_Out >  1500)        ERRORX_Out =  1500;
     
327.         else if(ERRORX_Out < -1500)        ERRORX_Out = -1500;        //積分限幅
     
328. 
     
329.         Out_PID_X = Ax*Out_XP + ERRORX_Out*Out_XI + (Ax-Last_Ax)*Out_XD;        //外環(huán)PI
     
330.         Last_Ax = Ax;
     
331.        
     
332.         if(YM > 35)        ERRORX_In += (Angle_gy - Out_PID_X);        //內(nèi)環(huán)積分(油門(mén)小于某個(gè)值時(shí)不積分)
     
333.         else                ERRORX_In = 0;        //油門(mén)小于定值時(shí)清除積分值
     
334.                  if(ERRORX_In >  500)        ERRORX_In =  500;
     
335.         else if(ERRORX_In < -500)        ERRORX_In = -500;        //積分限幅
     
336. 
     
337.         PID_x = (Angle_gy + Out_PID_X) * In_XP + ERRORX_In * In_XI + (Angle_gy - Last_Angle_gy) * In_XD;        //內(nèi)環(huán)PID
     
338.         if(PID_x >  500)        PID_x =  500;        //輸出量限幅
     
339.         if(PID_x < -500)        PID_x = -500;
     
340. 
     
341. //**************Y軸指向**************************************************
     
342.         Ay  = AngleY - a_y + ((float)FRY - 128) / 4.0;                //角度控制量加載至角度
     
343.        
     
344.         if(YM > 35)        ERRORY_Out += Ay,        ERRORY_Out += Ay,        ERRORY_Out += Ay;        //外環(huán)積分(油門(mén)小于某個(gè)值時(shí)不積分)
     
345.         else                ERRORY_Out = 0; //油門(mén)小于定值時(shí)清除積分值
     
346.                  if(ERRORY_Out >  1500)        ERRORY_Out =  1500;
     
347.         else if(ERRORY_Out < -1500)        ERRORY_Out = -1500;        //積分限幅
     
348.        
     
349.         Out_PID_Y = Ay * Out_YP + ERRORY_Out * Out_YI + (Ay-Last_Ay)*Out_YD;        //外環(huán)PID
     
350.         Last_Ay = Ay;
     
351. 
     
352.         if(YM > 35)        ERRORY_In += (Angle_gx - Out_PID_Y);  //內(nèi)環(huán)積分(油門(mén)小于某個(gè)值時(shí)不積分)
     
353.         else                ERRORY_In = 0; //油門(mén)小于定值時(shí)清除積分值
     
354.                  if(ERRORY_In >  500)        ERRORY_In =  500;
     
355.         else if(ERRORY_In < -500)        ERRORY_In = -500;        //積分限幅
     
356.        
     
357.         PID_y = (Angle_gx + Out_PID_Y) * In_YP + ERRORY_In * In_YI + (Angle_gx - Last_Angle_gx) * In_YD;        //內(nèi)環(huán)PID
     
358.        
     
359.         if(PID_y > 500)        PID_y =  500;        //輸出量限幅
     
360.         if(PID_y <-500)        PID_y = -500;
     
361. 
     
362. //**************Z軸指向(Z軸隨便啦，自旋控制沒(méi)必要上串級(jí)PID)*****************************       
     
363.         Az = Angle_gz - ((float)FRZ - 128);
     
364.        
     
365.         if(YM > 35)        Z_integral += Az;        //Z軸積分
     
366.         else                Z_integral = 0;                //油門(mén)小于40積分清零
     
367.                  if(Z_integral >  500.0f)        Z_integral =  500.0f;        //積分限幅
     
368.         else if(Z_integral < -500.0f)        Z_integral = -500.0f;        //積分限幅
     
369. 
     
370.         PID_z = Az * ZP + Z_integral * ZI + (Az - Last_Az) * ZD;
     
371.         Last_Az = Az;
     
372.         if(PID_z >  200)        PID_z =  200;        //輸出量限幅
     
373.         if(PID_z < -200)        PID_z = -200;
     
374. 
     
375.         speed0 = (int)(  PID_x + PID_y + PID_z);        //M1改為逆時(shí)針
     
376.         speed1 = (int)(  PID_x - PID_y - PID_z);
     
377.         speed2 = (int)( -PID_x - PID_y + PID_z);
     
378.         speed3 = (int)( -PID_x + PID_y - PID_z);
     
379. 
     
380. //**************將速度參數(shù)加載至PWM模塊*************************************************       
     
381.        
     
382.         if(YM < 10)        PWM0 = 1000, PWM1 = 1000, PWM2 = 1000, PWM3 = 1000;
     
383.         else if(YM < 35)        PWM0 = 860, PWM1 = 860, PWM2 = 860, PWM3 = 860;
     
384.         else
     
385.         {
     
386.                 int_tmp = 1000 - (int)YM * 4;
     
387. 
     
388.                 PWM0 = int_tmp - speed0;
     
389. 
     
390.                          if(PWM0 > 1000)        PWM0 = 1000;    //速度參數(shù)控制，防止超過(guò)PWM參數(shù)范圍0-1000
     
391.                 else if(PWM0 < 10)                PWM0 = 10;
     
392. 
     
393.                 PWM1 = int_tmp - speed1;
     
394. 
     
395.                          if(PWM1 > 1000)        PWM1 = 1000;
     
396.                 else if(PWM1 < 10)                PWM1 = 10;
     
397. 
     
398.                 PWM2 = int_tmp - speed2;
     
399. 
     
400.                          if(PWM2 > 1000)        PWM2 = 1000;
     
401.                 else if(PWM2 < 10)                PWM2 = 10;
     
402. 
     
403.                 PWM3 = int_tmp - speed3;
     
404. 
     
405.                          if(PWM3 > 1000)        PWM3 = 1000;
     
406.                 else if(PWM3 < 10)                PWM3 = 10;
     
407.         }
     
408. 
     
409.         SW2_tmp = P_SW2;        //保存SW2設(shè)置
     
410.         EAXSFR();        //訪(fǎng)問(wèn)XFR
     
411.         PWM0T2 = (u16)(PWM0 * 2);
     
412.         PWM1T2 = (u16)(PWM1 * 2);
     
413.         PWM2T2 = (u16)(PWM2 * 2);
     
414.         PWM3T2 = (u16)(PWM3 * 2);       
     
415.         P_SW2  = SW2_tmp;        //恢復(fù)SW2設(shè)置
     
416. 
     
417. }
     
418. 
     
419. 
     
420. /********************** 蜂鳴函數(shù) ************************/
     
421. void        beep(void)        //100ms調(diào)用
     
422. {
     
423.         if(BuzzerRepeat > 0)        //蜂鳴器處理, 重復(fù)次數(shù)不為0，則蜂鳴器要發(fā)聲
     
424.         {
     
425.                 if((BuzzerOnCnt == 0) && (BuzzerOffCnt == 0))        //On和OFF都為0，則開(kāi)始裝載On和Off的時(shí)間
     
426.                 {
     
427.                         P_BUZZER = 1;                        //允許蜂鳴
     
428.                         BuzzerOnCnt  = BuzzerOnTime;        //裝載on計(jì)數(shù)
     
429.                         BuzzerOffCnt = BuzzerOffTime;        //裝載off計(jì)數(shù)
     
430.                 }
     
431.                 else if(BuzzerOnCnt  > 0)        {if(--BuzzerOnCnt == 0)        P_BUZZER = 0;}        //On的時(shí)間
     
432.                 else if(BuzzerOffCnt > 0)        //Off的時(shí)間
     
433.                 {
     
434.                         if(--BuzzerOffCnt == 0)        BuzzerRepeat--;
     
435.                 }
     
436.         }
     
437.         else        P_BUZZER = 0;
     
438. }
     
439. 
     
440. void        SetBuzzer(u8 on,u8 off,u8 rep)        // rep: 重復(fù)次數(shù), on: on的時(shí)間, off: off的時(shí)間
     
441. {
     
442.         BuzzerRepeat = rep;
     
443.         BuzzerOnTime  = on;
     
444.         BuzzerOffTime = off;
     
445.         if(BuzzerOnTime  == 0)        BuzzerOnTime  = 1;
     
446.         if(BuzzerOffTime == 0)        BuzzerOffTime = 1;
     
447.         if(BuzzerRepeat == 1)        BuzzerOffTime = 1;
     
448.         BuzzerOnCnt = 0,        BuzzerOffCnt = 0;
     
449. }
     
450. 
     
451. // ===================== 自動(dòng)校準(zhǔn)序列 =====================
     
452. void        AutoCal(void)
     
453. {
     
454.         if(PPM3 < 40)        //停止時(shí)才允許校準(zhǔn)
     
455.         {
     
456.                 if(Cal_Setp == 1)        //進(jìn)入校準(zhǔn)序列
     
457.                 {
     
458.                         x_sum = 0;        y_sum = 0;        z_sum = 0;
     
459.                         Cal_cnt  = 0;
     
460.                         Cal_Setp = 2;
     
461.                 }
     
462.                 else if(Cal_Setp == 2)        //對(duì)陀螺儀累加
     
463.                 {
     
464.                         x_sum += ((int *)&tp)[4];  //讀取陀螺儀數(shù)據(jù)
     
465.                         y_sum += ((int *)&tp)[5];
     
466.                         z_sum += ((int *)&tp)[6];
     
467.                         if(++Cal_cnt >= 64)
     
468.                         {
     
469.                                 g_x = x_sum / 64;
     
470.                                 g_y = y_sum / 64;
     
471.                                 g_z = z_sum / 64;
     
472.                                 float_x_sum = 0;        float_y_sum = 0;
     
473.                                 Cal_cnt  = 0;
     
474.                                 Cal_Setp = 3;
     
475.                         }
     
476.                 }
     
477.                 else if(Cal_Setp == 3)        //對(duì)X Y角度累加
     
478.                 {
     
479.                         float_x_sum += AngleX;
     
480.                         float_y_sum += AngleY;
     
481.                         if(++Cal_cnt >= 64)
     
482.                         {
     
483.                                 Cal_cnt  = 0;
     
484.                                 Cal_Setp = 0;
     
485.                                 a_x = float_x_sum / 64.0;
     
486.                                 a_y = float_y_sum / 64.0;
     
487.                                 IAP_Gyro();
     
488.                                 SetBuzzer(5,1,1);
     
489.                         }
     
490.                 }
     
491.         }
     
492.         else
     
493.         {
     
494.                 Cal_Setp = 0;
     
495.                 Cal_cnt  = 0;
     
496.         }
     
497. }
     
498. 
     
499. // ===================== 主函數(shù) =====================
     
500. void main(void)
     
501. {
     
502. 
     
503.         //所有I/O口全設(shè)為準(zhǔn)雙向，弱上拉模式
     
504.         P0M0=0x00;        P0M1=0x00;
     
505.         P1M0=0x00;        P1M1=0x00;
     
506.         P2M0=0x00;        P2M1=0x00;
     
507.         P3M0=0x00;        P3M1=0x00;
     
508.         P4M0=0x00;        P4M1=0x00;
     
509.         P5M0=0x00;        P5M1=0x00;
     
510.         P6M0=0x00;        P6M1=0x00;
     
511.         P7M0=0x00;        P7M1=0x00;
     
512. 
     
513.         PPM1 = 128;
     
514.         PPM2 = 128;
     
515.         PPM3 = 0;
     
516.         PPM4 = 128;
     
517. 
     
518.         PWMGO();
     
519. 
     
520.         P_Light  = 0;
     
521.         P_BUZZER = 0;
     
522.         P5n_push_pull(0x30);
     
523. 
     
524.         adc_init();    //啟動(dòng)A/D
     
525.        
     
526.         PCA_config();
     
527. 
     
528.         delay_ms(100);
     
529.         IAPRead();                //讀取陀螺儀靜差
     
530.         InitMPU6050();        //初始化MPU-6050
     
531.         delay_ms(100);
     
532. 
     
533.         PWMCR =  0xc0;//ECBI;        //允許PWM計(jì)數(shù)器歸零中斷
     
534.         EA = 1;        //允許總中斷
     
535.        
     
536.         cnt_start = 0;
     
537.         while(cnt_start < 25)        //等待油門(mén)最小        20ms * 25 = 500ms
     
538.         {
     
539.                 if(B_PPM3_OK)        //油門(mén)
     
540.                 {
     
541.                         B_PPM3_OK = 0;
     
542.                         if(PPM3_Cap <= 1200)        cnt_start++;
     
543.                 }
     
544.                 delay_ms(1);
     
545.         }
     
546.         P_Light  = 0;
     
547.        
     
548.         cnt_start = 0;
     
549. 
     
550.         SetBuzzer(5,1,1);
     
551.        
     
552. 
     
553. //==============================================
     
554.         UART1_config();        // 選擇波特率, 2: 使用Timer2做波特率, 其它值: 使用Timer1做波特率.
     
555.         PrintString1("STC15W4K系列大四軸飛控程序!\r\n");        //SUART1發(fā)送一個(gè)字符串
     
556. //==============================================
     
557. 
     
558.         B_Start = 0;        //上電禁止運(yùn)行
     
559. 
     
560.         while(1)
     
561.         {
     
562.                 if(B_PPM1_OK)        //左右(橫滾)
     
563.                 {
     
564.                         B_PPM1_OK = 0;
     
565.                                  if(PPM1_Cap < 1120)        PPM1_Cap = 1120;
     
566.                         else if(PPM1_Cap > 1880)        PPM1_Cap = 1880;
     
567.                         PPM2 = (u8)((PPM1_Cap-1116)/3);        //轉(zhuǎn)為0~255, 中間值為128
     
568.                 }
     
569.                
     
570.                 if(B_PPM2_OK)        //前后(俯仰)
     
571.                 {
     
572.                         B_PPM2_OK = 0;
     
573.                                  if(PPM2_Cap < 1120)        PPM2_Cap = 1120;
     
574.                         else if(PPM2_Cap > 1880)        PPM2_Cap = 1880;
     
575.                         PPM1 = (u8)((PPM2_Cap-1116)/3);        //轉(zhuǎn)為0~255, 中間值為128
     
576.                 }
     
577.                
     
578.                 if(B_PPM4_OK)        //航向
     
579.                 {
     
580.                         B_PPM4_OK = 0;
     
581.                                  if(PPM4_Cap < 1056)        PPM4_Cap = 1056;
     
582.                                  if(PPM4_Cap > 1940)        PPM4_Cap = 1940;
     
583.                                  if(PPM4_Cap < 1440)        PPM4_Cap = PPM4_Cap + 60;
     
584.                         else if(PPM4_Cap > 1560)        PPM4_Cap = PPM4_Cap - 60;
     
585.                         else        PPM4_Cap = 1500;
     
586.                         PPM4 = (u8)((PPM4_Cap-1116)/3);        //轉(zhuǎn)為0~255, 中間值為128
     
587.                 }
     
588.                
     
589.                 if(B_PPM3_OK)        //油門(mén)
     
590.                 {
     
591.                         B_PPM3_OK = 0;
     
592.                         if(PPM3_Cap < 1000)        PPM3_Cap = 1000;
     
593.                         if(PPM3_Cap > 1900)        PPM3_Cap = 1900;
     
594. 
     
595.                         if(B_Start)                //正在運(yùn)行時(shí),
     
596.                         {
     
597.                                 PPM3 = (u8)((PPM3_Cap-1000)/4);        //轉(zhuǎn)為0~255,        實(shí)際8~225
     
598.                                 if(PPM3 < 32)        PPM3 = 32;
     
599.                                
     
600.                                 if((PPM1 < 50) && (PPM2 < 50) && (PPM3_Cap < 1120) && (PPM4 > 200))        //下外八, 禁止
     
601.                                 {
     
602.                                         if(++cnt_start >= 50)        //1秒
     
603.                                         {
     
604.                                                 cnt_start = 0;
     
605.                                                 B_Start = 0;
     
606.                                                 SetBuzzer(1,1,2);
     
607.                                         }
     
608.                                 }
     
609.                                 else        cnt_start = 0;
     
610.                         }
     
611.                         else        //禁止運(yùn)行時(shí), 等待內(nèi)八開(kāi)啟
     
612.                         {
     
613.                                 PPM3 = 0;
     
614.                                 if((PPM1 < 50) && (PPM2 > 200) && (PPM3_Cap < 1120) && (PPM4 < 50))        //下內(nèi)八, 啟動(dòng)
     
615.                                 {
     
616.                                         if(++cnt_start >= 50)        //1秒
     
617.                                         {
     
618.                                                 cnt_start = 0;
     
619.                                                 B_Start = 1;
     
620.                                                 SetBuzzer(5,1,1);
     
621.                                         }
     
622.                                 }
     
623.                                 else if((PPM1 > 200) && (PPM2 > 200) && (PPM3_Cap > 1850) && (PPM4 < 50))        //上內(nèi)八, 水平校準(zhǔn)
     
624.                                 {
     
625.                                         if(++cnt_start >= 50)        //1秒
     
626.                                         {
     
627.                                                 cnt_start = 0;
     
628.                                                 SetBuzzer(2,1,1);
     
629.                                                 Cal_Setp = 1;
     
630.                                         }
     
631.                                 }
     
632.                                 else if((PPM1 > 200) && (PPM2 < 50) && (PPM3_Cap > 1850) && (PPM4 > 200))        //上外八, 取消水平校準(zhǔn)
     
633.                                 {
     
634.                                         if(++cnt_start >= 50)        //1秒
     
635.                                         {
     
636.                                                 cnt_start = 0;
     
637.                                                 g_x = 0;
     
638.                                                 g_y = 0;
     
639.                                                 g_z = 0;
     
640.                                                 a_x = 0;
     
641.                                                 a_y = 0;
     
642.                                                 IAP_Gyro();
     
643.                                                 SetBuzzer(1,1,2);
     
644.                                         }
     
645.                                 }
     
646.                                 else        cnt_start = 0;
     
647.                         }
     
648.                 }
     
649. 
     
650. 
     
651.                 if(B_8ms)                //8ms到
     
652.                 {
     
653.                         B_8ms = 0;
     
654.                        
     
655.                         if(Cal_Setp != 0)        AutoCal();        //是否執(zhí)行自動(dòng)校準(zhǔn)序列
     
656.                         AD();                // 讀ADC計(jì)算電壓
     
657. 
     
658.                         if(++cnt_100ms >= 12)        cnt_100ms = 0,        beep();        //100ms處理一次蜂鳴器
     
659. 
     
660.                         B = cnt_ms;
     
661.                         ++cnt_ms;
     
662.                         B = (B ^ cnt_ms) & cnt_ms;
     
663. 
     
664.                         if(B2)                //64ms
     
665.                         {
     
666.                                 if(!B_BAT_LOW && (YM_LostCnt < 120))        //電壓足, 信號(hào)正常
     
667.                                 {
     
668.                                         if(!B_Start)        P_Light = 0;        // 空閑時(shí), 則慢閃(每2048ms亮64ms)
     
669.                                         else                         P_Light = 1;        // 啟動(dòng)后, 燈常亮
     
670.                                 }
     
671.                         }
     
672.                         else if(B4)                //256ms
     
673.                         {
     
674.                                 if(B_BAT_LOW || (YM_LostCnt >= 120))        P_Light = ~P_Light;                //電壓低, 或無(wú)信號(hào), 航燈閃爍 2HZ
     
675.                         }
     
676.                         else if(B6)                //1024ms
     
677.                         {
     
678.                                 if(Battery < 1090)        B_BAT_LOW = 1;        else if(Battery > 1110)        B_BAT_LOW = 0;        //<10.90V電壓低, >11.10V電壓夠
     
679.                                
     
680.                                 if(B_BAT_LOW)        SetBuzzer(1,1,2);        //電壓低
     
681.                                
     
682.                                 if(B_rtn_ADC0)        Return_Message();        //請(qǐng)求返回ADC0數(shù)據(jù)
     
683. 
     
684.                                 if(!B_BAT_LOW && (YM_LostCnt < 120))        P_Light = 1;        //遙控信號(hào)正常,        電壓正常時(shí)
     
685.                         }
     
686.                         else if(B7)                //2048ms
     
687.                         {
     
688.                                 if(!B_BAT_LOW && (YM_LostCnt >= 120))        SetBuzzer(1,1,3);        //電壓正常時(shí) 遙控信號(hào)丟失, 每?jī)擅攵跳Q3次,
     
689.                         }
     
690.                 }
     
691. 
     
692. 
     
693.                 if(UART1_cmd != 0)
     
694.                 {
     
695.                         if(UART1_cmd == 'a')                //PC發(fā)送a，飛控返回一些參數(shù)
     
696.                         {
     
697.                                 B_rtn_ADC0 = ~B_rtn_ADC0;
     
698.                         }
     
699.                         UART1_cmd = 0;
     
700.                 }
     
701.                
     
702.                
     
703.                 if((TX1_Read != TX1_Write) && (!B_TX1_Busy))        //有數(shù)據(jù)要發(fā)送, 并且發(fā)送空閑
     
704.                 {
     
705.                         SBUF = TX1_Buffer[TX1_Read];
     
706.                         B_TX1_Busy = 1;
     
707.                         if(++TX1_Read >= TX1_LENGTH)        TX1_Read = 0;
     
708.                 }
     
709. 
     
710.         }
     
711. }
     
712. 
     
713. //=========================================================
     
714. 
     
715. void        Return_Message(void)
     
716. {
     
717.         TX1_write2buff('V');
     
718.         TX1_write2buff('=');
     
719.         TX1_write2buff(Battery/1000 + '0');
     
720.         TX1_write2buff((Battery%1000)/100 + '0');
     
721.         TX1_write2buff('.');
     
722.         TX1_write2buff((Battery%100)/10 + '0');
     
723.         TX1_write2buff(Battery%10 + '0');
     
724.         TX1_write2buff(' ');
     
725.         TX1_write2buff(' ');
     
726. 
     
727.         PrintString1("AngleX=");
     
728.         TX1_int_value((int)(AngleX * 10));
     
729. 
     
730.         PrintString1("AngleY=");
     
731.         TX1_int_value((int)(AngleY * 10));
     
732. 
     
733.         PrintString1("AngleZ=");
     
734.         TX1_int_value((int)(Angle_gz * 10));
     
735. 
     
736.         PrintString1("a_x=");
     
737.         TX1_int_value(a_x * 10);
     
738.         PrintString1("a_y=");
     
739.         TX1_int_value(a_y * 10);
     
740.         PrintString1("g_z=");
     
741.         TX1_int_value(g_z * 10);
     
742. 
     
743.         TX1_cnt = 0;
     
744.         TX1_write2buff(0x0d);
     
745.         TX1_write2buff(0x0a);
     
746. }
     
747. 
     
748. 
     
749. void  delay_ms(u8 ms)
     
750. {
     
751.      u16 i;
     
752.          do
     
753.          {
     
754.                  i = MAIN_Fosc / 13000;
     
755.                 while(--i)        ;   //13T per loop
     
756.      }while(--ms);
     
757. }
     
758. 
     
759. 
     
760. void        TX1_int_value(int i)
     
761. {
     
762.         if(i < 0)        TX1_write2buff('-'),        i = 0 - i;
     
763.         else                TX1_write2buff(' ');
     
764.         TX1_write2buff(i / 1000 + '0');
     
765.         TX1_write2buff((i % 1000) / 100 + '0');
     
766.         TX1_write2buff((i % 100) / 10 + '0');
     
767.         TX1_write2buff('.');
     
768.         TX1_write2buff(i % 10 + '0');
     
769.         TX1_write2buff(' ');
     
770.         TX1_write2buff(' ');
     
771. }
     
772. 
     
773. /*************** 裝載串口1發(fā)送緩沖 *******************************/
     
774. void TX1_write2buff(u8 dat)        //寫(xiě)入發(fā)送緩沖，指針+1
     
775. {
     
776.         TX1_Buffer[TX1_Write] = dat;
     
777.         if(++TX1_Write >= TX1_LENGTH)        TX1_Write = 0;
     
778. }
     
779. 
     
780. 
     
781. //========================================================================
     
782. // 函數(shù): void PrintString1(u8 *puts)
     
783. // 描述: 串口1發(fā)送字符串函數(shù)。
     
784. // 參數(shù): puts:  字符串指針.
     
785. // 返回: none.
     
786. // 版本: VER1.0
     
787. // 日期: 2014-11-28
     
788. // 備注:
     
789. //========================================================================
     
790. void PrintString1(u8 *puts)        //發(fā)送一個(gè)字符串
     
791. {
     
792.         for (; *puts != 0;        puts++)   TX1_write2buff(*puts);        //遇到停止符0結(jié)束
     
793. }
     
794. 
     
795. //========================================================================
     
796. // 函數(shù): SetTimer2Baudrate(u16 dat)
     
797. // 描述: 設(shè)置Timer2做波特率發(fā)生器。
     
798. // 參數(shù): dat: Timer2的重裝值.
     
799. // 返回: none.
     
800. // 版本: VER1.0
     
801. // 日期: 2014-11-28
     
802. // 備注:
     
803. //========================================================================
     
804. 
     
805. void        SetTimer2Baudrate(u16 dat)        // 選擇波特率, 2: 使用Timer2做波特率, 其它值: 使用Timer1做波特率.
     
806. {
     
807.         AUXR &= ~(1<<4);        //Timer stop
     
808.         AUXR &= ~(1<<3);        //Timer2 set As Timer
     
809.         AUXR |=  (1<<2);        //Timer2 set as 1T mode
     
810.         TH2 = dat / 256;
     
811.         TL2 = dat % 256;
     
812.         IE2  &= ~(1<<2);        //禁止中斷
     
813.         AUXR |=  (1<<4);        //Timer run enable
     
814. }
     
815. 
     
816. 
     
817. //========================================================================
     
818. // 函數(shù): void        UART1_config(u8 brt)
     
819. // 描述: UART1初始化函數(shù)。
     
820. // 參數(shù): brt: 選擇波特率, 2: 使用Timer2做波特率, 其它值: 使用Timer1做波特率.
     
821. // 返回: none.
     
822. // 版本: VER1.0
     
823. // 日期: 2014-11-28
     
824. // 備注:
     
825. //========================================================================
     
826. void        UART1_config(void)
     
827. {
     
828.         /*********** 波特率使用定時(shí)器2 *****************/
     
829.         AUXR |= 0x01;                //S1 BRT Use Timer2;
     
830.         SetTimer2Baudrate(65536UL - (MAIN_Fosc / 4) / Baudrate1);
     
831. 
     
832.         /*********** 波特率使用定時(shí)器1 *****************/
     
833. /*        TR1 = 0;
     
834.         AUXR &= ~0x01;                //S1 BRT Use Timer1;
     
835.         AUXR |=  (1<<6);        //Timer1 set as 1T mode
     
836.         TMOD &= ~(1<<6);        //Timer1 set As Timer
     
837.         TMOD &= ~0x30;                //Timer1_16bitAutoReload;
     
838.         TH1 = (u8)((65536UL - (MAIN_Fosc / 4) / Baudrate1) / 256);
     
839.         TL1 = (u8)((65536UL - (MAIN_Fosc / 4) / Baudrate1) % 256);
     
840.         ET1 = 0;        //禁止中斷
     
841.         INT_CLKO &= ~0x02;        //不輸出時(shí)鐘
     
842.         TR1  = 1;
     
843. */        //========================================================================
     
844. 
     
845.         SCON = (SCON & 0x3f) | 0x40;        //UART1模式, 0x00: 同步移位輸出, 0x40: 8位數(shù)據(jù),可變波特率, 0x80: 9位數(shù)據(jù),固定波特率, 0xc0: 9位數(shù)據(jù),可變波特率
     
846.         PS  = 1;        //高優(yōu)先級(jí)中斷
     
847.         ES  = 1;        //允許中斷
     
848.         REN = 1;        //允許接收
     
849.         P_SW1 &= 0x3f;
     
850.         P_SW1 |= 0x00;                //UART1 switch to, 0x00: P3.0 P3.1, 0x40: P3.6 P3.7, 0x80: P1.6 P1.7 (必須使用內(nèi)部時(shí)鐘)
     
851. //        PCON2 |=  (1<<4);        //內(nèi)部短路RXD與TXD, 做中繼, ENABLE,DISABLE
     
852. 
     
853.         B_TX1_Busy = 0;
     
854.         TX1_Read   = 0;
     
855.         TX1_Write  = 0;
     
856.         UART1_cmd  = 0;
     
857.         TX1_cnt    = 0;
     
858. }
     
859. 
     
860. 
     
861. //========================================================================
     
862. // 函數(shù): void UART1_int (void) interrupt UART1_VECTOR
     
863. // 描述: UART1中斷函數(shù)。
     
864. // 參數(shù): nine.
     
865. // 返回: none.
     
866. // 版本: VER1.0
     
867. // 日期: 2014-11-28
     
868. // 備注:
     
869. //========================================================================
     
870. void UART1_int (void) interrupt 4
     
871. {
     
872.         if(RI)
     
873.         {
     
874.                 RI = 0;
     
875.                 UART1_cmd = SBUF;
     
876.         }
     
877. 
     
878.         if(TI)
     
879.         {
     
880.                 TI = 0;
     
881.                 B_TX1_Busy = 0;
     
882.         }
     
883. }
     
884. 
     
885. 
     
886. 
     
887. void        PCA_config(void)
     
888. {
     
889.         PPM1_Rise_TimeOut = 0;
     
890.         PPM2_Rise_TimeOut = 0;
     
891.         PPM3_Rise_TimeOut = 0;
     
892.         PPM4_Rise_TimeOut = 0;
     
893. 
     
894.         CR = 0;
     
895.         CH = 0;
     
896.         CL = 0;
     
897.         AUXR1 = (AUXR1 & ~(3<<4)) | PCA_P12_P17_P16_P15_P14;        //切換IO口
     
898.         CMOD  = (CMOD  & ~(7<<1)) | PCA_Clock_12T;                                //選擇時(shí)鐘源  STC8F8K D版本
     
899. //        CMOD  = (CMOD  & ~1) | 1;                                                                //ECF
     
900.         PPCA = 1;        //高優(yōu)先級(jí)中斷
     
901. 
     
902.         CCAPM0     = PCA_Mode_Capture | PCA_Rise_Active | PCA_Fall_Active | ENABLE;        //工作模式, 中斷模式
     
903.         PCA_PWM0   = PCA_PWM_8bit;        //PWM寬度
     
904. //        CCAP0L = (u8)CCAP0_tmp;                        //將影射寄存器寫(xiě)入捕獲寄存器，先寫(xiě)CCAPnL
     
905. //        CCAP0H = (u8)(CCAP0_tmp >> 8);        //后寫(xiě)CCAPnH
     
906. 
     
907.         CCAPM1     = PCA_Mode_Capture | PCA_Rise_Active | PCA_Fall_Active | ENABLE;        //工作模式, 中斷模式
     
908.         PCA_PWM1   = PCA_PWM_8bit;        //PWM寬度
     
909. //        CCAP1L = (u8)CCAP1_tmp;                        //將影射寄存器寫(xiě)入捕獲寄存器，先寫(xiě)CCAPnL
     
910. //        CCAP1H = (u8)(CCAP1_tmp >> 8);        //后寫(xiě)CCAPnH
     
911. 
     
912.         CCAPM2     = PCA_Mode_Capture | PCA_Rise_Active | PCA_Fall_Active | ENABLE;        //工作模式, 中斷模式
     
913.         PCA_PWM2   = PCA_PWM_8bit;        //PWM寬度
     
914. //        CCAP2L = (u8)CCAP2_tmp;                        //將影射寄存器寫(xiě)入捕獲寄存器，先寫(xiě)CCAPnL
     
915. //        CCAP2H = (u8)(CCAP2_tmp >> 8);        //后寫(xiě)CCAPnH
     
916. 
     
917.         CCAPM3     = PCA_Mode_Capture | PCA_Rise_Active | PCA_Fall_Active | ENABLE;        //工作模式, 中斷模式
     
918.         PCA_PWM3   = PCA_PWM_8bit;        //PWM寬度
     
919. //        CCAP3L = (u8)CCAP3_tmp;                        //將影射寄存器寫(xiě)入捕獲寄存器，先寫(xiě)CCAPnL
     
920. //        CCAP3H = (u8)(CCAP3_tmp >> 8);        //后寫(xiě)CCAPnH
     
921. 
     
922.         CR = 1;
     
923. }
     
924. 
     
925. 
     
926. //========================================================================
     
927. // 函數(shù): void        PCA_Handler (void) interrupt PCA_VECTOR
     
928. // 描述: PCA中斷處理程序.
     
929. // 參數(shù): None
     
930. // 返回: none.
     
931. // 版本: V1.0, 2012-11-22
     
932. //========================================================================
     
933. void        PCA_Handler (void) interrupt PCA_VECTOR
     
934. {
     
935.         if(CCF0)                //PCA模塊0中斷
     
936.         {
     
937.                 CCF0 = 0;                //清PCA模塊0中斷標(biāo)志
     
938.                 if(P17)        //上升沿
     
939.                 {
     
940.                         CCAP0_RiseTime = ((u16)CCAP0H << 8) + CCAP0L;        //讀CCAP0
     
941.                         PPM1_Rise_TimeOut = 1;        //收到上升沿, 高電平限時(shí)
     
942.                 }
     
943.                 else        //下降沿
     
944.                 {
     
945.                         CCAP_FallTime = ((u16)CCAP0H << 8) + CCAP0L;        //讀CCAP0
     
946.                         if((PPM1_Rise_TimeOut != 0) && (PPM1_Rise_TimeOut < 3))        //收到過(guò)上升沿, 高電平也沒(méi)有溢出
     
947.                         {
     
948.                                 CCAP_FallTime = (CCAP_FallTime - CCAP0_RiseTime) >> 1;        //為了好處理, 轉(zhuǎn)成單位為us
     
949.                                 if((CCAP_FallTime >= 800) && (CCAP_FallTime <= 2500))
     
950.                                 {
     
951.                                         if(++PPM1_RxCnt >= 5)        PPM1_RxCnt = 5;                //連續(xù)接收到5個(gè)脈沖
     
952.                                         if(PPM1_RxCnt == 5)
     
953.                                         {
     
954.                                                 if(!B_PPM1_OK)
     
955.                                                 {
     
956.                                                         PPM1_Cap = CCAP_FallTime;
     
957.                                                         B_PPM1_OK = 1;                //標(biāo)志收到一個(gè)脈沖
     
958.                                                         PPM1_Rx_TimerOut = 12;        //限時(shí)收不到脈沖
     
959.                                                 }
     
960.                                         }
     
961.                                 }
     
962. ……………………
     
963. 
     
964. …………限于本文篇幅 余下代碼請(qǐng)從51黑下載附件…………
     

復(fù)制代碼

所有資料51hei提供下載:
四軸飛控-STC8A8K64S4A12-LQFP44-PPM-V10.rar (3.86 MB, 下載次數(shù): 443)

2017-11-26 11:17 上傳

點(diǎn)擊文件名下載附件
下載積分: 黑幣 -5

---

作者: jlian168    時(shí)間: 2017-11-28 11:24
下來(lái)試試...

---

作者: wuxishun    時(shí)間: 2017-12-4 14:52
謝謝分享，有時(shí)間做一個(gè)玩下

---

作者: twjccmx    時(shí)間: 2018-1-5 14:40
感謝分享，有空學(xué)習(xí)下

---

作者: anoo    時(shí)間: 2018-5-6 22:02
感謝分享,支持原創(chuàng),

---

作者: zhoudianhong    時(shí)間: 2018-7-29 16:26
為什么怎么也下不完呢，是有問(wèn)題？

---

作者: yangzhe6140    時(shí)間: 2018-9-5 16:57
關(guān)于玩四軸，我認(rèn)為深入的玩法就是研究軟硬件，特別是軟件，如果只是組裝比人的成品，沒(méi)啥成就感；本著此想法所以下載學(xué)習(xí)此算法！

---

作者: mark13777    時(shí)間: 2018-9-25 09:12
感謝分享，對(duì)于學(xué)習(xí)很有幫助

---

作者: lc110    時(shí)間: 2018-9-27 16:05
謝謝你的分享

---

作者: war3mars    時(shí)間: 2019-4-16 09:24
樓主，可否加個(gè)QQ，跟你學(xué)習(xí)下，我的QQ 75750462

---

作者: caiseshamo    時(shí)間: 2019-5-7 11:22
感謝~~~

---

作者: samu12a    時(shí)間: 2019-6-9 10:08
謝謝樓主分享！

---

作者: 51單片小白v    時(shí)間: 2019-7-28 14:28
感謝樓主分享

---

作者: wulingqing    時(shí)間: 2019-7-28 17:02
謝謝分享，學(xué)著做一個(gè)四軸飛控

---

作者: 慢牽牛    時(shí)間: 2019-9-29 23:08
下載下來(lái)學(xué)習(xí)學(xué)習(xí)

---

作者: 987654321123    時(shí)間: 2019-11-2 18:18
下載下來(lái)好好學(xué)習(xí)一下代碼

---

作者: woojoon1981    時(shí)間: 2020-1-16 16:55
正在學(xué)習(xí)無(wú)人機(jī)，謝謝你的分享

---

作者: serafim    時(shí)間: 2020-2-5 11:30
感謝分享，想問(wèn)下，如果機(jī)身水平不變四個(gè)螺旋槳前后左右偏轉(zhuǎn)改變飛行方向是否可行？機(jī)身姿態(tài)與控制方向非耦合狀態(tài)，同時(shí)機(jī)身可在空中改變角度懸停。

---

作者: 鑒啊啊    時(shí)間: 2020-2-10 17:26
請(qǐng)問(wèn)您有飛行的視頻么

---

作者: peeta    時(shí)間: 2020-3-18 15:11
謝謝分享

---

作者: shenyuqiu    時(shí)間: 2021-4-22 08:05
關(guān)鍵還是軟件算法

---

作者: 小劉要加油    時(shí)間: 2021-4-30 22:28
這個(gè)厲害啊，想學(xué)但是沒(méi)怎么看懂哈哈

---

作者: 汪eric    時(shí)間: 2021-5-14 22:27
感謝樓主分享，小白一名前來(lái)膜拜學(xué)習(xí)

---

作者: WFX777888    時(shí)間: 2021-5-23 19:48
學(xué)習(xí)無(wú)人機(jī)，謝謝你的分享

---

作者: CHNO    時(shí)間: 2021-6-5 17:11
從宏晶官網(wǎng)下載，然后打過(guò)板子，四軸可以平穩(wěn)飛行，就是油門(mén)開(kāi)大后不能穩(wěn)定的停在原地，會(huì)偏，正在查找原因中，

---

作者: lzl12399    時(shí)間: 2021-6-10 11:45

CHNO 發(fā)表于 2021-6-5 17:11
從宏晶官網(wǎng)下載，然后打過(guò)板子，四軸可以平穩(wěn)飛行，就是油門(mén)開(kāi)大后不能穩(wěn)定的停在原地，會(huì)偏，正在查找原因 ...

板子打樣，自己畫(huà)得還是有現(xiàn)成得，小弟也想玩玩

---

作者: CHNO    時(shí)間: 2021-6-11 10:32

lzl12399 發(fā)表于 2021-6-10 11:45
板子打樣，自己畫(huà)得還是有現(xiàn)成得，小弟也想玩玩

我是自己畫(huà)的

---

作者: CHNO    時(shí)間: 2021-6-11 11:29
分享一下調(diào)試經(jīng)驗(yàn)
1.起飛后會(huì)飛斜：在水平的地板上將四軸放平，開(kāi)啟遙控，油門(mén)打到最低，串口以文本輸入a，開(kāi)始串口打印X，Y的角度，大概是+-2左右，將這個(gè)數(shù)記下來(lái)，然后在程序這里加補(bǔ)償（紅字的部分），然后重新編譯后下載程序，然后在開(kāi)啟串口打印，這時(shí)輸出一般都在0-0.1這兒穩(wěn)定了，起飛后，無(wú)風(fēng)的情況下不會(huì)亂飛。
        AngleX = asin(2*(q0*q2 - q1*q3 )) * 57.2957795f+1.9; // 俯仰   換算成度
        AngleY = asin(2*(q0*q1 + q2*q3 )) * 57.2957795f+0.8; // 橫滾

2.遙控器通道正反要按資料上的圖片來(lái)?yè)埽�資料要求美國(guó)手，我買(mǎi)的日本手，實(shí)則都能用。用日本手要改接收機(jī)和單片機(jī)線(xiàn)的位置，用萬(wàn)用表對(duì)照程序注釋一看便知。
3.這個(gè)程序配合MC6遙控器十分好用方便，可是把程序改成串口無(wú)線(xiàn)控制會(huì)發(fā)生一些問(wèn)題，感覺(jué)是串口接受數(shù)據(jù)多，中斷頻率較高，而PWM中斷等級(jí)較低，老是打斷PWM中斷，然后現(xiàn)象是某個(gè)固定的腳PWM輸出會(huì)比其他高很多，串口改一下波特率或者發(fā)射加個(gè)延時(shí)，這個(gè)現(xiàn)象又轉(zhuǎn)移到其他PWM引腳，目前還在優(yōu)化中，，，，，，
4.前人栽樹(shù)后人乘涼，由心的感謝程序作者無(wú)私的分享。

---

作者: putiz    時(shí)間: 2021-12-26 17:38
大家好，我看了幾天的代碼了，也嘗試打印了一些數(shù)據(jù)來(lái)分析。
現(xiàn)狀是剛開(kāi)始啟動(dòng)后，電機(jī)不會(huì)啟動(dòng)，不會(huì)低速旋轉(zhuǎn)。即使偶爾有幾個(gè)會(huì)旋轉(zhuǎn)，也不會(huì)受遙控器的控制。
經(jīng)過(guò)打印一些信息發(fā)現(xiàn)：

1.         SW2_tmp = P_SW2;        //保存SW2設(shè)置
   
2.         EAXSFR();        //訪(fǎng)問(wèn)XFR
   
3.         PWM0T2 = (u16)(PWM0 * 2);
   
4.         PWM1T2 = (u16)(PWM1 * 2);
   
5.         PWM2T2 = (u16)(PWM2 * 2);       
   
6.         PWM3T2 = (u16)(PWM3 * 2);        //進(jìn)行輸出脈沖信號(hào)更新
   
7.         P_SW2  = SW2_tmp;        //恢復(fù)SW2設(shè)置

復(fù)制代碼

這樣的代碼，這些值不會(huì)發(fā)生變化和更新

1.         PWM0T1 = 4000;//第一個(gè)翻轉(zhuǎn)點(diǎn) 4000
   
2.         PWM1T1 = 4000;
   
3.         PWM2T1 = 4000;
   
4.         PWM3T1 = 4000;
   
5. //        PWM4T1 = 4000;
   
6. //        PWM5T1 = 4000;
   
7. //        PWM6T1 = 4000;
   
8. //        PWM7T1 = 4000;
   
9. 
   
10.         PWM0T2 = 2000;  //第二個(gè)翻轉(zhuǎn)點(diǎn) 2000
    
11.         PWM1T2 = 2000;
    
12.         PWM2T2 = 2000;
    
13.         PWM3T2 = 2000;
    
14. //        PWM4T2 = 2000;
    
15. //        PWM5T2 = 2000;
    
16. //        PWM6T2 = 2000;
    
17. //        PWM7T2 = 2000;

復(fù)制代碼

在這樣的初始化后打印出來(lái)的數(shù)據(jù)卻是0 4000 8000 12337（PWM0T1 PWM1T1 PWM2T1 PWM3T1）
和初始化結(jié)果也是不一樣的。
為什么我找這樣的信息呢，因?yàn)槲腋杏X(jué)首先遙控器不能控制波形的輸出，剛開(kāi)始輸出的波形也不能讓電動(dòng)機(jī)啟動(dòng)。
因此我想試著找找這個(gè)原因，但是這個(gè)值不知道為什么不會(huì)發(fā)生變化
請(qǐng)問(wèn)大佬可以回復(fù)下嗎？真的不知道怎么辦

---

作者: putiz    時(shí)間: 2021-12-26 17:41

CHNO 發(fā)表于 2021-6-11 11:29
分享一下調(diào)試經(jīng)驗(yàn)
1.起飛后會(huì)飛斜：在水平的地板上將四軸放平，開(kāi)啟遙控，油門(mén)打到最低，串口以文本輸入a ...

大佬，可以加下聯(lián)系方式嗎？
自己遇到了一些問(wèn)題，想向您請(qǐng)教一下

---

作者: putiz    時(shí)間: 2021-12-26 18:05

CHNO 發(fā)表于 2021-6-5 17:11
從宏晶官網(wǎng)下載，然后打過(guò)板子，四軸可以平穩(wěn)飛行，就是油門(mén)開(kāi)大后不能穩(wěn)定的停在原地，會(huì)偏，正在查找原因 ...

大佬，可以加下好友交流一下嗎
2410025784

---

作者: CHNO    時(shí)間: 2022-1-1 10:49

putiz 發(fā)表于 2021-12-26 18:05
大佬，可以加下好友交流一下嗎
2410025784

你好，已加未見(jiàn)回復(fù)，因?yàn)椴磺宄�你那邊啥情況，就這邊回復(fù)一下我大概的調(diào)試過(guò)程吧
1.程序里面有個(gè)電池電壓檢測(cè)（ADC部分），要保證這邊沒(méi)問(wèn)題，檢測(cè)電壓低是啟動(dòng)不了的，我這邊是直接把該端口和VCC接一起的。（因?yàn)槲矣玫母綦x電源，不能和鋰電池共地）
2.然后你的電路是否用的資料提供的電路？還是略有修改？PCA輸入假如用光耦隔離的話(huà)，受光側(cè)上拉電阻取值會(huì)影響波形的好壞，我這用的521光耦，原本用的103電阻，發(fā)現(xiàn)也是無(wú)法啟動(dòng)，換成102就可以了，實(shí)際上要更小才合適。
3.先看看程序有沒(méi)有進(jìn)入循環(huán)，還是卡在油門(mén)最小等待這一步，我在這兒加上串口打印配合調(diào)試
  PrintString1("注意準(zhǔn)備等待油門(mén)最小了\r\n");
        while(cnt_start < 25)        //等待油門(mén)最小        20ms * 25 = 500ms
        {
                if(B_PPM3_OK)        //油門(mén)
                {
                        B_PPM3_OK = 0;
                        if(PPM3_Cap <= 1200)        cnt_start++;
                }
                delay_ms(1);
        }
        PrintString1("油門(mén)最小測(cè)試通過(guò)\r\n");
當(dāng)油門(mén)通過(guò)后就是主要的工作啦，用串口不停地查看接收的數(shù)據(jù)是否與遙控一起在變化，
void        Return_Message(void)
{
        TX1_write2buff('V');
        TX1_write2buff('=');
        TX1_write2buff(Battery/1000 + '0');
        TX1_write2buff((Battery%1000)/100 + '0');
        TX1_write2buff('.');
        TX1_write2buff((Battery%100)/10 + '0');
        TX1_write2buff(Battery%10 + '0');
        TX1_write2buff(' ');
        TX1_write2buff(' ');
        //從這兒開(kāi)始
        PrintString1("PPM1=");
        TX1_int_value((int)(PPM1 * 10));
        PrintString1("PPM2=");
        TX1_int_value((int)(PPM2 * 10));
        PrintString1("PPM3=");
        TX1_int_value((int)(PPM3 * 10));
        PrintString1("PPM4=");
        TX1_int_value((int)(PPM4 * 10));
/*     PrintString1("AngleX=");
        TX1_int_value((int)(AngleX * 10));
        PrintString1("AngleY=");
        TX1_int_value((int)(AngleY * 10));
        PrintString1("AngleZ=");
        TX1_int_value((int)(Angle_gz * 10));
*/
        PrintString1("a_x=");
        TX1_int_value(a_x * 10);
        PrintString1("a_y=");
        TX1_int_value(a_y * 10);
        PrintString1("g_z=");
        TX1_int_value(g_z * 10);
        TX1_cnt = 0;
        TX1_write2buff(0x0d);
        TX1_write2buff(0x0a);
}
這樣就可以很直觀(guān)的看出來(lái)遙控的接收情況了，即使線(xiàn)接反了，也可以看出實(shí)時(shí)的數(shù)據(jù)變化，找出對(duì)應(yīng)的搖桿，因?yàn)檫@樣很方便，我把買(mǎi)的日本手該成了中國(guó)手。。。。。
4.然后晶振頻率和EEPROM大小不能錯(cuò)，遙控器通道正反要按資料圖片來(lái)?yè)埽�其他沒(méi)什么了，串口這兒就可以看出很多情況了，最后LQFP44我用的G版本

---

作者: chenyucy72    時(shí)間: 2022-5-25 08:23
哈哈，剛在官網(wǎng)上看到，然后在家里又看到，更想做一臺(tái)來(lái)玩了

---

作者: ysld    時(shí)間: 2024-3-11 14:13

謝謝分享，有時(shí)間做一個(gè)玩

---

歡迎光臨 (http://www.zg4o1577.cn/bbs/)

Powered by Discuz! X3.1

感谢您访问我们的网站，您可能还对以下资源感兴趣：

久久久久久久999|99精品久久精品一区二区爱城|成人欧美一区二区三区在线播放|国产精品日本一区二区不卡视频|国产午夜视频|欧美精品在线观看免费

主站蜘蛛池模板： 国产91在线精品 | 产真a观专区 | 超碰在线播 | 综合国产 | 一二三四在线视频观看社区 | 日操操夜操操 | 国产精品一区二区三级 | 日韩一区在线播放 | 欧美精品三区 | 精品久久久久久亚洲综合网 | 精品1区2区 | 精品一区二区三区四区在线 | 色播视频在线观看 | 日韩最新网站 | 91久久精品国产91久久性色tv | 天天影视综合 | 欧美一级毛片在线播放 | 成人免费视频 | 国外激情av | 97免费视频在线观看 | 国产乱性| 在线91 | 在线观看国产视频 | 久久九九99 | 国产一区二区三区四区三区四 | 黄色大片在线视频 | 亚洲国产精品久久久 | 精品国产青草久久久久96 | 久久久久久av | 国产婷婷综合 | 一级欧美| 欧美在线一区二区三区 | 四色永久 | www.中文字幕.com | 欧美一级网站 | 黄a免费看| 亚洲传媒在线 | 日韩欧美在线观看视频 | 久久久久国产精品一区三寸 | 一区二区三区欧美 | 亚洲欧美日本在线 |

<dfn id="6askc"></dfn><kbd id="6askc"></kbd>

<tr id="6askc"></tr>

<strike id="6askc"></strike>