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