基于Arduino的四轴飞行器.docx
- 文档编号:29310134
- 上传时间:2023-07-22
- 格式:DOCX
- 页数:289
- 大小:90.63KB
基于Arduino的四轴飞行器.docx
《基于Arduino的四轴飞行器.docx》由会员分享,可在线阅读,更多相关《基于Arduino的四轴飞行器.docx(289页珍藏版)》请在冰豆网上搜索。
基于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;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 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 #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(900 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 rcData4Values[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];from 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_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_F
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- 基于 Arduino 飞行器