Paparazzi UAS  v5.14.0_stable-0-g3f680d1
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
v4l2.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2015 Freek van Tienen <freek.v.tienen@gmail.com>
3  * Copyright (C) 2011 Hugo Perquin - http://blog.perquin.com
4  *
5  * This file is part of Paparazzi.
6  *
7  * Paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * Paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, see
19  * <http://www.gnu.org/licenses/>.
20  *
21  */
22 
28 #include <stdio.h>
29 #include <string.h>
30 #include <assert.h>
31 #include <fcntl.h>
32 #include <unistd.h>
33 #include <errno.h>
34 #include <malloc.h>
35 #include <sys/mman.h>
36 #include <sys/ioctl.h>
37 #include <linux/videodev2.h>
38 #include <pthread.h>
39 
40 #include "v4l2.h"
41 
42 #include <sys/time.h>
43 #include "mcu_periph/sys_time.h"
44 
45 #define CLEAR(x) memset(&(x), 0, sizeof (x))
46 static void *v4l2_capture_thread(void *data);
47 
56 static void *v4l2_capture_thread(void *data)
57 {
58  struct v4l2_device *dev = (struct v4l2_device *)data;
59  struct v4l2_buffer buf;
60  struct timeval tv;
61  fd_set fds;
62 
63  while (TRUE) {
64  FD_ZERO(&fds);
65  FD_SET(dev->fd, &fds);
66 
67  // Set the timeout to 2 seconds
68  tv.tv_sec = 2;
69  tv.tv_usec = 0;
70 
71  // Wait until an image was taken, with a timeout of tv
72  int sr = select(dev->fd + 1, &fds, NULL, NULL, &tv);
73  uint32_t now_ts = get_sys_time_usec();
74 
75  if (sr < 0) {
76  // Was interrupted by a signal
77  if (EINTR == errno) { continue; }
78  printf("[v4l2-capture] Select error %d on %s: %s\n", errno, dev->name, strerror(errno));
79  dev->thread = (pthread_t) NULL;
80  return (void *) - 1;
81  } else if (sr == 0) {
82  printf("[v4l2-capture] Select timeout on %s\n", dev->name);
83  continue;
84  //dev->thread = (pthread_t) NULL;
85  //return (void *) - 2;
86  }
87 
88  // Dequeue a buffer
89  CLEAR(buf);
90  buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
91  buf.memory = V4L2_MEMORY_MMAP;
92  if (ioctl(dev->fd, VIDIOC_DQBUF, &buf) < 0) {
93  printf("[v4l2-capture] Dequeue of buffer failed for %s.\n", dev->name);
94  dev->thread = (pthread_t) NULL;
95  return (void *) - 3;
96  }
97  assert(buf.index < dev->buffers_cnt);
98 
99  // Copy the timestamp
100  dev->buffers[buf.index].timestamp = buf.timestamp;
101  dev->buffers[buf.index].pprz_timestamp = now_ts;
102 
103  // Update the dequeued id
104  // We need lock because between setting prev_idx and updating the deq_idx the deq_idx could change
105  pthread_mutex_lock(&dev->mutex);
106  uint16_t prev_idx = dev->buffers_deq_idx;
107  dev->buffers_deq_idx = buf.index;
108  pthread_mutex_unlock(&dev->mutex);
109 
110  // Enqueue the previous image if not empty
111  if (prev_idx != V4L2_IMG_NONE) {
112  // Enqueue the previous buffer
113  CLEAR(buf);
114  buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
115  buf.memory = V4L2_MEMORY_MMAP;
116  buf.index = prev_idx;
117  if (ioctl(dev->fd, VIDIOC_QBUF, &buf) < 0) {
118  printf("[v4l2-capture] Could not enqueue %d for %s\n", prev_idx, dev->name);
119  }
120  }
121 
122  }
123  return (void *)0;
124 }
125 
136 bool v4l2_init_subdev(char *subdev_name, uint8_t pad, uint16_t code, struct img_size_t size)
137 {
138  struct v4l2_subdev_format sfmt;
139  CLEAR(sfmt);
140 
141  // Try to open the subdevice
142  int fd = open(subdev_name, O_RDWR, 0);
143  if (fd < 0) {
144  printf("[v4l2] Cannot open subdevice '%s': %d, %s\n", subdev_name, errno, strerror(errno));
145  return false;
146  }
147 
148  // Try to get the subdevice data format settings
149  if (ioctl(fd, VIDIOC_SUBDEV_G_FMT, &sfmt) < 0) {
150  printf("[v4l2] Could not get subdevice data format settings of %s\n", subdev_name);
151  close(fd);
152  return false;
153  }
154 
155  // Set the new settings
156  sfmt.pad = pad;
157  sfmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
158  sfmt.format.width = size.w;
159  sfmt.format.height = size.h;
160  sfmt.format.code = code;
161  sfmt.format.field = V4L2_FIELD_NONE;
162  sfmt.format.colorspace = 1;
163 
164  if (ioctl(fd, VIDIOC_SUBDEV_S_FMT, &sfmt) < 0) {
165  printf("[v4l2] Could not set subdevice data format settings of %s\n", subdev_name);
166  close(fd);
167  return false;
168  }
169 
170  // Close the device
171  close(fd);
172  return true;
173 }
174 
183 struct v4l2_device *v4l2_init(char *device_name, struct img_size_t size, struct crop_t crop, uint8_t buffers_cnt,
184  uint32_t _pixelformat)
185 {
186  uint8_t i;
187  struct v4l2_capability cap;
188  struct v4l2_format fmt;
189  struct v4l2_requestbuffers req;
190  struct v4l2_fmtdesc fmtdesc;
191  struct v4l2_crop crp;
192  CLEAR(cap);
193  CLEAR(fmt);
194  CLEAR(req);
195  CLEAR(fmtdesc);
196  CLEAR(crp);
197 
198  // Try to open the device
199  int fd = open(device_name, O_RDWR | O_NONBLOCK, 0);
200  if (fd < 0) {
201  printf("[v4l2] Cannot open '%s': %d, %s\n", device_name, errno, strerror(errno));
202  return NULL;
203  }
204 
205  // Try to fetch the capabilities of the V4L2 device
206  if (ioctl(fd, VIDIOC_QUERYCAP, &cap) < 0) {
207  printf("[v4l2] %s is no V4L2 device\n", device_name);
208  close(fd);
209  return NULL;
210  }
211 
212  // Check if the device is capable of capturing and streaming
213  if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
214  printf("[v4l2] %s is no V4L2 video capturing device\n", device_name);
215  close(fd);
216  return NULL;
217  }
218  if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
219  printf("[v4l2] %s isn't capable of streaming (TODO: support reading)\n", device_name);
220  close(fd);
221  return NULL;
222  }
223 
224  fmtdesc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
225  while (ioctl(fd, VIDIOC_ENUM_FMT, &fmtdesc) == 0) {
226  fmtdesc.index++;
227  if(fmtdesc.pixelformat == _pixelformat)
228  break;
229  }
230 
231  // Accept if no format can be get
232  if(fmtdesc.index != 0 && fmtdesc.pixelformat != _pixelformat) {
233  printf("[v4l2] Pixelformat not available on device %s (wanted: %4X)\r\n", device_name, _pixelformat);
234  return NULL;
235  }
236 
237  // Set the cropping window
238  crp.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
239  crp.c.top = crop.y;
240  crp.c.left = crop.x;
241  crp.c.width = crop.w;
242  crp.c.height = crop.h;
243 
244  // Only crop when needed
245  if(crop.x != 0 || crop.y != 0 || crop.w != size.w || crop.h != size.h) {
246  if (ioctl(fd, VIDIOC_S_CROP, &crp) < 0) {
247  printf("[v4l2] Could not set crop window of %s\n", device_name);
248  close(fd);
249  return NULL;
250  }
251  }
252 
253  // Set the format settings
254  fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
255  fmt.fmt.pix.width = size.w;
256  fmt.fmt.pix.height = size.h;
257  fmt.fmt.pix.pixelformat = _pixelformat;
258  fmt.fmt.pix.colorspace = V4L2_COLORSPACE_REC709;
259  fmt.fmt.pix.field = V4L2_FIELD_NONE;
260 
261  if (ioctl(fd, VIDIOC_S_FMT, &fmt) < 0) {
262  printf("[v4l2] Could not set data format settings of %s\n", device_name);
263  close(fd);
264  return NULL;
265  }
266 
267  // Request MMAP buffers
268  req.count = buffers_cnt;
269  req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
270  req.memory = V4L2_MEMORY_MMAP;
271  if (ioctl(fd, VIDIOC_REQBUFS, &req) < 0) {
272  printf("[v4l2] %s Does not support memory mapping\n", device_name);
273  close(fd);
274  return NULL;
275  }
276 
277  // Allocate memory for the memory mapped buffers
278  struct v4l2_img_buf *buffers = calloc(req.count, sizeof(struct v4l2_img_buf));
279  if (buffers == NULL) {
280  printf("[v4l2] Not enough memory for %s to initialize %d MMAP buffers\n", device_name, req.count);
281  close(fd);
282  return NULL;
283  }
284 
285  // Go trough the buffers and initialize them
286  for (i = 0; i < req.count; ++i) {
287  struct v4l2_buffer buf;
288  CLEAR(buf);
289 
290  // Request the buffer information
291  buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
292  buf.memory = V4L2_MEMORY_MMAP;
293  buf.index = i;
294  if (ioctl(fd, VIDIOC_QUERYBUF, &buf) < 0) {
295  printf("[v4l2] Querying buffer %d from %s failed\n", i, device_name);
296  free(buffers);
297  close(fd);
298  return NULL;
299  }
300 
301  // Map the buffer
302  buffers[i].length = buf.length;
303  buffers[i].buf = mmap(NULL, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, fd, buf.m.offset);
304  if (MAP_FAILED == buffers[i].buf) {
305  printf("[v4l2] Mapping buffer %d with length %d from %s failed\n", i, buf.length, device_name);
306  free(buffers);
307  close(fd);
308  return NULL;
309  }
310  }
311 
312  // Create the device only when everything succeeded
313  struct v4l2_device *dev = (struct v4l2_device *)malloc(sizeof(struct v4l2_device));
314  CLEAR(*dev);
315  dev->name = strdup(device_name); // NOTE: needs to be freed
316  dev->fd = fd;
317  dev->w = size.w;
318  dev->h = size.h;
319  dev->buffers_cnt = req.count;
320  dev->buffers = buffers;
321  return dev;
322 }
323 
332 void v4l2_image_get(struct v4l2_device *dev, struct image_t *img)
333 {
334  uint16_t img_idx = V4L2_IMG_NONE;
335 
336  // Continu to wait for an image
337  while (img_idx == V4L2_IMG_NONE) {
338  // We first check if the deq_idx is ok, this reduces the amount of locks
339  if (dev->buffers_deq_idx != V4L2_IMG_NONE) {
340  pthread_mutex_lock(&dev->mutex);
341 
342  // We need to check it here again, because it could be changed
343  if (dev->buffers_deq_idx != V4L2_IMG_NONE) {
344  img_idx = dev->buffers_deq_idx;
346  }
347 
348  pthread_mutex_unlock(&dev->mutex);
349  }
350  }
351 
352  // Set the image
353  img->type = IMAGE_YUV422;
354  img->w = dev->w;
355  img->h = dev->h;
356  img->buf_idx = img_idx;
357  img->buf_size = dev->buffers[img_idx].length;
358  img->buf = dev->buffers[img_idx].buf;
359  img->ts = dev->buffers[img_idx].timestamp;
360  img->pprz_ts = dev->buffers[img_idx].pprz_timestamp;
361 }
362 
371 bool v4l2_image_get_nonblock(struct v4l2_device *dev, struct image_t *img)
372 {
373  uint16_t img_idx = V4L2_IMG_NONE;
374 
375  // Try to get the current image
376  pthread_mutex_lock(&dev->mutex);
377  if (dev->buffers_deq_idx != V4L2_IMG_NONE) {
378  img_idx = dev->buffers_deq_idx;
380  }
381  pthread_mutex_unlock(&dev->mutex);
382 
383  // Check if we really got an image
384  if (img_idx == V4L2_IMG_NONE) {
385  return false;
386  } else {
387  // Set the image
388  img->type = IMAGE_YUV422;
389  img->w = dev->w;
390  img->h = dev->h;
391  img->buf_idx = img_idx;
392  img->buf_size = dev->buffers[img_idx].length;
393  img->buf = dev->buffers[img_idx].buf;
394  img->ts = dev->buffers[img_idx].timestamp;
395  img->pprz_ts = dev->buffers[img_idx].pprz_timestamp;
396  return true;
397  }
398 }
399 
406 void v4l2_image_free(struct v4l2_device *dev, struct image_t *img)
407 {
408  struct v4l2_buffer buf;
409 
410  // Enqueue the buffer
411  CLEAR(buf);
412  buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
413  buf.memory = V4L2_MEMORY_MMAP;
414  buf.index = img->buf_idx;
415  if (ioctl(dev->fd, VIDIOC_QBUF, &buf) < 0) {
416  printf("[v4l2] Could not enqueue %d for %s\n", img->buf_idx, dev->name);
417  }
418 }
419 
428 {
429  uint8_t i;
430  enum v4l2_buf_type type;
431 
432  // Check if not already running
433  if (dev->thread != (pthread_t)NULL) {
434  printf("[v4l2] There is already a capturing thread running for %s\n", dev->name);
435  return false;
436  }
437 
438  // Enqueue all buffers
440  for (i = 0; i < dev->buffers_cnt; ++i) {
441  struct v4l2_buffer buf;
442 
443  CLEAR(buf);
444  buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
445  buf.memory = V4L2_MEMORY_MMAP;
446  buf.index = i;
447  if (ioctl(dev->fd, VIDIOC_QBUF, &buf) < 0) {
448  printf("[v4l2] Could not enqueue buffer %d during start capture for %s\n", i, dev->name);
449  return false;
450  }
451  }
452 
453  // Start the stream
454  type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
455  if (ioctl(dev->fd, VIDIOC_STREAMON, &type) < 0) {
456  printf("[v4l2] Could not start stream of %s, %d %s\n", dev->name, errno, strerror(errno));
457  return false;
458  }
459 
460  //Start the capturing thread
461  int rc = pthread_create(&dev->thread, NULL, v4l2_capture_thread, dev);
462  if (rc < 0) {
463  printf("[v4l2] Could not start capturing thread for %s (return code: %d)\n", dev->name, rc);
464 
465  // Stop the stream
466  type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
467  if (ioctl(dev->fd, VIDIOC_STREAMOFF, &type) < 0) {
468  printf("[v4l2] Could not stop stream of %s\n", dev->name);
469  }
470 
471  // Reset the thread
472  dev->thread = (pthread_t) NULL;
473  return false;
474  }
475 
476 #ifndef __APPLE__
477  pthread_setname_np(dev->thread, "v4l2");
478 #endif
479 
480  return true;
481 }
482 
491 {
492  enum v4l2_buf_type type;
493 
494  // First check if still running
495  if (dev->thread == (pthread_t) NULL) {
496  printf("[v4l2] Already stopped capture for %s\n", dev->name);
497  return false;
498  }
499 
500  // Stop the stream
501  type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
502  if (ioctl(dev->fd, VIDIOC_STREAMOFF, &type) < 0) {
503  printf("[v4l2] Could not stop stream of %s\n", dev->name);
504  return false;
505  }
506 
507  // Stop the thread
508  if (pthread_cancel(dev->thread) < 0) {
509  printf("[v4l2] Could not cancel thread for %s\n", dev->name);
510  return false;
511  }
512 
513  // Wait for the thread to be finished
514  pthread_join(dev->thread, NULL);
515  dev->thread = (pthread_t) NULL;
516  return true;
517 }
518 
526 {
527  uint8_t i;
528 
529  // Stop capturing (ignore result as it may already be stopped)
530  v4l2_stop_capture(dev);
531 
532  // Unmap all buffers
533  for (i = 0; i < dev->buffers_cnt; ++i) {
534  if (munmap(dev->buffers[i].buf, dev->buffers[i].length) < 0) {
535  printf("[v4l2] Could not unmap buffer %d for %s\n", i, dev->name);
536  }
537  }
538 
539  // Close the file pointer and free all memory
540  close(dev->fd);
541  free(dev->name);
542  free(dev);
543 }
unsigned short uint16_t
Definition: types.h:16
#define V4L2_IMG_NONE
There currently no image available.
Definition: v4l2.h:38
pthread_mutex_t mutex
Mutex lock for enqueue/dequeue of buffers (change the deq_idx)
Definition: v4l2.h:57
Definition: image.h:80
uint32_t buf_size
The buffer size.
Definition: image.h:52
uint16_t h
The height.
Definition: image.h:76
uint16_t h
height of the cropped area
Definition: image.h:84
void v4l2_image_free(struct v4l2_device *dev, struct image_t *img)
Free the image and enqueue the buffer (Thread safe) This must be done after processing the image...
Definition: v4l2.c:406
void v4l2_image_get(struct v4l2_device *dev, struct image_t *img)
Get the latest image buffer and lock it (Thread safe, BLOCKING) This functions blocks until image acc...
Definition: v4l2.c:332
Capture images from a V4L2 device (Video for Linux 2)
Definition: image.h:43
uint16_t y
Start position y (vertical)
Definition: image.h:82
uint16_t x
Start position x (horizontal)
Definition: image.h:81
uint32_t pprz_ts
The timestamp in us since system startup.
Definition: image.h:49
void v4l2_close(struct v4l2_device *dev)
Close the V4L2 device (Thread safe) This needs to be preformed to clean up all the buffers and close ...
Definition: v4l2.c:525
struct v4l2_device * v4l2_init(char *device_name, struct img_size_t size, struct crop_t crop, uint8_t buffers_cnt, uint32_t _pixelformat)
Initialize a V4L2(Video for Linux 2) device.
Definition: v4l2.c:183
bool v4l2_stop_capture(struct v4l2_device *dev)
Stop capturing of the image stream (Thread safe) This function is blocking until capturing thread is ...
Definition: v4l2.c:490
uint16_t w
Width of the cropped area.
Definition: image.h:83
uint8_t buf_idx
Buffer index for V4L2 freeing.
Definition: image.h:51
uint32_t pprz_timestamp
The time of the image in us since system startup.
Definition: v4l2.h:44
#define TRUE
Definition: std.h:4
Architecture independent timing functions.
uint16_t h
The height of the image.
Definition: v4l2.h:54
struct timeval timestamp
The time value of the image.
Definition: v4l2.h:43
uint16_t w
Image width.
Definition: image.h:45
unsigned long uint32_t
Definition: types.h:18
uint16_t h
Image height.
Definition: image.h:46
struct v4l2_img_buf * buffers
The memory mapped image buffers.
Definition: v4l2.h:58
void * buf
Image buffer (depending on the image_type)
Definition: image.h:53
size_t length
The size of the buffer.
Definition: v4l2.h:42
bool v4l2_image_get_nonblock(struct v4l2_device *dev, struct image_t *img)
Get the latest image and lock it (Thread safe, NON BLOCKING) This function returns NULL if it can't g...
Definition: v4l2.c:371
static void * v4l2_capture_thread(void *data)
The main capturing thread This thread handles the queue and dequeue of buffers, to make sure only the...
Definition: v4l2.c:56
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
void * buf
Pointer to the memory mapped buffer.
Definition: v4l2.h:45
static struct adc_buf * buffers[NB_ADC *2]
First NB_ADC for bank 0, others for bank 2.
Definition: adc_arch.c:51
char * name
The name of the device.
Definition: v4l2.h:50
unsigned char uint8_t
Definition: types.h:14
struct timeval ts
The timestamp of creation.
Definition: image.h:47
bool v4l2_init_subdev(char *subdev_name, uint8_t pad, uint16_t code, struct img_size_t size)
Initialize a V4L2 subdevice.
Definition: v4l2.c:136
int fd
Definition: serial.c:26
bool v4l2_start_capture(struct v4l2_device *dev)
Start capturing images in streaming mode (Thread safe)
Definition: v4l2.c:427
UYVY format (uint16 per pixel)
Definition: image.h:36
uint8_t buffers_cnt
The number of image buffers.
Definition: v4l2.h:55
#define CLEAR(x)
Definition: v4l2.c:45
volatile uint8_t buffers_deq_idx
The current dequeued index.
Definition: v4l2.h:56
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
pthread_t thread
The thread that handles the images.
Definition: v4l2.h:52
uint16_t w
The width.
Definition: image.h:75
uint16_t w
The width of the image.
Definition: v4l2.h:53
enum image_type type
The image type.
Definition: image.h:44
int fd
The file pointer to the device.
Definition: v4l2.h:51