四轴飞行器的设计之四轴主板的综合程序——STC15单片机实战指南连载

前面比较全面的讲述了四轴飞行器的硬件设计和软件中PID算法以及四元数与滤波算法，这节放出四轴主板的程序代码。　　
例程比较复杂，这里采用了模块化编程的思路，按功能将其各个函数进行分类。笔者刚开始计划这部分的例程不贴到书上，但怕读者又说读者开源不够，与此，将完整的一套四轴源码贴到这里，做到真正的开源，起到让读者学习的目的。但读者需要注意的，该例程不像前面的小实例那样简单，读者再学习的时候，要静下心来，边读例程，边理解，同时一定要做记录，达到真正的理解。再者，便于读者调试，这里保留了笔者调试时所用的串口示波器源码，这样，可大大降低读者调试的难度，同时，所有“.c”文件对应的头文件“.h”文件，这里全部省略，里面主要是对变量和函数的声明，学到这里，对于函数的声明等，应该能熟练于心了。接下来，我们开始上菜，读者慢慢“享用”。

1. /* =============         1        STC四轴正式版程序.c                 */
   
2. #include "includes.h"
   
3. #include "main.h"
   
4. 
   
5. #define Check2401  1    //2401与mcu通信检测           置0屏蔽置1开启
   
6. #define ReseveData 1    //飞行器接收数据控
   
7. #define OutData    1                //飞行器内部数据输出
   
8. 
   
9. void main()
   
10. {
    
11.     FlyAllInit();               //飞行器各模块初始化
    
12. 
    
13. #if Check2401
    
14.     Check24L01();                 //检测2401是否正常 不正常的话串口输出error
    
15. #endif        
    
16.     while(1)
    
17.     {         
    
18.       #if ReseveData
    
19.               Reseve2401();        //接收2401数据
    
20.       #endif                  
    
21.               
    
22.           #if  OutData
    
23.           OutPutData();                //输出飞机内部调试数据
    
24.           #endif                        
    
25.    }         
    
26. }
    
27.         /* =============         2        filtering.c                 */
    
28. #include
    
29. #include
    
30. #include
    
31. #include
    
32. #include
    
33. #include
    
34. #include <timer.h>
    
35. #include
    
36. #include
    
37. #include
    
38. #include "outputdata.h"
    
39. #include "stdio.h"
    
40. #include "filtering.h"
    
41. 
    
42. /*-- 3-axis accelerometer data collected from MPU --*/
    
43. short Z_angle,Y_angle,X_angle;
    
44. /*-- Y-axis gyroscope     data collected from MPU --*/
    
45. short Y_gyro,X_gyro,Z_gyro;
    
46. /*- The final angular data filtered by Kalman Filter --*/
    
47. short  X_gyroInit, Y_gyroInit, Z_gyroInit;
    
48. 
    
49. /*------- Data collection -------*/
    
50. void Read_MPU()
    
51. {
    
52.          
    
53.         Z_angle = GetData(ACCEL_ZOUT_H);;
    
54.         //Z_angle = (Z_angle/8192)*9.807;         //Z-axis accelerometer
    
55.          
    
56.         Y_angle = GetData(ACCEL_YOUT_H);
    
57.         //Y_angle = (Y_angle/8192)*9.807;         //Y-axis accelerometer
    
58.          
    
59.         X_angle = GetData(ACCEL_XOUT_H);
    
60.         //X_angle = (X_angle/8192)*9.807;
    
61.     X_gyro=xgy();
    
62.         Y_gyro=ygy();
    
63.         Z_gyro=zgy();
    
64. 
    
65. }
    
66. 
    
67. static short xgy(void)
    
68. {
    
69.          
    
70.         short i,j,k;
    
71.         short tmp;
    
72.    
    
73.         X_gyro = GetData(GYRO_XOUT_H);;
    
74.     X_gyro/= 16.4;         //X-axis accelerometer
    
75.         i = X_gyro;
    
76. 
    
77.         X_gyro = GetData(GYRO_XOUT_H);;
    
78.     X_gyro/= 16.4;         //X-axis accelerometer
    
79.         j = X_gyro;
    
80. 
    
81.         X_gyro = GetData(GYRO_XOUT_H);
    
82.     X_gyro/= 16.4;         //X-axis accelerometer
    
83.         k = X_gyro;
    
84.         if (i > j)
    
85.         {
    
86.                 tmp = i; i = j; j = tmp;
    
87.         }
    
88.         if (k > j)
    
89.           tmp = j;
    
90.         else if(k > i)
    
91.           tmp = k;
    
92.     else
    
93.       tmp = i;
    
94.         return tmp;
    
95. }
    
96. 
    
97. short ygy(void)
    
98. {
    
99. 
    
100.         short i,j,k;
     
101.         short tmp;
     
102.          
     
103.         Y_gyro = GetData(GYRO_YOUT_H);
     
104.         Y_gyro/= 16.4;                                                  //Y-axis gyroscope
     
105.         i = Y_gyro;
     
106. 
     
107.         Y_gyro =   GetData(GYRO_YOUT_H);
     
108.         Y_gyro/= 16.4;                                                  //Y-axis gyroscope
     
109.         j = Y_gyro;
     
110. 
     
111.         Y_gyro =   GetData(GYRO_YOUT_H);;
     
112.         Y_gyro/= 16.4;                                                  //Y-axis gyroscope
     
113.         k = Y_gyro;
     
114.         if (i > j)
     
115.         {
     
116.                 tmp = i; i = j; j = tmp;
     
117.         }
     
118.         if (k > j)
     
119.           tmp = j;
     
120.         else if(k > i)
     
121.           tmp = k;
     
122.     else
     
123.       tmp = i;
     
124.         return tmp;
     
125. }
     
126. 
     
127. short zgy(void)
     
128. {
     
129. 
     
130.         short i,j,k;
     
131.         short tmp;
     
132. 
     
133.         Z_gyro =  GetData(GYRO_ZOUT_H);;
     
134.         Z_gyro/= 16.4;
     
135.         i = Z_gyro;
     
136. 
     
137.         Z_gyro =  GetData(GYRO_ZOUT_H);
     
138.         Z_gyro/= 16.4;
     
139.         j = Z_gyro;
     
140. 
     
141.         Z_gyro =  GetData(GYRO_ZOUT_H);
     
142.         Z_gyro/= 16.4;
     
143.         k = Z_gyro;
     
144.         if (i > j)
     
145.         {
     
146.                 tmp = i; i = j; j = tmp;
     
147.         }
     
148.         if (k > j)
     
149.           tmp = j;
     
150.         else if(k > i)
     
151.           tmp = k;
     
152.     else
     
153.       tmp = i;
     
154.         return tmp;
     
155. }
     
156. void Delay1ms()                //@30.000MHz
     
157. {
     
158.         unsigned char i, j;
     
159. 
     
160.         i = 30;
     
161.         j = 43;
     
162.         do
     
163.         {
     
164.                 while (--j);
     
165.         } while (--i);
     
166. }
     
167. void gyro_init(void)
     
168. {
     
169.         int i=0;
     
170.         int x,y,z;
     
171.         short a,b,c;
     
172.         for(i=0;i<100;i++)
     
173.         {        
     
174.                 a=xgy() ;
     
175.                 b=ygy() ;
     
176.                 c=zgy() ;
     
177.                 x=x+a;
     
178.                 y=y+b;
     
179.                 z=z+c;
     
180.                 Delay1ms();
     
181.         }
     
182.     X_gyroInit=x/100;
     
183.     Y_gyroInit=y/100;
     
184.     Z_gyroInit=z/100;
     
185. }
     
186.         /* =============         3        control.c                 */
     
187. #include
     
188. #include
     
189. #include
     
190. #include
     
191. #include
     
192. #include
     
193. #include
     
194. #include
     
195. #include
     
196. #include
     
197. #include "outputdata.h"
     
198. #include "stdio.h"
     
199. #include "filtering.h"
     
200. #include "control.h"
     
201. #include "includes.h"
     
202. 
     
203. extern  unsigned char RXBUF[8];
     
204. extern  float Pitch;   //x
     
