diff --git a/conf/airframes/TUDelft/conf.xml b/conf/airframes/TUDelft/conf.xml index c3fb52c303..649cde7bfd 100644 --- a/conf/airframes/TUDelft/conf.xml +++ b/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" /> - + - - + + + + diff --git a/conf/modules/video_rtp_stream.xml b/conf/modules/video_rtp_stream.xml index 68e0d3885f..f69c654436 100644 --- a/conf/modules/video_rtp_stream.xml +++ b/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) - - - - - - - - + video_thread
- - - - - @@ -61,10 +49,6 @@ endif - - - - diff --git a/conf/modules/video_thread.xml b/conf/modules/video_thread.xml new file mode 100644 index 0000000000..81e52b1d7a --- /dev/null +++ b/conf/modules/video_thread.xml @@ -0,0 +1,52 @@ + + + + + + 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) + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + diff --git a/sw/airborne/modules/computer_vision/video_thread.c b/sw/airborne/modules/computer_vision/video_thread.c new file mode 100644 index 0000000000..19d7faaaf7 --- /dev/null +++ b/sw/airborne/modules/computer_vision/video_thread.c @@ -0,0 +1,255 @@ +/* + * Copyright (C) 2012-2014 The Paparazzi Community + * 2015 Freek van Tienen + * + * 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 + * . + * + */ + +/** + * @file modules/computer_vision/video_thread.c + */ + +// Own header +#include "modules/computer_vision/video_thread.h" +#include "modules/computer_vision/cv.h" + +#include +#include +#include +#include +#include +#include + +// 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 +#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; +} diff --git a/sw/airborne/modules/computer_vision/video_thread.h b/sw/airborne/modules/computer_vision/video_thread.h new file mode 100644 index 0000000000..818f2f257d --- /dev/null +++ b/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 + * . + * + */ + +/** + * @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 */ + diff --git a/sw/airborne/modules/computer_vision/viewvideo.c b/sw/airborne/modules/computer_vision/viewvideo.c index f7ba5a5671..a9b2908b68 100644 --- a/sw/airborne/modules/computer_vision/viewvideo.c +++ b/sw/airborne/modules/computer_vision/viewvideo.c @@ -41,40 +41,11 @@ #include // 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 -#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; -} diff --git a/sw/airborne/modules/computer_vision/viewvideo.h b/sw/airborne/modules/computer_vision/viewvideo.h index 577116b1b6..64d1dba6db 100644 --- a/sw/airborne/modules/computer_vision/viewvideo.h +++ b/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 */