MWC_v2.2_代码解读

1、精选文档转载MWC v2.2 代码解读LOOP()  (2013-04-07 20:01:27)转载标签： 转载原文地址：MWC v2.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 r

2、un or switch off motors  static uint8_t rcSticks;       / this hold sticks position for command combos  uint8_t axis,i;  int16_t error,errorAngle;  int16_t delta,deltaSum;  int16_t PTerm,ITerm,DTerm;  int16_t P

3、TermACC = 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;  static int16_t errorAngleI2 = 0,0;  static uint32_t rcTime  = 0;  sta

4、tic int16_t initialThrottleHold;  static uint32_t timestamp_fixated = 0;  #if defined(SPEKTRUM)    if (spekFrameFlags = 0x01) readSpektrum();          /支持的一种特殊遥控器 读取数据  #endif    #if de

5、fined(OPENLRSv2MULTI)     Read_OpenLRS_RC();                                       &#

6、160;          /支持的一种特殊的遥控器 读取数据  #endif   if (currentTime > rcTime )                          

7、60;                / 50Hz  时间过了20ms     rcTime = currentTime + 20000;    computeRC();   /对已经接收的遥控接收的信号进行循环滤波，取4组数据，80MS，算平均值，大于平均值的减小2，小于平均值的增大2.  

8、60; / Failsafe routine - added by MIS    #if defined(FAILSAFE)      if ( failsafeCnt > (5*FAILSAFE_DELAY) && f.ARMED)        / 使之稳定, 并设置油门到指定的值          &

9、#160;                       for(i=0; i<3; i+) rcDatai = MIDRC;                     

10、;         / 丢失信号(in 0.1sec)后，把全部通道数据设置为 MIDRC=1500          rcDataTHROTTLE = conf.failsafe_throttle;                 

11、60; /  把油门设置为conf.failsafe_throttle          if (failsafeCnt > 5*(FAILSAFE_DELAY+FAILSAFE_OFF_DELAY)  / 在特定时间之后关闭电机 (in 0.1sec)                  &#

12、160;             go_disarm();     / This will prevent the copter to automatically rearm if failsafe shuts it down and prevents            f.OK_TO_ARM = 0;

13、/ 进入锁定状态，之后起飞需要解锁                  failsafeEvents+;                           &#

14、160;                                  /掉落爱护大事标志位至1            if ( fai

15、lsafeCnt > (5*FAILSAFE_DELAY) && !f.ARMED)         /Turn of "Ok To arm to prevent the motors from spinning after repowering the RX with low throttle and aux to arm          go_disarm();  &#

16、160;  / This will prevent the copter to automatically rearm if failsafe shuts it down and prevents          f.OK_TO_ARM = 0; /进入锁定状态，之后起飞需要解锁          failsafeCnt+;     &

17、#160;          /掉落爱护计数+1  每1 代表20ms 大于5倍FAILSAFE_DELAY 则进入爱护    #endif    / end of failsafe routine - next change is made with RcOptions setting    / - STICKS COMMAND HANDLER - 

18、60;  / 检测把握杆位置    uint8_t stTmp = 0;    for(i=0;i<4;i+)           stTmp >>= 2;                   

19、;                                 /stTmp除以4      if(rcDatai > MINCHECK) stTmp |= 0x80;   

20、60;  / MINCHECK=1100     1000 0000B      if(rcDatai < MAXCHECK) stTmp |= 0x40;     / MAXCHECK=1900    0100 0000B   通过stTmp推断是否把握杆是否在最大最小之外        if(stT

21、mp = rcSticks)          if(rcDelayCommand<250) rcDelayCommand+;  /若把握杆在最大最小位置外的状态未转变（20ms内），则rcDelayCommand+1         else rcDelayCommand = 0;  /否则清0    rcSticks = stTmp; &

22、#160;                 /保存stTmp           / 实行行动        if (rcDataTHROTTLE <= MINCHECK)     /油门在最低

23、值                       errorGyroIROLL = 0; errorGyroIPITCH = 0; errorGyroIYAW = 0;            /把roll pitch yaw 误差置0   &

24、#160;    errorAngleIROLL = 0; errorAngleIPITCH = 0;        if (conf.activateBOXARM > 0) / Arming/Disarming via ARM BOX                    

25、;           if ( rcOptionsBOXARM && f.OK_TO_ARM ) go_arm();                            /解锁  

26、        else if (f.ARMED) go_disarm();                                      

27、;                              /上锁                 if(rcDelayComm

28、and = 20)                                                 &#

29、160;                            /若把握杆在最大最小位置外的状态未转变(20*20ms)            if(f.ARMED)   

30、0;            / 当处在解锁时                                     #

31、ifdef ALLOW_ARM_DISARM_VIA_TX_YAW                                             &#

32、160;   /上锁方式1             if (conf.activateBOXARM = 0 && rcSticks = THR_LO + YAW_LO + PIT_CE + ROL_CE) go_disarm();    / Disarm via YAW          #e

33、ndif          #ifdef ALLOW_ARM_DISARM_VIA_TX_ROLL                                  &#

34、160;              /上锁方式2             if (conf.activateBOXARM = 0 && rcSticks = THR_LO + YAW_CE + PIT_CE + ROL_LO) go_disarm();    / Disarm via

35、 ROLL          #endif                 else                    

36、60;      / 当处在未解锁时                                          i=0;&

37、#160;          if (rcSticks = THR_LO + YAW_LO + PIT_LO + ROL_CE)    / GYRO（陀螺仪） 校准                          &#

38、160;   calibratingG=512;             /校准G 512*20Ms              #if GPS              

39、   GPS_reset_home_position();        /GPS 设置HOME              #endif              #if BARO   &#

40、160;             calibratingB=10;             /  气压计设置基准气压(10 * 25 ms = 250 ms non blocking)             

41、 #endif                    #if defined(INFLIGHT_ACC_CALIBRATION)       /使得可以飞行中ACC校准  （怎么手在抖。）         else if (rcS

42、ticks = THR_LO + YAW_LO + PIT_HI + ROL_HI) / Inflight ACC calibration START/STOP                         if (AccInflightCalibrationMeasurementDone) / trigger saving into

