《MWC_v2.2_代码解读.doc》由会员分享，可在线阅读，更多相关《MWC_v2.2_代码解读.doc（19页珍藏版）》请在三一文库上搜索。
1、转载MWCv2.2代码解读LOOP() (2013-04-07 20:01:27)转载标签：转载原文地址：MWCv2.2代码解读LOOP()作者：问江南函数很长不用文字了 贴个流程图，说明一切：void loop () static uint8_t rcDelayCommand; / this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motorsstatic uint8_t rcSticks; / th
2、is hold sticks position for command combosuint8_t axis,i;int16_t error,errorAngle;int16_t delta,deltaSum;int16_t PTerm,ITerm,DTerm;int16_t PTermACC = 0 , ITermACC = 0 , PTermGYRO = 0 , ITermGYRO = 0;static int16_t lastGyro3 = 0,0,0;static int16_t delta13,delta23;static int16_t errorGyroI3 = 0,0,0;st
3、atic int16_t errorAngleI2 = 0,0;static uint32_t rcTime = 0;static int16_t initialThrottleHold;static uint32_t timestamp_fixated = 0;#if defined(SPEKTRUM)if (spekFrameFlags = 0x01) readSpektrum(); /支持的一种特殊遥控器 读取数据#endif#if defined(OPENLRSv2MULTI)Read_OpenLRS_RC(); /支持的一种特殊的遥控器 读取数据#endifif (currentTi
4、me rcTime ) / 50Hz 时间过了20msrcTime = currentTime + 20000;computeRC(); /对已经接收的遥控接收的信号进行循环滤波，取4组数据，80MS，算平均值，大于平均值的减小2，小于平均值的增大2./ Failsafe routine - added by MIS#if defined(FAILSAFE)if ( failsafeCnt (5*FAILSAFE_DELAY) & f.ARMED) / 使之稳定, 并设置油门到指定的值 for(i=0; i 5*(FAILSAFE_DELAY+FAILSAFE_OFF_DELAY) / 在特定
5、时间之后关闭电机 (in 0.1sec) go_disarm(); / This will prevent the copter to automatically rearm if failsafe shuts it down and preventsf.OK_TO_ARM = 0; / 进入锁定状态，之后起飞需要解锁failsafeEvents+; /掉落保护事件标志位至1if ( failsafeCnt (5*FAILSAFE_DELAY) & !f.ARMED) /Turn of Ok To arm to prevent the motors from spinning after re
6、powering the RX with low throttle and aux to armgo_disarm(); / This will prevent the copter to automatically rearm if failsafe shuts it down and preventsf.OK_TO_ARM = 0; /进入锁定状态，之后起飞需要解锁failsafeCnt+; /掉落保护计数+1 每1 代表20ms 大于5倍FAILSAFE_DELAY 则进入保护#endif/ end of failsafe routine - next change is made wi
7、th RcOptions setting/ - STICKS COMMAND HANDLER -/ 检测控制杆位置uint8_t stTmp = 0;for(i=0;i= 2; /stTmp除以4if(rcDatai MINCHECK) stTmp |= 0x80; / MINCHECK=1100 1000 0000Bif(rcDatai MAXCHECK) stTmp |= 0x40; / MAXCHECK=1900 0100 0000B 通过stTmp判断是否控制杆是否在最大最小之外if(stTmp = rcSticks)if(rcDelayCommand250) rcDelayComma
8、nd+; /若控制杆在最大最小位置外的状态未改变（20ms内），则rcDelayCommand+1else rcDelayCommand = 0; /否则清0rcSticks = stTmp; /保存stTmp/ 采取行动 if (rcDataTHROTTLE 0)/ Arming/Disarming via ARM BOX if ( rcOptionsBOXARM & f.OK_TO_ARM ) go_arm(); /解锁else if (f.ARMED) go_disarm(); /上锁if(rcDelayCommand = 20) /若控制杆在最大最小位置外的状态未改变(20*20ms)
9、if(f.ARMED) / 当处在解锁时 #ifdef ALLOW_ARM_DISARM_VIA_TX_YAW /上锁方式1if (conf.activateBOXARM = 0 & rcSticks = THR_LO + YAW_LO + PIT_CE + ROL_CE) go_disarm(); / Disarm via YAW#endif#ifdef ALLOW_ARM_DISARM_VIA_TX_ROLL /上锁方式2if (conf.activateBOXARM = 0 & rcSticks = THR_LO + YAW_CE + PIT_CE + ROL_LO) go_disarm
10、(); / Disarm via ROLL#endifelse/ 当处在未解锁时 i=0;if (rcSticks = THR_LO + YAW_LO + PIT_LO + ROL_CE) / GYRO（陀螺仪） 校准 calibratingG=512; /校准G 512*20Ms#if GPSGPS_reset_home_position(); /GPS 设置HOME#endif#if BAROcalibratingB=10; / 气压计设置基准气压(10 * 25 ms = 250 ms non blocking)#endif#if defined(INFLIGHT_ACC_CALIBRA
11、TION) /使得可以飞行中ACC校准 （怎么手在抖。）else if (rcSticks = THR_LO + YAW_LO + PIT_HI + ROL_HI)/ Inflight ACC calibration START/STOP if (AccInflightCalibrationMeasurementDone)/ trigger saving into eeprom after landing AccInflightCalibrationMeasurementDone = 0;AccInflightCalibrationSavetoEEProm = 1;elseAccInfligh
12、tCalibrationArmed = !AccInflightCalibrationArmed;#if defined(BUZZER)if (AccInflightCalibrationArmed) alarmArray0=2; else alarmArray0=3;#endif#endif#ifdef MULTIPLE_CONFIGURATION_PROFILES /多配置文件读取if (rcSticks = THR_LO + YAW_LO + PIT_CE + ROL_LO) i=1; / ROLL left - Profile 1 /配置1else if (rcSticks = THR
13、_LO + YAW_LO + PIT_HI + ROL_CE) i=2; / PITCH up - Profile 2 /配置2else if (rcSticks = THR_LO + YAW_LO + PIT_CE + ROL_HI) i=3; / ROLL right - Profile 3 /配置3if(i)global_conf.currentSet = i-1;writeGlobalSet(0);readEEPROM();blinkLED(2,40,i);alarmArray0 = i;#endifif(rcSticks = THR_LO + YAW_HI + PIT_HI + RO
14、L_CE) / 进入LCD配置 #if defined(LCD_CONF)configurationLoop(); / beginning LCD configuration#endifpreviousTime = micros(); /设置时间#ifdef ALLOW_ARM_DISARM_VIA_TX_YAW /允许使用YAW进行解锁else if (conf.activateBOXARM = 0 & rcSticks = THR_LO + YAW_HI + PIT_CE + ROL_CE) go_arm(); / Arm via YAW#endif#ifdef ALLOW_ARM_DIS
15、ARM_VIA_TX_ROLLelse if (conf.activateBOXARM = 0 & rcSticks = THR_LO + YAW_CE + PIT_CE + ROL_HI) go_arm(); / Arm via ROLL#endif#ifdef LCD_TELEMETRY_AUTO /与LCD有关telemetry 遥测else if (rcSticks = THR_LO + YAW_CE + PIT_HI + ROL_LO) / Auto telemetry ON/OFF if (telemetry_auto)telemetry_auto = 0;telemetry =
16、0;elsetelemetry_auto = 1;#endif#ifdef LCD_TELEMETRY_STEPelse if (rcSticks = THR_LO + YAW_CE + PIT_HI + ROL_HI) / Telemetry next step telemetry = telemetryStepSequence+telemetryStepIndex % strlen(telemetryStepSequence);#ifdef OLED_I2C_128x64if (telemetry != 0) i2c_OLED_init();#endifLCDclear();#endife
17、lse if (rcSticks = THR_HI + YAW_LO + PIT_LO + ROL_CE) calibratingA=512; /加速度校准else if (rcSticks = THR_HI + YAW_HI + PIT_LO + ROL_CE) f.CALIBRATE_MAG = 1; /电子罗盘校准 i=0;if (rcSticks = THR_HI + YAW_CE + PIT_HI + ROL_CE) conf.angleTrimPITCH+=2; i=1; /角度矫正 在计算PID时有用else if (rcSticks = THR_HI + YAW_CE + PI
18、T_LO + ROL_CE) conf.angleTrimPITCH-=2; i=1;else if (rcSticks = THR_HI + YAW_CE + PIT_CE + ROL_HI) conf.angleTrimROLL +=2; i=1;else if (rcSticks = THR_HI + YAW_CE + PIT_CE + ROL_LO) conf.angleTrimROLL -=2; i=1;if (i)writeParams(1);rcDelayCommand = 0; / allow autorepetition#if defined(LED_RING) /调整之后灯
19、闪blinkLedRing(); /灯闪烁 使用IIC接口#endif#if defined(LED_FLASHER)led_flasher_autoselect_sequence(); /仍在20MS循环中，LED闪烁#endif#if defined(INFLIGHT_ACC_CALIBRATION) /空中校准ACCif (AccInflightCalibrationArmed & f.ARMED & rcDataTHROTTLE MINCHECK & !rcOptionsBOXARM ) / Copter is airborne and you are turning it off v
20、ia boxarm : start measurementInflightcalibratingA = 50;AccInflightCalibrationArmed = 0; if (rcOptionsBOXCALIB) / 使用Calib来标定: Calib = TRUE 测试开始, 降落 且 Calib = 0 测量储存if (!AccInflightCalibrationActive & !AccInflightCalibrationMeasurementDone) /若空中校准ACC未运行InflightcalibratingA = 50; /设定校准时间 50else if(AccI
21、nflightCalibrationMeasurementDone & !f.ARMED) /若结束 就保存AccInflightCalibrationMeasurementDone = 0;AccInflightCalibrationSavetoEEProm = 1; #endifuint16_t auxState = 0; /测量辅助通道位置 小于1300 1300到1700 大于1700for(i=0;i4;i+) auxState |= (rcDataAUX1+i1300)(3*i) | (1300rcDataAUX1+i & rcDataAUX1+i1700)1700)(3*i+2)
22、;for(i=0;i0; /将辅助通道位置与选择的开启方式比较，保存开启的模式/ note: if FAILSAFE is disable, failsafeCnt 5*FAILSAFE_DELAY is always false#if ACCif ( rcOptionsBOXANGLE | (failsafeCnt 5*FAILSAFE_DELAY) )/ 开启自稳模式anglemodeif (!f.ANGLE_MODE)errorAngleIROLL = 0; errorAngleIPITCH = 0;f.ANGLE_MODE = 1; else/ failsafe模式时的动作f.ANGL
23、E_MODE = 0;if ( rcOptionsBOXHORIZON )/开启HORIZON模式 rc选择 f.ANGLE_MODE = 0; /关闭angle模式if (!f.HORIZON_MODE) /若horizon模式未开启errorAngleIROLL = 0; errorAngleIPITCH = 0;f.HORIZON_MODE = 1; /开启horizon模式else /否则f.HORIZON_MODE = 0; /关闭horizon模式#endifif (rcOptionsBOXARM = 0) f.OK_TO_ARM = 1;#if !defined(GPS_LED_
24、INDICATOR)if (f.ANGLE_MODE | f.HORIZON_MODE) STABLEPIN_ON; else STABLEPIN_OFF;#endif#if BARO#if (!defined(SUPPRESS_BARO_ALTHOLD) /若未宏定义SUPPRESS_BARO_ALTHOLDif (rcOptionsBOXBARO) /rc 若选择baroif (!f.BARO_MODE) /若baro模式未开启f.BARO_MODE = 1; /开启baro模式 气压计定高AltHold = EstAlt;initialThrottleHold = rcCommandTH
25、ROTTLE; /储存此时 rc 油门输出值errorAltitudeI = 0; /重置PID输出 和高度误差BaroPID=0; else /若RC未选择BARO模式f.BARO_MODE = 0; /关闭baro模式 #endif#ifdef VARIOMETER /若定义了VARIOMETERif (rcOptionsBOXVARIO) /rc 若选择vario模式if (!f.VARIO_MODE)f.VARIO_MODE = 1; /开启vario模式 else /rc未选择VARIO模式f.VARIO_MODE = 0; /关闭vario模式#endif#endif#if MAG
26、 /若配置了磁场传感器if (rcOptionsBOXMAG) /开启磁场传感器 与上面开启各种模式一样if (!f.MAG_MODE)f.MAG_MODE = 1;magHold = heading; elsef.MAG_MODE = 0;if (rcOptionsBOXHEADFREE)/开启无头模式与上面开启各种模式一样if (!f.HEADFREE_MODE)f.HEADFREE_MODE = 1;elsef.HEADFREE_MODE = 0;if (rcOptionsBOXHEADADJ)headFreeModeHold = heading; / acquire new headi
27、ng#endif#if GPSstatic uint8_t GPSNavReset = 1;if (f.GPS_FIX & GPS_numSat = 5 )if (rcOptionsBOXGPSHOME)/若GPS_HOME 和 GPS_HOLD 都被选择了额 GPS_HOME 具有优先权if (!f.GPS_HOME_MODE) f.GPS_HOME_MODE = 1;f.GPS_HOLD_MODE = 0;GPSNavReset = 0;#if defined(I2C_GPS)GPS_I2C_command(I2C_GPS_COMMAND_START_NAV,0); /waypoint z
28、ero#else / 串口GPS_set_next_wp(&GPS_homeLAT,&GPS_homeLON);nav_mode = NAV_MODE_WP;#endif else f.GPS_HOME_MODE = 0;if (rcOptionsBOXGPSHOLD & abs(rcCommandROLL) AP_MODE & abs(rcCommandPITCH) AP_MODE) if (!f.GPS_HOLD_MODE) f.GPS_HOLD_MODE = 1;GPSNavReset = 0;#if defined(I2C_GPS)GPS_I2C_command(I2C_GPS_COMMAND_POSHOLD,0);#elseGPS_holdLAT = GPS_coordLAT;GPS_holdLON = GPS_coordLON;GPS_set_next_wp(&GPS_holdLAT,&GPS_holdLON);nav_mode = NAV_MODE_POSHOLD;#endif else f.GPS_HOLD_MODE = 0;/ both boxes are