单片机/MCU论坛
直播中

xymbmcu

12年用户 1025经验值
擅长:可编程逻辑
私信 关注
[文章]

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

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

  5. #define Check2401  1    //2401与mcu通信检测           置0屏蔽置1开启
  6. #define ReseveData 1    //飞行器接收数据控
  7. #define OutData    1                //飞行器内部数据输出

  9. void main()
  10. {
  11.     FlyAllInit();               //飞行器各模块初始化

  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"

  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;

  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();

  65. }

  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;

  77.         X_gyro = GetData(GYRO_XOUT_H);;
  78.     X_gyro/= 16.4;         //X-axis accelerometer
  79.         j = X_gyro;

  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. }

  97. short ygy(void)
  98. {

  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;

  107.         Y_gyro =   GetData(GYRO_YOUT_H);
  108.         Y_gyro/= 16.4;                                                  //Y-axis gyroscope
  109.         j = Y_gyro;

  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. }

  127. short zgy(void)
  128. {

  130.         short i,j,k;
  131.         short tmp;

  133.         Z_gyro =  GetData(GYRO_ZOUT_H);;
  134.         Z_gyro/= 16.4;
  135.         i = Z_gyro;

  137.         Z_gyro =  GetData(GYRO_ZOUT_H);
  138.         Z_gyro/= 16.4;
  139.         j = Z_gyro;

  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;

  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"

  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;

  220. float PWM_XZ,PWM_XF,PWM_YZ,PWM_YF;//?????pwm?
  221. float Yaw1=0;

  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;

  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 ;

  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;}

  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;}

  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. }

  322. extern float IntegralZ;
  323. #define MINPWM 0
  324. #define MAXPWM 2699

  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;

  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;

  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

  376.         if( PWM_XZ
  377.          PWM_XZ=MINPWM;
  378.         else if( PWM_XZ>MAXPWM)
  379.          PWM_XZ=MAXPWM;

  381.         if( PWM_XF
  382.          PWM_XF=MINPWM;
  383.         else if( PWM_XF>MAXPWM)
  384.          PWM_XF=MAXPWM;

  386.         if( PWM_YZ
  387.          PWM_YZ=MINPWM;
  388.         else if( PWM_YZ>MAXPWM)
  389.          PWM_YZ=MAXPWM;

  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;

  407. #define FOSC            11059200L
  408. #define BAUD            (65536 - FOSC / 4 / 115200)

  410. #define null           0
  411. #define FALSE           0
  412. #define TRUE            1

  414. //sfr  AUXR           =   0x8e;      //辅助寄存器
  415. //sfr P_SW1           =   0xa2;      //外设功能切换寄存器1
  416. #define SPI_S0          0x04
  417. #define SPI_S1          0x08

  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数据寄存器

  435. //***it SS             =   P2^4;                   //SPI的SS脚,连接到Flash的CE

  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

  451. void InitSpi();
  452. BYTE SpiShift(BYTE dat);

  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)

  460.     SPSTAT = SPIF | WCOL;                 //清除SPI状态
  461.     SPCTL = SSIG | SPEN | MSTR;    //设置SPI为主模式
  462.         IE2&=0XFC;
  463. }

  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"

  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};

  490. void NRF24L01_Init(void)
  491. {
  492.     InitSpi();
  493.         NRF24L01_CE=0;
  494.         NRF24L01_CSN=1;        
  495. }

  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. }                  

  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. }

  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. }        

  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. }

  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. }                                   

  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. }

  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);         

  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"

  619. #define RemoteControlData 0                //串口输出遥控器发送数值使能
  620. #define AttitudeData      1                        //串口输出姿态值使能
  621. #define RollData          1                        //输出y轴姿态
  622. #define PitchData         0                        //输出x轴姿态

  624. unsigned char RXBUF[33]={0,0,0,0,0,0,0,0};          //2401数据存储区
  625. int keyk=0;
  626. int ee=0;

  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. }

  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. }

  716. void Delay100us()                //@30.000MHz
  717. {
  718.         unsigned char i, j;

  720.         i = 3;
  721.         j = 232;
  722.         do
  723.         {
  724.                 while (--j);
  725.         } while (--i);
  726. }

  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();

  750. bit   I2C_RecvACK();

  752. void  I2C_SendByte(unsigned char dat);
  753. uchar I2C_RecvByte();

  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;

  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. }

  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

  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;        

  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;        

  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;        

  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. }

  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;

  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.         

  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 ;

  1015.           ex = (ay*vz - az*vy) ;
  1016.           ey = (az*vx - ax*vz) ;
  1017.           ez = (ax*vy - ay*vx) ;

  1019.           exInt = exInt + ex * Ki;
  1020.           eyInt = eyInt + ey * Ki;
  1021.           ezInt = ezInt + ez * Ki;

  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;

  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;

  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;

  1043.         Pitch =b;
  1044.         Roll = c;

  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;

  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. }

  1101. float MidFilter(float a,float b,float c)
  1102. {

  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;

  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"

  1200. float OutData[4] = { 0 };

  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;

  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. }

  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. }

  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异常标志

  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;

  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;

  1369.         PWM2T1H=PWMa>>8;        
  1370.         //15位寄存器第一次翻转计数  第一次翻转是指从低电平翻转到高电平的计时
  1371.         PWM2T1L= PWMa;

  1373.         PWM3T1H=PWMb>>8;
  1374.         PWM3T1L=  PWMb;

  1376.         PWM4T1H=PWMc>>8;
  1377.         PWM4T1L=  PWMc;

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