43、eeprom after landing                                          AccInflightCalibrationMeasurement

44、Done = 0;               AccInflightCalibrationSavetoEEProm = 1;                        else  &#

45、160;                         AccInflightCalibrationArmed = !AccInflightCalibrationArmed;              

46、60; #if defined(BUZZER)               if (AccInflightCalibrationArmed) alarmArray0=2; else   alarmArray0=3;               #endif &

47、#160;                           #endif        #ifdef MULTIPLE_CONFIGURATION_PROFILES       

48、0;                         /多配置文件读取        if        (rcSticks = THR_LO + YAW_LO + PIT_CE + ROL_LO)

49、i=1;    / ROLL left  -> Profile 1          /配置1        else if (rcSticks = THR_LO + YAW_LO + PIT_HI + ROL_CE) i=2;    / PITCH up   -> Profile 2    &#

50、160;   /配置2        else if (rcSticks = THR_LO + YAW_LO + PIT_CE + ROL_HI) i=3;    / ROLL right -> Profile 3        /配置3        if(i)   &#

51、160;                global_conf.currentSet = i-1;           writeGlobalSet(0);           readEEPROM(); &#

52、160;         blinkLED(2,40,i);           alarmArray0 = i;                 #endif      

53、;  if (rcSticks = THR_LO + YAW_HI + PIT_HI + ROL_CE)                 / 进入LCD配置                      

54、        #if defined(LCD_CONF)            configurationLoop();                        

55、                                         / beginning LCD configuration    &

56、#160;     #endif          previousTime = micros();                              

57、                              /设置时间                #ifdef ALLOW_ARM_DI

58、SARM_VIA_TX_YAW                                /允许使用YAW进行解锁          else if (conf.activateBOXA

59、RM = 0 && rcSticks = THR_LO + YAW_HI + PIT_CE + ROL_CE) go_arm();      / Arm via YAW        #endif        #ifdef ALLOW_ARM_DISARM_VIA_TX_ROLL       