205. extern  float Roll;    //y
     
206. extern  float Average_Gx,Average_Gy,Average_Gz  ;
     
207. extern  int key;
     
208. unsigned int Controldata_THROTTLE_Set=0;//1880-50-50-100+30;
     
209. //************************************************
     
210. int  Controldata_THROTTLE=0  ;         //??
     
211. int  Controldata_PITCH   =0  ;         //??
     
212. int  Controldata_ROLL    =0  ;         //??
     
213. int  Controldata_YAW     =0  ;         //??
     
214. int  Controldata_OFFSET  =0  ;         //??
     
215. unsigned int   ControlNum=0  ;
     
216. //************************************************   
     
217. float  IUX=0.0,IUY=0.0;                              
     
218. float  COUT_PITCHZ,COUT_PITCHF,COUT_ROLLZ,COUT_ROLLF;
     
219. 
     
220. float PWM_XZ,PWM_XF,PWM_YZ,PWM_YF;//?????pwm?
     
221. float Yaw1=0;
     
222. 
     
223. //*************************************************
     
224. void MainControl()
     
225. {
     
226.         Get_Control_Data() ;
     
227.         Control();
     
228. }
     
229. //=============================================================
     
230. float Controldata_ROLLleft=0,Controldata_ROLLright=0;
     
231. float  Controldata_PITCHfront=0, Controldata_PITCHback=0;
     
232. int error=0;
     
233. extern int keyk;
     
234. extern unsigned int s;
     
235. int ss=0;
     
236. 
     
237. void Get_Control_Data()
     
238. {
     
239.         float a=0,b=530,c=521,d=527;
     
240.         if(keyk==1)
     
241.         {
     
242.                 d =    RXBUF[0]*16+RXBUF[1];         //oá1?
     
243.                 b =    RXBUF[2]*16+RXBUF[3];         //????
     
244.                 c =    RXBUF[4]*16+RXBUF[5];         //??o?
     
245.                 a =    RXBUF[6]*16+RXBUF[7];         //óí??
     
246.         //*********************脱控保护******************************        
     
247.                 if(error==1)
     
248.                 {
     
249.                         if(ss==0)
     
250.                         {ss=s;}
     
251.                         a=a-(s-ss)*20;
     
252.                 }         
     
253.                 else
     
254.                 ss=0;
     
255.         }
     
256.         //***********************************************************
     
257.         if(a>1024)
     
258.         {a=1024;}
     
259.         a= a/1024.0 ;
     
260. 
     
261.         if((b>=530&b<=540)||(b<=530&b>=520))
     
262.         {b=530;}
     
263.         b=b-530;
     
264.         if(b>0)
     
265.         {b=b/494.0;}
     
266.         else
     
267.         {b=b/530.0;}
     
268. 
     
269.         if((c>=521&c<=531)||(c<=521&c>=511))
     
270.         {c=521.0;}
     
271.         c=c-521;
     
272.         if(c>0)
     
273.         {c=c/523.0;}
     
274.         else
     
275.         {c=c/521.0;}
     
276. 
     
277.         if((d>=517&d<=527)||(d<=537&d>=527))
     
278.         {d=527;}
     
279.         d=d-527;
     
280.         if(d>0)
     
281.         {d=d/507.0;}
     
282.         else
     
283.         {d=d/527.0;}
     
284.                                                                           
     
285.         if(b>1)
     
286.          b=1;
     
287.         else if(b<-1)
     
288.          b=-1;
     
289.         //-------------
     
290.         if(c>1)
     
291.          c=1;
     
292.         else if(c<-1)
     
293.          c=-1;
     
294.         //-------------
     
295.         if(d>1)
     
296.          d=1;
     
297.         else if(d<-1)
     
298.          d=-1;  
     
299.         /*****************************/                                       
     
300.         Controldata_THROTTLE=a*2400;
     
301.         Controldata_PITCH=  b*20;
     
302.         Controldata_YAW =   c*180;
     
303.         Controldata_ROLL =  d*20;
     
304.                
     
305.         if( Controldata_PITCH>0)
     
306.         {Controldata_PITCHfront=0 ,Controldata_PITCHback=         Controldata_PITCH;}
     
307.         else
     
308.         {Controldata_PITCHfront=Controldata_PITCH ,Controldata_PITCHb        ack= 0;}
     
309.                  
     
310.         if(Controldata_ROLL>0)
     
311.         {
     
312.                  Controldata_ROLLleft=0;
     
313.                 Controldata_ROLLright=Controldata_ROLL;
     
314.         }
     
315.         else
     
316.         {
     
317.                 Controldata_ROLLleft=Controldata_ROLL;
     
318.                 Controldata_ROLLright=0;
     
319.         }
     
320. }
     
321. 
     
322. extern float IntegralZ;
     
323. #define MINPWM 0
     
324. #define MAXPWM 2699
     
325. 
     
326. void Control(void)
     
