四轴飞行器的设计之四轴主板的综合程序——STC15单片机实战指南连载
时间:10-02
整理:3721RD
点击:
前面比较全面的讲述了四轴飞行器的硬件设计和软件中PID算法以及四元数与滤波算法,这节放出四轴主板的程序代码。
例程比较复杂,这里采用了模块化编程的思路,按功能将其各个函数进行分类。笔者刚开始计划这部分的例程不贴到书上,但怕读者又说读者开源不够,与此,将完整的一套四轴源码贴到这里,做到真正的开源,起到让读者学习的目的。但读者需要注意的,该例程不像前面的小实例那样简单,读者再学习的时候,要静下心来,边读例程,边理解,同时一定要做记录,达到真正的理解。再者,便于读者调试,这里保留了笔者调试时所用的串口示波器源码,这样,可大大降低读者调试的难度,同时,所有“.c”文件对应的头文件“.h”文件,这里全部省略,里面主要是对变量和函数的声明,学到这里,对于函数的声明等,应该能熟练于心了。接下来,我们开始上菜,读者慢慢“享用”。
例程比较复杂,这里采用了模块化编程的思路,按功能将其各个函数进行分类。笔者刚开始计划这部分的例程不贴到书上,但怕读者又说读者开源不够,与此,将完整的一套四轴源码贴到这里,做到真正的开源,起到让读者学习的目的。但读者需要注意的,该例程不像前面的小实例那样简单,读者再学习的时候,要静下心来,边读例程,边理解,同时一定要做记录,达到真正的理解。再者,便于读者调试,这里保留了笔者调试时所用的串口示波器源码,这样,可大大降低读者调试的难度,同时,所有“.c”文件对应的头文件“.h”文件,这里全部省略,里面主要是对变量和函数的声明,学到这里,对于函数的声明等,应该能熟练于心了。接下来,我们开始上菜,读者慢慢“享用”。
- <font face="黑体">/* ============= 1 STC四轴正式版程序.c */
- #include "includes.h"
- #include "main.h"
- </font> <font face="黑体">
- #define Check2401 1 //2401与mcu通信检测 置0屏蔽置1开启
- #define ReseveData 1 //飞行器接收数据控
- #define OutData 1 //飞行器内部数据输出
- void main()
- {
- FlyAllInit(); //飞行器各模块初始化
- #if Check2401
- Check24L01(); //检测2401是否正常 不正常的话串口输出error
- #endif
- while(1)
- {
- #if ReseveData
- Reseve2401(); //接收2401数据
- #endif
-
- #if OutData
- OutPutData(); //输出飞机内部调试数据
- #endif
- }
- }
- /* ============= 2 filtering.c */
- #include <STC15W4K60S4.H>
- #include <intrins.h>
- #include <NRF24L01.H>
- #include <MPU6050.H>
- #include <math.h>
- #include <STC15W4KPWM.H>
- #include <Timer.h>
- #include <EEPROM.h>
- #include <USART.h>
- #include <IMU.H>
- #include "outputdata.h"
- #include "stdio.h"
- #include "filtering.h"
- /*-- 3-axis accelerometer data collected from MPU --*/
- short Z_angle,Y_angle,X_angle;
- /*-- Y-axis gyroscope data collected from MPU --*/
- short Y_gyro,X_gyro,Z_gyro;
- /*- The final angular data filtered by Kalman Filter --*/
- short X_gyroInit, Y_gyroInit, Z_gyroInit;
- /*------- Data collection -------*/
- void Read_MPU()
- {
-
- Z_angle = GetData(ACCEL_ZOUT_H);;
- //Z_angle = (Z_angle/8192)*9.807; //Z-axis accelerometer
-
- Y_angle = GetData(ACCEL_YOUT_H);
- //Y_angle = (Y_angle/8192)*9.807; //Y-axis accelerometer
-
- X_angle = GetData(ACCEL_XOUT_H);
- //X_angle = (X_angle/8192)*9.807;
- X_gyro=xgy();
- Y_gyro=ygy();
- Z_gyro=zgy();
- </font> <font face="黑体">
- }
- static short xgy(void)
- {
-
- short i,j,k;
- short tmp;
-
- X_gyro = GetData(GYRO_XOUT_H);;
- X_gyro/= 16.4; //X-axis accelerometer
- i = X_gyro;
- </font> <font face="黑体">
- X_gyro = GetData(GYRO_XOUT_H);;
- X_gyro/= 16.4; //X-axis accelerometer
- j = X_gyro;
- </font> <font face="黑体">
- X_gyro = GetData(GYRO_XOUT_H);
- X_gyro/= 16.4; //X-axis accelerometer
- k = X_gyro;
- if (i > j)
- {
- tmp = i; i = j; j = tmp;
- }
- if (k > j)
- tmp = j;
- else if(k > i)
- tmp = k;
- else
- tmp = i;
- return tmp;
- }
- short ygy(void)
- {
- </font> <font face="黑体">
- short i,j,k;
- short tmp;
-
- Y_gyro = GetData(GYRO_YOUT_H);
- Y_gyro/= 16.4; //Y-axis gyroscope
- i = Y_gyro;
- </font> <font face="黑体">
- Y_gyro = GetData(GYRO_YOUT_H);
- Y_gyro/= 16.4; //Y-axis gyroscope
- j = Y_gyro;
- </font> <font face="黑体">
- Y_gyro = GetData(GYRO_YOUT_H);;
- Y_gyro/= 16.4; //Y-axis gyroscope
- k = Y_gyro;
- if (i > j)
- {
- tmp = i; i = j; j = tmp;
- }
- if (k > j)
- tmp = j;
- else if(k > i)
- tmp = k;
- else
- tmp = i;
- return tmp;
- }
- short zgy(void)
- {
- </font> <font face="黑体">
- short i,j,k;
- short tmp;
- </font> <font face="黑体">
- Z_gyro = GetData(GYRO_ZOUT_H);;
- Z_gyro/= 16.4;
- i = Z_gyro;
- </font> <font face="黑体">
- Z_gyro = GetData(GYRO_ZOUT_H);
- Z_gyro/= 16.4;
- j = Z_gyro;
- </font> <font face="黑体">
- Z_gyro = GetData(GYRO_ZOUT_H);
- Z_gyro/= 16.4;
- k = Z_gyro;
- if (i > j)
- {
- tmp = i; i = j; j = tmp;
- }
- if (k > j)
- tmp = j;
- else if(k > i)
- tmp = k;
- else
- tmp = i;
- return tmp;
- }
- void Delay1ms() //@30.000MHz
- {
- unsigned char i, j;
- i = 30;
- j = 43;
- do
- {
- while (--j);
- } while (--i);
- }
- void gyro_init(void)
- {
- int i=0;
- int x,y,z;
- short a,b,c;
- for(i=0;i<100;i++)
- {
- a=xgy() ;
- b=ygy() ;
- c=zgy() ;
- x=x+a;
- y=y+b;
- z=z+c;
- Delay1ms();
- }
- X_gyroInit=x/100;
- Y_gyroInit=y/100;
- Z_gyroInit=z/100;
- }
- /* ============= 3 control.c */
- #include <STC15W4K60S4.H>
- #include <intrins.h>
- #include <NRF24L01.H>
- #include <MPU6050.H>
- #include <math.h>
- #include <STC15W4KPWM.H>
- #include <Timer.h>
- #include <EEPROM.h>
- #include <USART.h>
- #include <IMU.H>
- #include "outputdata.h"
- #include "stdio.h"
- #include "filtering.h"
- #include "control.h"
- #include "includes.h"
- extern unsigned char RXBUF[8];
- extern float Pitch; //x
- extern float Roll; //y
- extern float Average_Gx,Average_Gy,Average_Gz ;
- extern int key;
- unsigned int Controldata_THROTTLE_Set=0;//1880-50-50-100+30;
- //************************************************
- int Controldata_THROTTLE=0 ; //?
- int Controldata_PITCH =0 ; //?
- int Controldata_ROLL =0 ; //?
- int Controldata_YAW =0 ; //?
- int Controldata_OFFSET =0 ; //?
- unsigned int ControlNum=0 ;
- //************************************************
- float IUX=0.0,IUY=0.0;
- float COUT_PITCHZ,COUT_PITCHF,COUT_ROLLZ,COUT_ROLLF;
- float PWM_XZ,PWM_XF,PWM_YZ,PWM_YF;//?pwm?
- float Yaw1=0;
- //*************************************************
- void MainControl()
- {
- Get_Control_Data() ;
- Control();
- }
- //=============================================================
- float Controldata_ROLLleft=0,Controldata_ROLLright=0;
- float Controldata_PITCHfront=0, Controldata_PITCHback=0;
- int error=0;
- extern int keyk;
- extern unsigned int s;
- int ss=0;
- void Get_Control_Data()
- {
- float a=0,b=530,c=521,d=527;
- if(keyk==1)
- {
- d = RXBUF[0]*16+RXBUF[1]; //oá1?
- b = RXBUF[2]*16+RXBUF[3]; //?
- c = RXBUF[4]*16+RXBUF[5]; //?o?
- a = RXBUF[6]*16+RXBUF[7]; //óí?
- //*********************脱控保护******************************
- if(error==1)
- {
- if(ss==0)
- {ss=s;}
- a=a-(s-ss)*20;
- }
- else
- ss=0;
- }
- //***********************************************************
- if(a>1024)
- {a=1024;}
- a= a/1024.0 ;
- if((b>=530&b<=540)||(b<=530&b>=520))
- {b=530;}
- b=b-530;
- if(b>0)
- {b=b/494.0;}
- else
- {b=b/530.0;}
- if((c>=521&c<=531)||(c<=521&c>=511))
- {c=521.0;}
- c=c-521;
- if(c>0)
- {c=c/523.0;}
- else
- {c=c/521.0;}
- if((d>=517&d<=527)||(d<=537&d>=527))
- {d=527;}
- d=d-527;
- if(d>0)
- {d=d/507.0;}
- else
- {d=d/527.0;}
-
- if(b>1)
- b=1;
- else if(b<-1)
- b=-1;
- //-------------
- if(c>1)
- c=1;
- else if(c<-1)
- c=-1;
- //-------------
- if(d>1)
- d=1;
- else if(d<-1)
- d=-1;
- /*****************************/
- Controldata_THROTTLE=a*2400;
- Controldata_PITCH= b*20;
- Controldata_YAW = c*180;
- Controldata_ROLL = d*20;
-
- if( Controldata_PITCH>0)
- {Controldata_PITCHfront=0 ,Controldata_PITCHback= Controldata_PITCH;}
- else
- {Controldata_PITCHfront=Controldata_PITCH ,Controldata_PITCHb ack= 0;}
-
- if(Controldata_ROLL>0)
- {
- Controldata_ROLLleft=0;
- Controldata_ROLLright=Controldata_ROLL;
- }
- else
- {
- Controldata_ROLLleft=Controldata_ROLL;
- Controldata_ROLLright=0;
- }
- }
- extern float IntegralZ;
- #define MINPWM 0
- #define MAXPWM 2699
- void Control(void)
- {
- float EX0=0.0,EX1=0.0,EY0=0.0,EY1=0.0;
- //-----------------------------
- float PID_P_Y , PID_D_Y ;//, PID_I_Y;
- float PID_P_X , PID_D_X ;//, PID_I_X;
- float PID_P_Z , PID_D_Z;
- /***********************整定PID***************************/
- EX0=Pitch;
-
- EY0=Roll;
- //********************************************************
- PID_P_Y=3.2;//4;//4.0;//3.1;
- PID_D_Y=1.2;//0.4; 0.45; 0.43; 2.85; 0.6; 0.815+0.1; 0.86;
- PID_P_X=3.2;//3.2;;//4;//3.0;//9.41;//15-1-1-0.4;
- PID_D_X=1.2;//1.2;//1.2;//0.4;//0.4;//0.45;//0.6;//2.85;
- //0.6;//2.515;//1.555+1+0.3+0.2;
- PID_P_Z=0;//5;//0.2;
- PID_D_Z=0;//0.03;
- /**********************************************************/
- Yaw1=PID_P_Z*(IntegralZ-Controldata_YAW)+PID_D_Z* Average_Gz;
- if(Yaw1>300)
- {Yaw1=300;}
- else if(Yaw1<-300)
- {Yaw1=-300;}
- /*********************************************************/
- PWM_XF= (float)(Controldata_THROTTLE)- (EX0-
- Controldata_PITCHfront)*PID_P_X + (Average_Gy)*PID_D_X -
- (EY0-Controldata_ROLLleft)*PID_P_Y-(Average_Gx)*PID_D_Y +Yaw1;
- PWM_YF=(float)(Controldata_THROTTLE)-(EX0-
- Controldata_PITCHfront)*PID_P_X + (Average_Gy)*PID_D_X + (EY0-
- Controldata_ROLLright)*PID_P_Y+(Average_Gx)*PID_D_Y -Yaw1;
- PWM_XZ=(float)(Controldata_THROTTLE)+ (EX0-
- Controldata_PITCHback)*PID_P_X - (Average_Gy)*PID_D_X - (EY0-
- Controldata_ROLLleft)*PID_P_Y-(Average_Gx)*PID_D_Y -Yaw1;
- PWM_YZ=(float)(Controldata_THROTTLE)+ (EX0-
- Controldata_PITCHback)*PID_P_X – (Average_Gy)*PID_D_X + (EY0-
- Controldata_ROLLright)*PID_P_Y+(Average_Gx)*PID_D_Y +Yaw1;
- //**********************************************************
- #if 1 // 方便调试
- if(Controldata_THROTTLE<30)
- {
- PWM_XF=PWM_YF= PWM_XZ= PWM_YZ=0;
- IntegralZ=0;
- PWM(0,0,0,0) ;
- }
- #endif
- if( PWM_XZ<MINPWM)
- PWM_XZ=MINPWM;
- else if( PWM_XZ>MAXPWM)
- PWM_XZ=MAXPWM;
- if( PWM_XF<MINPWM)
- PWM_XF=MINPWM;
- else if( PWM_XF>MAXPWM)
- PWM_XF=MAXPWM;
- if( PWM_YZ<MINPWM)
- PWM_YZ=MINPWM;
- else if( PWM_YZ>MAXPWM)
- PWM_YZ=MAXPWM;
- if( PWM_YF<MINPWM)
- PWM_YF=MINPWM;
- else if( PWM_YF>MAXPWM)
- PWM_YF=MAXPWM;
- /**********************************************/
- PWM(PWM_XZ,PWM_XF,PWM_YF,PWM_YZ) ;
- }
- /* ============= 4 spi.c */
- #include "includes.h"
- #include "spi.h"
- #include <STC15W4K60S4.H>
- typedef bit BOOL;
- typedef unsigned char BYTE;
- typedef unsigned short WORD;
- typedef unsigned long DWORD;
- #define FOSC 11059200L
- #define BAUD (65536 - FOSC / 4 / 115200)
- #define null 0
- #define FALSE 0
- #define TRUE 1
- //sfr AUXR = 0x8e; //辅助寄存器
- //sfr P_SW1 = 0xa2; //外设功能切换寄存器1
- #define SPI_S0 0x04
- #define SPI_S1 0x08
- //sfr SPSTAT = 0xcd; //SPI状态寄存器
- #define SPIF 0x80 //SPSTAT.7
- #define WCOL 0x40 //SPSTAT.6
- //sfr SPCTL = 0xce; //SPI控制寄存器
- #define SSIG 0x80 //SPCTL.7
- #define SPEN 0x40 //SPCTL.6
- #define DORD 0x20 //SPCTL.5
- #define MSTR 0x10 //SPCTL.4
- #define CPOL 0x08 //SPCTL.3
- #define CPHA 0x04 //SPCTL.2
- #define SPDHH 0x00 //CPU_CLK/4
- #define SPDH 0x01 //CPU_CLK/16
- #define SPDL 0x02 //CPU_CLK/64
- #define SPDLL 0x03 //CPU_CLK/128
- //sfr SPDAT = 0xcf; //SPI数据寄存器
- //sbit SS = P2^4; //SPI的SS脚,连接到Flash的CE
- #define SFC_WREN 0x06 //串行Flash命令集
- #define SFC_WRDI 0x04
- #define SFC_RDSR 0x05
- #define SFC_WRSR 0x01
- #define SFC_READ 0x03
- #define SFC_FASTREAD 0x0B
- #define SFC_RDID 0xAB
- #define SFC_PAGEPROG 0x02
- #define SFC_RDCR 0xA1
- #define SFC_WRCR 0xF1
- #define SFC_SECTORER 0xD7
- #define SFC_BLOCKER 0xD8
- #define SFC_CHIPER 0xC7
- void InitSpi();
- BYTE SpiShift(BYTE dat);
- void InitSpi()
- {
- ACC = P_SW1; //切换到第一组SPI
- ACC &= ~(SPI_S0 | SPI_S1); //SPI_S0=0 SPI_S1=0
- P_SW1 = ACC; //(P1.2/SS, P1.3/MOSI, P1.4/MISO, P1.5/SCLK)
- SPSTAT = SPIF | WCOL; //清除SPI状态
- SPCTL = SSIG | SPEN | MSTR; //设置SPI为主模式
- IE2&=0XFC;
- }
- /************************************************
- 使用SPI方式与Flash进行数据交换
- 入口参数:
- dat : 准备写入的数据
- 出口参数:
- 从Flash中读出的数据
- ************************************************/
- BYTE SpiShift(BYTE dat)
- {
- SPDAT = dat; // 触发SPI发送
- while (!(SPSTAT & SPIF)); // 等待SPI数据传输完成
- SPSTAT = SPIF | WCOL; // 清除SPI状态
-
- return SPDAT;
- }
- /* ============= 5 NRF24L01.c */
- #include <STC15W4K60S4.H>
- #include "nrf24l01.h"
- #include "spi.h"
- </font> <font face="黑体">
- const unsigned char TX_ADDRESS[TX_ADR_WIDTH]=
- {0x34,0x43,0x10,0x10,0x01};
- const unsigned char RX_ADDRESS[RX_ADR_WIDTH]=
- {0x34,0x43,0x10,0x10,0x01};
- void NRF24L01_Init(void)
- {
- InitSpi();
- NRF24L01_CE=0;
- NRF24L01_CSN=1;
- }
- unsigned char NRF24L01_Check(void)
- {
- unsigned char buf[5]={0XA5,0XA5,0XA5,0XA5,0XA5};
- unsigned char i;
-
- NRF24L01_Write_Buf(WRITE_REG1+TX_ADDR,buf,5);
- NRF24L01_Read_Buf(TX_ADDR,buf,5);
- for(i=0;i<5;i++) if(buf[i]!=0XA5) break;
- if(i!=5) return 1;
- return 0;
- }
- unsigned char NRF24L01_Write_Reg(unsigned char reg,unsigned char value)
- {
- unsigned char status;
- NRF24L01_CSN=0;
- status =SpiShift(reg);
- SpiShift(value);
- NRF24L01_CSN=1;
- return(status);
- }
- </font> <font face="黑体">
- unsigned char NRF24L01_Read_Reg(unsigned char reg)
- {
- unsigned char reg_val;
- NRF24L01_CSN = 0;
- SpiShift(reg);
- reg_val=SpiShift(0XFF);
- NRF24L01_CSN = 1;
- return(reg_val);
- }
- </font> <font face="黑体">
- unsigned char NRF24L01_Read_Buf(unsigned char reg,unsigned char *pBuf,unsigned char len)
- {
- unsigned char status,u8_ctr;
- NRF24L01_CSN = 0;
- status=SpiShift(reg);
- for(u8_ctr=0;u8_ctr<len;u8_ctr++)pBuf[u8_ctr]=SpiShift(0XFF);
- NRF24L01_CSN=1;
- return status;
- }
- unsigned char NRF24L01_Write_Buf(unsigned char reg, unsigned char *pBuf, unsigned char len)
- {
- unsigned char status,u8_ctr;
- NRF24L01_CSN = 0;
- status = SpiShift(reg);
- for( u8_ctr=0; u8_ctr<len; u8_ctr++)SpiShift(*pBuf++);
- NRF24L01_CSN = 1;
- return status;
- }
- unsigned char NRF24L01_TxPacket(unsigned char *txbuf)
- {
- unsigned char sta;
-
- NRF24L01_CE=0;
- NRF24L01_Write_Buf(WR_TX_PLOAD,txbuf,TX_PLOAD_WIDTH);
- NRF24L01_CE=1;
- while(NRF24L01_IRQ!=0);
- sta=NRF24L01_Read_Reg(STATUS);
- NRF24L01_Write_Reg(WRITE_REG1+STATUS,sta);
- if(sta&MAX_TX)
- {
- NRF24L01_Write_Reg(FLUSH_TX,0xff);
- return MAX_TX;
- }
- if(sta&TX_OK)
- {
- return TX_OK;
- }
- return 0xff;
- }
- unsigned char NRF24L01_RxPacket(unsigned char *rxbuf)
- {
- unsigned char sta;
-
- sta=NRF24L01_Read_Reg(STATUS);
- NRF24L01_Write_Reg(WRITE_REG1+STATUS,sta);
- if(sta&RX_OK)
- {
- NRF24L01_Read_Buf(RD_RX_PLOAD,rxbuf,RX_PLOAD_WIDTH);
- NRF24L01_Write_Reg(FLUSH_RX,0xff);
- return 0;
- }
- return 1;
- }
-
- void RX_Mode(void)
- {
- NRF24L01_CE=0;
- NRF24L01_Write_Buf(WRITE_REG1+RX_ADDR_P0,(unsigned char*)RX_ADDRESS,RX_ADR_WIDTH);
-
- NRF24L01_Write_Reg(WRITE_REG1+EN_AA,0x01);
- NRF24L01_Write_Reg(WRITE_REG1+EN_RXADDR,0x01);
- NRF24L01_Write_Reg(WRITE_REG1+RF_CH,40);
- NRF24L01_Write_Reg(WRITE_REG1+RX_PW_P0,RX_PLOAD_WIDTH);
- NRF24L01_Write_Reg(WRITE_REG1+RF_SETUP,0x0f);
- NRF24L01_Write_Reg(WRITE_REG1+CONFIG1, 0x0f);
- NRF24L01_CE = 1;
- }
-
- void TX_Mode(void)
- {
- NRF24L01_CE=0;
- NRF24L01_Write_Buf(WRITE_REG1+TX_ADDR,(unsigned char*)TX_ADDRESS,TX_ADR_WIDTH);
- NRF24L01_Write_Buf(WRITE_REG1+RX_ADDR_P0,(unsigned char*)RX_ADDRESS,RX_ADR_WIDTH);
- NRF24L01_Write_Reg(WRITE_REG1+EN_AA,0x01);
- NRF24L01_Write_Reg(WRITE_REG1+EN_RXADDR,0x01);
- NRF24L01_Write_Reg(WRITE_REG1+SETUP_RETR,0x1a);
- NRF24L01_Write_Reg(WRITE_REG1+RF_CH,40);
- NRF24L01_Write_Reg(WRITE_REG1+RF_SETUP,0x0f);
- NRF24L01_Write_Reg(WRITE_REG1+CONFIG1,0x0e);
- NRF24L01_CE=1;
- }
- /* ============= 6 AllInit.c */
- #include "includes.h"
- #include "AllInit.h"
- #include "main.h"
- #define RemoteControlData 0 //串口输出遥控器发送数值使能
- #define AttitudeData 1 //串口输出姿态值使能
- #define RollData 1 //输出y轴姿态
- #define PitchData 0 //输出x轴姿态
- unsigned char RXBUF[33]={0,0,0,0,0,0,0,0}; //2401数据存储区
- int keyk=0;
- int ee=0;
- /************************************************
- 函数功能:检测2401与单片机通信是否正常
- *************************************************/
- void Check24L01()
- {
- while( NRF24L01_Check())
- {
- printf("24l01 is error,please replacement 24l01 module! \n");
- delayms__(100);
- }
- printf ("24l01 is ok!\n");
- }
- /************************************************
- 函数功能:初始化飞行器各个模块
- *************************************************/
- void FlyAllInit()
- {
- delayms_(100);
- InitMPU6050(); //初始化MPU-6050
- Usart_Init(); //初始化串口
- PWMGO(); //初始化PWM
- NRF24L01_Init(); //初始化2401
- RX_Mode(); //设为接收模式
- Time0_Init(); //初始化定时器
- printf ("All module is ok!\n Get ready to fly!\n");
- }
- /************************************************
- 函数功能:飞行器接收2401数据
- *************************************************/
- void Reseve2401(void)
- {
- if(NRF24L01_RxPacket(RXBUF)==0)
- {
- if((RXBUF[6]*16+RXBUF[7])<30)
- {keyk=1;}
- ee=0;error=0;
- RXBUF[32]=0;
- }
- else
- {
- if(keyk==1)
- ee++;
- }
- if(ee>=200)
- {error=1;}
- }
- /************************************************
- 函数功能:飞行器输出内部数据
- *************************************************/
- void OutPutData()
- {
- //遥控器数值输出
- #if RemoteControlData
- OutData[0]= RXBUF[0]*16+RXBUF[1]; //横滚
- OutData[1]= RXBUF[2]*16+RXBUF[3]; //俯仰
- OutData[2]= RXBUF[4]*16+RXBUF[5]; //偏航
- OutData[3]= RXBUF[6]*16+RXBUF[7]; //油门
- OutPut_Data();
- #endif
- //姿态输出
- #if AttitudeData
- #if PitchData
- OutData[0] = Pitch;
- OutData[1] = X_angle;
- OutData[2] = Average_Gy;
- // OutData[3] = A_angle_Y;;
- OutPut_Data();
- #endif
- #if RollData
- OutData[0] = Roll;
- OutData[1] = Y_angle;
- OutData[2] = Average_Gx;
- // OutData[3] = A_angle_Y;;
- OutPut_Data();
- #endif
- #endif
- }
- /************************************************
- 函数名称:MotorTest()
- 函数功能:飞行器电机测试
- *************************************************/
- void MotorTest(void)
- {
- //数值设定 0~2700;同时要屏蔽IMU.c 中断里面的MainControl
- PWM(0,0,0,0);
- }
- void Delay100us() //@30.000MHz
- {
- unsigned char i, j;
- i = 3;
- j = 232;
- do
- {
- while (--j);
- } while (--i);
- }
- void delayms__(int ms)
- {
- int x,y;
- for(x=ms;x>0;x--)
- {
- for(y=1000;y>0;y--)
- ;
- }
- }
- /* ============= 7 MPU-6050.c */
- #include <STC15W4K60S4.H>
- #include <intrins.h>
- #include <MPU6050.H>
- #include <NRF24L01.H>
- #define uchar unsigned char
- sbit SCL=P3^4; //IIC时钟引脚定义 Rev8.0硬件
- sbit SDA=P3^5; //IIC数据引脚定义
- void InitMPU6050(); //初始化MPU6050
- void Delay2us();
- void I2C_Start();
- void I2C_Stop();
- bit I2C_RecvACK();
- void I2C_SendByte(unsigned char dat);
- uchar I2C_RecvByte();
- void I2C_ReadPage();
- void I2C_WritePage();
- uchar Single_ReadI2C(uchar REG_Address); //读取I2C数据
- void Single_WriteI2C(uchar REG_Address,uchar REG_data);
- //向I2C写入数据
- //I^C时序中延时设置,具体参见各芯片的数据手册 6050推荐最小1.3us 但是会出问题,这里延时实际1.9us左右
- void Delay2us() //@27.000MHz
- {
- unsigned char i;
- i = 11;
- while (--i);
- }
- //**************************************
- //I2C起始信号
- //**************************************
- void I2C_Start()
- {
- SDA = 1; //拉高数据线
- SCL = 1; //拉高时钟线
- Delay2us(); //延时
- SDA = 0; //产生下降沿
- Delay2us(); //延时
- SCL = 0; //拉低时钟线
- }
- //**************************************
- //I2C停止信号
- //**************************************
- void I2C_Stop()
- {
- SDA = 0; //拉低数据线
- SCL = 1; //拉高时钟线
- Delay2us(); //延时
- SDA = 1; //产生上升沿
- Delay2us(); //延时
- }
- //**************************************
- //I2C接收应答信号
- //**************************************
- bit I2C_RecvACK()
- {
- SCL = 1; //拉高时钟线
- Delay2us(); //延时
- CY = SDA; //读应答信号
- SCL = 0; //拉低时钟线
- Delay2us(); //延时
- return CY;
- }
- //**************************************
- //向I2C总线发送一个字节数据
- //**************************************
- void I2C_SendByte(uchar dat)
- {
- uchar i;
- for (i=0; i<8; i++) //8位计数器
- {
- dat <<= 1; //移出数据的最高位
- SDA = CY; //送数据口
- SCL = 1; //拉高时钟线
- Delay2us(); //延时
- SCL = 0; //拉低时钟线
- Delay2us(); //延时
- }
- I2C_RecvACK();
- }
- //**************************************
- //从I2C总线接收一个字节数据
- //**************************************
- uchar I2C_RecvByte()
- {
- uchar i;
- uchar dat = 0;
- SDA = 1; //使能内部上拉,准备读取数据,
- for (i=0; i<8; i++) //8位计数器
- {
- dat <<= 1;
- SCL = 1; //拉高时钟线
- Delay2us(); //延时
- dat |= SDA; //读数据
- SCL = 0; //拉低时钟线
- Delay2us(); //延时
- }
- return dat;
- }
- //**************************************
- //向I2C设备写入一个字节数据
- //**************************************
- void Single_WriteI2C(uchar REG_Address,uchar REG_data)
- {
- I2C_Start(); //起始信号
- I2C_SendByte(SlaveAddress); //发送设备地址+写信号
- I2C_SendByte(REG_Address); //内部寄存器地址,
- I2C_SendByte(REG_data); //内部寄存器数据,
- I2C_Stop(); //发送停止信号
- }
- //**************************************
- //从I2C设备读取一个字节数据
- //**************************************
- uchar Single_ReadI2C(uchar REG_Address)
- {
- uchar REG_data;
- I2C_Start(); //起始信号
- I2C_SendByte(SlaveAddress); //发送设备地址+写信号
- I2C_SendByte(REG_Address); //发送存储单元地址,从0开始
- I2C_Start(); //起始信号
- I2C_SendByte(SlaveAddress+1); //发送设备地址+读信号
- REG_data=I2C_RecvByte(); //读出寄存器数据
-
- SDA = 1; //写应答信号
- SCL = 1; //拉高时钟线
- Delay2us(); //延时
- SCL = 0; //拉低时钟线
- Delay2us(); //延时
-
- I2C_Stop(); //停止信号
- return REG_data;
- }
- //**************************************
- //初始化MPU6050
- //**************************************
- void InitMPU6050()
- {
- Single_WriteI2C(PWR_MGMT_1, 0x00); //解除休眠状态
- Single_WriteI2C(SMPLRT_div, 0x07); //陀螺仪125hz
- Single_WriteI2C(CONFIG, 0x06); //21HZ滤波 延时A8.5ms G8.3ms
- //此处取值应相当注意,延时与系统周期相近为宜
- Single_WriteI2C(GYRO_CONFIG, 0x18); //陀螺仪500度/S 65.5LSB/g
- Single_WriteI2C(ACCEL_CONFIG, 0x01);//加速度+-4g 8192LSB/g
- }
- //**************************************
- //合成数据
- //**************************************
- int GetData(uchar REG_Address)
- {
- char H,L;
- H=Single_ReadI2C(REG_Address);
- L=Single_ReadI2C(REG_Address+1);
- return (H<<8)+L; //合成数据
- }
- /* ============= 8 IMU.c