• 日期: 2028-09-05
  • 标签: 常规

#include <stdio.h> #include <stdlib.h> #include <string.h> #include <unistd.h> #include <fcntl.h> #include <errno.h> #include <sys/ioctl.h> #include <sys/mman.h> #include <sys/select.h> #include <linux/videodev2.h>

#define CAMERA_NUM 3 #define BUFFER_DEPTH 4

struct buffer_t { void *start; size_t length; };

struct camera_t { int fd; int index; pthread_t thread; struct buffer_t buffers[BUFFER_DEPTH]; int buffer_index; int buffer_count; int frame_count; int fps; pthread_mutex_t mutex; pthread_cond_t cond; int running; struct timeval last_time; };

struct buffer_list_t { struct buffer_t *buffer; struct buffer_list_t *next; };

struct buffer_queue_t { struct buffer_list_t *head; struct buffer_list_t *tail; int length; pthread_mutex_t mutex; };

struct v4l2_t { int fd; struct buffer_t buffers[BUFFER_DEPTH]; int buffer_count; int frame_count; int fps; pthread_t thread; struct buffer_queue_t queue; pthread_mutex_t mutex; pthread_cond_t cond; int running; struct timeval last_time; };

static struct camera_t cameras[CAMERA_NUM]; static struct v4l2_t v4l2_dev;

static int xioctl(int fd, int request, void *arg) { int r; do { r = ioctl(fd, request, arg); } while (r == -1 && errno == EINTR); return r; }

static int init_camera(struct camera_t *camera, int index) { char device_name[32]; sprintf(device_name, "/dev/camera%d", index);

int fd = open(device_name, O_RDWR | O_NONBLOCK, 0);
if (fd == -1) {
    perror("open camera device");
    return -1;
}

struct v4l2_capability cap;
if (xioctl(fd, VIDIOC_QUERYCAP, &cap) < 0) {
    perror("query camera capabilities");
    return -1;
}

if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
    fprintf(stderr, "device is not a video capture device\n");
    return -1;
}

if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
    fprintf(stderr, "device does not support streaming i/o\n");
    return -1;
}

struct v4l2_format fmt;
memset(&fmt, 0, sizeof(fmt));
fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
fmt.fmt.pix.width = 640;
fmt.fmt.pix.height = 480;
fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUV420;
fmt.fmt.pix.field = V4L2_FIELD_NONE;

if (xioctl(fd, VIDIOC_S_FMT, &fmt) < 0) {
    perror("set camera format");
    return -1;
}

struct v4l2_requestbuffers req;
memset(&req, 0, sizeof(req));
req.count = BUFFER_DEPTH;
req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
req.memory = V4L2_MEMORY_MMAP;

if (xioctl(fd, VIDIOC_REQBUFS, &req) < 0) {
    perror("request camera buffers");
    return -1;
}

if (req.count < BUFFER_DEPTH) {
    fprintf(stderr, "insufficient buffer memory on device\n");
    return -1;
}

for (int i = 0; i < BUFFER_DEPTH; i++) {
    struct v4l2_buffer buf;
    memset(&buf, 0, sizeof(buf));
    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    buf.memory = V4L2_MEMORY_MMAP;
    buf.index = i;

    if (xioctl(fd, VIDIOC_QUERYBUF, &buf) < 0) {
        perror("query camera buffer");
        return -1;
    }

    camera->buffers[i].length = buf.length;
    camera->buffers[i].start = mmap(NULL, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, fd, buf.m.offset);

    if (camera->buffers[i].start == MAP_FAILED) {
        perror("map camera buffer");
        return -1;
    }
}

camera->fd = fd;
camera->index = index;
camera->buffer_index = 0;
camera->buffer_count = 0;
camera->frame_count = 0;
camera->fps = 0;
pthread_mutex_init(&camera->mutex, NULL);
pthread_cond_init(&camera->cond, NULL);
camera->running = 1;
gettimeofday(&camera->last_time, NULL);

return 0;

}

static void *camera_thread(void *arg) { struct camera_t *camera = (struct camera_t *)arg; struct v4l2_buffer buf; memset(&buf, 0, sizeof(buf)); buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; buf.memory = V4L2_MEMORY_MMAP;

while (camera->running) {
    if (camera->buffer_count < BUFFER_DEPTH) {
        if (xioctl(camera->fd, VIDIOC_QBUF, &buf) < 0) {
            perror("queue camera buffer");
            break;
        }

        pthread_mutex_lock(&camera->mutex);
        camera->buffer_count++;
        pthread_cond_signal(&camera->cond);
        pthread_mutex_unlock(&camera->mutex);
    } else {
        usleep(1000);
    }
}

return NULL;

}

