MWC四轴飞行器代码解读

MWC v2.2 代码解读annexCode()

红色是一些暂时没去顾及的部分,与我现在关心的地方并无太大关系。

函数对rcDate进行处理(去除死区,根据油门曲线,roll/pitch曲线,油门值,动态PID调整参数,在无头模式对于rcdata进行优化),生成rccommand值用于姿态控制。记录最大循环时间,最小循环时间,解锁时间,最大气压值。用LED表示一些传感器运行的状态。若定义了低压报警则进行电压测量。
rccommand【油门】在0-1000之间  rccommand【roll/pitch】-500 +500  小于1500为负 大于1500为正。
void annexCode() // 每个循环都会执行,若执行时间少于650MS则不会影响主循环的执行。
  static uint32_t calibratedAccTime;
  uint16_t tmp,tmp2;
  uint8_t axis,prop1,prop2;
 
  #define BREAKPOINT 1500
  // PITCH & ROLL 进行动态的PID参数调整,  调整取决于油门大小
  if (rcData[THROTTLE]
  {
    prop2 = 100;
  } 
  else     //油门大于1500
  {
    if (rcData[THROTTLE]<2000)   //油门小于2000
    {
       prop2 = 100 - (uint16_t)conf.dynThrPID*(rcData[THROTTLE]-BREAKPOINT)/(2000-BREAKPOINT); //dynThrPID 四轴默认0 可以GUI设置   TPA参数              PROP 与动态pid有关。dynThrPIDprop 越小 即油门大小对于PID影响越大。
    } 
    else   //油门大于2000 (不太可能)
    {
       prop2 = 100 - conf.dynThrPID;
    }
  }
  for(axis=0;axis<3;axis++) 
  {
     tmp = min(abs(rcData[axis]-MIDRC),500);
     #if defined(DEADBAND)          //死区  即偏移小于死区的去掉 大于死区的减去死区范围
       if (tmp>DEADBAND) { tmp -= DEADBAND; }
       else { tmp=0; }
     #endif
     if(axis!=2)  //ROLL & PITCH  yawaxis=2
     {
        tmp2 = tmp/100;   //偏移量除以100
        rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp-tmp2*100) * (lookupPitchRollRC[tmp2+1]-lookupPitchRollRC[tmp2]) /       100; //  差值法求用于控制的rc信号,取决于roll pitch 曲线。
        prop1 = 100-(uint16_t)conf.rollPitchRate*tmp/500;//tmp=rc数据与中立值差得绝对值  rate [0 100];
        prop1 = (uint16_t)prop1*prop2/100;             //根据prop2 修改 prop1 
      } 
      else // YAW      
      {      
         rcCommand[axis] = tmp;
         prop1 = 100-(uint16_t)conf.yawRate*tmp/500;   //tmp=RC距离中点值   yawRate[0 100] 默认为0
       }
       dynP8[axis] = (uint16_t)conf.P8[axis]*prop1/100;         //动态YAW PID参数
       dynD8[axis] = (uint16_t)conf.D8[axis]*prop1/100;
       if (rcData[axis]
  }
  tmp = constrain(rcData[THROTTLE],MINCHECK,2000);     // 令tmp=rcData[THROTTLE]; (前面等于各个轴的输入数值)
  tmp = (uint32_t)(tmp-MINCHECK)*1000/(2000-MINCHECK); // 把油门控制转化为【0 1000】
  tmp2 = tmp/100;
  rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp-tmp2*100) * (lookupThrottleRC[tmp2+1]-  lookupThrottleRC[tmp2]) / 100;                                          //通过油门曲线对油门控制量进行换算
 
  if(f.HEADFREE_MODE) //to optimize  开启HEADFREE模式时
  { 
    float radDiff = (heading - headFreeModeHold) * 0.0174533f; // 转化为弧度   PI/180 ~= 0.0174533
    float cosDiff = cos(radDiff);          //求余弦
    float sinDiff = sin(radDiff);           //求正弦
    int16_t rcCommand_PITCH = rcCommand[PITCH]*cosDiff + rcCommand[ROLL]*sinDiff;
    rcCommand[ROLL] =  rcCommand[ROLL]*cosDiff - rcCommand[PITCH]*sinDiff; 
    rcCommand[PITCH] = rcCommand_PITCH;
  }
 
  #if defined(POWERMETER_HARD)
    uint16_t pMeterRaw;               // used for current reading
    static uint16_t psensorTimer = 0;
    if (! (++psensorTimer % PSENSORFREQ)) {
      pMeterRaw =  analogRead(PSENSORPIN);
      //lcdprint_int16(pMeterRaw); LCDcrlf();
      powerValue = ( conf.psensornull > pMeterRaw ? conf.psensornull - pMeterRaw : pMeterRaw - conf.psensornull); // do not use abs(), it would induce implicit cast to uint and overrun
      if ( powerValue < 333) {  // only accept reasonable values. 333 is empirical
      #ifdef LCD_TELEMETRY
        if (powerValue > powerMax) powerMax = powerValue;
      #endif
      } else {
        powerValue = 333;
      }        
      pMeter[PMOTOR_SUM] += (uint32_t) powerValue;
    }
  #endif
  #if defined(BUZZER)       //关于低电压报警
    #if defined(VBAT)
      static uint8_t vbatTimer = 0;
      static uint8_t ind = 0;
      uint16_t vbatRaw = 0;
      static uint16_t vbatRawArray[8];
      if (! (++vbatTimer % VBATFREQ)) {
        vbatRawArray[(ind++)%8] = analogRead(V_BATPIN);
        for (uint8_t i=0;i<8;i++) vbatRaw += vbatRawArray[i];
        vbat = (vbatRaw*2) / conf.vbatscale; // result is Vbatt in 0.1V steps
      }
    #endif
    alarmHandler(); // external buzzer routine that handles buzzer events globally now
  #endif  
 
  #if defined(RX_RSSI)              //接收信号强度指示
    static uint8_t sig = 0;
    uint16_t rssiRaw = 0;
    static uint16_t rssiRawArray[8];
    rssiRawArray[(sig++)%8] = analogRead(RX_RSSI_PIN);
    for (uint8_t i=0;i<8;i++) rssiRaw += rssiRawArray[i];
    rssi = rssiRaw / 8;       
  #endif
 
  if ( (calibratingA>0 && ACC ) || (calibratingG>0) ) // Calibration phasis
  { 
    LEDPIN_TOGGLE;                                     //初始化LED引脚 digital pin13
  }  
  else 
  {
    if (f.ACC_CALIBRATED) {LEDPIN_OFF;}  //ACC校准结束 灯灭
    if (f.ARMED) {LEDPIN_ON;}                   //解锁灯亮
  }
 
  #if defined(LED_RING)                            
    static uint32_t LEDTime;
    if ( currentTime > LEDTime ) 
    {
       LEDTime = currentTime + 50000;      //50s闪烁一次led
       i2CLedRingState();
    }
  #endif
 
  #if defined(LED_FLASHER)
    auto_switch_led_flasher();
  #endif
 
  if ( currentTime > calibratedAccTime )   每100S检测一次
  {
     if (! f.SMALL_ANGLES_25)   //倾斜太大 或者未校准ACC
     {
        f.ACC_CALIBRATED = 0;       //校准标志位清0
        LEDPIN_TOGGLE;
        calibratedAccTime = currentTime + 100000;
     } 
     else 
     {
        f.ACC_CALIBRATED = 1;
     }
  }
 
  #if !(defined(SPEKTRUM) && defined(PROMINI))  //Only one serial port on ProMini.  Skip serial com if Spektrum Sat in use. Note: Spek code will auto-call serialCom if GUI data detected on serial0.
    #if defined(GPS_PROMINI)
      if(GPS_Enable == 0) {serialCom();}
    #else
      serialCom();
    #endif
  #endif
 
  #if defined(POWERMETER) //功率计有关
    intPowerMeterSum = (pMeter[PMOTOR_SUM]/conf.pleveldiv);
    intPowerTrigger1 = conf.powerTrigger1 * PLEVELSCALE; 
  #endif
 
  #ifdef LCD_TELEMETRY_AUTO
    static char telemetryAutoSequence []  = LCD_TELEMETRY_AUTO;
    static uint8_t telemetryAutoIndex = 0;
    static uint16_t telemetryAutoTimer = 0;
    if ( (telemetry_auto) && (! (++telemetryAutoTimer % LCD_TELEMETRY_AUTO_FREQ) )  ){
      telemetry = telemetryAutoSequence[++telemetryAutoIndex % strlen(telemetryAutoSequence)];
      LCDclear(); // make sure to clear away remnants
    }
  #endif  
  #ifdef LCD_TELEMETRY
    static uint16_t telemetryTimer = 0;
    if (! (++telemetryTimer % LCD_TELEMETRY_FREQ)) {
      #if (LCD_TELEMETRY_DEBUG+0 > 0)
        telemetry = LCD_TELEMETRY_DEBUG;
      #endif
      if (telemetry) lcd_telemetry();
    }
  #endif
 
  #if GPS & defined(GPS_LED_INDICATOR)       // 用LED来表示一些GPS的信息
    static uint32_t GPSLEDTime;              // - No GPS FIX -> LED blink at speed of incoming GPS frames
    static uint8_t blcnt;                    // - Fix and sat no. bellow 5 -> LED off
    if(currentTime > GPSLEDTime) {           // - Fix and sat no. >= 5 -> LED blinks, one blink for 5 sat, two blinks for 6 sat, three for 7 ...
      if(f.GPS_FIX && GPS_numSat >= 5) {
        if(++blcnt > 2*GPS_numSat) blcnt = 0;
        GPSLEDTime = currentTime + 150000;
        if(blcnt >= 10 && ((blcnt%2) == 0)) {STABLEPIN_ON;} else {STABLEPIN_OFF;}
      }else{
        if((GPS_update == 1) && !f.GPS_FIX) {STABLEPIN_ON;} else {STABLEPIN_OFF;}
        blcnt = 0;
      }
    }
  #endif
 
  #if defined(LOG_VALUES) && (LOG_VALUES >= 2)              // 记录值
    if (cycleTime > cycleTimeMax) cycleTimeMax = cycleTime; // 记录最大循环时间
    if (cycleTime < cycleTimeMin) cycleTimeMin = cycleTime; // 记录最小循环时间
  #endif
  if (f.ARMED)  
  {
    #if defined(LCD_TELEMETRY) || defined(ARMEDTIMEWARNING) || defined(LOG_PERMANENT)
      armedTime += (uint32_t)cycleTime;                                    //记录解锁时间
    #endif
    #if defined(VBAT)
      if ( (vbat > conf.no_vbat) && (vbat < vbatMin) ) vbatMin = vbat;  //记录最低电压值
    #endif
    #ifdef LCD_TELEMETRY
      #if BARO
        if ( (BaroAlt > BAROaltMax) ) BAROaltMax = BaroAlt;   //记录最大气压值
      #endif
    #endif
  }
}

