MWC v2.2 代码解读.docVIP

[转载]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 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; i3; 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++; ?????????????