Планирование и пикирование

Он заключается в том, чтобы подтягивать текущие значения вращения, полученный через гироскоп, соответственным по положениям стиков, формируя из их разности сигнал ошибки для ПИД-регулятора. Они воздействует на управляющие элементы - положения рулей.

Схема однокоптера -

          M1 CCW
          |
          |
.
          M2 (Servo)
          |
          |
   M5 ---- ---- M3
  (Servo) | (Servo)
          |
          M4 (Servo)

Выделим самые ответственные куски кода для файла kk.c -

//Входим в бесконечный цикл
//Значения, прочитанные с каждой гиры, за вычетом смещения нуля.
 gyroADC[ROLL]-= gyroZero[ROLL];
 gyroADC[PITCH]-= gyroZero[PITCH];
 gyroADC[YAW]-= gyroZero[YAW];

//Начальные значения на сервах и моторах, соответственные для M1-M5 - 
 MotorOut1 = RxInCollective;    //RxInCollective - значение throttle приемника
 MotorOut2 = 840;  //840
 MotorOut3 = 840;  //840
 MotorOut4 = 945;  //840+840/8  //Видимо, чтобы скомпенсировать момент винта
 MotorOut5 = 945;  //840+840/8

//Шаг 1, для вращения ROLL.
//Нормализация данных - с приемника RxInRoll и прочитанного с гироскопа gyroADC[ROLL]
//на коэффициент, заданный пот-ом на плате.
 RxInRoll = ((int32_t)RxInRoll * (uint32_t)GainInADC[ROLL]) >> STICK_GAIN_SHIFT;
 gyroADC[ROLL] = ((int32_t)gyroADC[ROLL] * (uint32_t)GainInADC[ROLL]) >> GYRO_GAIN_SHIFT;

//Пид-регулятор, желаемое положение RxInRoll, текущее - gyroADC[ROLL].
if (0) {
 error = RxInRoll - gyroADC[ROLL];
 integral[ROLL]+= error;
 derivative = error - last_error[ROLL];
 last_error[ROLL] = error;
 RxInRoll+= error + (integral[ROLL] >> 2) + (derivative >> 2);
} else {
 RxInRoll-= gyroADC[ROLL]; 
}

//применяем результат
 MotorOut2+=RxInRoll;
 MotorOut4-=RxInRoll;

//Далее, то же для крена, тангажа и т.д.
//конец бесконечного цикла

PS. MotorOut2, RxInRoll, gyroADC[ROLL] имеют одну размерность - частота вращения.