基于Arduino的四轴飞行器.docx

基于Arduino的四轴飞行器.docx

《基于Arduino的四轴飞行器.docx》由会员分享，可在线阅读，更多相关《基于Arduino的四轴飞行器.docx（246页珍藏版）》请在冰豆网上搜索。

基于基于Arduino的四轴飞行器的四轴飞行器第2章基于Arduino的四轴飞行器2.3.3模块介绍1、电机输出模块

（2）相关代码/主要函数定义externuint8_tPWM_PIN8;voidinitOutput（）;/初始化函数voidmixTable（）;/PID计算voidwriteMotors（）;/信号输出到电机/输出程序uint8_tPWM_PIN8=9,10,11,3,6,5,A2,12;/定义输出接口voidinitOutput（）for（uint8_ti=0;i4;i+）pinMode（PWM_PINi,OUTPUT）;/初始化，使PWM引脚作为输出引脚voidmixTable（）int16_tmaxMotor;/定义最大转速电机编号uint8_ti;#definePIDMIX（X,Y,Z）rcCommandTHROTTLE+axisPIDROLL*X+axisPIDPITCH*Y+YAW_DIRECTION*axisPIDYAW*Z/PID算法motor0=PIDMIX（-1,+1,-1）;motor1=PIDMIX（-1,-1,+1）;motor2=PIDMIX（+1,+1,+1）;motor3=PIDMIX（+1,-1,-1）;/4个电机输出计算（PID）maxMotor=motor0;/以下代码限制最大输出油门，防止异常for（i=1;imaxMotor）maxMotor=motori;for（i=0;iMAXTHROTTLE）/保证当某一个油门达到最大时，陀螺仪仍有良好的信号连接motori-=maxMotor-MAXTHROTTLE;motori=constrain（motori,conf.minthrottle,MAXTHROTTLE）;if（rcDataTHROTTLE3;/pin9输出电机1号OCR1B=motor13;/pin10输出电机2号OCR2A=motor23;/pin11输出电机3号OCR2B=motor33;/pin3输出电机4号voidwriteAllMotors（int16_tmc）/所有电机输出设定为mcfor（uint8_ti=0;i4;i+）motori=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）;#endifexit;#endif*/2、遥控器发送/接收模块

（2）相关代码/RX.h主要函数定义#includeArduino.h#defineRC_CHANS8#definePCINT_PIN_COUNT5#definePCINT_RX_PORTPORTB#definePCINT_RX_MASKPCMSK0#definePCIR_PORT_BIT（10）#defineRX_PC_INTERRUPTPCINT0_vect#defineRX_PCINT_PIN_PORTPINB#defineROLLPIN4/预定义各信道的名称#defineTHROTTLEPIN3#definePITCHPIN5#defineYAWPIN2#defineAUX1PIN6#defineAUX2PIN7#defineAUX3PIN1/保留#defineAUX4PIN0/保留#definePCINT_RX_BITS（11）,（12）,（13）,（14）,（10）voidconfigureReceiver（）;voidcomputeRC（）;uint16_treadRawRC（uint8_tchan）;/初始信号读取函数/接收代码volatileuint16_trcValueRC_CHANS=1502,1502,1502,1502,1502,1502,1502,1502;staticuint8_trcChannelRC_CHANS=ROLLPIN,PITCHPIN,YAWPIN,THROTTLEPIN,AUX1PIN,AUX2PIN,AUX3PIN,AUX4PIN;staticuint8_tPCInt_RX_PinsPCINT_PIN_COUNT=PCINT_RX_BITS;voidconfigureReceiver（）for（uint8_ti=0;iPCINT_PIN_COUNT;i+）PCINT_RX_PORT|=PCInt_RX_Pinsi;PCINT_RX_MASK|=PCInt_RX_Pinsi;PCICR=PCIR_PORT_BIT;PCICR|=（10）;#defineRX_PIN_CHECK（pin_pos,rc_value_pos）if（mask&PCInt_RX_Pinspin_pos）if（!

