MWC v2.2 代码解读LOOP()_何处不江南

http://blog.sina.com.cn/u/1703922654

首页博文目录关于我

个人资料

微博

加好友发纸条

写留言加关注

博客等级：
博客积分：

博客访问：
关注人气：
获赠金笔：0支
赠出金笔：0支
荣誉徽章：

正文字体大小：大中小

MWC v2.2 代码解读LOOP()

(2013-04-01 15:34:13)

标签：

arduino

mwc

代码

loop

分类：四轴飞行器

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

http://s14/mw690/658fcbdegd947e8ebeffd&690v2.2 代码解读LOOP()" TITLE="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

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

#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(); //电机输出

}

阅读┊ 收藏 ┊ 喜欢 ▼ ┊打印┊举报/Report

前一篇：MWC v2.2 代码解读annexCode()

后一篇：新机架

新浪BLOG意见反馈留言板　欢迎批评指正