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
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 thing
47 #if defined(BOARD_BEBOP) || defined(BOARD_DISCO)
48 #include "lib/isp/libisp.h"
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 
68 
69 // Main thread
70 static void *video_thread_function(void *data);
71 static bool initialize_camera(struct video_config_t *camera);
72 static void start_video_thread(struct video_config_t *camera);
73 static void stop_video_thread(struct video_config_t *device);;
74 
76 {
77  /* currently no direct periodic functionality */
78 }
79 
84 static void *video_thread_function(void *data)
85 {
86  struct video_config_t *vid = (struct video_config_t *)data;
87 
88  char print_tag[80];
89  snprintf(print_tag, 80, "video_thread-%s", vid->dev_name);
90 
91  struct image_t img_color;
92 
93  // create the images
94  if (vid->filters & VIDEO_FILTER_DEBAYER) {
95  // fixme: don't hardcode size, works for Bebop front camera for now
96 #define IMG_FLT_SIZE 272
98  }
99 
100  // Start the streaming of the V4L2 device
101  if (!v4l2_start_capture(vid->thread.dev)) {
102  fprintf(stderr, "[%s] Could not start capture.\n", print_tag);
103  return 0;
104  }
105 
106 #if defined(BOARD_BEBOP) || defined(BOARD_DISCO)
107  // Configure ISP if needed
108  if (vid->filters & VIDEO_FILTER_ISP) {
109  configure_isp(vid->thread.dev);
110  }
111 #endif
112 
113  // Be nice to the more important stuff
115  fprintf(stdout, "[%s] Set nice level to %i.\n", print_tag, VIDEO_THREAD_NICE_LEVEL);
116 
117  // Initialize timing
118  uint32_t time_begin;
119 
120  // Start streaming
121  vid->thread.is_running = true;
122  while (vid->thread.is_running) {
123  // Get start time
124  time_begin = get_sys_time_usec();
125 
126  // Wait for a new frame (blocking)
127  struct image_t img;
128  v4l2_image_get(vid->thread.dev, &img);
129 
130  // Pointer to the final image to pass for saving and further processing
131  struct image_t *img_final = &img;
132 
133  // Run selected filters
134  if (vid->filters & VIDEO_FILTER_DEBAYER) {
135  BayerToYUV(&img, &img_color, 0, 0);
136  // use color image for further processing
137  img_final = &img_color;
138  }
139 
140  // Run processing if required
141  cv_run_device(vid, img_final);
142 
143  // Free the image
144  v4l2_image_free(vid->thread.dev, &img);
145 
146  uint32_t dt_us = get_sys_time_usec() - time_begin;
147 
148  // sleep remaining time to limit to specified fps
149  if (vid->fps != 0) {
150  uint32_t fps_period_us = 1000000 / vid->fps;
151  if (dt_us < fps_period_us) {
152  sys_time_usleep(fps_period_us - dt_us);
153  } else {
154  fprintf(stderr, "[%s] desired %i fps, only managing %.1f fps\n", print_tag, vid->fps, 1000000.f / dt_us);
155  }
156  }
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/Disco(?) 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  // Initialized just fine
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 #ifndef __APPLE__
235  pthread_setname_np(tid, "camera");
236 #endif
237  }
238 }
239 
240 /*
241  * Stop a video thread for a camera
242  */
243 static void stop_video_thread(struct video_config_t *device)
244 {
245  if (device->thread.is_running) {
246  // Stop the streaming thread
247  device->thread.is_running = false;
248 
249  // Stop the capturing
250  if (!v4l2_stop_capture(device->thread.dev)) {
251  printf("[video_thread] Could not stop capture of %s.\n", device->thread.dev->name);
252  return;
253  }
254  }
255 
256 }
257 
262 {
263 }
264 
269 {
270  // Start every known camera device
271  for (int indexCameras = 0; indexCameras < VIDEO_THREAD_MAX_CAMERAS; indexCameras++) {
272  if (cameras[indexCameras] != NULL) {
273  start_video_thread(cameras[indexCameras]);
274  }
275  }
276 }
277 
283 {
284  for (int indexCameras = 0; indexCameras < VIDEO_THREAD_MAX_CAMERAS; indexCameras++) {
285  if (cameras[indexCameras] != NULL) {
286  stop_video_thread(cameras[indexCameras]);
287  }
288  }
289 
290  // TODO: wait for the thread to finish to be able to start the thread again!
291 }
uint32_t format
Video format.
Definition: video_device.h:61
struct crop_t crop
Cropped area definition.
Definition: video_device.h:58
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:56
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:64
Capture images from a V4L2 device (Video for Linux 2)
static struct video_config_t * cameras[VIDEO_THREAD_MAX_CAMERAS]
Definition: video_thread.c:67
void image_create(struct image_t *img, uint16_t width, uint16_t height, enum image_type type)
Create a new image.
Definition: image.c:39
Start a Video thread and grab images.
Definition: image.h:43
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:282
char * dev_name
path to device
Definition: video_device.h:59
uint8_t filters
filters to use (bitfield with VIDEO_FILTER_x)
Definition: video_device.h:64
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:63
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
void sys_time_usleep(uint32_t us)
sys_time_usleep(uint32_t us)
Definition: sys_time_arch.c:95
Image helper functions like resizing, color filter, converters...
int configure_isp(struct v4l2_device *dev)
Definition: libisp.c:163
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:57
void video_thread_init(void)
Initialize the view video.
Definition: video_thread.c:261
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:60
#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 separate thread, so it needs to be thread safe!
Definition: video_thread.c:84
static bool initialize_camera(struct video_config_t *camera)
Definition: video_thread.c:164
int fps
Target FPS.
Definition: video_device.h:67
void video_thread_start()
Starts the streaming of a all cameras.
Definition: video_thread.c:268
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:177
#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:36
static void stop_video_thread(struct video_config_t *device)
Definition: video_thread.c:243
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:63
uint32_t subdev_format
Subdevice video format.
Definition: video_device.h:62
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
bool add_video_device(struct video_config_t *device)
Definition: video_thread.c:189
#define VIDEO_THREAD_NICE_LEVEL
Definition: video_thread.c:57
static int set_nice_level(int level)
Definition: rt_priority.h:69
V4L2 device settings.
Definition: video_device.h:55
struct video_thread_t thread
Information about the thread this camera is running on.
Definition: video_device.h:65
void video_thread_periodic(void)
A dummy for now.
Definition: video_thread.c:75