MWC v2.2 代码解读LOOP()

 
函数很长不用文字了 贴个流程图,说明一切:
MWC <wbr>v2.2 <wbr>代码解读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 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 PTermACC = 0 , ITermACC = 0 , PTermGYRO = 0 , ITermGYRO = 0;
  static int16_t lastGyro[3] = {0,0,0};
  static int16_t delta1[3],delta2[3];
  static int16_t errorGyroI[3] = {0,0,0};
  static int16_t errorAngleI[2] = {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();                                                  //支持的一种特殊的遥控器 读取数据
  #endif 
 
  if (currentTime > rcTime )                                           // 50Hz  时间过了20ms
    rcTime = 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<3; i++) rcData[i] = MIDRC;                              // 丢失信号(in 0.1sec)后,把所有通道数据设置为 MIDRC=1500
          rcData[THROTTLE] = conf.failsafe_throttle;                   //  把油门设置为conf.failsafe_throttle
          if (failsafeCnt > 5*(FAILSAFE_DELAY+FAILSAFE_OFF_DELAY))  // 在特定时间之后关闭电机 (in 0.1sec)
          {          
            go_disarm();     // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents
            f.OK_TO_ARM = 0; // 进入锁定状态,之后起飞需要解锁
          }
        failsafeEvents++;                                                              //掉落保护事件标志位至1
      }
      if ( failsafeCnt > (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();     // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents
          f.OK_TO_ARM = 0; //进入锁定状态,之后起飞需要解锁
      }
    failsafeCnt++;                //掉落保护计数+1  每1 代表20ms 大于5倍FAILSAFE_DELAY 则进入保护
    #endif
    // end of failsafe routine - next change is made with RcOptions setting
 
    // ------------------ STICKS COMMAND HANDLER --------------------
    // 检测控制杆位置
    uint8_t stTmp = 0;
    for(i=0;i<4;i++) 
    {
      stTmp >>= 2;                                                    //stTmp除以4
      if(rcData[i] > MINCHECK) stTmp |= 0x80;      // MINCHECK=1100     1000 0000B
      if(rcData[i] < MAXCHECK) stTmp |= 0x40;     // MAXCHECK=1900    0100 0000B   通过stTmp判断是否控制杆是否在最大最小之外
    }
    if(stTmp == rcSticks) 
   {
      if(rcDelayCommand<250) rcDelayCommand++;  //若控制杆在最大最小位置外的状态未改变(20ms内),则rcDelayCommand+1
    } 
    else rcDelayCommand = 0;  //否则清0
    rcSticks = stTmp;                   //保存stTmp
       
    // 采取行动    
    if (rcData[THROTTLE] <= MINCHECK)     //油门在最低值
    {           
        errorGyroI[ROLL] = 0; errorGyroI[PITCH] = 0; errorGyroI[YAW] = 0;            //把roll pitch yaw 误差置0
        errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0;
        if (conf.activate[BOXARM] > 0) // Arming/Disarming via ARM BOX
        {             
          if ( rcOptions[BOXARM] && f.OK_TO_ARM ) go_arm();                            //解锁
          else if (f.ARMED) go_disarm();                                                                    //上锁
        }
     }
    if(rcDelayCommand == 20)                                                                              //若控制杆在最大最小位置外的状态未改变(20*20ms)
    {
        if(f.ARMED)                // 当处在解锁时
        {                   
          #ifdef ALLOW_ARM_DISARM_VIA_TX_YAW                                                 //上锁方式1
             if (conf.activate[BOXARM] == 0 && rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) go_disarm();    // Disarm via YAW
          #endif
          #ifdef ALLOW_ARM_DISARM_VIA_TX_ROLL                                                 //上锁方式2
             if (conf.activate[BOXARM] == 0 && rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_LO) go_disarm();    // Disarm via ROLL
          #endif
        } 
        else                           // 当处在未解锁时
        {                       
           i=0;
           if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE)    // GYRO(陀螺仪) 校准 
           {    
              calibratingG=512;             //校准G 512*20Ms
              #if GPS 
                GPS_reset_home_position();        //GPS 设置HOME
              #endif
              #if BARO
                 calibratingB=10;             //  气压计设置基准气压(10 * 25 ms = ~250 ms non blocking)
              #endif
           }
         #if defined(INFLIGHT_ACC_CALIBRATION)       //使得可以飞行中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;
            }
            else
            { 
               AccInflightCalibrationArmed = !AccInflightCalibrationArmed; 
               #if defined(BUZZER)
               if (AccInflightCalibrationArmed) alarmArray[0]=2; else   alarmArray[0]=3;
               #endif
            }
        } 
        #endif
        #ifdef MULTIPLE_CONFIGURATION_PROFILES                                 //多配置文件读取
        if        (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_LO) i=1;    // ROLL left  -> Profile 1          //配置1
        else if (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_CE) i=2;    // PITCH up   -> Profile 2        //配置2
        else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_HI) i=3;    // ROLL right -> Profile 3        //配置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配置
        {            
          #if defined(LCD_CONF)
            configurationLoop();                                                                 // beginning LCD configuration
          #endif
          previousTime = micros();                                                            //设置时间
        }
        #ifdef ALLOW_ARM_DISARM_VIA_TX_YAW                                //允许使用YAW进行解锁
          else if (conf.activate[BOXARM] == 0 && rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE) go_arm();      // Arm via YAW
        #endif
        #ifdef ALLOW_ARM_DISARM_VIA_TX_ROLL
          else if (conf.activate[BOXARM] == 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 = 0;
             } 
             else
                telemetry_auto = 1;
          }
        #endif
        #ifdef LCD_TELEMETRY_STEP
          else if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_HI)         // Telemetry next step
          {              
             telemetry = telemetryStepSequence[++telemetryStepIndex % strlen(telemetryStepSequence)];
             #ifdef OLED_I2C_128x64
               if (telemetry != 0) i2c_OLED_init();
             #endif
             LCDclear();
          }
        #endif
        else 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.angleTrim[PITCH]+=2; i=1;} //角度矫正 在计算PID时有用
        else if (rcSticks == THR_HI + YAW_CE + PIT_LO + ROL_CE) {conf.angleTrim[PITCH]-=2; i=1;}
        else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_HI) {conf.angleTrim[ROLL] +=2; i=1;}
        else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_LO) {conf.angleTrim[ROLL] -=2; i=1;}
        if (i) 
        {
            writeParams(1);
            rcDelayCommand = 0;    // allow autorepetition
            #if defined(LED_RING)                                                                                      //调整之后灯闪
              blinkLedRing();                                                                                                //灯闪烁 使用IIC接口
            #endif
        }
      }
    }
    #if defined(LED_FLASHER)
      led_flasher_autoselect_sequence();              //仍在20MS循环中,LED闪烁
    #endif
    
    #if defined(INFLIGHT_ACC_CALIBRATION)   //空中校准ACC
      if (AccInflightCalibrationArmed && f.ARMED && rcData[THROTTLE] > MINCHECK && !rcOptions[BOXARM] ){ // Copter is airborne and you are turning it off via boxarm : start measurement
        InflightcalibratingA = 50;
        AccInflightCalibrationArmed = 0;
      }  
      if (rcOptions[BOXCALIB]) {      // 使用Calib来标定 : Calib = TRUE 测试开始, 降落 且 Calib = 0 测量储存
        if (!AccInflightCalibrationActive && !AccInflightCalibrationMeasurementDone){   //若空中校准ACC未运行
          InflightcalibratingA = 50;                        //设定校准时间 50
        }
      }else if(AccInflightCalibrationMeasurementDone && !f.ARMED){         //若结束 就保存
        AccInflightCalibrationMeasurementDone = 0;
        AccInflightCalibrationSavetoEEProm = 1;                 
      }
    #endif
 
    uint16_t auxState = 0;                           //测量辅助通道位置  小于1300  1300到1700  大于1700
    for(i=0;i<4;i++)                                 
      auxState |= (rcData[AUX1+i]<1300)<<(3*i) | (1300<rcData[AUX1+i] && rcData[AUX1+i]<1700)<<(3*i+1) | (rcData[AUX1+i]>1700)<<(3*i+2);
    for(i=0;i<CHECKBOXITEMS;i++)
      rcOptions[i] = (auxState & conf.activate[i])>0;                                   //将辅助通道位置与选择的开启方式比较,保存开启的模式
 
    // note: if FAILSAFE is disable, failsafeCnt > 5*FAILSAFE_DELAY is always false
    #if ACC
      if ( rcOptions[BOXANGLE] || (failsafeCnt > 5*FAILSAFE_DELAY) )          // 开启自稳模式anglemode
      { 
         if (!f.ANGLE_MODE) 
         {
            errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0;
            f.ANGLE_MODE = 1;
         }  
      } 
      else  // failsafe模式时的动作
      {      
         f.ANGLE_MODE = 0;
      }
      if ( rcOptions[BOXHORIZON] ) //开启 HORIZON模式  rc选择
      {                           
         f.ANGLE_MODE = 0;            //关闭angle模式
         if (!f.HORIZON_MODE)       //若horizon模式未开启
         {
           errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0;
           f.HORIZON_MODE = 1;    //开启horizon模式
         }
      } 
      else                                       //否则
      {
        f.HORIZON_MODE = 0;     //关闭horizon模式
      }
    #endif
    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;}
    #endif
 
    #if BARO
      #if (!defined(SUPPRESS_BARO_ALTHOLD))  //若未宏定义SUPPRESS_BARO_ALTHOLD
        if (rcOptions[BOXBARO])                             //rc 若选择baro
        { 
            if (!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模式                   
        }
      #endif
      #ifdef VARIOMETER                   //若定义了VARIOMETER
        if (rcOptions[BOXVARIO])        //rc 若选择vario模式
        {
            if (!f.VARIO_MODE) 
            {
               f.VARIO_MODE = 1;           //开启vario模式
            }
        } else                                         //rc未选择VARIO模式
        {
          f.VARIO_MODE = 0;                //关闭vario模式
        }
      #endif
    #endif
    #if MAG                                     //若配置了磁场传感器
      if (rcOptions[BOXMAG])         //开启磁场传感器 与上面开启各种模式一样
      {
          if (!f.MAG_MODE)
          {
            f.MAG_MODE = 1;
            magHold = heading;
          }
      } else 
      {
         f.MAG_MODE = 0;
      }
      if (rcOptions[BOXHEADFREE]) //开启无头模式与上面开启各种模式一样
      {
         if (!f.HEADFREE_MODE) 
         {
           f.HEADFREE_MODE = 1;
         }
      }
      else 
      {
         f.HEADFREE_MODE = 0;
      }
      if (rcOptions[BOXHEADADJ]) 
      {
         headFreeModeHold = heading; // acquire new heading
      }
    #endif
    
    #if GPS
      static uint8_t GPSNavReset = 1;
      if (f.GPS_FIX && GPS_numSat >= 5 ) 
      {
        if (rcOptions[BOXGPSHOME])  //若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 zero
             #else // 串口
             GPS_set_next_wp(&GPS_home[LAT],&GPS_home[LON]);
             nav_mode    = NAV_MODE_WP;
             #endif
          }
        } else {
          f.GPS_HOME_MODE = 0;
          if (rcOptions[BOXGPSHOLD] && abs(rcCommand[ROLL])< AP_MODE && abs(rcCommand[PITCH]) < 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);
              #else
                GPS_hold[LAT] = GPS_coord[LAT];
                GPS_hold[LON] = GPS_coord[LON];
                GPS_set_next_wp(&GPS_hold[LAT],&GPS_hold[LON]);
                nav_mode = NAV_MODE_POSHOLD;
              #endif
            }
          } else {
            f.GPS_HOLD_MODE = 0;
            // both boxes are unselected here, nav is reset if not already done
            if (GPSNavReset == 0 ) {
              GPSNavReset = 1;
              GPS_reset_nav();
            }
          }
        }
      } else {
        f.GPS_HOME_MODE = 0;
        f.GPS_HOLD_MODE = 0;
        #if !defined(I2C_GPS)
          nav_mode = NAV_MODE_NONE;
        #endif
      }
    #endif
    
    #if defined(FIXEDWING) || defined(HELICOPTER)              //另外的机型的模式 与四轴无关
      if (rcOptions[BOXPASSTHRU]) {f.PASSTHRU_MODE = 1;}
      else {f.PASSTHRU_MODE = 0;}
    #endif
 
  }    //RC循环到此为止
  else      //若未进入RC则依次进行以下5个任务
  { 
    static uint8_t taskOrder=0; // 不把所有的任务放在一个循环中,避免高延迟使得RC循环无法进入。
    if(taskOrder>4) taskOrder-=5;
    switch (taskOrder) {
      case 0:
        taskOrder++;
        #if MAG                               //获取MAG数据
          if (Mag_getADC()) break; // max 350 µs (HMC5883) // only break when we actually did something
        #endif
      case 1:
        taskOrder++;
        #if BARO                              //获取BARO数据
          if (Baro_update() != 0 ) break;
        #endif
      case 2:
        taskOrder++;
        #if BARO                              //获取BARO数据
          if (getEstimatedAltitude() !=0) break;
        #endif    
      case 3:
        taskOrder++;
        #if GPS                                 //获取GPS数据
          if(GPS_Enable) GPS_NewData();
          break;
        #endif
      case 4:
        taskOrder++;
        #if SONAR                            //获取SONAR (声呐)数据
          Sonar_update();debug[2] = sonarAlt;
        #endif
        #ifdef LANDING_LIGHTS_DDR
          auto_switch_landing_lights();
        #endif
        #ifdef VARIOMETER
          if (f.VARIO_MODE) vario_signaling();
        #endif
        break;
    }
  }
 
  computeIMU();                          //计算IMU(惯性测量单元)
  // Measure loop rate just afer reading the sensors
  currentTime = micros();
  cycleTime = currentTime - previousTime;
  previousTime = currentTime;
 
  #ifdef CYCLETIME_FIXATED
    if (conf.cycletime_fixated) {
      if ((micros()-timestamp_fixated)>conf.cycletime_fixated) {
      } else {
         while((micros()-timestamp_fixated)<conf.cycletime_fixated) ; // waste away
      }
      timestamp_fixated=micros();
    }
  #endif
  //***********************************
  //**** Experimental FlightModes *****  实验的飞行模式
  //***********************************
  #if defined(ACROTRAINER_MODE)          //固定翼训练者模式
    if(f.ANGLE_MODE)
    {
        if (abs(rcCommand[ROLL]) + abs(rcCommand[PITCH]) >= ACROTRAINER_MODE )  //ACROTRAINER_MODE=200
        {
          f.ANGLE_MODE=0;                         //取消自稳 定向 定高 GPS回家 GPS定点
          f.HORIZON_MODE=0;                
          f.MAG_MODE=0;
          f.BARO_MODE=0;
          f.GPS_HOME_MODE=0;
          f.GPS_HOLD_MODE=0;
        }
    }
  #endif
 //*********************************** 
  #if MAG       //磁场定向的算法   保持机头方向不变
    if (abs(rcCommand[YAW]) <70 && f.MAG_MODE)    //开启定高,且yaw控制值在1430-1570之间
    {
       int16_t dif = heading - magHold;
       if (dif <= - 180) dif += 360;                                       //转过头了从另一方向转回去
       if (dif >= + 180) dif -= 360;                                       //转过头了从另一方向转回去
       if ( f.SMALL_ANGLES_25 ) rcCommand[YAW] -= dif*conf.P8[PIDMAG]>>5;   //dif*pidmag/32    
     } 
     else magHold = heading;
  #endif
 
  #if BARO && (!defined(SUPPRESS_BARO_ALTHOLD)) //气压计定高算法
    if (f.BARO_MODE)     //若开启了气压定高
    {
       static uint8_t isAltHoldChanged = 0;
       #if defined(ALTHOLD_FAST_THROTTLE_CHANGE)   //默认开启的
         if (abs(rcCommand[THROTTLE]-initialThrottleHold) > ALT_HOLD_THROTTLE_NEUTRAL_ZONE) //initialThrottleHold=开启气压定高时的油门值    ALT_HOLD_THROTTLE_NEUTRAL_ZONE=40;  控制量超过死区 则开始执行
         {
            errorAltitudeI = 0;     
            isAltHoldChanged = 1;         //气压定点改变标志位
            rcCommand[THROTTLE] += (rcCommand[THROTTLE] > initialThrottleHold) ? -ALT_HOLD_THROTTLE_NEUTRAL_ZONE : ALT_HOLD_THROTTLE_NEUTRAL_ZONE;    //减去 或者加上 死区
          } 
          else 
          {
             if (isAltHoldChanged) 
             {
                AltHold = EstAlt;               //改变定高高度为现在估计高度 cm
                isAltHoldChanged = 0;
             }
             rcCommand[THROTTLE] = initialThrottleHold + BaroPID;//油门控制量=initialThrottleHold+PID控制量 非增量式PID控制
          }
        #else
        static int16_t AltHoldCorr = 0;
        if (abs(rcCommand[THROTTLE]-initialThrottleHold)>ALT_HOLD_THROTTLE_NEUTRAL_ZONE)
        {
  // 缓慢增加或减少气压定高的高度,其值与操作杆位移有关(+100的油门(与开启定高时相比)使其1s升高50cm(程序循环时间3-4ms))
           AltHoldCorr+= rcCommand[THROTTLE] - initialThrottleHold;//每个循环累加
           if(abs(AltHoldCorr) > 500)                      //累加大于500
           {
              AltHold += AltHoldCorr/500;              //改变气压定高高度。单位cm
              AltHoldCorr %= 500;                            
           }
           errorAltitudeI = 0;
           isAltHoldChanged = 1;                           //高度改变标志位记1
        } 
        else if (isAltHoldChanged) 
        {
          AltHold = EstAlt;
          isAltHoldChanged = 0;
        }
        rcCommand[THROTTLE] = initialThrottleHold + BaroPID;//油门控制量=initialThrottleHold+PID控制量 非增量式PID控制
      #endif
    }
  #endif
  #if GPS   //与GPS有关
    if ( (f.GPS_HOME_MODE || f.GPS_HOLD_MODE) && f.GPS_FIX_HOME ) {
      float sin_yaw_y = sin(heading*0.0174532925f);
      float cos_yaw_x = cos(heading*0.0174532925f);
      #if defined(NAV_SLEW_RATE)     
        nav_rated[LON]   += constrain(wrap_18000(nav[LON]-nav_rated[LON]),-NAV_SLEW_RATE,NAV_SLEW_RATE);
        nav_rated[LAT]   += constrain(wrap_18000(nav[LAT]-nav_rated[LAT]),-NAV_SLEW_RATE,NAV_SLEW_RATE);
        GPS_angle[ROLL]   = (nav_rated[LON]*cos_yaw_x - nav_rated[LAT]*sin_yaw_y) /10;
        GPS_angle[PITCH]  = (nav_rated[LON]*sin_yaw_y + nav_rated[LAT]*cos_yaw_x) /10;
      #else 
        GPS_angle[ROLL]   = (nav[LON]*cos_yaw_x - nav[LAT]*sin_yaw_y) /10;
        GPS_angle[PITCH]  = (nav[LON]*sin_yaw_y + nav[LAT]*cos_yaw_x) /10;
      #endif
    } else {
      GPS_angle[ROLL]  = 0;
      GPS_angle[PITCH] = 0;
    }
  #endif
 
  //**** PITCH & ROLL & YAW PID ****
  int16_t prop;
  prop = min(max(abs(rcCommand[PITCH]),abs(rcCommand[ROLL])),500); // range [0;500]prop pitch和roll控制量大的。
  for(axis=0;axis<3;axis++) 
  {
     if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis<2 ) // 开启自稳或者HORIZON模式 axis=pitch|roll
     //此项为闭环控制,有角度作为反馈,控制信号消失,四轴回中。
     { 
        // 最大倾斜角为50度
        errorAngle = constrain((rcCommand[axis]<<1) + GPS_angle[axis],-500,+500) - angle[axis] + conf.angleTrim[axis]; //控制5为1度
        //误差角度(要到的角度与现角度之差+角度修正),角度修正 即微调ACC
        PTermACC = ((int32_t)errorAngle*conf.P8[PIDLEVEL])>>7;   //自稳ACC比例部分值。角度误差*P level除以128
        //计算需要32位 errorAngle*P8[PIDLEVEL] 会超过 32768  结果只需要16位数据。
        PTermACC = constrain(PTermACC,-conf.D8[PIDLEVEL]*5,+conf.D8[PIDLEVEL]*5); //限定PtermAcc的范围
        errorAngleI[axis]   = constrain(errorAngleI[axis]+errorAngle,-10000,+10000);        // 累加ACC角度积分项误差
        ITermACC              = ((int32_t)errorAngleI[axis]*conf.I8[PIDLEVEL])>>12;               // 自稳ACC积分部分值。/4096
     }
     if ( !f.ANGLE_MODE || f.HORIZON_MODE || axis == 2 ) //若普通模式或开启了HORIZON或者axis=yaw
     //此项为开环控制,即有控制量则一直进行动作,并且由于积分项的存在,动作速度会有加快。当控制信号消失,四轴保持该状态。
     { 
        if (abs(rcCommand[axis])<500) error =  (rcCommand[axis]<<6)/conf.P8[axis] ; //误差=rcCommand[axis]*64/P值 ?
        else error = ((int32_t)rcCommand[axis]<<6)/conf.P8[axis] ; // 32 bits is needed for calculation
        error -= gyroData[axis];
        PTermGYRO = rcCommand[axis];                // GYRO 比例部分值。感觉应该等于与ACC类似?
        errorGyroI[axis]  = constrain(errorGyroI[axis]+error,-16000,+16000);   //累加GYRO角度积分项误差
        if (abs(gyroData[axis])>640) errorGyroI[axis] = 0;
        ITermGYRO = ((errorGyroI[axis]>>7)*conf.I8[axis])>>6;  //GYRO积分部分值。errorGyroI[axis]除以128乘以I8除以64 总共右移13位 大于ACC的12位      I8<250;
     }
     if ( f.HORIZON_MODE && axis<2) 
     {
        PTerm = ((int32_t)PTermACC*(512-prop) + (int32_t)PTermGYRO*prop)>>9;         // the real factor should be 500, but 512 is ok
        ITerm = ((int32_t)ITermACC*(512-prop) + (int32_t)ITermGYRO*prop)>>9;             //综合 Pacc与Pgyro并用prop进行修正  控制量大调小自稳系数,控制量小,以自稳为主。     除以512
      } 
      else 
      {
         if ( f.ANGLE_MODE && axis<2)       //开启自稳,则使用 PTermACC  ITermACC  
         {    
            PTerm = PTermACC;
            ITerm = ITermACC;
         }
         else                                                   //未开自稳,则使用PTermGYRO  ITermGYRO
         {
            PTerm = PTermGYRO;
            ITerm = ITermGYRO;
         }
      }
      PTerm -= ((int32_t)gyroData[axis]*dynP8[axis])>>6; 
      // dynP8=P8*prop1/100;  与当时油门有关 与 rate大  则dyn小  用角加速度值对角度P值进行修正。 /64
      delta     = gyroData[axis] - lastGyro[axis];  // 2次连续的gyro 最大不会超过800  角速度变化(角加速度)
      lastGyro[axis] = gyroData[axis];                                   //保存当前角速度值
      deltaSum       = delta1[axis]+delta2[axis]+delta;       //对角速度差(角加速度)值进行累加  
      delta2[axis]    = delta1[axis];
      delta1[axis]    = delta;
      DTerm = ((int32_t)deltaSum*dynD8[axis])>>5;        // 与当时油门有关 与 rate大  则dyn小  用角加速度值对角度P值进行修正。 /32
      axisPID[axis] =  PTerm + ITerm - DTerm;
  }
  mixTable();                   //设置各个电机的输出
  writeServos();               //舵机输出
  writeMotors();             //电机输出
}

