[computervision] Setup a re-usable framework where computer vision methods can be added to the computer vision thread

conf/airframes/TUDelft/conf.xml

@@ -51,7 +51,7 @@
    telemetry="telemetry/default_rotorcraft.xml"
    flight_plan="flight_plans/rotorcraft_basic.xml"
    settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/estimation/ahrs_float_mlkf.xml settings/control/stabilization_att_int_quat.xml"
-   settings_modules="modules/geo_mag.xml modules/air_data.xml modules/video_rtp_stream.xml"
+   settings_modules="modules/geo_mag.xml modules/air_data.xml modules/video_thread.xml modules/video_rtp_stream.xml"
    gui_color="green"
   />
   <aircraft

conf/airframes/examples/bebop.xml

@@ -30,16 +30,18 @@
     <!--load name="logger_file.xml">
       <define name="FILE_LOGGER_PATH" value="/data/ftp/internal_000"/>
     </load-->
-    <load name="video_rtp_stream.xml">
+    <load name="video_thread.xml">
       <define name="VIEWVIDEO_FPS" value="4"/>
-      <define name="VIEWVIDEO_QUALITY_FACTOR" value="80"/>
-      <define name="VIEWVIDEO_DOWNSIZE_FACTOR" value="1"/>
       <define name="VIEWVIDEO_DEVICE" value="/dev/video0"/>
       <define name="VIEWVIDEO_SUBDEV" value="/dev/v4l-subdev0"/>
       <define name="VIEWVIDEO_DEVICE_SIZE" value="640,480"/>
       <define name="VIEWVIDEO_DEVICE_BUFFERS" value="60"/>
       <define name="VIEWVIDEO_SHOT_PATH" value="/data/ftp/internal_000/images"/>
     </load>
+    <load name="video_rtp_stream.xml">
+      <define name="VIEWVIDEO_QUALITY_FACTOR" value="80"/>
+      <define name="VIEWVIDEO_DOWNSIZE_FACTOR" value="1"/>
+    </load>
   </modules>
 
   <commands>

conf/modules/video_rtp_stream.xml

@@ -8,44 +8,32 @@
       - Sends a RTP/UDP stream of the camera
       - Possibility to save an image(shot) on the internal memory (JPEG, full size, best quality)
     </description>
-    <define name="VIEWVIDEO_DEVICE" value="/dev/video1" description="The video device to capture from"/>
-    <define name="VIEWVIDEO_DEVICE_SIZE" value="1280,720" description="Video capture size (width, height)"/>
-    <define name="VIEWVIDEO_DEVICE_BUFFERS" value="10" description="Amount of V4L2 image buffers"/>
     <define name="VIEWVIDEO_DOWNSIZE_FACTOR" value="4" description="Reduction factor of the video stream"/>
     <define name="VIEWVIDEO_QUALITY_FACTOR" value="50" description="JPEG encoding compression factor [0-99]"/>
-    <define name="VIEWVIDEO_FPS" value="4" description="Video stream frame rate"/>
-    <define name="VIEWVIDEO_SHOT_PATH" value="/data/video/images" description="Path where the images should be saved"/>
     <define name="VIEWVIDEO_USE_NETCAT" value="FALSE|TRUE" description="Use netcat for transfering images (default: FALSE)"/>
     <define name="VIEWVIDEO_USE_RTP" value="TRUE|FALSE" description="Use RTP for transfering images (default: TRUE)"/>
   </doc>
   <settings>
     <dl_settings>
       <dl_settings name="video">
-        <dl_setting var="viewvideo.take_shot" min="0" step="1" max="1" shortname="take_shot" module="computer_vision/viewvideo" handler="take_shot">
-          <strip_button name="Shoot" icon="digital-camera.png" value="1" group="video"/>
-        </dl_setting>
         <dl_setting var="viewvideo.use_rtp" min="0" step="1" max="1" values="FALSE|TRUE" shortname="rtp" module="computer_vision/viewvideo" param="VIEWVIDEO_USE_RTP"/>
       </dl_settings>
     </dl_settings>
   </settings>
 