60、0;  else if (conf.activateBOXARM = 0 && rcSticks = THR_LO + YAW_CE + PIT_CE + ROL_HI) go_arm();      / Arm via ROLL        #endif        #ifdef LCD_TELEMETRY_AUTO   &#

61、160;                                                 &#

62、160;           /与LCD有关 telemetry 遥测          else if (rcSticks = THR_LO + YAW_CE + PIT_HI + ROL_LO)       / Auto telemetry ON/OFF     &

63、#160;                               if (telemetry_auto)                

64、;              telemetry_auto = 0;                telemetry = 0;              &#

65、160;            else                telemetry_auto = 1;                 

66、60;#endif        #ifdef LCD_TELEMETRY_STEP          else if (rcSticks = THR_LO + YAW_CE + PIT_HI + ROL_HI)         / Telemetry next step     

67、0;                               telemetry = telemetryStepSequence+telemetryStepIndex % strlen(telemetryStepSequence);     &#

68、160;       #ifdef OLED_I2C_128x64               if (telemetry != 0) i2c_OLED_init();             #endif   

69、0;         LCDclear();                  #endif        else if (rcSticks = THR_HI + YAW_LO + PIT_LO + ROL_CE) calibratingA=512

70、;     /加速度校准        else if (rcSticks = THR_HI + YAW_HI + PIT_LO + ROL_CE) f.CALIBRATE_MAG = 1;  /电子罗盘校准           i=0;        if   

71、60;    (rcSticks = THR_HI + YAW_CE + PIT_HI + ROL_CE) conf.angleTrimPITCH+=2; i=1; /角度矫正 在计算PID时有用        else if (rcSticks = THR_HI + YAW_CE + PIT_LO + ROL_CE) conf.angleTrimPITCH-=2; i=1;        els

72、e 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)      

73、;               writeParams(1);            rcDelayCommand = 0;    / allow autorepetition          &#

74、160; #if defined(LED_RING)                                             

75、0;                                        /调整之后灯闪        

76、60;     blinkLedRing();                                           

77、;                                                  

78、;   /灯闪烁 使用IIC接口            #endif                      #if defined(LED_FLASHER)      

79、;led_flasher_autoselect_sequence();              /仍在20MS循环中，LED闪烁    #endif        #if defined(INFLIGHT_ACC_CALIBRATION)   /空中校准ACC     

80、0;if (AccInflightCalibrationArmed && f.ARMED && rcDataTHROTTLE > MINCHECK && !rcOptionsBOXARM ) / Copter is airborne and you are turning it off via boxarm : start measurement        InflightcalibratingA = 50;    &

81、#160;   AccInflightCalibrationArmed = 0;              if (rcOptionsBOXCALIB)      / 使用Calib来标定 : Calib = TRUE 测试开头, 降落 且 Calib = 0 测量储存        if (!Ac

82、cInflightCalibrationActive && !AccInflightCalibrationMeasurementDone)   /若空中校准ACC未运行          InflightcalibratingA = 50;                   

83、60;    /设定校准时间 50              else if(AccInflightCalibrationMeasurementDone && !f.ARMED)         /若结束 就保存        AccInfli

84、ghtCalibrationMeasurementDone = 0;        AccInflightCalibrationSavetoEEProm = 1;                           #endif  

85、0; uint16_t auxState = 0;                           /测量帮助通道位置  小于1300  1300到1700  大于1700    for(i=0;i<4;i+)    &

86、#160;                                  auxState |= (rcDataAUX1+i<1300)<<(3*i) | (1300<rcDataAUX1+i && rcDataAU

87、X1+i<1700)<<(3*i+1) | (rcDataAUX1+i>1700)<<(3*i+2);    for(i=0;i<CHECKBOXITEMS;i+)      rcOptionsi = (auxState & conf.activatei)>0;                

88、                   /将帮助通道位置与选择的开启方式比较，保存开启的模式    / note: if FAILSAFE is disable, failsafeCnt > 5*FAILSAFE_DELAY is always false    #if ACC    

