Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
video_thread.c
Go to the documentation of this file.
1 /*
2 * Copyright (C) 2015 The Paparazzi Team
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 #include <stdlib.h>
30 #include <stdio.h>
31 #include <errno.h>
32 #include <string.h>
33 #include <unistd.h>
34 #include <math.h>
35 
36 // Video
37 #include "lib/v4l/v4l2.h"
38 #include "lib/vision/image.h"
39 #include "lib/vision/bayer.h"
40 
41 #include "mcu_periph/sys_time.h"
42 
43 // include board for bottom_camera and front_camera on ARDrone2, Bebop and Disco
44 #include BOARD_CONFIG
45 
46 // Bebop and Disco can use the ISP (Image Signal Processors) to speed up things
47 #if defined(BOARD_BEBOP) || defined(BOARD_DISCO)
49 #endif
50 
51 // Threaded computer vision
52 #include <pthread.h>
53 #include "rt_priority.h"
54 
55 // Frames Per Seconds
56 #ifndef VIDEO_THREAD_NICE_LEVEL
57 #define VIDEO_THREAD_NICE_LEVEL 5
58 #endif
59 PRINT_CONFIG_VAR(VIDEO_THREAD_NICE_LEVEL)
60 
61 // The amount of cameras we can have
62 #ifndef VIDEO_THREAD_MAX_CAMERAS
63 #define VIDEO_THREAD_MAX_CAMERAS 4
64 #endif
65 PRINT_CONFIG_VAR(VIDEO_THREAD_MAX_CAMERAS)
66 
67 #ifndef VIDEO_THREAD_VERBOSE
68 #define VIDEO_THREAD_VERBOSE 0
69 #endif
70 
71 #define printf_debug if(VIDEO_THREAD_VERBOSE > 0) printf
72 
74 
75 // Main thread
76 static void *video_thread_function(void *data);
77 static bool initialize_camera(struct video_config_t *camera);
78 static void start_video_thread(struct video_config_t *camera);
79 static void stop_video_thread(struct video_config_t *device);;
80 
82 {
83  /* currently no direct periodic functionality */
84 }
85 
90 static void *video_thread_function(void *data)
91 {
92  struct video_config_t *vid = (struct video_config_t *)data;
93 
94  char print_tag[80];
95  snprintf(print_tag, 80, "video_thread-%s", vid->dev_name);
96 
97  struct image_t img_color;
98 
99  // create the images
100  if (vid->filters & VIDEO_FILTER_DEBAYER) {
101  // fixme: don't hardcode size, works for Bebop front camera for now
102 #define IMG_FLT_SIZE 272
104  }
105 
106  // Start the streaming of the V4L2 device
107  if (!v4l2_start_capture(vid->thread.dev)) {
108  fprintf(stderr, "[%s] Could not start capture.\n", print_tag);
109  return 0;
110  }
111 
112 #if defined(BOARD_BEBOP) || defined(BOARD_DISCO)
113  // Configure ISP if needed
114  if (vid->filters & VIDEO_FILTER_ISP) {
115  configure_isp(vid->thread.dev);
116  }
117 #endif
118 
119  // Be nice to the more important stuff
121  fprintf(stdout, "[%s] Set nice level to %i.\n", print_tag, VIDEO_THREAD_NICE_LEVEL);
122 
123  // Initialize timing
124  uint32_t time_begin = get_sys_time_usec();
125  uint32_t frame_dt_us, computation_dt_us;
126 
127  // Start streaming
128  vid->thread.is_running = true;
129  struct image_t img;
130  while (vid->thread.is_running) {
131  // Wait for a new frame (blocking)
132  v4l2_image_get(vid->thread.dev, &img);
133  frame_dt_us = get_sys_time_usec() - time_begin;
134 
135  // Get computation/frame start time
136  time_begin = get_sys_time_usec();
137 
138  // Pointer to the final image to pass for saving and further processing
139  struct image_t *img_final = &img;
140 
141  // Run selected filters
142  if (vid->filters & VIDEO_FILTER_DEBAYER) {
143  BayerToYUV(&img, &img_color, 0, 0);
144  // use color image for further processing
145  img_final = &img_color;
146  }
147 
148  // Run processing if required
149  cv_run_device(vid, img_final);
150 
151  // Free the image
152  v4l2_image_free(vid->thread.dev, &img);
153 
154  // sleep (most of the) remaining time to limit to specified fps
155  if (vid->fps > 0) {
156  uint32_t fps_period_us = 1000000 / vid->fps;
157  if (frame_dt_us > fps_period_us + 10000) {
158  fprintf(stderr, "[%s] desired %i fps, only managing %.1f fps\n", print_tag, vid->fps, 1000000.f / frame_dt_us);
159  }
160  computation_dt_us = get_sys_time_usec() - time_begin;
161  if (computation_dt_us + 1000 < fps_period_us) {
162  sys_time_usleep(fps_period_us - computation_dt_us - 1000);
163  }
164  }
165  }
166 
167  image_free(&img_color);
168 
169  return 0;
170 }
171 
172 static bool initialize_camera(struct video_config_t *camera)
173 {
174  // Initialize the V4L2 subdevice if needed
175  if (camera->subdev_name != NULL) {
176  // FIXME! add subdev format to config, only needed on Bebop/Disco(?) front camera so far
177  if (!v4l2_init_subdev(camera->subdev_name, 0, camera->subdev_format, camera->sensor_size)) {
178  fprintf(stderr, "[video_thread] Could not initialize the %s subdevice.\n", camera->subdev_name);
179  return false;
180  }
181  }
182 
183  // Initialize the V4L2 device
184  camera->thread.dev = v4l2_init(camera->dev_name, camera->output_size, camera->crop, camera->buf_cnt, camera->format);
185  if (camera->thread.dev == NULL) {
186  fprintf(stderr, "[video_thread] Could not initialize the %s V4L2 device.\n", camera->dev_name);
187  return false;
188  }
189 
190  // Initialized just fine
191  return true;
192 }
193 
194 /*
195  * Add a new video device to the list
196  */
197 bool add_video_device(struct video_config_t *device)
198 {
199  // Loop over camera array
200  for (int i = 0; i < VIDEO_THREAD_MAX_CAMERAS; ++i) {
201  // If device is already registered, break
202  if (cameras[i] == device) {
203  break;
204  }
205 
206  // If camera slot is already used, continue
207  if (cameras[i] != NULL) {
208  continue;
209  }
210 
211  // Initialize the camera
212  if (!initialize_camera(device)) {
213  return false;
214  }
215 
216  // Store device pointer
217  cameras[i] = device;
218 
219  // Debug statement
220  printf_debug("[video_thread] Added %s to camera array.\n", device->dev_name);
221 
222  // Successfully initialized
223  return true;
224  }
225 
226  // Camera array is full
227  return false;
228 }
229 
230 /*
231  * Start a new video thread for a camera
232  */
233 static void start_video_thread(struct video_config_t *camera)
234 {
235  if (!camera->thread.is_running) {
236  // Start the streaming thread for a camera
237  pthread_t tid;
238  if (pthread_create(&tid, NULL, video_thread_function, (void *)(camera)) != 0) {
239  fprintf(stderr, "[viewvideo] Could not create streaming thread for camera %s: Reason: %d.\n", camera->dev_name, errno);
240  return;
241  }
242 #ifndef __APPLE__
243  pthread_setname_np(tid, "camera");
244 #endif
245  }
246 }
247 
248 /*
249  * Stop a video thread for a camera
250  */
251 static void stop_video_thread(struct video_config_t *device)
252 {
253  if (device->thread.is_running) {
254  // Stop the streaming thread
255  device->thread.is_running = false;
256 
257  // Stop the capturing
258  if (!v4l2_stop_capture(device->thread.dev)) {
259  fprintf(stderr, "[video_thread] Could not stop capture of %s.\n", device->thread.dev->name);
260  return;
261  }
262  }
263 
264 }
265 
270 {
271  // Initialise all camera pointers to be NULL
272  for (int indexCameras = 0; indexCameras < VIDEO_THREAD_MAX_CAMERAS; indexCameras++) {
273  cameras[indexCameras] = NULL;
274  }
275 }
276 
281 {
282  // Start every known camera device
283  for (int indexCameras = 0; indexCameras < VIDEO_THREAD_MAX_CAMERAS; indexCameras++) {
284  if (cameras[indexCameras] != NULL) {
285  start_video_thread(cameras[indexCameras]);
286  }
287  }
288 }
289 
295 {
296  for (int indexCameras = 0; indexCameras < VIDEO_THREAD_MAX_CAMERAS; indexCameras++) {
297  if (cameras[indexCameras] != NULL) {
298  stop_video_thread(cameras[indexCameras]);
299  }
300  }
301 
302  // TODO: wait for the thread to finish to be able to start the thread again!
303 }
void BayerToYUV(struct image_t *in, struct image_t *out, int RedX, int RedY)
Decode Bayer Pattern.
Definition: bayer.h:39
void sys_time_usleep(uint32_t us)
sys_time_usleep(uint32_t us)
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:71
void cv_run_device(struct video_config_t *device, struct image_t *img)
Definition: cv.c:178
void image_free(struct image_t *img)
Free the image.
Definition: image.c:75
void image_create(struct image_t *img, uint16_t width, uint16_t height, enum image_type type)
Create a new image.
Definition: image.c:43
Image helper functions like resizing, color filter, converters...
@ IMAGE_YUV422
UYVY format (uint16 per pixel)
Definition: image.h:36
Definition: image.h:44
int configure_isp(struct v4l2_device *dev)
Definition: libisp.c:177
Functions to obtain rt priority or set the nice level.
static int set_nice_level(int level)
Definition: rt_priority.h:69
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
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
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
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
Capture images from a V4L2 device (Video for Linux 2)
char * name
The name of the device.
Definition: v4l2.h:51
struct video_thread_t thread
Information about the thread this camera is running on.
Definition: video_device.h:65
#define VIDEO_FILTER_DEBAYER
Enable software debayer.
Definition: video_device.h:34
char * subdev_name
path to sub device
Definition: video_device.h:60
#define VIDEO_FILTER_ISP
Enable ISP.
Definition: video_device.h:35
uint8_t filters
filters to use (bitfield with VIDEO_FILTER_x)
Definition: video_device.h:64
uint32_t format
Video format.
Definition: video_device.h:61
volatile bool is_running
When the device is running.
Definition: video_device.h:39
uint32_t subdev_format
Subdevice video format.
Definition: video_device.h:62
int fps
Target FPS.
Definition: video_device.h:67
struct crop_t crop
Cropped area definition.
Definition: video_device.h:58
uint8_t buf_cnt
Amount of V4L2 video device buffers.
Definition: video_device.h:63
struct img_size_t output_size
Output image size.
Definition: video_device.h:56
char * dev_name
path to device
Definition: video_device.h:59
struct v4l2_device * dev
The V4L2 device that is used for the video stream.
Definition: video_device.h:40
struct img_size_t sensor_size
Original sensor size.
Definition: video_device.h:57
V4L2 device settings.
Definition: video_device.h:55
void video_thread_start()
Starts the streaming of a all cameras.
Definition: video_thread.c:280
void video_thread_init(void)
Initialize the view video.
Definition: video_thread.c:269
#define VIDEO_THREAD_MAX_CAMERAS
Definition: video_thread.c:63
static void stop_video_thread(struct video_config_t *device)
Definition: video_thread.c:251
static struct video_config_t * cameras[VIDEO_THREAD_MAX_CAMERAS]
Definition: video_thread.c:73
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:294
static void * video_thread_function(void *data)
Handles all the video streaming and saving of the image shots This is a separate thread,...
Definition: video_thread.c:90
bool add_video_device(struct video_config_t *device)
Definition: video_thread.c:197
#define IMG_FLT_SIZE
static void start_video_thread(struct video_config_t *camera)
Definition: video_thread.c:233
#define VIDEO_THREAD_NICE_LEVEL
Definition: video_thread.c:57
static bool initialize_camera(struct video_config_t *camera)
Definition: video_thread.c:172
void video_thread_periodic(void)
A dummy for now.
Definition: video_thread.c:81
#define printf_debug
Definition: video_thread.c:71
Start a Video thread and grab images.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78