+  <depends>video_thread</depends>
   <header>
     <file name="viewvideo.h"/>
   </header>
 
   <init fun="viewvideo_init()"/>
-  <periodic fun="viewvideo_periodic()" freq="1" start="viewvideo_start()" stop="viewvideo_stop()" autorun="TRUE"/>
   <makefile target="ap">
 
     <file name="viewvideo.c"/>
-    <file name="cv.c"/>
 
     <!-- Include the needed Computer Vision files -->
     <define name="modules/computer_vision" type="include"/>
-    <file name="image.c" dir="modules/computer_vision/lib/vision"/>
-    <file name="jpeg.c" dir="modules/computer_vision/lib/encoding"/>
     <file name="rtp.c" dir="modules/computer_vision/lib/encoding"/>
-    <file name="v4l2.c" dir="modules/computer_vision/lib/v4l"/>
 
     <!-- Define the network connection to send images over -->
     <raw>
@@ -61,10 +49,6 @@
       endif
     </raw>
 
-    <!-- Random flags -->
-    <define name="__USE_GNU"/>
-    <flag name="LDFLAGS" value="lrt"/>
-    <flag name="LDFLAGS" value="static-libgcc"/>
   </makefile>
   <makefile target="nps">
     <file name="viewvideo_nps.c"/>

conf/modules/video_thread.xml

@@ -0,0 +1,52 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="video_thread" dir="computer_vision">
+  <doc>
+    <description>
+      Video streaming for Linux devices
+
+      - Sends a RTP/UDP stream of the camera
+      - Possibility to save an image(shot) on the internal memory (JPEG, full size, best quality)
+    </description>
+    <define name="video_thread_DEVICE" value="/dev/video1" description="The video device to capture from"/>
+    <define name="video_thread_DEVICE_SIZE" value="1280,720" description="Video capture size (width, height)"/>
+    <define name="video_thread_DEVICE_BUFFERS" value="10" description="Amount of V4L2 image buffers"/>
+    <define name="video_thread_FPS" value="4" description="Video stream frame rate"/>
+    <define name="video_thread_SHOT_PATH" value="/data/video/images" description="Path where the images should be saved"/>
+  </doc>
+  <settings>
+    <dl_settings>
+      <dl_settings name="video">
+        <dl_setting var="video_thread.take_shot" min="0" step="1" max="1" shortname="take_shot" module="computer_vision/video_thread" handler="take_shot">
+          <strip_button name="Shoot" icon="digital-camera.png" value="1" group="video"/>
+        </dl_setting>
+      </dl_settings>
+    </dl_settings>
+  </settings>
+
+  <header>
+    <file name="video_thread.h"/>
+  </header>
+
+  <init fun="video_thread_init()"/>
+  <periodic fun="video_thread_periodic()" freq="1" start="video_thread_start()" stop="video_thread_stop()" autorun="TRUE"/>
+  <makefile target="ap">
+
+    <file name="video_thread.c"/>
+    <file name="cv.c"/>
+
+    <!-- Include the needed Computer Vision files -->
+    <define name="modules/computer_vision" type="include"/>
+    <file name="image.c" dir="modules/computer_vision/lib/vision"/>
+    <file name="v4l2.c" dir="modules/computer_vision/lib/v4l"/>
+    <file name="jpeg.c" dir="modules/computer_vision/lib/encoding"/>
+
+    <!-- Random flags -->
+    <define name="__USE_GNU"/>
+    <flag name="LDFLAGS" value="lrt"/>
+    <flag name="LDFLAGS" value="static-libgcc"/>
+  </makefile>
+  <makefile target="nps">
+    <file name="video_thread_nps.c"/>
+  </makefile>
+</module>

sw/airborne/modules/computer_vision/video_thread.c