MWC v2.2 代码解读ACC_Common()

 
ACC校准 以及改变传感器的朝向问题以解决某些时候无法正常安装传感器,得到最终结果是ACCADC【3】
void ACC_Common() 
{
   static int32_t a[3];
   if (calibratingA>0)                                        //校准ACC
   {
      for (uint8_t axis = 0; axis < 3; axis++) 
      {     
         if (calibratingA == 512) a[axis]=0;         // 在校准的最开始,清零a[axis]
         a[axis] +=accADC[axis];                          // 对512次读取求和      
         accADC[axis]=0;
         global_conf.accZero[axis]=0;                 // 清零全局变量
      }
    // Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
      if (calibratingA == 1)
      {
         global_conf.accZero[ROLL]  = a[ROLL]>>9;                //除以512
         global_conf.accZero[PITCH] = a[PITCH]>>9;              //除以512
         global_conf.accZero[YAW]   = (a[YAW]>>9)-acc_1G; //  ADXL345 acc_1G=265
         conf.angleTrim[ROLL]   = 0;
         conf.angleTrim[PITCH]  = 0;
         writeGlobalSet(1);                                                          //将零点信息写入 EEPROM
       }
       calibratingA--;             //每个主函数减1,512次主循环校准结束,花费大概512*3ms=1.5S
   }
   #if defined(INFLIGHT_ACC_CALIBRATION)
     static int32_t b[3];
     static int16_t accZero_saved[3]  = {0,0,0};
     static int16_t  angleTrim_saved[2] = {0, 0};
      //在校准之前,存储原来的零点
      if (InflightcalibratingA==50)    //空中校准ACC 50个循环之内        可改为64便于之后除法使用移位的方法
      {
         accZero_saved[ROLL]  = global_conf.accZero[ROLL] ;
         accZero_saved[PITCH] = global_conf.accZero[PITCH];
         accZero_saved[YAW]   = global_conf.accZero[YAW] ;
         angleTrim_saved[ROLL]  = conf.angleTrim[ROLL] ;
         angleTrim_saved[PITCH] = conf.angleTrim[PITCH] ;
      }
      if (InflightcalibratingA>0)
      {
        for (uint8_t axis = 0; axis < 3; axis++) 
        {
           if (InflightcalibratingA == 50) b[axis]=0;  
           b[axis] +=accADC[axis];                   //求和50次测量值
           accADC[axis]=0;
           global_conf.accZero[axis]=0;          // 清零全局变量
         }
        if (InflightcalibratingA == 1)             //校准结束
        {
          AccInflightCalibrationActive = 0;
          AccInflightCalibrationMeasurementDone = 1;
          #if defined(BUZZER)
            alarmArray[7] = 1;      //buzzer for indicatiing the end of calibration
          #endif
          // recover saved values to maintain current flight behavior until new values are transferred
          global_conf.accZero[ROLL]  = accZero_saved[ROLL] ;
          global_conf.accZero[PITCH] = accZero_saved[PITCH];
          global_conf.accZero[YAW]   = accZero_saved[YAW] ;
          conf.angleTrim[ROLL]  = angleTrim_saved[ROLL] ;
          conf.angleTrim[PITCH] = angleTrim_saved[PITCH] ;
        }
        InflightcalibratingA--;
      }
      // 校准结束后,计算平均值, 更改 Z 通过1G 存储值在 EEPROM 
      if (AccInflightCalibrationSavetoEEProm == 1)//飞行器降落, 锁定之后有效
      {  
        AccInflightCalibrationSavetoEEProm = 0;
        global_conf.accZero[ROLL]  = b[ROLL]/50;
        global_conf.accZero[PITCH] = b[PITCH]/50;
        global_conf.accZero[YAW]   = b[YAW]/50-acc_1G; // for nunchuk 200=1G
        conf.angleTrim[ROLL]   = 0;
        conf.angleTrim[PITCH]  = 0;
        writeGlobalSet(1);
      }
  #endif
  accADC[ROLL]  -=  global_conf.accZero[ROLL] ;
  accADC[PITCH] -=  global_conf.accZero[PITCH];
  accADC[YAW]   -=  global_conf.accZero[YAW] ;
 
  #if defined(SENSORS_TILT_45DEG_LEFT)                   //旋转45度  有精度损失。
    int16_t temp = ((accADC[PITCH] - accADC[ROLL] )*7)/10;
    accADC[ROLL] = ((accADC[ROLL]  + accADC[PITCH])*7)/10;
    accADC[PITCH] = temp;
  #endif
  #if defined(SENSORS_TILT_45DEG_RIGHT)
    int16_t temp = ((accADC[PITCH] + accADC[ROLL] )*7)/10;
    accADC[ROLL] = ((accADC[ROLL]  - accADC[PITCH])*7)/10;
    accADC[PITCH] = temp;
  #endif
}

