From 7aea2ca0308295b2e2dca30f758a17b6ef34e759 Mon Sep 17 00:00:00 2001 From: Michael Schaeuble Date: Tue, 10 Jan 2017 17:03:16 +0100 Subject: [PATCH] BebopFlow: Add V4L2 interface and image functionality --- cmake/configs/posix_bebop_default.cmake | 1 + posix-configs/bebop/px4.config | 1 + .../posix/drivers/bebop_flow/CMakeLists.txt | 4 +- .../posix/drivers/bebop_flow/bebop_flow.cpp | 140 ++++++- .../posix/drivers/bebop_flow/dump_pgm.cpp | 88 ++++ .../posix/drivers/bebop_flow/dump_pgm.h | 46 +++ .../posix/drivers/bebop_flow/video_device.cpp | 385 +++++++++++++++++- .../posix/drivers/bebop_flow/video_device.h | 43 +- 8 files changed, 685 insertions(+), 23 deletions(-) create mode 100644 src/platforms/posix/drivers/bebop_flow/dump_pgm.cpp create mode 100644 src/platforms/posix/drivers/bebop_flow/dump_pgm.h diff --git a/cmake/configs/posix_bebop_default.cmake b/cmake/configs/posix_bebop_default.cmake index 8b3b897dc9..7b2b5f4f0d 100644 --- a/cmake/configs/posix_bebop_default.cmake +++ b/cmake/configs/posix_bebop_default.cmake @@ -108,4 +108,5 @@ set(config_df_driver_list ak8963 bebop_bus bebop_rangefinder + mt9v117 ) diff --git a/posix-configs/bebop/px4.config b/posix-configs/bebop/px4.config index 146936cbfc..c6f1ae0b99 100644 --- a/posix-configs/bebop/px4.config +++ b/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 diff --git a/src/platforms/posix/drivers/bebop_flow/CMakeLists.txt b/src/platforms/posix/drivers/bebop_flow/CMakeLists.txt index b73d689039..eb2c8ebaa0 100644 --- a/src/platforms/posix/drivers/bebop_flow/CMakeLists.txt +++ b/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 : diff --git a/src/platforms/posix/drivers/bebop_flow/bebop_flow.cpp b/src/platforms/posix/drivers/bebop_flow/bebop_flow.cpp index 1eab0c8779..e99b75c846 100644 --- a/src/platforms/posix/drivers/bebop_flow/bebop_flow.cpp +++ b/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 #include "video_device.h" +#include "dump_pgm.h" +#include 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; diff --git a/src/platforms/posix/drivers/bebop_flow/dump_pgm.cpp b/src/platforms/posix/drivers/bebop_flow/dump_pgm.cpp new file mode 100644 index 0000000000..ad7ea506c5 --- /dev/null +++ b/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 +#include +#include + +#include + +#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); +} diff --git a/src/platforms/posix/drivers/bebop_flow/dump_pgm.h b/src/platforms/posix/drivers/bebop_flow/dump_pgm.h new file mode 100644 index 0000000000..9be5d13143 --- /dev/null +++ b/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 + +void dump_pgm(const void *data, uint32_t size, uint32_t seq, uint32_t timestamp); diff --git a/src/platforms/posix/drivers/bebop_flow/video_device.cpp b/src/platforms/posix/drivers/bebop_flow/video_device.cpp index 7bd437db1f..80bb06d83e 100644 --- a/src/platforms/posix/drivers/bebop_flow/video_device.cpp +++ b/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 +#include +#include +#include +#include + +#include + +#include + 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; } diff --git a/src/platforms/posix/drivers/bebop_flow/video_device.h b/src/platforms/posix/drivers/bebop_flow/video_device.h index 9c08ca4418..8ed07cb2c8 100644 --- a/src/platforms/posix/drivers/bebop_flow/video_device.h +++ b/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 +#include + +#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) : - _dev_name(dev_name) {}; + VideoDevice(char const *dev_name, uint32_t n_buffers) : + _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(); };