From 0338184df3971075b54fe1cea15a8a8f01cdcda5 Mon Sep 17 00:00:00 2001 From: Freek van Tienen Date: Sun, 8 Feb 2015 22:37:30 +0100 Subject: [PATCH] [v4l2] Rewrite of the Video For Linux driver --- conf/airframes/ardrone2_raw.xml | 2 +- .../modules/computer_vision/lib/v4l/video.c | 453 ++++++++++++------ .../modules/computer_vision/lib/v4l/video.h | 93 ++-- .../opticflow/opticflow_thread.c | 40 +- .../computer_vision/opticflow_module.c | 4 +- 5 files changed, 388 insertions(+), 204 deletions(-) diff --git a/conf/airframes/ardrone2_raw.xml b/conf/airframes/ardrone2_raw.xml index a59efa45a8f..6e7883874eb 100644 --- a/conf/airframes/ardrone2_raw.xml +++ b/conf/airframes/ardrone2_raw.xml @@ -29,7 +29,7 @@ - + diff --git a/sw/airborne/modules/computer_vision/lib/v4l/video.c b/sw/airborne/modules/computer_vision/lib/v4l/video.c index e0a9fb5c318..a6f8ce5363e 100644 --- a/sw/airborne/modules/computer_vision/lib/v4l/video.c +++ b/sw/airborne/modules/computer_vision/lib/v4l/video.c @@ -1,234 +1,405 @@ /* - video.c - video driver - - Copyright (C) 2011 Hugo Perquin - http://blog.perquin.com - - This program is free software; you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation; either version 2 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. + * Copyright (C) 2015 Freek van Tienen + * Copyright (C) 2011 Hugo Perquin - http://blog.perquin.com + * + * This file is part of Paparazzi. + * + * Paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * Paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + * + */ - You should have received a copy of the GNU General Public License - along with this program; if not, write to the Free Software - Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, - MA 02110-1301 USA. +/** + * @file modules/computer_vision/lib/v4l/v4l.c + * Capture images from a V4L2 device (Video for Linux 2) */ #include -#include #include #include -#include #include #include #include #include -#include -#include -#include #include #include -#include #include #include #include "video.h" #define CLEAR(x) memset (&(x), 0, sizeof (x)) +static void *v4l2_capture_thread(void *data); - -pthread_t video_thread; -void *video_thread_main(void *data); -void *video_thread_main(void *data) +/** + * The main capturing thread + * This thread handles the queue and dequeue of buffers, to make sure only the latest + * image buffer is preserved for image processing. + */ +static void *v4l2_capture_thread(void *data) { - struct vid_struct *vid = (struct vid_struct *)data; - printf("video_thread_main started\n"); - while (1) { - fd_set fds; - struct timeval tv; - int r; + struct v4l2_device *dev = (struct v4l2_device *)data; + struct v4l2_buffer buf; + struct timeval tv; + fd_set fds; + while (TRUE) { FD_ZERO(&fds); - FD_SET(vid->fd, &fds); + FD_SET(dev->fd, &fds); + // Set the timeout to 2 seconds tv.tv_sec = 2; tv.tv_usec = 0; - r = select(vid->fd + 1, &fds, NULL, NULL, &tv); - if (-1 == r) { + // Wait until an image was taken, with a timeout of tv + int sr = select(dev->fd + 1, &fds, NULL, NULL, &tv); + if (sr < 0) { + // Was interrupted by a signal if (EINTR == errno) { continue; } - printf("select err\n"); + printf("[v4l2-capture] Select error %d on %s: %s\n", errno, dev->name, strerror(errno)); + dev->thread = (pthread_t) NULL; + return (void *) -1; } - - if (0 == r) { - fprintf(stderr, "select timeout\n"); - return 0; + else if(sr == 0) { + printf("[v4l2-capture] Select timeout on %s\n", dev->name); + //continue; + dev->thread = (pthread_t) NULL; + return (void *) -2; } - struct v4l2_buffer buf; - + // Dequeue a buffer CLEAR(buf); buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; buf.memory = V4L2_MEMORY_MMAP; - if (ioctl(vid->fd, VIDIOC_DQBUF, &buf) < 0) { - printf("ioctl() VIDIOC_DQBUF failed.\n"); - break; + if (ioctl(dev->fd, VIDIOC_DQBUF, &buf) < 0) { + printf("[v4l2-capture] Dequeue of buffer failed for %s.\n", dev->name); + dev->thread = (pthread_t) NULL; + return (void *) -3; } - - assert(buf.index < vid->n_buffers); - - vid->seq++; - - if (vid->trigger) { - - // todo add timestamp again - //vid->img->timestamp = util_timestamp(); - vid->img->seq = vid->seq; - memcpy(vid->img->buf, vid->buffers[buf.index].buf, vid->w * vid->h * 2); - vid->trigger = 0; + assert(buf.index < dev->buffers_cnt); + + //printf("Got image %d %d\n", buf.timestamp.tv_sec, buf.timestamp.tv_usec); + + // Check if the current image is being processed + struct v4l2_img_buf *prev_img = &dev->buffers[dev->buffers_deq_idx]; + if (dev->buffers_deq_idx != 255 && pthread_mutex_trylock(&prev_img->mutex) == 0) { + // Change the current dequeued index + dev->buffers_deq_idx = buf.index; + + // Enqueue the previous buffer + buf.index = prev_img->idx; + if (ioctl(dev->fd, VIDIOC_QBUF, &buf) < 0) { + printf("[v4l2-capture] Could not enqueue %d for %s\n", prev_img->idx, dev->name); + } + pthread_mutex_unlock(&prev_img->mutex); } - - if (ioctl(vid->fd, VIDIOC_QBUF, &buf) < 0) { - printf("ioctl() VIDIOC_QBUF failed.\n"); - break; + // Image is being processed so enqueue is done after processing + else { + // Change the current index + dev->buffers_deq_idx = buf.index; + //printf("[v4l2-capture] No enqueue, keep image and buffers_deq_idx: %d\n", dev->buffers_deq_idx); } } - return 0; + return (void *)0; } -int video_init(struct vid_struct *vid) -{ +/** + * Initialize a V4L2(Video for Linux 2) device + * The device name should be something like "/dev/video1" + * When the width and height are 0 the width and height of the device is queried + * The buffer_cnt are the amount of buffers used in memory mapping + * Not that you need to close this device at the end of you program! + */ +struct v4l2_device *v4l2_init(char *device_name, uint16_t width, uint16_t height, uint8_t buffers_cnt) { + uint8_t i; struct v4l2_capability cap; struct v4l2_format fmt; - unsigned int i; - enum v4l2_buf_type type; + struct v4l2_requestbuffers req; + CLEAR(cap); + CLEAR(fmt); + CLEAR(req); - vid->seq = 0; - vid->trigger = 0; - if (vid->n_buffers == 0) { vid->n_buffers = 4; } + // Try to open the device + int fd = open(device_name, O_RDWR | O_NONBLOCK, 0); + if (fd < 0) { + printf("[v4l2] Cannot open '%s': %d, %s\n", device_name, errno, strerror(errno)); + return NULL; + } - vid->fd = open(vid->device, O_RDWR | O_NONBLOCK, 0); + // Try to fetch the capabilities of the V4L2 device + if (ioctl(fd, VIDIOC_QUERYCAP, &cap) < 0) { + printf("[v4l2] %s is no V4L2 device\n", device_name); + close(fd); + return NULL; + } - if (ioctl(vid->fd, VIDIOC_QUERYCAP, &cap) < 0) { - printf("ioctl() VIDIOC_QUERYCAP failed.\n"); - return -1; + // Check if the device is capable of capturing and streaming + if (!(cap.capabilities &V4L2_CAP_VIDEO_CAPTURE)) { + printf("[v4l2] %s is no V4L2 video capturing device\n", device_name); + close(fd); + return NULL; + } + if (!(cap.capabilities &V4L2_CAP_STREAMING)) { + printf("[v4l2] %s isn't capable of streaming (TODO: support reading)\n", device_name); + close(fd); + return NULL; } - //printf("2 driver = %s, card = %s, version = %d, capabilities = 0x%x\n", cap.driver, cap.card, cap.version, cap.capabilities); + // TODO: Read video cropping and scaling information VIDIOC_CROPCAP - CLEAR(fmt); + // Set the format settings fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; - fmt.fmt.pix.width = vid->w; - fmt.fmt.pix.height = vid->h; + fmt.fmt.pix.width = width; + fmt.fmt.pix.height = height; fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_UYVY; - if (ioctl(vid->fd, VIDIOC_S_FMT, &fmt) < 0) { - printf("ioctl() VIDIOC_S_FMT failed.\n"); - return -1; + if (ioctl(fd, VIDIOC_S_FMT, &fmt) < 0) { + printf("[v4l2] Could not set data format settings of %s\n", device_name); + close(fd); + return NULL; } - //image_size = fmt.fmt.pix.width * fmt.fmt.pix.height *3/2; - - struct v4l2_requestbuffers req; - - CLEAR(req); - req.count = vid->n_buffers; + // Request MMAP buffers + req.count = buffers_cnt; req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; req.memory = V4L2_MEMORY_MMAP; - - if (ioctl(vid->fd, VIDIOC_REQBUFS, &req) < 0) { - printf("ioctl() VIDIOC_REQBUFS failed.\n"); - return -1; + if (ioctl(fd, VIDIOC_REQBUFS, &req) < 0) { + printf("[v4l2] %s Does not support memory mapping\n", device_name); + close(fd); + return NULL; } - printf("Buffer count = %d\n", vid->n_buffers); - - vid->buffers = (struct buffer_struct *)calloc(vid->n_buffers, sizeof(struct buffer_struct)); + // Allocate memory for the memory mapped buffers + struct v4l2_img_buf *buffers = calloc(req.count, sizeof(struct v4l2_img_buf)); + if (buffers == NULL) { + printf("[v4l2] Not enough memory for %s to initialize %d MMAP buffers\n", device_name, req.count); + close(fd); + return NULL; + } - for (i = 0; i < vid->n_buffers; ++i) { + // Go trough the buffers and initialize them + for (i = 0; i < req.count; ++i) { struct v4l2_buffer buf; - CLEAR(buf); + + // Request the buffer information buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; buf.memory = V4L2_MEMORY_MMAP; buf.index = i; + if (ioctl(fd, VIDIOC_QUERYBUF, &buf) < 0) { + printf("[v4l2] Querying buffer %d from %s failed\n", i, device_name); + free(buffers); + close(fd); + return NULL; + } - if (ioctl(vid->fd, VIDIOC_QUERYBUF, &buf) < 0) { - printf("ioctl() VIDIOC_QUERYBUF failed.\n"); - return -1; + // Map the buffer + buffers[i].idx = i; + buffers[i].length = buf.length; + buffers[i].buf = mmap(NULL, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, fd, buf.m.offset); + if (MAP_FAILED == buffers[i].buf) { + printf("[v4l2] Mapping buffer %d with length %d from %s failed\n", i, buf.length, device_name); + free(buffers); + close(fd); + return NULL; } + } - vid->buffers[i].length = buf.length; - printf("buffer%d.length=%d\n", i, buf.length); - vid->buffers[i].buf = mmap(NULL, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, vid->fd, buf.m.offset); + // Create the device only when everything succeeded + struct v4l2_device *dev = (struct v4l2_device *)malloc(sizeof(struct v4l2_device)); + CLEAR(*dev); + dev->name = strdup(device_name); // NOTE: needs to be freed + dev->fd = fd; + dev->w = width; + dev->h = height; + dev->buffers_cnt = req.count; + dev->buffers = buffers; + return dev; +} - if (MAP_FAILED == vid->buffers[i].buf) { - printf("mmap() failed.\n"); - return -1; - } +/** + * Get the latest image buffer and lock it (Thread safe, BLOCKING) + * This functions blocks until image access is granted. This should not take that long, because + * it is only locked while enqueueing an image. + * Make sure you free the image after processing! + */ +struct v4l2_img_buf *v4l2_image_get(struct v4l2_device *dev) { + // Try to get access to the current image + uint16_t deq_idx = dev->buffers_deq_idx; + while (TRUE) { + if(deq_idx != 255 && pthread_mutex_trylock(&dev->buffers[deq_idx].mutex) == 0) + break; + + deq_idx = dev->buffers_deq_idx; + usleep(1); + } + return &dev->buffers[deq_idx]; +} + +/** + * Get the latest image and lock it (Thread safe, NON BLOCKING) + * This function returns NULL if it can't get access to the current image. + * Make sure you free the image after processing! + */ +struct v4l2_img_buf *v4l2_image_get_nonblock(struct v4l2_device *dev) { + struct v4l2_img_buf *img_buf = &dev->buffers[dev->buffers_deq_idx]; + if (dev->buffers_deq_idx == 255 || pthread_mutex_trylock(&img_buf->mutex) != 0) { + return NULL; + } + return img_buf; +} + +/** + * Free the image and enqueue the buffer (Thread safe) + * This must be done after processing the image, because else all buffers are locked + */ +void v4l2_image_free(struct v4l2_device *dev, struct v4l2_img_buf *img_buf) +{ + struct v4l2_buffer buf; + + // Enqueue the buffer + CLEAR(buf); + buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + buf.memory = V4L2_MEMORY_MMAP; + buf.index = img_buf->idx; + if (ioctl(dev->fd, VIDIOC_QBUF, &buf) < 0) { + printf("[v4l2] Could not enqueue %d for %s\n", img_buf->idx, dev->name); + } + + //RACE CONDITION!! + if(dev->buffers_deq_idx == img_buf->idx) + dev->buffers_deq_idx = 255; + + // Unlock the buffer + pthread_mutex_unlock(&img_buf->mutex); +} + +/** + * Start capturing images in streaming mode (Thread safe) + * Returns true when successfully started capturing. Not that it also returns + * FALSE when it already is in capturing mode. + */ +bool_t v4l2_start_capture(struct v4l2_device *dev) +{ + uint8_t i; + enum v4l2_buf_type type; + + // Check if not already running + if (dev->thread != (pthread_t)NULL) { + printf("[v4l2] There is already a capturing thread running for %s\n", dev->name); + return FALSE; } - for (i = 0; i < vid->n_buffers; ++i) { + // Enqueue all buffers + dev->buffers_deq_idx = 255; // Set none to dequeued + for (i = 0; i < dev->buffers_cnt; ++i) { struct v4l2_buffer buf; CLEAR(buf); buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; buf.memory = V4L2_MEMORY_MMAP; buf.index = i; - - if (ioctl(vid->fd, VIDIOC_QBUF, &buf) < 0) { - printf("ioctl() VIDIOC_QBUF failed.\n"); - return -1; + if (ioctl(dev->fd, VIDIOC_QBUF, &buf) < 0) { + printf("[v4l2] Could not enqueue buffer %d during start capture for %s\n", i, dev->name); + return FALSE; } } + // Start the stream type = V4L2_BUF_TYPE_VIDEO_CAPTURE; - if (ioctl(vid->fd, VIDIOC_STREAMON, &type) < 0) { - printf("ioctl() VIDIOC_STREAMON failed.\n"); - return -1; + if (ioctl(dev->fd, VIDIOC_STREAMON, &type) < 0) { + printf("[v4l2] Could not start stream of %s\n", dev->name); + return FALSE; } - //start video thread - int rc = pthread_create(&video_thread, NULL, video_thread_main, vid); - if (rc) { - printf("ctl_Init: Return code from pthread_create(mot_thread) is %d\n", rc); - return 202; + //Start the capturing thread + int rc = pthread_create(&dev->thread, NULL, v4l2_capture_thread, dev); + if (rc < 0) { + printf("[v4l2] Could not start capturing thread for %s (return code: %d)\n", dev->name, rc); + + // Stop the stream + type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + if (ioctl(dev->fd, VIDIOC_STREAMOFF, &type) < 0) { + printf("[v4l2] Could not stop stream of %s\n", dev->name); + } + + // Reset the thread + dev->thread = (pthread_t) NULL; + return FALSE; } - return 0; + return TRUE; } -void video_close(struct vid_struct *vid) +/** + * Stop capturing of the image stream (Thread safe) + * Returns TRUE if it successfully stopped capturing. Note that it also returns FALSE + * when the capturing is already stopped. This function is blocking until capturing + * thread is closed. + */ +bool_t v4l2_stop_capture(struct v4l2_device *dev) { - int i; - for (i = 0; i < (int)vid->n_buffers; ++i) { - if (-1 == munmap(vid->buffers[i].buf, vid->buffers[i].length)) { printf("munmap() failed.\n"); } + enum v4l2_buf_type type; + + // First check if still running + if (dev->thread == (pthread_t) NULL) { + printf("[v4l2] Already stopped capture for %s\n", dev->name); + return FALSE; + } + + // Stop the stream + type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + if (ioctl(dev->fd, VIDIOC_STREAMOFF, &type) < 0) { + printf("[v4l2] Could not stop stream of %s\n", dev->name); + return FALSE; } - close(vid->fd); + + // Stop the thread + if (pthread_cancel(dev->thread) < 0) { + printf("[v4l2] Could not cancel thread for %s\n", dev->name); + return FALSE; + } + + // Wait for the thread to be finished + pthread_join(dev->thread, NULL); + dev->thread = (pthread_t) NULL; + return TRUE; } -struct img_struct *video_create_image(struct vid_struct *vid) +/** + * Close the V4L2 device (Thread safe) + * This needs to be preformed to clean up all the buffers and close the device. + * Note that this also stops the capturing if it is still capturing. + */ +void v4l2_close(struct v4l2_device *dev) { - struct img_struct *img = (struct img_struct *)malloc(sizeof(struct img_struct)); - img->w = vid->w; - img->h = vid->h; - img->buf = (unsigned char *)malloc(vid->h * vid->w * 2); - return img; -} + uint8_t i; -pthread_mutex_t video_grab_mutex = PTHREAD_MUTEX_INITIALIZER; + // Stop capturing (ignore result as it may already be stopped) + v4l2_stop_capture(dev); -void video_grab_image(struct vid_struct *vid, struct img_struct *img) -{ - pthread_mutex_lock(&video_grab_mutex); - vid->img = img; - vid->trigger = 1; - // while(vid->trigger) pthread_yield(); - while (vid->trigger) { usleep(1); } - pthread_mutex_unlock(&video_grab_mutex); + // Unmap all buffers + for (i = 0; i < dev->buffers_cnt; ++i) { + pthread_mutex_lock(&dev->buffers[i].mutex); + if (munmap(dev->buffers[i].buf, dev->buffers[i].length) < 0) { + printf("[v4l2] Could not unmap buffer %d for %s\n", i, dev->name); + } + pthread_mutex_unlock(&dev->buffers[i].mutex); + } + + // Close the file pointer and free all memory + close(dev->fd); + free(dev->name); + free(dev); } diff --git a/sw/airborne/modules/computer_vision/lib/v4l/video.h b/sw/airborne/modules/computer_vision/lib/v4l/video.h index 030f746d4b6..638620337a8 100644 --- a/sw/airborne/modules/computer_vision/lib/v4l/video.h +++ b/sw/airborne/modules/computer_vision/lib/v4l/video.h @@ -1,53 +1,62 @@ /* - video.h - video driver - - Copyright (C) 2011 Hugo Perquin - http://blog.perquin.com - - This program is free software; you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation; either version 2 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. + * Copyright (C) 2015 Freek van Tienen + * Copyright (C) 2011 Hugo Perquin - http://blog.perquin.com + * + * This file is part of Paparazzi. + * + * Paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * Paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + * + */ - You should have received a copy of the GNU General Public License - along with this program; if not, write to the Free Software - Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, - MA 02110-1301 USA. +/** + * @file modules/computer_vision/lib/v4l/v4l.h + * Capture images from a V4L2 device (Video for Linux 2) */ -#ifndef _VIDEO_H -#define _VIDEO_H +#ifndef _V4L2_H +#define _V4L2_H -#include "modules/computer_vision/cv/image.h" +#include "std.h" -struct buffer_struct { - void *buf; - size_t length; +/* V4L2 memory mapped image buffer */ +struct v4l2_img_buf { + uint8_t idx; //< The index of the buffer + pthread_mutex_t mutex; //< Mutex lock of an image (the image is being processed) + size_t length; //< The size of the buffer + void *buf; //< Pointer to the memory mapped buffer }; -struct vid_struct { - char *device; - int w; - int h; - int seq; - unsigned int n_buffers; - - //private members - int trigger; - struct img_struct *img; - struct buffer_struct *buffers; - int fd; +/* V4L2 device */ +struct v4l2_device { + char *name; //< The name of the device + int fd; //< The file pointer to the device + pthread_t thread; //< The thread that handles the images + uint16_t w; //< The width of the image + uint16_t h; //< The height of the image + uint8_t buffers_cnt; //< The number of image buffers + uint8_t buffers_deq_idx; //< The current dequeued index + struct v4l2_img_buf *buffers; //< The memory mapped image buffers }; +/* External functions */ +struct v4l2_device *v4l2_init(char *device_name, uint16_t width, uint16_t height, uint8_t buffers_cnt); +struct v4l2_img_buf *v4l2_image_get(struct v4l2_device *dev); +struct v4l2_img_buf *v4l2_image_get_nonblock(struct v4l2_device *dev); +void v4l2_image_free(struct v4l2_device *dev, struct v4l2_img_buf *img_buf); +bool_t v4l2_start_capture(struct v4l2_device *dev); +bool_t v4l2_stop_capture(struct v4l2_device *dev); +void v4l2_close(struct v4l2_device *dev); -int video_init(struct vid_struct *vid); -struct img_struct *video_create_image(struct vid_struct *vid); - -void video_grab_image(struct vid_struct *vid, struct img_struct *img); -void video_close(struct vid_struct *vid); - -#endif +#endif /* _V4L2_H */ diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.c b/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.c index 0566279f1b8..61cbdf36f76 100644 --- a/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.c +++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.c @@ -70,27 +70,26 @@ void *computervision_thread_main(void *args) // Status computer_vision_thread_command = RUN; - // Video Input - struct vid_struct vid; + /* On ARDrone2: * video1 = front camera; video2 = bottom camera */ - vid.device = (char *)"/dev/video2"; - vid.w = 320; - vid.h = 240; - vid.n_buffers = 4; - - if (video_init(&vid) < 0) { + // Create a V4L2 device + struct v4l2_device *dev = v4l2_init("/dev/video2", 320, 240, 10); + if (dev == NULL) { printf("Error initialising video\n"); return 0; } - // Video Grabbing - struct img_struct *img_new = video_create_image(&vid); + // Start the streaming on the V4L2 device + if(!v4l2_start_capture(dev)) { + printf("Could not start capture\n"); + return 0; + } #ifdef DOWNLINK_VIDEO // Video Compression - uint8_t *jpegbuf = (uint8_t *)malloc(vid.h * vid.w * 2); + uint8_t *jpegbuf = (uint8_t *)malloc(dev->w * dev->h * 2); // Network Transmit struct UdpSocket *vsock; @@ -100,12 +99,12 @@ void *computervision_thread_main(void *args) #endif // First Apply Settings before init - opticflow_plugin_init(vid.w, vid.h, &vision_results); + opticflow_plugin_init(dev->w, dev->h, &vision_results); while (computer_vision_thread_command == RUN) { // Wait for a new frame - video_grab_image(&vid, img_new); + struct v4l2_img_buf *img = v4l2_image_get(dev); // Get most recent State information int bytes_read = sizeof(autopilot_data); @@ -121,7 +120,9 @@ void *computervision_thread_main(void *args) DEBUG_INFO("[thread] Read # %d\n",autopilot_data.cnt); // Run Image Processing with image and data and get results - opticflow_plugin_run(img_new->buf, &autopilot_data, &vision_results); + opticflow_plugin_run(img->buf, &autopilot_data, &vision_results); + + //printf("Vision result %f %f\n", vision_results.Velx, vision_results.Vely); /* Send results to main */ vision_results.cnt++; @@ -136,15 +137,18 @@ void *computervision_thread_main(void *args) uint32_t quality_factor = 10; //20 if no resize, uint8_t dri_header = 0; uint32_t image_format = FOUR_TWO_TWO; // format (in jpeg.h) - uint8_t *end = encode_image(small.buf, jpegbuf, quality_factor, image_format, small.w, small.h, dri_header); + uint8_t *end = encode_image(img->buf, jpegbuf, quality_factor, image_format, dev->w, dev->h, dri_header); uint32_t size = end - (jpegbuf); - printf("Sending an image ...%u\n", size); - send_rtp_frame(vsock, jpegbuf, size, small.w, small.h, 0, quality_factor, dri_header, 0); + //printf("Sending an image ...%u\n", size); + send_rtp_frame(vsock, jpegbuf, size, dev->w, dev->h, 0, quality_factor, dri_header, 0); #endif + + // Free the image + v4l2_image_free(dev, img); } printf("Thread Closed\n"); - video_close(&vid); + v4l2_close(dev); return 0; } diff --git a/sw/airborne/modules/computer_vision/opticflow_module.c b/sw/airborne/modules/computer_vision/opticflow_module.c index 3687f65e2ce..dfd33ef3e39 100644 --- a/sw/airborne/modules/computer_vision/opticflow_module.c +++ b/sw/airborne/modules/computer_vision/opticflow_module.c @@ -99,8 +99,8 @@ void opticflow_module_run(void) opticflow_module_data.phi = stateGetNedToBodyEulers_f()->phi; opticflow_module_data.theta = stateGetNedToBodyEulers_f()->theta; int bytes_written = write(cv_sockets[0], &opticflow_module_data, sizeof(opticflow_module_data)); - if (bytes_written != sizeof(opticflow_module_data)){ - printf("[module] Failed to write to socket: written = %d, error=%d.\n",bytes_written, errno); + if (bytes_written != sizeof(opticflow_module_data) && errno !=4){ + printf("[module] Failed to write to socket: written = %d, error=%d, %s.\n",bytes_written, errno, strerror(errno)); } else { DEBUG_INFO("[module] Write # %d (%d bytes)\n",opticflow_module_data.cnt, bytes_written);