[video_thread] run debayer filter if set

- and only free img buf if it was actually allocated

sw/airborne/boards/ardrone/board.c

@@ -43,7 +43,7 @@ struct video_config_t front_camera = {
   .subdev_name = NULL,
   .format = V4L2_PIX_FMT_UYVY,
   .buf_cnt = 10,
-  .filters = NULL
+  .filters = 0
 };
 
 struct video_config_t bottom_camera = {
@@ -53,7 +53,7 @@ struct video_config_t bottom_camera = {
   .subdev_name = NULL,
   .format = V4L2_PIX_FMT_UYVY,
   .buf_cnt = 10,
-  .filters = NULL
+  .filters = 0
 };
 
 

sw/airborne/boards/bebop/video.c

@@ -46,7 +46,7 @@ struct video_config_t bottom_camera = {
   .subdev_name = NULL,
   .format = V4L2_PIX_FMT_UYVY,
   .buf_cnt = 60,
-  .filters = NULL
+  .filters = 0
 };
 
 struct video_config_t front_camera = {
@@ -56,7 +56,7 @@ struct video_config_t front_camera = {
   .subdev_name = "/dev/v4l-subdev1",
   .format = V4L2_PIX_FMT_SGBRG10,
   .buf_cnt = 10,
-  .filters = NULL //{DeMosaic, AEC, ABW}
+  .filters = VIDEO_FILTER_DEBAYER
 };
 
 static bool_t write_reg(int fd, char *addr_val, uint8_t cnt)

sw/airborne/modules/computer_vision/lib/vision/image.c

@@ -62,7 +62,10 @@ void image_create(struct image_t *img, uint16_t width, uint16_t height, enum ima
  */
 void image_free(struct image_t *img)
 {
-  free(img->buf);
+  if (img->buf != NULL) {
+    free(img->buf);
+    img->buf = NULL;
+  }
 }
 
 /**

sw/airborne/modules/computer_vision/video_thread.c

@@ -37,6 +37,7 @@
 // Video
 #include "lib/v4l/v4l2.h"
 #include "lib/vision/image.h"
+#include "lib/vision/bayern.h"
 #include "lib/encoding/jpeg.h"
 #include "peripherals/video_device.h"
 
@@ -63,7 +64,7 @@ PRINT_CONFIG_VAR(VIDEO_THREAD_DEVICE_BUFFERS)
 #define VIDEO_THREAD_SUBDEV NULL
 #endif
 #ifndef VIDEO_THREAD_FILTERS
-#define VIDEO_THREAD_FILTERS NULL
+#define VIDEO_THREAD_FILTERS 0
 #endif
 struct video_config_t custom_camera = {
   .w = VIDEO_THREAD_VIDEO_WIDTH,
@@ -102,12 +103,60 @@ struct video_thread_t video_thread = {
   .shot_number = 0
 };
 
+static void video_thread_save_shot(struct image_t *img, struct image_t *img_jpeg)
+{
+
+  // Search for a file where we can write to
+  char save_name[128];
+  for (; video_thread.shot_number < 99999; video_thread.shot_number++) {
+    sprintf(save_name, "%s/img_%05d.jpg", STRINGIFY(VIDEO_THREAD_SHOT_PATH), video_thread.shot_number);
+    // Check if file exists or not
+    if (access(save_name, F_OK) == -1) {
+
+      // Create a high quality image (99% JPEG encoded)
+      jpeg_encode_image(img, img_jpeg, 99, TRUE);
+
+#if JPEG_WITH_EXIF_HEADER
+      write_exif_jpeg(save_name, img_jpeg->buf, img_jpeg->buf_size, img_jpeg->w, img_jpeg->h);
+#else
+      FILE *fp = fopen(save_name, "w");
+      if (fp == NULL) {
+        printf("[video_thread-thread] Could not write shot %s.\n", save_name);
+      } else {
+        // Save it to the file and close it
+        fwrite(img_jpeg->buf, sizeof(uint8_t), img_jpeg->buf_size, fp);
+        fclose(fp);
+      }
+#endif
+
+      // We don't need to seek for a next index anymore
+      break;
+    }
+  }
+}
+
+
 /**
  * Handles all the video streaming and saving of the image shots
  * This is a sepereate thread, so it needs to be thread safe!
  */
 static void *video_thread_function(void *data)
 {
+  struct video_config_t *vid = (struct video_config_t *)&(VIDEO_THREAD_CAMERA);
+
+  struct image_t img_jpeg;
+  struct image_t img_color;
+
+  // create the images
+  if (vid->filters) {
+    // fixme: don't hardcode size, works for bebop front camera for now
+#define IMG_FLT_SIZE 272
+    image_create(&img_color, IMG_FLT_SIZE, IMG_FLT_SIZE, IMAGE_YUV422);
+    image_create(&img_jpeg, IMG_FLT_SIZE, IMG_FLT_SIZE, IMAGE_JPEG);
+  }
+  else {
+    image_create(&img_jpeg, vid->w, vid->h, IMAGE_JPEG);
+  }
 
   // Start the streaming of the V4L2 device
   if (!v4l2_start_capture(video_thread.dev)) {
@@ -138,48 +187,34 @@ static void *video_thread_function(void *data)
     struct image_t img;
     v4l2_image_get(video_thread.dev, &img);
 
+    // pointer to the final image to pass for saving and further processing
+    struct image_t *img_final = &img;
+
+    // run selected filters
+    if (vid->filters) {
+      if (vid->filters & VIDEO_FILTER_DEBAYER) {
+        BayernToYUV(&img, &img_color, 0, 0);
+      }
+      // use color image for further processing
+      img_final = &img_color;
+    }
+
     // Check if we need to take a shot
     if (video_thread.take_shot) {
-      // Create a high quality image (99% JPEG encoded)
-      struct image_t jpeg_hr;
-      image_create(&jpeg_hr, img.w, img.h, IMAGE_JPEG);
-      jpeg_encode_image(&img, &jpeg_hr, 99, TRUE);
-
-      // Search for a file where we can write to
-      char save_name[128];
-      for (; video_thread.shot_number < 99999; video_thread.shot_number++) {
-        sprintf(save_name, "%s/img_%05d.jpg", STRINGIFY(VIDEO_THREAD_SHOT_PATH), video_thread.shot_number);
-        // Check if file exists or not
-        if (access(save_name, F_OK) == -1) {
-#if JPEG_WITH_EXIF_HEADER
-          write_exif_jpeg(save_name, jpeg_hr.buf, jpeg_hr.buf_size, img.w, img.h);
-#else
-          FILE *fp = fopen(save_name, "w");
-          if (fp == NULL) {
-            printf("[video_thread-thread] Could not write shot %s.\n", save_name);
-          } else {
-            // Save it to the file and close it
-            fwrite(jpeg_hr.buf, sizeof(uint8_t), jpeg_hr.buf_size, fp);
-            fclose(fp);
-          }
-#endif
-          // We don't need to seek for a next index anymore
-          break;
-        }
-      }
-
-      // We finished the shot
-      image_free(&jpeg_hr);
+      video_thread_save_shot(img_final, &img_jpeg);
       video_thread.take_shot = FALSE;
     }
 
     // Run processing if required
-    cv_run(&img);
+    cv_run(img_final);
 
     // Free the image
     v4l2_image_free(video_thread.dev, &img);
   }
 
+  image_free(&img_jpeg);
+  image_free(&img_color);
+
   return 0;
 }
 
@@ -195,7 +230,7 @@ void video_thread_init(void)
     // FIXME! add subdev format to config, only needed on bebop front camera so far
     if (!v4l2_init_subdev(vid->subdev_name, 0, 0, V4L2_MBUS_FMT_SGBRG10_1X10, vid->w, vid->h)) {
       printf("[video_thread] Could not initialize the %s subdevice.\n", vid->subdev_name);
-      return 0;
+      return;
     }
   }
 
@@ -203,7 +238,7 @@ void video_thread_init(void)
   video_thread.dev = v4l2_init(vid->dev_name, vid->w, vid->h, vid->buf_cnt, vid->format);
   if (video_thread.dev == NULL) {
     printf("[video_thread] Could not initialize the %s V4L2 device.\n", vid->dev_name);
-    return 0;
+    return;
   }
 
   // Create the shot directory
@@ -227,7 +262,7 @@ void video_thread_start(void)
 
   // Start the streaming thread
   pthread_t tid;
-  if (pthread_create(&tid, NULL, video_thread_function, NULL) != 0) {
+  if (pthread_create(&tid, NULL, video_thread_function, (void*)(&VIDEO_THREAD_CAMERA)) != 0) {
     printf("[vievideo] Could not create streaming thread.\n");
     return;
   }

sw/airborne/peripherals/video_device.h

@@ -31,6 +31,8 @@
 #include "std.h"
 #include "modules/computer_vision/lib/v4l/v4l2.h"
 
+#define VIDEO_FILTER_DEBAYER 0x01
+
 /** V4L2 device settings */
 struct video_config_t {
   int w;              ///< Width
@@ -39,7 +41,7 @@ struct video_config_t {
   char* subdev_name;  ///< path to sub device
   uint32_t format;    ///< Video format
   uint8_t buf_cnt;    ///< Amount of V4L2 video device buffers
-  void* filters;      ///< filters to use
+  uint8_t filters;    ///< filters to use (bitfield with VIDEO_FILTER_x)
 };