Page 363 - 2

//**** PITCH & ROLL & YAW PID ****
#if PID_CONTROLLER == 1 // evolved oldschool
if ( f.HORIZON_MODE ) prop = min(max(abs(rcCommand[PITCH]),abs(rcCommand[ROLL])),512);

// PITCH & ROLL
for(axis=0;axis<2;axis++) {
rc = rcCommand[axis]<<1;
error = rc - imu.gyroData[axis];
errorGyroI[axis] = constrain(errorGyroI[axis]+error,-16000,+16000); // WindUp 16
bits is ok here
if (abs(imu.gyroData[axis])>640) errorGyroI[axis] = 0;

ITerm = (errorGyroI[axis]>>7)*conf.pid[axis].I8>>6; // 16 bits is ok h
ere 16000/125 = 128 ; 128*250 = 32000

PTerm = mul(rc,conf.pid[axis].P8)>>6;

if (f.ANGLE_MODE || f.HORIZON_MODE) { // axis relying on ACC
// 50 degrees max inclination
errorAngle = constrain(rc + GPS_angle[axis],-500,+500) - att.angle[axis] + conf.
angleTrim[axis]; //16 bits is ok here
errorAngleI[axis] = constrain(errorAngleI[axis]+errorAngle,-10000,+10000);
// WindUp //16 bits is ok here

PTermACC = mul(errorAngle,conf.pid[PIDLEVEL].P8)>>7; // 32 bits is needed fo
r calculation: errorAngle*P8 could exceed 32768 16 bits is ok for result

int16_t limit = conf.pid[PIDLEVEL].D8*5;
PTermACC = constrain(PTermACC,-limit,+limit);

ITermACC = mul(errorAngleI[axis],conf.pid[PIDLEVEL].I8)>>12; // 32 bits is n
eeded for calculation:10000*I8 could exceed 32768 16 bits is ok for result

ITerm = ITermACC + ((ITerm-ITermACC)*prop>>9);
PTerm = PTermACC + ((PTerm-PTermACC)*prop>>9);
}

PTerm -= mul(imu.gyroData[axis],dynP8[axis])>>6; // 32 bits is needed for calculation

delta = imu.gyroData[axis] - lastGyro[axis]; // 16 bits is ok here, the dif between
2 consecutive gyro reads is limited to 800
lastGyro[axis] = imu.gyroData[axis];
DTerm = delta1[axis]+delta2[axis]+delta;
delta2[axis] = delta1[axis];

- 363 -