V4L2編程實例

#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <fcntl.h>
#include <unistd.h>
#include <assert.h>
#include <ctype.h>
#include <errno.h>
#include <sys/mman.h>
#include <sys/time.h>
#include <sys/ioctl.h>
#include <asm/types.h> 
#include <linux/videodev2.h>
#include <linux/fb.h>
#define REQ_BUF_NUM 4		//申請的緩衝區個數，最多5個，緩衝區太少可能會導致圖像有間斷
unsigned char bmp_head_t[] = {	//bmp圖像頭文件
        0x42,0x4d,0x42,0x58,0x02,0x00,0x00,0x00,0x00,0x00,
        0x42,0x00,0x00,0x00,0x28,0x00,0x00,0x00,0xf0,0x00,
        0x00,0x00,0x40,0x01,0x00,0x00,0x01,0x00,0x10,0x00,
        0x03,0x00,0x00,0x00,0x00,0x58,0x02,0x00,0x00,0x00,
        0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
        0x00,0x00,0x00,0x00,0x00,0xf8,0x00,0x00,0xe0,0x07,
        0x00,0x00,0x1f,0x00,0x00,0x00
};
typedef struct buffer_type_r buffer_type;
struct buffer_type_r		//申請到的緩衝區信息
{
	char *start;
	int length;
};
buffer_type *user_buffer;
int buffer_count = 0;		//實際申請到的緩衝區個數
int open_video()
{
	int fb = -1;
	if((fb = open("/dev/video0",O_RDWR)) == -1){
		perror("open");
		exit(1);
	}
	return fb;
}
void init_mmap(int fb)
{
	struct v4l2_requestbuffers tV4L2_reqbuf;
	int i = 0;
	memset(&tV4L2_reqbuf, 0, sizeof(struct v4l2_requestbuffers ));

	tV4L2_reqbuf.count = REQ_BUF_NUM;             //申請緩衝區的個數
	tV4L2_reqbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	tV4L2_reqbuf.memory = V4L2_MEMORY_MMAP;        //mmap方式
	if(ioctl(fb, VIDIOC_REQBUFS, &tV4L2_reqbuf)){
		perror("alloc reqbuf");
	} 
	buffer_count = tV4L2_reqbuf.count;
	printf("succefful get %d buffer\n",tV4L2_reqbuf.count);
	user_buffer = calloc(tV4L2_reqbuf.count,sizeof(*user_buffer));
	if(user_buffer == NULL){
		perror("calloc");
		exit(1); 
	}
	for(i = 0;i < tV4L2_reqbuf.count;i++){
		struct v4l2_buffer tV4L2buf;
		memset(&tV4L2buf, 0, sizeof(struct v4l2_buffer));
		tV4L2buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		tV4L2buf.memory = V4L2_MEMORY_MMAP;
		tV4L2buf.index = i; // 要獲取內核視頻緩衝區的信息編號

		if(ioctl(fb, VIDIOC_QUERYBUF, &tV4L2buf)){
			perror("search");
		}
		
		// 把內核空間緩衝區映射到用戶空間緩衝區
		user_buffer[i].length = tV4L2buf.length;
		user_buffer[i].start = (char *)mmap( NULL,                   /* start anywhere */
                        tV4L2buf.length,
                        PROT_READ | PROT_WRITE, /* access privilege */
                        MAP_SHARED,             /* recommended */
                        fb,
                        tV4L2buf.m.offset);
    	if(MAP_FAILED == user_buffer[i].start){
    		error("mmap");
    		exit(1);
   	 	}
	}
	
}
void init_video(int fb)
{
	struct v4l2_capability cap;
	struct v4l2_fmtdesc fmt;
	struct v4l2_format v4_format;	
	if((ioctl(fb,VIDIOC_QUERYCAP,&cap)) != 0){
		perror("get video capablity");
	}
	printf("driver name %s card = %s cap = %0x\n",cap.driver,cap.card,cap.capabilities);
	
	fmt.index = 0;
	while((ioctl(fb,VIDIOC_ENUM_FMT, &fmt)) == 0){
		fmt.index++;
        printf("{ pixelformat = ''%c%c%c%c'', description = ''%s'' }\n",
                  fmt.pixelformat & 0xFF, (fmt.pixelformat >> 8) & 0xFF, (fmt.pixelformat >> 16) & 0xFF, 
                  (fmt.pixelformat >> 24) & 0xFF, fmt.description);
	}
	v4_format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	v4_format.fmt.pix.width = 480;
	v4_format.fmt.pix.height = 272;
	v4_format.fmt.pix.pixelformat = V4L2_PIX_FMT_RGB565;
	v4_format.fmt.pix.field = V4L2_FIELD_INTERLACED;
	
	if((ioctl(fb, VIDIOC_S_FMT, &v4_format)) != 0){
		perror("set fmt error");
	}
	printf("set fmt OK!\n");
	init_mmap(fb);
}
void start_capturing(int fb)
{
	enum v4l2_buf_type v4l2type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	int i;
	for(i = 0;i < buffer_count;i++){
		struct v4l2_buffer tV4L2buf;
		memset(&tV4L2buf, 0, sizeof(struct v4l2_buffer));

		tV4L2buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		tV4L2buf.memory = V4L2_MEMORY_MMAP;
		tV4L2buf.index = i; //指令(指定)要投放到視頻輸入隊列中的內核空間視頻緩衝區的編號;

		if(ioctl(fb, VIDIOC_QBUF, &tV4L2buf)){			//投放一個空的視頻緩衝區到視頻緩衝區輸入隊列中
			perror("VIDIOC_QBUF");
		}
		if(ioctl(fb, VIDIOC_STREAMON, &v4l2type)){		//啓動視頻採集命令
			perror("VIDIOC_STREAMON");
		}
	}
	
}