（pin&PCInt_RX_Pinspin_pos）dTime=cTime-edgeTimepin_pos;if（900dTime&dTime0）RX_PIN_CHECK（0,2）;#endif#if（PCINT_PIN_COUNT1）RX_PIN_CHECK（1,4）;#endif#if（PCINT_PIN_COUNT2）RX_PIN_CHECK（2,5）;#endif#if（PCINT_PIN_COUNT3）RX_PIN_CHECK（3,6）;#endif#if（PCINT_PIN_COUNT4）RX_PIN_CHECK（4,7）;#endif#if（PCINT_PIN_COUNT5）RX_PIN_CHECK（5,0）;#endif#if（PCINT_PIN_COUNT6）RX_PIN_CHECK（6,1）;#endif#if（PCINT_PIN_COUNT7）RX_PIN_CHECK（7,3）;#endifISR（PCINT0_vect）uint8_tpin;uint16_tcTime,dTime;staticuint16_tedgeTime;pin=PINB;cTime=micros（）;sei（）;dTime=cTime-edgeTime;if（900dTime&dTime2200）rcValue0=dTime;elseedgeTime=cTime;/如果bit2在高电平（PPMpulse上升）,存储时间uint16_treadRawRC（uint8_tchan）/读取原始信号uint16_tdata;uint8_toldSREG;oldSREG=SREG;cli（）;/禁止中断data=rcValuercChannelchan;SREG=oldSREG;returndata;voidcomputeRC（）/提取遥控器信号，进行数据处理，主要取平均值staticuint16_trcData4ValuesRC_CHANS4,rcDataMeanRC_CHANS;staticuint8_trc4ValuesIndex=0;uint8_tchan,a;rc4ValuesIndex+;if（rc4ValuesIndex=4）rc4ValuesIndex=0;for（chan=0;chanRC_CHANS;chan+）rcData4Valueschanrc4ValuesIndex=readRawRC（chan）;rcDataMeanchan=0;for（a=0;a2;if（rcDataMeanchan（uint16_t）rcDatachan+3）rcDatachan=rcDataMeanchan-2;if（chan0）rcSerialCount-;if（rcSerialchan900）rcDatachan=rcSerialchan;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/设置低频滤波系数为4voidi2c_BMP085_UT_Start（void）;voidwaitTransmissionI2C（）;voidi2c_MS561101BA_UT_Start（）;voidDevice_Mag_getADC（）;voidBaro_init（）;/三个初始化函数voidMag_init（）;voidACC_init（）;uint8_trawADC6;staticuint32_tneutralizeTime=0;voidi2c_init（void）TWSR=0;TWBR=（F_CPU/I2C_SPEED）-16）/2;TWCR=1TWEN;voidi2c_rep_start（uint8_taddress）/传感器开始读取TWCR=（1TWINT）|（1TWSTA）|（1TWEN）;waitTransmissionI2C（）;TWDR=address;TWCR=（1TWINT）|（1TWEN）;waitTransmissionI2C（）;voidi2c_stop（void）/传感器结束读取TWCR=（1TWINT）|（1TWEN）|（1TWSTO）;voidi2c_write（uint8_tdata）/TWDR=data;TWCR=（1TWINT）|（1TWEN）;waitTransmissionI2C（）;uint8_ti2c_read（uint8_tack）/读取函数TWCR=（1TWINT）|（1TWEN）|（ack?

（1TWEA）:

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&（1TWINT）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（add0）;bytes_read+;returnbytes_read;size_ti2c_read_reg_to_buf（uint8_tadd,uint8_treg,void*buf,size_tsize）i2c_rep_start（add1）;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=&fromsize-1;fromto;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_rep_start（add0）for（axis=0;axis9;blinkLED（10,15,1）;calibratingG-；for（axis=0;axis0）for（uint8_taxis=0;axis9;global_conf.accZeroPITCH=（aPITCH+256）9;global_conf.accZeroYAW=（aYAW+256）9）-ACC_1G;conf.angleTrimROLL=0;conf.angleTrimPITCH=0;writeGlobalSet