@@ -0,0 +1,255 @@
+/*
+ * Copyright (C) 2012-2014 The Paparazzi Community
+ *               2015 Freek van Tienen <freek.v.tienen@gmail.com>
+ *
+ * This file is part of Paparazzi.
+ *
+ * Paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * Paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * @file modules/computer_vision/video_thread.c
+ */
+
+// Own header
+#include "modules/computer_vision/video_thread.h"
+#include "modules/computer_vision/cv.h"
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+#include <sys/time.h>
+#include <math.h>
+
+// Video
+#include "lib/v4l/v4l2.h"
+#include "lib/vision/image.h"
+#include "lib/encoding/jpeg.h"
+
+#if JPEG_WITH_EXIF_HEADER
+#include "lib/exif/exif_module.h"
+#endif
+
+// Threaded computer vision
+#include <pthread.h>
+#include "rt_priority.h"
+
+// The video device
+#ifndef VIEWVIDEO_DEVICE
+#define VIEWVIDEO_DEVICE /dev/video1
+#endif
+PRINT_CONFIG_VAR(VIEWVIDEO_DEVICE)
+
+// The video device size (width, height)
+#ifndef VIEWVIDEO_DEVICE_SIZE
+#define VIEWVIDEO_DEVICE_SIZE 1280,720
+#endif
+#define __SIZE_HELPER(x, y) #x", "#y
+#define _SIZE_HELPER(x) __SIZE_HELPER(x)
+PRINT_CONFIG_MSG("VIEWVIDEO_DEVICE_SIZE = " _SIZE_HELPER(VIEWVIDEO_DEVICE_SIZE))
+
+// The video device buffers (the amount of V4L2 buffers)
+#ifndef VIEWVIDEO_DEVICE_BUFFERS
+#define VIEWVIDEO_DEVICE_BUFFERS 10
+#endif
+PRINT_CONFIG_VAR(VIEWVIDEO_DEVICE_BUFFERS)
+
+// Frames Per Seconds
+#ifndef VIEWVIDEO_FPS
+#define VIEWVIDEO_FPS 4
+#endif
+PRINT_CONFIG_VAR(VIEWVIDEO_FPS)
+
+// The place where the shots are saved (without slash on the end)
+#ifndef VIEWVIDEO_SHOT_PATH
+#define VIEWVIDEO_SHOT_PATH "/data/video/images"
+#endif
+PRINT_CONFIG_VAR(VIEWVIDEO_SHOT_PATH)
+
+// Main thread
+static void *video_thread_function(void *data);
+void video_thread_periodic(void) { }
+
+// Initialize the video_thread structure with the defaults
+struct video_thread_t video_thread = {
+  .is_running = FALSE,
+  .fps = VIEWVIDEO_FPS,
+  .take_shot = FALSE,
+  .shot_number = 0
+};
+
+/**
+ * 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 __attribute__((unused)))
+{
+  // Start the streaming of the V4L2 device
+  if (!v4l2_start_capture(video_thread.dev)) {
+    printf("[video_thread-thread] Could not start capture of %s.\n", video_thread.dev->name);
+    return 0;
+  }
+
+  // be nice to the more important stuff
+  set_nice_level(10);
+
+  // Initialize timing
+  uint32_t microsleep = (uint32_t)(1000000. / (float)video_thread.fps);
+  struct timeval last_time;
+  gettimeofday(&last_time, NULL);
+
+  // Start streaming
+  video_thread.is_running = TRUE;
+  while (video_thread.is_running) {
+    // compute usleep to have a more stable frame rate
+    struct timeval vision_thread_sleep_time;
+    gettimeofday(&vision_thread_sleep_time, NULL);
+    int dt = (int)(vision_thread_sleep_time.tv_sec - last_time.tv_sec) * 1000000 +
+             (int)(vision_thread_sleep_time.tv_usec - last_time.tv_usec);
+    if (dt < microsleep) { usleep(microsleep - dt); }
+    last_time = vision_thread_sleep_time;
+
+    // Wait for a new frame (blocking)
+    struct image_t img;
+    v4l2_image_get(video_thread.dev, &img);
+
+    // 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(VIEWVIDEO_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.take_shot = FALSE;
+    }
+
+    // Run processing if required
+    cv_run(&img);
+
+    // Free the image
+    v4l2_image_free(video_thread.dev, &img);
+  }
+
+  return 0;
+}
+
+/**
+ * Initialize the view video
+ */
+void video_thread_init(void)
+{
+#ifdef VIEWVIDEO_SUBDEV
+  PRINT_CONFIG_MSG("[video_thread] Configuring a subdevice!")
+  PRINT_CONFIG_VAR(VIEWVIDEO_SUBDEV)
+
+  // Initialize the V4L2 subdevice (TODO: fix hardcoded path, which and code)
+  if (!v4l2_init_subdev(STRINGIFY(VIEWVIDEO_SUBDEV), 0, 1, V4L2_MBUS_FMT_UYVY8_2X8, VIEWVIDEO_DEVICE_SIZE)) {
+    printf("[video_thread] Could not initialize the %s subdevice.\n", STRINGIFY(VIEWVIDEO_SUBDEV));
+    return;
+  }
+#endif
+
+  // Initialize the V4L2 device
+  video_thread.dev = v4l2_init(STRINGIFY(VIEWVIDEO_DEVICE), VIEWVIDEO_DEVICE_SIZE, VIEWVIDEO_DEVICE_BUFFERS);
+  if (video_thread.dev == NULL) {
+    printf("[video_thread] Could not initialize the %s V4L2 device.\n", STRINGIFY(VIEWVIDEO_DEVICE));
+    return;
+  }
+
+  // Create the shot directory
+  char save_name[128];
+  sprintf(save_name, "mkdir -p %s", STRINGIFY(VIEWVIDEO_SHOT_PATH));
+  if (system(save_name) != 0) {
+    printf("[video_thread] Could not create shot directory %s.\n", STRINGIFY(VIEWVIDEO_SHOT_PATH));
+    return;
+  }
+}
+
+/**
+ * Start with streaming
+ */
+void video_thread_start(void)
+{
+  // Check if we are already running
+  if (video_thread.is_running) {
+    return;
+  }
+
+  // Start the streaming thread
+  pthread_t tid;
+  if (pthread_create(&tid, NULL, video_thread_function, NULL) != 0) {
+    printf("[vievideo] Could not create streaming thread.\n");
+    return;
+  }
+}
+
+/**
+ * Stops the streaming
+ * This could take some time, because the thread is stopped asynchronous.
+ */
+void video_thread_stop(void)
+{
+  // Check if not already stopped streaming
+  if (!video_thread.is_running) {
+    return;
+  }
+
+  // Stop the streaming thread
+  video_thread.is_running = FALSE;
+
+  // Stop the capturing
+  if (!v4l2_stop_capture(video_thread.dev)) {
+    printf("[video_thread] Could not stop capture of %s.\n", video_thread.dev->name);
+    return;
+  }
+
+  // TODO: wait for the thread to finish to be able to start the thread again!
+}
+
+/**
+ * Take a shot and save it
+ * This will only work when the streaming is enabled
+ */
+void video_thread_take_shot(bool_t take)
+{
+  video_thread.take_shot = take;
+}