回帖（19）

孙学荣

2016-4-27 22:00:49
啊u盾萨比传达设计等哈就是的哈时间的哈
举报

马可菠萝

2016-4-28 12:33:01
看一下，比较感兴趣
举报

zhangdonglin

2016-5-10 15:57:15
看看，最近在开始做这个
举报

孙泽文

2016-5-10 16:37:25
赞一个！！！！！！！！！！！！！！
举报

snafceleplay175

2016-5-14 12:18:43
论坛代码的排版有点糟糕，看着累
举报

代码战士

2016-5-17 11:28:31
求这本书的PDF格式。
举报

lee_st

2016-5-25 20:06:35
看看。正在学习中，支持下！
举报

lee_st

2016-5-25 20:07:04
看看。正在学习中，支持下！
举报

lee_st

2016-5-25 20:07:28
看看。正在学习中，支持下！
举报

lee_st

2016-5-25 20:07:41
看看。正在学习中，支持下！
举报

烟艳炎龙

2016-6-16 22:27:00
6666666666666666666666666666666666666666666666666666666666666666666666666
举报

jiangyi

2016-6-25 22:28:44
同求啊，书出版了没啊
举报

Benj

2016-7-25 09:43:20
学习学习
举报

Stonx_sail

2016-9-11 14:16:45
学习。。。。。。。。。。。。。。。。。。。。。
举报

xymbmcu

2016-9-12 11:47:38
引用: jiangyimfs 发表于 2016-6-25 22:28
同求啊，书出版了没啊

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

charlion

2016-11-17 14:11:50
真不错
举报

张仕杰

2016-12-11 10:19:05

谢谢分享谢谢分享谢谢分享
       谢谢分享谢谢分享                       谢谢分享
       谢谢分享谢谢分享                谢谢分享         
                  谢谢分享              谢谢分享谢谢分享谢谢分享
             谢谢分享              谢谢分享        谢                谢
             谢谢分享              谢谢分享        谢                谢
             谢谢分享              谢谢分享        分                分
             谢谢分享              谢谢分享        享                享
             谢谢分享              谢谢分享        谢                谢
             谢谢分享              谢谢分享        谢                谢
谢        谢谢分享               谢谢分享       分                分
谢谢     谢谢分享              谢谢分享        享                享
谢谢分 谢谢分享              谢谢分享        谢                谢
谢谢分 谢谢分享                               谢       谢
谢谢分谢谢分享                         谢                 谢
         谢谢谢谢                    分                               分
             谢谢                   享                                     享
举报

何金明

2018-6-22 16:31:01
受到警告
提示: 作者被禁止或删除 内容自动屏蔽
举报

刘梦想

2018-12-2 23:10:21
四轴飞行器的设计之四轴主板的综合程序——STC15单片机实战指南连载
举报

更多回帖

19 0 15

相关帖子

单片机 飞行器 四轴飞行器
发帖
电子发烧友APP
|投诉反馈|电子发烧友网
© 2021 bbs.elecfans.com
湘ICP备2023018690号
×
20
完善资料，
赚取积分