飞控MWC v22 代码解读.docx
- 文档编号:5080682
- 上传时间:2022-12-13
- 格式:DOCX
- 页数:22
- 大小:234.45KB
飞控MWC v22 代码解读.docx
《飞控MWC v22 代码解读.docx》由会员分享,可在线阅读,更多相关《飞控MWC v22 代码解读.docx(22页珍藏版)》请在冰豆网上搜索。
飞控MWCv22代码解读
LOOP()
转载]MWCv2.2[代码解读
(2013-04-0720:
01:
27)
转载▼标签:
转载原文地址:
MWCv2.2代码解读LOOP()作者:
问江南
函数很长不用文字了贴个流程图,说明一切:
voidloop(){
staticuint8_trcDelayCommand;//thisindicatesthenumberoftime(multipleofRCmeasurementat50Hz)thesticksmustbe
maintainedtorunorswitchoffmotors
staticuint8_trcSticks;//thisholdstickspositionforcommandcombos
uint8_taxis,i;
int16_terror,errorAngle;
int16_tdelta,deltaSum;
int16_tPTerm,ITerm,DTerm;
int16_tPTermACC=0,ITermACC=0,PTermGYRO=0,ITermGYRO=0;
staticint16_tlastGyro[3]={0,0,0};
staticint16_tdelta1[3],delta2[3];
staticint16_terrorGyroI[3]={0,0,0};
staticint16_terrorAngleI[2]={0,0};
staticuint32_trcTime=0;
staticint16_tinitialThrottleHold;
staticuint32_ttimestamp_fixated=0;
#ifdefined(SPEKTRUM)
if(spekFrameFlags==0x01)readSpektrum();//支持的一种特殊遥控器读取数据
#endif
#ifdefined(OPENLRSv2MULTI)
Read_OpenLRS_RC();//支持的一种特殊的遥控器读取数据
#endif
if(currentTime>rcTime)//50Hz时间过了20ms
{
rcTime=currentTime+20000;
computeRC();//对已经接收的遥控接收的信号进行循环滤波,取4组数据,80MS,算平均值,大于平均值的减小2,小于平均值的增大2.
//Failsaferoutine-addedbyMIS
#ifdefined(FAILSAFE)
if(failsafeCnt>(5*FAILSAFE_DELAY)&&f.ARMED)//使之稳定,并设置油门到指定的值
{
for(i=0;i<3;i++)rcData[i]=MIDRC;//丢失信号(in0.1sec)后,把所有通道数据设置为MIDRC=1500
rcData[THROTTLE]=conf.failsafe_throttle;//把油门设置为conf.failsafe_throttle
if(failsafeCnt>5*(FAILSAFE_DELAY+FAILSAFE_OFF_DELAY))//在特定时间之后关闭电机(in0.1sec)
{
go_disarm();//Thiswillpreventthecoptertoautomaticallyrearmiffailsafeshutsitdownandprevents
进入锁定状态,之后起飞需要解锁f.OK_TO_ARM=0;//
}
failsafeEvents++;//掉落保护事件标志位至1
}
if(failsafeCnt>(5*FAILSAFE_DELAY)&&!
f.ARMED)
{//TurnofOkToarmtopreventthemotorsfromspinningafterrepoweringtheRXwithlowthrottleandauxtoarm
go_disarm();//Thiswillpreventthecoptertoautomaticallyrearmiffailsafeshutsitdownandprevents
f.OK_TO_ARM=0;//进入锁定状态,之后起飞需要解锁
}
failsafeCnt++;//掉落保护计数+1每1代表20ms大于5倍FAILSAFE_DELAY则进入保护
#endif
//endoffailsaferoutine-nextchangeismadewithRcOptionssetting
//------------------STICKSCOMMANDHANDLER--------------------
//检测控制杆位置
uint8_tstTmp=0;
for(i=0;i<4;i++)
{
stTmp>>=2;//stTmp除以4
if(rcData[i]>MINCHECK)stTmp|=0x80;//MINCHECK=110010000000B
if(rcData[i] } if(stTmp==rcSticks) { if(rcDelayCommand<250)rcDelayCommand++;//若控制杆在最大最小位置外的状态未改变(20ms内),则rcDelayCommand+1 } elsercDelayCommand=0;//否则清0 rcSticks=stTmp;//保存stTmp //采取行动 if(rcData[THROTTLE]<=MINCHECK)//油门在最低值 { errorGyroI[ROLL]=0;errorGyroI[PITCH]=0;errorGyroI[YAW]=0;//把rollpitchyaw误差置0 errorAngleI[ROLL]=0;errorAngleI[PITCH]=0; if(conf.activate[BOXARM]>0)//Arming/DisarmingviaARMBOX { if(rcOptions[BOXARM]&&f.OK_TO_ARM)go_arm();//解锁 elseif(f.ARMED)go_disarm();//上锁 } } if(rcDelayCommand==20)//若控制杆在最大最小位置外的状态未改变(20*20ms) { if(f.ARMED)//当处在解锁时 { #ifdefALLOW_ARM_DISARM_VIA_TX_YAW//上锁方式1 if(conf.activate[BOXARM]==0&&rcSticks==THR_LO+YAW_LO+PIT_CE+ROL_CE)go_disarm();//DisarmviaYAW #endif #ifdefALLOW_ARM_DISARM_VIA_TX_ROLL//上锁方式2 if(conf.activate[BOXARM]==0&&rcSticks==THR_LO+YAW_CE+PIT_CE+ROL_LO)go_disarm();//DisarmviaROLL #endif } else//当处在未解锁时 { i=0; if(rcSticks==THR_LO+YAW_LO+PIT_LO+ROL_CE)//GYRO(陀螺仪)校准 { calibratingG=512;//校准G512*20Ms #ifGPS GPS_reset_home_position();//GPS设置HOME #endif #ifBARO calibratingB=10;//气压计设置基准气压(10*25ms=~250msnonblocking) #endif } #ifdefined(INFLIGHT_ACC_CALIBRATION)//使得可以飞行中ACC校准(怎么手在抖。 。 ) elseif(rcSticks==THR_LO+YAW_LO+PIT_HI+ROL_HI)//InflightACCcalibrationSTART/STOP { if(AccInflightCalibrationMeasurementDone)//triggersavingintoeepromafterlanding { AccInflightCalibrationMeasurementDone=0; AccInflightCalibrationSavetoEEProm=1; } else { AccInflightCalibrationArmed=! AccInflightCalibrationArmed; #ifdefined(BUZZER) if(AccInflightCalibrationArmed)alarmArray[0]=2;elsealarmArray[0]=3; #endif } } #endif #ifdefMULTIPLE_CONFIGURATION_PROFILES//多配置文件读取 if(rcSticks==THR_LO+YAW_LO+PIT_CE+ROL_LO)i=1;//ROLLleft->Profile1//配置1 elseif(rcSticks==THR_LO+YAW_LO+PIT_HI+ROL_CE)i=2;//PITCHup->Profile2//配置2 elseif(rcSticks==THR_LO+YAW_LO+PIT_CE+ROL_HI)i=3;//ROLLright->Profile3//配置3 if(i) { global_conf.currentSet=i-1; writeGlobalSet(0); readEEPROM(); blinkLED(2,40,i); alarmArray[0]=i; } #endif if(rcSticks==THR_LO+YAW_HI+PIT_HI+ROL_CE)//进入LCD配置 { #ifdefined(LCD_CONF) configurationLoop();//beginningLCDconfiguration #endif 设置时间//previousTime=micros(); } #ifdefALLOW_ARM_DISARM_VIA_TX_YAW//允许使用YAW进行解锁 elseif(conf.activate[BOXARM]==0&&rcSticks==THR_LO+YAW_HI+PIT_CE+ROL_CE)go_arm();//ArmviaYAW #endif #ifdefALLOW_ARM_DISARM_VIA_TX_ROLL elseif(conf.activate[BOXARM]==0&&rcSticks==THR_LO+YAW_CE+PIT_CE+ROL_HI)go_arm();//ArmviaROLL #endif #ifdefLCD_TELEMETRY_AUTO//与LCD有关telemetry遥测 elseif(rcSticks==THR_LO+YAW_CE+PIT_HI+ROL_LO)//AutotelemetryON/OFF { if(telemetry_auto) { telemetry_auto=0; telemetry=0; } else telemetry_auto=1; } #endif #ifdefLCD_TELEMETRY_STEP elseif(rcSticks==THR_LO+YAW_CE+PIT_HI+ROL_HI)//Telemetrynextstep { telemetry=telemetryStepSequence[++telemetryStepIndex%strlen(telemetryStepSequence)]; #ifdefOLED_I2C_128x64 if(telemetry! =0)i2c_OLED_init(); #endif LCDclear(); } #endif elseif(rcSticks==THR_HI+YAW_LO+PIT_LO+ROL_CE)calibratingA=512;//加速度校准 elseif(rcSticks==THR_HI+YAW_HI+PIT_LO+ROL_CE)f.CALIBRATE_MAG=1;//电子罗盘校准 i=0; 时有用PID在计算角度矫正(rcSticks==THR_HI+YAW_CE+PIT_HI+ROL_CE){conf.angleTrim[PITCH]+=2;i=1;}//if elseif(rcSticks==THR_HI+YAW_CE+PIT_LO+ROL_CE){conf.angleTrim[PITCH]-=2;i=1;} elseif(rcSticks==THR_HI+YAW_CE+PIT_CE+ROL_HI){conf.angleTrim[ROLL]+=2;i=1;} elseif(rcSticks==THR_HI+YAW_CE+PIT_CE+ROL_LO){conf.angleTrim[ROLL]-=2;i=1;} if(i) { writeParams (1); rcDelayCommand=0;//allowautorepetition #ifdefined(LED_RING)//调整之后灯闪 blinkLedRing();//灯闪烁使用IIC接口 #endif } } } #ifdefined(LED_FLASHER) led_flasher_autoselect_sequence();//仍在20MS循环中,LED闪烁 #endif #ifdefined(INFLIGHT_ACC_CALIBRATION)//空中校准ACC if(AccInflightCalibrationArmed&&f.ARMED&&rcData[THROTTLE]>MINCHECK&&! rcOptions[BOXARM]){//Copteris airborneandyouareturningitoffviaboxarm: startmeasurement InflightcalibratingA=50; AccInflightCalibrationArmed=0; } if(rcOptions[BOXCALIB]){//使用Calib来标定: Calib=TRUE测试开始,降落且Calib=0测量储存 if(! AccInflightCalibrationActive&&! AccInflightCalibrationMeasurementDone){//若空中校准ACC未运行 InflightcalibratingA=50;//设定校准时间50 } }elseif(AccInflightCalibrationMeasurementDone&&! f.ARMED){//若结束就保存 AccInflightCalibrationMeasurementDone=0; AccInflightCalibrationSavetoEEProm=1; } #endif uint16_tauxState=0;//测量辅助通道位置小于13001300到1700大于1700 for(i=0;i<4;i++) auxState|=(rcData[AUX1+i]<1300)<<(3*i)|(1300 (rcData[AUX1+i]>1700)<<(3*i+2); for(i=0;i rcOptions[i]=(auxState&conf.activate[i])>0;//将辅助通道位置与选择的开启方式比较,保存开启的模式 //note: ifFAILSAFEisdisable,failsafeCnt>5*FAILSAFE_DELAYisalwaysfalse #ifACC if(rcOptions[BOXANGLE]||(failsafeCnt>5*FAILSAFE_DELAY))//开启自稳模式anglemode { if(! f.ANGLE_MODE) { errorAngleI[ROLL]=0;errorAngleI[PITCH]=0; f.ANGLE_MODE=1; } } else//failsafe模式时的动作 { f.ANGLE_MODE=0; } if(rcOptions[BOXHORIZON])//开启HORIZON模式rc选择 { f.ANGLE_MODE=0;//关闭angle模式 if(! f.HORIZON_MODE)//若horizon模式未开启 { errorAngleI[ROLL]=0;errorAngleI[PITCH]=0; f.HORIZON_MODE=1;//开启horizon模式 } } else//否则 { 模式horizon关闭//f.HORIZON_MODE=0; } #endif if(rcOptions[BOXARM]==0)f.OK_TO_ARM=1; #if! defined(GPS_LED_INDICATOR) if(f.ANGLE_MODE||f.HORIZON_MODE){STABLEPIN_ON;}else{STABLEPIN_OFF;} #endif #ifBARO #if(! defined(SUPPRESS_BARO_ALTHOLD))//若未宏定义SUPPRESS_BARO_ALTHOLD if(rcOptions[BOXBARO])//rc若选择baro { if(! f.BARO_MODE)//若baro模式未开启 { f.BARO_MODE=1;//开启baro模式气压计定高 AltHold=EstAlt; initialThrottleHold=rcCommand[THROTTLE];//储存此时rc油门输出值 errorAltitudeI=0;//重置PID输出和高度误差 BaroPID=0; } }else//若RC未选择BARO模式 { f.BARO_MODE=0;//关闭baro模式 } #endif #ifdefVARIOMETER//若定义了VARIOMETER if(rcOptions[BOXVARIO])//rc若选择vario模式 { if(! f.VARIO_MODE) { f.VARIO_MODE=1;//开启vario模式 } }else//rc未选择VARIO模式 { f.VARIO_MODE=0;//关闭vario模式 } #endif #endif #ifMAG//若配置了磁场传感器 if(rcOptions[BOXMAG])//开启磁场传感器与上面开启各种模式一样 { if(! f.MAG_MODE) { f.MAG_MODE=1; magHold=heading; } }else { f.MAG_MODE=0; } if(rcOptions[BOXHEADFREE])//开启无头模式与上面开启各种模式一样 { if(! f.HEADFREE_MODE) { f.HEADFREE_MODE=1; } } else { f.HEADFREE_MODE=0; } if(rcOptions[BOXHEADADJ]) { headFreeModeHold=heading;//acquirenewheading } #endif #ifGPS staticuint8_tGPSNavReset=1; if(f.GPS_FIX&&GPS_numSat>=5) {
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- 飞控MWC v22 代码解读 飞控 MWC 代码 解读