sw/airborne/modules/computer_vision/video_thread.h

@@ -0,0 +1,54 @@
+/*
+ * Copyright (C) 2015
+ *
+ * This file is part of Paparazzi.
+ *
+ * Paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * Paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * @file modules/computer_vision/video_thread.h
+ *
+ * Start a Video thread and grab images
+ *
+ * Works on Linux platforms
+ */
+
+#ifndef VIDEO_THREAD_H
+#define VIDEO_THREAD_H
+
+#include "std.h"
+
+// Main video_thread structure
+struct video_thread_t {
+  volatile bool_t is_running;   ///< When the device is running
+  struct v4l2_device *dev;        ///< The V4L2 device that is used for the video stream
+  uint8_t fps;                    ///< The amount of frames per second
+
+  volatile bool_t take_shot;      ///< Wether to take an image
+  uint16_t shot_number;           ///< The last shot number
+};
+extern struct video_thread_t video_thread;
+
+// Module functions
+extern void video_thread_init(void);
+extern void video_thread_periodic(void); ///< A dummy for now
+extern void video_thread_start(void);
+extern void video_thread_stop(void);
+extern void video_thread_take_shot(bool_t take);
+
+#endif /* VIDEO_THREAD_H */
+

sw/airborne/modules/computer_vision/viewvideo.c

@@ -41,40 +41,11 @@
 #include <math.h>
 
 // Video
