BebopFlow: Add V4L2 interface and image functionality

cmake/configs/posix_bebop_default.cmake

@@ -108,4 +108,5 @@ set(config_df_driver_list
 	ak8963
 	bebop_bus
 	bebop_rangefinder
+	mt9v117
 )

posix-configs/bebop/px4.config

@@ -32,6 +32,7 @@ df_mpu6050_wrapper start -R 8
 df_ak8963_wrapper start -R 32
 df_bebop_rangefinder_wrapper start
 gps start -d /dev/ttyPA1
+bebop_flow start
 sensors start
 commander start
 ekf2 start

src/platforms/posix/drivers/bebop_flow/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2017 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -39,8 +39,10 @@ px4_add_module(
 	SRCS
 		bebop_flow.cpp
 		video_device.cpp
+		dump_pgm.cpp
 	DEPENDS
 		platforms__common
 		df_driver_framework
+		df_mt9v117
 	)
 # vim: set noet ft=cmake fenc=utf-8 ff=unix :

src/platforms/posix/drivers/bebop_flow/bebop_flow.cpp

@@ -1,7 +1,6 @@
-
 /****************************************************************************
  *
- *   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2017 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -32,7 +31,6 @@
  *
  ****************************************************************************/
 
-
 /**
  * @file bebop_flow.cpp
  *
@@ -47,49 +45,119 @@
 #include <px4_posix.h>
 
 #include "video_device.h"
+#include "dump_pgm.h"
+#include <mt9v117/MT9V117.hpp>
 
 extern "C" { __EXPORT int bebop_flow_main(int argc, char *argv[]); }
 
+using namespace DriverFramework;
+
 namespace bebop_flow
 {
+MT9V117 *image_sensor = nullptr;				 // I2C image sensor
 VideoDevice *g_dev = nullptr;            // interface to the video device
 volatile bool _task_should_exit = false; // flag indicating if bebop flow task should exit
 static bool _is_running = false;         // flag indicating if bebop flow  app is running
 static px4_task_t _task_handle = -1;     // handle to the task main thread
+volatile unsigned int _trigger = 0;			 // Number of images to write as pgm
 
-static char *dev_name = "/dev/video0";
+const char *dev_name = "/dev/video0";		 // V4L video device
 
 int start();
 int stop();
 int info();
+int trigger(int count);
 int clear_errors();
 void usage();
 void task_main(int argc, char *argv[]);
 
-
 void task_main(int argc, char *argv[])
 {
 	_is_running = true;
+	int ret = 0;
+	struct frame_data frame;
+	memset(&frame, 0, sizeof(frame));
+	uint32_t timeout_cnt = 0;
 
 	// Main loop
 	while (!_task_should_exit) {
 
+		ret = g_dev->get_frame(frame);
+
+		if (ret < 0) {
+			PX4_ERR("Get Frame failed");
+			continue;
+
+		} else if (ret == 1) {
+			// No image in buffer
+			usleep(1000);
+			++timeout_cnt;
+
+			if (timeout_cnt > 1000) {
+				PX4_WARN("No frames received for 1 sec");
+				timeout_cnt = 0;
+			}
+
+			continue;
+		}
+
+		timeout_cnt = 0;
+
+		// Write images into a file
+		if (_trigger > 0) {
+			PX4_INFO("Trigger camera");
+
+			dump_pgm(frame.data, frame.bytes, frame.seq, frame.timestamp);
+			--_trigger;
+		}
+
+		/***************************************************************
+		 *
+		 * Optical Flow computation
+		 *
+		 **************************************************************/
+
+		ret = g_dev->put_frame(frame);
+
+		if (ret < 0) {
+			PX4_ERR("Put Frame failed");
+		}
 	}
 
 	_is_running = false;
