camera 应用

前段时间写了一个测试 camera 的应用程序，很简单的实现了在平台上拍照和录像的功能，bmp文件头暂时只支持固定分辨率 640 x 480：

#include <stdio.h>#include <errno.h>#include <stdint.h>#include <signal.h>#include <string.h>#include <stdlib.h>#include <getopt.h> #include <unistd.h>#include <fcntl.h>#include <sys/ioctl.h>#include <sys/stat.h>#include <sys/types.h>#include <sys/time.h>#include <sys/mman.h>#include <linux/fb.h>#include <linux/videodev2.h>#define CLIP_MIN (-278)#define CLIP_MAX (535)static volatile int clip_init_done = 0;static uint8_t clip[CLIP_MAX - CLIP_MIN + 1];uint8_t *init_clip(){int i;if(clip_init_done)return &clip[-CLIP_MIN];for (i = CLIP_MIN; i <= CLIP_MAX; ++i)clip[i - CLIP_MIN] = (i < 0) ? 0 : (i > 255) ? 255 : (uint8_t)i;clip_init_done = 1;return &clip[-CLIP_MIN];}/* * Convert YUV planner to RGB565 * * For YUV420: y_stride = width, yu_stride = width / 2 * For YUV422: y_stride = width, yu_stride = width * src_y src_u src_v: start address of Y U V data block in source frame. * dst: start address of results frame. */static void yuv_planar_2_rgb(size_t width, size_t height,size_t y_stride, size_t uv_stride,const uint8_t *src_y, const uint8_t *src_u,const uint8_t *src_v, void *dst){size_t x, y;uint8_t *adj_clip = init_clip();uint32_t *dst_ptr = (uint32_t *)dst;int y1, y2, u, v, u_b, u_g, v_g, v_r, tmp1, b1, g1, r1, tmp2, b2, g2, r2;uint32_t rgb1, rgb2;for (y = 0; y < height; ++y) {for (x = 0; x < width; x += 2) {y1 = (int)src_y[x] - 16;y2 = (int)src_y[x + 1] - 16;u = (int)src_u[x / 2] - 128;v = (int)src_v[x / 2] - 128;u_b = u * 517;u_g = -u * 100;v_g = -v * 208;v_r = v * 409;tmp1 = y1 * 298;b1 = (tmp1 + u_b) / 256;g1 = (tmp1 + v_g + u_g) / 256;r1 = (tmp1 + v_r) / 256;tmp2 = y2 * 298;b2 = (tmp2 + u_b) / 256;g2 = (tmp2 + v_g + u_g) / 256;r2 = (tmp2 + v_r) / 256;rgb1 = ((adj_clip[r1] >> 3) << 11)| ((adj_clip[g1] >> 2) << 5)| (adj_clip[b1] >> 3);rgb2 = ((adj_clip[r2] >> 3) << 11)| ((adj_clip[g2] >> 2) << 5)| (adj_clip[b2] >> 3);dst_ptr[x / 2] = (rgb2 << 16) | rgb1;}src_y += y_stride;if (y & 1) {src_u += uv_stride;src_v += uv_stride;}dst_ptr += width / 2;}}/* * Convert RGB565 to RGB888 * * src: start address of RGB565 data block in source frame. * dst: start address of results frame. */static void rgb565_2_rgb888(uint16_t *src, uint8_t *dst, int width, int height){int i;for(i = 0; i < width * height; i++, src++, dst += 3){/* 顺序: b、g、r, 每个分量都是 8 bit 对齐 */dst[0] = (uint8_t)(((*src & 0x001f) << 3) | ((*src & 0x001f) >> 2));dst[1] = (uint8_t)((((*src & 0x07e0) >> 5) << 2) | (((*src & 0x07e0) >> 5) >> 4));dst[2] = (uint8_t)(((*src >> 11) << 3) | ((*src >> 11) >> 2));}}/* used for calculate FPS */static struct timeval start, end;static unsigned long long g_tick, g_precision = 1000000;static inline void clock_start(){gettimeofday(&start, NULL);return;}/* for calculate fps */static inline void clock_end(){struct timeval val;gettimeofday(&end, NULL);timersub(&end, &start, &val);g_tick = val.tv_sec * 1000000 + val.tv_usec;return;}/* bmp file's head */char bmp_head[] = {0x42, 0x4d, 0x36, 0x10, 0x0e, 0x00, 0x00, 0x00,   0x00, 0x00, 0x36, 0x00, 0x00, 0x00, 0x28, 0x00,   0x00, 0x00, 0x80, 0x02, 0x00, 0x00, 0xe0, 0x01,   0x00, 0x00, 0x01, 0x00, 0x18, 0x00, 0x00, 0x00,   0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc4, 0x0e,   0x00, 0x00, 0xc4, 0x0e, 0x00, 0x00, 0x00, 0x00,   0x00, 0x00, 0x00, 0x00, 0x00, 0x00};struct cam_buffer {void *start;void *vstart;unsigned long length;};static struct cam_info {int num;                            /* pic statistics */int count;                          /* take pic number, -1 means not stop */int dev_fd;                         /* camera dev file descriptor */int yuv_fd;                         /* camera yuv file descriptor */int rgb_fd;                         /* camera rgb file descriptor */unsigned long index;                /* buffer index */unsigned long nbuffer;              /* total buffer number */unsigned long bufsize;              /* buffer size */unsigned long stop_stream;          /* flag indicate to stop work */struct cam_buffer *buffers;         /* mmaped buffer info */struct v4l2_format format;          /* pic format */struct v4l2_requestbuffers req;} cam_info;static void open_yuv_file(){cam_info.yuv_fd = open("/sdcard/cam_test.yuv", O_CREAT | O_TRUNC | O_RDWR, 0777);if (cam_info.yuv_fd < 0) {printf("open /sdcard/cam_test.yuv failed!\n");}}static void open_rgb_file(){cam_info.rgb_fd = open("/sdcard/cam_test.bmp", O_CREAT | O_TRUNC | O_RDWR, 0777);if (cam_info.rgb_fd < 0) {printf("open /sdcard/cam_test.bmp failed!\n");}}static int start_capturing(){int ret;unsigned long i;struct v4l2_buffer buf;enum v4l2_buf_type type;for (i = 0; i < cam_info.nbuffer; ++i) {memset(&buf, 0, sizeof(buf));buf.type   = V4L2_BUF_TYPE_VIDEO_CAPTURE;buf.memory = V4L2_MEMORY_MMAP;buf.index  = i;ret = ioctl(cam_info.dev_fd, VIDIOC_QBUF, &buf);if (ret < 0) {printf("ioctl VIDIOC_QBUF failed, %d\n", errno);return -1;}}type = V4L2_BUF_TYPE_VIDEO_CAPTURE;ret = ioctl(cam_info.dev_fd, VIDIOC_STREAMON, &type);if (ret < 0) {printf("ioctl VIDIOC_STREAMON failed!, %d\n", errno);return -1;}return 0;}static int write_frame(){int ret;struct v4l2_buffer buf;memset(&buf, 0, sizeof(struct v4l2_buffer));buf.type   = V4L2_BUF_TYPE_VIDEO_CAPTURE;buf.memory = V4L2_MEMORY_MMAP;ret = ioctl(cam_info.dev_fd, VIDIOC_DQBUF, &buf);if (ret < 0) {if(errno == EPIPE) {printf("Streaming stop, we need restart stream\n");start_capturing();return 0;} else {printf("ioctl VIDIOC_DQBUF failed!, %d\n", errno);return -1;}}if (cam_info.nbuffer < buf.index) {printf("Kernel return wrong index %d, totoal %lu\n", buf.index, cam_info.nbuffer);return -1;}ret = write(cam_info.yuv_fd, cam_info.buffers[buf.index].vstart, cam_info.format.fmt.pix.sizeimage);if(ret < cam_info.format.fmt.pix.sizeimage) {printf("write failed, ret = %d\n", ret);return ret;}ret = ioctl(cam_info.dev_fd, VIDIOC_QBUF, &buf);if (ret < 0) {printf("ioctl VIDIOC_QBUF failed!, %d\n", errno);return -1;}cam_info.num++;return 0;}static int stop_capturing(){int ret;enum v4l2_buf_type type;type = V4L2_BUF_TYPE_VIDEO_CAPTURE;ret = ioctl(cam_info.dev_fd, VIDIOC_STREAMOFF, &type);if (ret < 0) {printf("ioctl VIDIOC_STREAMOFF failed!, %d\n", errno);return -1;}return 0;}static int start_preview(){int ret;fd_set fds;struct timeval tv;/* Timeout. */tv.tv_sec = 5;tv.tv_usec = 0;open_yuv_file();clock_start();while (!cam_info.stop_stream && (cam_info.count < 0 || cam_info.num < cam_info.count)) {FD_ZERO(&fds);FD_SET(cam_info.dev_fd, &fds);ret = select(cam_info.dev_fd + 1, &fds, NULL, NULL, &tv);if (ret < 0) {printf("select() failed, %d\n", errno);break;}if (ret == 0)printf("select() timeout\n");ret = write_frame();if (ret < 0)break;}clock_end();printf("\nget %d frame, fps: %f\n", cam_info.num, (cam_info.num * 1.0 * g_precision) / g_tick);cam_info.num = 0;close(cam_info.yuv_fd);stop_capturing();printf("preview done, streaming off\n");return ret;}static int init_device(){int i, ret;struct v4l2_buffer buf;struct v4l2_capability cap;/* capabilities related */memset(&cap, 0, sizeof(cap));ret = ioctl(cam_info.dev_fd, VIDIOC_QUERYCAP, &cap);if (ret < 0) {printf("ioctl VIDIOC_QUERYCAP failed! ret = %d\n", ret);return -1;}if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {printf("Opening a no video capture device\n");return -1;}if (!(cap.capabilities & V4L2_CAP_STREAMING)) {printf("device does not support streaming i/o\n");return -1;}/* set format */cam_info.format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;cam_info.format.fmt.pix.pixelformat = V4L2_PIX_FMT_YUV422P;ret = ioctl(cam_info.dev_fd, VIDIOC_S_FMT, &cam_info.format);if (ret < 0) {printf("ioctl VIDIOC_S_FMT failed! ret = %d\n", ret);return -1;}/* request buffer */cam_info.req.count  = cam_info.nbuffer;cam_info.req.memory = V4L2_MEMORY_MMAP;cam_info.req.type   = V4L2_BUF_TYPE_VIDEO_CAPTURE;ret = ioctl(cam_info.dev_fd, VIDIOC_REQBUFS, &cam_info.req);if (ret < 0) {printf("ioctl VIDIOC_REQBUFS failed! ret = %d\n", ret);return -1;}cam_info.buffers = malloc(cam_info.req.count * sizeof(struct cam_buffer));if (!cam_info.buffers) {printf("malloc() failed!\n");return -1;}/* query buffer */for (i = 0; i < cam_info.req.count; i++) {memset(&buf, 0, sizeof(buf));buf.index  = i;buf.memory = V4L2_MEMORY_MMAP;buf.type   = V4L2_BUF_TYPE_VIDEO_CAPTURE;ret = ioctl(cam_info.dev_fd, VIDIOC_QUERYBUF, &buf);if (ret < 0) {printf("ioctl VIDIOC_QUERYBUF failed! ret = %d\n", ret);free(cam_info.buffers);return -1;}cam_info.buffers[i].length = buf.length;cam_info.buffers[i].vstart = mmap (NULL, buf.length,PROT_READ | PROT_WRITE, MAP_SHARED,cam_info.dev_fd, buf.m.offset);printf("mmap virt %p, phy %p, size %lu\n",(void *)cam_info.buffers[i].vstart,(void *)buf.reserved,(unsigned long)buf.length);if (MAP_FAILED == cam_info.buffers[i].vstart) {printf("mmap() failed!\n");free(cam_info.buffers);return -1;}}return 0;}/* read frame */static int take_photo(){int i, ret, len, rgb565_len, rgb888_len;char *buffer, *rgb565_buffer, *rgb888_buffer;len = cam_info.format.fmt.pix.sizeimage;rgb565_len = cam_info.format.fmt.pix.sizeimage;rgb888_len = cam_info.format.fmt.pix.sizeimage * 3 / 2;buffer = malloc(len);rgb565_buffer = malloc(rgb565_len);rgb888_buffer = malloc(rgb888_len);open_yuv_file();clock_start();for(i = 0; i < cam_info.req.count; i++) {ret = read(cam_info.dev_fd, buffer, len);if(ret < len) {printf("read failed, ret = %d, len = %d\n", ret, len);goto exit;}ret = write(cam_info.yuv_fd, buffer, len);if(ret < len) {printf("write failed, ret = %d\n", ret);goto exit;}}clock_end();close(cam_info.yuv_fd);printf("get %d frame, fps: %f\n", i, (i * 1.0 * g_precision) / g_tick);open_rgb_file();ret = write(cam_info.rgb_fd, bmp_head, sizeof(bmp_head));if(ret < sizeof(bmp_head)) {printf("write bmp_head failed, ret = %d\n", ret);}yuv_planar_2_rgb(cam_info.format.fmt.pix.width, cam_info.format.fmt.pix.height,cam_info.format.fmt.pix.width, cam_info.format.fmt.pix.width,(const uint8_t *)buffer,(const uint8_t *)(buffer + rgb565_len / 2),(const uint8_t *)(buffer + 3 * rgb565_len / 4),rgb565_buffer);rgb565_2_rgb888(rgb565_buffer, rgb888_buffer, cam_info.format.fmt.pix.width, cam_info.format.fmt.pix.height);ret = write(cam_info.rgb_fd, rgb888_buffer, rgb888_len);if(ret < rgb888_len) {printf("write rgb_buffer failed, ret = %d\n", ret);goto exit;}close(cam_info.rgb_fd);free(buffer);free(rgb565_buffer);free(rgb888_buffer);return 0;exit:free(buffer);free(rgb565_buffer);free(rgb888_buffer);return ret;}static int take_video(){int ret;ret = start_capturing();if(ret < 0)return ret;return start_preview();}static void show_usage(){printf("cam_test -d <dev> -c <mode> -w <width> -h <height> -n <count>\n");printf("Options:\n");printf("   -d:    device num. 0 - back, 1 - front.\n");printf("   -c:    capture mode. 0 - video, 1 - photo.\n");printf("   -w:    specify frame width.\n");printf("   -h:    specify frame height.\n");printf("   -n:    specify the count of frames.\n");}int main(int argc, char *argv[]){int c, cam_dev, cam_ctl, pix_width, pix_height, pix_count;if (argc < 2) {show_usage();return -1;}while ((c = getopt(argc, argv, "d:c:n:h:w:")) != -1) {switch(c) {case 'd':cam_dev = atoi(optarg);break;case 'c':cam_ctl = atoi(optarg);break;case 'n':pix_count = atoi(optarg);break;case 'h':pix_height = atoi(optarg);break;case 'w':pix_width = atoi(optarg);break;default:show_usage();return -1;}}printf("pix_width = %d, pix_height = %d, pix_count = %d\n", pix_width, pix_height, pix_count);if(cam_dev)cam_info.dev_fd = open("/dev/video1", O_RDWR);elsecam_info.dev_fd = open("/dev/video0", O_RDWR);cam_info.format.fmt.pix.width  = pix_width;cam_info.format.fmt.pix.height = pix_height;cam_info.count = pix_count;cam_info.nbuffer = 10;cam_info.nbuffer = cam_info.nbuffer < pix_count ? cam_info.nbuffer : pix_count;init_device();if(cam_ctl)take_photo();elsetake_video();close(cam_info.dev_fd);return 0;}