-#include "lib/v4l/v4l2.h"
 #include "lib/vision/image.h"
 #include "lib/encoding/jpeg.h"
 #include "lib/encoding/rtp.h"
 #include "udp_socket.h"
 
-#if JPEG_WITH_EXIF_HEADER
-#include "lib/exif/exif_module.h"
-#endif
-
-// Threaded computer vision
-#include <pthread.h>
-#include "rt_priority.h"
-
-// The video device
-#ifndef VIEWVIDEO_DEVICE
-#define VIEWVIDEO_DEVICE /dev/video1
-#endif
-PRINT_CONFIG_VAR(VIEWVIDEO_DEVICE)
-
-// The video device size (width, height)
-#ifndef VIEWVIDEO_DEVICE_SIZE
-#define VIEWVIDEO_DEVICE_SIZE 1280,720
-#endif
-#define __SIZE_HELPER(x, y) #x", "#y
-#define _SIZE_HELPER(x) __SIZE_HELPER(x)
-PRINT_CONFIG_MSG("VIEWVIDEO_DEVICE_SIZE = " _SIZE_HELPER(VIEWVIDEO_DEVICE_SIZE))
-
-// The video device buffers (the amount of V4L2 buffers)
-#ifndef VIEWVIDEO_DEVICE_BUFFERS
-#define VIEWVIDEO_DEVICE_BUFFERS 10
-#endif
-PRINT_CONFIG_VAR(VIEWVIDEO_DEVICE_BUFFERS)
-
 // Downsize factor for video stream
 #ifndef VIEWVIDEO_DOWNSIZE_FACTOR
 #define VIEWVIDEO_DOWNSIZE_FACTOR 4
@@ -93,18 +64,6 @@ PRINT_CONFIG_VAR(VIEWVIDEO_QUALITY_FACTOR)
 #endif
 PRINT_CONFIG_VAR(VIEWVIDEO_RTP_TIME_INC)
 
-// Frames Per Seconds
-#ifndef VIEWVIDEO_FPS
-#define VIEWVIDEO_FPS 4
-#endif
-PRINT_CONFIG_VAR(VIEWVIDEO_FPS)
-
-// The place where the shots are saved (without slash on the end)
-#ifndef VIEWVIDEO_SHOT_PATH
-#define VIEWVIDEO_SHOT_PATH "/data/video/images"
-#endif
-PRINT_CONFIG_VAR(VIEWVIDEO_SHOT_PATH)
-
 // Check if we are using netcat instead of RTP/UDP
 #ifndef VIEWVIDEO_USE_NETCAT
 #define VIEWVIDEO_USE_NETCAT FALSE
@@ -127,118 +86,47 @@ PRINT_CONFIG_MSG("[viewvideo] Using RTP/UDP stream.")
 PRINT_CONFIG_VAR(VIEWVIDEO_HOST)
 PRINT_CONFIG_VAR(VIEWVIDEO_PORT_OUT)
 
