四轴飞行器的设计之四轴主板的综合程序——STC15单片机实战指南连载

通信

前面比较全面的讲述了四轴飞行器的硬件设计和软件中PID算法以及四元数与滤波算法，这节放出四轴主板的程序代码。　　
例程比较复杂，这里采用了模块化编程的思路，按功能将其各个函数进行分类。笔者刚开始计划这部分的例程不贴到书上，但怕读者又说读者开源不够，与此，将完整的一套四轴源码贴到这里，做到真正的开源，起到让读者学习的目的。但读者需要注意的，该例程不像前面的小实例那样简单，读者再学习的时候，要静下心来，边读例程，边理解，同时一定要做记录，达到真正的理解。再者，便于读者调试，这里保留了笔者调试时所用的串口示波器源码，这样，可大大降低读者调试的难度，同时，所有“.c”文件对应的头文件“.h”文件，这里全部省略，里面主要是对变量和函数的声明，学到这里，对于函数的声明等，应该能熟练于心了。接下来，我们开始上菜，读者慢慢“享用”。

/* ============= 1 STC四轴正式版程序.c */
#include "includes.h"
#include "main.h"
#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
#include
#include
#include
#include
#include
#include <timer.h>
#include
#include
#include
#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();
}
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;
X_gyro = GetData(GYRO_XOUT_H);;
X_gyro/= 16.4; //X-axis accelerometer
j = X_gyro;
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)
{
short i,j,k;
short tmp;
Y_gyro = GetData(GYRO_YOUT_H);
Y_gyro/= 16.4; //Y-axis gyroscope
i = Y_gyro;
Y_gyro = GetData(GYRO_YOUT_H);
Y_gyro/= 16.4; //Y-axis gyroscope
j = Y_gyro;
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)
{
short i,j,k;
short tmp;
Z_gyro = GetData(GYRO_ZOUT_H);;
Z_gyro/= 16.4;
i = Z_gyro;
Z_gyro = GetData(GYRO_ZOUT_H);
Z_gyro/= 16.4;
j = Z_gyro;
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
#include
#include
#include
#include
#include
#include
#include
#include
#include
#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
PWM_XZ=MINPWM;
else if( PWM_XZ>MAXPWM)
PWM_XZ=MAXPWM;
if( PWM_XF
PWM_XF=MINPWM;
else if( PWM_XF>MAXPWM)
PWM_XF=MAXPWM;
if( PWM_YZ
PWM_YZ=MINPWM;
else if( PWM_YZ>MAXPWM)
PWM_YZ=MAXPWM;
if( PWM_YF
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
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数据寄存器
//***it 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
#include "nrf24l01.h"
#include "spi.h"
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);
}
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);
}
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
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
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
#include
#include
#include
#define uchar unsigned char
***it SCL=P3^4; //IIC时钟引脚定义 Rev8.0硬件
***it 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 */
#include
#include "includes.h"
#include "IMU.H"
#include "math.H"
//#define Kp 28.1f //10.1f
//#define Ki 0.001f//0.011f
//#define halfT 0.0020f
#define Kp 15.1f //10.1f
#define Ki 0.001 //0.001f//0.011f
#define halfT 0.001 //0.0019f
float idata q0=1,q1=0,q2=0,q3=0;
float idata exInt=0,eyInt=0,ezInt=0;
float Pitch , Roll;
float a,b,c;
float IntegralZ;
/*********************************/
#define KALMAN_Qy 0.015
#define KALMAN_Ry 10.0000
#define KALMAN_Qx 0.015
#define KALMAN_Rx 10.0000
#define KALMAN_Qz 0.015
#define KALMAN_Rz 10.0000
static double KalmanFilter_x(const double ResrcData,double ProcessNiose_Q,double MeasureNoise_R)
{
double R = MeasureNoise_R;
double Q = ProcessNiose_Q;
static double x_last;
double x_mid = x_last;
double x_now;
static double p_last;
double p_mid ;
double p_now;
double kg;
x_mid=x_last; //x_last=x(k-1|k-1),x_mid=x(k|k-1)
p_mid=p_last+Q; //p_mid=p(k|k-1),p_last=p(k-1|k-1),
kg=p_mid/(p_mid+R);
x_now=x_mid+kg*(ResrcData-x_mid);
p_now=(1-kg)*p_mid;
p_last = p_now;
x_last = x_now;
return x_now;
}
static double KalmanFilter_y(const double ResrcData,double ProcessNiose_Q,double MeasureNoise_R)
{
double R = MeasureNoise_R;
double Q = ProcessNiose_Q;
static double y_last;
double y_mid = y_last;
double y_now;
static double p_last;
double p_mid ;
double p_now;
double kg;
y_mid=y_last; //x_last=x(k-1|k-1),x_mid=x(k|k-1)
p_mid=p_last+Q; //p_mid=p(k|k-1),p_last=p(k-1|k-1),Q=??éù
kg=p_mid/(p_mid+R);
y_now=y_mid+kg*(ResrcData-y_mid);
p_now=(1-kg)*p_mid;
p_last = p_now;
y_last = y_now;
return y_now;
}
static double KalmanFilter_z(const double ResrcData,double ProcessNiose_Q,double MeasureNoise_R)
{
double R = MeasureNoise_R;
double Q = ProcessNiose_Q;
static double z_last;
double z_mid = z_last;
double z_now;
static double p_last;
double p_mid ;
double p_now;
double kg;
z_mid=z_last; //x_last=x(k-1|k-1),x_mid=x(k|k-1)
p_mid=p_last+Q;
kg=p_mid/(p_mid+R);
z_now=z_mid+kg*(ResrcData-z_mid);
p_now=(1-kg)*p_mid;
p_last = p_now;
z_last = z_now;
return z_now;
}
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
{
float norm;
// float hx, hy, hz, bx, bz;
float vx, vy, vz;// wx, wy, wz;
float ex, ey, ez;
float q0q0 = q0*q0;
float q0q1 = q0*q1;
float q0q2 = q0*q2;
// float q0q3 = q0*q3;
float q1q1 = q1*q1;
// float q1q2 = q1*q2;
float q1q3 = q1*q3;
float q2q2 = q2*q2;
float q2q3 = q2*q3;
float q3q3 = q3*q3;
norm = sqrt(ax*ax + ay*ay + az*az);
ax = ax /norm;
ay = ay / norm;
az = az / norm;
vx = 2*(q1q3 - q0q2);
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3 ;
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;
b = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
c = atan2(2 * q2 * q3 + 2 * q0 * q1, q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3)* 57.3; // roll
//d = atan2(2 * q1 * q2+2 * q0 * q3,q0 * q0+q1 * q1-q2 * q2-q3 * q3)*57.3;
Pitch =b;
Roll = c;
}
/************************************************************/
extern short Y_gyro,X_gyro,Z_gyro;
extern short Z_angle,Y_angle,X_angle;
extern short X_gyroInit, Y_gyroInit, Z_gyroInit;
float Average_Gx,Average_Gy,Average_Gz,Average_Ax,Average_Ay,
Average_Az=0;
float Average_Ax_old[4],Average_Ay_old[4],Average_Az_old[4];
float A_angle_X,A_angle_Y,A_angle_Z;
/*------- Data processing -------*/
void MPU_pro()
{
Average_Gx=X_gyro;//+15;//- X_gyroInit;
Average_Gy=Y_gyro;//+15;//- Y_gyroInit;
Average_Gz=Z_gyro;//- Z_gyroInit;
Average_Ax=X_angle;
Average_Ay=Y_angle;
Average_Az=Z_angle;
Average_Ax_old[2]=Average_Ax_old[1];
Average_Ay_old[2]=Average_Ay_old[1];
Average_Az_old[2]=Average_Az_old[1];
Average_Ax_old[1]=Average_Ax_old[0];
Average_Ay_old[1]=Average_Ay_old[0];
Average_Az_old[1]=Average_Az_old[0];
Average_Ax_old[0]=Average_Ax;
Average_Ay_old[0]=Average_Ay;
Average_Az_old[0]=Average_Az;
//**************************************************
if(Average_Ay_old[2]!=0)
{
IntegralZ+= Average_Gz *0.01;
if( IntegralZ>=359)
IntegralZ=0;
if( IntegralZ<=-359)
IntegralZ=0;
Average_Ax=MidFilter( Average_Ax_old[0],Average_Ax_old[1], Average_Ax_old[2]);
Average_Ay=MidFilter( Average_Ay_old[0],Average_Ay_old[1], Average_Ay_old[2]);
Average_Az=MidFilter( Average_Az_old[0],Average_Az_old[1], Average_Az_old[2]);
A_angle_X=atan((float)(Average_Ax/sqrt(Average_Ay*
Average_Ay+Average_Az*Average_Az)))*57.3;
A_angle_Y =atan((float)(Average_Ay/sqrt(Average_Ax*
Average_Ax+Average_Az*Average_Az)))*57.3;
A_angle_Z =atan((float)(Average_Az/sqrt(Average_Ax*
Average_Ax+Average_Ay*Average_Ay)))*57.3;
//IMUupdate( Average_Gx*0.05044,Average_Gy*-0.05044,
//Average_Gz*0.03744,-Average_Ax,Average_Ay, Average_Az);
IMUupdate( Average_Gx*0.05544,Average_Gy*-0.05544,
Average_Gz*0.05544,-Average_Ax,Average_Ay, Average_Az);
}
}
float MidFilter(float a,float b,float c)
{
float i,j,k;
float tmp;
i = a;
j = b;
k = c;
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;
}
extern int Controldata_THROTTLE ;
int mmms=0;
int s=0;
void Angle_Calculate() interrupt 1
{
Read_MPU(); // 读取mpu6050数据
MPU_pro(); // 进行imu数据转换
MainControl(); // 进行姿态控制
}
/* ============= 9 USART.c */
#include
#include
#include
bit busy;
void Usart_Init() //30m 波特率9600
{
SCON = 0x50; //8位数据,可变波特率
AUXR |= 0x40; //定时器1时钟为Fosc,即1T
AUXR &= 0xFE; //串口1选择定时器1为波特率发生器
TMOD &= 0x0F; //设定定时器1为16位自动重装方式
TL1 = 0x41; //设定定时初值
TH1 = 0xFD; //设定定时初值
ET1 = 0; //禁止定时器1中断
TR1 = 1; //启动定时器1
REN=1;
ES=1;
EA=1;
TI=1;
}
void Uart() interrupt 4 using 1
{
if (RI)
{
RI = 0;
}
if (TI)
{
TI = 0;
busy = 0;
}
}
void SendData(unsigned char dat)
{
SBUF = dat;
while(!TI);
TI = 0;
}
void Send(int Ax,int Ay,int Az,int Gx,int Gy,int Gz)
{
unsigned char sum=0;
ES = 1; //打开串口中断
SendData(0xAA); //帧头
SendData(0xAA); //帧头
SendData(0x02); //功能字
SendData(12); //发送的数据长度
SendData(Ax); //低8位
SendData(Ax>>8); //高8位
SendData(Ay);
SendData(Ay>>8);
SendData(Az);
SendData(Az>>8);
SendData(Gx);
SendData(Gx>>8);
SendData(Gy);
SendData(Gy>>8);
SendData(Gz);
SendData(Gz>>8);
sum+=0xAA;sum+=0xAA;sum+=0x02;sum+=12;
sum+=Ax>>8;sum+=Ax;sum+=Ay>>8;sum+=Ay;sum+=Az>>8;sum+=Az;
sum+=Gx>>8;sum+=Gx;sum+=Gy>>8;sum+=Gy;sum+=Gz>>8;sum+=Gz;
SendData(sum); //校验和
ES = 0; //关闭串口中断
}
/* ============= 10 outputdata.c */
#include "outputdata.h"
#include "USART.h"
float OutData[4] = { 0 };
unsigned short CRC_CHECK(unsigned char *Buf, unsigned char CRC_CNT)
{
unsigned short CRC_Temp;
unsigned char i,j;
CRC_Temp = 0xffff;
for (i=0;i
CRC_Temp ^= Buf[i];
for (j=0;j<8;j++) {
if (CRC_Temp & 0x01)
CRC_Temp = (CRC_Temp >>1 ) ^ 0xa001;
else
CRC_Temp = CRC_Temp >> 1;
}
}
return(CRC_Temp);
}
void OutPut_Data(void)
{
int temp[4] = {0};
unsigned int temp1[4] = {0};
unsigned char databuf[10] = {0};
unsigned char i;
unsigned short CRC16 = 0;
for(i=0;i<4;i++)
{
temp[i] = (int)OutData[i];
temp1[i] = (unsigned int)temp[i];
}
for(i=0;i<4;i++)
{
databuf[i*2] = (unsigned char)(temp1[i]%256);
databuf[i*2+1] = (unsigned char)(temp1[i]/256);
}
CRC16 = CRC_CHECK(databuf,8);
databuf[8] = CRC16%256;
databuf[9] = CRC16/256;
for(i=0;i<10;i++)
SendData(databuf[i]);
}
/***************************************************************
* 功能：串口示波器放数据用
***************************************************************/
void uart_putstr(char ch[])
{
unsigned char ptr=0;
while(ch[ptr]){
SendData((char)ch[ptr++]);
}
}
/* ============= 11 Timer.c */
#include
#include
void Time0_Init() //10ms@27MHz 定时器0 16位12T自动重载
{
AUXR &= 0x7F; //定时器时钟12T模式
TMOD &= 0xF0; //设置定时器模式
TL0 = 0x1C; //设置定时初值
TH0 = 0xA8; //设置定时初值
TF0 = 0; //清除TF0标志
TR0 = 1; //定时器0开始计时
IE=0X82 ;
EA=1;
}
void Timer1Init(void)
{
// AUXR |= 0x40;
TMOD &= 0x0F;
IE = 0x8a;
TL1 = 0x54;
TH1 = 0xF2;
TF1 = 0;
TR1 = 1;
}
/* ============= 12 STC15W4KPWM.c */
#include
#include
#include
#include
extern unsigned char RxBuf[20];
void PWMGO()
{
//所有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;
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; // 置0 0-系统时钟分频分频参数设定
// 1-定时器2溢出时钟=系统时钟/([3:0]+1)
PWMIF=0x00; //置0 计数器归零中断标志相应PWM端口中断标志
PWMFDCR=0x00; //7位 6位 5位 4位
//置0 置0 外部异常检测开关外部异常时0-无反应 1-高阻状态
//3位 2位 1位 0位
//PWM异常中断比较器与异常的关系 P2.4与异常的关系 PWM异常标志
PWMCH=0x0b;//15位寄存器，决定PWM周期，数值为1-32767，单位：脉冲时钟
PWMCL=0xb9;
// 以下为每个PWM输出口单独设置
PWM2CR=0x00; //7位6位5位4位 3位 2位 1位 0位
// 置0 输出切换中断开关 T2中断开关 T1中断开关
PWM3CR=0x00;
PWM4CR=0x00;
PWM5CR=0x00;
PWM2T1H=0x0a;
//15位寄存器第一次翻转计数第一次翻转是指从低电平翻转到高电平的计时
PWM2T1L=0x8c;
PWM2T2H=0x0a;
//15位寄存器第二次翻转计数第二次翻转是指从高电平翻转到低电平的计时
PWM2T2L=0x8d; //第二次翻转应比精度等级要高，否则会工作不正常
// 比如精度1000，第二次翻转就必须小于1000
PWM3T1H=0x0a;
PWM3T1L=0x8c;
PWM3T2H=0x0a;
PWM3T2L=0x8d;
PWM4T1H=0x0a;
PWM4T1L=0x8c;
PWM4T2H=0x0a;
PWM4T2L=0x8d;
PWM5T1H=0x0a;
PWM5T1L=0x8c;
PWM5T2H=0x0a;
PWM5T2L=0x8d;
//以上为每个PWM输出口单独设置
PWMCR=0x8f;
//7位/6位5位 4位 3位 2位 1位 0位 10001111
//PWM开关计数归零中断开关相应I/O为GPIO模式(0)或PWM模式(1)
PWMCKS=0x00;
PWM(0,0,0,0);
}
//本函数输入的4个值取值范围为0-3000 0电机停，1000转速最高
//输入数据不能超过取值范围;
void PWM(unsigned int PWMa,unsigned int PWMb,unsigned int PWMc,unsigned int PWMd)
{
PWMa=2700-PWMa;
PWMb=2700-PWMb;
PWMc=2700-PWMc;
PWMd=2700-PWMd;
PWM2T1H=PWMa>>8;
//15位寄存器第一次翻转计数第一次翻转是指从低电平翻转到高电平的计时
PWM2T1L= PWMa;
PWM3T1H=PWMb>>8;
PWM3T1L= PWMb;
PWM4T1H=PWMc>>8;
PWM4T1L= PWMc;
PWM5T1H=PWMd>>8;
PWM5T1L= PWMd;
}

