fix no video_thread in master

sw/airborne/modules/computer_vision/cv.c

@@ -33,11 +33,12 @@
 
 
 void cv_attach_listener(struct video_config_t *device, struct video_listener *new_listener);
-void cv_async_function(struct cv_async *async, struct image_t *img);
+int8_t cv_async_function(struct cv_async *async, struct image_t *img);
 void *cv_async_thread(void *args);
 
 
-static inline uint32_t timeval_diff(struct timeval *A, struct timeval *B) {
+static inline uint32_t timeval_diff(struct timeval *A, struct timeval *B)
+{
   return (B->tv_sec - A->tv_sec) * 1000000 + (B->tv_usec - A->tv_usec);
 }
 
@@ -66,8 +67,9 @@ struct video_listener *cv_add_to_device(struct video_config_t *device, cv_functi
     struct video_listener *listener = device->cv_listener;
 
     // Loop through linked list to last listener
-    while (listener->next != NULL)
+    while (listener->next != NULL) {
       listener = listener->next;
+    }
 
     // Add listener to end
     listener->next = new_listener;
@@ -77,7 +79,8 @@ struct video_listener *cv_add_to_device(struct video_config_t *device, cv_functi
 }
 
 
-struct video_listener *cv_add_to_device_async(struct video_config_t *device, cv_function func, int nice_level) {
+struct video_listener *cv_add_to_device_async(struct video_config_t *device, cv_function func, int nice_level)
+{
   // Create a normal listener
   struct video_listener *listener = cv_add_to_device(device, func);
 
@@ -99,10 +102,11 @@ struct video_listener *cv_add_to_device_async(struct video_config_t *device, cv_
 }
 
 
-void cv_async_function(struct cv_async *async, struct image_t *img) {
+int8_t cv_async_function(struct cv_async *async, struct image_t *img)
+{
   // If the previous image is not yet processed, return
   if (pthread_mutex_trylock(&async->img_mutex) != 0 || !async->img_processed) {
-    return;
+    return -1;
   }
 
   // If the image has not been initialized, do it
@@ -117,10 +121,12 @@ void cv_async_function(struct cv_async *async, struct image_t *img) {
   async->img_processed = false;
   pthread_cond_signal(&async->img_available);
   pthread_mutex_unlock(&async->img_mutex);
+  return 0;
 }
 
 
-void *cv_async_thread(void *args) {
+void *cv_async_thread(void *args)
+{
   struct video_listener *listener = args;
   struct cv_async *async = listener->async;
   async->thread_running = true;
@@ -168,19 +174,22 @@ void cv_run_device(struct video_config_t *device, struct image_t *img)
       continue;
     }
 
-    // Store timestamp
-    listener->ts = img->ts;
-
     if (listener->async != NULL) {
-      // Send image to asynchronous thread
-      cv_async_function(listener->async, img);
+      // Send image to asynchronous thread, only update listener if succesful
+      if (!cv_async_function(listener->async, img)) {
+        // Store timestamp
+        listener->ts = img->ts;
+      }
     } else {
       // Execute the cvFunction and catch result
       result = listener->func(img);
 
       // If result gives an image pointer, use it in the next stage
-      if (result != NULL)
+      if (result != NULL) {
         img = result;
+      }
+      // Store timestamp
+      listener->ts = img->ts;
     }
   }
 }

sw/airborne/modules/computer_vision/cv.h

@@ -39,24 +39,24 @@
 typedef struct image_t *(*cv_function)(struct image_t *img);
 
 struct cv_async {
-    pthread_t thread_id;
-    volatile bool thread_running;
-    volatile int thread_priority;
-    pthread_mutex_t img_mutex;
-    pthread_cond_t img_available;
-    volatile bool img_processed;
-    struct image_t img_copy;
+  pthread_t thread_id;
+  volatile bool thread_running;
+  volatile int thread_priority;
+  pthread_mutex_t img_mutex;
+  pthread_cond_t img_available;
+  volatile bool img_processed;
+  struct image_t img_copy;
 };
 
 struct video_listener {
-    struct video_listener *next;
-    struct cv_async *async;
-    struct timeval ts;
-    cv_function func;
+  struct video_listener *next;
+  struct cv_async *async;
+  struct timeval ts;
+  cv_function func;
 
-    // Can be set by user
-    uint16_t maximum_fps;
-    volatile bool active;
+  // Can be set by user
+  uint16_t maximum_fps;
+  volatile bool active;
 };
 
 extern bool add_video_device(struct video_config_t *device);

sw/airborne/modules/computer_vision/video_thread.c

@@ -62,7 +62,7 @@ PRINT_CONFIG_VAR(VIDEO_THREAD_NICE_LEVEL)
 #endif
 PRINT_CONFIG_VAR(VIDEO_THREAD_MAX_CAMERAS)
 
-struct video_config_t *cameras[VIDEO_THREAD_MAX_CAMERAS];
+static struct video_config_t *cameras[VIDEO_THREAD_MAX_CAMERAS] = {NULL};
 
 // Main thread
 static void *video_thread_function(void *data);
@@ -78,7 +78,7 @@ void video_thread_periodic(void)
 
 /**
  * Handles all the video streaming and saving of the image shots
- * This is a sepereate thread, so it needs to be thread safe!
+ * This is a separate thread, so it needs to be thread safe!
  */
 static void *video_thread_function(void *data)
 {
@@ -257,10 +257,6 @@ static void stop_video_thread(struct video_config_t *device)
  */
 void video_thread_init(void)
 {
-  // Initialise all camera pointers to be NULL
-  for (int indexCameras = 0; indexCameras < VIDEO_THREAD_MAX_CAMERAS; indexCameras++) {
-    cameras[indexCameras] = NULL;
-  }
 }
 
 /**
@@ -283,13 +279,11 @@ void video_thread_start()
  */
 void video_thread_stop()
 {
-
   for (int indexCameras = 0; indexCameras < VIDEO_THREAD_MAX_CAMERAS; indexCameras++) {
     if (cameras[indexCameras] != NULL) {
       stop_video_thread(cameras[indexCameras]);
     }
   }
 
-
   // TODO: wait for the thread to finish to be able to start the thread again!
 }