-// Main thread
-static void *viewvideo_thread(void *data);
-void viewvideo_periodic(void) { }
-
 // Initialize the viewvideo structure with the defaults
 struct viewvideo_t viewvideo = {
   .is_streaming = FALSE,
   .downsize_factor = VIEWVIDEO_DOWNSIZE_FACTOR,
   .quality_factor = VIEWVIDEO_QUALITY_FACTOR,
-  .fps = VIEWVIDEO_FPS,
-  .take_shot = FALSE,
   .use_rtp = VIEWVIDEO_USE_RTP,
-  .shot_number = 0
 };
 
 /**
  * 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 *viewvideo_thread(void *data __attribute__((unused)))
+struct UdpSocket video_sock;
+bool_t viewvideo_function(struct image_t* img);
+bool_t viewvideo_function(struct image_t* img)
 {
-  // Start the streaming of the V4L2 device
-  if (!v4l2_start_capture(viewvideo.dev)) {
-    printf("[viewvideo-thread] Could not start capture of %s.\n", viewvideo.dev->name);
-    return 0;
-  }
-
-  // be nice to the more important stuff
-  set_nice_level(10);
-
   // Resize image if needed
   struct image_t img_small;
   image_create(&img_small,
-               viewvideo.dev->w / viewvideo.downsize_factor,
-               viewvideo.dev->h / viewvideo.downsize_factor,
+               img->w / viewvideo.downsize_factor,
+               img->h / viewvideo.downsize_factor,
                IMAGE_YUV422);
 
   // Create the JPEG encoded image
   struct image_t img_jpeg;
   image_create(&img_jpeg, img_small.w, img_small.h, IMAGE_JPEG);
 
-  // Initialize timing
-  uint32_t microsleep = (uint32_t)(1000000. / (float)viewvideo.fps);
-  struct timeval last_time;
-  gettimeofday(&last_time, NULL);
-
 #if VIEWVIDEO_USE_NETCAT
   char nc_cmd[64];
   sprintf(nc_cmd, "nc %s %d 2>/dev/null", STRINGIFY(VIEWVIDEO_HOST), VIEWVIDEO_PORT_OUT);
 #else
-  struct UdpSocket video_sock;
-  udp_socket_create(&video_sock, STRINGIFY(VIEWVIDEO_HOST), VIEWVIDEO_PORT_OUT, -1, VIEWVIDEO_BROADCAST);
 #endif
 
-  // Start streaming
-  viewvideo.is_streaming = TRUE;
-  while (viewvideo.is_streaming) {
-    // compute usleep to have a more stable frame rate
-    struct timeval vision_thread_sleep_time;
-    gettimeofday(&vision_thread_sleep_time, NULL);
-    int dt = (int)(vision_thread_sleep_time.tv_sec - last_time.tv_sec) * 1000000 +
-             (int)(vision_thread_sleep_time.tv_usec - last_time.tv_usec);
-    if (dt < microsleep) { usleep(microsleep - dt); }
-    last_time = vision_thread_sleep_time;
-
-    // Wait for a new frame (blocking)
-    struct image_t img;
-    v4l2_image_get(viewvideo.dev, &img);
-
-    // Check if we need to take a shot
-    if (viewvideo.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 (; viewvideo.shot_number < 99999; viewvideo.shot_number++) {
-        sprintf(save_name, "%s/img_%05d.jpg", STRINGIFY(VIEWVIDEO_SHOT_PATH), viewvideo.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("[viewvideo-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);
-      viewvideo.take_shot = FALSE;
-    }
-
-    cv_run(&img);
+  if (viewvideo.is_streaming) {
 
     // Only resize when needed
     if (viewvideo.downsize_factor != 1) {
-      image_yuv422_downsample(&img, &img_small, viewvideo.downsize_factor);
+      image_yuv422_downsample(img, &img_small, viewvideo.downsize_factor);
       jpeg_encode_image(&img_small, &img_jpeg, VIEWVIDEO_QUALITY_FACTOR, VIEWVIDEO_USE_NETCAT);
     } else {
-      jpeg_encode_image(&img, &img_jpeg, VIEWVIDEO_QUALITY_FACTOR, VIEWVIDEO_USE_NETCAT);
+      jpeg_encode_image(img, &img_jpeg, VIEWVIDEO_QUALITY_FACTOR, VIEWVIDEO_USE_NETCAT);
     }
 
 #if VIEWVIDEO_USE_NETCAT
@@ -286,14 +174,12 @@ static void *viewvideo_thread(void *data __attribute__((unused)))
     }
 #endif
 
-    // Free the image
-    v4l2_image_free(viewvideo.dev, &img);
   }
 
   // Free all buffers
-  image_free(&img_jpeg);
-  image_free(&img_small);
-  return 0;
+  //image_free(&img_jpeg);
+  //image_free(&img_small);
+  return TRUE;
 }
 
 /**
@@ -301,31 +187,13 @@ static void *viewvideo_thread(void *data __attribute__((unused)))
  */
 void viewvideo_init(void)
 {
-#ifdef VIEWVIDEO_SUBDEV
-  PRINT_CONFIG_MSG("[viewvideo] Configuring a subdevice!")
-  PRINT_CONFIG_VAR(VIEWVIDEO_SUBDEV)
+	char save_name[512];
+//  struct UdpSocket video_sock;
+  udp_socket_create(&video_sock, STRINGIFY(VIEWVIDEO_HOST), VIEWVIDEO_PORT_OUT, -1, VIEWVIDEO_BROADCAST);
 
-  // Initialize the V4L2 subdevice (TODO: fix hardcoded path, which and code)
-  if (!v4l2_init_subdev(STRINGIFY(VIEWVIDEO_SUBDEV), 0, 1, V4L2_MBUS_FMT_UYVY8_2X8, VIEWVIDEO_DEVICE_SIZE)) {
-    printf("[viewvideo] Could not initialize the %s subdevice.\n", STRINGIFY(VIEWVIDEO_SUBDEV));
-    return;
-  }
-#endif
+	cv_add(viewvideo_function);
 
-  // Initialize the V4L2 device
-  viewvideo.dev = v4l2_init(STRINGIFY(VIEWVIDEO_DEVICE), VIEWVIDEO_DEVICE_SIZE, VIEWVIDEO_DEVICE_BUFFERS);
-  if (viewvideo.dev == NULL) {
-    printf("[viewvideo] Could not initialize the %s V4L2 device.\n", STRINGIFY(VIEWVIDEO_DEVICE));
-    return;
-  }
-
-  // Create the shot directory
-  char save_name[128];
-  sprintf(save_name, "mkdir -p %s", STRINGIFY(VIEWVIDEO_SHOT_PATH));
-  if (system(save_name) != 0) {
-    printf("[viewvideo] Could not create shot directory %s.\n", STRINGIFY(VIEWVIDEO_SHOT_PATH));
-    return;
-  }
+	viewvideo.is_streaming = TRUE;
 
 #if VIEWVIDEO_USE_NETCAT
   // Create an Netcat receiver file for the streaming
@@ -354,52 +222,3 @@ void viewvideo_init(void)
 #endif
 }
 
