基于Arduino的四轴飞行器.docx
《基于Arduino的四轴飞行器.docx》由会员分享，可在线阅读，更多相关《基于Arduino的四轴飞行器.docx（246页珍藏版）》请在冰点文库上搜索。
基于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）;
voidi2c_write（uint8_tdata）;
voidi2c_stop（void）;
voidi2c_write（uint8_tdata）;
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<}
voidi2c_rep_start（uint8_taddress）{//传感器开始读取
TWCR=（1<waitTransmissionI2C（）;
TWDR=address;
TWCR=（1<waitTransmissionI2C（）;
voidi2c_stop（void）{//传感器结束读取
TWCR=（1<}
voidi2c_write（uint8_tdata）{//
TWDR=data;
TWCR=（1<waitTransmissionI2C（）;
}
uint8_ti2c_read（uint8_tack）{//读取函数
TWCR=（1<（1<0）;
waitTransmissionI2C（）;
uint8_tr=TWDR;
if（!
ack）i2c_stop（）;
returnr;
}
uint8_ti2c_readAck（）{
returni2c_read
（1）;
}
uint8_ti2c_readNak（void）{
returni2c_read（0）;
}
voidwaitTransmissionI2C（）{
uint16_tcount=255;
while（!
（TWCR&（1<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>0）;
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];fromtray=*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_rep_start（add<<1）;
i2c_write（reg）;
i2c_write（val）;
i2c_stop（）;
}//完整写入执行函数
uint8_ti2c_readReg（uint8_tadd,uint8_treg）{
uint8_tval;
i2c_read_reg_to_buf（add,reg,&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;axis<3;axis++）{
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>0）{
for（uint8_taxis=0;axis<3;axis++）{
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）>>9;
global_conf.accZero[PITCH]=（a[PITCH]+256）>>9;
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（）;
for（axis=0;axis<3;axis++）
gyroADCp[axis]=imu.gyroADC[axis];
timeInterleave=micros（）;
annexCode（）;
uint8_tt=0;
while（（uint16_t）（micros（）-timeInterleave）<650）t=1;
if（!
t）annex650_overrun_count++;
Gyro_getADC（）;
for（axis=0;axis<3;axis++）{
gyroADCinter[axis]=imu.gyroADC[axis]+gyroADCp[axis];
//采用经验值
imu.gyroData[axis]=（gyroADCinter[axis]+gyroADCprevious[axis]）/3;
gyroADCprevious[axis]=gyroADCinter[axis]>>1;
if（!
ACC）imu.accADC[axis]=0;
}
staticint16_tgyroSmooth[3]={0,0,0};
for（axis=0;axis<3;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
升级会员 