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
cv.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 
28 #include <stdlib.h> // for malloc
29 #include <stdio.h>
30 
31 #include "cv.h"
32 #include "rt_priority.h"
33 
34 
35 void cv_attach_listener(struct video_config_t *device, struct video_listener *new_listener);
36 int8_t cv_async_function(struct cv_async *async, struct image_t *img);
37 void *cv_async_thread(void *args);
38 
39 
40 static inline uint32_t timeval_diff(struct timeval *A, struct timeval *B)
41 {
42  return (B->tv_sec - A->tv_sec) * 1000000 + (B->tv_usec - A->tv_usec);
43 }
44 
45 
47 {
48  // Create a new video listener
49  struct video_listener *new_listener = malloc(sizeof(struct video_listener));
50 
51  // Assign function to listener
52  new_listener->active = true;
53  new_listener->func = func;
54  new_listener->next = NULL;
55  new_listener->async = NULL;
56  new_listener->maximum_fps = fps;
57 
58  // Initialise the device that we want our function to use
59  add_video_device(device);
60 
61  // Check if device already has a listener
62  if (device->cv_listener == NULL) {
63  // Add as first listener
64  device->cv_listener = new_listener;
65  } else {
66  // Create pointer to first listener
67  struct video_listener *listener = device->cv_listener;
68 
69  // Loop through linked list to last listener
70  while (listener->next != NULL) {
71  listener = listener->next;
72  }
73 
74  // Add listener to end
75  listener->next = new_listener;
76  }
77 
78  return new_listener;
79 }
80 
81 
82 struct video_listener *cv_add_to_device_async(struct video_config_t *device, cv_function func, int nice_level, uint16_t fps)
83 {
84  // Create a normal listener
85  struct video_listener *listener = cv_add_to_device(device, func, fps);
86 
87  // Add asynchronous structure to override default synchronous behavior
88  listener->async = malloc(sizeof(struct cv_async));
89  listener->async->thread_priority = nice_level;
90 
91  // Explicitly mark img_copy as uninitialized
92  listener->async->img_copy.buf = NULL;
93  listener->async->img_copy.buf_size = 0;
94 
95  // Initialize mutex and condition variable
96  pthread_mutex_init(&listener->async->img_mutex, NULL);
97  pthread_cond_init(&listener->async->img_available, NULL);
98 
99  // Create new processing thread
100  pthread_create(&listener->async->thread_id, NULL, cv_async_thread, listener);
101 
102 #ifndef __APPLE__
103  pthread_setname_np(listener->async->thread_id, "cv");
104 #endif
105 
106  return listener;
107 }
108 
109 
111 {
112  // If the previous image is not yet processed, return
113  if (!async->img_processed || pthread_mutex_trylock(&async->img_mutex) != 0) {
114  return -1;
115  }
116 
117  // update image copy if input image size changed or not yet initialised
118  if (async->img_copy.buf_size != img->buf_size) {
119  if(async->img_copy.buf != NULL){
120  image_free(&async->img_copy);
121  }
122  image_create(&async->img_copy, img->w, img->h, img->type);
123  }
124 #if CV_ALLOW_VIDEO_TO_CHANGE_SIZE
125  // Note: must be enabled explicitly as not all modules may support this. (See issue #2187)
126  if (img->buf_size > async->img_copy.buf_size) {
127  image_free(&async->img_copy);
128  image_create(&async->img_copy, img->w, img->h, img->type);
129  }
130 #endif
131 
132  // Copy image
133  // TODO:this takes time causing some thread lag, should be replaced with gpu operation
134  image_copy(img, &async->img_copy);
135 
136  // Inform thread of new image
137  async->img_processed = false;
138  pthread_cond_signal(&async->img_available);
139  pthread_mutex_unlock(&async->img_mutex);
140  return 0;
141 }
142 
143 
144 void *cv_async_thread(void *args)
145 {
146  struct video_listener *listener = args;
147  struct cv_async *async = listener->async;
148  async->thread_running = true;
149 
151 
152  // Request new image from video thread
153  pthread_mutex_lock(&async->img_mutex);
154  async->img_processed = true;
155 
156  while (async->thread_running) {
157  // Wait for img available signal
158  pthread_cond_wait(&async->img_available, &async->img_mutex);
159 
160  // Img might have been processed already (spurious wake-ups)
161  if (async->img_processed) {
162  continue;
163  }
164 
165  // Execute vision function from this thread
166  listener->func(&async->img_copy);
167 
168  // Mark image as processed
169  async->img_processed = true;
170  }
171 
172  pthread_mutex_unlock(&async->img_mutex);
173  pthread_exit(NULL);
174 }
175 
176 
177 void cv_run_device(struct video_config_t *device, struct image_t *img)
178 {
179  struct image_t *result;
180 
181  // Loop through computer vision pipeline
182  for (struct video_listener *listener = device->cv_listener; listener != NULL; listener = listener->next) {
183  // If the listener is not active, skip it
184  if (!listener->active) {
185  continue;
186  }
187 
188  // If the desired frame time for this listener is not reached, skip it
189  if (listener->maximum_fps > 0 && timeval_diff(&listener->ts, &img->ts) < (1000000 / listener->maximum_fps)) {
190  continue;
191  }
192 
193  if (listener->async != NULL) {
194  // Send image to asynchronous thread, only update listener if successful
195  if (!cv_async_function(listener->async, img)) {
196  // Store timestamp
197  listener->ts = img->ts;
198  }
199  } else {
200  // Execute the cvFunction and catch result
201  result = listener->func(img);
202 
203  // If result gives an image pointer, use it in the next stage
204  if (result != NULL) {
205  img = result;
206  }
207  // Store timestamp
208  listener->ts = img->ts;
209  }
210  }
211 }
unsigned short uint16_t
Definition: types.h:16
volatile bool active
Definition: cv.h:59
pthread_t thread_id
Definition: cv.h:42
uint32_t buf_size
The buffer size.
Definition: image.h:52
volatile bool thread_running
Definition: cv.h:43
uint16_t maximum_fps
Definition: cv.h:58
struct cv_async * async
Definition: cv.h:53
void cv_attach_listener(struct video_config_t *device, struct video_listener *new_listener)
void image_free(struct image_t *img)
Free the image.
Definition: image.c:64
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
Definition: image.h:43
void image_copy(struct image_t *input, struct image_t *output)
Copy an image from inut to output This will only work if the formats are the same.
Definition: image.c:78
#define B
struct video_listener * cv_add_to_device(struct video_config_t *device, cv_function func, uint16_t fps)
Definition: cv.c:46
Functions to obtain rt priority or set the nice level.
struct video_listener * listener
Definition: colorfilter.c:43
struct video_listener * cv_listener
The first computer vision listener in the linked list for this video device.
Definition: video_device.h:66
int8_t cv_async_function(struct cv_async *async, struct image_t *img)
Definition: cv.c:110
uint16_t w
Image width.
Definition: image.h:45
unsigned long uint32_t
Definition: types.h:18
Computer vision framework for onboard processing.
uint16_t h
Image height.
Definition: image.h:46
volatile int thread_priority
Definition: cv.h:44
void * buf
Image buffer (depending on the image_type)
Definition: image.h:53
bool add_video_device(struct video_config_t *device)
Keep track of video devices added by modules.
Definition: video_thread.c:189
static uint32_t timeval_diff(struct timeval *A, struct timeval *B)
Definition: cv.c:40
struct video_listener * cv_add_to_device_async(struct video_config_t *device, cv_function func, int nice_level, uint16_t fps)
Definition: cv.c:82
pthread_cond_t img_available
Definition: cv.h:46
struct timeval ts
Definition: cv.h:54
struct image_t *(* cv_function)(struct image_t *img)
Definition: cv.h:39
void cv_run_device(struct video_config_t *device, struct image_t *img)
Definition: cv.c:177
volatile bool img_processed
Definition: cv.h:47
struct timeval ts
The timestamp of creation.
Definition: image.h:47
void * cv_async_thread(void *args)
Definition: cv.c:144
cv_function func
Definition: cv.h:55
pthread_mutex_t img_mutex
Definition: cv.h:45
#define A
struct video_listener * next
Definition: cv.h:52
signed char int8_t
Definition: types.h:15
struct image_t img_copy
Definition: cv.h:48
static int set_nice_level(int level)
Definition: rt_priority.h:69
Definition: cv.h:41
V4L2 device settings.
Definition: video_device.h:55
enum image_type type
The image type.
Definition: image.h:44