函数很长不用文字了 贴个流程图，说明一切：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 motorsstatic uint8_t rcSticks; // this hold sticks position for command combosuint8_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) i 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 preventsf.OK_TO_ARM = 0; // 进入锁定状态，之后起飞需要解锁}failsafeEvents++; //掉落保护事件标志位至 1}if ( failsafeCnt > (5*FAILSAFE_DELAY) // This will prevent the copter to automatically rearm if failsafe shuts it down and preventsf.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>= 2; //stTmp 除以 4if(rcData[i] > MINCHECK) stTmp |= 0x80; // MINCHECK=1100 1000 0000Bif(rcData[i] 0) // Arming/Disarming via ARM BOX{ if ( rcOptions[BOXARM] //解锁else if (f.ARMED) go_disarm(); //上锁}}if(rcDelayCommand == 20) //若控制杆在最大最小位置外的状态未改变(20*20ms){if(f.ARMED) // 当处在解锁时{ #ifdef ALLOW_ARM_DISARM_VIA_TX_YAW //上锁方式 1if (conf.activate[BOXARM] == 0 // Disarm via YAW#endif#ifdef ALLOW_ARM_DISARM_VIA_TX_ROLL //上锁方式 2if (conf.activate[BOXARM] == 0 // 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 BAROcalibratingB=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 //配置 1else if (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_CE) i=2; // PITCH up -> Profile 2 //配置 2else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_HI) i=3; // ROLL right -> Profile 3 //配置 3if(i) {global_conf.currentSet = i-1;writeGlobalSet(0);readEEPROM();blinkLED(2,40,i);alarmArray[0] = i;}#endifif (rcSticks == THR_LO + YAW_HI + PIT_HI + ROL_CE) // 进入 LCD配置{ #if defined(LCD_CONF)configurationLoop(); // beginning LCD configuration#endifpreviousTime = micros(); //设置时间}#ifdef ALLOW_ARM_DISARM_VIA_TX_YAW //允许使用YAW 进行解锁else if (conf.activate[BOXARM] == 0 // Arm via YAW#endif#ifdef ALLOW_ARM_DISARM_VIA_TX_ROLLelse if (conf.activate[BOXARM] == 0 // 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;} elsetel。