-/**
- * Start with streaming
- */
-void viewvideo_start(void)
-{
-  // Check if we are already running
-  if (viewvideo.is_streaming) {
-    return;
-  }
-
-  // Start the streaming thread
-  pthread_t tid;
-  if (pthread_create(&tid, NULL, viewvideo_thread, NULL) != 0) {
-    printf("[vievideo] Could not create streaming thread.\n");
-    return;
-  }
-}
-
-/**
- * Stops the streaming
- * This could take some time, because the thread is stopped asynchronous.
- */
-void viewvideo_stop(void)
-{
-  // Check if not already stopped streaming
-  if (!viewvideo.is_streaming) {
-    return;
-  }
-
-  // Stop the streaming thread
-  viewvideo.is_streaming = FALSE;
-
-  // Stop the capturing
-  if (!v4l2_stop_capture(viewvideo.dev)) {
-    printf("[viewvideo] Could not stop capture of %s.\n", viewvideo.dev->name);
-    return;
-  }
-
-  // TODO: wait for the thread to finish to be able to start the thread again!
-}
-
-/**
- * Take a shot and save it
- * This will only work when the streaming is enabled
- */
-void viewvideo_take_shot(bool_t take)
-{
-  viewvideo.take_shot = take;
-}

sw/airborne/modules/computer_vision/viewvideo.h

@@ -37,23 +37,14 @@
 // Main viewvideo structure
 struct viewvideo_t {
   volatile bool_t is_streaming;   ///< When the device is streaming
-  struct v4l2_device *dev;        ///< The V4L2 device that is used for the video stream
   uint8_t downsize_factor;        ///< Downsize factor during the stream
   uint8_t quality_factor;         ///< Quality factor during the stream
-  uint8_t fps;                    ///< The amount of frames per second
-
   bool_t use_rtp;                 ///< Stream over RTP
-  volatile bool_t take_shot;      ///< Wether to take an image
-  uint16_t shot_number;           ///< The last shot number
 };
 extern struct viewvideo_t viewvideo;
 
 // Module functions
 extern void viewvideo_init(void);
-extern void viewvideo_periodic(void); ///< A dummy for now
-extern void viewvideo_start(void);
-extern void viewvideo_stop(void);
-extern void viewvideo_take_shot(bool_t take);
 
 #endif /* VIEW_VIDEO_H */