diff --git a/conf/airframes/BR/bebop_default.xml b/conf/airframes/BR/bebop_default.xml
index f825a0e4df..e5ee58d688 100644
--- a/conf/airframes/BR/bebop_default.xml
+++ b/conf/airframes/BR/bebop_default.xml
@@ -32,11 +32,8 @@
+
-
-
-
-
diff --git a/conf/airframes/BR/bebop_indi.xml b/conf/airframes/BR/bebop_indi.xml
index d4f1303d35..ecf72eceb6 100644
--- a/conf/airframes/BR/bebop_indi.xml
+++ b/conf/airframes/BR/bebop_indi.xml
@@ -38,10 +38,7 @@
-
-
-
-
+
diff --git a/conf/airframes/BR/bebop_indi_frog.xml b/conf/airframes/BR/bebop_indi_frog.xml
index 9540dd272f..aa17310dd4 100644
--- a/conf/airframes/BR/bebop_indi_frog.xml
+++ b/conf/airframes/BR/bebop_indi_frog.xml
@@ -37,11 +37,8 @@
+
-
-
-
-
diff --git a/conf/airframes/BR/bebop_indi_frog_flip.xml b/conf/airframes/BR/bebop_indi_frog_flip.xml
index 05bdfe4174..cfcf01e939 100644
--- a/conf/airframes/BR/bebop_indi_frog_flip.xml
+++ b/conf/airframes/BR/bebop_indi_frog_flip.xml
@@ -37,11 +37,8 @@
+
-
-
-
-
diff --git a/conf/airframes/CDW/bebop.xml b/conf/airframes/CDW/bebop.xml
index b7bc238e76..aa78a38ee9 100644
--- a/conf/airframes/CDW/bebop.xml
+++ b/conf/airframes/CDW/bebop.xml
@@ -38,10 +38,7 @@
-
-
-
-
+
diff --git a/conf/airframes/ENAC/quadrotor/ard2_base_digit.xml b/conf/airframes/ENAC/quadrotor/ard2_base_digit.xml
index 80de02ddac..ae0fd8395e 100644
--- a/conf/airframes/ENAC/quadrotor/ard2_base_digit.xml
+++ b/conf/airframes/ENAC/quadrotor/ard2_base_digit.xml
@@ -36,7 +36,9 @@
-
+
+
+
diff --git a/conf/airframes/ENAC/quadrotor/ard2_base_vision.xml b/conf/airframes/ENAC/quadrotor/ard2_base_vision.xml
index 77e4f37658..84bac7c4b1 100644
--- a/conf/airframes/ENAC/quadrotor/ard2_base_vision.xml
+++ b/conf/airframes/ENAC/quadrotor/ard2_base_vision.xml
@@ -36,7 +36,9 @@
-
+
+
+
diff --git a/conf/airframes/examples/ardrone2.xml b/conf/airframes/examples/ardrone2.xml
index f901ed2b18..0a6c2e9ec0 100644
--- a/conf/airframes/examples/ardrone2.xml
+++ b/conf/airframes/examples/ardrone2.xml
@@ -32,7 +32,11 @@
-
+
diff --git a/conf/airframes/examples/bebop.xml b/conf/airframes/examples/bebop.xml
index e988d250be..cb1924ad22 100644
--- a/conf/airframes/examples/bebop.xml
+++ b/conf/airframes/examples/bebop.xml
@@ -33,10 +33,7 @@
-
-
-
-
+
diff --git a/conf/modules/video_thread.xml b/conf/modules/video_thread.xml
index af08272795..579335e753 100644
--- a/conf/modules/video_thread.xml
+++ b/conf/modules/video_thread.xml
@@ -3,16 +3,20 @@
- Video streaming for Linux devices
+ Read video in a thread.
+ Only for Linux devices.
+ To be used in other modules for further processing (e.g. opticflow, QR code, streaming).
- - Sends a RTP/UDP stream of the camera
- Possibility to save an image(shot) on the internal memory (JPEG, full size, best quality)
-
-
-
+
+
+
+
+
+
@@ -20,6 +24,7 @@
+
diff --git a/sw/airborne/arch/linux/mcu_periph/sys_time_arch.h b/sw/airborne/arch/linux/mcu_periph/sys_time_arch.h
index 0828993893..ad932315d0 100644
--- a/sw/airborne/arch/linux/mcu_periph/sys_time_arch.h
+++ b/sw/airborne/arch/linux/mcu_periph/sys_time_arch.h
@@ -57,4 +57,17 @@ static inline void sys_time_usleep(uint32_t us)
usleep(us);
}
+/** elapsed time in microsecs between two timespecs */
+static inline unsigned int sys_time_elapsed_us(struct timespec *prev, struct timespec *now)
+{
+ time_t d_sec = now->tv_sec - prev->tv_sec;
+ long d_nsec = now->tv_nsec - prev->tv_nsec;
+ /* wrap if negative nanoseconds */
+ if (d_nsec < 0) {
+ d_sec -= 1;
+ d_nsec += 1000000000L;
+ }
+ return d_sec * 1000000 + d_nsec / 1000;
+}
+
#endif /* SYS_TIME_ARCH_H */
diff --git a/sw/airborne/boards/ardrone/board.c b/sw/airborne/boards/ardrone/board.c
index cf802acc58..9602d7556d 100644
--- a/sw/airborne/boards/ardrone/board.c
+++ b/sw/airborne/boards/ardrone/board.c
@@ -34,6 +34,29 @@
#warning No battery voltage measurement available! Please add to your modules.
#endif
+#include "peripherals/video_device.h"
+
+struct video_config_t front_camera = {
+ .w = 1280,
+ .h = 720,
+ .dev_name = "/dev/video1",
+ .subdev_name = NULL,
+ .format = V4L2_PIX_FMT_UYVY,
+ .buf_cnt = 10,
+ .filters = 0
+};
+
+struct video_config_t bottom_camera = {
+ .w = 320,
+ .h = 240,
+ .dev_name = "/dev/video2",
+ .subdev_name = NULL,
+ .format = V4L2_PIX_FMT_UYVY,
+ .buf_cnt = 10,
+ .filters = 0
+};
+
+
void board_init(void)
{
// First we try to kill the program.elf and its respawner if it is running
diff --git a/sw/airborne/boards/ardrone2.h b/sw/airborne/boards/ardrone2.h
index db17a0ac11..19b6c8f2bb 100644
--- a/sw/airborne/boards/ardrone2.h
+++ b/sw/airborne/boards/ardrone2.h
@@ -3,10 +3,16 @@
#define BOARD_ARDRONE2
+#include "peripherals/video_device.h"
+
#ifndef UART1_DEV
#define UART1_DEV /dev/ttyUSB0
#endif
+/* Cameras */
+extern struct video_config_t bottom_camera;
+extern struct video_config_t front_camera;
+
/* Default actuators driver */
#define DEFAULT_ACTUATORS "boards/ardrone/actuators.h"
#define ActuatorDefaultSet(_x,_y) ActuatorArdroneSet(_x,_y)
diff --git a/sw/airborne/boards/bebop.h b/sw/airborne/boards/bebop.h
index 04158f6468..52bcfba76e 100644
--- a/sw/airborne/boards/bebop.h
+++ b/sw/airborne/boards/bebop.h
@@ -25,6 +25,8 @@
#define BOARD_BEBOP
+#include "peripherals/video_device.h"
+
/** uart connected to GPS internally */
#define UART1_DEV /dev/ttyPA1
@@ -34,6 +36,9 @@
#define ActuatorsDefaultInit() ActuatorsBebopInit()
#define ActuatorsDefaultCommit() ActuatorsBebopCommit()
+/* Cameras */
+extern struct video_config_t bottom_camera;
+extern struct video_config_t front_camera;
/* by default activate onboard baro */
#ifndef USE_BARO_BOARD
diff --git a/sw/airborne/boards/bebop/video.c b/sw/airborne/boards/bebop/video.c
index 4863542011..02ce1de85e 100644
--- a/sw/airborne/boards/bebop/video.c
+++ b/sw/airborne/boards/bebop/video.c
@@ -37,6 +37,28 @@
#include
#include
+#include "boards/bebop.h"
+
+struct video_config_t bottom_camera = {
+ .w = 640,
+ .h = 480,
+ .dev_name = "/dev/video0",
+ .subdev_name = NULL,
+ .format = V4L2_PIX_FMT_UYVY,
+ .buf_cnt = 60,
+ .filters = 0
+};
+
+struct video_config_t front_camera = {
+ .w = 1408,
+ .h = 2112,
+ .dev_name = "/dev/video1",
+ .subdev_name = "/dev/v4l-subdev1",
+ .format = V4L2_PIX_FMT_SGBRG10,
+ .buf_cnt = 10,
+ .filters = VIDEO_FILTER_DEBAYER
+};
+
static bool_t write_reg(int fd, char *addr_val, uint8_t cnt)
{
char resp[cnt - 2];
diff --git a/sw/airborne/modules/computer_vision/lib/v4l/v4l2.h b/sw/airborne/modules/computer_vision/lib/v4l/v4l2.h
index a42fb363e6..3d211a9448 100644
--- a/sw/airborne/modules/computer_vision/lib/v4l/v4l2.h
+++ b/sw/airborne/modules/computer_vision/lib/v4l/v4l2.h
@@ -33,7 +33,7 @@
#include
#include "std.h"
-#include "lib/vision/image.h"
+#include "modules/computer_vision/lib/vision/image.h"
#define V4L2_IMG_NONE 255 ///< There currently no image available
diff --git a/sw/airborne/modules/computer_vision/lib/vision/image.c b/sw/airborne/modules/computer_vision/lib/vision/image.c
index 532d8947b7..c1ac85ab1e 100644
--- a/sw/airborne/modules/computer_vision/lib/vision/image.c
+++ b/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;
+ }
}
/**
diff --git a/sw/airborne/modules/computer_vision/video_thread.c b/sw/airborne/modules/computer_vision/video_thread.c
index a200087991..80b9d11f4d 100644
--- a/sw/airborne/modules/computer_vision/video_thread.c
+++ b/sw/airborne/modules/computer_vision/video_thread.c
@@ -37,7 +37,14 @@
// 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"
+
+#include "mcu_periph/sys_time.h"
+
+// include board for bottom_camera and front_camera on ARDrone2 and Bebop
+#include BOARD_CONFIG
#if JPEG_WITH_EXIF_HEADER
#include "lib/exif/exif_module.h"
@@ -47,25 +54,32 @@
#include
#include "rt_priority.h"
-// The video device
-#ifndef VIDEO_THREAD_DEVICE
-#define VIDEO_THREAD_DEVICE /dev/video1
-#endif
-PRINT_CONFIG_VAR(VIDEO_THREAD_DEVICE)
-
-// The video device size (width, height)
-#ifndef VIDEO_THREAD_DEVICE_SIZE
-#define VIDEO_THREAD_DEVICE_SIZE 1280,720
-#endif
-#define __SIZE_HELPER(x, y) #x", "#y
-#define _SIZE_HELPER(x) __SIZE_HELPER(x)
-PRINT_CONFIG_MSG("VIDEO_THREAD_DEVICE_SIZE = " _SIZE_HELPER(VIDEO_THREAD_DEVICE_SIZE))
-
+/// The camera video config (usually bottom_camera or front_camera)
+#ifndef VIDEO_THREAD_CAMERA
+#warning "Are you sure you don't want to use the bottom_camera or front_camera?"
// The video device buffers (the amount of V4L2 buffers)
#ifndef VIDEO_THREAD_DEVICE_BUFFERS
#define VIDEO_THREAD_DEVICE_BUFFERS 10
#endif
PRINT_CONFIG_VAR(VIDEO_THREAD_DEVICE_BUFFERS)
+#ifndef VIDEO_THREAD_SUBDEV
+#define VIDEO_THREAD_SUBDEV NULL
+#endif
+#ifndef VIDEO_THREAD_FILTERS
+#define VIDEO_THREAD_FILTERS 0
+#endif
+struct video_config_t custom_camera = {
+ .w = VIDEO_THREAD_VIDEO_WIDTH,
+ .h = VIDEO_THREAD_VIDEO_HEIGHT,
+ .dev_name = STRINGIFY(VIDEO_THREAD_DEVICE),
+ .subdev_name = VIDEO_THREAD_SUBDEV,
+ .buf_cnt = VIDEO_THREAD_DEVICE_BUFFERS,
+ .filters = VIDEO_THREAD_FILTERS
+};
+#define VIDEO_THREAD_CAMERA custom_camera
+#endif
+PRINT_CONFIG_VAR(VIDEO_THREAD_CAMERA)
+
// Frames Per Seconds
#ifndef VIDEO_THREAD_FPS
@@ -91,12 +105,61 @@ 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 __attribute__((unused)))
+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)) {
printf("[video_thread-thread] Could not start capture of %s.\n", video_thread.dev->name);
@@ -107,67 +170,57 @@ static void *video_thread_function(void *data __attribute__((unused)))
set_nice_level(10);
// Initialize timing
- uint32_t microsleep = (uint32_t)(1000000. / (float)video_thread.fps);
- struct timeval last_time;
- gettimeofday(&last_time, NULL);
+ struct timespec time_now;
+ struct timespec time_prev;
+ clock_gettime(CLOCK_MONOTONIC, &time_prev);
// 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;
+
+ // get time in us since last run
+ clock_gettime(CLOCK_MONOTONIC, &time_now);
+ unsigned int dt_us = sys_time_elapsed_us(&time_prev, &time_now);
+ time_prev = time_now;
+
+ // sleep remaining time to limit to specified fps
+ uint32_t fps_period_us = (uint32_t)(1000000. / (float)video_thread.fps);
+ if (dt_us < fps_period_us) {
+ usleep(fps_period_us - dt_us);
+ }
// Wait for a new frame (blocking)
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;
}
@@ -176,21 +229,21 @@ static void *video_thread_function(void *data __attribute__((unused)))
*/
void video_thread_init(void)
{
-#ifdef VIDEO_THREAD_SUBDEV
- PRINT_CONFIG_MSG("[video_thread] Configuring a subdevice!")
- PRINT_CONFIG_VAR(VIDEO_THREAD_SUBDEV)
+ struct video_config_t *vid = (struct video_config_t *)&(VIDEO_THREAD_CAMERA);
- // Initialize the V4L2 subdevice (TODO: fix hardcoded path, which and code)
- if (!v4l2_init_subdev(STRINGIFY(VIDEO_THREAD_SUBDEV), 0, 1, V4L2_MBUS_FMT_UYVY8_2X8, VIDEO_THREAD_DEVICE_SIZE)) {
- printf("[video_thread] Could not initialize the %s subdevice.\n", STRINGIFY(VIDEO_THREAD_SUBDEV));
- return;
+ // Initialize the V4L2 subdevice if needed
+ if (vid->subdev_name != NULL) {
+ // 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;
+ }
}
-#endif
// Initialize the V4L2 device
- video_thread.dev = v4l2_init(STRINGIFY(VIDEO_THREAD_DEVICE), VIDEO_THREAD_DEVICE_SIZE, VIDEO_THREAD_DEVICE_BUFFERS, V4L2_PIX_FMT_UYVY);
+ 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", STRINGIFY(VIDEO_THREAD_DEVICE));
+ printf("[video_thread] Could not initialize the %s V4L2 device.\n", vid->dev_name);
return;
}
@@ -215,7 +268,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;
}
diff --git a/sw/airborne/peripherals/video_device.h b/sw/airborne/peripherals/video_device.h
new file mode 100644
index 0000000000..965d0a0b0b
--- /dev/null
+++ b/sw/airborne/peripherals/video_device.h
@@ -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 peripherals/video_device.h
+ * @brief v4l2 video device settings interface
+ * Works on Linux platforms
+ */
+
+#ifndef VIDEO_DEVICE_H
+#define VIDEO_DEVICE_H
+
+#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
+ int h; ///< Height
+ char* dev_name; ///< path to device
+ char* subdev_name; ///< path to sub device
+ uint32_t format; ///< Video format
+ uint8_t buf_cnt; ///< Amount of V4L2 video device buffers
+ uint8_t filters; ///< filters to use (bitfield with VIDEO_FILTER_x)
+};
+
+
+#endif /* VIDEO_DEVICE_H */