327. {
     
328.         float  EX0=0.0,EX1=0.0,EY0=0.0,EY1=0.0;      
     
329.         //-----------------------------  
     
330.         float PID_P_Y , PID_D_Y ;//, PID_I_Y;
     
331.         float PID_P_X , PID_D_X ;//, PID_I_X;
     
332.         float PID_P_Z , PID_D_Z;
     
333.         /***********************整定PID***************************/
     
334.         EX0=Pitch;  
     
335.         
     
336.         EY0=Roll;
     
337.         //********************************************************
     
338.         PID_P_Y=3.2;//4;//4.0;//3.1;  
     
339.         PID_D_Y=1.2;//0.4; 0.45; 0.43; 2.85; 0.6; 0.815+0.1; 0.86;
     
340. 
     
341.         PID_P_X=3.2;//3.2;;//4;//3.0;//9.41;//15-1-1-0.4;
     
342.         PID_D_X=1.2;//1.2;//1.2;//0.4;//0.4;//0.45;//0.6;//2.85;
     
343.         //0.6;//2.515;//1.555+1+0.3+0.2;
     
344. 
     
345.         PID_P_Z=0;//5;//0.2;
     
346.         PID_D_Z=0;//0.03;
     
347.         /**********************************************************/
     
348.         Yaw1=PID_P_Z*(IntegralZ-Controldata_YAW)+PID_D_Z* Average_Gz;
     
349.         if(Yaw1>300)
     
350.         {Yaw1=300;}
     
351.         else if(Yaw1<-300)
     
352.         {Yaw1=-300;}
     
353.         /*********************************************************/
     
354. PWM_XF= (float)(Controldata_THROTTLE)- (EX0-
     
355. Controldata_PITCHfront)*PID_P_X + (Average_Gy)*PID_D_X  -
     
356. (EY0-Controldata_ROLLleft)*PID_P_Y-(Average_Gx)*PID_D_Y +Yaw1;
     
357.     PWM_YF=(float)(Controldata_THROTTLE)-(EX0-
     
358.         Controldata_PITCHfront)*PID_P_X + (Average_Gy)*PID_D_X  + (EY0-
     
359.         Controldata_ROLLright)*PID_P_Y+(Average_Gx)*PID_D_Y -Yaw1;  
     
360. PWM_XZ=(float)(Controldata_THROTTLE)+ (EX0-
     
361. Controldata_PITCHback)*PID_P_X - (Average_Gy)*PID_D_X  - (EY0-
     
362. Controldata_ROLLleft)*PID_P_Y-(Average_Gx)*PID_D_Y  -Yaw1;  
     
363.         PWM_YZ=(float)(Controldata_THROTTLE)+ (EX0-
     
364.         Controldata_PITCHback)*PID_P_X – (Average_Gy)*PID_D_X + (EY0-
     
365.         Controldata_ROLLright)*PID_P_Y+(Average_Gx)*PID_D_Y +Yaw1;
     
366.         //**********************************************************
     
367.         #if 1         // 方便调试
     
368.          if(Controldata_THROTTLE<30)
     
369.          {
     
370.                  PWM_XF=PWM_YF= PWM_XZ= PWM_YZ=0;
     
371.                 IntegralZ=0;
     
372.                 PWM(0,0,0,0) ;
     
373.          }
     
374.         #endif
     
375. 
     
376.         if( PWM_XZ
377.          PWM_XZ=MINPWM;
     
378.         else if( PWM_XZ>MAXPWM)
     
379.          PWM_XZ=MAXPWM;
     
380. 
     
381.         if( PWM_XF
382.          PWM_XF=MINPWM;
     
383.         else if( PWM_XF>MAXPWM)
     
384.          PWM_XF=MAXPWM;
     
385. 
     
386.         if( PWM_YZ
387.          PWM_YZ=MINPWM;
     
388.         else if( PWM_YZ>MAXPWM)
     
389.          PWM_YZ=MAXPWM;
     
390. 
     
391.         if( PWM_YF
392.          PWM_YF=MINPWM;
     
393.         else if( PWM_YF>MAXPWM)
     
394.          PWM_YF=MAXPWM;
     
395.         /**********************************************/
     
396.         PWM(PWM_XZ,PWM_XF,PWM_YF,PWM_YZ) ;
     
397. }
     
398.         /* =============         4        spi.c                 */
     
399. #include "includes.h"
     
400. #include "spi.h"
     
401. #include
     
402. typedef bit BOOL;
     
403. typedef unsigned char BYTE;
     
404. typedef unsigned short WORD;
     
405. typedef unsigned long DWORD;
     
406. 
     
407. #define FOSC            11059200L
     
408. #define BAUD            (65536 - FOSC / 4 / 115200)
     
409. 
     
410. #define null           0
     
411. #define FALSE           0
     
412. #define TRUE            1
     
413. 
     
414. //sfr  AUXR           =   0x8e;      //辅助寄存器
     
415. //sfr P_SW1           =   0xa2;      //外设功能切换寄存器1
     
416. #define SPI_S0          0x04
     
417. #define SPI_S1          0x08
     
418. 
     
419. //sfr SPSTAT          =   0xcd;                   //SPI状态寄存器
     
420. #define SPIF            0x80                    //SPSTAT.7
     
421. #define WCOL            0x40                    //SPSTAT.6
     
422. //sfr SPCTL           =   0xce;                   //SPI控制寄存器
     
423. #define SSIG            0x80                    //SPCTL.7
     
424. #define SPEN            0x40                    //SPCTL.6
     
425. #define DORD            0x20                    //SPCTL.5
     
426. #define MSTR            0x10                    //SPCTL.4
     
427. #define CPOL            0x08                    //SPCTL.3
     
428. #define CPHA            0x04                    //SPCTL.2
     
429. #define SPDHH           0x00                    //CPU_CLK/4
     
430. #define SPDH            0x01                    //CPU_CLK/16
     
431. #define SPDL            0x02                    //CPU_CLK/64
     
432. #define SPDLL           0x03                    //CPU_CLK/128
     
433. //sfr SPDAT           =   0xcf;                   //SPI数据寄存器
     
434. 
     
435. //***it SS             =   P2^4;                   //SPI的SS脚,连接到Flash的CE
     
436. 
     
437. #define SFC_WREN        0x06                    //串行Flash命令集
     
438. #define SFC_WRDI        0x04
     
439. #define SFC_RDSR        0x05
     
440. #define SFC_WRSR        0x01
     
441. #define SFC_READ        0x03
     
442. #define SFC_FASTREAD    0x0B
     
443. #define SFC_RDID        0xAB
     
444. #define SFC_PAGEPROG    0x02
     
445. #define SFC_RDCR        0xA1
     
446. #define SFC_WRCR        0xF1
     
447. #define SFC_SECTORER    0xD7
     
448. #define SFC_BLOCKER     0xD8
     
449. #define SFC_CHIPER      0xC7
     
450. 
     
451. void InitSpi();
     
452. BYTE SpiShift(BYTE dat);
     
453. 
     
454. void InitSpi()
     
455. {
     
456.   ACC = P_SW1;                                       //切换到第一组SPI
     
457.   ACC &= ~(SPI_S0 | SPI_S1);           //SPI_S0=0 SPI_S1=0
     
458.   P_SW1 = ACC;            //(P1.2/SS, P1.3/MOSI, P1.4/MISO, P1.5/SCLK)
     
459. 
     
460.     SPSTAT = SPIF | WCOL;                 //清除SPI状态
     
461.     SPCTL = SSIG | SPEN | MSTR;    //设置SPI为主模式
     
462.         IE2&=0XFC;
     
463. }
     
464. 
     
465. /************************************************
     
466. 使用SPI方式与Flash进行数据交换
     
467. 入口参数:
     
468.     dat : 准备写入的数据
     
469. 出口参数:
     
470.     从Flash中读出的数据
     
471. ************************************************/
     
472. BYTE SpiShift(BYTE dat)
     
473. {
     
474.     SPDAT = dat;                          // 触发SPI发送
     
475.     while (!(SPSTAT & SPIF));          // 等待SPI数据传输完成
     
476.     SPSTAT = SPIF | WCOL;                         // 清除SPI状态
     
477.    
     
478.     return SPDAT;
     
479. }
     
480.         /* =============         5        NRF24L01.c                 */
     
481. #include
     
482. #include "nrf24l01.h"
     
483. #include "spi.h"
     
484. 
     
485. const unsigned char TX_ADDRESS[TX_ADR_WIDTH]=
     
486.                                                 {0x34,0x43,0x10,0x10,0x01};
     
487. const unsigned char RX_ADDRESS[RX_ADR_WIDTH]=
     
488.                                                 {0x34,0x43,0x10,0x10,0x01};
     
489. 
     
490. void NRF24L01_Init(void)
     
491. {
     
492.     InitSpi();
     
493.         NRF24L01_CE=0;
     
494.         NRF24L01_CSN=1;        
     
495. }
     
496. 
     
497. unsigned char NRF24L01_Check(void)
     
498. {
     
499.     unsigned char buf[5]={0XA5,0XA5,0XA5,0XA5,0XA5};
     
500.         unsigned char i;
     
501.          
     
502.         NRF24L01_Write_Buf(WRITE_REG1+TX_ADDR,buf,5);         
     
503.         NRF24L01_Read_Buf(TX_ADDR,buf,5);   
     
504.         for(i=0;i<5;i++) if(buf[i]!=0XA5) break;                                                                    
     
505.         if(i!=5) return 1;
     
506.         return 0;                 
     
507. }                  
     
508. 
     
509. unsigned char  NRF24L01_Write_Reg(unsigned char  reg,unsigned char  value)
     
510. {
     
511.         unsigned char  status;        
     
512.            NRF24L01_CSN=0;
     
513.           status =SpiShift(reg);
     
514.           SpiShift(value);
     
515.           NRF24L01_CSN=1;  
     
516.           return(status);               
     
517. }
     
518. 
     
519. unsigned char NRF24L01_Read_Reg(unsigned char reg)
     
520. {
     
521.         unsigned char  reg_val;            
     
522.          NRF24L01_CSN = 0;
     
523.           SpiShift(reg);
     
524.           reg_val=SpiShift(0XFF);
     
525.           NRF24L01_CSN = 1;                    
     
526.           return(reg_val);
     
527. }        
     
528. 
     
529. unsigned char NRF24L01_Read_Buf(unsigned char  reg,unsigned char  *pBuf,unsigned char  len)
     
530. {
     
531.         unsigned char  status,u8_ctr;               
     
532.           NRF24L01_CSN = 0;
     
533.           status=SpiShift(reg);           
     
534.          for(u8_ctr=0;u8_ctr
535.           NRF24L01_CSN=1;
     
536.           return status;
     
537. }
     
538. 
     
539. unsigned char  NRF24L01_Write_Buf(unsigned char  reg, unsigned char  *pBuf, unsigned char  len)
     
540. {
     
541.         unsigned char  status,u8_ctr;            
     
542.          NRF24L01_CSN = 0;
     
543.           status = SpiShift(reg);
     
544.           for(  u8_ctr=0; u8_ctr
545.           NRF24L01_CSN = 1;
     
546.           return status;
     
547. }                                   
     
548. 
     
549. unsigned char  NRF24L01_TxPacket(unsigned char  *txbuf)
     
550. {
     
551.         unsigned char  sta;
     
552.    
     
553.         NRF24L01_CE=0;
     
554.           NRF24L01_Write_Buf(WR_TX_PLOAD,txbuf,TX_PLOAD_WIDTH);
     
555.          NRF24L01_CE=1;
     
556.         while(NRF24L01_IRQ!=0);
     
557.         sta=NRF24L01_Read_Reg(STATUS);           
     
558.         NRF24L01_Write_Reg(WRITE_REG1+STATUS,sta);
     
559.         if(sta&MAX_TX)
     
560.         {
     
561.                 NRF24L01_Write_Reg(FLUSH_TX,0xff);
     
562.                 return MAX_TX;
     
563.         }
     
564.         if(sta&TX_OK)
     
565.         {
     
566.                 return TX_OK;
     
567.         }
     
568.         return 0xff;
     
569. }
     
570. 
     
571. unsigned char  NRF24L01_RxPacket(unsigned char  *rxbuf)
     
572. {
     
573.         unsigned char  sta;                                                                              
     
574.   
     
575.         sta=NRF24L01_Read_Reg(STATUS);            
     
576.         NRF24L01_Write_Reg(WRITE_REG1+STATUS,sta);
     
577.         if(sta&RX_OK)
     
578.         {
     
579.                 NRF24L01_Read_Buf(RD_RX_PLOAD,rxbuf,RX_PLOAD_WIDTH);
     
580.                 NRF24L01_Write_Reg(FLUSH_RX,0xff);
     
581.                 return 0;
     
582.         }           
     
583.         return 1;
     
584. }                                            
     
585.            
     
586. void RX_Mode(void)
     
587. {
     
588.         NRF24L01_CE=0;         
     
589.           NRF24L01_Write_Buf(WRITE_REG1+RX_ADDR_P0,(unsigned         char*)RX_ADDRESS,RX_ADR_WIDTH);
     
590.          
     
591.           NRF24L01_Write_Reg(WRITE_REG1+EN_AA,0x01);   
     
592.           NRF24L01_Write_Reg(WRITE_REG1+EN_RXADDR,0x01);
     
593.           NRF24L01_Write_Reg(WRITE_REG1+RF_CH,40);                  
     
594.           NRF24L01_Write_Reg(WRITE_REG1+RX_PW_P0,RX_PLOAD_WIDTH);   
     
595.           NRF24L01_Write_Reg(WRITE_REG1+RF_SETUP,0x0f);  
     
596.           NRF24L01_Write_Reg(WRITE_REG1+CONFIG1, 0x0f);
     
597.           NRF24L01_CE = 1;
     
598. }                                                
     
599.          
     
600. void TX_Mode(void)
     
601. {                                                                                                                 
     
602.         NRF24L01_CE=0;            
     
603.           NRF24L01_Write_Buf(WRITE_REG1+TX_ADDR,(unsigned         char*)TX_ADDRESS,TX_ADR_WIDTH);
     
604.           NRF24L01_Write_Buf(WRITE_REG1+RX_ADDR_P0,(unsigned         char*)RX_ADDRESS,RX_ADR_WIDTH);         
     
605. 
     
606.           NRF24L01_Write_Reg(WRITE_REG1+EN_AA,0x01);
     
607.           NRF24L01_Write_Reg(WRITE_REG1+EN_RXADDR,0x01);
     
608.           NRF24L01_Write_Reg(WRITE_REG1+SETUP_RETR,0x1a);
     
609.           NRF24L01_Write_Reg(WRITE_REG1+RF_CH,40);
     
610.           NRF24L01_Write_Reg(WRITE_REG1+RF_SETUP,0x0f);
     
611.           NRF24L01_Write_Reg(WRITE_REG1+CONFIG1,0x0e);
     
612.         NRF24L01_CE=1;
     
613. }                  
     
614.         /* =============         6        AllInit.c                 */
     
615. #include "includes.h"
     
616. #include "AllInit.h"
     
617. #include "main.h"
     
618. 
     
619. #define RemoteControlData 0                //串口输出遥控器发送数值使能
     
620. #define AttitudeData      1                        //串口输出姿态值使能
     
621. #define RollData          1                        //输出y轴姿态
     
622. #define PitchData         0                        //输出x轴姿态
     
623. 
     
624. unsigned char RXBUF[33]={0,0,0,0,0,0,0,0};          //2401数据存储区
     
625. int keyk=0;
     
626. int ee=0;
     
627. 
     
628. /************************************************
     
629. 函数功能:检测2401与单片机通信是否正常
     
630. *************************************************/
     
631. void Check24L01()
     
632. {
     
633.     while( NRF24L01_Check())
     
634.         {   
     
635.                 printf("24l01 is error,please replacement 24l01 module! n");
     
636.                 delayms__(100);
     
637.         }
     
638.         printf ("24l01 is ok!n");
     
639. }
     
640. /************************************************
     
641. 函数功能:初始化飞行器各个模块
     
642. *************************************************/
     
643. void FlyAllInit()
     
644. {
     
645.    delayms_(100);
     
646.    InitMPU6050();                              //初始化MPU-6050
     
647.    Usart_Init();                  //初始化串口
     
648.    PWMGO();                               //初始化PWM
     
649.    NRF24L01_Init();                        //初始化2401
     
650.    RX_Mode();                                        //设为接收模式
     
651.    Time0_Init();             //初始化定时器
     
652.    printf ("All module is ok!n Get ready to fly!n");
     
653. }
     
654. /************************************************
     
655. 函数功能:飞行器接收2401数据
     
656. *************************************************/
     
657. void Reseve2401(void)
     
658. {
     
659.         if(NRF24L01_RxPacket(RXBUF)==0)        
     
660.         {
     
661.                 if((RXBUF[6]*16+RXBUF[7])<30)
     
662.                 {keyk=1;}
     
663.                 ee=0;error=0;
     
664.                 RXBUF[32]=0;
     
665.         }
     
666.         else
     
667.         {  
     
668.                 if(keyk==1)
     
669.                 ee++;
     
670.         }
     
671.         if(ee>=200)
     
672.         {error=1;}
     
673. }
     
674. 
     
675. /************************************************
     
676. 函数功能:飞行器输出内部数据
     
677. *************************************************/
     
678. void OutPutData()
     
679. {
     
680. //遥控器数值输出
     
681. #if RemoteControlData
     
682.         OutData[0]=    RXBUF[0]*16+RXBUF[1];  //横滚        
     
683.         OutData[1]=    RXBUF[2]*16+RXBUF[3];  //俯仰        
     
684.         OutData[2]=    RXBUF[4]*16+RXBUF[5];  //偏航        
     
685.         OutData[3]=    RXBUF[6]*16+RXBUF[7];  //油门      
     
686.         OutPut_Data();        
     
687. #endif                                         
     
688. //姿态输出
     
689. #if AttitudeData
     
690.         #if        PitchData
     
691.                 OutData[0] = Pitch;
     
692.                 OutData[1] = X_angle;
     
693.                 OutData[2] = Average_Gy;
     
694.                 // OutData[3] = A_angle_Y;;
     
695.                 OutPut_Data();
     
696.         #endif
     
697.         #if RollData
     
698.                 OutData[0] = Roll;
     
699.                 OutData[1] = Y_angle;
     
700.                 OutData[2] = Average_Gx;
     
701.                 // OutData[3] = A_angle_Y;;
     
702.                 OutPut_Data();
     
703.         #endif  
     
704. #endif  
     
705. }
     
706. /************************************************
     
707. 函数名称:MotorTest()
     
708. 函数功能:飞行器电机测试
     
709. *************************************************/
     
710. void MotorTest(void)
     
711. {
     
712.         //数值设定 0~2700；同时要屏蔽IMU.c 中断里面的MainControl
     
713.         PWM(0,0,0,0);
     
714. }
     
715. 
     
716. void Delay100us()                //@30.000MHz
     
717. {
     
718.         unsigned char i, j;
     
719. 
     
720.         i = 3;
     
721.         j = 232;
     
722.         do
     
723.         {
     
724.                 while (--j);
     
725.         } while (--i);
     
726. }
     
727. 
     
728. void delayms__(int ms)
     
729. {
     
730.   int x,y;
     
731.   for(x=ms;x>0;x--)
     
732.   {
     
733.    for(y=1000;y>0;y--)
     
734.     ;
     
735.   }
     
736. }
     
737. /* =============         7        MPU-6050.c                 */
     
738. #include
     
739. #include
     
740. #include
     
741. #include
     
742. #define  uchar        unsigned char
     
743. ***it    SCL=P3^4;                        //IIC时钟引脚定义    Rev8.0硬件
     
744. ***it    SDA=P3^5;                        //IIC数据引脚定义
     
745. void  InitMPU6050();                                                                                //初始化MPU6050
     
746. void  Delay2us();
     
747. void  I2C_Start();
     
748. void  I2C_Stop();
     
749. 
     
750. bit   I2C_RecvACK();
     
751. 
     
752. void  I2C_SendByte(unsigned char dat);
     
753. uchar I2C_RecvByte();
     
754. 
     
755. void  I2C_ReadPage();
     
756. void  I2C_WritePage();
     
757. uchar Single_ReadI2C(uchar REG_Address);        //读取I2C数据
     
758. void  Single_WriteI2C(uchar REG_Address,uchar REG_data);        
     
759. //向I2C写入数据
     
760. //I^C时序中延时设置，具体参见各芯片的数据手册  6050推荐最小1.3us 但是会出问题，这里延时实际1.9us左右
     
761. void Delay2us()                //@27.000MHz
     
762. {
     
763.         unsigned char i;
     
764. 
     
765.         i = 11;
     
766.         while (--i);
     
767. }
     
768. //**************************************
     
769. //I2C起始信号
     
770. //**************************************
     
771. void I2C_Start()
     
772. {
     
773.     SDA = 1;                    //拉高数据线
     
774.     SCL = 1;                    //拉高时钟线
     
775.     Delay2us();                 //延时
     
776.     SDA = 0;                    //产生下降沿
     
777.     Delay2us();                 //延时
     
778.     SCL = 0;                    //拉低时钟线
     
779. }
     
780. //**************************************
     
781. //I2C停止信号
     
782. //**************************************
     
783. void I2C_Stop()
     
784. {
     
785.     SDA = 0;                    //拉低数据线
     
786.     SCL = 1;                    //拉高时钟线
     
787.     Delay2us();                 //延时
     
788.     SDA = 1;                    //产生上升沿
     
789.     Delay2us();                 //延时
     
790. }
     
791. //**************************************
     
792. //I2C接收应答信号
     
793. //**************************************
     
794. bit I2C_RecvACK()
     
795. {
     
796.     SCL = 1;                    //拉高时钟线
     
797.     Delay2us();                 //延时
     
798.     CY = SDA;                   //读应答信号
     
799.     SCL = 0;                    //拉低时钟线
     
800.     Delay2us();                 //延时
     
801.     return CY;
     
802. }
     
803. //**************************************
     
804. //向I2C总线发送一个字节数据
     
805. //**************************************
     
806. void I2C_SendByte(uchar dat)
     
807. {
     
808.     uchar i;
     
809.     for (i=0; i<8; i++)         //8位计数器
     
810.     {
     
811.         dat <<= 1;              //移出数据的最高位
     
812.         SDA = CY;               //送数据口
     
813.         SCL = 1;                //拉高时钟线
     
814.         Delay2us();             //延时
     
815.         SCL = 0;                //拉低时钟线
     
816.         Delay2us();             //延时
     
817.     }
     
818.     I2C_RecvACK();
     
819. }
     
820. //**************************************
     
821. //从I2C总线接收一个字节数据
     
822. //**************************************
     
823. uchar I2C_RecvByte()
     
824. {
     
825.     uchar i;
     
826.     uchar dat = 0;
     
827.     SDA = 1;                    //使能内部上拉,准备读取数据,
     
828.     for (i=0; i<8; i++)         //8位计数器
     
829.     {
     
830.         dat <<= 1;
     
831.         SCL = 1;                //拉高时钟线
     
832.         Delay2us();             //延时
     
833.         dat |= SDA;             //读数据
     
834.         SCL = 0;                //拉低时钟线
     
835.         Delay2us();             //延时
     
836.     }
     
837.     return dat;
     
838. }
     
839. //**************************************
     
840. //向I2C设备写入一个字节数据
     
841. //**************************************
     
842. void Single_WriteI2C(uchar REG_Address,uchar REG_data)
     
843. {
     
844.     I2C_Start();                  //起始信号
     
845.     I2C_SendByte(SlaveAddress);   //发送设备地址+写信号
     
846.     I2C_SendByte(REG_Address);    //内部寄存器地址，
     
847.     I2C_SendByte(REG_data);       //内部寄存器数据，
     
848.     I2C_Stop();                   //发送停止信号
     
849. }
     
850. //**************************************
     
851. //从I2C设备读取一个字节数据
     
852. //**************************************
     
853. uchar Single_ReadI2C(uchar REG_Address)
     
854. {
     
855.         uchar REG_data;
     
856.         I2C_Start();                   //起始信号
     
857.         I2C_SendByte(SlaveAddress);    //发送设备地址+写信号
     
858.         I2C_SendByte(REG_Address);     //发送存储单元地址，从0开始
     
859.         I2C_Start();                   //起始信号
     
860.         I2C_SendByte(SlaveAddress+1);  //发送设备地址+读信号
     
861.         REG_data=I2C_RecvByte();       //读出寄存器数据
     
862.         
     
863.         SDA = 1;                    //写应答信号
     
864.         SCL = 1;                    //拉高时钟线
     
865.         Delay2us();                 //延时
     
866.         SCL = 0;                    //拉低时钟线
     
867.         Delay2us();                 //延时
     
868.         
     
869.         I2C_Stop();                    //停止信号
     
870.         return REG_data;
     
871. }
     
872. 
     
873. //**************************************
     
874. //初始化MPU6050
     
875. //**************************************
     
876. void InitMPU6050()
     
877. {
     
878.         Single_WriteI2C(PWR_MGMT_1, 0x00);        //解除休眠状态
     
879.         Single_WriteI2C(SMPLRT_DIV, 0x07);  //陀螺仪125hz
     
880.         Single_WriteI2C(CONFIG, 0x06);      //21HZ滤波 延时A8.5ms G8.3ms  
     
881.         //此处取值应相当注意，延时与系统周期相近为宜
     
882.         Single_WriteI2C(GYRO_CONFIG, 0x18); //陀螺仪500度/S 65.5LSB/g
     
883.         Single_WriteI2C(ACCEL_CONFIG, 0x01);//加速度+-4g  8192LSB/g
     
884. }
     
885. //**************************************
     
886. //合成数据
     
887. //**************************************
     
888. int GetData(uchar REG_Address)
     
889. {
     
890.         char H,L;
     
891.         H=Single_ReadI2C(REG_Address);
     
892.         L=Single_ReadI2C(REG_Address+1);
     
893.         return (H<<8)+L;   //合成数据
     
894. }
     
895. /* =============         8        IMU.c                 */
     
896. #include
     
897. #include "includes.h"
     
898. #include "IMU.H"
     
899. #include "math.H"
     
900.                            
     
901. //#define Kp  28.1f   //10.1f
     
902. //#define Ki   0.001f//0.011f                        
     
903. //#define halfT 0.0020f   
     
904. #define Kp   15.1f  //10.1f                        
     
905. #define Ki   0.001        //0.001f//0.011f                        
     
906. #define halfT 0.001        //0.0019f     
     
907. float idata q0=1,q1=0,q2=0,q3=0;   
     
908. float idata exInt=0,eyInt=0,ezInt=0;  
     
909. float         Pitch ,        Roll;
     
910. float a,b,c;
     
911. float IntegralZ;
     
912. /*********************************/
     
913. #define KALMAN_Qy        0.015
     
914. #define KALMAN_Ry        10.0000
     
915. #define KALMAN_Qx        0.015
     
916. #define KALMAN_Rx       10.0000
     
917. #define KALMAN_Qz        0.015
     
918. #define KALMAN_Rz        10.0000
     
919. 
     
920. static double KalmanFilter_x(const double ResrcData,double ProcessNiose_Q,double MeasureNoise_R)
     
921. {
     
922.         double R = MeasureNoise_R;
     
923.            double Q = ProcessNiose_Q;
     
924.            static double x_last;
     
925.            double x_mid = x_last;
     
926.            double x_now;
     
927.            static double p_last;
     
928.            double p_mid ;
     
929.            double p_now;
     
930.            double kg;        
     
931. 
     
932.            x_mid=x_last;                 //x_last=x(k-1|k-1),x_mid=x(k|k-1)
     
933.            p_mid=p_last+Q;                 //p_mid=p(k|k-1),p_last=p(k-1|k-1),
     
934.            kg=p_mid/(p_mid+R);
     
935.            x_now=x_mid+kg*(ResrcData-x_mid);
     
936.                
     
937.            p_now=(1-kg)*p_mid;   
     
938.            p_last = p_now;
     
939.            x_last = x_now;
     
940.            return x_now;               
     
941. }
     
942. static double KalmanFilter_y(const double ResrcData,double ProcessNiose_Q,double MeasureNoise_R)
     
943. {
     
944.            double R = MeasureNoise_R;
     
945.            double Q = ProcessNiose_Q;
     
946.            static double y_last;
     
947.            double y_mid = y_last;
     
948.            double y_now;
     
949.            static double p_last;
     
950.            double p_mid ;
     
951.            double p_now;
     
952.            double kg;        
     
953. 
     
954.            y_mid=y_last;                 //x_last=x(k-1|k-1),x_mid=x(k|k-1)
     
955.            p_mid=p_last+Q;                 //p_mid=p(k|k-1),p_last=p(k-1|k-1),Q=??éù
     
956.            kg=p_mid/(p_mid+R);
     
957.            y_now=y_mid+kg*(ResrcData-y_mid);
     
958.                
     
959.            p_now=(1-kg)*p_mid;   
     
960.            p_last = p_now;
     
961.            y_last = y_now;
     
962.            return y_now;               
     
963. }
     
964. static double KalmanFilter_z(const double ResrcData,double ProcessNiose_Q,double MeasureNoise_R)
     
965. {
     
966.            double R = MeasureNoise_R;
     
967.            double Q = ProcessNiose_Q;
     
968.            static double z_last;
     
969.            double z_mid = z_last;
     
970.            double z_now;
     
971.            static double p_last;
     
972.            double p_mid ;
     
973.            double p_now;
     
974.            double kg;        
     
975. 
     
976.            z_mid=z_last;                 //x_last=x(k-1|k-1),x_mid=x(k|k-1)
     
977.            p_mid=p_last+Q;
     
978.            kg=p_mid/(p_mid+R);
     
979.            z_now=z_mid+kg*(ResrcData-z_mid);
     
980.                
     
981.            p_now=(1-kg)*p_mid;   
     
982.            p_last = p_now;
     
983.            z_last = z_now;
     
984.            return z_now;               
     
985. }
     
986. 
     
987. void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
     
988. {
     
989.          float norm;
     
990.         //  float hx, hy, hz, bx, bz;
     
991.           float vx, vy, vz;// wx, wy, wz;
     
992.           float ex, ey, ez;
     
993. 
     
994.           float q0q0 = q0*q0;
     
995.           float q0q1 = q0*q1;
     
996.           float q0q2 = q0*q2;
     
997.          // float q0q3 = q0*q3;
     
998.           float q1q1 = q1*q1;
     
999.          // float q1q2 = q1*q2;
     
1000.           float q1q3 = q1*q3;
      
1001.           float q2q2 = q2*q2;
      
1002.           float q2q3 = q2*q3;
      
1003.           float q3q3 = q3*q3;
      
1004.         
      
1005. 
      
1006.           norm = sqrt(ax*ax + ay*ay + az*az);
      
1007.           ax = ax /norm;
      
1008.           ay = ay / norm;
      
1009.           az = az / norm;
      
1010.            
      
1011.           vx = 2*(q1q3 - q0q2);
      
1012.           vy = 2*(q0q1 + q2q3);
      
1013.           vz = q0q0 - q1q1 - q2q2 + q3q3 ;
      
1014. 
      
1015.           ex = (ay*vz - az*vy) ;
      
1016.           ey = (az*vx - ax*vz) ;
      
1017.           ez = (ax*vy - ay*vx) ;
      
1018. 
      
1019.           exInt = exInt + ex * Ki;
      
1020.           eyInt = eyInt + ey * Ki;
      
1021.           ezInt = ezInt + ez * Ki;
      
1022. 
      
1023.           gx = gx + Kp*ex + exInt;
      
1024.           gy = gy + Kp*ey + eyInt;
      
1025.           gz = gz + Kp*ez + ezInt;
      
1026.         
      
1027.                                           
      
1028.           q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
      
1029.           q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
      
1030.           q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
      
1031.           q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;
      
1032. 
      
1033.           norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
      
1034.           q0 = q0 / norm;
      
1035.           q1 = q1 / norm;
      
1036.           q2 = q2 / norm;
      
1037.           q3 = q3 / norm;
      
1038. 
      
1039.           b = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
      
1040.           c = atan2(2 * q2 * q3 + 2 * q0 * q1, q0 * q0 - q1 * q1 - q2 * q2         + q3 * q3)* 57.3; // roll
      
1041.           //d = atan2(2 * q1 * q2+2 * q0 * q3,q0 * q0+q1 * q1-q2 * q2-q3         * q3)*57.3;
      
1042. 
      
1043.         Pitch =b;
      
1044.         Roll = c;
      
1045. 
      
1046. }
      
1047. /************************************************************/
      
1048. extern   short Y_gyro,X_gyro,Z_gyro;
      
1049. extern   short Z_angle,Y_angle,X_angle;
      
1050. extern  short  X_gyroInit, Y_gyroInit, Z_gyroInit;
      
1051. float         Average_Gx,Average_Gy,Average_Gz,Average_Ax,Average_Ay,
      
1052.                 Average_Az=0;
      
1053. float Average_Ax_old[4],Average_Ay_old[4],Average_Az_old[4];
      
1054. float A_angle_X,A_angle_Y,A_angle_Z;
      
1055. /*------- Data processing -------*/
      
1056. void MPU_pro()
      
1057. {
      
1058.         Average_Gx=X_gyro;//+15;//- X_gyroInit;
      
1059.         Average_Gy=Y_gyro;//+15;//- Y_gyroInit;
      
1060.         Average_Gz=Z_gyro;//- Z_gyroInit;
      
1061.         
      
1062.         Average_Ax=X_angle;
      
1063.         Average_Ay=Y_angle;
      
1064.     Average_Az=Z_angle;
      
1065. 
      
1066.         Average_Ax_old[2]=Average_Ax_old[1];
      
1067.         Average_Ay_old[2]=Average_Ay_old[1];
      
1068.         Average_Az_old[2]=Average_Az_old[1];
      
1069.         Average_Ax_old[1]=Average_Ax_old[0];
      
1070.         Average_Ay_old[1]=Average_Ay_old[0];
      
1071.         Average_Az_old[1]=Average_Az_old[0];
      
1072.         Average_Ax_old[0]=Average_Ax;
      
1073.         Average_Ay_old[0]=Average_Ay;
      
1074.         Average_Az_old[0]=Average_Az;
      
1075. //**************************************************        
      
1076.          if(Average_Ay_old[2]!=0)
      
1077.          {
      
1078.                  IntegralZ+= Average_Gz        *0.01;
      
1079.                  if( IntegralZ>=359)
      
1080.                   IntegralZ=0;
      
1081.                   if( IntegralZ<=-359)
      
1082.                    IntegralZ=0;
      
1083.          
      
1084.                 Average_Ax=MidFilter( Average_Ax_old[0],Average_Ax_old[1],                 Average_Ax_old[2]);  
      
1085.                 Average_Ay=MidFilter( Average_Ay_old[0],Average_Ay_old[1],                 Average_Ay_old[2]);  
      
1086.             Average_Az=MidFilter( Average_Az_old[0],Average_Az_old[1],                 Average_Az_old[2]);   
      
1087.         
      
1088.                 A_angle_X=atan((float)(Average_Ax/sqrt(Average_Ay*
      
1089.                 Average_Ay+Average_Az*Average_Az)))*57.3;
      
1090.                 A_angle_Y =atan((float)(Average_Ay/sqrt(Average_Ax*
      
1091.                 Average_Ax+Average_Az*Average_Az)))*57.3;
      
1092.                 A_angle_Z =atan((float)(Average_Az/sqrt(Average_Ax*
      
1093.                 Average_Ax+Average_Ay*Average_Ay)))*57.3;
      
1094.                 //IMUupdate( Average_Gx*0.05044,Average_Gy*-0.05044,
      
1095.                 //Average_Gz*0.03744,-Average_Ax,Average_Ay, Average_Az);
      
1096.             IMUupdate( Average_Gx*0.05544,Average_Gy*-0.05544,
      
1097.                 Average_Gz*0.05544,-Average_Ax,Average_Ay, Average_Az);
      
1098.         }
      
1099. }
      
1100. 
      
1101. float MidFilter(float a,float b,float c)
      
1102. {
      
1103. 
      
1104.         float i,j,k;
      
1105.         float tmp;
      
1106.          
      
1107.         i = a;
      
1108.         j = b;
      
1109.         k = c;
      
1110.         if (i > j)
      
1111.         {
      
1112.                 tmp = i; i = j; j = tmp;
      
1113.         }
      
1114.         if (k > j)
      
1115.           tmp = j;
      
1116.         else if(k > i)
      
1117.           tmp = k;
      
1118.     else
      
1119.       tmp = i;
      
1120.         return tmp;
      
1121. }
      
1122. extern int  Controldata_THROTTLE  ;
      
1123. int mmms=0;
      
1124. int s=0;
      
1125. 
      
1126. void Angle_Calculate() interrupt 1
      
1127. {         
      
1128.       Read_MPU();          // 读取mpu6050数据
      
1129.       MPU_pro();                   // 进行imu数据转换
      
1130.       MainControl();             // 进行姿态控制
      
1131. }
      
1132.         /* =============         9        USART.c                 */
      
1133. #include
      
1134. #include
      
1135. #include    
      
1136. bit busy;
      
1137. void Usart_Init()  //30m 波特率9600
      
1138. {               
      
1139.          SCON = 0x50;                //8位数据,可变波特率
      
1140.     AUXR |= 0x40;                //定时器1时钟为Fosc,即1T
      
1141.         AUXR &= 0xFE;                //串口1选择定时器1为波特率发生器
      
1142.         TMOD &= 0x0F;                //设定定时器1为16位自动重装方式
      
1143.         TL1 = 0x41;                        //设定定时初值
      
1144.         TH1 = 0xFD;                        //设定定时初值
      
1145.         ET1 = 0;                        //禁止定时器1中断
      
1146.         TR1 = 1;                        //启动定时器1
      
1147.         REN=1;
      
1148.         ES=1;
      
1149.         EA=1;
      
1150.         TI=1;
      
1151. }
      
1152. void Uart() interrupt 4 using 1
      
1153. {
      
1154.     if (RI)
      
1155.     {
      
1156.         RI = 0;                       
      
1157.     }
      
1158.     if (TI)
      
1159.     {
      
1160.         TI = 0;               
      
1161.         busy = 0;            
      
1162.     }
      
1163. }
      
1164. void SendData(unsigned char dat)
      
1165. {
      
1166.         SBUF = dat;
      
1167.         while(!TI);
      
1168.           TI = 0;      
      
1169. }
      
1170. void Send(int Ax,int Ay,int Az,int Gx,int Gy,int Gz)
      
1171. {
      
1172.         unsigned char sum=0;
      
1173.         ES = 1;                          //打开串口中断
      
1174.         SendData(0xAA);           //帧头
      
1175.         SendData(0xAA);           //帧头
      
1176.         SendData(0x02);           //功能字
      
1177.         SendData(12);             //发送的数据长度
      
1178.         SendData(Ax);             //低8位
      
1179.         SendData(Ax>>8);          //高8位
      
1180.         SendData(Ay);
      
1181.         SendData(Ay>>8);
      
1182.         SendData(Az);
      
1183.         SendData(Az>>8);
      
1184.         SendData(Gx);
      
1185.         SendData(Gx>>8);
      
1186.         SendData(Gy);
      
1187.         SendData(Gy>>8);
      
1188.         SendData(Gz);
      
1189.         SendData(Gz>>8);
      
1190.         sum+=0xAA;sum+=0xAA;sum+=0x02;sum+=12;
      
1191.         sum+=Ax>>8;sum+=Ax;sum+=Ay>>8;sum+=Ay;sum+=Az>>8;sum+=Az;
      
1192.         sum+=Gx>>8;sum+=Gx;sum+=Gy>>8;sum+=Gy;sum+=Gz>>8;sum+=Gz;
      
1193.         SendData(sum);          //校验和
      
1194.         ES = 0;                           //关闭串口中断
      
1195. }
      
1196.         /* =============         10        outputdata.c                 */
      
1197. #include "outputdata.h"
      
1198. #include "USART.h"
      
1199. 
      
1200. float OutData[4] = { 0 };
      
1201. 
      
1202. unsigned short CRC_CHECK(unsigned char *Buf, unsigned char CRC_CNT)
      
1203. {
      
1204.     unsigned short CRC_Temp;
      
1205.     unsigned char i,j;
      
1206.     CRC_Temp = 0xffff;
      
1207. 
      
1208.     for (i=0;i
1209.         CRC_Temp ^= Buf[i];
      
1210.         for (j=0;j<8;j++) {
      
1211.             if (CRC_Temp & 0x01)
      
1212.                 CRC_Temp = (CRC_Temp >>1 ) ^ 0xa001;
      
1213.             else
      
1214.                 CRC_Temp = CRC_Temp >> 1;
      
1215.         }
      
1216.     }
      
1217.         return(CRC_Temp);
      
1218. }
      
1219. 
      
1220. void OutPut_Data(void)
      
1221. {
      
1222.           int temp[4] = {0};
      
1223.           unsigned int temp1[4] = {0};
      
1224.           unsigned char databuf[10] = {0};
      
1225.           unsigned char i;
      
1226.           unsigned short CRC16 = 0;
      
1227.           for(i=0;i<4;i++)
      
1228.            {
      
1229.             temp[i]  = (int)OutData[i];
      
1230.             temp1[i] = (unsigned int)temp[i];
      
1231.            }
      
1232.    
      
1233.           for(i=0;i<4;i++)
      
1234.           {
      
1235.             databuf[i*2]   = (unsigned char)(temp1[i]%256);
      
1236.             databuf[i*2+1] = (unsigned char)(temp1[i]/256);
      
1237.           }
      
1238.   
      
1239.           CRC16 = CRC_CHECK(databuf,8);
      
1240.           databuf[8] = CRC16%256;
      
1241.           databuf[9] = CRC16/256;
      
1242.   
      
1243.           for(i=0;i<10;i++)
      
1244.                    SendData(databuf[i]);
      
1245. }
      
1246. /***************************************************************
      
1247. * 功    能：串口示波器放数据用
      
1248. ***************************************************************/
      
1249. void uart_putstr(char ch[])
      
1250. {
      
1251.           unsigned char ptr=0;
      
1252.           while(ch[ptr]){
      
1253.     SendData((char)ch[ptr++]);
      
1254. }
      
1255. }
      
1256.         /* =============         11        Timer.c                 */
      
1257. #include
      
1258. #include
      
1259. void Time0_Init()                //10ms@27MHz 定时器0 16位12T自动重载
      
1260. {
      
1261.         AUXR &= 0x7F;                //定时器时钟12T模式
      
1262.         TMOD &= 0xF0;                //设置定时器模式
      
1263.     TL0 = 0x1C;                //设置定时初值
      
1264.         TH0 = 0xA8;                //设置定时初值
      
1265.         TF0 = 0;                        //清除TF0标志
      
1266.         TR0 = 1;                        //定时器0开始计时
      
1267.     IE=0X82        ;
      
1268.         EA=1;
      
1269. }
      
1270. 
      
1271. void Timer1Init(void)
      
1272. {
      
1273.         //        AUXR |= 0x40;
      
1274.         TMOD &= 0x0F;
      
1275.         IE  = 0x8a;
      
1276.         TL1 = 0x54;
      
1277.         TH1 = 0xF2;
      
1278.         TF1 = 0;
      
1279.         TR1 = 1;
      
1280. }
      
1281.         /* =============         12        STC15W4KPWM.c                 */
      
1282. #include
      
1283. #include
      
1284. #include
      
1285. #include
      
1286. extern unsigned char RxBuf[20];
      
1287. void PWMGO()
      
1288. {
      
1289.         //所有I/O口全设为准双向，弱上拉模式
      
1290.         P0M0=0x00;P0M1=0x00;
      
1291.         P1M0=0x00;P1M1=0x00;
      
1292.         P2M0=0x00;P2M1=0x00;
      
1293.         P3M0=0x00;P3M1=0x00;
      
1294.         P4M0=0x00;P4M1=0x00;
      
1295.         P5M0=0x00;P5M1=0x00;
      
1296.         P6M0=0x00;P6M1=0x00;
      
1297.         P7M0=0x00;P7M1=0x00;
      
1298.         //设置需要使用的PWM输出口为强推挽模式
      
1299.         P2M0=0x0e;
      
1300.         P2M1=0x00;
      
1301.         P3M0=0x80;
      
1302.         P3M1=0x00;
      
1303.         
      
1304.         P_SW2=0x80;    //最高位置1才能访问和PWM相关的特殊寄存器
      
1305.         
      
1306.     PWMCFG=0xb0;    //7位6位5位4位3位 2位 1位 0位
      
1307.         //置0 1-计数器归零触发ADC C7INI  C6INI  C5INI  C4INI  C3INI  C2INI
      
1308.         // 0-归零时不触发ADC  (值为1时上电高电平，为0低电平）   
      
1309.         
      
1310.         PWMCKS=0x10;        //  置0   0-系统时钟分频   分频参数设定
      
1311.                         //  1-定时器2溢出   时钟=系统时钟/([3:0]+1)
      
1312.         
      
1313.         PWMIF=0x00;    //置0  计数器归零中断标志    相应PWM端口中断标志
      
1314.         
      
1315.         PWMFDCR=0x00;   //7位    6位  5位   4位     
      
1316.         //置0    置0 外部异常检测开关  外部异常时0-无反应 1-高阻状态
      
1317.         //3位             2位                 1位                0位      
      
1318.     //PWM异常中断  比较器与异常的关系   P2.4与异常的关系  PWM异常标志
      
1319. 
      
1320.         PWMCH=0x0b;//15位寄存器，决定PWM周期，数值为1-32767，单位：脉冲时钟
      
1321.         PWMCL=0xb9;
      
1322.         
      
1323.         // 以下为每个PWM输出口单独设置
      
1324.         PWM2CR=0x00;  //7位6位5位4位   3位      2位       1位        0位
      
1325.                         //    置0      输出切换 中断开关 T2中断开关 T1中断开关
      
1326.         PWM3CR=0x00;
      
1327.         PWM4CR=0x00;
      
1328.         PWM5CR=0x00;
      
1329.         
      
1330.         PWM2T1H=0x0a;
      
1331.         //15位寄存器第一次翻转计数  第一次翻转是指从低电平翻转到高电平的计时
      
1332.         PWM2T1L=0x8c;
      
1333.         PWM2T2H=0x0a;         
      
1334.         //15位寄存器第二次翻转计数  第二次翻转是指从高电平翻转到低电平的计时
      
1335.         PWM2T2L=0x8d;    //第二次翻转应比精度等级要高，否则会工作不正常
      
1336.         // 比如精度1000，第二次翻转就必须小于1000
      
1337.          
      
1338.         PWM3T1H=0x0a;
      
1339.         PWM3T1L=0x8c;
      
1340.         PWM3T2H=0x0a;  
      
1341.         PWM3T2L=0x8d;
      
1342.          
      
1343.         PWM4T1H=0x0a;
      
1344.     PWM4T1L=0x8c;
      
1345.         PWM4T2H=0x0a;  
      
1346.         PWM4T2L=0x8d;
      
1347.          
      
1348.         PWM5T1H=0x0a;
      
1349.         PWM5T1L=0x8c;
      
1350.         PWM5T2H=0x0a;  
      
1351.         PWM5T2L=0x8d;
      
1352.         //以上为每个PWM输出口单独设置
      
1353. PWMCR=0x8f;   
      
1354. //7位/6位5位 4位 3位 2位 1位 0位     10001111
      
1355.         //PWM开关 计数归零中断开关   相应I/O为GPIO模式(0)或PWM模式(1)                                                               
      
1356.         PWMCKS=0x00;
      
1357. 
      
1358.         PWM(0,0,0,0);         
      
1359. }
      
1360. //本函数输入的4个值取值范围为0-3000 0电机停，1000转速最高
      
1361. //输入数据不能超过取值范围;
      
1362. void PWM(unsigned int PWMa,unsigned int PWMb,unsigned int PWMc,unsigned int PWMd)
      
1363. {  
      
1364.         PWMa=2700-PWMa;
      
1365.         PWMb=2700-PWMb;
      
1366.         PWMc=2700-PWMc;
      
1367.         PWMd=2700-PWMd;
      
1368. 
      
1369.         PWM2T1H=PWMa>>8;        
      
1370.         //15位寄存器第一次翻转计数  第一次翻转是指从低电平翻转到高电平的计时
      
1371.         PWM2T1L= PWMa;
      
1372. 
      
1373.         PWM3T1H=PWMb>>8;
      
1374.         PWM3T1L=  PWMb;
      
1375. 
      
1376.         PWM4T1H=PWMc>>8;
      
1377.         PWM4T1L=  PWMc;
      
1378. 
      
1379.         PWM5T1H=PWMd>>8;
      
1380.         PWM5T1L= PWMd;         
      
1381. }
      

复制代码

最后说明下和本文配套的STC15开发板目前正在电子发烧友销售，如果需要请戳这里购买： https://bbs.elecfans.com/product/stc15.html  我将持续更新内容。

啊u盾萨比传达设计等哈就是的哈时间的哈

看一下，比较感兴趣

看看，最近在开始做这个

赞一个！！！！！！！！！！！！！！

论坛代码的排版有点糟糕，看着累

求这本书的PDF格式。

看看。正在学习中，支持下！

看看。正在学习中，支持下！

看看。正在学习中，支持下！

看看。正在学习中，支持下！

6666666666666666666666666666666666666666666666666666666666666666666666666

同求啊，书出版了没啊

学习学习

学习。。。。。。。。。。。。。。。。。。。。。

jiangyimfs 发表于 2016-6-25 22:28
同求啊，书出版了没啊

书出版了，可以到京东购买了

真不错

谢谢分享谢谢分享谢谢分享
       谢谢分享谢谢分享                       谢谢分享
       谢谢分享谢谢分享                谢谢分享         
                  谢谢分享              谢谢分享谢谢分享谢谢分享
             谢谢分享              谢谢分享        谢                谢
             谢谢分享              谢谢分享        谢                谢
             谢谢分享              谢谢分享        分                分
             谢谢分享              谢谢分享        享                享
             谢谢分享              谢谢分享        谢                谢
             谢谢分享              谢谢分享        谢                谢
谢        谢谢分享               谢谢分享       分                分
谢谢     谢谢分享              谢谢分享        享                享
谢谢分 谢谢分享              谢谢分享        谢                谢
谢谢分 谢谢分享                               谢       谢
谢谢分谢谢分享                         谢                 谢
         谢谢谢谢                    分                               分
             谢谢                   享                                     享

四轴飞行器的设计之四轴主板的综合程序——STC15单片机实战指南连载