基于Arduino的四轴飞行器Word文件下载.docx
《基于Arduino的四轴飞行器Word文件下载.docx》由会员分享，可在线阅读，更多相关《基于Arduino的四轴飞行器Word文件下载.docx（246页珍藏版）》请在冰豆网上搜索。
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<
i++）
if（motor[i]>
maxMotor）maxMotor=motor[i];
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）;
voidi
升级会员 