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

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

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

итак,

//Пид-регулятор, желаемое положение 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 - константа)
 MotorOut2+=RxInRoll;

Сразу бросается в глаза, что это НЕ ПИД-регулятор. Расстройка должна добавляться не к RxInRoll, а к gyroADC[ROLL], и уже этот результат заводИться на MotorOut2. Поэтому оригинальный код похож на ПИД-регулятор, но таким не является, например, если промоделировать в сравнении с нормальным ПИДР -

Здесь синяя линия - положения стиков, красная - как ведет к ним ПИДР, зеленая - «ПИДР» от КК. В принципе, одинаково, но если приблизить -

Когда вращение постоянно, то они совпадают по поведению, а вот если скорость вращения будет линейно расти, то будет разница, в примере ~0.15. Много это или мало? В принципе пофигу, может, автор кода пожертвовал точностью в угоду простоте кода.

Моделирующая программа на Дельфи