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

时间：10-02 整理：3721RD 点击：

前面比较全面的讲述了四轴飞行器的硬件设计和软件中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 <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();
}
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 <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"
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<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

上一篇：谁知道异或的符号？
下一篇：想做这样一个东西，大家觉得期间会遇到什么困难？我想提前了解下

单片机飞行器相关文章：

栏目分类