飞控MWC v22 代码解读

LOOP()

转载]MWC v2.2 [代码解读 (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; 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