Вертолет на 4 лопастях.

Решил создать вертолет (quadcopter), управление через компьютер используя беспроводную сеть WiFi.
Компоненты:
Arduino (к нему подключены - MPU6050, ESC 30A, 4 двигателя) - контролер управления.
Raspberry pi (Wifi + COM-порт подключение к Arduino) - Скрипты на python для управления вертолетам
Все что я перечислил подключено и работает.

Для начала задача просто взлететь

Чего я добился:
1. Работая с MPU6050 я понял что чип дает большие шумы, что бы их сгладить я использовал фильтр Калмана, после чего я получаю стабильное горизонтальное значение, горизонт x=180 и y=180, изменяя угол чипа у меня меняется значение по горизонтали.
2. Для PID регуляции я использую библиотеку PID_v1.

Код (Text):

PID esc1PID(&Input, &Output, &Setpoint, STAB_P_VAL, STAB_I_VAL, STAB_D_VAL, DIRECT);
PID esc2PID(&Input, &Output, &Setpoint, STAB_P_VAL, STAB_I_VAL, STAB_D_VAL, REVERSE);

Соответственно:
esc1PID направление изменения выходного сигнала, когда начальный входящий сигнал меньше заданного — DIRECT, esc2PID когда больше REVERSE.
При взлете я использую код:

Код (Text):

currentTime = millis();
if(currentTime >= (loopTime + t)){
    if (kalAngleX<175){
      Input = kalAngleX;
      esc1PID.Compute();
      speed2 = speed2 + Output;
      motor2.write(speed2);
    }
    if (kalAngleX>185){
      Input = kalAngleX;
      esc2PID.Compute();
      speed3 = speed3 + Output;
      motor3.write(speed3);
    }
    if (kalAngleY<175){
      Input = kalAngleY;
      esc1PID.Compute();
      speed4 = speed4 + Output;
      motor4.write(speed4);
    }
    if (kalAngleY>185){
      Input = kalAngleY;
      esc2PID.Compute();
      speed1 = speed1 + Output;
      motor1.write(speed1);
     }
loopTime = currentTime;
}
 

На погрешность я оставляю +- 5 градусов.
PID Значение:

Код (Text):

#define STAB_P_VAL 0.5
#define STAB_I_VAL 0
#define STAB_D_VAL 2

Setpoint как я понял это — Заданное значение контролируемого сигнала.
Установил значение к которому стремимся 180:

Код (Text):

Setpoint = 180;

Input(double) — Входящий сигнал, который мы контролируем, из кода видно чему ровна.
Output(double) — Выходящий сигнал, которым мы управляем.
Время итерации:

Код (Text):

int t = 100;

Минимальное максимальное значение ESC:

Код (Text):

#define ESC_MIN 22
#define ESC_MAX 115

Устанавил границы выходящего сигнала:

Код (Text):

#define PID_PITCH_INFLUENCE 20

esc1PID.SetMode(AUTOMATIC);
esc1PID.SetOutputLimits(-PID_PITCH_INFLUENCE, PID_PITCH_INFLUENCE);
esc2PID.SetMode(AUTOMATIC);
esc2PID.SetOutputLimits(-PID_PITCH_INFLUENCE, PID_PITCH_INFLUENCE);

Результат:
Скорость вращения двигателей меняется при наклоне, но вертолет не взлетает, переваливается на бок.
Возможно я чего та не учитываю или не правильно интерпритирую библиотеку PID_v1.
Возможно нужно еще какие то расчеты сделать с Выходящим сигналом.

 

Что вы думаете по поводу того что я не правильно взял за основу положение в пространстве по x y.
Что если за основу взять показания гироскопа?