diff --git a/conf/airframes/BR/bebop_default.xml b/conf/airframes/BR/bebop_default.xml index f260940a64d..bfec37fcd26 100644 --- a/conf/airframes/BR/bebop_default.xml +++ b/conf/airframes/BR/bebop_default.xml @@ -30,16 +30,18 @@ - - - - + + - + + + + + diff --git a/conf/airframes/BR/bebop_indi.xml b/conf/airframes/BR/bebop_indi.xml index 58200e3ef33..cbbfa04ba34 100644 --- a/conf/airframes/BR/bebop_indi.xml +++ b/conf/airframes/BR/bebop_indi.xml @@ -35,18 +35,18 @@ - + - - - - + + + + @@ -206,7 +206,6 @@
- @@ -214,11 +213,6 @@
-
- - -
-
diff --git a/conf/airframes/ENAC/quadrotor/ard2_base_digit.xml b/conf/airframes/ENAC/quadrotor/ard2_base_digit.xml index e8d4b5c50d4..80de02ddac9 100644 --- a/conf/airframes/ENAC/quadrotor/ard2_base_digit.xml +++ b/conf/airframes/ENAC/quadrotor/ard2_base_digit.xml @@ -36,6 +36,7 @@ + diff --git a/conf/airframes/ENAC/quadrotor/ard2_base_vision.xml b/conf/airframes/ENAC/quadrotor/ard2_base_vision.xml index acf6d6cc479..77e4f376586 100644 --- a/conf/airframes/ENAC/quadrotor/ard2_base_vision.xml +++ b/conf/airframes/ENAC/quadrotor/ard2_base_vision.xml @@ -36,6 +36,7 @@ + diff --git a/conf/airframes/TUDelft/conf.xml b/conf/airframes/TUDelft/conf.xml index c3fb52c3038..649cde7bfd3 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/airframes/examples/bebop.xml b/conf/airframes/examples/bebop.xml index 6b99e455632..4aa71ffe3c1 100644 --- a/conf/airframes/examples/bebop.xml +++ b/conf/airframes/examples/bebop.xml @@ -7,6 +7,7 @@ + @@ -30,16 +31,18 @@ - + - - + + + + diff --git a/conf/boards/pc.makefile b/conf/boards/pc.makefile index ab42d91bae5..b4e056ff260 100644 --- a/conf/boards/pc.makefile +++ b/conf/boards/pc.makefile @@ -12,3 +12,5 @@ BOARD_CFG = \"boards/pc_sim.h\" $(TARGET).ARCHDIR = $(ARCH) +MODEM_HOST ?= 127.0.0.1 + diff --git a/conf/modules/digital_cam_video.xml b/conf/modules/digital_cam_video.xml index 803a66fa223..0d20ec88e6f 100644 --- a/conf/modules/digital_cam_video.xml +++ b/conf/modules/digital_cam_video.xml @@ -34,7 +34,7 @@ - video_rtp_stream + video_thread digital_cam,digital_cam_servo,digital_cam_i2c,digital_cam_uart diff --git a/conf/modules/video_exif.xml b/conf/modules/video_exif.xml index ad8c2bdcb61..5820b32cca0 100644 --- a/conf/modules/video_exif.xml +++ b/conf/modules/video_exif.xml @@ -10,6 +10,8 @@ + video_thread +
diff --git a/conf/modules/video_rtp_stream.xml b/conf/modules/video_rtp_stream.xml index bef813a870d..4e511321d14 100644 --- a/conf/modules/video_rtp_stream.xml +++ b/conf/modules/video_rtp_stream.xml @@ -8,43 +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
- - + - - - @@ -55,17 +44,11 @@ VIEWVID_CFLAGS = -DVIEWVIDEO_HOST=$(VIEWVIDEO_HOST) -DVIEWVIDEO_PORT_OUT=$(VIEWVIDEO_PORT_OUT) ifeq ($(VIEWVIDEO_USE_NC),) ap.CFLAGS += $(VIEWVID_CFLAGS) -DVIEWVIDEO_BROADCAST=$(VIEWVIDEO_BROADCAST) + nps.CFLAGS += $(VIEWVID_CFLAGS) -DVIEWVIDEO_BROADCAST=FALSE else - ap.CFLAGS += $(VIEWVID_CFLAGS) -DVIEWVIDEO_USE_NC + $(TARGET).CFLAGS += $(VIEWVID_CFLAGS) -DVIEWVIDEO_USE_NC endif - - - - - - - diff --git a/conf/modules/video_thread.xml b/conf/modules/video_thread.xml new file mode 100644 index 00000000000..85b58655717 --- /dev/null +++ b/conf/modules/video_thread.xml @@ -0,0 +1,56 @@ + + + + + + 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/cv.c b/sw/airborne/modules/computer_vision/cv.c new file mode 100644 index 00000000000..a18ad02ced0 --- /dev/null +++ b/sw/airborne/modules/computer_vision/cv.c @@ -0,0 +1,48 @@ +/* + * 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/cv.c + * + * Computer vision framework for onboard processing + */ + +#include "cv.h" + +#define MAX_CV_FUNC 10 + +int cv_func_cnt = 0; +cvFunction cv_func[MAX_CV_FUNC]; + +void cv_add(cvFunction func) +{ + if (cv_func_cnt < (MAX_CV_FUNC - 1)) { + cv_func[cv_func_cnt] = func; + cv_func_cnt++; + } +} + +void cv_run(struct image_t *img) +{ + for (int i = 0; i < cv_func_cnt; i++) { + cv_func[i](img); + } +} diff --git a/sw/airborne/modules/computer_vision/cv.h b/sw/airborne/modules/computer_vision/cv.h new file mode 100644 index 00000000000..4ac6d344710 --- /dev/null +++ b/sw/airborne/modules/computer_vision/cv.h @@ -0,0 +1,40 @@ +/* + * 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/cv.h + * + * Computer vision framework for onboard processing + */ + + +#ifndef CV_H_ +#define CV_H_ + +#include "std.h" +#include "lib/vision/image.h" + +typedef bool_t (*cvFunction)(struct image_t *img); + +extern void cv_add(cvFunction func); +extern void cv_run(struct image_t *img); + +#endif /* CV_H_ */ 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 00000000000..196e641eeab --- /dev/null +++ b/sw/airborne/modules/computer_vision/video_thread.c @@ -0,0 +1,254 @@ +/* +* 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.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 00000000000..818f2f257d6 --- /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/video_thread_nps.c b/sw/airborne/modules/computer_vision/video_thread_nps.c new file mode 100644 index 00000000000..042bf5c3f8f --- /dev/null +++ b/sw/airborne/modules/computer_vision/video_thread_nps.c @@ -0,0 +1,77 @@ +/* + * 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, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * Dummy C implementation for simulation + * The V4L2 could also work in simulation, but must be adapted a bit. + */ + +// Own header +#include "video_thread.h" +#include "cv.h" + +// Initialize the video_thread structure with the defaults +struct video_thread_t video_thread = { + .is_running = FALSE, + .fps = 30, + .take_shot = FALSE, + .shot_number = 0 +}; + +// All dummy functions +void video_thread_init(void) {} +void video_thread_periodic(void) +{ + struct image_t img; + image_create(&img, 320, 240, IMAGE_YUV422); + int i, j; + uint8_t u, v; + +#ifdef SMARTUAV_SIMULATOR + SMARTUAV_IMPORT(&img); +#else + if (video_thread.is_running) { + u = 0; + v = 255; + } else { + u = 255; + v = 0; + } + uint8_t *p = (uint8_t *) img.buf; + for (j = 0; j < img.h; j++) { + for (i = 0; i < img.w; i += 2) { + *p++ = u; + *p++ = j; + *p++ = v; + *p++ = j; + } + } + video_thread.is_running = ! video_thread.is_running; +#endif + + cv_run(&img); + + image_free(&img); +} + +void video_thread_start(void) {} +void video_thread_stop(void) {} +void video_thread_take_shot(bool_t take __attribute__((unused))) {} diff --git a/sw/airborne/modules/computer_vision/viewvideo.c b/sw/airborne/modules/computer_vision/viewvideo.c index d95fa1e93dc..8bba9ec4533 100644 --- a/sw/airborne/modules/computer_vision/viewvideo.c +++ b/sw/airborne/modules/computer_vision/viewvideo.c @@ -31,6 +31,7 @@ // Own header #include "modules/computer_vision/viewvideo.h" +#include "modules/computer_vision/cv.h" #include #include @@ -40,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 @@ -92,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 @@ -126,117 +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; - } + 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 @@ -284,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; } /** @@ -299,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) - - // 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 + 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 device - viewvideo.dev = v4l2_init(STRINGIFY(VIEWVIDEO_DEVICE), VIEWVIDEO_DEVICE_SIZE, VIEWVIDEO_DEVICE_BUFFERS, V4L2_PIX_FMT_UYVY); - if (viewvideo.dev == NULL) { - printf("[viewvideo] Could not initialize the %s V4L2 device.\n", STRINGIFY(VIEWVIDEO_DEVICE)); - return; - } + cv_add(viewvideo_function); - // 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 @@ -352,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 577116b1b6e..64d1dba6db0 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 */ diff --git a/sw/airborne/modules/computer_vision/viewvideo_nps.c b/sw/airborne/modules/computer_vision/viewvideo_nps.c index f2f91a3ea33..ad6acaa3daa 100644 --- a/sw/airborne/modules/computer_vision/viewvideo_nps.c +++ b/sw/airborne/modules/computer_vision/viewvideo_nps.c @@ -32,9 +32,6 @@ struct viewvideo_t viewvideo = { .is_streaming = FALSE, .downsize_factor = 1, .quality_factor = 99, - .fps = 30, - .take_shot = FALSE, - .shot_number = 0 }; // All dummy functions