MWC代码解读.docx

MWC代码解读.docx

《MWC代码解读.docx》由会员分享，可在线阅读，更多相关《MWC代码解读.docx（22页珍藏版）》请在冰豆网上搜索。

MWC代码解读

[转载]MWC v2.2 代码解读LOOP（）

（2013-04-0720:

01:

27）

转载▼

标签：

转载

原文地址：

MWC v2.2 代码解读LOOP（）作者：

问江南

函数很长不用文字了贴个流程图，说明一切：

voidloop（）{

  staticuint8_trcDelayCommand;//thisindicatesthenumberoftime（multipleofRCmeasurementat50Hz）thesticksmustbemaintainedtorunorswitchoffmotors

  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） 

      { //Turnof"OkToarmtopreventthemotorsfromspinningafterrepoweringtheRXwithlowthrottleandauxtoarm

          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=1100    10000000B

      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;else  alarmArray[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;

        if       （rcSticks==THR_HI+YAW_CE+PIT_HI+ROL_CE）{conf.angleTrim[PITCH]+=2;i=1;}//角度矫正在计算PID时有用

        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]）{//Copterisairborneandyouareturningitoffviaboxarm:

startmeasurement

        InflightcalibratingA=50;

        AccInflightCalibrationArmed=0;

      } 

      if（rcOptions[BOXCALIB]）{     //使用Calib来标定 :

Calib=TRUE测试开始,降落且Calib=0测量储存

        if（!

AccInflightCalibrationActive&&!

AccInflightCalibrationMeasurementDone）{  //若空中校准ACC未运行