Page 364 - 2

delta1[axis] = delta;

DTerm = mul(DTerm,dynD8[axis])>>5; // 32 bits is needed for calculation

axisPID[axis] = PTerm + ITerm - DTerm;
}

//YAW
#define GYRO_P_MAX 300
#define GYRO_I_MAX 250

rc = mul(rcCommand[YAW] , (2*conf.yawRate + 30)) >> 5;

error = rc - imu.gyroData[YAW];
errorGyroI_YAW += mul(error,conf.pid[YAW].I8);
errorGyroI_YAW = constrain(errorGyroI_YAW, 2-((int32_t)1<<28), -2+((int32_t)1<<28));
if (abs(rc) > 50) errorGyroI_YAW = 0;

PTerm = mul(error,conf.pid[YAW].P8)>>6;
#ifndef COPTER_WITH_SERVO
int16_t limit = GYRO_P_MAX-conf.pid[YAW].D8;
PTerm = constrain(PTerm,-limit,+limit);
#endif

ITerm = constrain((int16_t)(errorGyroI_YAW>>13),-GYRO_I_MAX,+GYRO_I_MAX);

axisPID[YAW] = PTerm + ITerm;

#elif PID_CONTROLLER == 2 // alexK
#define GYRO_I_MAX 256
#define ACC_I_MAX 256
prop = min(max(abs(rcCommand[PITCH]),abs(rcCommand[ROLL])),500); // range [0;500]

//----------PID controller----------
for(axis=0;axis<3;axis++) {
//-----Get the desired angle rate depending on flight mode
if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis<2 ) { // MODE relying on ACC
// calculate error and limit the angle to 50 degrees max inclination
errorAngle = constrain((rcCommand[axis]<<1) + GPS_angle[axis],-500,+500) - att.angle[a
xis] + conf.angleTrim[axis]; //16 bits is ok here
}
if (axis == 2) {//YAW is always gyro-controlled (MAG correction is applied to rcCommand)
AngleRateTmp = (((int32_t) (conf.yawRate + 27) * rcCommand[2]) >> 5);
} else {
if (!f.ANGLE_MODE) {//control is GYRO based (ACRO and HORIZON - direct sticks control

- 364 -