Page 365 - 2

is applied to rate PID
AngleRateTmp = ((int32_t) (conf.rollPitchRate + 27) * rcCommand[axis]) >> 4;
if (f.HORIZON_MODE) {
//mix up angle error to desired AngleRateTmp to add a little auto-level feel
AngleRateTmp += ((int32_t) errorAngle * conf.pid[PIDLEVEL].I8)>>8;
}
} else {//it's the ANGLE mode - control is angle based, so control loop is needed
AngleRateTmp = ((int32_t) errorAngle * conf.pid[PIDLEVEL].P8)>>4;
}
}

//--------low-level gyro-based PID. ----------
//Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
//-----calculate scaled error.AngleRates
//multiplication of rcCommand corresponds to changing the sticks scaling here
RateError = AngleRateTmp - imu.gyroData[axis];

//-----calculate P component
PTerm = ((int32_t) RateError * conf.pid[axis].P8)>>7;

//-----calculate I component
//there should be no division before accumulating the error to integrator, because the precis
ion would be reduced.
//Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used.
//Time correction (to avoid different I scaling for different builds based on average cycle ti
me)
//is normalized to cycle time = 2048.
errorGyroI[axis] += (((int32_t) RateError * cycleTime)>>11) * conf.pid[axis].I8;
//limit maximum integrator value to prevent WindUp - accumulating extreme values when sy
stem is saturated.
//I coefficient (I8) moved before integration to make limiting independent from PID settings
errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) -GYRO_I_MAX<<13, (int32_t) +GYR
O_I_MAX<<13);
ITerm = errorGyroI[axis]>>13;

//-----calculate D-term
delta = RateError - lastError[axis]; // 16 bits is ok here, the dif between 2 conse
cutive gyro reads is limited to 800
lastError[axis] = RateError;

//Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calcul
ated difference
// would be scaled by different dt each time. Division by dT fixes that.
delta = ((int32_t) delta * ((uint16_t)0xFFFF / (cycleTime>>4)))>>6;
//add moving average here to reduce noise

- 365 -