+++ /dev/null
-/*
- * V4L2 (video4linux v2) camera capture
- *
- * based on "V4L2 video capture example" capture.c
- * http://v4l2spec.bytesex.org/v4l2spec/capture.c
- *
- * adapted by Marek Peca <mp@duch.cz> 2009/04
- * for the EUROBOT competition team
- * of the Dept. of Control Engineering http://dce.fel.cvut.cz/
- */
-
-#include <inttypes.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <assert.h>
-
-#include <fcntl.h>
-#include <unistd.h>
-#include <errno.h>
-#include <malloc.h>
-#include <sys/stat.h>
-#include <sys/types.h>
-#include <sys/time.h>
-#include <sys/mman.h>
-#include <sys/ioctl.h>
-
-#include <asm/types.h>
-#include <linux/videodev2.h>
-#include <libv4lconvert.h>
-
-#include "camera.h"
-
-#define CLEAR(x) memset(&(x), 0, sizeof(x))
-
-struct buffer {
- void *start;
- size_t length;
-};
-
-static int fd = -1;
-static struct v4lconvert_data *conv;
-struct buffer *buffers = NULL;
-unsigned int n_buffers = 0;
-
-int xioctl(int fd, int request, void *arg) {
- int r;
-
- do
- r = ioctl(fd, request, arg);
- while (-1 == r && EINTR == errno);
-
- return r;
-}
-
-int read_frame(process_image_callback_t cb) {
- struct v4l2_buffer buf;
-
- CLEAR(buf);
- buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
- buf.memory = V4L2_MEMORY_MMAP;
-
- if (-1 == xioctl(fd, VIDIOC_DQBUF, &buf)) {
- switch (errno) {
- case EAGAIN:
- return 1;
-
- case EIO:
- /* Could ignore EIO, see spec. */
-
- default:
- /* error */
- return -1;
- }
- }
-
- assert(buf.index < n_buffers);
-
- unsigned char rgb[640*480*3];
- if (cb) {
- struct v4l2_format src_fmt = {
- .type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
- .fmt = {
- .pix = { .width = 640,
- .height = 480,
- .pixelformat = V4L2_PIX_FMT_YUYV,
- .field = V4L2_FIELD_INTERLACED,
- }}};
- struct v4l2_format dst_fmt = {
- .type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
- .fmt = {
- .pix = { .width = 640,
- .height = 480,
- .pixelformat = V4L2_PIX_FMT_RGB24,
- .field = V4L2_FIELD_INTERLACED,
- }}};
-
- v4lconvert_convert(conv,
- &src_fmt, &dst_fmt,
- buffers[buf.index].start, buffers[buf.index].length,
- rgb, sizeof(rgb));
- cb(rgb);
- }
-
- if (-1 == xioctl(fd, VIDIOC_QBUF, &buf))
- return -1;
-
- return 0;
-}
-
-#if 0
-void mainloop() {
- unsigned int count;
-
- count = 100;
-
- while (count-- > 0) {
- for (;;) {
- fd_set fds;
- struct timeval tv;
- int r;
-
- FD_ZERO(&fds);
- FD_SET(fd, &fds);
-
- /* Timeout. */
- tv.tv_sec = 2;
- tv.tv_usec = 0;
-
- r = select(fd + 1, &fds, NULL, NULL, &tv);
-
- if (-1 == r) {
- if (EINTR == errno)
- continue;
-
- errno_exit("select");
- }
-
- if (0 == r) {
- fprintf(stderr, "select timeout\n");
- exit(EXIT_FAILURE);
- }
-
- if (read_frame())
- break;
-
- /* EAGAIN - continue select loop. */
- }
- }
-}
-#endif
-
-int stop_capturing() {
- enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-
- if (-1 == xioctl(fd, VIDIOC_STREAMOFF, &type))
- return -1;
-
- return 0;
-}
-
-int start_capturing() {
- unsigned int i;
- enum v4l2_buf_type type;
-
- for (i = 0; i < n_buffers; ++i) {
- struct v4l2_buffer buf;
-
- CLEAR(buf);
- buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
- buf.memory = V4L2_MEMORY_MMAP;
- buf.index = i;
-
- if (-1 == xioctl(fd, VIDIOC_QBUF, &buf))
- return -1;
- }
-
- type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-
- if (-1 == xioctl(fd, VIDIOC_STREAMON, &type))
- return -1;
-
- return 0;
-}
-
-int uninit_device() {
- unsigned int i;
-
- v4lconvert_destroy(conv);
-
- for (i = 0; i < n_buffers; ++i)
- if (-1 == munmap(buffers[i].start, buffers[i].length))
- return -1;
-
- free(buffers);
- return 0;
-}
-
-int init_mmap() {
- struct v4l2_requestbuffers req;
-
- CLEAR(req);
- /* req.count = 4; */
- req.count = 2;
- req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
- req.memory = V4L2_MEMORY_MMAP;
-
- if (-1 == xioctl(fd, VIDIOC_REQBUFS, &req))
- return -1;
- if (req.count < 2)
- return -1;
-
- if ((buffers = calloc(req.count, sizeof(*buffers))) == NULL)
- return -1;
-
- for (n_buffers = 0; n_buffers < req.count; ++n_buffers) {
- struct v4l2_buffer buf;
-
- CLEAR(buf);
- buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
- buf.memory = V4L2_MEMORY_MMAP;
- buf.index = n_buffers;
-
- if (-1 == xioctl(fd, VIDIOC_QUERYBUF, &buf))
- return -1;
-
- buffers[n_buffers].length = buf.length;
- buffers[n_buffers].start = mmap(NULL /* start anywhere */ ,
- buf.length,
- PROT_READ | PROT_WRITE
- /* required */ ,
- MAP_SHARED /* recommended */ ,
- fd, buf.m.offset);
-
- if (MAP_FAILED == buffers[n_buffers].start)
- return -1;
- }
-
- return 0;
-}
-
-int init_device(unsigned w, unsigned h, uint32_t pix_fmt) {
- struct v4l2_capability cap;
- struct v4l2_cropcap cropcap;
- struct v4l2_crop crop;
- struct v4l2_format fmt;
- unsigned int min;
-
- if (-1 == xioctl(fd, VIDIOC_QUERYCAP, &cap))
- return -1;
- if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE))
- return -1;
- if (!(cap.capabilities & V4L2_CAP_STREAMING))
- return -1;
-
- conv = v4lconvert_create(fd);
-
- /* Select video input, video standard and tune here. */
- CLEAR(cropcap);
- cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-
- if (0 == xioctl(fd, VIDIOC_CROPCAP, &cropcap)) {
- crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
- crop.c = cropcap.defrect; /* reset to default */
-
- if (-1 == xioctl(fd, VIDIOC_S_CROP, &crop)) {
- switch (errno) {
- case EINVAL:
- /* Cropping not supported. */
- break;
- default:
- /* Errors ignored. */
- break;
- }
- }
- } else {
- /* Errors ignored. */
- }
-
- CLEAR(fmt);
- fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
- fmt.fmt.pix.width = w;
- fmt.fmt.pix.height = h;
- //fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
- //fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG;
- fmt.fmt.pix.pixelformat = pix_fmt;
- fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;
-
- if (-1 == xioctl(fd, VIDIOC_S_FMT, &fmt))
- return -1;
-
- /* Note VIDIOC_S_FMT may change width and height. */
-
- /* Buggy driver paranoia. */
- min = fmt.fmt.pix.width * 2;
- if (fmt.fmt.pix.bytesperline < min)
- fmt.fmt.pix.bytesperline = min;
- min = fmt.fmt.pix.bytesperline * fmt.fmt.pix.height;
- if (fmt.fmt.pix.sizeimage < min)
- fmt.fmt.pix.sizeimage = min;
-
- return 0;
-}
-
-void close_device() {
- close(fd);
-}
-
-int open_device(char *dev_name) {
- return fd = open(dev_name, O_RDWR /* required */ | O_NONBLOCK, 0);
-}
-
-/* * */
-
-int camera_on(char *dev_name, unsigned w, unsigned h, uint32_t pix_fmt) {
- return -((open_device(dev_name) == -1) ||
- init_device(w, h, pix_fmt) ||
- init_mmap() ||
- start_capturing());
-}
-
-void camera_off() {
- stop_capturing();
- uninit_device();
- close_device();
-}
-
-int camera_get_frame(process_image_callback_t cb, int timeout_usec) {
- for (;;) {
- fd_set fds;
- struct timeval tv, *tvp;
- int r;
-
- FD_ZERO(&fds);
- FD_SET(fd, &fds);
-
- if (timeout_usec > 0) {
- tv.tv_sec = timeout_usec/1000000;
- tv.tv_usec = timeout_usec%1000000;
- tvp = &tv;
- }
- else
- tvp = NULL;
-
- r = select(fd + 1, &fds, NULL, NULL, tvp);
-
- if (-1 == r) {
- if (EINTR == errno)
- continue;
- return -1;
- }
-
- if (0 == r)
- /* timeout */
- return 1;
-
- r = read_frame(cb);
- if (r < 0)
- return -1;
- if (r == 0)
- return 0;
-
- /* EAGAIN - continue select loop. */
- }
-}