复制代码

最后说明下和本文配套的STC15开发板目前正在电子发烧友销售，如果需要请戳这里购买： https://bbs.elecfans.com/product/stc15.html 我将持续更新内容。

孙学荣 · 2016-4-27 22:00:49

啊u盾萨比传达设计等哈就是的哈时间的哈

马可菠萝 · 2016-4-28 12:33:01

看一下，比较感兴趣

zhangdonglin · 2016-5-10 15:57:15

看看，最近在开始做这个

孙泽文 · 2016-5-10 16:37:25

赞一个！！！！！！！！！！！！！！

snafceleplay175 · 2016-5-14 12:18:43

论坛代码的排版有点糟糕，看着累

代码战士 · 2016-5-17 11:28:31

求这本书的PDF格式。

lee_st · 2016-5-25 20:06:35

看看。正在学习中，支持下！

lee_st · 2016-5-25 20:07:04

看看。正在学习中，支持下！

lee_st · 2016-5-25 20:07:28

看看。正在学习中，支持下！

lee_st · 2016-5-25 20:07:41

看看。正在学习中，支持下！

烟艳炎龙 · 2016-6-16 22:27:00

6666666666666666666666666666666666666666666666666666666666666666666666666

jiangyi · 2016-6-25 22:28:44

同求啊，书出版了没啊

