Paparazzi UAS  v5.10_stable-5-g83a0da5-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
video_thread.c
Go to the documentation of this file.
1 /*
2 * Copyright (C) 2015
3 *
4 * This file is part of Paparazzi.
5 *
6 * Paparazzi is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2, or (at your option)
9 * any later version.
10 *
11 * Paparazzi is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 * GNU General Public License for more details.
15 *
16 * You should have received a copy of the GNU General Public License
17 * along with paparazzi; see the file COPYING. If not, see
18 * <http://www.gnu.org/licenses/>.
19 *
20 */
21 
26 // Own header
28 
29 
30 #include <stdlib.h>
31 #include <stdio.h>
32 #include <errno.h>
33 #include <string.h>
34 #include <unistd.h>
35 #include <sys/time.h>
36 #include <math.h>
37 
38 // Video
39 #include "lib/v4l/v4l2.h"
40 #include "lib/vision/image.h"
41 #include "lib/vision/bayer.h"
42 #include "lib/isp/libisp.h"
43 
44 #include "mcu_periph/sys_time.h"
45 
46 // include board for bottom_camera and front_camera on ARDrone2 and Bebop
47 #include BOARD_CONFIG
48 
49 // Threaded computer vision
50 #include <pthread.h>
51 #include "rt_priority.h"
52 
53 // Frames Per Seconds
54 #ifndef VIDEO_THREAD_NICE_LEVEL
55 #define VIDEO_THREAD_NICE_LEVEL 5
56 #endif
57 PRINT_CONFIG_VAR(VIDEO_THREAD_NICE_LEVEL)
58 
59 // The amount of cameras we can have
60 #ifndef VIDEO_THREAD_MAX_CAMERAS
61 #define VIDEO_THREAD_MAX_CAMERAS 4
62 #endif
63 PRINT_CONFIG_VAR(VIDEO_THREAD_MAX_CAMERAS)
64 
66 
67 // Main thread
68 static void *video_thread_function(void *data);
69 static bool initialize_camera(struct video_config_t *camera);
70 static void start_video_thread(struct video_config_t *camera);
71 static void stop_video_thread(struct video_config_t *device);;
72 
74 {
75  /* currently no direct periodic functionality */
76 }
77 
78 
83 static void *video_thread_function(void *data)
84 {
85  struct video_config_t *vid = (struct video_config_t *)data;
86 
87  char print_tag[80];
88  snprintf(print_tag, 80, "video_thread-%s", vid->dev_name);
89 
90  struct image_t img_color;
91 
92  // create the images
93  if (vid->filters & VIDEO_FILTER_DEBAYER) {
94  // fixme: don't hardcode size, works for bebop front camera for now
95 #define IMG_FLT_SIZE 272
97  }
98 
99  // Start the streaming of the V4L2 device
100  if (!v4l2_start_capture(vid->thread.dev)) {
101  fprintf(stderr, "[%s] Could not start capture.\n", print_tag);
102  return 0;
103  }
104 
105  // Configure ISP if needed
106  if (vid->filters & VIDEO_FILTER_ISP) {
107  configure_isp(vid->thread.dev);
108  }
109 
110  // be nice to the more important stuff
112  fprintf(stdout, "[%s] Set nice level to %i.\n", print_tag, VIDEO_THREAD_NICE_LEVEL);
113 
114  // Initialize timing
115  struct timespec time_now;
116  struct timespec time_prev;
117  clock_gettime(CLOCK_MONOTONIC, &time_prev);
118 
119  // Start streaming
120  vid->thread.is_running = true;
121  while (vid->thread.is_running) {
122 
123  // get time in us since last run
124  clock_gettime(CLOCK_MONOTONIC, &time_now);
125  uint32_t dt_us = sys_time_elapsed_us(&time_prev, &time_now);
126  time_prev = time_now;
127 
128  // sleep remaining time to limit to specified fps
129  if (vid->fps != 0) {
130  uint32_t fps_period_us = 1000000 / vid->fps;
131  if (dt_us < fps_period_us) {
132  usleep(fps_period_us - dt_us);
133  } else {
134  fprintf(stderr, "[%s] desired %i fps, only managing %.1f fps\n", print_tag, vid->fps, 1000000.f / dt_us);
135  }
136  }
137 
138  // Wait for a new frame (blocking)
139  struct image_t img;
140  v4l2_image_get(vid->thread.dev, &img);
141 
142  // pointer to the final image to pass for saving and further processing
143  struct image_t *img_final = &img;
144 
145  // run selected filters
146  if (vid->filters & VIDEO_FILTER_DEBAYER) {
147  BayerToYUV(&img, &img_color, 0, 0);
148  // use color image for further processing
149  img_final = &img_color;
150  }
151 
152  // Run processing if required
153  cv_run_device(vid, img_final);
154 
155  // Free the image
156  v4l2_image_free(vid->thread.dev, &img);
157  }
158 
159  image_free(&img_color);
160 
161  return 0;
162 }
163 
164 static bool initialize_camera(struct video_config_t *camera)
165 {
166  // Initialize the V4L2 subdevice if needed
167  if (camera->subdev_name != NULL) {
168  // FIXME! add subdev format to config, only needed on bebop front camera so far
169  if (!v4l2_init_subdev(camera->subdev_name, 0, camera->subdev_format, camera->sensor_size)) {
170  printf("[video_thread] Could not initialize the %s subdevice.\n", camera->subdev_name);
171  return false;
172  }
173  }
174 
175  // Initialize the V4L2 device
176  camera->thread.dev = v4l2_init(camera->dev_name, camera->output_size, camera->crop, camera->buf_cnt, camera->format);
177  if (camera->thread.dev == NULL) {
178  printf("[video_thread] Could not initialize the %s V4L2 device.\n", camera->dev_name);
179  return false;
180  }
181 
182  // Initialize OK
183  return true;
184 }
185 
186 /*
187  * Add a new video device to the list
188  */
189 bool add_video_device(struct video_config_t *device)
190 {
191  // Loop over camera array
192  for (int i = 0; i < VIDEO_THREAD_MAX_CAMERAS; ++i) {
193  // If device is already registered, break
194  if (cameras[i] == device) {
195  break;
196  }
197 
198  // If camera slot is already used, continue
199  if (cameras[i] != NULL) {
200  continue;
201  }
202 
203  // Initialize the camera
204  if (!initialize_camera(device)) {
205  return false;
206  }
207 
208  // Store device pointer
209  cameras[i] = device;
210 
211  // Debug statement
212  printf("[video_thread] Added %s to camera array.\n", device->dev_name);
213 
214  // Successfully initialized
215  return true;
216  }
217 
218  // Camera array is full
219  return false;
220 }
221 
222 /*
223  * Start a new video thread for a camera
224  */
225 static void start_video_thread(struct video_config_t *camera)
226 {
227  if (!camera->thread.is_running) {
228  // Start the streaming thread for a camera
229  pthread_t tid;
230  if (pthread_create(&tid, NULL, video_thread_function, (void *)(camera)) != 0) {
231  printf("[viewvideo] Could not create streaming thread for camera %s: Reason: %d.\n", camera->dev_name, errno);
232  return;
233  }
234  }
235 }
236 
237 /*
238  * Stop a video thread for a camera
239  */
240 static void stop_video_thread(struct video_config_t *device)
241 {
242  if (device->thread.is_running) {
243  // Stop the streaming thread
244  device->thread.is_running = false;
245 
246  // Stop the capturing
247  if (!v4l2_stop_capture(device->thread.dev)) {
248  printf("[video_thread] Could not stop capture of %s.\n", device->thread.dev->name);
249  return;
250  }
251  }
252 
253 }
254 
259 {
260  // Initialise all camera pointers to be NULL
261  for (int indexCameras = 0; indexCameras < VIDEO_THREAD_MAX_CAMERAS; indexCameras++) {
262  cameras[indexCameras] = NULL;
263  }
264 }
265 
270 {
271  // Start every known camera device
272  for (int indexCameras = 0; indexCameras < VIDEO_THREAD_MAX_CAMERAS; indexCameras++) {
273  if (cameras[indexCameras] != NULL) {
274  start_video_thread(cameras[indexCameras]);
275  }
276  }
277 }
278 
279 
285 {
286 
287  for (int indexCameras = 0; indexCameras < VIDEO_THREAD_MAX_CAMERAS; indexCameras++) {
288  if (cameras[indexCameras] != NULL) {
289  stop_video_thread(cameras[indexCameras]);
290  }
291  }
292 
293 
294  // TODO: wait for the thread to finish to be able to start the thread again!
295 }
uint32_t format
Video format.
Definition: video_device.h:50
struct crop_t crop
Cropped area definition.
Definition: video_device.h:47
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
struct img_size_t output_size
Output image size.
Definition: video_device.h:45
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
void image_free(struct image_t *img)
Free the image.
Definition: image.c:63
Capture images from a V4L2 device (Video for Linux 2)
struct video_config_t * cameras[VIDEO_THREAD_MAX_CAMERAS]
Definition: video_thread.c:65
void image_create(struct image_t *img, uint16_t width, uint16_t height, enum image_type type)
Create a new image.
Definition: image.c:38
Start a Video thread and grab images.
Definition: image.h:42
void video_thread_stop()
Stops the streaming of all cameras This could take some time, because the thread is stopped asynchron...
Definition: video_thread.c:284
char * dev_name
path to device
Definition: video_device.h:48
uint8_t filters
filters to use (bitfield with VIDEO_FILTER_x)
Definition: video_device.h:53
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
Functions to obtain rt priority or set the nice level.
uint8_t buf_cnt
Amount of V4L2 video device buffers.
Definition: video_device.h:52
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:486
Image helper functions like resizing, color filter, converters...
int configure_isp(struct v4l2_device *dev)
Definition: libisp.c:154
static void start_video_thread(struct video_config_t *camera)
Definition: video_thread.c:225
Architecture independent timing functions.
struct img_size_t sensor_size
Original sensor size.
Definition: video_device.h:46
void video_thread_init(void)
Initialize the view video.
Definition: video_thread.c:258
void BayerToYUV(struct image_t *Input, struct image_t *out, int RedX, int RedY)
Decode Bayer Pattern.
Definition: bayer.h:55
unsigned long uint32_t
Definition: types.h:18
char * subdev_name
path to sub device
Definition: video_device.h:49
static unsigned int sys_time_elapsed_us(struct timespec *prev, struct timespec *now)
elapsed time in microsecs between two timespecs
Definition: sys_time_arch.h:61
#define VIDEO_FILTER_DEBAYER
Enable software debayer.
Definition: video_device.h:34
static void * video_thread_function(void *data)
Handles all the video streaming and saving of the image shots This is a sepereate thread...
Definition: video_thread.c:83
static bool initialize_camera(struct video_config_t *camera)
Definition: video_thread.c:164
int fps
Target FPS.
Definition: video_device.h:56
void video_thread_start()
Starts the streaming of a all cameras.
Definition: video_thread.c:269
char * name
The name of the device.
Definition: v4l2.h:50
void cv_run_device(struct video_config_t *device, struct image_t *img)
Definition: cv.c:155
#define VIDEO_FILTER_ISP
Enable ISP.
Definition: video_device.h:35
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
volatile bool is_running
When the device is running.
Definition: video_device.h:39
bool v4l2_start_capture(struct v4l2_device *dev)
Start capturing images in streaming mode (Thread safe)
Definition: v4l2.c:427
#define IMG_FLT_SIZE
UYVY format (uint16 per pixel)
Definition: image.h:35
static void stop_video_thread(struct video_config_t *device)
Definition: video_thread.c:240
struct v4l2_device * dev
The V4L2 device that is used for the video stream.
Definition: video_device.h:40
#define VIDEO_THREAD_MAX_CAMERAS
Definition: video_thread.c:61
uint32_t subdev_format
Subdevice video format.
Definition: video_device.h:51
bool add_video_device(struct video_config_t *device)
Definition: video_thread.c:189
#define VIDEO_THREAD_NICE_LEVEL
Definition: video_thread.c:55
static int set_nice_level(int level)
Definition: rt_priority.h:69
V4L2 device settings.
Definition: video_device.h:44
struct video_thread_t thread
Information about the thread this camera is running on.
Definition: video_device.h:54
void video_thread_periodic(void)
A dummy for now.
Definition: video_thread.c:73