標(biāo)題: 51單片機四軸飛控STC8A8K16S4A12-LQFP44-PPM-V10源碼與說明資料下載 [打印本頁]
作者: avicii    時間: 2018-3-26 12:24
標(biāo)題: 51單片機四軸飛控STC8A8K16S4A12-LQFP44-PPM-V10源碼與說明資料下載
開源了，感興趣的朋友看一下，里面有代碼和pcb都有的.

STC8A8K飛控使用說明
本飛控使用世界上速度最快的51:宏晶科技出品的STC8系列單片機STC8A8K64S4A12 LQFP44
嘴主控，單片機價在3yuan左右，更宜好用。
本飛控已嚴(yán)格調(diào)試通過，用戶可d行參考、消化程序，不提供技術(shù)支持。
持別注意：下載時選杼內(nèi)部時鐘24MHZ，設(shè)罝用戶EEPROM大小為2K或以上.
資源簡介：
內(nèi)核：
超髙速805丨內(nèi)核（1T),是全球最快速的8051,比傳統(tǒng)805丨玦丨2倍，井且價格低廉.
指令代碼究全兼容傳統(tǒng)8051的111條指令
22個中斷源，4級中斷優(yōu)先級.
支持在線編程（丨SP).
支持在線仿真《
工作電壓：2(K5.5V.內(nèi)建LDO.
工作溫度：WC〜85-C,
FLASH存儲器：最大64K字節(jié)FLASH空間.
用戶可將部分FLASH配置為EEPROM. 512字節(jié)單頁療除。
SRA.X1: 128字節(jié)直接或間接訪問RAM  (DATA或IDATA訪問）
128 字節(jié)間接 RAM ( IDATA)
8192字節(jié)內(nèi)部擴展RAM (內(nèi)部XDATA)
外部可擴展64K字節(jié)RAM (外部XDATA)
時鐘：內(nèi)部24MI1Z高精度時鐘（ISP編程可調(diào)整)，常溫（25)誤差±0.3%。
溫度范圍-20°C〜65 °C時溫漂-丨.0%~-0.5%,溫度范圍-40°C ~85 °C時溫漂-1.8%—0.8%
內(nèi)部32KJIZ低速時鐘（誤差較大）。
外部晶振或外部時鐘（4MI1Z〜33VI11Z),用戶軟件自由選擇這3種時鐘源。
中斷： 22個中斷源。
數(shù)字外設(shè)：5個16位定時器，4個高速串口，4路PCA模塊，8路15位增強型PWM,   SPI ,   I2C。
模擬外設(shè)：16通道12位ADC, 1組比較器。
GPIO:  LQFP44 封裝又 39 個io, LQFP48 有 43 個丨0, LQFP64S 有 59 個io。


飛控的電路原理圖：

本飛控僅僅是姿態(tài)飛行控制，沒有GPS、電子羅盤、氣壓高度計、超聲波測距、光流傳感器等等， 不能實現(xiàn)定點懸停，但是飛行感覺非常好，穩(wěn)定，特別是暴力飛行的刺激，是很多玩家所喜歡的。用戶 可以A行增加這些傳感器，編寫相關(guān)的程序，以獲得更好的飛行性能。
本飛控通過調(diào)整PID參數(shù)可以適應(yīng)從250mm軸距到750mm軸距的，都實際裝機驗證過，效果很 好。
本例實用F450的四軸機架，大約40元，安裝簡單，入門快，讓玩家可以快速的裝配成功，如動手 能力強可以A己買配件做機架，鋁合金或碳纖維均可。我喜歡用鋁合金方管，好加工，強度好，還很輕。配套使用MC6B的遙控、接收套件，左手油門（俗稱“美國手”)，大約130元，是我能找到的最 便宜的遙控器了，不差錢的玩家可以使用更昂貴的遙控器套件。四軸實物照片如K閣所示。本例只說明如何使用飛控,對于四軸工作原理不做描述，要科普的用戶可以A行上網(wǎng)搜索相關(guān)知識。
主要零件：
STC8A8K16S4A12 LQFP44 做的飛控 1 塊。
MPU-6050三軸陀螺儀、三軸加速度傳感器模塊1塊。
MC6B遙控、接收機1套。
F450玻纖四軸機架1套。
2212無刷電機4個（配香蕉插)。
20A電調(diào)4個。
9450正反槳2對（實際買多些，因為新手會有損耗)。
3S鋰電池4200mAH  1塊（用戶可以購買多塊爽飛）。
B6平衡充（帶12V 5A電源）1套。
T插插頭1個。
12號硅膠線紅、黑色給20cm。
魔術(shù)帶（捆綁電池的）1條。
3M雙而膠（3*7cm,粘電調(diào)、飛控用）2片。
扎絲或扎帶若千（我喜歡扎絲，因為可以方便的擰下來)。
紅光、綠光LED燈條（可選）各2條。（用戶可以先不裝，要夜航的才裝，我就喜歡夜航)。


