基于Arduino的四轴飞行器Word文件下载.docx
《基于Arduino的四轴飞行器Word文件下载.docx》由会员分享,可在线阅读,更多相关《基于Arduino的四轴飞行器Word文件下载.docx(246页珍藏版)》请在冰点文库上搜索。
for(i=0;
i++){
if(maxMotor>
MAXTHROTTLE)//保证当某一个油门达到最大时,陀螺仪仍有良好的信号连接
motor[i]-=maxMotor-MAXTHROTTLE;
motor[i]=constrain(motor[i],conf.minthrottle,
MAXTHROTTLE);
if((rcData[THROTTLE]<
MINCHECK)&
&
!
f.BARO_MODE)
motor[i]=conf.minthrottle;
if(!
f.ARMED)
motor[i]=MINCOMMAND;
}
voidwriteMotors(){
OCR1A=motor[0]>
>
3;
//pin9输出电机1号
OCR1B=motor[1]>
//pin10输出电机2号
OCR2A=motor[2]>
//pin11输出电机3号
OCR2B=motor[3]>
//pin3输出电机4号
voidwriteAllMotors(int16_tmc){//所有电机输出设定为mc
for(uint8_ti=0;
motor[i]=mc;
}
writeMotors();
/*电调初始化函数,电调初始化完成后注释掉defined重新上传
#ifdefined(ESC_CALIB_CANNOT_FLY)
writeAllMotors(ESC_CALIB_HIGH);
blinkLED(2,20,2);
delay(4000);
writeAllMotors(ESC_CALIB_LOW);
blinkLED(3,20,2);
while
(1){
delay(5000);
blinkLED(4,20,2);
#endif
exit;
#endif*/
2、遥控器发送/接收模块
//RX.h主要函数定义
#include"
Arduino.h"
#defineRC_CHANS8
#definePCINT_PIN_COUNT5
#definePCINT_RX_PORTPORTB
#definePCINT_RX_MASKPCMSK0
#definePCIR_PORT_BIT(1<
<
0)
#defineRX_PC_INTERRUPTPCINT0_vect
#defineRX_PCINT_PIN_PORTPINB
#defineROLLPIN4//预定义各信道的名称
#defineTHROTTLEPIN3
#definePITCHPIN5
#defineYAWPIN2
#defineAUX1PIN6
#defineAUX2PIN7
#defineAUX3PIN1//保留
#defineAUX4PIN0//保留
#definePCINT_RX_BITS(1<
1),(1<
2),(1<
3),(1<
4),(1<
voidconfigureReceiver();
voidcomputeRC();
uint16_treadRawRC(uint8_tchan);
//初始信号读取函数
//接收代码
volatileuint16_trcValue[RC_CHANS]={1502,1502,1502,1502,1502,1502,1502,1502};
staticuint8_trcChannel[RC_CHANS]={ROLLPIN,PITCHPIN,YAWPIN,THROTTLEPIN,AUX1PIN,AUX2PIN,AUX3PIN,AUX4PIN};
staticuint8_tPCInt_RX_Pins[PCINT_PIN_COUNT]={PCINT_RX_BITS};
voidconfigureReceiver(){
for(uint8_ti=0;
i<
PCINT_PIN_COUNT;
i++){
PCINT_RX_PORT|=PCInt_RX_Pins[i];
PCINT_RX_MASK|=PCInt_RX_Pins[i];
PCICR=PCIR_PORT_BIT;
PCICR|=(1<
0);
#defineRX_PIN_CHECK(pin_pos,rc_value_pos)\
if(mask&
PCInt_RX_Pins[pin_pos]){\
if(!
(pin&
PCInt_RX_Pins[pin_pos])){\
dTime=cTime-edgeTime[pin_pos];
\
if(900<
dTime&
dTime<
2200){\
rcValue[rc_value_pos]=dTime;
\
}\
}elseedgeTime[pin_pos]=cTime;
\
ISR(RX_PC_INTERRUPT){//中断函数用于响应
uint8_tmask;
uint8_tpin;
uint16_tcTime,dTime;
staticuint16_tedgeTime[8];
staticuint8_tPCintLast;
pin=RX_PCINT_PIN_PORT;
mask=pin^PCintLast;
cTime=micros();
sei();
PCintLast=pin;
#if(PCINT_PIN_COUNT>
0)
RX_PIN_CHECK(0,2);
#endif
1)
RX_PIN_CHECK(1,4);
2)
RX_PIN_CHECK(2,5);
3)
RX_PIN_CHECK(3,6);
4)
RX_PIN_CHECK(4,7);
5)
RX_PIN_CHECK(5,0);
6)
RX_PIN_CHECK(6,1);
7)
RX_PIN_CHECK(7,3);
ISR(PCINT0_vect){
uint8_tpin;
uint16_tcTime,dTime;
staticuint16_tedgeTime;
pin=PINB;
dTime=cTime-edgeTime;
2200)rcValue[0]=dTime;
}
else
edgeTime=cTime;
//如果bit2在高电平(PPMpulse上升),存储时间
uint16_treadRawRC(uint8_tchan){//读取原始信号
uint16_tdata;
uint8_toldSREG;
oldSREG=SREG;
cli();
//禁止中断
data=rcValue[rcChannel[chan]];
SREG=oldSREG;
returndata;
voidcomputeRC(){//提取遥控器信号,进行数据处理,主要取平均值
staticuint16_trcData4Values[RC_CHANS][4],rcDataMean[RC_CHANS];
staticuint8_trc4ValuesIndex=0;
uint8_tchan,a;
rc4ValuesIndex++;
if(rc4ValuesIndex==4)rc4ValuesIndex=0;
for(chan=0;
chan<
RC_CHANS;
chan++){
rcData4Values[chan][rc4ValuesIndex]=readRawRC(chan);
rcDataMean[chan]=0;
for(a=0;
a<
a++)rcDataMean[chan]+=rcData4Values[chan][a];
rcDataMean[chan]=(rcDataMean[chan]+2)>
2;
if(rcDataMean[chan]<
(uint16_t)rcData[chan]-3)
rcData[chan]=rcDataMean[chan]+2;
if(rcDataMean[chan]>
(uint16_t)rcData[chan]+3)
rcData[chan]=rcDataMean[chan]-2;
if(chan<
8&
rcSerialCount>
0){
rcSerialCount--;
if(rcSerial[chan]>
900){rcData[chan]=rcSerial[chan];
3、传感器读取模块
voidACC_getADC();
voidGyro_getADC();
voidinitSensors();
voidi2c_rep_start(uint8_taddress);
voidi2c_write(uint8_tdata);
voidi2c_stop(void);
voidi2c_writeReg(uint8_tadd,uint8_treg,uint8_tval);
uint8_ti2c_readReg(uint8_tadd,uint8_treg);
uint8_ti2c_readAck();
uint8_ti2c_readNak();
#defineMPU6050_DLPF_CFG4//设置低频滤波系数为4
voidi2c_BMP085_UT_Start(void);
voidwaitTransmissionI2C();
voidi2c_MS561101BA_UT_Start();
voidDevice_Mag_getADC();
voidBaro_init();
//三个初始化函数
voidMag_init();
voidACC_init();
uint8_trawADC[6];
staticuint32_tneutralizeTime=0;
voidi2c_init(void){
TWSR=0;
TWBR=((F_CPU/I2C_SPEED)-16)/2;
TWCR=1<
TWEN;
voidi2c_rep_start(uint8_taddress){//传感器开始读取
TWCR=(1<
TWINT)|(1<
TWSTA)|(1<
TWEN);
waitTransmissionI2C();
TWDR=address;
voidi2c_stop(void){//传感器结束读取
TWCR=(1<
TWINT)|(1<
TWEN)|(1<
TWSTO);
voidi2c_write(uint8_tdata){//
TWDR=data;
uint8_ti2c_read(uint8_tack){//读取函数
TWEN)|(ack?
(1<
TWEA):
0);
uint8_tr=TWDR;
ack)i2c_stop();
returnr;
uint8_ti2c_readAck(){
returni2c_read
(1);
uint8_ti2c_readNak(void){
returni2c_read(0);
voidwaitTransmissionI2C(){
uint16_tcount=255;
while(!
(TWCR&
TWINT))){
count--;
if(count==0){
TWCR=0;
neutralizeTime=micros();
i2c_errors_count++;
break;
size_ti2c_read_to_buf(uint8_tadd,void*buf,size_tsize){
i2c_rep_start((add<
1)|1);
//I2Creaddirection
size_tbytes_read=0;
uint8_t*b=(uint8_t*)buf;
while(size--){
*b++=i2c_read(size>
bytes_read++;
returnbytes_read;
size_ti2c_read_reg_to_buf(uint8_tadd,uint8_treg,void*buf,size_tsize){
i2c_rep_start(add<
1);
i2c_write(reg);
returni2c_read_to_buf(add,buf,size);
voidswap_endianness(void*buf,size_tsize){
uint8_ttray;
uint8_t*from;
uint8_t*to;
for(from=(uint8_t*)buf,to=&
from[size-1];
from<
to;
from++,to--){
tray=*from;
*from=*to;
*to=tray;
voidi2c_getSixRawADC(uint8_tadd,uint8_treg){
i2c_read_reg_to_buf(add,reg,&
rawADC,6);
voidi2c_writeReg(uint8_tadd,uint8_treg,uint8_tval){
i2c_write(val);
i2c_stop();
}//完整写入执行函数
uint8_ti2c_readReg(uint8_tadd,uint8_treg){
uint8_tval;
val,1);
returnval;
}//完整读取执行函数
voidGYRO_Common(){
staticint16_tpreviousGyroADC[3]={0,0,0};
staticint32_tg[3];
uint8_taxis,tilt=0;
if(calibratingG>
0){
for(axis=0;
axis<
3;
axis++){
if(calibratingG==512){
g[axis]=0;
g[axis]+=imu.gyroADC[axis];
imu.gyroADC[axis]=0;
gyroZero[axis]=0;
if(calibratingG==1){
gyroZero[axis]=(g[axis]+256)>
9;
blinkLED(10,15,1);
calibratingG--;
for(axis=0;
imu.gyroADC[axis]-=gyroZero[axis];
imu.gyroADC[axis]=constrain(imu.gyroADC[axis],previousGyroADC[axis]-800,previousGyroADC[axis]+800);
previousGyroADC[axis]=imu.gyroADC[axis];
voidACC_Common(){//获得加速度数值
staticint32_ta[3];
if(calibratingA>
for(uint8_taxis=0;
if(calibratingA==512)a[axis]=0;
a[axis]+=imu.accADC[axis];
imu.accADC[axis]=0;
global_conf.accZero[axis]=0;
if(calibratingA==1){
global_conf.accZero[ROLL]=(a[ROLL]+256)>
global_conf.accZero[PITCH]=(a[PITCH]+256)>
global_conf.accZero[YAW]=((a[YAW]+256)>
9)-ACC_1G;
conf.angleTrim[ROLL]=0;
conf.angleTrim[PITCH]=0;
writeGlobalSet
(1);
calibratingA--;
imu.accADC[ROLL]-=global_conf.accZero[ROLL];
imu.accADC[PITCH]-=global_conf.accZero[PITCH];
imu.accADC[YAW]-=global_conf.accZero[YAW];
4、姿态计算函数相关代码
//姿态算法IUM.h文件
#defineBARO_TAB_SIZE21
voidcomputeIMU();
//IUM.cpp
voidgetEstimatedAttitude();
voidcomputeIMU(){//姿态计算函数
uint8_taxis;
staticint16_tgyroADCprevious[3]={0,0,0};
int16_tgyroADCp[3];
int16_tgyroADCinter[3];
staticuint32_ttimeInterleave=0;
ACC_getADC();
getEstimatedAttitude();
Gyro_getADC();
axis++)
gyroADCp[axis]=imu.gyroADC[axis];
timeInterleave=micros();
annexCode();
uint8_tt=0;
while((uint16_t)(micros()-timeInterleave)<
650)t=1;
t)annex650_overrun_count++;
gyroADCinter[axis]=imu.gyroADC[axis]+gyroADCp[axis];
//采用经验值
imu.gyroData[axis]=(gyroADCinter[axis]+gyroADCprevious[axis])/3;
gyroADCprevious[axis]=gyroADCinter[axis]>
1;
ACC)imu.accADC[axis]=0;
staticint16_tgyroSmooth[3]={0,0,0};
axis++)
{imu.gyroData[axis]=(int16_t)(((int32_t)((int32_t)gyroSmooth[axis]*(conf.Smoothing[axis]-1))+imu.gyroData[axis]+1)/conf.Smoothing[axis]);
gyroSmooth[axis]=imu.gyroData[axis];
}//获得陀螺仪数据
#defineGYR_CMPFM_FACTOR250
#defineINV_GYR_CMPF_FA