Added PID_pitch and PID_yaw, worked correctly