camera 应用

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

[cpp]  view plain copy

1. #include <stdio.h>  
2. #include <errno.h>  
3. #include <stdint.h>  
4. #include <signal.h>  
5. #include <string.h>  
6. #include <stdlib.h>  
7. #include <getopt.h>   
8. #include <unistd.h>  
9. #include <fcntl.h>  
10. #include <sys/ioctl.h>  
11. #include <sys/stat.h>  
12. #include <sys/types.h>  
13. #include <sys/time.h>  
14. #include <sys/mman.h>  
15. #include <linux/fb.h>  
16. #include <linux/videodev2.h>  
17.   
18. #define CLIP_MIN (-278)  
19. #define CLIP_MAX (535)  
20.   
21. static volatile int clip_init_done = 0;  
22. static uint8_t clip[CLIP_MAX - CLIP_MIN + 1];  
23.   
24. uint8_t *init_clip()  
25. {  
26.     int i;  
27.       
28.     if(clip_init_done)  
29.         return &clip[-CLIP_MIN];  
30.   
31.     for (i = CLIP_MIN; i <= CLIP_MAX; ++i)  
32.         clip[i - CLIP_MIN] = (i < 0) ? 0 : (i > 255) ? 255 : (uint8_t)i;  
33.   
34.     clip_init_done = 1;  
35.   
36.     return &clip[-CLIP_MIN];  
37. }  
38.   
39. /* 
40.  * Convert YUV planner to RGB565 
41.  * 
42.  * For YUV420: y_stride = width, yu_stride = width / 2 
43.  * For YUV422: y_stride = width, yu_stride = width 
44.  * src_y src_u src_v: start address of Y U V data block in source frame. 
45.  * dst: start address of results frame. 
46.  */  
47. static void yuv_planar_2_rgb(size_t width, size_t height,  
48.             size_t y_stride, size_t uv_stride,  
49.             const uint8_t *src_y, const uint8_t *src_u,  
50.             const uint8_t *src_v, void *dst)  
51. {  
52.     size_t x, y;  
53.     uint8_t *adj_clip = init_clip();  
54.     uint32_t *dst_ptr = (uint32_t *)dst;  
55.     int y1, y2, u, v, u_b, u_g, v_g, v_r, tmp1, b1, g1, r1, tmp2, b2, g2, r2;  
56.     uint32_t rgb1, rgb2;  
57.   
58.     for (y = 0; y < height; ++y) {  
59.         for (x = 0; x < width; x += 2) {  
60.             y1 = (int)src_y[x] - 16;  
61.             y2 = (int)src_y[x + 1] - 16;  
62.   
63.             u = (int)src_u[x / 2] - 128;  
64.             v = (int)src_v[x / 2] - 128;  
65.   
66.             u_b = u * 517;  
67.             u_g = -u * 100;  
68.             v_g = -v * 208;  
69.             v_r = v * 409;  
70.   
71.             tmp1 = y1 * 298;  
72.             b1 = (tmp1 + u_b) / 256;  
73.             g1 = (tmp1 + v_g + u_g) / 256;  
74.             r1 = (tmp1 + v_r) / 256;  
75.   
76.             tmp2 = y2 * 298;  
77.             b2 = (tmp2 + u_b) / 256;  
78.             g2 = (tmp2 + v_g + u_g) / 256;  
79.             r2 = (tmp2 + v_r) / 256;  
80.   
81.             rgb1 = ((adj_clip[r1] >> 3) << 11)  
82.                 | ((adj_clip[g1] >> 2) << 5)  
83.                 | (adj_clip[b1] >> 3);  
84.   
85.             rgb2 = ((adj_clip[r2] >> 3) << 11)  
86.                 | ((adj_clip[g2] >> 2) << 5)  
87.                 | (adj_clip[b2] >> 3);  
88.   
89.             dst_ptr[x / 2] = (rgb2 << 16) | rgb1;  
90.         }  
91.   
92.         src_y += y_stride;  
93.   
94.         if (y & 1) {  
95.             src_u += uv_stride;  
96.             src_v += uv_stride;  
97.         }  
98.   
99.         dst_ptr += width / 2;  
100.     }  
101. }  
102.   
103. /* 
104.  * Convert RGB565 to RGB888 
105.  * 
106.  * src: start address of RGB565 data block in source frame. 
107.  * dst: start address of results frame. 
108.  */  
109. static void rgb565_2_rgb888(uint16_t *src, uint8_t *dst, int width, int height)  
110. {  
111.     int i;  
112.   
113.     for(i = 0; i < width * height; i++, src++, dst += 3)  
114.     {  
115.         /* 顺序: b、g、r, 每个分量都是 8 bit 对齐 */  
116.         dst[0] = (uint8_t)(((*src & 0x001f) << 3) | ((*src & 0x001f) >> 2));  
117.         dst[1] = (uint8_t)((((*src & 0x07e0) >> 5) << 2) | (((*src & 0x07e0) >> 5) >> 4));  
118.         dst[2] = (uint8_t)(((*src >> 11) << 3) | ((*src >> 11) >> 2));  
119.     }  
120. }  
121.   
122. /* used for calculate FPS */  
123. static struct timeval start, end;  
124. static unsigned long long g_tick, g_precision = 1000000;  
125.   
126. static inline void clock_start()  
127. {  
128.     gettimeofday(&start, NULL);  
129.     return;  
130. }  
131.   
132. /* for calculate fps */  
133. static inline void clock_end()  
134. {  
135.     struct timeval val;  
136.     gettimeofday(&end, NULL);  
137.     timersub(&end, &start, &val);  
138.     g_tick = val.tv_sec * 1000000 + val.tv_usec;  
139.     return;  
140. }  
141.   
142. /* bmp file's head */  
143. char bmp_head[] = {0x42, 0x4d, 0x36, 0x10, 0x0e, 0x00, 0x00, 0x00,  
144.                    0x00, 0x00, 0x36, 0x00, 0x00, 0x00, 0x28, 0x00,  
145.                    0x00, 0x00, 0x80, 0x02, 0x00, 0x00, 0xe0, 0x01,  
146.                    0x00, 0x00, 0x01, 0x00, 0x18, 0x00, 0x00, 0x00,  
147.                    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc4, 0x0e,  
148.                    0x00, 0x00, 0xc4, 0x0e, 0x00, 0x00, 0x00, 0x00,  
149.                    0x00, 0x00, 0x00, 0x00, 0x00, 0x00};  
150.   
151. struct cam_buffer {  
152.     void *start;  
153.     void *vstart;  
154.     unsigned long length;  
155. };  
156.   
157. static struct cam_info {  
158.     int num;                            /* pic statistics */  
159.     int count;                          /* take pic number, -1 means not stop */  
160.     int dev_fd;                         /* camera dev file descriptor */  
161.     int yuv_fd;                         /* camera yuv file descriptor */  
162.     int rgb_fd;                         /* camera rgb file descriptor */  
163.   
164.     unsigned long index;                /* buffer index */  
165.     unsigned long nbuffer;              /* total buffer number */  
166.     unsigned long bufsize;              /* buffer size */  
167.     unsigned long stop_stream;          /* flag indicate to stop work */  
168.   
169.     struct cam_buffer *buffers;         /* mmaped buffer info */      
170.     struct v4l2_format format;          /* pic format */  
171.     struct v4l2_requestbuffers req;  
172. } cam_info;  
173.   
174. static void open_yuv_file()  
175. {  
176.     cam_info.yuv_fd = open("/sdcard/cam_test.yuv", O_CREAT | O_TRUNC | O_RDWR, 0777);  
177.     if (cam_info.yuv_fd < 0) {  
178.         printf("open /sdcard/cam_test.yuv failed!\n");        
179.     }  
180. }  
181.   
182. static void open_rgb_file()  
183. {  
184.     cam_info.rgb_fd = open("/sdcard/cam_test.bmp", O_CREAT | O_TRUNC | O_RDWR, 0777);  
185.     if (cam_info.rgb_fd < 0) {  
186.         printf("open /sdcard/cam_test.bmp failed!\n");  
187.     }  
188. }  
189.   
190. static int start_capturing()  
191. {  
192.     int ret;  
193.     unsigned long i;  
194.     struct v4l2_buffer buf;  
195.     enum v4l2_buf_type type;  
196.   
197.     for (i = 0; i < cam_info.nbuffer; ++i) {  
198.         memset(&buf, 0, sizeof(buf));  
199.   
200.         buf.type   = V4L2_BUF_TYPE_VIDEO_CAPTURE;  
201.         buf.memory = V4L2_MEMORY_MMAP;  
202.         buf.index  = i;  
203.   
204.         ret = ioctl(cam_info.dev_fd, VIDIOC_QBUF, &buf);  
205.         if (ret < 0) {  
206.             printf("ioctl VIDIOC_QBUF failed, %d\n", errno);  
207.             return -1;  
208.         }  
209.     }  
210.   
211.     type = V4L2_BUF_TYPE_VIDEO_CAPTURE;  
212.   
213.     ret = ioctl(cam_info.dev_fd, VIDIOC_STREAMON, &type);  
214.     if (ret < 0) {  
215.         printf("ioctl VIDIOC_STREAMON failed!, %d\n", errno);  
216.         return -1;  
217.     }  
218.     return 0;  
219. }  
220.   
221. static int write_frame()  
222. {  
223.     int ret;  
224.     struct v4l2_buffer buf;  
225.   
226.     memset(&buf, 0, sizeof(struct v4l2_buffer));  
227.     buf.type   = V4L2_BUF_TYPE_VIDEO_CAPTURE;  
228.     buf.memory = V4L2_MEMORY_MMAP;  
229.   
230.     ret = ioctl(cam_info.dev_fd, VIDIOC_DQBUF, &buf);  
231.     if (ret < 0) {  
232.         if(errno == EPIPE) {  
233.             printf("Streaming stop, we need restart stream\n");  
234.             start_capturing();  
235.             return 0;  
236.         } else {  
237.             printf("ioctl VIDIOC_DQBUF failed!, %d\n", errno);  
238.             return -1;  
239.         }  
240.     }  
241.   
242.     if (cam_info.nbuffer < buf.index) {  
243.         printf("Kernel return wrong index %d, totoal %lu\n", buf.index, cam_info.nbuffer);  
244.         return -1;  
245.     }  
246.   
247.     ret = write(cam_info.yuv_fd, cam_info.buffers[buf.index].vstart, cam_info.format.fmt.pix.sizeimage);  
248.     if(ret < cam_info.format.fmt.pix.sizeimage) {  
249.         printf("write failed, ret = %d\n", ret);  
250.         return ret;  
251.     }  
252.   
253.     ret = ioctl(cam_info.dev_fd, VIDIOC_QBUF, &buf);  
254.     if (ret < 0) {  
255.         printf("ioctl VIDIOC_QBUF failed!, %d\n", errno);  
256.         return -1;  
257.     }  
258.     cam_info.num++;  
259.   
260.     return 0;  
261. }  
262.   
263. static int stop_capturing()  
264. {  
265.     int ret;  
266.     enum v4l2_buf_type type;  
267.   
268.     type = V4L2_BUF_TYPE_VIDEO_CAPTURE;  
269.   
270.     ret = ioctl(cam_info.dev_fd, VIDIOC_STREAMOFF, &type);  
271.     if (ret < 0) {  
272.         printf("ioctl VIDIOC_STREAMOFF failed!, %d\n", errno);  
273.         return -1;  
274.     }  
275.     return 0;  
276. }  
277.   
278. static int start_preview()  
279. {  
280.     int ret;  
281.     fd_set fds;  
282.     struct timeval tv;  
283.   
284.     /* Timeout. */  
285.     tv.tv_sec = 5;  
286.     tv.tv_usec = 0;  
287.   
288.     open_yuv_file();  
289.     clock_start();  
290.     while (!cam_info.stop_stream && (cam_info.count < 0 || cam_info.num < cam_info.count)) {  
291.         FD_ZERO(&fds);  
292.         FD_SET(cam_info.dev_fd, &fds);  
293.   
294.         ret = select(cam_info.dev_fd + 1, &fds, NULL, NULL, &tv);  
295.   
296.         if (ret < 0) {  
297.             printf("select() failed, %d\n", errno);  
298.             break;  
299.         }  
300.   
301.         if (ret == 0)  
302.             printf("select() timeout\n");  
303.   
304.         ret = write_frame();  
305.   
306.         if (ret < 0)  
307.             break;  
308.     }  
309.     clock_end();  
310.     printf("\nget %d frame, fps: %f\n", cam_info.num, (cam_info.num * 1.0 * g_precision) / g_tick);  
311.     cam_info.num = 0;  
312.     close(cam_info.yuv_fd);  
313.     stop_capturing();  
314.     printf("preview done, streaming off\n");  
315.     return ret;  
316. }  
317.   
318. static int init_device()  
319. {  
320.     int i, ret;  
321.     struct v4l2_buffer buf;  
322.     struct v4l2_capability cap;  
323.   
324.     /* capabilities related */  
325.     memset(&cap, 0, sizeof(cap));  
326.     ret = ioctl(cam_info.dev_fd, VIDIOC_QUERYCAP, &cap);  
327.     if (ret < 0) {  
328.         printf("ioctl VIDIOC_QUERYCAP failed! ret = %d\n", ret);  
329.         return -1;  
330.     }  
331.   
332.     if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {  
333.         printf("Opening a no video capture device\n");  
334.         return -1;  
335.     }  
336.   
337.     if (!(cap.capabilities & V4L2_CAP_STREAMING)) {  
338.         printf("device does not support streaming i/o\n");  
339.         return -1;  
340.     }  
341.   
342.     /* set format */  
343.     cam_info.format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;  
344.     cam_info.format.fmt.pix.pixelformat = V4L2_PIX_FMT_YUV422P;  
345.     ret = ioctl(cam_info.dev_fd, VIDIOC_S_FMT, &cam_info.format);  
346.     if (ret < 0) {  
347.         printf("ioctl VIDIOC_S_FMT failed! ret = %d\n", ret);  
348.         return -1;  
349.     }  
350.   
351.     /* request buffer */  
352.     cam_info.req.count  = cam_info.nbuffer;  
353.     cam_info.req.memory = V4L2_MEMORY_MMAP;  
354.     cam_info.req.type   = V4L2_BUF_TYPE_VIDEO_CAPTURE;  
355.   
356.     ret = ioctl(cam_info.dev_fd, VIDIOC_REQBUFS, &cam_info.req);  
357.     if (ret < 0) {  
358.         printf("ioctl VIDIOC_REQBUFS failed! ret = %d\n", ret);  
359.         return -1;  
360.     }  
361.   
362.     cam_info.buffers = malloc(cam_info.req.count * sizeof(struct cam_buffer));  
363.     if (!cam_info.buffers) {  
364.         printf("malloc() failed!\n");  
365.         return -1;  
366.     }  
367.   
368.     /* query buffer */  
369.     for (i = 0; i < cam_info.req.count; i++) {  
370.         memset(&buf, 0, sizeof(buf));  
371.         buf.index  = i;  
372.         buf.memory = V4L2_MEMORY_MMAP;  
373.         buf.type   = V4L2_BUF_TYPE_VIDEO_CAPTURE;  
374.   
375.         ret = ioctl(cam_info.dev_fd, VIDIOC_QUERYBUF, &buf);  
376.         if (ret < 0) {  
377.             printf("ioctl VIDIOC_QUERYBUF failed! ret = %d\n", ret);  
378.             free(cam_info.buffers);  
379.             return -1;  
380.         }  
381.   
382.         cam_info.buffers[i].length = buf.length;  
383.         cam_info.buffers[i].vstart = mmap (NULL, buf.length,  
384.                         PROT_READ | PROT_WRITE, MAP_SHARED,  
385.                         cam_info.dev_fd, buf.m.offset);  
386.         printf("mmap virt %p, phy %p, size %lu\n",  
387.                         (void *)cam_info.buffers[i].vstart,  
388.                         (void *)buf.reserved,  
389.                         (unsigned long)buf.length);  
390.   
391.         if (MAP_FAILED == cam_info.buffers[i].vstart) {  
392.             printf("mmap() failed!\n");  
393.             free(cam_info.buffers);  
394.             return -1;  
395.         }  
396.     }  
397.   
398.     return 0;  
399. }  
400.   
401. /* read frame */  
402. static int take_photo()  
403. {  
404.     int i, ret, len, rgb565_len, rgb888_len;  
405.     char *buffer, *rgb565_buffer, *rgb888_buffer;  
406.   
407.     len = cam_info.format.fmt.pix.sizeimage;  
408.     rgb565_len = cam_info.format.fmt.pix.sizeimage;  
409.     rgb888_len = cam_info.format.fmt.pix.sizeimage * 3 / 2;  
410.   
411.     buffer = malloc(len);  
412.     rgb565_buffer = malloc(rgb565_len);  
413.     rgb888_buffer = malloc(rgb888_len);  
414.   
415.     open_yuv_file();  
416.     clock_start();  
417.     for(i = 0; i < cam_info.req.count; i++) {  
418.         ret = read(cam_info.dev_fd, buffer, len);  
419.         if(ret < len) {  
420.             printf("read failed, ret = %d, len = %d\n", ret, len);  
421.             goto exit;  
422.         }  
423.   
424.         ret = write(cam_info.yuv_fd, buffer, len);  
425.         if(ret < len) {  
426.             printf("write failed, ret = %d\n", ret);  
427.             goto exit;  
428.         }  
429.     }  
430.     clock_end();  
431.     close(cam_info.yuv_fd);  
432.     printf("get %d frame, fps: %f\n", i, (i * 1.0 * g_precision) / g_tick);  
433.   
434.     open_rgb_file();  
435.     ret = write(cam_info.rgb_fd, bmp_head, sizeof(bmp_head));  
436.     if(ret < sizeof(bmp_head)) {  
437.         printf("write bmp_head failed, ret = %d\n", ret);  
438.     }  
439.   
440.     yuv_planar_2_rgb(cam_info.format.fmt.pix.width, cam_info.format.fmt.pix.height,  
441.                 cam_info.format.fmt.pix.width, cam_info.format.fmt.pix.width,  
442.                 (const uint8_t *)buffer,  
443.                 (const uint8_t *)(buffer + rgb565_len / 2),  
444.                 (const uint8_t *)(buffer + 3 * rgb565_len / 4),  
445.                 rgb565_buffer);  
446.   
447.     rgb565_2_rgb888(rgb565_buffer, rgb888_buffer, cam_info.format.fmt.pix.width, cam_info.format.fmt.pix.height);  
448.   
449.     ret = write(cam_info.rgb_fd, rgb888_buffer, rgb888_len);  
450.     if(ret < rgb888_len) {  
451.         printf("write rgb_buffer failed, ret = %d\n", ret);  
452.         goto exit;  
453.     }  
454.     close(cam_info.rgb_fd);  
455.   
456.     free(buffer);  
457.     free(rgb565_buffer);  
458.     free(rgb888_buffer);  
459.   
460.     return 0;  
461.   
462. exit:  
463.     free(buffer);  
464.     free(rgb565_buffer);  
465.     free(rgb888_buffer);  
466.     return ret;  
467. }  
468.   
469. static int take_video()  
470. {  
471.     int ret;  
472.   
473.     ret = start_capturing();  
474.     if(ret < 0)  
475.         return ret;  
476.       
477.     return start_preview();  
478. }  
479.   
480. static void show_usage()  
481. {  
482.     printf("cam_test -d <dev> -c <mode> -w <width> -h <height> -n <count>\n");  
483.     printf("Options:\n");  
484.     printf("   -d:    device num. 0 - back, 1 - front.\n");  
485.     printf("   -c:    capture mode. 0 - video, 1 - photo.\n");  
486.     printf("   -w:    specify frame width.\n");  
487.     printf("   -h:    specify frame height.\n");  
488.     printf("   -n:    specify the count of frames.\n");  
489. }  
490.   
491. int main(int argc, char *argv[])  
492. {  
493.     int c, cam_dev, cam_ctl, pix_width, pix_height, pix_count;  
494.   
495.     if (argc < 2) {  
496.         show_usage();  
497.         return -1;  
498.     }  
499.   
500.     while ((c = getopt(argc, argv, "d:c:n:h:w:")) != -1) {  
501.         switch(c) {  
502.         case 'd':  
503.             cam_dev = atoi(optarg);  
504.             break;  
505.               
506.         case 'c':  
507.             cam_ctl = atoi(optarg);  
508.             break;  
509.               
510.         case 'n':  
511.             pix_count = atoi(optarg);  
512.             break;  
513.   
514.         case 'h':  
515.             pix_height = atoi(optarg);  
516.             break;  
517.   
518.         case 'w':  
519.             pix_width = atoi(optarg);  
520.             break;  
521.               
522.         default:  
523.             show_usage();  
524.             return -1;  
525.         }  
526.     }  
527.   
528.     printf("pix_width = %d, pix_height = %d, pix_count = %d\n", pix_width, pix_height, pix_count);  
529.   
530.     if(cam_dev)  
531.         cam_info.dev_fd = open("/dev/video1", O_RDWR);  
532.     else  
533.         cam_info.dev_fd = open("/dev/video0", O_RDWR);  
534.       
535.     cam_info.format.fmt.pix.width  = pix_width;  
536.     cam_info.format.fmt.pix.height = pix_height;  
537.   
538.     cam_info.count = pix_count;  
539.     cam_info.nbuffer = 10;  
540.     cam_info.nbuffer = cam_info.nbuffer < pix_count ? cam_info.nbuffer : pix_count;  
541.   
542.     init_device();  
543.   
544.     if(cam_ctl)  
545.         take_photo();  
546.     else  
547.         take_video();  
548.   
549.     close(cam_info.dev_fd);  
550.   
551.     return 0;  
552. }