|
|
四軸飛控-STC8A8K64S4A12-LQFP44-PPM-V10，來源于STC發布，按需取用
控制信號排針要靠在一起布線成3排針3*9*2.54
P1~P4對應接收機的俯仰、橫滾、油門、航向PPM信號。M1~M4輸出到4個電調的信號。P5備用。
四個電機安排順序如下：
0.png (4.83 KB, 下載次數: 164)
下載附件
2017-11-27 04:12 上傳
串口1: 下載、調試、控制信號輸入.
串口2: 備用為GPS接口或數傳接口.
原理圖：
0.png (116.99 KB, 下載次數: 165)
下載附件
2017-11-27 04:09 上傳
0.png (13.76 KB, 下載次數: 174)
下載附件
2017-11-27 04:09 上傳
飛控的stc單片機程序源碼：
- /* 本程序經過測試完全正常, 不提供電話技術支持, 如不能理解, 請自行補充相關基礎. */
- /*** 特別注意: 下載時選擇內部時鐘24MHZ, 設置用戶EEPROM大小為2K或以上. ****/
- /*********************************************
- 四軸飛控-V10.C
- 使用遙控接收器型號: MC6B六通道2.4G 100mW.
- 四軸上電待機：上電后，航燈不亮，接收機LED閃爍，此時打開遙控器，將左右油門下拉到最小，接收機收到信號LED常亮，
- 表示RF通訊已連接。此時蜂鳴器"嗶"一聲，航燈閃爍，表示待機模式。
- 四軸啟動：將遙控器左右操縱桿掰成下內八，啟動四軸，四軸"嗶"一聲，4個螺旋槳開始低速旋轉，航燈常亮。
- 此后提升油門，就可以加速螺旋槳，直到起飛。
- 四軸飛行：起飛后，可以操縱右手的俯仰、橫滾操縱桿，實現前后左右或任意方向的飛行。
- 左手油門桿左掰是航向逆時針轉，右掰是航向順時鐘轉。
- 四軸下降停止：收油門，四軸逐漸下降到地面，然后兩操縱桿掰成下外八，停止四軸，重新處于待機模式。
- 四軸水平校準：將四軸放置于水平地面，處于待機模式，然后兩操縱桿掰成上內八，四軸"嗶"一聲進入校準，完成后"嗶嗶"兩聲完成校準。
- 四軸取消水平校準：將四軸放置于水平地面，處于待機模式，然后兩操縱桿掰成上外八，四軸"嗶"一聲取消校準。取消水平校準或未進行水平校準過的四軸，起飛時即使無風也可能會有明顯漂移。
- 電池低壓報警：當電池低壓時，蜂鳴器"嗶嗶"報警，同時航燈閃爍，此時請盡快回航降落。
- 無遙控信號異常：當四軸在空中突然收不到遙控信號時，四軸蜂鳴器發出"嗶嗶嗶"報警，同時航燈閃爍，四軸保持水平，逐漸自動減小油門降落。
- ***********************************************/
- #define Baudrate1 115200UL
- #define TX1_LENGTH 128
- #define RX1_LENGTH 128
- #include "config.h"
- #include "STC8xxx_PWM.H"
- #include "MPU6050.H"
- #include "AD.H"
- #include "EEPROM.H"
- #include "PCA.h"
- #include <math.H>
- sbit P_Light = P5^4; //航燈
- sbit P_BUZZER = P5^5; //蜂鳴器
- int xdata g_x=0,g_y=0,g_z=0; //陀螺儀矯正參數
- float xdata a_x=0,a_y=0; //角度矯正參數
- float data AngleX=0,AngleY=0; //四元數解算出的歐拉角
- float xdata Angle_gx=0,Angle_gy=0,Angle_gz=0; //由角速度計算的角速率(角度制)
- float xdata Angle_ax=0,Angle_ay=0,Angle_az=0; //由加速度計算的加速度(弧度制)
- float xdata Ax=0,Ay=0,Az=0; //加入遙控器控制量后的角度
- float data PID_x=0,PID_y=0,PID_z=0; //PID最終輸出量
- int data speed0=0,speed1=0,speed2=0,speed3=0; //電機速度參數
- int data PWM0=0,PWM1=0,PWM2=0,PWM3=0;//,PWM4=0,PWM5=0; //加載至PWM模塊的參數
- int int_tmp;
- u8 YM=0,FRX=128,FRY=128,FRZ=128; //4通道遙控信號.
- u8 xdata tp[16]; //讀MP6050緩沖
- //****************姿態處理和PID*********************************************
- float xdata Out_PID_X=0,Last_Angle_gx=0; //外環PI輸出量 上一次陀螺儀數據
- float xdata ERRORX_Out=0,ERRORX_In=0; //外環P 外環I 外環誤差積分
- float xdata Out_PID_Y=0,Last_Angle_gy=0;
- float xdata ERRORY_Out=0,ERRORY_In=0; //規則1:內外環P乘積等于10.5
- float xdata Last_Ax=0,Last_Ay=0,Last_Az=0;
- /******************************************************************************/
- #define Out_XP 6.65f //ADC0 外環P V1 / 10
- #define Out_XI 0.0074f //ADC4 外環I V2 / 10000
- #define Out_XD 6.0f //ADC5 外環D V3 / 10
- #define In_XP 0.8275f //ADC6 內環P V4 / 100
- #define In_XI 0.0074f //ADC4 內環I V2 / 10000
- #define In_XD 6.0f //ADC5 內環D V3 / 10
- #define Out_YP Out_XP
- #define Out_YI Out_XI
- #define Out_YD Out_XD
- #define In_YP In_XP
- #define In_YI In_XI
- #define In_YD In_XD
- #define ZP 5.0f
- #define ZI 0.1f
- #define ZD 4.0f //自旋控制的P D
- float Z_integral=0;//Z軸積分
- #define ERR_MAX 500
- //======================================================================
- u8 data YM_LostCnt=0, Lost16S; //上一次RxBuf[0]數據(RxBuf[0]數據在不斷變動的) 狀態標識
- u8 SW2_tmp;
- //======================================================================
- bit B_8ms; //8ms標志
- bit B_rtn_ADC0; //請求返回信息
- bit B_BAT_LOW; //低電壓標志
- u8 xdata cnt_ms; //時間計數
- u8 xdata UART1_cmd=0; //串口命令
- u8 xdata TX1_Read=0; //發送讀指針
- u8 xdata TX1_Write=0; //發送寫指針
- u8 xdata TX1_cnt=0; //發送計數
- u8 xdata TX1_Buffer[TX1_LENGTH]; //發送緩沖
- bit B_TX1_Busy; //發送忙標志
- u8 xdata RX1_Cnt,RX1_Timer;
- u8 xdata RX1_Buffer[RX1_LENGTH];
- bit B_RX1_OK;
- u8 xdata Cal_Setp=0; //校準步驟
- u8 xdata Cal_cnt=0; //校準平均值計數
- int xdata x_sum,y_sum,z_sum; //校準累加和
- float xdata float_x_sum,float_y_sum; //校準累加和
- u8 xdata BuzzerOnTime,BuzzerOffTime,BuzzerRepeat,BuzzerOnCnt,BuzzerOffCnt;
- u8 xdata cnt_100ms;
- /* =================== PPM接收相關變量 ========================== */
- u16 xdata CCAP0_RiseTime; //捕捉到的上升沿時刻
- u8 xdata PPM1_Rise_TimeOut; //高電平限時
- u8 xdata PPM1_Rx_TimerOut; //接收超時計數
- u8 xdata PPM1_RxCnt; //接收次數計數
- u16 xdata PPM1_Cap; //捕捉到的PPM脈沖寬度
- bit B_PPM1_OK; //接收到一個PPM脈沖寬度
- u16 xdata CCAP1_RiseTime;
- u8 xdata PPM2_Rise_TimeOut; //高電平限時
- u8 xdata PPM2_Rx_TimerOut;
- u8 xdata PPM2_RxCnt;
- u16 xdata PPM2_Cap;
- bit B_PPM2_OK;
- u16 xdata CCAP2_RiseTime;
- u8 xdata PPM3_Rise_TimeOut; //高電平限時
- u8 xdata PPM3_Rx_TimerOut;
- u8 xdata PPM3_RxCnt;
- u16 xdata PPM3_Cap;
- bit B_PPM3_OK;
- u16 xdata CCAP3_RiseTime;
- u8 xdata PPM4_Rise_TimeOut; //高電平限時
- u8 xdata PPM4_Rx_TimerOut;
- u8 xdata PPM4_RxCnt;
- u16 xdata PPM4_Cap;
- bit B_PPM4_OK;
- u16 xdata CCAP_FallTime;
- u8 PPM1,PPM2,PPM3,PPM4;
- bit B_Start;
- u8 cnt_start;
- /* ============================================= */
- void UART1_config(void);
- void PrintString1(u8 *puts); //發送一個字符串
- void TX1_write2buff(u8 dat); //寫入發送緩沖，指針+1
- void TX1_int_value(int i);
- void delay_ms(u8 ms);
- void Return_Message(void);
- u16 MODBUS_CRC16(u8 *p,u8 n); //input: *p--->First Data Address,n----->Data Number, return: CRC16
- void PCA_config(void);
- void Timer0_Config(void);
- void Timer1_Config(void);
- void return_TTMx(u8 id,PPMx);
- void Timer0_Config(void);
- u16 MODBUS_CRC16(u8 *p,u8 n); //input: *p--->First Data Address,n----->Data Number, return: CRC16
- extern xdata u16 adc0;
- extern xdata int Battery;
- //*********************************************************************
- //****************角度計算*********************************************
- //*********************************************************************
- #define pi 3.14159265f
- #define Kp 0.8f
- #define Ki 0.001f
- #define halfT 0.004f
- float idata q0=1,q1=0,q2=0,q3=0;
- float idata exInt=0,eyInt=0,ezInt=0;
- void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
- {
- float data norm;
- float idata vx, vy, vz;
- float idata ex, ey, ez;
- norm = sqrt(ax*ax + ay*ay + az*az); //把加速度計的三維向量轉成單維向量
- ax = ax / norm;
- ay = ay / norm;
- az = az / norm;
- // 下面是把四元數換算成《方向余弦矩陣》中的第三列的三個元素。
- // 根據余弦矩陣和歐拉角的定義，地理坐標系的重力向量，轉到機體坐標系，正好是這三個元素
- // 所以這里的vx vy vz，其實就是當前的歐拉角（即四元數）的機體坐標參照系上，換算出來的
- // 重力單位向量。
- vx = 2*(q1*q3 - q0*q2);
- vy = 2*(q0*q1 + q2*q3);
- vz = q0*q0 - q1*q1 - q2*q2 + q3*q3 ;
- ex = (ay*vz - az*vy) ;
- ey = (az*vx - ax*vz) ;
- ez = (ax*vy - ay*vx) ;
- exInt = exInt + ex * Ki;
- eyInt = eyInt + ey * Ki;
- ezInt = ezInt + ez * Ki;
- gx = gx + Kp*ex + exInt;
- gy = gy + Kp*ey + eyInt;
- gz = gz + Kp*ez + ezInt;
- q0 = q0 + (-q1*gx - q2*gy - q3*gz) * halfT;
- q1 = q1 + ( q0*gx + q2*gz - q3*gy) * halfT;
- q2 = q2 + ( q0*gy - q1*gz + q3*gx) * halfT;
- q3 = q3 + ( q0*gz + q1*gy - q2*gx) * halfT;
- norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
- q0 = q0 / norm;
- q1 = q1 / norm;
- q2 = q2 / norm;
- q3 = q3 / norm;
- AngleX = asin(2*(q0*q2 - q1*q3 )) * 57.2957795f; // 俯仰 換算成度
- AngleY = asin(2*(q0*q1 + q2*q3 )) * 57.2957795f; // 橫滾
- }
- //****************姿態計算*********************************************
- void PWM_int (void) interrupt 22 //PWM中斷函數
- {
- PWMCFG = 0; //CBIF; //清除中斷標志
- B_8ms = 1;
- //======================== 超時溢出處理 ==============================================
- PPM1_Rise_TimeOut++; //高電平限時
- PPM2_Rise_TimeOut++; //高電平限時
- PPM3_Rise_TimeOut++; //高電平限時
- PPM4_Rise_TimeOut++; //高電平限時
- if(--PPM1_Rx_TimerOut == 0) //超過100ms收不到信號
- {
- PPM1_RxCnt = 0; //一旦出現溢出, 則開始的n個脈沖無效
- PPM1 = 128;; //默認中點
- }
- if(--PPM2_Rx_TimerOut == 0) //超過100ms收不到信號
- {
- PPM2_RxCnt = 0; //一旦出現溢出, 則開始的n個脈沖無效
- PPM2 = 128;; //默認中點
- }
- if(--PPM3_Rx_TimerOut == 0) //超過200ms收不到信號
- {
- PPM3_RxCnt = 0; //一旦出現溢出, 則開始的n個脈沖無效
- }
- if(--PPM4_Rx_TimerOut == 0) //超過100ms收不到信號
- {
- PPM4_RxCnt = 0; //一旦出現溢出, 則開始的n個脈沖無效
- PPM4 = 128; //默認中點
- }
- //======================================================================
- if(++YM_LostCnt >= 250) //失聯2秒后
- {
- YM_LostCnt = 200; //重復0.4秒，失控保護
- if(PPM3 > 80) PPM3--;
- else if(++Lost16S >= 40)
- {
- Lost16S = 250;
- PPM3 = 0;
- B_Start = 0;
- }
- }
- if(YM_LostCnt >= 25) //失聯200ms
- {
- PPM1 = 128;
- PPM2 = 128; //俯仰 橫滾 航向均歸0
- PPM4 = 128;
- }
- FRX = PPM1;
- FRY = PPM2;
- YM = PPM3; //油門
- FRZ = PPM4;
-
- //********************************************************************************************
- Read_MPU6050(tp); //680us
- Angle_ax = ((float)(((int *)&tp)[0])) / 8192.0; //加速度處理 結果單位是 +- g
- Angle_ay = ((float)(((int *)&tp)[1])) / 8192.0; //轉換關系 8192 LSB/g, 1g對應讀數8192
- Angle_az = ((float)(((int *)&tp)[2])) / 8192.0; //加速度量程 +-4g/S
- Last_Angle_gx = Angle_gx; //儲存上一次角速度數據
- Last_Angle_gy = Angle_gy;
- Angle_gx = ((float)(((int *)&tp)[4] - g_x)) / 65.5; //陀螺儀處理 結果單位是 +-度
- Angle_gy = ((float)(((int *)&tp)[5] - g_y)) / 65.5; //陀螺儀量程 +-500度/S, 1度/秒 對應讀數 65.536
- Angle_gz = ((float)(((int *)&tp)[6] - g_z)) / 65.5; //轉換關系65.5 LSB/度
- IMUupdate(Angle_gx*0.0174533f, Angle_gy*0.0174533f, Angle_gz*0.0174533f, Angle_ax,Angle_ay,Angle_az);
- //**********************************X軸指向************************************************
- Ax = AngleX - a_x - ((float)FRX - 128) / 4.0; //角度控制量加載至角度
- if(YM > 35) ERRORX_Out += Ax, ERRORX_Out += Ax, ERRORX_Out += Ax; //外環積分(油門小于某個值時不積分)
- else ERRORX_Out = 0; //油門小于定值時清除積分值
- if(ERRORX_Out > 1500) ERRORX_Out = 1500;
- else if(ERRORX_Out < -1500) ERRORX_Out = -1500; //積分限幅
- Out_PID_X = Ax*Out_XP + ERRORX_Out*Out_XI + (Ax-Last_Ax)*Out_XD; //外環PI
- Last_Ax = Ax;
-
- if(YM > 35) ERRORX_In += (Angle_gy - Out_PID_X); //內環積分(油門小于某個值時不積分)
- else ERRORX_In = 0; //油門小于定值時清除積分值
- if(ERRORX_In > 500) ERRORX_In = 500;
- else if(ERRORX_In < -500) ERRORX_In = -500; //積分限幅
- PID_x = (Angle_gy + Out_PID_X) * In_XP + ERRORX_In * In_XI + (Angle_gy - Last_Angle_gy) * In_XD; //內環PID
- if(PID_x > 500) PID_x = 500; //輸出量限幅
- if(PID_x < -500) PID_x = -500;
- //**************Y軸指向**************************************************
- Ay = AngleY - a_y + ((float)FRY - 128) / 4.0; //角度控制量加載至角度
-
- if(YM > 35) ERRORY_Out += Ay, ERRORY_Out += Ay, ERRORY_Out += Ay; //外環積分(油門小于某個值時不積分)
- else ERRORY_Out = 0; //油門小于定值時清除積分值
- if(ERRORY_Out > 1500) ERRORY_Out = 1500;
- else if(ERRORY_Out < -1500) ERRORY_Out = -1500; //積分限幅
-
- Out_PID_Y = Ay * Out_YP + ERRORY_Out * Out_YI + (Ay-Last_Ay)*Out_YD; //外環PID
- Last_Ay = Ay;
- if(YM > 35) ERRORY_In += (Angle_gx - Out_PID_Y); //內環積分(油門小于某個值時不積分)
- else ERRORY_In = 0; //油門小于定值時清除積分值
- if(ERRORY_In > 500) ERRORY_In = 500;
- else if(ERRORY_In < -500) ERRORY_In = -500; //積分限幅
-
- PID_y = (Angle_gx + Out_PID_Y) * In_YP + ERRORY_In * In_YI + (Angle_gx - Last_Angle_gx) * In_YD; //內環PID
-
- if(PID_y > 500) PID_y = 500; //輸出量限幅
- if(PID_y <-500) PID_y = -500;
- //**************Z軸指向(Z軸隨便啦，自旋控制沒必要上串級PID)*****************************
- Az = Angle_gz - ((float)FRZ - 128);
-
- if(YM > 35) Z_integral += Az; //Z軸積分
- else Z_integral = 0; //油門小于40積分清零
- if(Z_integral > 500.0f) Z_integral = 500.0f; //積分限幅
- else if(Z_integral < -500.0f) Z_integral = -500.0f; //積分限幅
- PID_z = Az * ZP + Z_integral * ZI + (Az - Last_Az) * ZD;
- Last_Az = Az;
- if(PID_z > 200) PID_z = 200; //輸出量限幅
- if(PID_z < -200) PID_z = -200;
- speed0 = (int)( PID_x + PID_y + PID_z); //M1改為逆時針
- speed1 = (int)( PID_x - PID_y - PID_z);
- speed2 = (int)( -PID_x - PID_y + PID_z);
- speed3 = (int)( -PID_x + PID_y - PID_z);
- //**************將速度參數加載至PWM模塊*************************************************
-
- if(YM < 10) PWM0 = 1000, PWM1 = 1000, PWM2 = 1000, PWM3 = 1000;
- else if(YM < 35) PWM0 = 860, PWM1 = 860, PWM2 = 860, PWM3 = 860;
- else
- {
- int_tmp = 1000 - (int)YM * 4;
- PWM0 = int_tmp - speed0;
- if(PWM0 > 1000) PWM0 = 1000; //速度參數控制，防止超過PWM參數范圍0-1000
- else if(PWM0 < 10) PWM0 = 10;
- PWM1 = int_tmp - speed1;
- if(PWM1 > 1000) PWM1 = 1000;
- else if(PWM1 < 10) PWM1 = 10;
- PWM2 = int_tmp - speed2;
- if(PWM2 > 1000) PWM2 = 1000;
- else if(PWM2 < 10) PWM2 = 10;
- PWM3 = int_tmp - speed3;
- if(PWM3 > 1000) PWM3 = 1000;
- else if(PWM3 < 10) PWM3 = 10;
- }
- SW2_tmp = P_SW2; //保存SW2設置
- EAXSFR(); //訪問XFR
- PWM0T2 = (u16)(PWM0 * 2);
- PWM1T2 = (u16)(PWM1 * 2);
- PWM2T2 = (u16)(PWM2 * 2);
- PWM3T2 = (u16)(PWM3 * 2);
- P_SW2 = SW2_tmp; //恢復SW2設置
- }
- /********************** 蜂鳴函數 ************************/
- void beep(void) //100ms調用
- {
- if(BuzzerRepeat > 0) //蜂鳴器處理, 重復次數不為0，則蜂鳴器要發聲
- {
- if((BuzzerOnCnt == 0) && (BuzzerOffCnt == 0)) //On和OFF都為0，則開始裝載On和Off的時間
- {
- P_BUZZER = 1; //允許蜂鳴
- BuzzerOnCnt = BuzzerOnTime; //裝載on計數
- BuzzerOffCnt = BuzzerOffTime; //裝載off計數
- }
- else if(BuzzerOnCnt > 0) {if(--BuzzerOnCnt == 0) P_BUZZER = 0;} //On的時間
- else if(BuzzerOffCnt > 0) //Off的時間
- {
- if(--BuzzerOffCnt == 0) BuzzerRepeat--;
- }
- }
- else P_BUZZER = 0;
- }
- void SetBuzzer(u8 on,u8 off,u8 rep) // rep: 重復次數, on: on的時間, off: off的時間
- {
- BuzzerRepeat = rep;
- BuzzerOnTime = on;
- BuzzerOffTime = off;
- if(BuzzerOnTime == 0) BuzzerOnTime = 1;
- if(BuzzerOffTime == 0) BuzzerOffTime = 1;
- if(BuzzerRepeat == 1) BuzzerOffTime = 1;
- BuzzerOnCnt = 0, BuzzerOffCnt = 0;
- }
- // ===================== 自動校準序列 =====================
- void AutoCal(void)
- {
- if(PPM3 < 40) //停止時才允許校準
- {
- if(Cal_Setp == 1) //進入校準序列
- {
- x_sum = 0; y_sum = 0; z_sum = 0;
- Cal_cnt = 0;
- Cal_Setp = 2;
- }
- else if(Cal_Setp == 2) //對陀螺儀累加
- {
- x_sum += ((int *)&tp)[4]; //讀取陀螺儀數據
- y_sum += ((int *)&tp)[5];
- z_sum += ((int *)&tp)[6];
- if(++Cal_cnt >= 64)
- {
- g_x = x_sum / 64;
- g_y = y_sum / 64;
- g_z = z_sum / 64;
- float_x_sum = 0; float_y_sum = 0;
- Cal_cnt = 0;
- Cal_Setp = 3;
- }
- }
- else if(Cal_Setp == 3) //對X Y角度累加
- {
- float_x_sum += AngleX;
- float_y_sum += AngleY;
- if(++Cal_cnt >= 64)
- {
- Cal_cnt = 0;
- Cal_Setp = 0;
- a_x = float_x_sum / 64.0;
- a_y = float_y_sum / 64.0;
- IAP_Gyro();
- SetBuzzer(5,1,1);
- }
- }
- }
- else
- {
- Cal_Setp = 0;
- Cal_cnt = 0;
- }
- }
- // ===================== 主函數 =====================
- void main(void)
- {
- //所有I/O口全設為準雙向，弱上拉模式
- P0M0=0x00; P0M1=0x00;
- P1M0=0x00; P1M1=0x00;
- P2M0=0x00; P2M1=0x00;
- P3M0=0x00; P3M1=0x00;
- P4M0=0x00; P4M1=0x00;
- P5M0=0x00; P5M1=0x00;
- P6M0=0x00; P6M1=0x00;
- P7M0=0x00; P7M1=0x00;
- PPM1 = 128;
- PPM2 = 128;
- PPM3 = 0;
- PPM4 = 128;
- PWMGO();
- P_Light = 0;
- P_BUZZER = 0;
- P5n_push_pull(0x30);
- adc_init(); //啟動A/D
-
- PCA_config();
- delay_ms(100);
- IAPRead(); //讀取陀螺儀靜差
- InitMPU6050(); //初始化MPU-6050
- delay_ms(100);
- PWMCR = 0xc0;//ECBI; //允許PWM計數器歸零中斷
- EA = 1; //允許總中斷
-
- cnt_start = 0;
- while(cnt_start < 25) //等待油門最小 20ms * 25 = 500ms
- {
- if(B_PPM3_OK) //油門
- {
- B_PPM3_OK = 0;
- if(PPM3_Cap <= 1200) cnt_start++;
- }
- delay_ms(1);
- }
- P_Light = 0;
-
- cnt_start = 0;
- SetBuzzer(5,1,1);
-
- //==============================================
- UART1_config(); // 選擇波特率, 2: 使用Timer2做波特率, 其它值: 使用Timer1做波特率.
- PrintString1("STC15W4K系列大四軸飛控程序!\r\n"); //SUART1發送一個字符串
- //==============================================
- B_Start = 0; //上電禁止運行
- while(1)
- {
- if(B_PPM1_OK) //左右(橫滾)
- {
- B_PPM1_OK = 0;
- if(PPM1_Cap < 1120) PPM1_Cap = 1120;
- else if(PPM1_Cap > 1880) PPM1_Cap = 1880;
- PPM2 = (u8)((PPM1_Cap-1116)/3); //轉為0~255, 中間值為128
- }
-
- if(B_PPM2_OK) //前后(俯仰)
- {
- B_PPM2_OK = 0;
- if(PPM2_Cap < 1120) PPM2_Cap = 1120;
- else if(PPM2_Cap > 1880) PPM2_Cap = 1880;
- PPM1 = (u8)((PPM2_Cap-1116)/3); //轉為0~255, 中間值為128
- }
-
- if(B_PPM4_OK) //航向
- {
- B_PPM4_OK = 0;
- if(PPM4_Cap < 1056) PPM4_Cap = 1056;
- if(PPM4_Cap > 1940) PPM4_Cap = 1940;
- if(PPM4_Cap < 1440) PPM4_Cap = PPM4_Cap + 60;
- else if(PPM4_Cap > 1560) PPM4_Cap = PPM4_Cap - 60;
- else PPM4_Cap = 1500;
- PPM4 = (u8)((PPM4_Cap-1116)/3); //轉為0~255, 中間值為128
- }
-
- if(B_PPM3_OK) //油門
- {
- B_PPM3_OK = 0;
- if(PPM3_Cap < 1000) PPM3_Cap = 1000;
- if(PPM3_Cap > 1900) PPM3_Cap = 1900;
- if(B_Start) //正在運行時,
- {
- PPM3 = (u8)((PPM3_Cap-1000)/4); //轉為0~255, 實際8~225
- if(PPM3 < 32) PPM3 = 32;
-
- if((PPM1 < 50) && (PPM2 < 50) && (PPM3_Cap < 1120) && (PPM4 > 200)) //下外八, 禁止
- {
- if(++cnt_start >= 50) //1秒
- {
- cnt_start = 0;
- B_Start = 0;
- SetBuzzer(1,1,2);
- }
- }
- else cnt_start = 0;
- }
- else //禁止運行時, 等待內八開啟
- {
- PPM3 = 0;
- if((PPM1 < 50) && (PPM2 > 200) && (PPM3_Cap < 1120) && (PPM4 < 50)) //下內八, 啟動
- {
- if(++cnt_start >= 50) //1秒
- {
- cnt_start = 0;
- B_Start = 1;
- SetBuzzer(5,1,1);
- }
- }
- else if((PPM1 > 200) && (PPM2 > 200) && (PPM3_Cap > 1850) && (PPM4 < 50)) //上內八, 水平校準
- {
- if(++cnt_start >= 50) //1秒
- {
- cnt_start = 0;
- SetBuzzer(2,1,1);
- Cal_Setp = 1;
- }
- }
- else if((PPM1 > 200) && (PPM2 < 50) && (PPM3_Cap > 1850) && (PPM4 > 200)) //上外八, 取消水平校準
- {
- if(++cnt_start >= 50) //1秒
- {
- cnt_start = 0;
- g_x = 0;
- g_y = 0;
- g_z = 0;
- a_x = 0;
- a_y = 0;
- IAP_Gyro();
- SetBuzzer(1,1,2);
- }
- }
- else cnt_start = 0;
- }
- }
- if(B_8ms) //8ms到
- {
- B_8ms = 0;
-
- if(Cal_Setp != 0) AutoCal(); //是否執行自動校準序列
- AD(); // 讀ADC計算電壓
- if(++cnt_100ms >= 12) cnt_100ms = 0, beep(); //100ms處理一次蜂鳴器
- B = cnt_ms;
- ++cnt_ms;
- B = (B ^ cnt_ms) & cnt_ms;
- if(B2) //64ms
- {
- if(!B_BAT_LOW && (YM_LostCnt < 120)) //電壓足, 信號正常
- {
- if(!B_Start) P_Light = 0; // 空閑時, 則慢閃(每2048ms亮64ms)
- else P_Light = 1; // 啟動后, 燈常亮
- }
- }
- else if(B4) //256ms
- {
- if(B_BAT_LOW || (YM_LostCnt >= 120)) P_Light = ~P_Light; //電壓低, 或無信號, 航燈閃爍 2HZ
- }
- else if(B6) //1024ms
- {
- if(Battery < 1090) B_BAT_LOW = 1; else if(Battery > 1110) B_BAT_LOW = 0; //<10.90V電壓低, >11.10V電壓夠
-
- if(B_BAT_LOW) SetBuzzer(1,1,2); //電壓低
-
- if(B_rtn_ADC0) Return_Message(); //請求返回ADC0數據
- if(!B_BAT_LOW && (YM_LostCnt < 120)) P_Light = 1; //遙控信號正常, 電壓正常時
- }
- else if(B7) //2048ms
- {
- if(!B_BAT_LOW && (YM_LostCnt >= 120)) SetBuzzer(1,1,3); //電壓正常時 遙控信號丟失, 每兩秒短鳴3次,
- }
- }
- if(UART1_cmd != 0)
- {
- if(UART1_cmd == 'a') //PC發送a，飛控返回一些參數
- {
- B_rtn_ADC0 = ~B_rtn_ADC0;
- }
- UART1_cmd = 0;
- }
-
-
- if((TX1_Read != TX1_Write) && (!B_TX1_Busy)) //有數據要發送, 并且發送空閑
- {
- SBUF = TX1_Buffer[TX1_Read];
- B_TX1_Busy = 1;
- if(++TX1_Read >= TX1_LENGTH) TX1_Read = 0;
- }
- }
- }
- //=========================================================
- 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("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);
- }
- void delay_ms(u8 ms)
- {
- u16 i;
- do
- {
- i = MAIN_Fosc / 13000;
- while(--i) ; //13T per loop
- }while(--ms);
- }
- void TX1_int_value(int i)
- {
- if(i < 0) TX1_write2buff('-'), i = 0 - i;
- else TX1_write2buff(' ');
- TX1_write2buff(i / 1000 + '0');
- TX1_write2buff((i % 1000) / 100 + '0');
- TX1_write2buff((i % 100) / 10 + '0');
- TX1_write2buff('.');
- TX1_write2buff(i % 10 + '0');
- TX1_write2buff(' ');
- TX1_write2buff(' ');
- }
- /*************** 裝載串口1發送緩沖 *******************************/
- void TX1_write2buff(u8 dat) //寫入發送緩沖，指針+1
- {
- TX1_Buffer[TX1_Write] = dat;
- if(++TX1_Write >= TX1_LENGTH) TX1_Write = 0;
- }
- //========================================================================
- // 函數: void PrintString1(u8 *puts)
- // 描述: 串口1發送字符串函數。
- // 參數: puts: 字符串指針.
- // 返回: none.
- // 版本: VER1.0
- // 日期: 2014-11-28
- // 備注:
- //========================================================================
- void PrintString1(u8 *puts) //發送一個字符串
- {
- for (; *puts != 0; puts++) TX1_write2buff(*puts); //遇到停止符0結束
- }
- //========================================================================
- // 函數: SetTimer2Baudrate(u16 dat)
- // 描述: 設置Timer2做波特率發生器。
- // 參數: dat: Timer2的重裝值.
- // 返回: none.
- // 版本: VER1.0
- // 日期: 2014-11-28
- // 備注:
- //========================================================================
- void SetTimer2Baudrate(u16 dat) // 選擇波特率, 2: 使用Timer2做波特率, 其它值: 使用Timer1做波特率.
- {
- AUXR &= ~(1<<4); //Timer stop
- AUXR &= ~(1<<3); //Timer2 set As Timer
- AUXR |= (1<<2); //Timer2 set as 1T mode
- TH2 = dat / 256;
- TL2 = dat % 256;
- IE2 &= ~(1<<2); //禁止中斷
- AUXR |= (1<<4); //Timer run enable
- }
- //========================================================================
- // 函數: void UART1_config(u8 brt)
- // 描述: UART1初始化函數。
- // 參數: brt: 選擇波特率, 2: 使用Timer2做波特率, 其它值: 使用Timer1做波特率.
- // 返回: none.
- // 版本: VER1.0
- // 日期: 2014-11-28
- // 備注:
- //========================================================================
- void UART1_config(void)
- {
- /*********** 波特率使用定時器2 *****************/
- AUXR |= 0x01; //S1 BRT Use Timer2;
- SetTimer2Baudrate(65536UL - (MAIN_Fosc / 4) / Baudrate1);
- /*********** 波特率使用定時器1 *****************/
- /* TR1 = 0;
- AUXR &= ~0x01; //S1 BRT Use Timer1;
- AUXR |= (1<<6); //Timer1 set as 1T mode
- TMOD &= ~(1<<6); //Timer1 set As Timer
- TMOD &= ~0x30; //Timer1_16bitAutoReload;
- TH1 = (u8)((65536UL - (MAIN_Fosc / 4) / Baudrate1) / 256);
- TL1 = (u8)((65536UL - (MAIN_Fosc / 4) / Baudrate1) % 256);
- ET1 = 0; //禁止中斷
- INT_CLKO &= ~0x02; //不輸出時鐘
- TR1 = 1;
- */ //========================================================================
- SCON = (SCON & 0x3f) | 0x40; //UART1模式, 0x00: 同步移位輸出, 0x40: 8位數據,可變波特率, 0x80: 9位數據,固定波特率, 0xc0: 9位數據,可變波特率
- PS = 1; //高優先級中斷
- ES = 1; //允許中斷
- REN = 1; //允許接收
- P_SW1 &= 0x3f;
- P_SW1 |= 0x00; //UART1 switch to, 0x00: P3.0 P3.1, 0x40: P3.6 P3.7, 0x80: P1.6 P1.7 (必須使用內部時鐘)
- // PCON2 |= (1<<4); //內部短路RXD與TXD, 做中繼, ENABLE,DISABLE
- B_TX1_Busy = 0;
- TX1_Read = 0;
- TX1_Write = 0;
- UART1_cmd = 0;
- TX1_cnt = 0;
- }
- //========================================================================
- // 函數: void UART1_int (void) interrupt UART1_VECTOR
- // 描述: UART1中斷函數。
- // 參數: nine.
- // 返回: none.
- // 版本: VER1.0
- // 日期: 2014-11-28
- // 備注:
- //========================================================================
- void UART1_int (void) interrupt 4
- {
- if(RI)
- {
- RI = 0;
- UART1_cmd = SBUF;
- }
- if(TI)
- {
- TI = 0;
- B_TX1_Busy = 0;
- }
- }
- void PCA_config(void)
- {
- PPM1_Rise_TimeOut = 0;
- PPM2_Rise_TimeOut = 0;
- PPM3_Rise_TimeOut = 0;
- PPM4_Rise_TimeOut = 0;
- CR = 0;
- CH = 0;
- CL = 0;
- AUXR1 = (AUXR1 & ~(3<<4)) | PCA_P12_P17_P16_P15_P14; //切換IO口
- CMOD = (CMOD & ~(7<<1)) | PCA_Clock_12T; //選擇時鐘源 STC8F8K D版本
- // CMOD = (CMOD & ~1) | 1; //ECF
- PPCA = 1; //高優先級中斷
- CCAPM0 = PCA_Mode_Capture | PCA_Rise_Active | PCA_Fall_Active | ENABLE; //工作模式, 中斷模式
- PCA_PWM0 = PCA_PWM_8bit; //PWM寬度
- // CCAP0L = (u8)CCAP0_tmp; //將影射寄存器寫入捕獲寄存器，先寫CCAPnL
- // CCAP0H = (u8)(CCAP0_tmp >> 8); //后寫CCAPnH
- CCAPM1 = PCA_Mode_Capture | PCA_Rise_Active | PCA_Fall_Active | ENABLE; //工作模式, 中斷模式
- PCA_PWM1 = PCA_PWM_8bit; //PWM寬度
- // CCAP1L = (u8)CCAP1_tmp; //將影射寄存器寫入捕獲寄存器，先寫CCAPnL
- // CCAP1H = (u8)(CCAP1_tmp >> 8); //后寫CCAPnH
- CCAPM2 = PCA_Mode_Capture | PCA_Rise_Active | PCA_Fall_Active | ENABLE; //工作模式, 中斷模式
- PCA_PWM2 = PCA_PWM_8bit; //PWM寬度
- // CCAP2L = (u8)CCAP2_tmp; //將影射寄存器寫入捕獲寄存器，先寫CCAPnL
- // CCAP2H = (u8)(CCAP2_tmp >> 8); //后寫CCAPnH
- CCAPM3 = PCA_Mode_Capture | PCA_Rise_Active | PCA_Fall_Active | ENABLE; //工作模式, 中斷模式
- PCA_PWM3 = PCA_PWM_8bit; //PWM寬度
- // CCAP3L = (u8)CCAP3_tmp; //將影射寄存器寫入捕獲寄存器，先寫CCAPnL
- // CCAP3H = (u8)(CCAP3_tmp >> 8); //后寫CCAPnH
- CR = 1;
- }
- //========================================================================
- // 函數: void PCA_Handler (void) interrupt PCA_VECTOR
- // 描述: PCA中斷處理程序.
- // 參數: None
- // 返回: none.
- // 版本: V1.0, 2012-11-22
- //========================================================================
- void PCA_Handler (void) interrupt PCA_VECTOR
- {
- if(CCF0) //PCA模塊0中斷
- {
- CCF0 = 0; //清PCA模塊0中斷標志
- if(P17) //上升沿
- {
- CCAP0_RiseTime = ((u16)CCAP0H << 8) + CCAP0L; //讀CCAP0
- PPM1_Rise_TimeOut = 1; //收到上升沿, 高電平限時
- }
- else //下降沿
- {
- CCAP_FallTime = ((u16)CCAP0H << 8) + CCAP0L; //讀CCAP0
- if((PPM1_Rise_TimeOut != 0) && (PPM1_Rise_TimeOut < 3)) //收到過上升沿, 高電平也沒有溢出
- {
- CCAP_FallTime = (CCAP_FallTime - CCAP0_RiseTime) >> 1; //為了好處理, 轉成單位為us
- if((CCAP_FallTime >= 800) && (CCAP_FallTime <= 2500))
- {
- if(++PPM1_RxCnt >= 5) PPM1_RxCnt = 5; //連續接收到5個脈沖
- if(PPM1_RxCnt == 5)
- {
- if(!B_PPM1_OK)
- {
- PPM1_Cap = CCAP_FallTime;
- B_PPM1_OK = 1; //標志收到一個脈沖
- PPM1_Rx_TimerOut = 12; //限時收不到脈沖
- }
- }
- }
- ……………………
- …………限于本文篇幅 余下代碼請從51黑下載附件…………
捕獲1.PNG (72.99 KB, 下載次數: 148)
下載附件
四軸飛控-STC8A8K64S4A12-LQFP44-PPM-V10
2017-11-26 11:17 上傳
所有資料51hei提供下載:
四軸飛控-STC8A8K64S4A12-LQFP44-PPM-V10.rar
(3.86 MB, 下載次數: 443)
2017-11-26 11:17 上傳
點擊文件名下載附件
下載積分: 黑幣 -5
|
|
[復制鏈接]
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
來自 33#