Benj · 2016-7-25 09:43:20

学习学习

Stonx_sail · 2016-9-11 14:16:45

学习。。。。。。。。。。。。。。。。。。。。。

xymbmcu · 2016-9-12 11:47:38

jiangyimfs 发表于 2016-6-25 22:28
同求啊，书出版了没啊

书出版了，可以到京东购买了

charlion · 2016-11-17 14:11:50

真不错

张仕杰 · 2016-12-11 10:19:05

谢谢分享谢谢分享谢谢分享
   谢谢分享谢谢分享                      谢谢分享
   谢谢分享谢谢分享             谢谢分享
               谢谢分享             谢谢分享谢谢分享谢谢分享
         谢谢分享             谢谢分享       谢             谢
         谢谢分享             谢谢分享       谢             谢
         谢谢分享             谢谢分享       分             分
         谢谢分享             谢谢分享       享             享
         谢谢分享             谢谢分享       谢             谢
         谢谢分享             谢谢分享       谢             谢
谢       谢谢分享             谢谢分享    分             分
谢谢    谢谢分享             谢谢分享       享             享
谢谢分谢谢分享             谢谢分享       谢             谢
谢谢分谢谢分享                            谢    谢
谢谢分谢谢分享                      谢                谢
      谢谢谢谢                   分                            分
         谢谢                享                                  享