（1）;calibratingA-;imu.accADCROLL-=global_conf.accZeroROLL;imu.accADCPITCH-=global_conf.accZeroPITCH;imu.accADCYAW-=global_conf.accZeroYAW;4、姿态计算函数相关代码/姿态算法IUM.h文件#defineBARO_TAB_SIZE21voidcomputeIMU（）;/IUM.cppvoidgetEstimatedAttitude（）;voidcomputeIMU（）/姿态计算函数uint8_taxis;staticint16_tgyroADCprevious3=0,0,0;int16_tgyroADCp3;int16_tgyroADCinter3;staticuint32_ttimeInterleave=0;ACC_getADC（）;getEstimatedAttitude（）;Gyro_getADC（）;for（axis=0;axis3;axis+）gyroADCpaxis=imu.gyroADCaxis;timeInterleave=micros（）;annexCode（）;uint8_tt=0;while（uint16_t）（micros（）-timeInterleave）650）t=1;if（!

t）annex650_overrun_count+;Gyro_getADC（）;for（axis=0;axis1;if（!

ACC）imu.accADCaxis=0;staticint16_tgyroSmooth3=0,0,0;for（axis=0;axis3;axis+）imu.gyroDataaxis=（int16_t）（int32_t）（int32_t）gyroSmoothaxis*（conf.Smoothingaxis-1）+imu.gyroDataaxis+1）/conf.Smoothingaxis）;gyroSmoothaxis=imu.gyroDataaxis;/获得陀螺仪数据#defineGYR_CMPFM_FACTOR250#defineINV_GYR_CMPF_FACTOR（1.0f/（GYR_CMPF_FACTOR+1.0f）#defineINV_GYR_CMPFM_FACTOR（1.0f/（GYR_CMPFM_FACTOR+1.0f）typedefstructfp_vectorfloatX,Y,Z;t_fp_vector_def;typedefunionfloatA3;t_fp_vector_defV;t_fp_vector;typedefstructint32_t_vectorint32_tX,Y,Z;t_int32_t_vector_def;typedefunionint32_tA3;t_int32_t_vector_defV;t_int32_t_vector;int16_t_atan2（int32_ty,int32_tx）floatz=（float）y/x;int16_ta;if（abs（y）abs（x）a=573*z/（1.0f+0.28f*z*z）;if（x0）if（y0）a-=1800;elsea+=1800;elsea=900-573*z/（z*z+0.28f）;if（y1）;return0.5f*conv.f*（3.0f-x*conv.f*conv.f）;voidrotateV（structfp_vector*v,float*delta）fp_vectorv_tmp=*v;v-Z-=deltaROLL*v_tmp.X+deltaPITCH*v_tmp.Y;v-X+=deltaROLL*v_tmp.Z-deltaYAW*v_tmp.Y;v-Y+=deltaPITCH*v_tmp.Z+deltaYAW*v_tmp.X;/xyz三轴的旋转角度计算staticint32_taccLPF323=0,0,1;staticfloatinvG;statict_fp_vectorEstG;statict_int32_t_vectorEstG32;voidgetEstimatedAttitude（）uint8_taxis;int32_taccMag=0;floatscale,deltaGyroAngle3;uint8_tvalidAcc;staticuint16_tpreviousT;uint16_tcurrentT=micros（）;scale=（currentT-previousT）*GYRO_SCALE;previousT=currentT;/初始化for（axis=0;axisACC_LPF_FACTOR;accLPF32axis+=imu.accADCaxis;imu.accSmoothaxis=accLPF32axisACC_LPF_FACTOR;accMag+=（int32_t）imu.accSmoothaxis*imu.accSmoothaxis;rotateV（&EstG.V,deltaGyroAngle）;accMag=accMag*100/（int32_t）ACC_1G*ACC_1G）;validAcc=72（uint16_t）accMag&（uint16_t）accMag133;for（axis=0;axisACCZ_25deg）f.SMALL_ANGLES_25=1;elsef.SMALL_ANGLES_25=0;int32_tsqGX_sqGZ=sq（EstG32.V.X）+sq（EstG32.V.Z）;invG=InvSqrt（sqGX_sqGZ+sq（EstG32.V.Y）;att.angleROLL=_atan2（EstG32.V.X,EstG32.V.Z）;att.anglePITCH=_atan2（EstG32.V.Y,InvSqrt（sqGX_sqGZ）*sqGX_sqGZ）;5、主函数相关代码/control.cppvoidsetup（）/setup函数LEDPIN_PINMODE;POWERPIN_PINMODE;BUZZERPIN_PINMODE;STABLEPIN_PINMODE;POWERPIN_OFF;initOutput（）;readGlobalSet（）;uint16_ti=65000;uint16_tflashsum=0;uint8_tpbyt;while（i-）pbyt=pgm_read_byte（i）;/计算字节flashsum+=pbyt;flashsum=（pbytrcTime）/50HzrcTime=currentTime+20000;computeRC（）;uint8_tstTmp=0;for（i=0;i=2;if（rcDataiMINCHECK）stTmp|=0x80;/飞行器解锁赋值if（rcDataiMAXCHECK）stTmp|=0x40;/飞行器上锁赋值if（stTmp=rcSticks）if（rcDelayCommand250）rcDelayCommand+;elsercDelayCommand=0;rcSticks=stTmp;if（rcDataTHROTTLE0）if（rcOptionsBOXARM&f.OK_TO_ARM）go_arm（）;elseif（f.ARMED）go_disarm（）;if（rcDelayCommand=20）if（f.ARMED）if（conf.activateBOXARM=0&rcSticks=THR_LO+YAW_LO+PIT_CE+ROL_CE）go_disarm（）;/解锁函数else/未解锁的情况i=0;if（rcSticks=THR_LO+YAW_LO+PIT_LO+ROL_CE）calibratingG=512;computeIMU（）;/计算当前的姿态currentTime=micros（）;cycleTime=currentTime-previousTime;previousTime=currentTime;if（f.HORIZON_MODE）prop=min（max（abs（rcCommandPITCH）,abs（rcCommandROLL）,512）;for（axis=0;axis2;axis+）rc=rcCommandaxis640）errorGyroIaxis=0;ITerm=（errorGyroIaxis7）*conf.pidaxis.I86;PTerm=（int32_t）rc*conf.pidaxis.P86;if（f.ANGLE_MODE）/自稳定模块算法errorAngle=constrain（rc+GPS_angleaxis,-500,+500）-att.angleaxis+conf.angleTrimaxis;errorAngleIaxis=constrain（errorAngleIaxis+errorAngle,-10000,+10000）;PTermACC=（int32_t）errorAngle*conf.pidPIDLEVEL.P8）7;int16_tlimit=conf.pidPIDLEVEL.D8*5;PTermACC=constrain（PTermACC,-limit,+limit）;ITermACC=（int32_t）errorAngleIaxis*conf.pidPIDLEVEL.I8）12;ITerm=ITermACC+（ITerm-ITermACC）*prop9）;PTerm=PTermACC+（PTerm-PTermACC）*prop9）;PTerm-=（int32_t）imu.gyroDataaxis*dynP8axis）6;delta=imu.gyroDataaxis-lastGyroaxis;lastGyroaxis=imu.gyroDataaxis;DTerm=delta1axis+delta2axis+delta;delta2axis=delta1axis;delta1axis=delta;DTerm=（int32_t）DTerm*dynD8axis）5;/计算需要32位axisPIDaxis=PTerm+ITerm-DTerm;#defineGYRO_P_MAX300#defineGYRO_I_MAX250rc=（int32_t）rcCommandYA