void process_image(char *addr,int length)
{
	
	static int capframe = 0;
	char filename[20];
	FILE *bmpFile;
	sprintf(filename,"0%d.bmp",capframe++);
   	if((bmpFile=fopen(filename, "w+")) == NULL)
	{
		perror("Fail to fopen");
		exit(EXIT_FAILURE);
	}
	fwrite(bmp_head_t,1,66,bmpFile);
	if(addr != NULL)//經測試，申請緩衝區個數爲2時，編譯會出現段錯誤，追蹤發現addr爲NULL
	fwrite((void *)addr,1,length,bmpFile);
   	fclose(bmpFile);
}
int read_buffer(int fb)
{
	struct v4l2_buffer buf;
	memset(&buf, 0, sizeof(struct v4l2_buffer));

	buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	buf.memory = V4L2_MEMORY_MMAP;
	assert(buf.index < buffer_count);
	if(ioctl(fb, VIDIOC_DQBUF, &buf) != 0){
		perror("VIDIOC_DQBUF");
		exit(1);
	}
	process_image(user_buffer[buf.index].start,user_buffer[buf.index].length);
	
	return 0;
}
void main_loop(int fb)
{
	int count = 10;

	while(count-- > 0)
	{
		for(;;)
		{
			fd_set fds;
			struct timeval tv;
			int r;

			FD_ZERO(&fds);
			FD_SET(fb,&fds);

			/*Timeout*/
			tv.tv_sec = 2;
			tv.tv_usec = 0;
		
			r = select(fb + 1,&fds,NULL,NULL,&tv);

			if(-1 == r)
			{
				if(EINTR == errno)
					continue;
				
				perror("Fail to select");
				exit(EXIT_FAILURE);
			}

			if(0 == r)
			{
				fprintf(stderr,"select Timeout\n");
				exit(EXIT_FAILURE);
			}

			if(read_buffer(fb) == 0)
				break;
		}
	}

}
void stop_capturing(int fb)
{
	enum v4l2_buf_type type;
	
	type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	if(-1 == ioctl(fb,VIDIOC_STREAMOFF,&type))
	{
		perror("Fail to ioctl 'VIDIOC_STREAMOFF'");
		exit(EXIT_FAILURE);
	}
	
}
void uninit_camer_device(int fb)
{
	unsigned int i;

	for(i = 0;i < buffer_count;i++)
	{
		if(-1 == munmap(user_buffer[i].start,user_buffer[i].length))
		{
			exit(EXIT_FAILURE);
		}
	}
	
	free(user_buffer);

}
void close_camer_device(int fb)
{
	if(-1 == close(fb))
	{
		perror("Fail to close fd");
		exit(EXIT_FAILURE);
	}

}
int main()
{	*((unsigned int*)(bmp_head_t+18)) = 480;			//240
	*((unsigned int*)(bmp_head_t+22)) = 232;	
	int fb = open_video();
	init_video(fb); 
	start_capturing(fb);
	main_loop(fb);
	stop_capturing(fb);
	uninit_camer_device(fb);
	close_camer_device(fb);
	
	return 0;
}