-
 }
 
 int start()
 {
-	g_dev = new VideoDevice();
+	if (_is_running) {
+		PX4_WARN("bebop_flow already running");
+		return -1;
+	}
+
+	// Prepare the I2C device
+	image_sensor = new MT9V117(IMAGE_DEVICE_PATH);
+
+	if (image_sensor == nullptr) {
+		PX4_ERR("failed instantiating image sensor object");
+		return -1;
+	}
+
+	int ret = image_sensor->start();
+
+	if (ret != 0) {
+		PX4_ERR("Image sensor start failed");
+		return ret;
+	}
+
+	// Start the video device
+	g_dev = new VideoDevice(dev_name, 6);
 
 	if (g_dev == nullptr) {
 		PX4_ERR("failed instantiating video device object");
 		return -1;
 	}
 
-	int ret = g_dev->start();
+	ret = g_dev->start();
 
 	if (ret != 0) {
 		PX4_ERR("Video device start failed");
@@ -105,7 +173,7 @@ int start()
 					  nullptr);
 
 	if (_task_handle < 0) {
-		warn("task start failed");
+		PX4_WARN("task start failed");
 		return -1;
 	}
 
@@ -123,13 +191,14 @@ int stop()
 	}
 
 	_task_handle = -1;
+	_task_should_exit = false;
+	_trigger = 0;
 
 	if (g_dev == nullptr) {
 		PX4_ERR("driver not running");
-		return 1;
+		return -1;
 	}
 
-	// Stop DF device
 	int ret = g_dev->stop();
 
 	if (ret != 0) {
@@ -137,8 +206,22 @@ int stop()
 		return ret;
 	}
 
+	if (image_sensor == nullptr) {
+		PX4_ERR("Image sensor not running");
+		return -1;
+	}
+
+	ret = image_sensor->stop();
+
+	if (ret != 0) {
+		PX4_ERR("Image sensor driver  could not be stopped");
+		return ret;
+	}
+
 	delete g_dev;
+	delete image_sensor;
 	g_dev = nullptr;
+	image_sensor = nullptr;
 	return 0;
 }
 
@@ -165,10 +248,22 @@ info()
 	return 0;
 }
 
+int trigger(int count)
+{
+	if (_is_running) {
+		_trigger = count;
+
+	} else {
+		PX4_WARN("bebop_flow is not running");
+	}
+
+	return OK;
+}
+
 void
 usage()
 {
-	PX4_INFO("Usage: bebop_flow 'start', 'info', 'stop'");
+	PX4_INFO("Usage: bebop_flow 'start', 'info', 'stop', 'trigger [-n #]'");
 }
 
 } /* bebop flow namespace*/