單片機源程序如下:
  1. /* 本程序經(jīng)過測試完全正常, 不提供電話技術(shù)支持, 如不能理解, 請自行補充相關(guān)基礎(chǔ).  */
  2. /***  特別注意: 下載時選擇內(nèi)部時鐘24MHZ, 設(shè)置用戶EEPROM大小為2K或以上.  ****/
  3. /*********************************************
  4.   四軸飛控-V10.C
  5. 使用遙控接收器型號: MC6B六通道2.4G 100mW.

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

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

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

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

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

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

  22. 電池低壓報警：當(dāng)電池低壓時，蜂鳴器"嗶嗶"報警，同時航燈閃爍，此時請盡快回航降落。

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

  27. #define                Baudrate1                        115200UL
  28. #define                TX1_LENGTH        128
  29. #define                RX1_LENGTH        128


  32. #include "config.h"
  33. #include "STC8xxx_PWM.H"
  34. #include "MPU6050.H"
  35. #include "AD.H"       
  36. #include "EEPROM.H"
  37. #include "PCA.h"
  38. #include <math.H>

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


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

  54. int                int_tmp;
  55. u8                YM=0,FRX=128,FRY=128,FRZ=128;                                //4通道遙控信號.
  56. u8                xdata        tp[16];                //讀MP6050緩沖


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

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

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


  69. /******************************************************************************/
  70. #define        Out_XP        6.65f        //ADC0        外環(huán)P        V1 / 10
  71. #define        Out_XI        0.0074f        //ADC4        外環(huán)I        V2 / 10000
  72. #define        Out_XD        6.0f        //ADC5        外環(huán)D        V3 / 10

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


  79. #define        Out_YP        Out_XP
  80. #define        Out_YI        Out_XI
  81. #define        Out_YD        Out_XD

  83. #define        In_YP        In_XP
  84. #define        In_YI        In_XI
  85. #define        In_YD        In_XD


  88. #define        ZP        5.0f
  89. #define        ZI        0.1f
  90. #define        ZD        4.0f        //自旋控制的P D
  91. float Z_integral=0;//Z軸積分

  93. #define        ERR_MAX        500
  94. //======================================================================


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


  101. //======================================================================
  102. bit        B_8ms;        //8ms標(biāo)志

  104. bit        B_rtn_ADC0;        //請求返回信息
  105. bit        B_BAT_LOW;        //低電壓標(biāo)志
  106. u8        xdata cnt_ms;                //時間計數(shù)


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


  120. u8                xdata Cal_Setp=0;                        //校準(zhǔn)步驟
  121. u8                xdata Cal_cnt=0;                        //校準(zhǔn)平均值計數(shù)
  122. int                xdata x_sum,y_sum,z_sum;        //校準(zhǔn)累加和
  123. float        xdata float_x_sum,float_y_sum;        //校準(zhǔn)累加和

  125. u8        xdata BuzzerOnTime,BuzzerOffTime,BuzzerRepeat,BuzzerOnCnt,BuzzerOffCnt;
  126. u8        xdata cnt_100ms;


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

  137. u16        xdata CCAP1_RiseTime;
  138. u8        xdata PPM2_Rise_TimeOut;        //高電平限時
  139. u8        xdata PPM2_Rx_TimerOut;
  140. u8        xdata PPM2_RxCnt;
  141. u16        xdata PPM2_Cap;
  142. bit        B_PPM2_OK;

  144. u16        xdata CCAP2_RiseTime;
  145. u8        xdata PPM3_Rise_TimeOut;        //高電平限時
  146. u8        xdata PPM3_Rx_TimerOut;
  147. u8        xdata PPM3_RxCnt;
  148. u16        xdata PPM3_Cap;
  149. bit        B_PPM3_OK;

  151. u16        xdata CCAP3_RiseTime;
  152. u8        xdata PPM4_Rise_TimeOut;        //高電平限時
  153. u8        xdata PPM4_Rx_TimerOut;
  154. u8        xdata PPM4_RxCnt;
  155. u16        xdata PPM4_Cap;
  156. bit        B_PPM4_OK;

  158. u16        xdata CCAP_FallTime;

  160. u8        PPM1,PPM2,PPM3,PPM4;
  161. bit        B_Start;
  162. u8        cnt_start;

  164. /* ============================================= */


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

  181. extern xdata u16        adc0;
  182. extern xdata int        Battery;


  185. //*********************************************************************
  186. //****************角度計算*********************************************
  187. //*********************************************************************
  188. #define        pi                3.14159265f                           
  189. #define        Kp                0.8f                        
  190. #define        Ki                0.001f                        
  191. #define        halfT        0.004f           

  193. float idata q0=1,q1=0,q2=0,q3=0;   
  194. float idata exInt=0,eyInt=0,ezInt=0;  


  197. void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
  198. {
  199.         float data norm;
  200.         float idata vx, vy, vz;
  201.         float idata ex, ey, ez;

  203.         norm = sqrt(ax*ax + ay*ay + az*az);        //把加速度計的三維向量轉(zhuǎn)成單維向量   
  204.         ax = ax / norm;
  205.         ay = ay / norm;
  206.         az = az / norm;

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

  216.         ex = (ay*vz - az*vy) ;
  217.         ey = (az*vx - ax*vz) ;
  218.         ez = (ax*vy - ay*vx) ;

  220.         exInt = exInt + ex * Ki;
  221.         eyInt = eyInt + ey * Ki;
  222.         ezInt = ezInt + ez * Ki;

  224.         gx = gx + Kp*ex + exInt;
  225.         gy = gy + Kp*ey + eyInt;
  226.         gz = gz + Kp*ez + ezInt;

  228.         q0 = q0 + (-q1*gx - q2*gy - q3*gz) * halfT;
  229.         q1 = q1 + ( q0*gx + q2*gz - q3*gy) * halfT;
  230.         q2 = q2 + ( q0*gy - q1*gz + q3*gx) * halfT;
  231.         q3 = q3 + ( q0*gz + q1*gy - q2*gx) * halfT;

  233.         norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
  234.         q0 = q0 / norm;
  235.         q1 = q1 / norm;
  236.         q2 = q2 / norm;
  237.         q3 = q3 / norm;

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



  245. //****************姿態(tài)計算*********************************************
  246. void PWM_int (void) interrupt         22        //PWM中斷函數(shù)
  247. {
  248.         PWMCFG = 0;        //CBIF;        //清除中斷標(biāo)志

  250.         B_8ms = 1;

  252. //======================== 超時溢出處理 ==============================================
  253.         PPM1_Rise_TimeOut++;        //高電平限時
  254.         PPM2_Rise_TimeOut++;        //高電平限時
  255.         PPM3_Rise_TimeOut++;        //高電平限時
  256.         PPM4_Rise_TimeOut++;        //高電平限時

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

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

  297.         FRX = PPM1;
  298.         FRY = PPM2;
  299.         YM  = PPM3;        //油門
  300.         FRZ = PPM4;
  301.        

  303. //********************************************************************************************
  304.         Read_MPU6050(tp);        //680us

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

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

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

  320.         if(YM > 35)        ERRORX_Out += Ax,        ERRORX_Out += Ax,        ERRORX_Out += Ax;        //外環(huán)積分(油門小于某個值時不積分)
  321.         else                ERRORX_Out = 0; //油門小于定值時清除積分值
  322.                  if(ERRORX_Out >  1500)        ERRORX_Out =  1500;
  323.         else if(ERRORX_Out < -1500)        ERRORX_Out = -1500;        //積分限幅

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

  333.         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
  334.         if(PID_x >  500)        PID_x =  500;        //輸出量限幅
  335.         if(PID_x < -500)        PID_x = -500;

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

  348.         if(YM > 35)        ERRORY_In += (Angle_gx - Out_PID_Y);  //內(nèi)環(huán)積分(油門小于某個值時不積分)
  349.         else                ERRORY_In = 0; //油門小于定值時清除積分值
  350.                  if(ERRORY_In >  500)        ERRORY_In =  500;
  351.         else if(ERRORY_In < -500)        ERRORY_In = -500;        //積分限幅
  352.        
  353.         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
  354.        
  355.         if(PID_y > 500)        PID_y =  500;        //輸出量限幅
  356.         if(PID_y <-500)        PID_y = -500;

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

  366.         PID_z = Az * ZP + Z_integral * ZI + (Az - Last_Az) * ZD;
  367.         Last_Az = Az;
  368.         if(PID_z >  200)        PID_z =  200;        //輸出量限幅
  369.         if(PID_z < -200)        PID_z = -200;

  371.         speed0 = (int)(  PID_x + PID_y + PID_z);        //M1改為逆時針
  372.         speed1 = (int)(  PID_x - PID_y - PID_z);
  373.         speed2 = (int)( -PID_x - PID_y + PID_z);
  374.         speed3 = (int)( -PID_x + PID_y - PID_z);

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

  384.                 PWM0 = int_tmp - speed0;

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

  389.                 PWM1 = int_tmp - speed1;

  391.                          if(PWM1 > 1000)        PWM1 = 1000;
  392.                 else if(PWM1 < 10)                PWM1 = 10;

  394.                 PWM2 = int_tmp - speed2;

  396.                          if(PWM2 > 1000)        PWM2 = 1000;
  397.                 else if(PWM2 < 10)                PWM2 = 10;

  399.                 PWM3 = int_tmp - speed3;

  401.                          if(PWM3 > 1000)        PWM3 = 1000;
  402.                 else if(PWM3 < 10)                PWM3 = 10;
  403.         }

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

  413. }


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

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

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

  495. // ===================== 主函數(shù) =====================
  496. void main(void)
  497. {

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

  509.         PPM1 = 128;
  510.         PPM2 = 128;
  511.         PPM3 = 0;
  512.         PPM4 = 128;

  514.         PWMGO();

  516.         P_Light  = 0;
  517.         P_BUZZER = 0;
  518.         P5n_push_pull(0x30);

  520.         adc_init();    //啟動A/D
  521.        
  522.         PCA_config();

  524.         delay_ms(100);
  525.         IAPRead();                //讀取陀螺儀靜差
  526.         InitMPU6050();        //初始化MPU-6050
  527.         delay_ms(100);

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

  546.         SetBuzzer(5,1,1);
  547.        

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

  554.         B_Start = 0;        //上電禁止運行

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

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


  647.                 if(B_8ms)                //8ms到
  648.                 {
  649.                         B_8ms = 0;
  650.                        
  651.                         if(Cal_Setp != 0)        AutoCal();        //是否執(zhí)行自動校準(zhǔn)序列
  652.                         AD();                // 讀ADC計算電壓

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

  656.                         B = cnt_ms;
  657.                         ++cnt_ms;
  658.                         B = (B ^ cnt_ms) & cnt_ms;

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

  680.                                 if(!B_BAT_LOW && (YM_LostCnt < 120))        P_Light = 1;        //遙控信號正常,        電壓正常時
  681.                         }
  682.                         else if(B7)                //2048ms
  683.                         {
  684.                                 if(!B_BAT_LOW && (YM_LostCnt >= 120))        SetBuzzer(1,1,3);        //電壓正常時 遙控信號丟失, 每兩秒短鳴3次,
  685.                         }
  686.                 }


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

  706.         }
  707. }

  709. //=========================================================

  711. void        Return_Message(void)
  712. {
  713.         TX1_write2buff('V');
  714.         TX1_write2buff('=');
  715.         TX1_write2buff(Battery/1000 + '0');
  716.         TX1_write2buff((Battery%1000)/100 + '0');
  717.         TX1_write2buff('.');
  718.         TX1_write2buff((Battery%100)/10 + '0');
  719.         TX1_write2buff(Battery%10 + '0');
  720.         TX1_write2buff(' ');
  721.         TX1_write2buff(' ');

  723.         PrintString1("AngleX=");
  724.         TX1_int_value((int)(AngleX * 10));

  726.         PrintString1("AngleY=");
  727.         TX1_int_value((int)(AngleY * 10));

  729.         PrintString1("AngleZ=");
  730.         TX1_int_value((int)(Angle_gz * 10));

  732.         PrintString1("a_x=");
  733.         TX1_int_value(a_x * 10);
  734.         PrintString1("a_y=");
  735.         TX1_int_value(a_y * 10);
  736.         PrintString1("g_z=");
  737.         TX1_int_value(g_z * 10);

  739.         TX1_cnt = 0;
  740.         TX1_write2buff(0x0d);
  741.         TX1_write2buff(0x0a);
  742. }


  745. void  delay_ms(u8 ms)
  746. {
  747.      u16 i;
  748.          do
  749.          {
  750.                  i = MAIN_Fosc / 13000;
  751.                 while(--i)        ;   //13T per loop
  752.      }while(--ms);
  753. }


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

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


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

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

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


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

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

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

  849.         B_TX1_Busy = 0;
  850.         TX1_Read   = 0;
  851.         TX1_Write  = 0;
  852.         UART1_cmd  = 0;
  853.         TX1_cnt    = 0;
  854. }


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

  874.         if(TI)
  875.         {
  876.                 TI = 0;
  877.                 B_TX1_Busy = 0;
  878.         }
  879. }



  883. void        PCA_config(void)
  884. {
  885.         PPM1_Rise_TimeOut = 0;
  886.         PPM2_Rise_TimeOut = 0;
  887.         PPM3_Rise_TimeOut = 0;
  888.         PPM4_Rise_TimeOut = 0;

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

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

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

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

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

  918.         CR = 1;
  919. }


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

  963.         if(CCF1)        //PCA模塊1中斷
  964.         {
  965.                 CCF1 = 0;                //清PCA模塊1中斷標(biāo)志
  966.                 if(P16)        //上升沿
  967.                 {
  968.                         CCAP1_RiseTime = ((u16)CCAP1H << 8) + CCAP1L;        //讀CCAP1
  969.                         PPM2_Rise_TimeOut = 1;        //收到上升沿, 高電平限時
  970.                 }
  971.                 else        //下降沿
  972.                 {
  973.                         CCAP_FallTime = ((u16)CCAP1H << 8) + CCAP1L;        //讀CCAP1
  974.                         if((PPM2_Rise_TimeOut != 0) && (PPM2_Rise_TimeOut < 3))        //收到過上升沿, 高電平也沒有溢出
  975.                         {
  976.                                 CCAP_FallTime = (CCAP_FallTime - CCAP1_RiseTime) >> 1;        //為了好處理, 轉(zhuǎn)成單位為us
  977.                                 if((CCAP_FallTime >= 800) && (CCAP_FallTime <= 2500))
  978.                                 {
  979.                                         if(++PPM2_RxCnt >= 5)        PPM2_RxCnt = 5;
  980.                                         if(PPM2_RxCnt == 5)
  981.                                         {
  982.                                                 if(!B_PPM2_OK)
  983.                                                 {
  984.                                                         PPM2_Cap = CCAP_FallTime;
  985.                                                         B_PPM2_OK = 1;                //標(biāo)志收到一個脈沖
  986.                                                         PPM2_Rx_TimerOut = 12;        //限時收不到脈沖
  987.                                                 }
  988.                                         }
  989.                                 }
  990.                         }
  991.                         PPM2_Rise_TimeOut = 0;
  992.                 }
  993.         }

  995.         if(CCF2)        //PCA模塊2中斷
  996.         {
  997.                 CCF2 = 0;                //清PCA模塊1中斷標(biāo)志
  998.                 if(P15)        //上升沿
  999.                 {
  1000.                         CCAP2_RiseTime = ((u16)CCAP2H << 8) + CCAP2L;        //讀CCAP2
  1001.                         PPM3_Rise_TimeOut = 1;        //收到上升沿, 高電平限時
  1002.                 }
  1003.                 else        //下降沿
  1004.                 {
  1005.                         CCAP_FallTime = ((u16)CCAP2H << 8) + CCAP2L;        //讀CCAP2
  1006.                         if((PPM3_Rise_TimeOut != 0) && (PPM3_Rise_TimeOut < 3))        //收到過上升沿, 高電平也沒有溢出
  1007.                         {
  1008.                                 CCAP_FallTime = (CCAP_FallTime - CCAP2_RiseTime) >> 1;        //為了好處理, 轉(zhuǎn)成單位為us
  1009.                                 if((CCAP_FallTime >= 800) && (CCAP_FallTime <= 2500))
  1010.                                 {
  1011.                                         if(++PPM3_RxCnt >= 5)        PPM3_RxCnt = 5;
  1012.                                         if(PPM3_RxCnt == 5)
  1013.                                         {
  1014.                                                 if(!B_PPM3_OK)
  1015.                                                 {
  1016.                                                         PPM3_Cap = CCAP_FallTime;
  1017.                                                         B_PPM3_OK = 1;                //標(biāo)志收到一個脈沖
  1018.                                                         PPM3_Rx_TimerOut = 25;        //限時收不到脈沖
  1019.                                                         YM_LostCnt = 0;
  1020.                                                         Lost16S    = 0;
  1021.                                                 }
  1022.                                         }
  1023.                                 }
  1024.                         }
  1025.                         PPM3_Rise_TimeOut = 0;
  1026.                 }
  1027.         }

  1029.         if(CCF3)        //PCA模塊3中斷
  1030.         {
  1031.                 CCF3 = 0;                //清PCA模塊1中斷標(biāo)志
  1032.                 if(P14)        //上升沿
  1033.                 {
  1034.                         CCAP3_RiseTime = ((u16)CCAP3H << 8) + CCAP3L;        //讀CCAP3
  1035.                         PPM4_Rise_TimeOut = 1;        //收到上升沿, 高電平限時
  1036.                 }
  1037.                 else        //下降沿
  1038.                 {
  1039.                         CCAP_FallTime = ((u16)CCAP3H << 8) + CCAP3L;        //讀CCAP3
  1040.                         if((PPM4_Rise_TimeOut != 0) && (PPM4_Rise_TimeOut < 3))        //收到過上升沿, 高電平也沒有溢出
  1041.                         {
  1042.                                 CCAP_FallTime = (CCAP_FallTime - CCAP3_RiseTime) >> 1;        //為了好處理, 轉(zhuǎn)成單位為us
  1043.                                 if((CCAP_FallTime >= 800) && (CCAP_FallTime <= 2500))
  1044.                                 {
  1045.                                         if(++PPM4_RxCnt >= 5)        PPM4_RxCnt = 5;
  1046.                                         if(PPM4_RxCnt == 5)
  1047.                                         {
  1048.                                                 if(!B_PPM4_OK)
  1049.                                                 {
  1050.                                                         PPM4_Cap = CCAP_FallTime;
  1051.                                                         B_PPM4_OK = 1;                //標(biāo)志收到一個脈沖
  1052.                                                         PPM4_Rx_TimerOut = 12;        //限時收不到脈沖
  1053.                                                 }
  1054.                                         }
  1055.                                 }
  1056.                         }
  1057.                         PPM4_Rise_TimeOut = 0;
  1058.                 }
  1059.         }

  1061. //        if(CF)        //PCA溢出中斷
  1062. //        {
  1063. //                CF = 0;                        //清PCA溢出中斷標(biāo)志
  1064. //        }

  1066. }
復(fù)制代碼


所有資料51hei提供下載:
四軸飛控-STC8A8K16S4A12-LQFP44-PPM-V10.zip (3.87 MB, 下載次數(shù): 144)


作者: 老婆當(dāng)家    時間: 2018-6-3 20:38
好像沒有人喜歡這個帖子啊
作者: 合吾小舍    時間: 2018-9-21 10:43
算法方面有嗎，截圖好像沒有工程啊
作者: qq451987409    時間: 2019-6-26 19:51
感謝分享
作者: 987654321123    時間: 2019-11-3 00:26
沒有pcb板程序呀？pcb板上的走線布置，信號處理能學(xué)習(xí)到很多知識
作者: CuChulainn    時間: 2019-11-27 14:36
謝謝分享
作者: 騰飛的龍    時間: 2019-12-14 15:37
感謝分享�。。�！贊



歡迎光臨 (http://www.torrancerestoration.com/bbs/) Powered by Discuz! X3.1