MWC v2.2 代码解读go_arm() go_disarm()

 
解锁,上锁函数。解锁时进行无头模式头标定和气压计基准标定。
void go_arm()
{
   if(calibratingG == 0 && f.ACC_CALIBRATED 
   #if defined(FAILSAFE)
     && failsafeCnt < 2
   #endif
    ) 
    {
       if(!f.ARMED) //若未解锁
       { 
         f.ARMED = 1;                                             //解锁标志位置1
         headFreeModeHold = heading;              //定义无头模式的头 即刺客四轴头的朝向
         #if defined(VBAT)
         if (vbat > conf.no_vbat) vbatMin = vbat; //跟电池电压有关,无关紧要。
         #endif
         #ifdef LCD_TELEMETRY                            // 解锁时重置一些参数
           #if BARO
               BAROaltMax = BaroAlt;                      //气压最大值等于解锁时的气压值
           #endif
         #endif
         #ifdef LOG_PERMANENT
         plog.arm++;                                              // 解锁事件
         plog.running = 1;                                     // 飞行器运行事件
         // write now.
         writePLog();                                              //写入eerom
         #endif
        }
   } 
   else if(!f.ARMED) 
   { 
      blinkLED(2,255,1);                                       //灯闪烁
      alarmArray[8] = 1;
   }
}
void go_disarm() 
{
   if (f.ARMED) 
   {
     f.ARMED = 0;            //解锁标志位清0
     #ifdef LOG_PERMANENT
     plog.disarm++;        // 锁定事件
     plog.armed_time = armedTime ;        //解锁时间保存入日志
     if (failsafeEvents) plog.failsafe++;      // 掉落保护事件进日志
     if (i2c_errors_count > 10) plog.i2c++; // i2c错误事件进日志
     plog.running = 0;       //飞行停止事件
     // write now.
     writePLog();                //日志写入eerom
     #endif
    }
}
MWC v2.2 代码解读computeRC()
对遥控接收的信号进行循环滤波,取4组数据,即80MS,算出平均值,把大于平均值的减小2,小于平均值的增大2.
void computeRC() 
{
  static uint16_t rcData4Values[RC_CHANS][4], rcDataMean[RC_CHANS];  //  chan 通道
  static uint8_t rc4ValuesIndex = 0;
  uint8_t chan,a;
  #if !(defined(RCSERIAL) || defined(OPENLRSv2MULTI)) // dont know if this is right here
     #if defined(SBUS)
       readSBus();
     #endif
     rc4ValuesIndex++;                      
     if (rc4ValuesIndex == 4) rc4ValuesIndex = 0;           // 到达4 就重置为0
     for (chan = 0; chan < RC_CHANS; chan++)             //rc_CHAN 遥控通道数
    {            
        #if defined(FAILSAFE)                                               
        uint16_t rcval = readRawRC(chan);                       //读取 rcValue[rcChannel[chan]]
        if(rcval>FAILSAFE_DETECT_TRESHOLD || chan > 3)
       // 当脉冲大于AILSAFE_DETECT_TRESHOLD时上传通道值  剔除太短的坏值。
       {     
           rcData4Values[chan][rc4ValuesIndex] = rcval;   //4组通道值
        }
      #else
        rcData4Values[chan][rc4ValuesIndex] = readRawRC(chan);  //直接赋值4组通道值
      #endif
      rcDataMean[chan] = 0;
      for (a=0;a<4;a++) rcDataMean[chan] += rcData4Values[chan][a];
      rcDataMean[chan]= (rcDataMean[chan]+2)>>2;                    //求4通道平均值
      if ( rcDataMean[chan] < (uint16_t)rcData[chan] -3)  rcData[chan] = rcDataMean[chan]+2;  //对各个通道进行循环滤波
      if ( rcDataMean[chan] > (uint16_t)rcData[chan] +3)  rcData[chan] = rcDataMean[chan]-2;
    }
  #endif
}
MWC V2.2 代码解读setup()
函数主要功能是开启一些传输外设,设置一些引脚状态,读取之前四轴一些常规参数。很出乎意料没有对于传感器的校准。本人接触Arduino时间不长,难免有错误,欢迎指正。
void setup() {
  #if !defined(GPS_PROMINI)                                                 设置GPS串口通信
    SerialOpen(0,SERIAL0_COM_SPEED);
    #if defined(PROMICRO)
      SerialOpen(1,SERIAL1_COM_SPEED);
    #endif
    #if defined(MEGA)
      SerialOpen(1,SERIAL1_COM_SPEED);
      SerialOpen(2,SERIAL2_COM_SPEED);
      SerialOpen(3,SERIAL3_COM_SPEED);
    #endif
  #endif
  LEDPIN_PINMODE;                   设置LED引脚状态
  POWERPIN_PINMODE;             设置POWER引脚状态
  BUZZERPIN_PINMODE;            设置BUZZER引脚状态
  STABLEPIN_PINMODE;             设置 STABLE引脚状态
  POWERPIN_OFF;                        POWER引脚输出0
  initOutput();                              使能所有PWM引脚
  #ifdef MULTIPLE_CONFIGURATION_PROFILES     多种配置支持 存储在EEPROM中
    for(global_conf.currentSet=0; global_conf.currentSet<3; global_conf.currentSet++) {  // check all settings integrity
      readEEPROM();
    }
  #else
    global_conf.currentSet=0;               
    readEEPROM();
  #endif
  readGlobalSet();                                   //读取acc零点  mag零点 currentset
  readEEPROM();                                    // load current setting data
  blinkLED(2,40,global_conf.currentSet+1);          
  configureReceiver();                            //设置RC接收引脚状态,如aux2 
  #if defined (PILOTLAMP)                    //信号灯有关
    PL_INIT;
  #endif
  #if defined(OPENLRSv2MULTI)          //sensors 板
    initOpenLRS();                                   //初始化一些引脚
  #endif
  initSensors();                                       //初始化IIC接口,以及10DOF传感器芯片,并把标志位f.I2C_INIT_DONE置1
  #if defined(I2C_GPS) || defined(GPS_SERIAL) || defined(GPS_FROM_OSD)
    GPS_set_pids();
  #endif
  previousTime = micros();                   //运行微秒数
  #if defined(GIMBAL)                         //平衡
   calibratingA = 512;                           //矫正时间=512=12s
  #endif
  calibratingG = 512;                            //矫正时间G=512*25M=12s
  calibratingB = 200;  // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles  
  #if defined(POWERMETER)             //功率计
    for(uint8_t i=0;i<=PMOTOR_SUM;i++)
      pMeter[i]=0;
  #endif
 
  #if defined(GPS_SERIAL)                 //串行GPS
    GPS_SerialInit();
    for(uint8_t i=0;i<=5;i++){               
      GPS_NewData();                          //更新GPS数据
      LEDPIN_ON
      delay(20);
      LEDPIN_OFF
      delay(80);
    }
    if(!GPS_Present){                          //校验和正确标志位
      SerialEnd(GPS_SERIAL);                           //开普通串口 关闭GPS串口
      SerialOpen(0,SERIAL0_COM_SPEED);
    }
    #if !defined(GPS_PROMINI)
      GPS_Present = 1;
    #endif
    GPS_Enable = GPS_Present;    
  #endif
 
 
  #if defined(I2C_GPS) || defined(TINY_GPS) || defined(GPS_FROM_OSD)
   GPS_Enable = 1;                         //启用GPS
  #endif
  
  #if defined(LCD_ETPP) || defined(LCD_LCD03) || defined(OLED_I2C_128x64) || defined(LCD_TELEMETRY_STEP)
    initLCD();                                   //初始化LCD
  #endif
  #ifdef LCD_TELEMETRY_DEBUG
    telemetry_auto = 1;
  #endif
  #ifdef LCD_CONF_DEBUG
    configurationLoop();
  #endif
  #ifdef LANDING_LIGHTS_DDR
    init_landing_lights();
  #endif
  ADCSRA |= _BV(ADPS2) ; ADCSRA &= ~_BV(ADPS1); ADCSRA &= ~_BV(ADPS0); // 这要的模拟数据读取速度使得分辨率损失小
  #if defined(LED_FLASHER)
    init_led_flasher();
    led_flasher_set_sequence(LED_FLASHER_SEQUENCE);
  #endif
  f.SMALL_ANGLES_25=1; // important for gyro only conf  倾斜角小于25度
  #ifdef LOG_PERMANENT
    // read last stored set
    readPLog();                    //读取一些设置 是否锁定,解锁时间,IIC错误数等
    plog.lifetime += plog.armed_time / 1000000;
    plog.start++;         // #powercycle/reset/initialize events
    // dump plog data to terminal
    #ifdef LOG_PERMANENT_SHOW_AT_STARTUP
      dumpPLog(0);
    #endif
    plog.armed_time = 0;   // lifetime in seconds
    //plog.running = 0;       // toggle on arm & disarm to monitor for clean shutdown vs. powercut
  #endif
 
  debugmsg_append_str("initialization completed\n");
}
posted @ 2014-08-02 13:14  落日归侠  阅读(6888)  评论(0编辑  收藏  举报