找回密碼
 立即注冊(cè)

QQ登錄

只需一步，快速開始

搜索
查看: 26718|回復(fù): 33
打印 上一主題 下一主題
收起左側(cè)

STC51單片機(jī)四軸飛控開源項(xiàng)目原理圖與源碼資料分享

  [復(fù)制鏈接]
跳轉(zhuǎn)到指定樓層
#
ID:253663 發(fā)表于 2017-11-26 11:19 | 只看該作者 |只看大圖 回帖獎(jiǎng)勵(lì) |正序?yàn)g覽 |閱讀模式
四軸飛控-STC8A8K64S4A12-LQFP44-PPM-V10，來源于STC發(fā)布，按需取用
控制信號(hào)排針要靠在一起布線成3排針3*9*2.54
P1~P4對(duì)應(yīng)接收機(jī)的俯仰、橫滾、油門、航向PPM信號(hào)。M1~M4輸出到4個(gè)電調(diào)的信號(hào)。P5備用。
四個(gè)電機(jī)安排順序如下：

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


原理圖：



飛控的stc單片機(jī)程序源碼：
  1. /* 本程序經(jīng)過測(cè)試完全正常, 不提供電話技術(shù)支持, 如不能理解, 請(qǐng)自行補(bǔ)充相關(guān)基礎(chǔ).  */

  3. /***  特別注意: 下載時(shí)選擇內(nèi)部時(shí)鐘24MHZ, 設(shè)置用戶EEPROM大小為2K或以上.  ****/

  5. /*********************************************
  6.   四軸飛控-V10.C

  8. 使用遙控接收器型號(hào): MC6B六通道2.4G 100mW.

  10. 四軸上電待機(jī)：上電后，航燈不亮，接收機(jī)LED閃爍，此時(shí)打開遙控器，將左右油門下拉到最小，接收機(jī)收到信號(hào)LED常亮，
  11.               表示RF通訊已連接。此時(shí)蜂鳴器"嗶"一聲，航燈閃爍，表示待機(jī)模式。

  13. 四軸啟動(dòng)：將遙控器左右操縱桿掰成下內(nèi)八，啟動(dòng)四軸，四軸"嗶"一聲，4個(gè)螺旋槳開始低速旋轉(zhuǎn)，航燈常亮。
  14.           此后提升油門，就可以加速螺旋槳，直到起飛。

  16. 四軸飛行：起飛后，可以操縱右手的俯仰、橫滾操縱桿，實(shí)現(xiàn)前后左右或任意方向的飛行。
  17.           左手油門桿左掰是航向逆時(shí)針轉(zhuǎn)，右掰是航向順時(shí)鐘轉(zhuǎn)。

  19. 四軸下降停止：收油門，四軸逐漸下降到地面，然后兩操縱桿掰成下外八，停止四軸，重新處于待機(jī)模式。

  21. 四軸水平校準(zhǔn)：將四軸放置于水平地面，處于待機(jī)模式，然后兩操縱桿掰成上內(nèi)八，四軸"嗶"一聲進(jìn)入校準(zhǔn)，完成后"嗶嗶"兩聲完成校準(zhǔn)。

  23. 四軸取消水平校準(zhǔn)：將四軸放置于水平地面，處于待機(jī)模式，然后兩操縱桿掰成上外八，四軸"嗶"一聲取消校準(zhǔn)。取消水平校準(zhǔn)或未進(jìn)行水平校準(zhǔn)過的四軸，起飛時(shí)即使無風(fēng)也可能會(huì)有明顯漂移。

  25. 電池低壓報(bào)警：當(dāng)電池低壓時(shí)，蜂鳴器"嗶嗶"報(bào)警，同時(shí)航燈閃爍，此時(shí)請(qǐng)盡快回航降落。

  27. 無遙控信號(hào)異常：當(dāng)四軸在空中突然收不到遙控信號(hào)時(shí)，四軸蜂鳴器發(fā)出"嗶嗶嗶"報(bào)警，同時(shí)航燈閃爍，四軸保持水平，逐漸自動(dòng)減小油門降落。

  29. ***********************************************/

  31. #define                Baudrate1                        115200UL
  32. #define                TX1_LENGTH        128
  33. #define                RX1_LENGTH        128


  36. #include "config.h"
  37. #include "STC8xxx_PWM.H"
  38. #include "MPU6050.H"
  39. #include "AD.H"       
  40. #include "EEPROM.H"
  41. #include "PCA.h"
  42. #include <math.H>

  44. sbit        P_Light  = P5^4;        //航燈
  45. sbit        P_BUZZER = P5^5;        //蜂鳴器


  48. int                xdata g_x=0,g_y=0,g_z=0;                                        //陀螺儀矯正參數(shù)
  49. float        xdata a_x=0,a_y=0;                                                        //角度矯正參數(shù)
  50. float        data  AngleX=0,AngleY=0;                                        //四元數(shù)解算出的歐拉角
  51. float        xdata Angle_gx=0,Angle_gy=0,Angle_gz=0;                //由角速度計(jì)算的角速率(角度制)
  52. float        xdata Angle_ax=0,Angle_ay=0,Angle_az=0;                //由加速度計(jì)算的加速度(弧度制)
  53. float        xdata Ax=0,Ay=0,Az=0;                                                //加入遙控器控制量后的角度   
  54. float        data PID_x=0,PID_y=0,PID_z=0;                                //PID最終輸出量
  55. int                data  speed0=0,speed1=0,speed2=0,speed3=0;        //電機(jī)速度參數(shù)
  56. int                data  PWM0=0,PWM1=0,PWM2=0,PWM3=0;//,PWM4=0,PWM5=0;                        //加載至PWM模塊的參數(shù)

  58. int                int_tmp;
  59. u8                YM=0,FRX=128,FRY=128,FRZ=128;                                //4通道遙控信號(hào).
  60. u8                xdata        tp[16];                //讀MP6050緩沖


  63. //****************姿態(tài)處理和PID*********************************************

  65. float xdata Out_PID_X=0,Last_Angle_gx=0;                                        //外環(huán)PI輸出量  上一次陀螺儀數(shù)據(jù)
  66. float xdata ERRORX_Out=0,ERRORX_In=0;                        //外環(huán)P  外環(huán)I  外環(huán)誤差積分
  67. float xdata Out_PID_Y=0,Last_Angle_gy=0;
  68. float xdata ERRORY_Out=0,ERRORY_In=0;            //規(guī)則1:內(nèi)外環(huán)P乘積等于10.5

  70. float xdata Last_Ax=0,Last_Ay=0,Last_Az=0;


  73. /******************************************************************************/
  74. #define        Out_XP        6.65f        //ADC0        外環(huán)P        V1 / 10
  75. #define        Out_XI        0.0074f        //ADC4        外環(huán)I        V2 / 10000
  76. #define        Out_XD        6.0f        //ADC5        外環(huán)D        V3 / 10

  78. #define        In_XP        0.8275f        //ADC6        內(nèi)環(huán)P        V4 / 100
  79. #define        In_XI        0.0074f        //ADC4        內(nèi)環(huán)I        V2 / 10000
  80. #define        In_XD        6.0f        //ADC5        內(nèi)環(huán)D        V3 / 10


  83. #define        Out_YP        Out_XP
  84. #define        Out_YI        Out_XI
  85. #define        Out_YD        Out_XD

  87. #define        In_YP        In_XP
  88. #define        In_YI        In_XI
  89. #define        In_YD        In_XD


  92. #define        ZP        5.0f
  93. #define        ZI        0.1f
  94. #define        ZD        4.0f        //自旋控制的P D
  95. float Z_integral=0;//Z軸積分

  97. #define        ERR_MAX        500
  98. //======================================================================


  101. u8        data YM_LostCnt=0, Lost16S; //上一次RxBuf[0]數(shù)據(jù)(RxBuf[0]數(shù)據(jù)在不斷變動(dòng)的)   狀態(tài)標(biāo)識(shí)
  102. u8        SW2_tmp;


  105. //======================================================================
  106. bit        B_8ms;        //8ms標(biāo)志

  108. bit        B_rtn_ADC0;        //請(qǐng)求返回信息
  109. bit        B_BAT_LOW;        //低電壓標(biāo)志
  110. u8        xdata cnt_ms;                //時(shí)間計(jì)數(shù)


  113. u8                xdata UART1_cmd=0;        //串口命令
  114. u8                xdata TX1_Read=0;        //發(fā)送讀指針
  115. u8                xdata TX1_Write=0;        //發(fā)送寫指針
  116. u8                xdata TX1_cnt=0;        //發(fā)送計(jì)數(shù)
  117. u8                 xdata TX1_Buffer[TX1_LENGTH];        //發(fā)送緩沖
  118. bit                B_TX1_Busy;                        //發(fā)送忙標(biāo)志
  119. u8                 xdata RX1_Cnt,RX1_Timer;
  120. u8                 xdata RX1_Buffer[RX1_LENGTH];
  121. bit         B_RX1_OK;


  124. u8                xdata Cal_Setp=0;                        //校準(zhǔn)步驟
  125. u8                xdata Cal_cnt=0;                        //校準(zhǔn)平均值計(jì)數(shù)
  126. int                xdata x_sum,y_sum,z_sum;        //校準(zhǔn)累加和
  127. float        xdata float_x_sum,float_y_sum;        //校準(zhǔn)累加和

  129. u8        xdata BuzzerOnTime,BuzzerOffTime,BuzzerRepeat,BuzzerOnCnt,BuzzerOffCnt;
  130. u8        xdata cnt_100ms;


  133. /* =================== PPM接收相關(guān)變量 ========================== */
  134. u16        xdata CCAP0_RiseTime;                //捕捉到的上升沿時(shí)刻
  135. u8        xdata PPM1_Rise_TimeOut;        //高電平限時(shí)
  136. u8        xdata PPM1_Rx_TimerOut;                //接收超時(shí)計(jì)數(shù)
  137. u8        xdata PPM1_RxCnt;                        //接收次數(shù)計(jì)數(shù)
  138. u16        xdata PPM1_Cap;                                //捕捉到的PPM脈沖寬度
  139. bit        B_PPM1_OK;                                        //接收到一個(gè)PPM脈沖寬度

  141. u16        xdata CCAP1_RiseTime;
  142. u8        xdata PPM2_Rise_TimeOut;        //高電平限時(shí)
  143. u8        xdata PPM2_Rx_TimerOut;
  144. u8        xdata PPM2_RxCnt;
  145. u16        xdata PPM2_Cap;
  146. bit        B_PPM2_OK;

  148. u16        xdata CCAP2_RiseTime;
  149. u8        xdata PPM3_Rise_TimeOut;        //高電平限時(shí)
  150. u8        xdata PPM3_Rx_TimerOut;
  151. u8        xdata PPM3_RxCnt;
  152. u16        xdata PPM3_Cap;
  153. bit        B_PPM3_OK;

  155. u16        xdata CCAP3_RiseTime;
  156. u8        xdata PPM4_Rise_TimeOut;        //高電平限時(shí)
  157. u8        xdata PPM4_Rx_TimerOut;
  158. u8        xdata PPM4_RxCnt;
  159. u16        xdata PPM4_Cap;
  160. bit        B_PPM4_OK;

  162. u16        xdata CCAP_FallTime;

  164. u8        PPM1,PPM2,PPM3,PPM4;
  165. bit        B_Start;
  166. u8        cnt_start;

  168. /* ============================================= */


  171. void        UART1_config(void);
  172. void         PrintString1(u8 *puts);        //發(fā)送一個(gè)字符串
  173. void        TX1_write2buff(u8 dat);        //寫入發(fā)送緩沖，指針+1
  174. void        TX1_int_value(int i);
  175. void        delay_ms(u8 ms);
  176. void        Return_Message(void);
  177. u16         MODBUS_CRC16(u8 *p,u8 n);        //input:        *p--->First Data Address,n----->Data Number,        return:        CRC16
  178. void        PCA_config(void);
  179. void         Timer0_Config(void);
  180. void         Timer1_Config(void);
  181. void        return_TTMx(u8 id,PPMx);
  182. void         Timer0_Config(void);
  183. u16         MODBUS_CRC16(u8 *p,u8 n);        //input:        *p--->First Data Address,n----->Data Number,        return:        CRC16

  185. extern xdata u16        adc0;
  186. extern xdata int        Battery;


  189. //*********************************************************************
  190. //****************角度計(jì)算*********************************************
  191. //*********************************************************************
  192. #define        pi                3.14159265f                           
  193. #define        Kp                0.8f                        
  194. #define        Ki                0.001f                        
  195. #define        halfT        0.004f           

  197. float idata q0=1,q1=0,q2=0,q3=0;   
  198. float idata exInt=0,eyInt=0,ezInt=0;  


  201. void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
  202. {
  203.         float data norm;
  204.         float idata vx, vy, vz;
  205.         float idata ex, ey, ez;

  207.         norm = sqrt(ax*ax + ay*ay + az*az);        //把加速度計(jì)的三維向量轉(zhuǎn)成單維向量   
  208.         ax = ax / norm;
  209.         ay = ay / norm;
  210.         az = az / norm;

  212.                 //        下面是把四元數(shù)換算成《方向余弦矩陣》中的第三列的三個(gè)元素。
  213.                 //        根據(jù)余弦矩陣和歐拉角的定義，地理坐標(biāo)系的重力向量，轉(zhuǎn)到機(jī)體坐標(biāo)系，正好是這三個(gè)元素
  214.                 //        所以這里的vx vy vz，其實(shí)就是當(dāng)前的歐拉角（即四元數(shù)）的機(jī)體坐標(biāo)參照系上，換算出來的
  215.                 //        重力單位向量。
  216.         vx = 2*(q1*q3 - q0*q2);
  217.         vy = 2*(q0*q1 + q2*q3);
  218.         vz = q0*q0 - q1*q1 - q2*q2 + q3*q3 ;

  220.         ex = (ay*vz - az*vy) ;
  221.         ey = (az*vx - ax*vz) ;
  222.         ez = (ax*vy - ay*vx) ;

  224.         exInt = exInt + ex * Ki;
  225.         eyInt = eyInt + ey * Ki;
  226.         ezInt = ezInt + ez * Ki;

  228.         gx = gx + Kp*ex + exInt;
  229.         gy = gy + Kp*ey + eyInt;
  230.         gz = gz + Kp*ez + ezInt;

  232.         q0 = q0 + (-q1*gx - q2*gy - q3*gz) * halfT;
  233.         q1 = q1 + ( q0*gx + q2*gz - q3*gy) * halfT;
  234.         q2 = q2 + ( q0*gy - q1*gz + q3*gx) * halfT;
  235.         q3 = q3 + ( q0*gz + q1*gy - q2*gx) * halfT;

  237.         norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
  238.         q0 = q0 / norm;
  239.         q1 = q1 / norm;
  240.         q2 = q2 / norm;
  241.         q3 = q3 / norm;

  243.         AngleX = asin(2*(q0*q2 - q1*q3 )) * 57.2957795f; // 俯仰   換算成度
  244.         AngleY = asin(2*(q0*q1 + q2*q3 )) * 57.2957795f; // 橫滾
  245. }



  249. //****************姿態(tài)計(jì)算*********************************************
  250. void PWM_int (void) interrupt         22        //PWM中斷函數(shù)
  251. {
  252.         PWMCFG = 0;        //CBIF;        //清除中斷標(biāo)志

  254.         B_8ms = 1;

  256. //======================== 超時(shí)溢出處理 ==============================================
  257.         PPM1_Rise_TimeOut++;        //高電平限時(shí)
  258.         PPM2_Rise_TimeOut++;        //高電平限時(shí)
  259.         PPM3_Rise_TimeOut++;        //高電平限時(shí)
  260.         PPM4_Rise_TimeOut++;        //高電平限時(shí)

  262.         if(--PPM1_Rx_TimerOut == 0)                //超過100ms收不到信號(hào)
  263.         {
  264.                 PPM1_RxCnt = 0;                        //一旦出現(xiàn)溢出, 則開始的n個(gè)脈沖無效
  265.                 PPM1 = 128;;                        //默認(rèn)中點(diǎn)
  266.         }
  267.         if(--PPM2_Rx_TimerOut == 0)                //超過100ms收不到信號(hào)
  268.         {
  269.                 PPM2_RxCnt = 0;                        //一旦出現(xiàn)溢出, 則開始的n個(gè)脈沖無效
  270.                 PPM2 = 128;;                        //默認(rèn)中點(diǎn)
  271.         }
  272.         if(--PPM3_Rx_TimerOut == 0)                //超過200ms收不到信號(hào)
  273.         {
  274.                 PPM3_RxCnt = 0;                        //一旦出現(xiàn)溢出, 則開始的n個(gè)脈沖無效
  275.         }
  276.         if(--PPM4_Rx_TimerOut == 0)                //超過100ms收不到信號(hào)
  277.         {
  278.                 PPM4_RxCnt = 0;                        //一旦出現(xiàn)溢出, 則開始的n個(gè)脈沖無效
  279.                 PPM4 = 128;                                //默認(rèn)中點(diǎn)
  280.         }
  281. //======================================================================

  283.         if(++YM_LostCnt >= 250)                //失聯(lián)2秒后
  284.         {
  285.                 YM_LostCnt = 200;                //重復(fù)0.4秒，失控保護(hù)
  286.                 if(PPM3 > 80)        PPM3--;
  287.                 else if(++Lost16S >= 40)
  288.                 {
  289.                         Lost16S = 250;
  290.                         PPM3 = 0;
  291.                         B_Start = 0;
  292.                 }
  293.         }
  294.         if(YM_LostCnt  >= 25)        //失聯(lián)200ms
  295.         {
  296.                 PPM1 = 128;
  297.                 PPM2 = 128;                //俯仰 橫滾 航向均歸0
  298.                 PPM4 = 128;
  299.         }

  301.         FRX = PPM1;
  302.         FRY = PPM2;
  303.         YM  = PPM3;        //油門
  304.         FRZ = PPM4;
  305.        

  307. //********************************************************************************************
  308.         Read_MPU6050(tp);        //680us

  310.         Angle_ax = ((float)(((int *)&tp)[0])) / 8192.0;        //加速度處理        結(jié)果單位是 +- g
  311.         Angle_ay = ((float)(((int *)&tp)[1])) / 8192.0;        //轉(zhuǎn)換關(guān)系        8192 LSB/g, 1g對(duì)應(yīng)讀數(shù)8192
  312.         Angle_az = ((float)(((int *)&tp)[2])) / 8192.0;        //加速度量程 +-4g/S
  313.         Last_Angle_gx = Angle_gx;                //儲(chǔ)存上一次角速度數(shù)據(jù)
  314.         Last_Angle_gy = Angle_gy;
  315.         Angle_gx = ((float)(((int *)&tp)[4] - g_x)) / 65.5;        //陀螺儀處理        結(jié)果單位是 +-度
  316.         Angle_gy = ((float)(((int *)&tp)[5] - g_y)) / 65.5;        //陀螺儀量程 +-500度/S, 1度/秒 對(duì)應(yīng)讀數(shù) 65.536
  317.         Angle_gz = ((float)(((int *)&tp)[6] - g_z)) / 65.5;        //轉(zhuǎn)換關(guān)系65.5 LSB/度

  319.         IMUupdate(Angle_gx*0.0174533f, Angle_gy*0.0174533f, Angle_gz*0.0174533f, Angle_ax,Angle_ay,Angle_az);

  321. //**********************************X軸指向************************************************
  322.         Ax  = AngleX - a_x - ((float)FRX - 128) / 4.0;                //角度控制量加載至角度

  324.         if(YM > 35)        ERRORX_Out += Ax,        ERRORX_Out += Ax,        ERRORX_Out += Ax;        //外環(huán)積分(油門小于某個(gè)值時(shí)不積分)
  325.         else                ERRORX_Out = 0; //油門小于定值時(shí)清除積分值
  326.                  if(ERRORX_Out >  1500)        ERRORX_Out =  1500;
  327.         else if(ERRORX_Out < -1500)        ERRORX_Out = -1500;        //積分限幅

  329.         Out_PID_X = Ax*Out_XP + ERRORX_Out*Out_XI + (Ax-Last_Ax)*Out_XD;        //外環(huán)PI
  330.         Last_Ax = Ax;
  331.        
  332.         if(YM > 35)        ERRORX_In += (Angle_gy - Out_PID_X);        //內(nèi)環(huán)積分(油門小于某個(gè)值時(shí)不積分)
  333.         else                ERRORX_In = 0;        //油門小于定值時(shí)清除積分值
  334.                  if(ERRORX_In >  500)        ERRORX_In =  500;
  335.         else if(ERRORX_In < -500)        ERRORX_In = -500;        //積分限幅

  337.         PID_x = (Angle_gy + Out_PID_X) * In_XP + ERRORX_In * In_XI + (Angle_gy - Last_Angle_gy) * In_XD;        //內(nèi)環(huán)PID
  338.         if(PID_x >  500)        PID_x =  500;        //輸出量限幅
  339.         if(PID_x < -500)        PID_x = -500;

  341. //**************Y軸指向**************************************************
  342.         Ay  = AngleY - a_y + ((float)FRY - 128) / 4.0;                //角度控制量加載至角度
  343.        
  344.         if(YM > 35)        ERRORY_Out += Ay,        ERRORY_Out += Ay,        ERRORY_Out += Ay;        //外環(huán)積分(油門小于某個(gè)值時(shí)不積分)
  345.         else                ERRORY_Out = 0; //油門小于定值時(shí)清除積分值
  346.                  if(ERRORY_Out >  1500)        ERRORY_Out =  1500;
  347.         else if(ERRORY_Out < -1500)        ERRORY_Out = -1500;        //積分限幅
  348.        
  349.         Out_PID_Y = Ay * Out_YP + ERRORY_Out * Out_YI + (Ay-Last_Ay)*Out_YD;        //外環(huán)PID
  350.         Last_Ay = Ay;

  352.         if(YM > 35)        ERRORY_In += (Angle_gx - Out_PID_Y);  //內(nèi)環(huán)積分(油門小于某個(gè)值時(shí)不積分)
  353.         else                ERRORY_In = 0; //油門小于定值時(shí)清除積分值
  354.                  if(ERRORY_In >  500)        ERRORY_In =  500;
  355.         else if(ERRORY_In < -500)        ERRORY_In = -500;        //積分限幅
  356.        
  357.         PID_y = (Angle_gx + Out_PID_Y) * In_YP + ERRORY_In * In_YI + (Angle_gx - Last_Angle_gx) * In_YD;        //內(nèi)環(huán)PID
  358.        
  359.         if(PID_y > 500)        PID_y =  500;        //輸出量限幅
  360.         if(PID_y <-500)        PID_y = -500;

  362. //**************Z軸指向(Z軸隨便啦，自旋控制沒必要上串級(jí)PID)*****************************       
  363.         Az = Angle_gz - ((float)FRZ - 128);
  364.        
  365.         if(YM > 35)        Z_integral += Az;        //Z軸積分
  366.         else                Z_integral = 0;                //油門小于40積分清零
  367.                  if(Z_integral >  500.0f)        Z_integral =  500.0f;        //積分限幅
  368.         else if(Z_integral < -500.0f)        Z_integral = -500.0f;        //積分限幅

  370.         PID_z = Az * ZP + Z_integral * ZI + (Az - Last_Az) * ZD;
  371.         Last_Az = Az;
  372.         if(PID_z >  200)        PID_z =  200;        //輸出量限幅
  373.         if(PID_z < -200)        PID_z = -200;

  375.         speed0 = (int)(  PID_x + PID_y + PID_z);        //M1改為逆時(shí)針
  376.         speed1 = (int)(  PID_x - PID_y - PID_z);
  377.         speed2 = (int)( -PID_x - PID_y + PID_z);
  378.         speed3 = (int)( -PID_x + PID_y - PID_z);

  380. //**************將速度參數(shù)加載至PWM模塊*************************************************       
  381.        
  382.         if(YM < 10)        PWM0 = 1000, PWM1 = 1000, PWM2 = 1000, PWM3 = 1000;
  383.         else if(YM < 35)        PWM0 = 860, PWM1 = 860, PWM2 = 860, PWM3 = 860;
  384.         else
  385.         {
  386.                 int_tmp = 1000 - (int)YM * 4;

  388.                 PWM0 = int_tmp - speed0;

  390.                          if(PWM0 > 1000)        PWM0 = 1000;    //速度參數(shù)控制，防止超過PWM參數(shù)范圍0-1000
  391.                 else if(PWM0 < 10)                PWM0 = 10;

  393.                 PWM1 = int_tmp - speed1;

  395.                          if(PWM1 > 1000)        PWM1 = 1000;
  396.                 else if(PWM1 < 10)                PWM1 = 10;

  398.                 PWM2 = int_tmp - speed2;

  400.                          if(PWM2 > 1000)        PWM2 = 1000;
  401.                 else if(PWM2 < 10)                PWM2 = 10;

  403.                 PWM3 = int_tmp - speed3;

  405.                          if(PWM3 > 1000)        PWM3 = 1000;
  406.                 else if(PWM3 < 10)                PWM3 = 10;
  407.         }

  409.         SW2_tmp = P_SW2;        //保存SW2設(shè)置
  410.         EAXSFR();        //訪問XFR
  411.         PWM0T2 = (u16)(PWM0 * 2);
  412.         PWM1T2 = (u16)(PWM1 * 2);
  413.         PWM2T2 = (u16)(PWM2 * 2);
  414.         PWM3T2 = (u16)(PWM3 * 2);       
  415.         P_SW2  = SW2_tmp;        //恢復(fù)SW2設(shè)置

  417. }


  420. /********************** 蜂鳴函數(shù) ************************/
  421. void        beep(void)        //100ms調(diào)用
  422. {
  423.         if(BuzzerRepeat > 0)        //蜂鳴器處理, 重復(fù)次數(shù)不為0，則蜂鳴器要發(fā)聲
  424.         {
  425.                 if((BuzzerOnCnt == 0) && (BuzzerOffCnt == 0))        //On和OFF都為0，則開始裝載On和Off的時(shí)間
  426.                 {
  427.                         P_BUZZER = 1;                        //允許蜂鳴
  428.                         BuzzerOnCnt  = BuzzerOnTime;        //裝載on計(jì)數(shù)
  429.                         BuzzerOffCnt = BuzzerOffTime;        //裝載off計(jì)數(shù)
  430.                 }
  431.                 else if(BuzzerOnCnt  > 0)        {if(--BuzzerOnCnt == 0)        P_BUZZER = 0;}        //On的時(shí)間
  432.                 else if(BuzzerOffCnt > 0)        //Off的時(shí)間
  433.                 {
  434.                         if(--BuzzerOffCnt == 0)        BuzzerRepeat--;
  435.                 }
  436.         }
  437.         else        P_BUZZER = 0;
  438. }

  440. void        SetBuzzer(u8 on,u8 off,u8 rep)        // rep: 重復(fù)次數(shù), on: on的時(shí)間, off: off的時(shí)間
  441. {
  442.         BuzzerRepeat = rep;
  443.         BuzzerOnTime  = on;
  444.         BuzzerOffTime = off;
  445.         if(BuzzerOnTime  == 0)        BuzzerOnTime  = 1;
  446.         if(BuzzerOffTime == 0)        BuzzerOffTime = 1;
  447.         if(BuzzerRepeat == 1)        BuzzerOffTime = 1;
  448.         BuzzerOnCnt = 0,        BuzzerOffCnt = 0;
  449. }

  451. // ===================== 自動(dòng)校準(zhǔn)序列 =====================
  452. void        AutoCal(void)
  453. {
  454.         if(PPM3 < 40)        //停止時(shí)才允許校準(zhǔn)
  455.         {
  456.                 if(Cal_Setp == 1)        //進(jìn)入校準(zhǔn)序列
  457.                 {
  458.                         x_sum = 0;        y_sum = 0;        z_sum = 0;
  459.                         Cal_cnt  = 0;
  460.                         Cal_Setp = 2;
  461.                 }
  462.                 else if(Cal_Setp == 2)        //對(duì)陀螺儀累加
  463.                 {
  464.                         x_sum += ((int *)&tp)[4];  //讀取陀螺儀數(shù)據(jù)
  465.                         y_sum += ((int *)&tp)[5];
  466.                         z_sum += ((int *)&tp)[6];
  467.                         if(++Cal_cnt >= 64)
  468.                         {
  469.                                 g_x = x_sum / 64;
  470.                                 g_y = y_sum / 64;
  471.                                 g_z = z_sum / 64;
  472.                                 float_x_sum = 0;        float_y_sum = 0;
  473.                                 Cal_cnt  = 0;
  474.                                 Cal_Setp = 3;
  475.                         }
  476.                 }
  477.                 else if(Cal_Setp == 3)        //對(duì)X Y角度累加
  478.                 {
  479.                         float_x_sum += AngleX;
  480.                         float_y_sum += AngleY;
  481.                         if(++Cal_cnt >= 64)
  482.                         {
  483.                                 Cal_cnt  = 0;
  484.                                 Cal_Setp = 0;
  485.                                 a_x = float_x_sum / 64.0;
  486.                                 a_y = float_y_sum / 64.0;
  487.                                 IAP_Gyro();
  488.                                 SetBuzzer(5,1,1);
  489.                         }
  490.                 }
  491.         }
  492.         else
  493.         {
  494.                 Cal_Setp = 0;
  495.                 Cal_cnt  = 0;
  496.         }
  497. }

  499. // ===================== 主函數(shù) =====================
  500. void main(void)
  501. {

  503.         //所有I/O口全設(shè)為準(zhǔn)雙向，弱上拉模式
  504.         P0M0=0x00;        P0M1=0x00;
  505.         P1M0=0x00;        P1M1=0x00;
  506.         P2M0=0x00;        P2M1=0x00;
  507.         P3M0=0x00;        P3M1=0x00;
  508.         P4M0=0x00;        P4M1=0x00;
  509.         P5M0=0x00;        P5M1=0x00;
  510.         P6M0=0x00;        P6M1=0x00;
  511.         P7M0=0x00;        P7M1=0x00;

  513.         PPM1 = 128;
  514.         PPM2 = 128;
  515.         PPM3 = 0;
  516.         PPM4 = 128;

  518.         PWMGO();

  520.         P_Light  = 0;
  521.         P_BUZZER = 0;
  522.         P5n_push_pull(0x30);

  524.         adc_init();    //啟動(dòng)A/D
  525.        
  526.         PCA_config();

  528.         delay_ms(100);
  529.         IAPRead();                //讀取陀螺儀靜差
  530.         InitMPU6050();        //初始化MPU-6050
  531.         delay_ms(100);

  533.         PWMCR =  0xc0;//ECBI;        //允許PWM計(jì)數(shù)器歸零中斷
  534.         EA = 1;        //允許總中斷
  535.        
  536.         cnt_start = 0;
  537.         while(cnt_start < 25)        //等待油門最小        20ms * 25 = 500ms
  538.         {
  539.                 if(B_PPM3_OK)        //油門
  540.                 {
  541.                         B_PPM3_OK = 0;
  542.                         if(PPM3_Cap <= 1200)        cnt_start++;
  543.                 }
  544.                 delay_ms(1);
  545.         }
  546.         P_Light  = 0;
  547.        
  548.         cnt_start = 0;

  550.         SetBuzzer(5,1,1);
  551.        

  553. //==============================================
  554.         UART1_config();        // 選擇波特率, 2: 使用Timer2做波特率, 其它值: 使用Timer1做波特率.
  555.         PrintString1("STC15W4K系列大四軸飛控程序!\r\n");        //SUART1發(fā)送一個(gè)字符串
  556. //==============================================

  558.         B_Start = 0;        //上電禁止運(yùn)行

  560.         while(1)
  561.         {
  562.                 if(B_PPM1_OK)        //左右(橫滾)
  563.                 {
  564.                         B_PPM1_OK = 0;
  565.                                  if(PPM1_Cap < 1120)        PPM1_Cap = 1120;
  566.                         else if(PPM1_Cap > 1880)        PPM1_Cap = 1880;
  567.                         PPM2 = (u8)((PPM1_Cap-1116)/3);        //轉(zhuǎn)為0~255, 中間值為128
  568.                 }
  569.                
  570.                 if(B_PPM2_OK)        //前后(俯仰)
  571.                 {
  572.                         B_PPM2_OK = 0;
  573.                                  if(PPM2_Cap < 1120)        PPM2_Cap = 1120;
  574.                         else if(PPM2_Cap > 1880)        PPM2_Cap = 1880;
  575.                         PPM1 = (u8)((PPM2_Cap-1116)/3);        //轉(zhuǎn)為0~255, 中間值為128
  576.                 }
  577.                
  578.                 if(B_PPM4_OK)        //航向
  579.                 {
  580.                         B_PPM4_OK = 0;
  581.                                  if(PPM4_Cap < 1056)        PPM4_Cap = 1056;
  582.                                  if(PPM4_Cap > 1940)        PPM4_Cap = 1940;
  583.                                  if(PPM4_Cap < 1440)        PPM4_Cap = PPM4_Cap + 60;
  584.                         else if(PPM4_Cap > 1560)        PPM4_Cap = PPM4_Cap - 60;
  585.                         else        PPM4_Cap = 1500;
  586.                         PPM4 = (u8)((PPM4_Cap-1116)/3);        //轉(zhuǎn)為0~255, 中間值為128
  587.                 }
  588.                
  589.                 if(B_PPM3_OK)        //油門
  590.                 {
  591.                         B_PPM3_OK = 0;
  592.                         if(PPM3_Cap < 1000)        PPM3_Cap = 1000;
  593.                         if(PPM3_Cap > 1900)        PPM3_Cap = 1900;

  595.                         if(B_Start)                //正在運(yùn)行時(shí),
  596.                         {
  597.                                 PPM3 = (u8)((PPM3_Cap-1000)/4);        //轉(zhuǎn)為0~255,        實(shí)際8~225
  598.                                 if(PPM3 < 32)        PPM3 = 32;
  599.                                
  600.                                 if((PPM1 < 50) && (PPM2 < 50) && (PPM3_Cap < 1120) && (PPM4 > 200))        //下外八, 禁止
  601.                                 {
  602.                                         if(++cnt_start >= 50)        //1秒
  603.                                         {
  604.                                                 cnt_start = 0;
  605.                                                 B_Start = 0;
  606.                                                 SetBuzzer(1,1,2);
  607.                                         }
  608.                                 }
  609.                                 else        cnt_start = 0;
  610.                         }
  611.                         else        //禁止運(yùn)行時(shí), 等待內(nèi)八開啟
  612.                         {
  613.                                 PPM3 = 0;
  614.                                 if((PPM1 < 50) && (PPM2 > 200) && (PPM3_Cap < 1120) && (PPM4 < 50))        //下內(nèi)八, 啟動(dòng)
  615.                                 {
  616.                                         if(++cnt_start >= 50)        //1秒
  617.                                         {
  618.                                                 cnt_start = 0;
  619.                                                 B_Start = 1;
  620.                                                 SetBuzzer(5,1,1);
  621.                                         }
  622.                                 }
  623.                                 else if((PPM1 > 200) && (PPM2 > 200) && (PPM3_Cap > 1850) && (PPM4 < 50))        //上內(nèi)八, 水平校準(zhǔn)
  624.                                 {
  625.                                         if(++cnt_start >= 50)        //1秒
  626.                                         {
  627.                                                 cnt_start = 0;
  628.                                                 SetBuzzer(2,1,1);
  629.                                                 Cal_Setp = 1;
  630.                                         }
  631.                                 }
  632.                                 else if((PPM1 > 200) && (PPM2 < 50) && (PPM3_Cap > 1850) && (PPM4 > 200))        //上外八, 取消水平校準(zhǔn)
  633.                                 {
  634.                                         if(++cnt_start >= 50)        //1秒
  635.                                         {
  636.                                                 cnt_start = 0;
  637.                                                 g_x = 0;
  638.                                                 g_y = 0;
  639.                                                 g_z = 0;
  640.                                                 a_x = 0;
  641.                                                 a_y = 0;
  642.                                                 IAP_Gyro();
  643.                                                 SetBuzzer(1,1,2);
  644.                                         }
  645.                                 }
  646.                                 else        cnt_start = 0;
  647.                         }
  648.                 }


  651.                 if(B_8ms)                //8ms到
  652.                 {
  653.                         B_8ms = 0;
  654.                        
  655.                         if(Cal_Setp != 0)        AutoCal();        //是否執(zhí)行自動(dòng)校準(zhǔn)序列
  656.                         AD();                // 讀ADC計(jì)算電壓

  658.                         if(++cnt_100ms >= 12)        cnt_100ms = 0,        beep();        //100ms處理一次蜂鳴器

  660.                         B = cnt_ms;
  661.                         ++cnt_ms;
  662.                         B = (B ^ cnt_ms) & cnt_ms;

  664.                         if(B2)                //64ms
  665.                         {
  666.                                 if(!B_BAT_LOW && (YM_LostCnt < 120))        //電壓足, 信號(hào)正常
  667.                                 {
  668.                                         if(!B_Start)        P_Light = 0;        // 空閑時(shí), 則慢閃(每2048ms亮64ms)
  669.                                         else                         P_Light = 1;        // 啟動(dòng)后, 燈常亮
  670.                                 }
  671.                         }
  672.                         else if(B4)                //256ms
  673.                         {
  674.                                 if(B_BAT_LOW || (YM_LostCnt >= 120))        P_Light = ~P_Light;                //電壓低, 或無信號(hào), 航燈閃爍 2HZ
  675.                         }
  676.                         else if(B6)                //1024ms
  677.                         {
  678.                                 if(Battery < 1090)        B_BAT_LOW = 1;        else if(Battery > 1110)        B_BAT_LOW = 0;        //<10.90V電壓低, >11.10V電壓夠
  679.                                
  680.                                 if(B_BAT_LOW)        SetBuzzer(1,1,2);        //電壓低
  681.                                
  682.                                 if(B_rtn_ADC0)        Return_Message();        //請(qǐng)求返回ADC0數(shù)據(jù)

  684.                                 if(!B_BAT_LOW && (YM_LostCnt < 120))        P_Light = 1;        //遙控信號(hào)正常,        電壓正常時(shí)
  685.                         }
  686.                         else if(B7)                //2048ms
  687.                         {
  688.                                 if(!B_BAT_LOW && (YM_LostCnt >= 120))        SetBuzzer(1,1,3);        //電壓正常時(shí) 遙控信號(hào)丟失, 每?jī)擅攵跳Q3次,
  689.                         }
  690.                 }


  693.                 if(UART1_cmd != 0)
  694.                 {
  695.                         if(UART1_cmd == 'a')                //PC發(fā)送a，飛控返回一些參數(shù)
  696.                         {
  697.                                 B_rtn_ADC0 = ~B_rtn_ADC0;
  698.                         }
  699.                         UART1_cmd = 0;
  700.                 }
  701.                
  702.                
  703.                 if((TX1_Read != TX1_Write) && (!B_TX1_Busy))        //有數(shù)據(jù)要發(fā)送, 并且發(fā)送空閑
  704.                 {
  705.                         SBUF = TX1_Buffer[TX1_Read];
  706.                         B_TX1_Busy = 1;
  707.                         if(++TX1_Read >= TX1_LENGTH)        TX1_Read = 0;
  708.                 }

  710.         }
  711. }

  713. //=========================================================

  715. void        Return_Message(void)
  716. {
  717.         TX1_write2buff('V');
  718.         TX1_write2buff('=');
  719.         TX1_write2buff(Battery/1000 + '0');
  720.         TX1_write2buff((Battery%1000)/100 + '0');
  721.         TX1_write2buff('.');
  722.         TX1_write2buff((Battery%100)/10 + '0');
  723.         TX1_write2buff(Battery%10 + '0');
  724.         TX1_write2buff(' ');
  725.         TX1_write2buff(' ');

  727.         PrintString1("AngleX=");
  728.         TX1_int_value((int)(AngleX * 10));

  730.         PrintString1("AngleY=");
  731.         TX1_int_value((int)(AngleY * 10));

  733.         PrintString1("AngleZ=");
  734.         TX1_int_value((int)(Angle_gz * 10));

  736.         PrintString1("a_x=");
  737.         TX1_int_value(a_x * 10);
  738.         PrintString1("a_y=");
  739.         TX1_int_value(a_y * 10);
  740.         PrintString1("g_z=");
  741.         TX1_int_value(g_z * 10);

  743.         TX1_cnt = 0;
  744.         TX1_write2buff(0x0d);
  745.         TX1_write2buff(0x0a);
  746. }


  749. void  delay_ms(u8 ms)
  750. {
  751.      u16 i;
  752.          do
  753.          {
  754.                  i = MAIN_Fosc / 13000;
  755.                 while(--i)        ;   //13T per loop
  756.      }while(--ms);
  757. }


  760. void        TX1_int_value(int i)
  761. {
  762.         if(i < 0)        TX1_write2buff('-'),        i = 0 - i;
  763.         else                TX1_write2buff(' ');
  764.         TX1_write2buff(i / 1000 + '0');
  765.         TX1_write2buff((i % 1000) / 100 + '0');
  766.         TX1_write2buff((i % 100) / 10 + '0');
  767.         TX1_write2buff('.');
  768.         TX1_write2buff(i % 10 + '0');
  769.         TX1_write2buff(' ');
  770.         TX1_write2buff(' ');
  771. }

  773. /*************** 裝載串口1發(fā)送緩沖 *******************************/
  774. void TX1_write2buff(u8 dat)        //寫入發(fā)送緩沖，指針+1
  775. {
  776.         TX1_Buffer[TX1_Write] = dat;
  777.         if(++TX1_Write >= TX1_LENGTH)        TX1_Write = 0;
  778. }


  781. //========================================================================
  782. // 函數(shù): void PrintString1(u8 *puts)
  783. // 描述: 串口1發(fā)送字符串函數(shù)。
  784. // 參數(shù): puts:  字符串指針.
  785. // 返回: none.
  786. // 版本: VER1.0
  787. // 日期: 2014-11-28
  788. // 備注:
  789. //========================================================================
  790. void PrintString1(u8 *puts)        //發(fā)送一個(gè)字符串
  791. {
  792.         for (; *puts != 0;        puts++)   TX1_write2buff(*puts);        //遇到停止符0結(jié)束
  793. }

  795. //========================================================================
  796. // 函數(shù): SetTimer2Baudrate(u16 dat)
  797. // 描述: 設(shè)置Timer2做波特率發(fā)生器。
  798. // 參數(shù): dat: Timer2的重裝值.
  799. // 返回: none.
  800. // 版本: VER1.0
  801. // 日期: 2014-11-28
  802. // 備注:
  803. //========================================================================

  805. void        SetTimer2Baudrate(u16 dat)        // 選擇波特率, 2: 使用Timer2做波特率, 其它值: 使用Timer1做波特率.
  806. {
  807.         AUXR &= ~(1<<4);        //Timer stop
  808.         AUXR &= ~(1<<3);        //Timer2 set As Timer
  809.         AUXR |=  (1<<2);        //Timer2 set as 1T mode
  810.         TH2 = dat / 256;
  811.         TL2 = dat % 256;
  812.         IE2  &= ~(1<<2);        //禁止中斷
  813.         AUXR |=  (1<<4);        //Timer run enable
  814. }


  817. //========================================================================
  818. // 函數(shù): void        UART1_config(u8 brt)
  819. // 描述: UART1初始化函數(shù)。
  820. // 參數(shù): brt: 選擇波特率, 2: 使用Timer2做波特率, 其它值: 使用Timer1做波特率.
  821. // 返回: none.
  822. // 版本: VER1.0
  823. // 日期: 2014-11-28
  824. // 備注:
  825. //========================================================================
  826. void        UART1_config(void)
  827. {
  828.         /*********** 波特率使用定時(shí)器2 *****************/
  829.         AUXR |= 0x01;                //S1 BRT Use Timer2;
  830.         SetTimer2Baudrate(65536UL - (MAIN_Fosc / 4) / Baudrate1);

  832.         /*********** 波特率使用定時(shí)器1 *****************/
  833. /*        TR1 = 0;
  834.         AUXR &= ~0x01;                //S1 BRT Use Timer1;
  835.         AUXR |=  (1<<6);        //Timer1 set as 1T mode
  836.         TMOD &= ~(1<<6);        //Timer1 set As Timer
  837.         TMOD &= ~0x30;                //Timer1_16bitAutoReload;
  838.         TH1 = (u8)((65536UL - (MAIN_Fosc / 4) / Baudrate1) / 256);
  839.         TL1 = (u8)((65536UL - (MAIN_Fosc / 4) / Baudrate1) % 256);
  840.         ET1 = 0;        //禁止中斷
  841.         INT_CLKO &= ~0x02;        //不輸出時(shí)鐘
  842.         TR1  = 1;
  843. */        //========================================================================

  845.         SCON = (SCON & 0x3f) | 0x40;        //UART1模式, 0x00: 同步移位輸出, 0x40: 8位數(shù)據(jù),可變波特率, 0x80: 9位數(shù)據(jù),固定波特率, 0xc0: 9位數(shù)據(jù),可變波特率
  846.         PS  = 1;        //高優(yōu)先級(jí)中斷
  847.         ES  = 1;        //允許中斷
  848.         REN = 1;        //允許接收
  849.         P_SW1 &= 0x3f;
  850.         P_SW1 |= 0x00;                //UART1 switch to, 0x00: P3.0 P3.1, 0x40: P3.6 P3.7, 0x80: P1.6 P1.7 (必須使用內(nèi)部時(shí)鐘)
  851. //        PCON2 |=  (1<<4);        //內(nèi)部短路RXD與TXD, 做中繼, ENABLE,DISABLE

  853.         B_TX1_Busy = 0;
  854.         TX1_Read   = 0;
  855.         TX1_Write  = 0;
  856.         UART1_cmd  = 0;
  857.         TX1_cnt    = 0;
  858. }


  861. //========================================================================
  862. // 函數(shù): void UART1_int (void) interrupt UART1_VECTOR
  863. // 描述: UART1中斷函數(shù)。
  864. // 參數(shù): nine.
  865. // 返回: none.
  866. // 版本: VER1.0
  867. // 日期: 2014-11-28
  868. // 備注:
  869. //========================================================================
  870. void UART1_int (void) interrupt 4
  871. {
  872.         if(RI)
  873.         {
  874.                 RI = 0;
  875.                 UART1_cmd = SBUF;
  876.         }

  878.         if(TI)
  879.         {
  880.                 TI = 0;
  881.                 B_TX1_Busy = 0;
  882.         }
  883. }



  887. void        PCA_config(void)
  888. {
  889.         PPM1_Rise_TimeOut = 0;
  890.         PPM2_Rise_TimeOut = 0;
  891.         PPM3_Rise_TimeOut = 0;
  892.         PPM4_Rise_TimeOut = 0;

  894.         CR = 0;
  895.         CH = 0;
  896.         CL = 0;
  897.         AUXR1 = (AUXR1 & ~(3<<4)) | PCA_P12_P17_P16_P15_P14;        //切換IO口
  898.         CMOD  = (CMOD  & ~(7<<1)) | PCA_Clock_12T;                                //選擇時(shí)鐘源  STC8F8K D版本
  899. //        CMOD  = (CMOD  & ~1) | 1;                                                                //ECF
  900.         PPCA = 1;        //高優(yōu)先級(jí)中斷

  902.         CCAPM0     = PCA_Mode_Capture | PCA_Rise_Active | PCA_Fall_Active | ENABLE;        //工作模式, 中斷模式
  903.         PCA_PWM0   = PCA_PWM_8bit;        //PWM寬度
  904. //        CCAP0L = (u8)CCAP0_tmp;                        //將影射寄存器寫入捕獲寄存器，先寫CCAPnL
  905. //        CCAP0H = (u8)(CCAP0_tmp >> 8);        //后寫CCAPnH

  907.         CCAPM1     = PCA_Mode_Capture | PCA_Rise_Active | PCA_Fall_Active | ENABLE;        //工作模式, 中斷模式
  908.         PCA_PWM1   = PCA_PWM_8bit;        //PWM寬度
  909. //        CCAP1L = (u8)CCAP1_tmp;                        //將影射寄存器寫入捕獲寄存器，先寫CCAPnL
  910. //        CCAP1H = (u8)(CCAP1_tmp >> 8);        //后寫CCAPnH

  912.         CCAPM2     = PCA_Mode_Capture | PCA_Rise_Active | PCA_Fall_Active | ENABLE;        //工作模式, 中斷模式
  913.         PCA_PWM2   = PCA_PWM_8bit;        //PWM寬度
  914. //        CCAP2L = (u8)CCAP2_tmp;                        //將影射寄存器寫入捕獲寄存器，先寫CCAPnL
  915. //        CCAP2H = (u8)(CCAP2_tmp >> 8);        //后寫CCAPnH

  917.         CCAPM3     = PCA_Mode_Capture | PCA_Rise_Active | PCA_Fall_Active | ENABLE;        //工作模式, 中斷模式
  918.         PCA_PWM3   = PCA_PWM_8bit;        //PWM寬度
  919. //        CCAP3L = (u8)CCAP3_tmp;                        //將影射寄存器寫入捕獲寄存器，先寫CCAPnL
  920. //        CCAP3H = (u8)(CCAP3_tmp >> 8);        //后寫CCAPnH

  922.         CR = 1;
  923. }


  926. //========================================================================
  927. // 函數(shù): void        PCA_Handler (void) interrupt PCA_VECTOR
  928. // 描述: PCA中斷處理程序.
  929. // 參數(shù): None
  930. // 返回: none.
  931. // 版本: V1.0, 2012-11-22
  932. //========================================================================
  933. void        PCA_Handler (void) interrupt PCA_VECTOR
  934. {
  935.         if(CCF0)                //PCA模塊0中斷
  936.         {
  937.                 CCF0 = 0;                //清PCA模塊0中斷標(biāo)志
  938.                 if(P17)        //上升沿
  939.                 {
  940.                         CCAP0_RiseTime = ((u16)CCAP0H << 8) + CCAP0L;        //讀CCAP0
  941.                         PPM1_Rise_TimeOut = 1;        //收到上升沿, 高電平限時(shí)
  942.                 }
  943.                 else        //下降沿
  944.                 {
  945.                         CCAP_FallTime = ((u16)CCAP0H << 8) + CCAP0L;        //讀CCAP0
  946.                         if((PPM1_Rise_TimeOut != 0) && (PPM1_Rise_TimeOut < 3))        //收到過上升沿, 高電平也沒有溢出
  947.                         {
  948.                                 CCAP_FallTime = (CCAP_FallTime - CCAP0_RiseTime) >> 1;        //為了好處理, 轉(zhuǎn)成單位為us
  949.                                 if((CCAP_FallTime >= 800) && (CCAP_FallTime <= 2500))
  950.                                 {
  951.                                         if(++PPM1_RxCnt >= 5)        PPM1_RxCnt = 5;                //連續(xù)接收到5個(gè)脈沖
  952.                                         if(PPM1_RxCnt == 5)
  953.                                         {
  954.                                                 if(!B_PPM1_OK)
  955.                                                 {
  956.                                                         PPM1_Cap = CCAP_FallTime;
  957.                                                         B_PPM1_OK = 1;                //標(biāo)志收到一個(gè)脈沖
  958.                                                         PPM1_Rx_TimerOut = 12;        //限時(shí)收不到脈沖
  959.                                                 }
  960.                                         }
  961.                                 }
  962. ……………………

  964. …………限于本文篇幅 余下代碼請(qǐng)從51黑下載附件…………
復(fù)制代碼




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



分享到:  QQ好友和群QQ好友和群 QQ空間QQ空間 騰訊微博騰訊微博 騰訊朋友騰訊朋友
收藏收藏32 分享淘帖 頂頂8 踩踩

相關(guān)帖子
回復(fù)

使用道具 舉報(bào)

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

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


評(píng)分

參與人數(shù) 1黑幣 +20 收起 理由
admin + 20 回帖助人的獎(jiǎng)勵(lì)！

查看全部評(píng)分

回復(fù)

使用道具 舉報(bào)

32#
ID:1103557 發(fā)表于 2024-3-11 14:13 | 只看該作者

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

使用道具 舉報(bào)

31#
ID:704640 發(fā)表于 2022-5-25 08:23 | 只看該作者
哈哈，剛在官網(wǎng)上看到，然后在家里又看到，更想做一臺(tái)來玩了
回復(fù)

使用道具 舉報(bào)

30#
ID:925561 發(fā)表于 2022-1-1 10:49 | 只看該作者
putiz 發(fā)表于 2021-12-26 18:05
大佬，可以加下好友交流一下嗎
2410025784

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

使用道具 舉報(bào)

29#
ID:975881 發(fā)表于 2021-12-26 18:05 | 只看該作者
CHNO 發(fā)表于 2021-6-5 17:11
從宏晶官網(wǎng)下載，然后打過板子，四軸可以平穩(wěn)飛行，就是油門開大后不能穩(wěn)定的停在原地，會(huì)偏，正在查找原因 ...

大佬，可以加下好友交流一下嗎
2410025784
回復(fù)

使用道具 舉報(bào)

28#
ID:975881 發(fā)表于 2021-12-26 17:41 | 只看該作者
CHNO 發(fā)表于 2021-6-11 11:29
分享一下調(diào)試經(jīng)驗(yàn)
1.起飛后會(huì)飛斜：在水平的地板上將四軸放平，開啟遙控，油門打到最低，串口以文本輸入a ...

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

使用道具 舉報(bào)

27#
ID:975881 發(fā)表于 2021-12-26 17:38 | 只看該作者
大家好，我看了幾天的代碼了，也嘗試打印了一些數(shù)據(jù)來分析。
現(xiàn)狀是剛開始啟動(dòng)后，電機(jī)不會(huì)啟動(dòng)，不會(huì)低速旋轉(zhuǎn)。即使偶爾有幾個(gè)會(huì)旋轉(zhuǎn)，也不會(huì)受遙控器的控制。
經(jīng)過打印一些信息發(fā)現(xiàn)：
  1.         SW2_tmp = P_SW2;        //保存SW2設(shè)置
  2.         EAXSFR();        //訪問XFR
  3.         PWM0T2 = (u16)(PWM0 * 2);
  4.         PWM1T2 = (u16)(PWM1 * 2);
  5.         PWM2T2 = (u16)(PWM2 * 2);       
  6.         PWM3T2 = (u16)(PWM3 * 2);        //進(jìn)行輸出脈沖信號(hào)更新
  7.         P_SW2  = SW2_tmp;        //恢復(fù)SW2設(shè)置
復(fù)制代碼

這樣的代碼，這些值不會(huì)發(fā)生變化和更新
  1.         PWM0T1 = 4000;//第一個(gè)翻轉(zhuǎn)點(diǎn) 4000
  2.         PWM1T1 = 4000;
  3.         PWM2T1 = 4000;
  4.         PWM3T1 = 4000;
  5. //        PWM4T1 = 4000;
  6. //        PWM5T1 = 4000;
  7. //        PWM6T1 = 4000;
  8. //        PWM7T1 = 4000;

  10.         PWM0T2 = 2000;  //第二個(gè)翻轉(zhuǎn)點(diǎn) 2000
  11.         PWM1T2 = 2000;
  12.         PWM2T2 = 2000;
  13.         PWM3T2 = 2000;
  14. //        PWM4T2 = 2000;
  15. //        PWM5T2 = 2000;
  16. //        PWM6T2 = 2000;
  17. //        PWM7T2 = 2000;
復(fù)制代碼

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

使用道具 舉報(bào)

26#
ID:925561 發(fā)表于 2021-6-11 10:32 | 只看該作者
lzl12399 發(fā)表于 2021-6-10 11:45
板子打樣，自己畫得還是有現(xiàn)成得，小弟也想玩玩

我是自己畫的
回復(fù)

使用道具 舉報(bào)

25#
ID:97350 發(fā)表于 2021-6-10 11:45 | 只看該作者
CHNO 發(fā)表于 2021-6-5 17:11
從宏晶官網(wǎng)下載，然后打過板子，四軸可以平穩(wěn)飛行，就是油門開大后不能穩(wěn)定的停在原地，會(huì)偏，正在查找原因 ...

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

使用道具 舉報(bào)

24#
ID:925561 發(fā)表于 2021-6-5 17:11 來自手機(jī) | 只看該作者
從宏晶官網(wǎng)下載，然后打過板子，四軸可以平穩(wěn)飛行，就是油門開大后不能穩(wěn)定的停在原地，會(huì)偏，正在查找原因中，
回復(fù)

使用道具 舉報(bào)

23#
ID:63317 發(fā)表于 2021-5-23 19:48 | 只看該作者
學(xué)習(xí)無人機(jī)，謝謝你的分享
回復(fù)

使用道具 舉報(bào)

22#
ID:915207 發(fā)表于 2021-5-14 22:27 | 只看該作者
感謝樓主分享，小白一名前來膜拜學(xué)習(xí)
回復(fù)

使用道具 舉報(bào)

21#
ID:906597 發(fā)表于 2021-4-30 22:28 | 只看該作者
這個(gè)厲害啊，想學(xué)但是沒怎么看懂哈哈
回復(fù)

使用道具 舉報(bào)

20#
ID:385637 發(fā)表于 2021-4-22 08:05 | 只看該作者
關(guān)鍵還是軟件算法
回復(fù)

使用道具 舉報(bào)

19#
ID:628703 發(fā)表于 2020-3-18 15:11 | 只看該作者
謝謝分享
回復(fù)

使用道具 舉報(bào)

18#
ID:517004 發(fā)表于 2020-2-10 17:26 | 只看該作者
請(qǐng)問您有飛行的視頻么
回復(fù)

使用道具 舉報(bào)

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

使用道具 舉報(bào)

16#
ID:266164 發(fā)表于 2020-1-16 16:55 | 只看該作者
正在學(xué)習(xí)無人機(jī)，謝謝你的分享
回復(fù)

使用道具 舉報(bào)

15#
ID:490427 發(fā)表于 2019-11-2 18:18 | 只看該作者
下載下來好好學(xué)習(xí)一下代碼
回復(fù)

使用道具 舉報(bào)

14#
ID:186764 發(fā)表于 2019-9-29 23:08 | 只看該作者
下載下來學(xué)習(xí)學(xué)習(xí)
回復(fù)

使用道具 舉報(bào)

13#
ID:282431 發(fā)表于 2019-7-28 17:02 | 只看該作者
謝謝分享，學(xué)著做一個(gè)四軸飛控
回復(fù)

使用道具 舉報(bào)

12#
ID:591598 發(fā)表于 2019-7-28 14:28 | 只看該作者
感謝樓主分享
回復(fù)

使用道具 舉報(bào)

11#
ID:272906 發(fā)表于 2019-6-9 10:08 | 只看該作者
謝謝樓主分享！
回復(fù)

使用道具 舉報(bào)

10#
ID:119159 發(fā)表于 2019-5-7 11:22 | 只看該作者
感謝~~~
回復(fù)

使用道具 舉報(bào)

9#
ID:508475 發(fā)表于 2019-4-16 09:24 | 只看該作者
樓主，可否加個(gè)QQ，跟你學(xué)習(xí)下，我的QQ 75750462
回復(fù)

使用道具 舉報(bào)

8#
ID:403129 發(fā)表于 2018-9-27 16:05 | 只看該作者
謝謝你的分享
回復(fù)

使用道具 舉報(bào)

7#
ID:291952 發(fā)表于 2018-9-25 09:12 | 只看該作者
感謝分享，對(duì)于學(xué)習(xí)很有幫助
回復(fù)

使用道具 舉報(bào)

6#
ID:394146 發(fā)表于 2018-9-5 16:57 | 只看該作者
關(guān)于玩四軸，我認(rèn)為深入的玩法就是研究軟硬件，特別是軟件，如果只是組裝比人的成品，沒啥成就感；本著此想法所以下載學(xué)習(xí)此算法！
回復(fù)

使用道具 舉報(bào)

5#
ID:380499 發(fā)表于 2018-7-29 16:26 | 只看該作者
為什么怎么也下不完呢，是有問題？
回復(fù)

使用道具 舉報(bào)

地板
ID:149866 發(fā)表于 2018-5-6 22:02 | 只看該作者
感謝分享,支持原創(chuàng),
回復(fù)

使用道具 舉報(bào)

板凳
ID:155817 發(fā)表于 2018-1-5 14:40 | 只看該作者
感謝分享，有空學(xué)習(xí)下
回復(fù)

使用道具 舉報(bào)

沙發(fā)
ID:243748 發(fā)表于 2017-12-4 14:52 來自手機(jī) | 只看該作者
謝謝分享，有時(shí)間做一個(gè)玩下
回復(fù)

使用道具 舉報(bào)

樓主
ID:61218 發(fā)表于 2017-11-28 11:24 | 只看該作者
下來試試...
回復(fù)

使用道具 舉報(bào)

本版積分規(guī)則

手機(jī)版|小黑屋|51黑電子論壇 |51黑電子論壇6群 QQ 管理員QQ:125739409;技術(shù)交流QQ群281945664

Powered by 單片機(jī)教程網(wǎng)

快速回復(fù) 返回頂部 返回列表