基于Arduino的四轴飞行器.docx
《基于Arduino的四轴飞行器.docx》由会员分享,可在线阅读,更多相关《基于Arduino的四轴飞行器.docx(246页珍藏版)》请在冰豆网上搜索。
![基于Arduino的四轴飞行器.docx](https://file1.bdocx.com/fileroot1/2022-11/15/8ae7dd36-5a3e-4c59-9b1c-51f03fab4498/8ae7dd36-5a3e-4c59-9b1c-51f03fab44981.gif)
基于Arduino的四轴飞行器
第2章基于Arduino的四轴飞行器
2.3.3模块介绍
1、电机输出模块
(2)相关代码
//主要函数定义
externuint8_tPWM_PIN[8];
voidinitOutput();//初始化函数
voidmixTable();//PID计算
voidwriteMotors();//信号输出到电机
//输出程序
uint8_tPWM_PIN[8]={9,10,11,3,6,5,A2,12};//定义输出接口
voidinitOutput(){
for(uint8_ti=0;i<4;i++){
pinMode(PWM_PIN[i],OUTPUT);
}//初始化,使PWM引脚作为输出引脚
voidmixTable(){
int16_tmaxMotor;//定义最大转速电机编号
uint8_ti;
#definePIDMIX(X,Y,Z)rcCommand[THROTTLE]+axisPID[ROLL]*X+axisPID[PITCH]*Y+YAW_DIRECTION*axisPID[YAW]*Z//PID算法
motor[0]=PIDMIX(-1,+1,-1);
motor[1]=PIDMIX(-1,-1,+1);
motor[2]=PIDMIX(+1,+1,+1);
motor[3]=PIDMIX(+1,-1,-1);//4个电机输出计算(PID)
maxMotor=motor[0];//以下代码限制最大输出油门,防止异常
for(i=1;i<4;i++)
if(motor[i]>maxMotor)maxMotor=motor[i];
for(i=0;i<4;i++){
if(maxMotor>MAXTHROTTLE)//保证当某一个油门达到最大时,陀螺仪仍有良好的信号连接
motor[i]-=maxMotor-MAXTHROTTLE;
motor[i]=constrain(motor[i],conf.minthrottle,
MAXTHROTTLE);
if((rcData[THROTTLE]f.BARO_MODE)
motor[i]=conf.minthrottle;
if(!
f.ARMED)
motor[i]=MINCOMMAND;
}
}
voidwriteMotors(){
OCR1A=motor[0]>>3;//pin9输出电机1号
OCR1B=motor[1]>>3;//pin10输出电机2号
OCR2A=motor[2]>>3;//pin11输出电机3号
OCR2B=motor[3]>>3;//pin3输出电机4号
}
voidwriteAllMotors(int16_tmc){//所有电机输出设定为mc
for(uint8_ti=0;i<4;i++){
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、遥控器发送/接收模块
(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<<0)
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;iPCINT_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(900rcValue[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
#if(PCINT_PIN_COUNT>1)
RX_PIN_CHECK(1,4);
#endif
#if(PCINT_PIN_COUNT>2)
RX_PIN_CHECK(2,5);
#endif
#if(PCINT_PIN_COUNT>3)
RX_PIN_CHECK(3,6);
#endif
#if(PCINT_PIN_COUNT>4)
RX_PIN_CHECK(4,7);
#endif
#if(PCINT_PIN_COUNT>5)
RX_PIN_CHECK(5,0);
#endif
#if(PCINT_PIN_COUNT>6)
RX_PIN_CHECK(6,1);
#endif
#if(PCINT_PIN_COUNT>7)
RX_PIN_CHECK(7,3);
#endif
ISR(PCINT0_vect){
uint8_tpin;
uint16_tcTime,dTime;
staticuint16_tedgeTime;
pin=PINB;
cTime=micros();
sei();
dTime=cTime-edgeTime;if(900else
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;chanrcData4Values[chan][rc4ValuesIndex]=readRawRC(chan);
rcDataMean[chan]=0;
for(a=0;a<4;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、传感器读取模块
(2)相关代码
voidACC_getADC();
voidGyro_getADC();
voidinitSensors();
voidi2c_rep_start(uint8_taddress);
voidi