Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 #include "virt2phys.h"
42 
43 #include <sys/time.h>
44 #include "mcu_periph/sys_time.h"
45 
46 #define CLEAR(x) memset(&(x), 0, sizeof (x))
47 static void *v4l2_capture_thread(void *data);
48 
57 static void *v4l2_capture_thread(void *data)
58 {
59  struct v4l2_device *dev = (struct v4l2_device *)data;
60  struct v4l2_buffer buf;
61  struct timeval tv;
62  fd_set fds;
63 
64  while (TRUE) {
65  FD_ZERO(&fds);
66  FD_SET(dev->fd, &fds);
67 
68  // Set the timeout to 2 seconds
69  tv.tv_sec = 2;
70  tv.tv_usec = 0;
71 
72  // Wait until an image was taken, with a timeout of tv
73  int sr = select(dev->fd + 1, &fds, NULL, NULL, &tv);
74  uint32_t now_ts = get_sys_time_usec();
75 
76  if (sr < 0) {
77  // Was interrupted by a signal
78  if (EINTR == errno) { continue; }
79  printf("[v4l2-capture] Select error %d on %s: %s\n", errno, dev->name, strerror(errno));
80  dev->thread = (pthread_t) NULL;
81  return (void *) - 1;
82  } else if (sr == 0) {
83  printf("[v4l2-capture] Select timeout on %s\n", dev->name);
84  continue;
85  //dev->thread = (pthread_t) NULL;
86  //return (void *) - 2;
87  }
88 
89  // Dequeue a buffer
90  CLEAR(buf);
91  buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
92  buf.memory = V4L2_MEMORY_MMAP;
93  if (ioctl(dev->fd, VIDIOC_DQBUF, &buf) < 0) {
94  printf("[v4l2-capture] Dequeue of buffer failed for %s.\n", dev->name);
95  dev->thread = (pthread_t) NULL;
96  return (void *) - 3;
97  }
98  assert(buf.index < dev->buffers_cnt);
99 
100  // Copy the timestamp
101  dev->buffers[buf.index].timestamp = buf.timestamp;
102  dev->buffers[buf.index].pprz_timestamp = now_ts;
103 
104  // Update the dequeued id
105  // We need lock because between setting prev_idx and updating the deq_idx the deq_idx could change
106  pthread_mutex_lock(&dev->mutex);
107  uint16_t prev_idx = dev->buffers_deq_idx;
108  dev->buffers_deq_idx = buf.index;
109  pthread_mutex_unlock(&dev->mutex);
110 
111  // Enqueue the previous image if not empty
112  if (prev_idx != V4L2_IMG_NONE) {
113  // Enqueue the previous buffer
114  CLEAR(buf);
115  buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
116  buf.memory = V4L2_MEMORY_MMAP;
117  buf.index = prev_idx;
118  if (ioctl(dev->fd, VIDIOC_QBUF, &buf) < 0) {
119  printf("[v4l2-capture] Could not enqueue %d for %s\n", prev_idx, dev->name);
120  }
121  }
122 
123  }
124  return (void *)0;
125 }
126 
137 bool v4l2_init_subdev(char *subdev_name, uint8_t pad, uint16_t code, struct img_size_t size)
138 {
139  struct v4l2_subdev_format sfmt;
140  CLEAR(sfmt);
141 
142  // Try to open the subdevice
143  int fd = open(subdev_name, O_RDWR, 0);
144  if (fd < 0) {
145  printf("[v4l2] Cannot open subdevice '%s': %d, %s\n", subdev_name, errno, strerror(errno));
146  return false;
147  }
148 
149  // Try to get the subdevice data format settings
150  if (ioctl(fd, VIDIOC_SUBDEV_G_FMT, &sfmt) < 0) {
151  printf("[v4l2] Could not get subdevice data format settings of %s\n", subdev_name);
152  close(fd);
153  return false;
154  }
155 
156  // Set the new settings
157  sfmt.pad = pad;
158  sfmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
159  sfmt.format.width = size.w;
160  sfmt.format.height = size.h;
161  sfmt.format.code = code;
162  sfmt.format.field = V4L2_FIELD_NONE;
163  sfmt.format.colorspace = 1;
164 
165  if (ioctl(fd, VIDIOC_SUBDEV_S_FMT, &sfmt) < 0) {
166  printf("[v4l2] Could not set subdevice data format settings of %s\n", subdev_name);
167  close(fd);
168  return false;
169  }
170 
171  // Close the device
172  close(fd);
173  return true;
174 }
175 
184 struct v4l2_device *v4l2_init(char *device_name, struct img_size_t size, struct crop_t crop, uint8_t buffers_cnt,
185  uint32_t _pixelformat)
186 {
187  uint8_t i;
188  struct v4l2_capability cap;
189  struct v4l2_format fmt;
190  struct v4l2_requestbuffers req;
191  struct v4l2_fmtdesc fmtdesc;
192  struct v4l2_crop crp;
193  struct physmem pmem;
194  CLEAR(cap);
195  CLEAR(fmt);
196  CLEAR(req);
197  CLEAR(fmtdesc);
198  CLEAR(crp);
199 
200  // Try to open the device
201  int fd = open(device_name, O_RDWR | O_NONBLOCK, 0);
202  if (fd < 0) {
203  printf("[v4l2] Cannot open '%s': %d, %s\n", device_name, errno, strerror(errno));
204  return NULL;
205  }
206 
207  // Try to fetch the capabilities of the V4L2 device
208  if (ioctl(fd, VIDIOC_QUERYCAP, &cap) < 0) {
209  printf("[v4l2] %s is no V4L2 device\n", device_name);
210  close(fd);
211  return NULL;
212  }
213 
214  // Check if the device is capable of capturing and streaming
215  if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
216  printf("[v4l2] %s is no V4L2 video capturing device\n", device_name);
217  close(fd);
218  return NULL;
219  }
220 
221  if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
222  printf("[v4l2] %s isn't capable of streaming (TODO: support reading)\n", device_name);
223  close(fd);
224  return NULL;
225  }
226 
227  fmtdesc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
228  while (ioctl(fd, VIDIOC_ENUM_FMT, &fmtdesc) == 0) {
229  fmtdesc.index++;
230  if(fmtdesc.pixelformat == _pixelformat)
231  break;
232  }
233 
234  // Accept if no format can be get
235  if(fmtdesc.index != 0 && fmtdesc.pixelformat != _pixelformat) {
236  printf("[v4l2] Pixelformat not available on device %s (wanted: %4X)\r\n", device_name, _pixelformat);
237  return NULL;
238  }
239 
240  // Set the cropping window
241  crp.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
242  crp.c.top = crop.y;
243  crp.c.left = crop.x;
244  crp.c.width = crop.w;
245  crp.c.height = crop.h;
246 
247  // Only crop when needed
248  if(crop.x != 0 || crop.y != 0 || crop.w != size.w || crop.h != size.h) {
249  if (ioctl(fd, VIDIOC_S_CROP, &crp) < 0) {
250  printf("[v4l2] Could not set crop window of %s\n", device_name);
251  close(fd);
252  return NULL;
253  }
254  }
255 
256  // Set the format settings
257  fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
258  fmt.fmt.pix.width = size.w;
259  fmt.fmt.pix.height = size.h;
260  fmt.fmt.pix.pixelformat = _pixelformat;
261  fmt.fmt.pix.colorspace = V4L2_COLORSPACE_REC709;
262  fmt.fmt.pix.field = V4L2_FIELD_NONE;
263 
264  if (ioctl(fd, VIDIOC_S_FMT, &fmt) < 0) {
265  printf("[v4l2] Could not set data format settings of %s\n", device_name);
266  close(fd);
267  return NULL;
268  }
269 
270  // Request MMAP buffers
271  req.count = buffers_cnt;
272  req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
273  req.memory = V4L2_MEMORY_MMAP;
274  if (ioctl(fd, VIDIOC_REQBUFS, &req) < 0) {
275  printf("[v4l2] %s Does not support memory mapping\n", device_name);
276  close(fd);
277  return NULL;
278  }
279 
280  // Allocate memory for the memory mapped buffers
281  struct v4l2_img_buf *buffers = calloc(req.count, sizeof(struct v4l2_img_buf));
282  if (buffers == NULL) {
283  printf("[v4l2] Not enough memory for %s to initialize %d MMAP buffers\n", device_name, req.count);
284  close(fd);
285  return NULL;
286  }
287 
288  // Go trough the buffers and initialize them
289  for (i = 0; i < req.count; ++i) {
290  struct v4l2_buffer buf;
291  CLEAR(buf);
292 
293  // Request the buffer information
294  buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
295  buf.memory = V4L2_MEMORY_MMAP;
296  buf.index = i;
297  if (ioctl(fd, VIDIOC_QUERYBUF, &buf) < 0) {
298  printf("[v4l2] Querying buffer %d from %s failed\n", i, device_name);
299  free(buffers);
300  close(fd);
301  return NULL;
302  }
303 
304  // Map the buffer
305  buffers[i].length = buf.length;
306  buffers[i].buf = mmap(NULL, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, fd, buf.m.offset);
307  if (MAP_FAILED == buffers[i].buf) {
308  printf("[v4l2] Mapping buffer %d with length %d from %s failed\n", i, buf.length, device_name);
309  free(buffers);
310  close(fd);
311  return NULL;
312  }
313 
314  if(check_contiguity((unsigned long)buffers[i].buf, getpid(), &pmem, buf.length)) {
315  printf("[v4l2] Physical memory %d is not contiguous with length %d from %s\n", i, buf.length, device_name);
316  free(buffers);
317  close(fd);
318  return NULL;
319  }
320 
321  buffers[i].physp = pmem.paddr;
322  }
323 
324  // Create the device only when everything succeeded
325  struct v4l2_device *dev = (struct v4l2_device *)malloc(sizeof(struct v4l2_device));
326  CLEAR(*dev);
327  dev->name = strdup(device_name); // NOTE: needs to be freed
328  dev->fd = fd;
329  dev->w = size.w;
330  dev->h = size.h;
331  dev->buffers_cnt = req.count;
332  dev->buffers = buffers;
333  return dev;
334 }
335 
344 void v4l2_image_get(struct v4l2_device *dev, struct image_t *img)
345 {
346  uint16_t img_idx = V4L2_IMG_NONE;
347 
348  // Continu to wait for an image
349  while (img_idx == V4L2_IMG_NONE) {
350  // We first check if the deq_idx is ok, this reduces the amount of locks
351  if (dev->buffers_deq_idx != V4L2_IMG_NONE) {
352  pthread_mutex_lock(&dev->mutex);
353 
354  // We need to check it here again, because it could be changed
355  if (dev->buffers_deq_idx != V4L2_IMG_NONE) {
356  img_idx = dev->buffers_deq_idx;
357  dev->buffers_deq_idx = V4L2_IMG_NONE;
358  }
359 
360  pthread_mutex_unlock(&dev->mutex);
361  }
362  }
363 
364  // Set the image
365  img->type = IMAGE_YUV422;
366  img->w = dev->w;
367  img->h = dev->h;
368  img->buf_idx = img_idx;
369  img->buf_size = dev->buffers[img_idx].length;
370  img->buf = dev->buffers[img_idx].buf;
371  img->ts = dev->buffers[img_idx].timestamp;
372  img->pprz_ts = dev->buffers[img_idx].pprz_timestamp;
373 }
374 
383 bool v4l2_image_get_nonblock(struct v4l2_device *dev, struct image_t *img)
384 {
385  uint16_t img_idx = V4L2_IMG_NONE;
386 
387  // Try to get the current image
388  pthread_mutex_lock(&dev->mutex);
389  if (dev->buffers_deq_idx != V4L2_IMG_NONE) {
390  img_idx = dev->buffers_deq_idx;
391  dev->buffers_deq_idx = V4L2_IMG_NONE;
392  }
393  pthread_mutex_unlock(&dev->mutex);
394 
395  // Check if we really got an image
396  if (img_idx == V4L2_IMG_NONE) {
397  return false;
398  }
399 
400  // Set the image
401  img->type = IMAGE_YUV422;
402  img->w = dev->w;
403  img->h = dev->h;
404  img->buf_idx = img_idx;
405  img->buf_size = dev->buffers[img_idx].length;
406  img->buf = dev->buffers[img_idx].buf;
407  img->ts = dev->buffers[img_idx].timestamp;
408  img->pprz_ts = dev->buffers[img_idx].pprz_timestamp;
409  return true;
410 }
411 
418 void v4l2_image_free(struct v4l2_device *dev, struct image_t *img)
419 {
420  struct v4l2_buffer buf;
421 
422  // Enqueue the buffer
423  CLEAR(buf);
424  buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
425  buf.memory = V4L2_MEMORY_MMAP;
426  buf.index = img->buf_idx;
427  if (ioctl(dev->fd, VIDIOC_QBUF, &buf) < 0) {
428  printf("[v4l2] Could not enqueue %d for %s\n", img->buf_idx, dev->name);
429  }
430 }
431 
440 {
441  uint8_t i;
442  enum v4l2_buf_type type;
443 
444  // Check if not already running
445  if (dev->thread != (pthread_t)NULL) {
446  printf("[v4l2] There is already a capturing thread running for %s\n", dev->name);
447  return false;
448  }
449 
450  // Enqueue all buffers
451  dev->buffers_deq_idx = V4L2_IMG_NONE;
452  for (i = 0; i < dev->buffers_cnt; ++i) {
453  struct v4l2_buffer buf;
454 
455  CLEAR(buf);
456  buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
457  buf.memory = V4L2_MEMORY_MMAP;
458  buf.index = i;
459  if (ioctl(dev->fd, VIDIOC_QBUF, &buf) < 0) {
460  printf("[v4l2] Could not enqueue buffer %d during start capture for %s\n", i, dev->name);
461  return false;
462  }
463  }
464 
465  // Start the stream
466  type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
467  if (ioctl(dev->fd, VIDIOC_STREAMON, &type) < 0) {
468  printf("[v4l2] Could not start stream of %s, %d %s\n", dev->name, errno, strerror(errno));
469  return false;
470  }
471 
472  //Start the capturing thread
473  int rc = pthread_create(&dev->thread, NULL, v4l2_capture_thread, dev);
474  if (rc < 0) {
475  printf("[v4l2] Could not start capturing thread for %s (return code: %d)\n", dev->name, rc);
476 
477  // Stop the stream
478  type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
479  if (ioctl(dev->fd, VIDIOC_STREAMOFF, &type) < 0) {
480  printf("[v4l2] Could not stop stream of %s\n", dev->name);
481  }
482 
483  // Reset the thread
484  dev->thread = (pthread_t) NULL;
485  return false;
486  }
487 
488 #ifndef __APPLE__
489  pthread_setname_np(dev->thread, "v4l2");
490 #endif
491 
492  return true;
493 }
494 
503 {
504  enum v4l2_buf_type type;
505 
506  // First check if still running
507  if (dev->thread == (pthread_t) NULL) {
508  printf("[v4l2] Already stopped capture for %s\n", dev->name);
509  return false;
510  }
511 
512  // Stop the stream
513  type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
514  if (ioctl(dev->fd, VIDIOC_STREAMOFF, &type) < 0) {
515  printf("[v4l2] Could not stop stream of %s\n", dev->name);
516  return false;
517  }
518 
519  // Stop the thread
520  if (pthread_cancel(dev->thread) < 0) {
521  printf("[v4l2] Could not cancel thread for %s\n", dev->name);
522  return false;
523  }
524 
525  // Wait for the thread to be finished
526  pthread_join(dev->thread, NULL);
527  dev->thread = (pthread_t) NULL;
528  return true;
529 }
530 
538 {
539  uint8_t i;
540 
541  // Stop capturing (ignore result as it may already be stopped)
543 
544  // Unmap all buffers
545  for (i = 0; i < dev->buffers_cnt; ++i) {
546  if (munmap(dev->buffers[i].buf, dev->buffers[i].length) < 0) {
547  printf("[v4l2] Could not unmap buffer %d for %s\n", i, dev->name);
548  }
549  }
550 
551  // Close the file pointer and free all memory
552  close(dev->fd);
553  free(dev->name);
554  free(dev);
555 }
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:71
uint16_t h
height of the cropped area
Definition: image.h:96
struct timeval ts
The timestamp of creation.
Definition: image.h:48
uint16_t h
The height.
Definition: image.h:88
uint16_t w
Width of the cropped area.
Definition: image.h:95
uint32_t buf_size
The buffer size.
Definition: image.h:53
void * buf
Image buffer (depending on the image_type)
Definition: image.h:54
uint32_t pprz_ts
The timestamp in us since system startup.
Definition: image.h:50
uint16_t x
Start position x (horizontal)
Definition: image.h:93
uint16_t y
Start position y (vertical)
Definition: image.h:94
uint16_t h
Image height.
Definition: image.h:47
uint8_t buf_idx
Buffer index for V4L2 freeing.
Definition: image.h:52
uint16_t w
Image width.
Definition: image.h:46
enum image_type type
The image type.
Definition: image.h:45
@ IMAGE_YUV422
UYVY format (uint16 per pixel)
Definition: image.h:36
uint16_t w
The width.
Definition: image.h:87
Definition: image.h:92
Definition: image.h:44
int fd
Definition: serial.c:26
#define TRUE
Definition: std.h:4
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
Architecture independent timing functions.
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:184
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:57
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:137
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:537
bool v4l2_start_capture(struct v4l2_device *dev)
Start capturing images in streaming mode (Thread safe)
Definition: v4l2.c:439
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:344
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:418
#define CLEAR(x)
Definition: v4l2.c:46
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:502
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:383
Capture images from a V4L2 device (Video for Linux 2)
void * buf
Pointer to the memory mapped buffer.
Definition: v4l2.h:45
struct v4l2_img_buf * buffers
The memory mapped image buffers.
Definition: v4l2.h:59
uint8_t buffers_cnt
The number of image buffers.
Definition: v4l2.h:56
#define V4L2_IMG_NONE
There currently no image available.
Definition: v4l2.h:38
uint32_t physp
Physical address pointer.
Definition: v4l2.h:46
size_t length
The size of the buffer.
Definition: v4l2.h:42
int check_contiguity(unsigned long vaddr, pid_t pid, struct physmem *pmem, size_t size)
Definition: virt2phys.c:111
unsigned long paddr
Definition: virt2phys.h:8
size_t size
Definition: virt2phys.h:9
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98