飞控MWC v22 代码解读Word文档格式.docx
- 文档编号:18041364
- 上传时间:2022-12-13
- 格式:DOCX
- 页数:22
- 大小:234.45KB
飞控MWC v22 代码解读Word文档格式.docx
《飞控MWC v22 代码解读Word文档格式.docx》由会员分享,可在线阅读,更多相关《飞控MWC v22 代码解读Word文档格式.docx(22页珍藏版)》请在冰豆网上搜索。
#ifdefined(SPEKTRUM)
if(spekFrameFlags==0x01)readSpektrum();
//支持的一种特殊遥控器读取数据
#endif
#ifdefined(OPENLRSv2MULTI)
Read_OpenLRS_RC();
//支持的一种特殊的遥控器读取数据
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
!
f.ARMED)
{//TurnofOkToarmtopreventthemotorsfromspinningafterrepoweringtheRXwithlowthrottleandauxtoarm
f.OK_TO_ARM=0;
//进入锁定状态,之后起飞需要解锁
failsafeCnt++;
//掉落保护计数+1每1代表20ms大于5倍FAILSAFE_DELAY则进入保护
//endoffailsaferoutine-nextchangeismadewithRcOptionssetting
//------------------STICKSCOMMANDHANDLER--------------------
//检测控制杆位置
uint8_tstTmp=0;
i<
4;
i++)
stTmp>
>
=2;
//stTmp除以4
if(rcData[i]>
MINCHECK)stTmp|=0x80;
//MINCHECK=110010000000B
if(rcData[i]<
MAXCHECK)stTmp|=0x40;
//MAXCHECK=190001000000B通过stTmp判断是否控制杆是否在最大最小之外
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
#ifdefALLOW_ARM_DISARM_VIA_TX_ROLL//上锁方式2
rcSticks==THR_LO+YAW_CE+PIT_CE+ROL_LO)go_disarm();
//DisarmviaROLL
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
#ifBARO
calibratingB=10;
//气压计设置基准气压(10*25ms=~250msnonblocking)
#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;
#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;
if(rcSticks==THR_LO+YAW_HI+PIT_HI+ROL_CE)//进入LCD配置
{
#ifdefined(LCD_CONF)
configurationLoop();
//beginningLCDconfiguration
设置时间//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
#ifdefALLOW_ARM_DISARM_VIA_TX_ROLL
rcSticks==THR_LO+YAW_CE+PIT_CE+ROL_HI)go_arm();
//ArmviaROLL
#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;
telemetry_auto=1;
#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();
LCDclear();
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;
//电子罗盘校准
时有用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;
}
elseif(rcSticks==THR_HI+YAW_CE+PIT_CE+ROL_HI){conf.angleTrim[ROLL]+=2;
elseif(rcSticks==THR_HI+YAW_CE+PIT_CE+ROL_LO){conf.angleTrim[ROLL]-=2;
if(i)
writeParams
(1);
rcDelayCommand=0;
//allowautorepetition
#ifdefined(LED_RING)//调整之后灯闪
blinkLedRing();
//灯闪烁使用IIC接口
#ifdefined(LED_FLASHER)
led_flasher_autoselect_sequence();
//仍在20MS循环中,LED闪烁
#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未运行
//设定校准时间50
}elseif(AccInflightCalibrationMeasurementDone&
f.ARMED){//若结束就保存
uint16_tauxState=0;
//测量辅助通道位置小于13001300到1700大于1700
auxState|=(rcData[AUX1+i]<
1300)<
<
(3*i)|(1300<
rcData[AUX1+i]&
rcData[AUX1+i]<
1700)<
(3*i+1)|
(rcData[AUX1+i]>
(3*i+2);
CHECKBOXITEMS;
rcOptions[i]=(auxState&
conf.activate[i])>
0;
//将辅助通道位置与选择的开启方式比较,保存开启的模式
//note:
ifFAILSAFEisdisable,failsafeCnt>
5*FAILSAFE_DELAYisalwaysfalse
#ifACC
if(rcOptions[BOXANGLE]||(failsafeCnt>
5*FAILSAFE_DELAY))//开启自稳模式anglemode
f.ANGLE_MODE)
f.ANGLE_MODE=1;
else//failsafe模式时的动作
f.ANGLE_MODE=0;
if(rcOptions[BOXHORIZON])//开启HORIZON模式rc选择
//关闭angle模式
f.HORIZON_MODE)//若horizon模式未开启
f.HORIZON_MODE=1;
//开启horizon模式
else//否则
模式horizon关闭//f.HORIZON_MODE=0;
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;
#if(!
defined(SUPPRESS_BARO_ALTHOLD))//若未宏定义SUPPRESS_BARO_ALTHOLD
if(rcOptions[BOXBARO])//rc若选择baro
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模式
#ifdefVARIOMETER//若定义了VARIOMETER
if(rcOptions[BOXVARIO])//rc若选择vario模式
f.VARIO_MODE)
f.VARIO_MODE=1;
//开启vario模式
}else//rc未选择VARIO模式
f.VARIO_MODE=0;
//关闭vario模式
#ifMAG//若配置了磁场传感器
if(rcOptions[BOXMAG])//开启磁场传感器与上面开启各种模式一样
f.MAG_MODE)
f.MAG_MODE=1;
magHold=heading;
}else
f.MAG_MODE=0;
if(rcOptions[BOXHEADFREE])//开启无头模式与上面开启各种模式一样
f.HEADFREE_MODE)
f.HEADFREE_MODE=1;
f.HEADFREE_MODE=0;
if(rcOptions[BOXHEADADJ])
headFreeModeHold=heading;
//acquirenewheading
staticuint8_tGPSNavReset=1;
if(f.GPS_FIX&
GPS_numSat>
=5)
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- 飞控MWC v22 代码解读 飞控 MWC 代码 解读
![提示](https://static.bdocx.com/images/bang_tan.gif)