宏晶开源基于stc8a8k的四轴

/*------------------------------------------------------------------*/ /* --- STC MCU Limited ---------------------------------------------*/ /* --- STC89-90xx Series MCU UART (8-bit/9-bit)Demo ----------------*/ /* --- Mobile: (86)13922805190 -------------------------------------*/ /* --- Fax: 86-0513-55012956,55012947,55012969 ---------------------*/ /* --- Tel: 86-0513-55012928,55012929,55012966----------------------*/ /* --- Web: www.STCMCU.com -----------------------------------------*/ /* --- Web: www.GXWMCU.com -----------------------------------------*/ /* If you want to use the program or the program referenced in the */ /* article, please specify in which data and procedures from 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; e