89、  if ( rcOptionsBOXANGLE | (failsafeCnt > 5*FAILSAFE_DELAY) )          / 开启自稳模式anglemode                if (!f.ANGLE_MODE)      

90、;                errorAngleIROLL = 0; errorAngleIPITCH = 0;            f.ANGLE_MODE = 1;             

91、;           else  / failsafe模式时的动作                     f.ANGLE_MODE = 0;         

92、0;  if ( rcOptionsBOXHORIZON ) /开启 HORIZON模式  rc选择                                      &#

93、160;   f.ANGLE_MODE = 0;            /关闭angle模式         if (!f.HORIZON_MODE)       /若horizon模式未开启          

94、;          errorAngleIROLL = 0; errorAngleIPITCH = 0;           f.HORIZON_MODE = 1;    /开启horizon模式             

95、60;        else                                       /否则  &

96、#160;           f.HORIZON_MODE = 0;     /关闭horizon模式          #endif    if (rcOptionsBOXARM = 0) f.OK_TO_ARM = 1;    #if !defined(GPS_L

97、ED_INDICATOR)      if (f.ANGLE_MODE | f.HORIZON_MODE) STABLEPIN_ON; else STABLEPIN_OFF;    #endif    #if BARO      #if (!defined(SUPPRESS_BARO_ALTHOLD)  /若未宏定义SUPPRESS_BARO_ALTHOLD  

98、60;     if (rcOptionsBOXBARO)                             /rc 若选择baro           

99、          if (!f.BARO_MODE)                                   /若baro模式未开启 &

100、#160;                         f.BARO_MODE = 1;                      &#

101、160;        /开启baro模式 气压计定高               AltHold = EstAlt;               initialThrottleHold = rcCommandTHROT

102、TLE;  /储存此时 rc 油门输出值               errorAltitudeI = 0;                            

103、60;                    /重置PID输出 和高度误差               BaroPID=0;          

104、60;          else  /若RC未选择BARO模式                    f.BARO_MODE = 0;              

105、60; /关闭baro模式                                 #endif      #ifdef VARIOMETER      

106、;             /若定义了VARIOMETER        if (rcOptionsBOXVARIO)        /rc 若选择vario模式             &

107、#160;      if (!f.VARIO_MODE)                            f.VARIO_MODE = 1;          &

108、#160;/开启vario模式                     else                           

109、60;             /rc未选择VARIO模式                  f.VARIO_MODE = 0;              &#

110、160; /关闭vario模式              #endif    #endif    #if MAG                      &#

111、160;              /若配置了磁场传感器      if (rcOptionsBOXMAG)         /开启磁场传感器 与上面开启各种模式一样             

112、;   if (!f.MAG_MODE)                      f.MAG_MODE = 1;            magHold = heading;    

113、0;            else                f.MAG_MODE = 0;            if (rcOptionsBOXHEADFREE) /开启无头模式与上面开启各

114、种模式一样               if (!f.HEADFREE_MODE)                     f.HEADFREE_MODE = 1;      &#

115、160;              else                f.HEADFREE_MODE = 0;            if (rcOptionsBOXHEAD

116、ADJ)                headFreeModeHold = heading; / acquire new heading          #endif        #if GPS     

117、 static uint8_t GPSNavReset = 1;      if (f.GPS_FIX && GPS_numSat >= 5 )               if (rcOptionsBOXGPSHOME)  /若GPS_HOME 和 GPS_HOLD 都被选择了额 GPS_HOME 具有优先权  &

118、#160;                 if (!f.GPS_HOME_MODE)                          f.GPS_HOME_MODE = 1;&

119、#160;            f.GPS_HOLD_MODE = 0;             GPSNavReset = 0;             #if defined(I2C_GPS)  

120、;             GPS_I2C_command(I2C_GPS_COMMAND_START_NAV,0);        /waypoint zero             #else / 串口     &