static int init_v4l2(struct v4l2_t *v4l2) { int fd = open("/dev/video0", O_RDWR | O_NONBLOCK, 0); if (fd == -1) { perror("open v4l2 device"); return -1; }

struct v4l2_capability cap;
if (xioctl(fd, VIDIOC_QUERYCAP, &cap) < 0) {
    perror("query v4l2 capabilities");
    return -1;
}

if (!(cap.capabilities & V4L2_CAP_VIDEO_OUTPUT)) {
    fprintf(stderr, "device is not a video output device\n");
    return -1;
}

struct v4l2_format fmt;
memset(&fmt, 0, sizeof(fmt));
fmt.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
fmt.fmt.pix.width = 640;
fmt.fmt.pix.height = 480;
fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUV420;
fmt.fmt.pix.field = V4L2_FIELD_NONE;

if (xioctl(fd, VIDIOC_S_FMT, &fmt) < 0) {
    perror("set v4l2 format");
    return -1;
}

struct v4l2_requestbuffers req;
memset(&req, 0, sizeof(req));
req.count = BUFFER_DEPTH;
req.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
req.memory = V4L2_MEMORY_MMAP;

if (xioctl(fd, VIDIOC_REQBUFS, &req) < 0) {
    perror("request v4l2 buffers");
    return -1;
}

if (req.count < BUFFER_DEPTH) {
    fprintf(stderr, "insufficient buffer memory on device\n");
    return -1;
}

for (int i = 0; i < BUFFER_DEPTH; i++) {
    struct v4l2_buffer buf;
    memset(&buf, 0, sizeof(buf));
    buf.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
    buf.memory = V4L2_MEMORY_MMAP;
    buf.index = i;

    if (xioctl(fd, VIDIOC_QUERYBUF, &buf) < 0) {
        perror("query v4l2 buffer");
        return -1;
    }

    v4l2->buffers[i].length = buf.length;
    v4l2->buffers[i].start = mmap(NULL, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, fd, buf.m.offset);

    if (v4l2->buffers[i].start == MAP_FAILED) {
        perror("map v4l2 buffer");
        return -1;
    }

    struct buffer_list_t *list = (struct buffer_list_t *)malloc(sizeof(struct buffer_list_t));
    list->buffer = &v4l2->buffers[i];
    list->next = NULL;

    pthread_mutex_lock(&v4l2->queue.mutex);
    if (v4l2->queue.head == NULL) {
        v4l2->queue.head = list;
    } else {
        v4l2->queue.tail->next = list;
    }
    v4l2->queue.tail = list;
    v4l2->queue.length++;
    pthread_mutex_unlock(&v4l2->queue.mutex);
}

v4l2->fd = fd;
v4l2->buffer_count = 0;
v4l2->frame_count = 0;
v4l2->fps = 0;
pthread_mutex_init(&v4l2->mutex, NULL);
pthread_cond_init(&v4l2->cond, NULL);
v4l2->running = 1;
gettimeofday(&v4l2->last_time, NULL);

return 0;

}

static void *v4l2_thread(void *arg) { struct v4l2_t *v4l2 = (struct v4l2_t *)arg; fd_set fds; struct timeval tv; int r;

while (v4l2->running) {
    FD_ZERO(&fds);
    FD_SET(v4l2->fd, &fds);

    tv.tv_sec = 1;
    tv.tv_usec = 0;

    r = select(v4l2->fd + 1, NULL, &fds, NULL, &tv);
    if (r == -1) {
        perror("select");
        break;
    }

    if (r == 0) {
        fprintf(stderr, "select timeout\n");
        continue;
    }

    if (v4l2->buffer_count > 0) {
        struct buffer_list_t *list = NULL;

        pthread_mutex_lock(&v4l2->queue.mutex);
        if (v4l2->queue.head != NULL) {
            list = v4l2->queue.head;
            v4l2->queue.head = list->next;
            if (v4l2->queue.head == NULL) {
                v4l2->queue.tail = NULL;
            }
            v4l2->queue.length--;
        }
        pthread_mutex_unlock(&v4l2->queue.mutex);

        if (list != NULL) {
            struct buffer_t *buffer = list->buffer;
            free(list);

            struct v4l2_buffer buf;
            memset(&buf, 0, sizeof(buf));
            buf.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
            buf.memory = V4L2_MEMORY_MMAP;
            buf.index = buffer - v4l2->buffers;

            if (xioctl(v4l2->fd, VIDIOC_QBUF, &buf) < 0) {
                perror("queue v4l2 buffer");
                break;
            }

            pthread_mutex_lock(&v4l2->mutex);
            v4l2->buffer_count--;
            pthread_cond_signal(&v4l2->cond);
            pthread_mutex_unlock(&v4l2->mutex);
        }
    } else {
        usleep(1000);
    }
}

return NULL;

}

static int start_camera(struct camera_t *camera) { for (int i = 0; i < BUFFER_DEPTH; i++) { struct v4l2_buffer buf; memset(&buf, 0, sizeof(buf)); buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; buf.memory = V4L2_MEMORY_MMAP; buf.index = i;

    if (xioctl(camera->fd, VIDIOC_QBUF, &buf) < 0) {
        perror("queue camera buffer");
        return -1;
    }
}

enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
if (xioctl(camera->fd, VIDIOC_STREAMON, &type) < 0) {
    perror("start camera streaming");
    return -1;
}

if (pthread_create(&camera->thread, NULL, camera_thread, camera) != 0) {
    perror("create camera thread");
    return -1;
}

return 0;

}

