MWC_v2.2_代碼解讀

1、精選文檔轉(zhuǎn)載MWC v2.2 代碼解讀LOOP()  (2013-04-07 20:01:27)轉(zhuǎn)載標(biāo)簽： 轉(zhuǎn)載原文地址：MWC v2.2 代碼解讀LOOP()作者：問江南函數(shù)很長不用文字了 貼個(gè)流程圖，說明一切：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();          /支持的一種特殊遙控器 讀取數(shù)據(jù)  #endif    #if de

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

6、160;          /支持的一種特殊的遙控器 讀取數(shù)據(jù)  #endif   if (currentTime > rcTime )                          

7、60;                / 50Hz  時(shí)間過了20ms     rcTime = currentTime + 20000;    computeRC();   /對已經(jīng)接收的遙控接收的信號進(jìn)行循環(huán)濾波，取4組數(shù)據(jù)，80MS，算平均值，大于平均值的減小2，小于平均值的增大2.  

8、60; / Failsafe routine - added by MIS    #if defined(FAILSAFE)      if ( failsafeCnt > (5*FAILSAFE_DELAY) && f.ARMED)        / 使之穩(wěn)定, 并設(shè)置油門到指定的值          &

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

10、;         / 丟失信號(in 0.1sec)后，把全部通道數(shù)據(jù)設(shè)置為 MIDRC=1500          rcDataTHROTTLE = conf.failsafe_throttle;                 

11、60; /  把油門設(shè)置為conf.failsafe_throttle          if (failsafeCnt > 5*(FAILSAFE_DELAY+FAILSAFE_OFF_DELAY)  / 在特定時(shí)間之后關(guān)閉電機(jī) (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、/ 進(jìn)入鎖定狀態(tài)，之后起飛需要解鎖                  failsafeEvents+;                           &#

14、160;                                  /掉落愛護(hù)大事標(biāo)志位至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; /進(jìn)入鎖定狀態(tài)，之后起飛需要解鎖          failsafeCnt+;     &

17、#160;          /掉落愛護(hù)計(jì)數(shù)+1  每1 代表20ms 大于5倍FAILSAFE_DELAY 則進(jìn)入愛護(hù)    #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+;  /若把握桿在最大最小位置外的狀態(tài)未轉(zhuǎn)變（20ms內(nèi)），則rcDelayCommand+1         else rcDelayCommand = 0;  /否則清0    rcSticks = stTmp; &

22、#160;                 /保存stTmp           / 實(shí)行行動(dòng)        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;                            /若把握桿在最大最小位置外的狀態(tài)未轉(zhuǎn)變(20*20ms)            if(f.ARMED)   

30、0;            / 當(dāng)處在解鎖時(shí)                                     #

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;      / 當(dāng)處在未解鎖時(shí)                                          i=0;&

37、#160;          if (rcSticks = THR_LO + YAW_LO + PIT_LO + ROL_CE)    / GYRO（陀螺儀） 校準(zhǔn)                          &#

38、160;   calibratingG=512;             /校準(zhǔn)G 512*20Ms              #if GPS              

39、   GPS_reset_home_position();        /GPS 設(shè)置HOME              #endif              #if BARO   &#

40、160;             calibratingB=10;             /  氣壓計(jì)設(shè)置基準(zhǔn)氣壓(10 * 25 ms = 250 ms non blocking)             

41、 #endif                    #if defined(INFLIGHT_ACC_CALIBRATION)       /使得可以飛行中ACC校準(zhǔn)  （怎么手在抖。）         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)                 / 進(jìn)入LCD配置                      

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

55、                                         / beginning LCD configuration    &

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

57、                              /設(shè)置時(shí)間                #ifdef ALLOW_ARM_DI

58、SARM_VIA_TX_YAW                                /允許使用YAW進(jìn)行解鎖          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有關(guān) 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、;     /加速度校準(zhǔn)        else if (rcSticks = THR_HI + YAW_HI + PIT_LO + ROL_CE) f.CALIBRATE_MAG = 1;  /電子羅盤校準(zhǔn)           i=0;        if   

71、60;    (rcSticks = THR_HI + YAW_CE + PIT_HI + ROL_CE) conf.angleTrimPITCH+=2; i=1; /角度矯正 在計(jì)算PID時(shí)有用        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;                                        /調(diào)整之后燈閃        

76、60;     blinkLedRing();                                           

77、;                                                  

78、;   /燈閃爍 使用IIC接口            #endif                      #if defined(LED_FLASHER)      

79、;led_flasher_autoselect_sequence();              /仍在20MS循環(huán)中，LED閃爍    #endif        #if defined(INFLIGHT_ACC_CALIBRATION)   /空中校準(zhǔn)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來標(biāo)定 : Calib = TRUE 測試開頭, 降落 且 Calib = 0 測量儲存        if (!Ac

82、cInflightCalibrationActive && !AccInflightCalibrationMeasurementDone)   /若空中校準(zhǔn)ACC未運(yùn)行          InflightcalibratingA = 50;                   

83、60;    /設(shè)定校準(zhǔn)時(shí)間 50              else if(AccInflightCalibrationMeasurementDone && !f.ARMED)         /若結(jié)束 就保存        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) )          / 開啟自穩(wěn)模式anglemode                if (!f.ANGLE_MODE)      

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

91、;           else  / failsafe模式時(shí)的動(dòng)作                     f.ANGLE_MODE = 0;         

92、0;  if ( rcOptionsBOXHORIZON ) /開啟 HORIZON模式  rc選擇                                      &#

93、160;   f.ANGLE_MODE = 0;            /關(guān)閉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;     /關(guān)閉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模式 氣壓計(jì)定高               AltHold = EstAlt;               initialThrottleHold = rcCommandTHROT

102、TLE;  /儲存此時(shí) rc 油門輸出值               errorAltitudeI = 0;                            

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

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

105、60; /關(guān)閉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; /關(guān)閉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 具有優(yōu)先權(quán)  &

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;           /另外的機(jī)型的模式 與四軸無關(guān)      if (rcOptionsBOXPASSTHRU) f.PASSTHRU_MODE = 1;      else f.PASSTHRU_MODE = 0;    #endif       /RC循環(huán)到此

133、為止  else      /若未進(jìn)入RC則依次進(jìn)行以下5個(gè)任務(wù)       static uint8_t taskOrder=0; / 不把全部的任務(wù)放在一個(gè)循環(huán)中，避開高延遲使得RC循環(huán)無法進(jìn)入。    if(taskOrder>4) taskOrder-=5;    switch (taskOrder)      &

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

135、; /獵取MAG數(shù)據(jù)          if (Mag_getADC() break; / max 350 µs (HMC5883) / only break when we actually did something        #endif      case 1:     

136、60;  taskOrder+;        #if BARO                              /獵取BARO數(shù)據(jù)     

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

138、                        /獵取BARO數(shù)據(jù)          if (getEstimatedAltitude() !=0) break;        #

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

140、0;            /獵取GPS數(shù)據(jù)          if(GPS_Enable) GPS_NewData();          break;        #endif 

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

142、;  /獵取SONAR （聲吶）數(shù)據(jù)          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();                          /計(jì)算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 *  試驗(yàn)的飛行模式  /*  #if defined(ACROTRAINER_MODE)          /固定翼訓(xùn)練者模式    if(f.ANGLE_MODE

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

149、#160;                       /取消自穩(wěn) 定向 定高 GPS回家 GPS定點(diǎn)          f.HORIZON_MODE=0;                          f.MAG_MODE=0;