@@ -176,8 +271,24 @@ usage()
 int
 bebop_flow_main(int argc, char *argv[])
 {
+	int ch;
 	int ret = 0;
 	int myoptind = 1;
+	const char *myoptarg = NULL;
+	unsigned int trigger_count = 1;
+
+	/* jump over start/off/etc and look at options first */
+	while ((ch = px4_getopt(argc, argv, "n:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'n':
+			trigger_count = atoi(myoptarg);
+			break;
+
+		default:
+			bebop_flow::usage();
+			return 0;
+		}
+	}
 
 	if (argc <= 1) {
 		bebop_flow::usage();
@@ -186,7 +297,6 @@ bebop_flow_main(int argc, char *argv[])
 
 	const char *verb = argv[myoptind];
 
-
 	if (!strcmp(verb, "start")) {
 		ret = bebop_flow::start();
 	}
@@ -199,6 +309,10 @@ bebop_flow_main(int argc, char *argv[])
 		ret = bebop_flow::info();
 	}
 
+	else if (!strcmp(verb, "trigger")) {
+		ret = bebop_flow::trigger(trigger_count);
+	}
+
 	else {
 		bebop_flow::usage();
 		return 1;

src/platforms/posix/drivers/bebop_flow/dump_pgm.cpp

@@ -0,0 +1,88 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2017 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "dump_pgm.h"
+
+#include <string.h>
+#include <sys/fcntl.h>
+#include <sys/stat.h>
+
+#include <px4_posix.h>
+
+#define HRES_STR "320"
+#define VRES_STR "240"
+
+char pgm_header[] = "P5\n#99999999999999 usec \n" HRES_STR " " VRES_STR "\n255\n";
+char pgm_dumpname[] = "image";
+char pgm_path[] = "/home/root/images/";
+
+void dump_pgm(const void *data, uint32_t size, uint32_t seq, uint32_t timestamp)
+{
+	int written, total, fd;
+	struct stat sb;
+
+	// Check if dump directory exists
+	if (!(stat(pgm_path, &sb) == 0 && S_ISDIR(sb.st_mode))) {
+		PX4_ERR("Dump directory does not exist: %s", pgm_path);
+		PX4_ERR("No images are written!");
+		return;
+	}
+
+	// Construct the absolute filename
+	char file_path[100] = {0};
+	snprintf(file_path, sizeof(file_path), "%s%s%08u.pgm", pgm_path, pgm_dumpname, seq);
+	PX4_INFO("%s", file_path);
+
+	fd = open(file_path, O_WRONLY | O_NONBLOCK | O_CREAT, 00666);
+
+	if (fd < 0) {
+		PX4_ERR("Dump: Unable to open file");
+		return;
+	}
+
+	// Write pgm header
+	snprintf(&pgm_header[4], 15, "%014d", (int)timestamp);
+	written = write(fd, pgm_header, sizeof(pgm_header));
+
+	// Write image data
+	total = 0;
+
+	do {
+		written = write(fd, data, size);
+		total += written;
+	} while (total < size);
+
+	PX4_INFO("Wrote %d bytes\n", total);
+
+	close(fd);
+}

src/platforms/posix/drivers/bebop_flow/dump_pgm.h

@@ -0,0 +1,46 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2017 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+
+/**
+ * @file dump_pgm.h
+ *
+ * This is a simple implementation to write an image into a pgm file.
+ *
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+void dump_pgm(const void *data, uint32_t size, uint32_t seq, uint32_t timestamp);

src/platforms/posix/drivers/bebop_flow/video_device.cpp

@@ -1,26 +1,401 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2017 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file video_device.cpp
+ *
+ * Wrapper for a V4L2 device using the memory mapped interface.
+ *
+ */
+
 #include "video_device.h"
 
+#include <sys/stat.h>
+#include <sys/ioctl.h>
+#include <sys/mman.h>
+#include <errno.h>
+#include <string.h>
+
+#include <linux/videodev2.h>
+
+#include <px4_posix.h>
+
 int VideoDevice::start()
 {
-	return -1;
+	int ret = open_device();
+
+	if (ret < 0) {
+		return ret;
+	}
+
+	ret = init_device();
+
+	if (ret < 0) {
+		return ret;
+	}
+
+	return start_capturing();
 }
 
 int VideoDevice::stop()
 {
-	return -1;
+
+	int result = stop_capturing();
+
+	if (result < 0) {
+		PX4_ERR("Error stop stream");
+	}
+
+	// Unmap buffers
+	for (unsigned int i = 0; i < _n_buffers; ++i) {
+		result = munmap(_buffers[i].start, _buffers[i].length);
+
+		if (result < 0) {
+			PX4_ERR("Error: Unmapping buffer");
+		}
+	}
+
+	free(_buffers);
+
+	result = close_device();
+
+	if (result < 0) {
+		return result;
+	}
+
+	return OK;
 }
 
 int VideoDevice::print_info()
 {
-	return -1;
+	PX4_INFO("Device: %s", _dev_name);
+	return 0;
 }
 
 int VideoDevice::open_device()
 {
-	return -1;
+	struct stat st;
+
+	// Check if device is usable
+	int ret = stat(_dev_name, &st);
+
+	if (ret < 0) {
+		PX4_ERR("Cannot identify %s: %d", _dev_name, errno);
+		return -EIO;
+	}
+
+	if (!S_ISCHR(st.st_mode)) {
+		PX4_ERR("%s is no device", _dev_name);
+		return -EIO;
+	}
+
+	// Open V4L2 device nonblocking
+	_fd = open(_dev_name, O_RDWR | O_NONBLOCK);
+
+	if (_fd < 0) {
+		PX4_ERR("Unable to open %s", _dev_name);
+		return -EIO;
+	}
+
+	return OK;
 }
 
 int VideoDevice::close_device()
 {
-	return -1;
+	int ret = close(_fd);
+
+	if (ret < 0) {
+		PX4_ERR("Error closing %s", _dev_name);
+		return -EIO;
+	}
+
+	return OK;
+}
+
+int VideoDevice::init_device()
+{
+	struct v4l2_capability cap;
+	memset(&cap, 0, sizeof(cap));
+
+	int ret = ioctl(_fd, VIDIOC_QUERYCAP, &cap);
+
+	if (ret < 0) {
+		if (EINVAL == errno) {
+			PX4_ERR("No V4L2 device: %s", _dev_name);
+			return -EINVAL;
+
+		} else {
+			PX4_ERR("VIDIOC_QUERYCAP failed: %s", _dev_name);
+			return -1;
+		}
+	}
+
+	if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
+		PX4_ERR("Device is no video capture device: %s", _dev_name);
+		return -EIO;
+	}
+
+	ret = init_crop();
+
+	if (ret < 0) {
+		PX4_ERR("Error when setting image crop");
+		return -1;
+	}
+
+	return init_buffers();
+}
+
+int VideoDevice::init_crop()
+{
+	struct v4l2_cropcap cropcap;
+	struct v4l2_crop crop;
+
+	memset(&cropcap, 0, sizeof(cropcap));
+	memset(&crop, 0, sizeof(crop));
+
+	cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	int ret = ioctl(_fd, VIDIOC_CROPCAP, &cropcap);
+
+	if (ret < 0) {
+		PX4_WARN("CROPCAP failed");
+	}
+
+	crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	crop.c = cropcap.defrect; // reset to default
+	ret = ioctl(_fd, VIDIOC_S_CROP, &crop);
+
+	if (ret < 0) {
+		PX4_WARN("S_CROP failed");
+	}
+
+	return init_format();
+}
+
+int VideoDevice::init_format()
+{
+	usleep(10000);
+	struct v4l2_format fmt;
+
+	memset(&fmt, 0, sizeof(fmt));
+
+	fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	fmt.fmt.pix.width = VIDEO_DEVICE_IMAGE_WIDTH;
+	fmt.fmt.pix.height = VIDEO_DEVICE_IMAGE_HEIGHT;
+	fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_NV12;
+	fmt.fmt.pix.colorspace = V4L2_COLORSPACE_REC709;
+
+	int ret = ioctl(_fd, VIDIOC_S_FMT, &fmt);
+
+	if (ret < 0) {
+		PX4_ERR("Unable to set format");
+		return -1;
+	}
+
+	const char *format_string;
+
+	switch (fmt.fmt.pix.pixelformat) {
+	case V4L2_PIX_FMT_YUYV:
+		format_string = "YUYV";
+		break;
+
+	case V4L2_PIX_FMT_YVYU:
+		format_string = "YVYU";
+		break;
+
+	case V4L2_PIX_FMT_NV12:
+		format_string = "NV12";
+		break;
+
+	default:
+		format_string = "None";
+	}
+
+	PX4_INFO("Set image format: %ux%u\n    Format: %s\n    Size: %u",
+		 fmt.fmt.pix.width,
+		 fmt.fmt.pix.height,
+		 format_string,
+		 fmt.fmt.pix.sizeimage);
+
+	return OK;
+}
+
+int VideoDevice::init_buffers()
+{
+	struct v4l2_requestbuffers req;
+
+	memset(&req, 0, sizeof(req));
+
+	req.count = _n_buffers;
+	req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	req.memory = V4L2_MEMORY_MMAP;
+
+	int ret = ioctl(_fd, VIDIOC_REQBUFS, &req);
+
+	if (ret < 0) {
+		PX4_ERR("Unable to request buffers: %s", _dev_name);
+		return -1;
+	}
+
+	_buffers = (struct buffer *) malloc(_n_buffers * sizeof(_buffers[0]));
+
+	if (_buffers == nullptr) {
+		PX4_ERR("Unable to allocate buffers");
+		return -1;
+	}
+
+	for (unsigned int i = 0; i < _n_buffers; ++i) {
+		struct v4l2_buffer buf;
+		memset(&buf, 0, sizeof(buf));
+
+		buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+		buf.memory = V4L2_MEMORY_MMAP;
+		buf.index = i;
+
+		ret = ioctl(_fd, VIDIOC_QUERYBUF, &buf);
+
+		if (ret < 0) {
+			PX4_ERR("Error QUERYBUF");
+			return -1;
+		}
+
+		_buffers[i].length = buf.length;
+		_buffers[i].start = mmap(nullptr, buf.length, PROT_READ | PROT_WRITE,
+					 MAP_SHARED, _fd, buf.m.offset);
+
+		if (_buffers[i].start == MAP_FAILED) {
+			PX4_ERR("Out of memory");
+			return -1;
+		}
+	}
+
+	return OK;
+}
+
+int VideoDevice::start_capturing()
+{
+	for (unsigned int i = 0; i < _n_buffers; ++i) {
+		struct v4l2_buffer buf;
+
+		memset(&buf, 0, sizeof(buf));
+
+		buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+		buf.memory = V4L2_MEMORY_MMAP;
+		buf.index = i;
+
+		int ret = ioctl(_fd, VIDIOC_QBUF, &buf);
+
+		if (ret < 0) {
+			PX4_ERR("Unable to queue buffer: %d", i);
+			return -1;
+		}
+	}
+
+	enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	int ret = ioctl(_fd, VIDIOC_STREAMON, &type);
+
+	if (ret < 0) {
+		PX4_ERR("Unable to start streaming");
+		return -1;
+	}
+
+	PX4_INFO("Streaming started: %s", _dev_name);
+	return OK;
+}
+
+int VideoDevice::stop_capturing()
+{
+	enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	int ret = ioctl(_fd, VIDIOC_STREAMOFF, &type);
+
+	if (ret < 0) {
+		PX4_ERR("Unable to stop streaming");
+		return -1;
+	}
+
+	PX4_INFO("Streaming stopped: %s", _dev_name);
+	return OK;
+}
+
+int VideoDevice::get_frame(struct frame_data &frame)
+{
+	struct v4l2_buffer buf;
+	memset(&buf, 0, sizeof(buf));
+
+	buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	buf.memory = V4L2_MEMORY_MMAP;
+
+	int ret = ioctl(_fd, VIDIOC_DQBUF, &buf);
+
+	if (ret < 0) {
+		if (errno == EAGAIN) {
+			//PX4_INFO("No frame ready");
+			return 1;
+
+		} else if (errno == EIO) {
+			PX4_INFO("EIO");
+			return 1;
+
+		} else {
+			PX4_ERR("Buffer deque error");
+			return -1;
+		}
+	}
+
+	frame.data = _buffers[buf.index].start;
+	frame.seq = buf.sequence;
+	frame.timestamp = buf.timestamp.tv_sec * 1000000 + buf.timestamp.tv_usec;
+	frame.bytes = buf.bytesused;
+	frame.index = buf.index;
+
+	return 0;
+}
+
+int VideoDevice::put_frame(struct frame_data &frame)
+{
+	struct v4l2_buffer buf;
+	memset(&buf, 0, sizeof(buf));
+
+	buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	buf.memory = V4L2_MEMORY_MMAP;
+	buf.index = frame.index;
+	buf.length = _buffers[frame.index].length;
+
+	int ret = ioctl(_fd, VIDIOC_QBUF, &buf);
+
+	if (ret < 0) {
+		PX4_ERR("Buffer deque error");
+		return -1;
+	}
+
+	return OK;
 }

src/platforms/posix/drivers/bebop_flow/video_device.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2017 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -33,11 +33,30 @@
 
 #pragma once
 
+#include <stdint.h>
+#include <unistd.h>
+
+#define VIDEO_DEVICE_IMAGE_WIDTH 320
+#define VIDEO_DEVICE_IMAGE_HEIGHT 240
+
+struct frame_data {
+	uint32_t timestamp;
+	uint32_t seq;
+	uint32_t bytes;
+	uint32_t index;
+	void *data;
+};
+
+struct buffer {
+	void *start;
+	size_t length;
+};
+
 class VideoDevice
 {
 public:
-	VideoDevice(const char *dev_name) :
+	VideoDevice(char const *dev_name, uint32_t n_buffers) :
-		_dev_name(dev_name) {};
+		_fd(-1), _dev_name(dev_name), _n_buffers(n_buffers), _buffers(nullptr) {};
 
 	~VideoDevice() = default;
 
@@ -50,9 +69,25 @@ public:
 	/// Print various infos
 	int print_info();
 
+	/// Non-blocking call to fetch an image. Returns 0 if the images was read, -1 on error
+	/// and 1 no new image is ready.
+	int get_frame(struct frame_data &frame);
+
+	/// Return a frame when the data is not needed any more.
+	int put_frame(struct frame_data &frame);
+
 private:
-	char *_dev_name;
+	int _fd;
+	const char *_dev_name;
+	uint32_t _n_buffers;
+	struct buffer *_buffers;
 
 	int open_device();
 	int close_device();
+	int init_device();
+	int init_buffers();
+	int init_crop();
+	int init_format();
+	int start_capturing();
+	int stop_capturing();
 };