static void stop_camera(struct camera_t *camera) { camera->running = 0;

if (pthread_join(camera->thread, NULL) != 0) {
    perror("join camera thread");
}

enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
if (xioctl(camera->fd, VIDIOC_STREAMOFF, &type) < 0) {
    perror("stop camera streaming");
}

for (int i = 0; i < BUFFER_DEPTH; i++) {
    munmap(camera->buffers[i].start, camera->buffers[i].length);
}

close(camera->fd);

}

static void start_v4l2(struct v4l2_t *v4l2) { for (int i = 0; i < BUFFER_DEPTH; i++) { struct v4l2_buffer buf; memset(&buf, 0, sizeof(buf)); buf.type = V4L2_BUF_TYPE_VIDEO_OUTPUT; buf.memory = V4L2_MEMORY_MMAP; buf.index = i;

    if (xioctl(v4l2->fd, VIDIOC_QBUF, &buf) < 0) {
        perror("queue v4l2 buffer");
        return;
    }
}

enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
if (xioctl(v4l2->fd, VIDIOC_STREAMON, &type) < 0) {
    perror("start v4l2 streaming");
    return;
}

if (pthread_create(&v4l2->thread, NULL, v4l2_thread, v4l2) != 0) {
    perror("create v4l2 thread");
    return;
}

}

static void stop_v4l2(struct v4l2_t *v4l2) { v4l2->running = 0;

if (pthread_join(v4l2->thread, NULL) != 0) {
    perror("join v4l2 thread");
}

enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
if (xioctl(v4l2->fd, VIDIOC_STREAMOFF, &type) < 0) {
    perror("stop v4l2 streaming");
}

for (int i = 0; i < BUFFER_DEPTH; i++) {
    munmap(v4l2->buffers[i].start, v4l2->buffers[i].length);
}

close(v4l2->fd);

}

int main(int argc, char **argv) { for (int i = 0; i < CAMERA_NUM; i++) { if (init_camera(&cameras[i], i) != 0) { fprintf(stderr, "failed to initialize camera %d\n", i); return EXIT_FAILURE; }

    if (start_camera(&cameras[i]) != 0) {
        fprintf(stderr, "failed to start camera %d\n", i);
        return EXIT_FAILURE;
    }
}

if (init_v4l2(&v4l2_dev) != 0) {
    fprintf(stderr, "failed to initialize v4l2 device\n");
    return EXIT_FAILURE;
}

start_v4l2(&v4l2_dev);

while (1) {
    pthread_mutex_lock(&v4l2_dev.mutex);
    while (v4l2_dev.buffer_count == 0) {
        pthread_cond_wait(&v4l2_dev.cond, &v4l2_dev.mutex);
    }
    pthread_mutex_unlock(&v4l2_dev.mutex);

    for (int i = 0; i < CAMERA_NUM; i++) {
        pthread_mutex_lock(&cameras[i].mutex);
        while (cameras[i].buffer_count == 0) {
            pthread_cond_wait(&cameras[i].cond, &cameras[i].mutex);
        }
        pthread_mutex_unlock(&cameras[i].mutex);

        struct buffer_t *src_buffer = &cameras[i].buffers[cameras[i].buffer_index];
        struct buffer_t *dst_buffer = &v4l2_dev.buffers[v4l2_dev.buffer_count];

        memcpy(dst_buffer->start, src_buffer->start, src_buffer->length);

        pthread_mutex_lock(&v4l2_dev.mutex);
        v4l2_dev.buffer_count++;
        pthread_mutex_unlock(&v4l2_dev.mutex);

        cameras[i].buffer_index = (cameras[i].buffer_index + 1) % BUFFER_DEPTH;
        cameras[i].buffer_count--;
        cameras[i].frame_count++;
    }

    struct timeval now;
    gettimeofday(&now, NULL);
    int elapsed = now.tv_sec - v4l2_dev.last_time.tv_sec;
    if (elapsed >= 1) {
        v4l2_dev.fps = v4l2_dev.frame_count / elapsed;
        v4l2_dev.frame_count = 0;
        v4l2_dev.last_time = now;
    }

    for (int i = 0; i < CAMERA_NUM; i++) {
        struct timeval now;
        gettimeofday(&now, NULL);
        int elapsed = now.tv_sec - cameras[i].last_time.tv_sec;
        if (elapsed >= 1) {
            cameras[i].fps = cameras[i].frame_count / elapsed;
            cameras[i].frame_count = 0;
            cameras[i].last_time = now;
        }
    }
}

stop_v4l2(&v4l2_dev);

for (int i = 0; i < CAMERA_NUM; i++) {
    stop_camera(&cameras[i]);
}

return EXIT_SUCCESS;
Linux C 语言三码流视频采集 - V4L2 框架

原文地址: https://www.cveoy.top/t/topic/mRRR 著作权归作者所有。请勿转载和采集!

免费AI点我,无需注册和登录