四轴飞行器：姿态解算采用四元数，PID采用串级方法.zip

#include <STC15W4K60S4.H> #include <STC15W4KPWM.H> #include <NRF24L01.H> #include <Timer.h> extern unsigned char RxBuf[20]; void PWMGO() { int i=1; //所有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; //设置需要使用的PWM输出口为强推挽模式 P2M0=0x0e; P2M1=0x00; P3M0=0x80; P3M1=0x00; //使用定时器2作为时钟源 Time2_Init(); P_SW2=0x80; //最高位置1才能访问和PWM相关的特殊寄存器 PWMCFG=0xb0; //7位 6位 5位 4位 3位 2位 1位 0位 //置0 1-计数器归零触发ADC C7INI C6INI C5INI C4INI C3INI C2INI // 0-归零时不触发ADC (值为1时上电高电平，为0低电平） PWMCKS=0x10; //7位6位5位 4位 3位 2位 1位 0位 // 置0 0-系统时钟分频 分频参数设定 // 1-定时器2溢出 时钟=系统时钟/([3:0]+1) PWMIF=0x00; //7位 6位 5位 4位 3位 2位 1位 0位 //置0 计数器归零中断标志 相应PWM端口中断标志 PWMFDCR=0x00; //7位 6位 5位 4位 //置0 置0 外部异常检测开关 外部异常时0-无反应 1-高阻状态 //3位 2位 1位 0位 //PWM异常中断 比较器与异常的关系 P2.4与异常的关系 PWM异常标志 PWMCH=0x03; //15位寄存器，决定PWM周期，数值为1-32767，单位：脉冲时钟 PWMCL=0xe9; // 以下为每个PWM输出口单独设置 PWM2CR=0x00; //7位6位5位4位 3位 2位 1位 0位 // 置0 输出切换 中断开关 T2中断开关 T1中断开关 PWM3CR=0x00; PWM4CR=0x00; PWM5CR=0x00; PWM2T1H=0x03; //15位寄存器第一次翻转计数 第一次翻转是指从低电平翻转到高电平的计时 PWM2T1L=0xe8; PWM2T2H=0x03; //15位寄存器第二次翻转计数 第二次翻转是指从高电平翻转到低电平的计时 PWM2T2L=0xe9; //第二次翻转应比精度等级要高，否则会工作不正常，比如精度1000，第二次翻转就必须小于1000 PWM3T1H=0x03; PWM3T1L=0xe8; PWM3T2H=0x03; PWM3T2L=0xe9; PWM4T1H=0x03; PWM4T1L=0xe8; PWM4T2H=0x03; PWM4T2L=0xe9; PWM5T1H=0x03; PWM5T1L=0xe8; PWM5T2H=0x03; PWM5T2L=0xe9; //以上为每个PWM输出口单独设置 PWMCR=0x8f; //7位 6位 5位 4位 3位 2位 1位 0位 10001111 //PWM开关 计数归零中断开关 相应I/O为GPIO模式(0)或PWM模式(1) //*********************************以下为装逼模拟电调的声音****************************************************** PWM(960,960,960,960); Delay(60000);Delay(30000); T2L = 0xE8; T2H = 0xFF; Delay(60000);Delay(30000); T2L = 0xE5; T2H = 0xFF; Delay(60000);Delay(30000); PWM(1000,1000,1000,1000); Delay(60000);Delay(60000);Delay(60000);Delay(60000); T2L = 0xE5; T2H = 0xFF; PWM(960,960,960,960); Delay(60000);Delay(30000); PWM(1000,1000,1000,1000); Delay(60000); PWM(960,960,960,960); Delay(60000);Delay(30000); PWM(1000,1000,1000,1000); Delay(60000); PWM(960,960,960,960); Delay(60000);Delay(30000); PWM(1000,1000,1000,1000); init_NRF24L01();//初始化无线模块 SetRX_Mode(); while(i) { if(RxBuf[0]==0) { Delay(60000);Delay(60000);Delay(60000);Delay(60000);Delay(60000);Delay(60000); Delay(60000);Delay(60000);Delay(60000);Delay(60000);Delay(60000);Delay(60000); playmusic(4); } if(RxBuf[0]==0) { Delay(60000);Delay(60000);Delay(60000);Delay(60000);Delay(60000);Delay(60000); Delay(60000);Delay(60000);Delay(60000);Delay(60000);Delay(60000);Delay(60000); playmusic(2); } if(RxBuf[0]==0) { Delay(60000);Delay(60000);Delay(60000);Delay(60000);Delay(60000);Delay(60000); Delay(60000);Delay(60000);Delay(60000);Delay(60000);Delay(60000);Delay(60000); playmusic(1); } if(RxBuf[0]==0) { Delay(60000);Delay(60000);Delay(60000);Delay(60000);Delay(60000);Delay(60000); Delay(60000);Delay(60000);Delay(60000);Delay(60000);Delay(60000);Delay(60000); playmusic(3); } nRF24L01_RxPacket(RxBuf); if(RxBuf[0]>0){i=0;} } T2L = 0xE5; PWM(960,960,960,960); Delay(60000);Delay(60000);Delay(60000);Delay(60000);Delay(60000);Delay(60000); PWM(1000,1000,1000,1000); //**************************************以上为装逼模拟电调的声音************************************************** PWMCKS=0x00; } //本函数输入的4个值取值范围为0-1000 1000电机停，0转速最高，输入数据不能超过取值范围，否则会大姨妈 void PWM(unsigned int PWMa,unsigned int PWMb,unsigned int PWMc,unsigned int PWMd) { PWM2T1H=PWMa>>8; //15位寄存器第一次翻转计数 第一次翻转是指从低电平翻转到高电平的计时 PWM2T1L=PWMa; PWM3T1H=PWMb>>8; PWM3T1L=PWMb; PWM4T1H=PWMc>>8; PWM4T1L=PWMc; PWM5T1H=PWMd>>8; PWM5T1L=PWMd; } // 高音 中音 //1 T2L = 0xE5, T2L = 0xCB; //2 T2L = 0xE8, T2L = 0xD0; //3 T2L = 0xEB, T2L = 0xD6; //4 T2L = 0xEC, T2L = 0xD8; //5 T2L = 0xEE, T2L = 0xDC; //6 T2L = 0xF0, T2L = 0xE0; //7 T2L = 0xF2, T2L = 0xE4; unsigned char code high[]={0x00,0xE5,0xE8,0xEB,0xEC,0xEE,0xF0,0xF2}; //默认为高音 unsigned char code mid[]={0x00,0xCB,0xD0,0xD6,0xD8,0xDC,0xE0,0xE4}; //需播放中音对音符加10即可，比如1为哆高音，11就为哆中音 //1155665 4433221 5544332 5544332 1155665 4433221 小星星 unsigned char code music1[]={1,1,5,5,6,6,5,0,4,4,3,3,2,2,1,0, 5,5,4,4,3,3,2,0,5,5,4,4,3,3,2,0, 1,1,5,5,6,6,5,0,4,4,3,3,2,2,1,0,0xff}; //5175565 5175636 6665334 44456342 5175565 5175636 66653334 44431121 征服 unsigned char code music2[]={15,1,17,15,15,16,15,0,15,1,17,15,16,13,16,0, 16,16,16,15,13,13,14,0,14,14,14,15,16,13,14,12,0, //每首歌曲必须以0xFF结尾 15,1,17,15,15,16,15,0,15,1,17,15,16,13,16,0, //中间如需停顿，以0代替，0就是停顿一个音符 16,16,16,15,13,13,13,14,0,14,14,14,13,11,11,12,11,0,0xff}; //5154321 11233135 5154352 4326521 53617653 2221765 53617672 5671251 我爱北京天安门 unsigned char code music3[]={15,1,15,14,13,12,11,0,11,11,12,13,13,11,13,15,0, 15,1,15,14,13,15,12,0,14,13,12,16,15,12,11,0, 15,13,16,1,17,16,15,0,2,2,2,1,17,16,15,0, 15,13,16,1,17,17,2,0,15,16,17,1,2,15,1,0,0xff}; //情歌王 unsigned char code music4[]={13,0,15,4,3,2,2,3,2,1,1,16,1,0,0,15,16,1,2,0,3,0,15,4,3,2,0,3,0,2,1,1,16,1,0,3,4,5,1,17,1,0,1,2,1,5,1,17,1,0,1,1,1,7,7,7,6,6,6,6,6,5,3,4,3,4,4,5,5,15,15,14,13,12,5,0,5,5,5,4,3,2,1,1,2,3,0,2,1,17,16,2,1,3,15,5,5,5,4,3,4,3,1,2,15,5,4,3,3,4,3,3,4,3,4,3,2,1,0,1,3,5,6,6,6,5,2,2,4,3,0,0,1,3,5,6,6,6,5,2,2,4,3,4,3,2,1,1,1,2,3,1,16,2,7,7,7,1,1,15,16,11,1,2,3,3,5,2,3,2,2,5,2,1,15,16,17,1,0,0,15,5,3,2,1,0,15,5,3,2,3,2,2,1,0,1,1,7,7,16,5,6,5,3,5,0,1,1,7,7,6,0,5,5,1,3,2,1,3,4,3,1,16,1,6,15,0,5,0,3,3,5,16,0,2,2,4,13,0,1,1,17,1,6,3,2,1,2,0,5,0,0,3,5,5,5,5,5,3,5,0,3,5,5,5,5,3,2,1,0,3,5,5, 6,1,1,0,5,5,5,6,1,1,0,5,5,5,6,1,1,1,1,1,7,7,7,0,3,0,3,0,3,2,1,1,1,1,1,2,2,1,1,0,1,2,1,3,5,3,6,5,3,2,1,2,2,3,1,1,0,17,11,12,13,13,13,13,14,13,12,11,12,17,11,12,12,12,13,13,12,11,17,11,16,17,11,16,11,11,11,11,11,11,12,12,11,15,0,0,1,2,4,5,3,3,2,1,2,2,2,2,3,1,2,2,1,0,15,16,1,3,3,3,2,1,2,2,2,3,2,1,0,0,15,3,3,3,2,2,0,16,17,16,16,0,2,0,16,3,3,3,2,1,2,2,2,3,2,1,0,16,16,17,1,17,1,3,3,4,17,16,15,2,2,3,16,15,16,16,17,1,1,2,2,2,1,17,0,0,4,4,4,3,4,3,2,2,2,2,3,3,3,2,1,1,1,2,2,2,2,1,17,0,17,17,17,17,1,17,6,0,3,2,0,3,1,0,1,17,15,16,0,1,2,30,1,3,0,0,3,3,3,3,3,3,4,3,2,1,16,16,4,4,3,1,2,1,0,15,1,15,1,2,3,15,15,15,4,3,3,2,2,2,16,3,2,1,1,1,16,2,1,1,0,15,1,15,1,2,3,15,15,15,4,3,3,2,2,2,16,3,1,1,1,1,16,2,1,16,1,17,0,0,15,16,1,1,1,1,1,16,1,0,2,2,1,1,1,16,15,15,15,15,16,1,1