121、#160;       GPS_set_next_wp(&GPS_homeLAT,&GPS_homeLON);             nav_mode    = NAV_MODE_WP;             #endi

122、f                   else           f.GPS_HOME_MODE = 0;          if (rcOptionsBOXGPSHOLD && abs(rcComm

123、andROLL)< AP_MODE && abs(rcCommandPITCH) < AP_MODE)             if (!f.GPS_HOLD_MODE)               f.GPS_HOLD_MODE = 1;      

124、        GPSNavReset = 0;              #if defined(I2C_GPS)                GPS_I2C_command(I2C_GPS_COMMAND_POSH

125、OLD,0);              #else                GPS_holdLAT = GPS_coordLAT;             

126、60;  GPS_holdLON = GPS_coordLON;                GPS_set_next_wp(&GPS_holdLAT,&GPS_holdLON);                nav_mode = NAV_

127、MODE_POSHOLD;              #endif                       else           

128、;  f.GPS_HOLD_MODE = 0;            / both boxes are unselected here, nav is reset if not already done            if (GPSNavReset = 0 )       

129、;        GPSNavReset = 1;              GPS_reset_nav();                      

130、60;              else         f.GPS_HOME_MODE = 0;        f.GPS_HOLD_MODE = 0;        #if !defined(I2C_GPS) &#

131、160;        nav_mode = NAV_MODE_NONE;        #endif          #endif        #if defined(FIXEDWING) | defined(HELICOPTER)  

132、0;           /另外的机型的模式 与四轴无关      if (rcOptionsBOXPASSTHRU) f.PASSTHRU_MODE = 1;      else f.PASSTHRU_MODE = 0;    #endif       /RC循环到此

133、为止  else      /若未进入RC则依次进行以下5个任务       static uint8_t taskOrder=0; / 不把全部的任务放在一个循环中，避开高延迟使得RC循环无法进入。    if(taskOrder>4) taskOrder-=5;    switch (taskOrder)      &

134、#160;case 0:        taskOrder+;        #if MAG                              

135、; /猎取MAG数据          if (Mag_getADC() break; / max 350 µs (HMC5883) / only break when we actually did something        #endif      case 1:     

136、60;  taskOrder+;        #if BARO                              /猎取BARO数据     

137、0;    if (Baro_update() != 0 ) break;        #endif      case 2:        taskOrder+;        #if BARO      

138、                        /猎取BARO数据          if (getEstimatedAltitude() !=0) break;        #

139、endif          case 3:        taskOrder+;        #if GPS                    

140、0;            /猎取GPS数据          if(GPS_Enable) GPS_NewData();          break;        #endif 

141、60;    case 4:        taskOrder+;        #if SONAR                          

142、;  /猎取SONAR （声呐）数据          Sonar_update();debug2 = sonarAlt;        #endif        #ifdef LANDING_LIGHTS_DDR        

143、0; auto_switch_landing_lights();        #endif        #ifdef VARIOMETER          if (f.VARIO_MODE) vario_signaling();        

144、#endif        break;         computeIMU();                          /计算IMU（惯性测量单元） &#

145、160;/ Measure loop rate just afer reading the sensors  currentTime = micros();  cycleTime = currentTime - previousTime;  previousTime = currentTime;  #ifdef CYCLETIME_FIXATED    if (conf.cycletime_fixated)      

146、0;if (micros()-timestamp_fixated)>conf.cycletime_fixated)        else          while(micros()-timestamp_fixated)<conf.cycletime_fixated) ; / waste away           &#

147、160;timestamp_fixated=micros();      #endif  /*  /* Experimental FlightModes *  试验的飞行模式  /*  #if defined(ACROTRAINER_MODE)          /固定翼训练者模式    if(f.ANGLE_MODE

148、)            if (abs(rcCommandROLL) + abs(rcCommandPITCH) >= ACROTRAINER_MODE )  /ACROTRAINER_MODE=200                  f.ANGLE_MODE=0; &

149、#160;                       /取消自稳 定向 定高 GPS回家 GPS定点          f.HORIZON_MODE=0;                          f.MAG_MODE=0;