From 25df4d03ffe0a633ae0ba81dcff63fad6a13ca6d Mon Sep 17 00:00:00 2001 From: Julien BERAUD Date: Tue, 24 Nov 2015 09:50:33 +0100 Subject: [PATCH] AP_HAL_Linux: Add support for Video Input VideoIn class is created that allows to setup a v4l2 interface and capture buffers. I is based on yavta utility by Laurent Pinchart and has been tested only on the bebop, though yavta works on any linux platform. --- libraries/AP_HAL_Linux/VideoIn.cpp | 313 +++++++++++++++++++++++++++++ libraries/AP_HAL_Linux/VideoIn.h | 69 +++++++ 2 files changed, 382 insertions(+) create mode 100644 libraries/AP_HAL_Linux/VideoIn.cpp create mode 100644 libraries/AP_HAL_Linux/VideoIn.h diff --git a/libraries/AP_HAL_Linux/VideoIn.cpp b/libraries/AP_HAL_Linux/VideoIn.cpp new file mode 100644 index 0000000000..0ea6d3b651 --- /dev/null +++ b/libraries/AP_HAL_Linux/VideoIn.cpp @@ -0,0 +1,313 @@ +/* + This class has been implemented based on + yavta -- Yet Another V4L2 Test Application written by: + Laurent Pinchart + + This program 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 3 of the License, or + (at your option) any later version. + + This program 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 this program. If not, see . + */ + +#include +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP +#include "VideoIn.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +extern const AP_HAL::HAL& hal; + +using namespace Linux; + +bool VideoIn::get_frame(Frame &frame) +{ + if (!_streaming) { + if (!_set_streaming(true)) { + AP_HAL::panic("couldn't start streaming\n"); + return false; + } + _streaming = true; + } + + if (_dequeue_frame(frame)) { + return true; + } else { + return false; + } +} + +void VideoIn::put_frame(Frame &frame) +{ + _queue_buffer((uint32_t)frame.buf_index); +} + +bool VideoIn::open_device(const char *device_path, uint32_t memtype) +{ + struct v4l2_capability cap; + int ret; + + _fd = -1; + _buffers = NULL; + _fd = open(device_path, O_RDWR); + _memtype = memtype; + if (_fd < 0) { + hal.console->printf("Error opening device %s: %s (%d).\n", + device_path, + strerror(errno), errno); + return false; + } + + memset(&cap, 0, sizeof cap); + ret = ioctl(_fd, VIDIOC_QUERYCAP, &cap); + if (ret < 0) { + hal.console->printf("Error querying caps\n"); + return false; + } + + if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) { + hal.console->printf("Error opening device %s: is not a video capture" + "device\n", device_path); + close(_fd); + return false; + } + + return true; +} + +bool VideoIn::allocate_buffers(uint32_t nbufs) +{ + struct v4l2_requestbuffers rb; + struct v4l2_buffer buf; + struct buffer *buffers; + unsigned int i; + int ret; + + memset(&rb, 0, sizeof rb); + rb.count = nbufs; + rb.type = V4L2_CAP_VIDEO_CAPTURE; + rb.memory = _memtype; + + ret = ioctl(_fd, VIDIOC_REQBUFS, &rb); + if (ret < 0) { + printf("Unable to request buffers: %s (%d).\n", strerror(errno), + errno); + return ret; + } + + buffers = (struct buffer *)malloc(rb.count * sizeof buffers[0]); + if (buffers == NULL) { + hal.console->printf("Unable to allocate buffers\n"); + return false; + } + + for (i=0; i < rb.count; i++) { + memset(&buf, 0, sizeof(buf)); + buf.index = i; + buf.type = rb.type; + buf.memory = rb.memory; + ret = ioctl(_fd, VIDIOC_QUERYBUF, &buf); + if (ret < 0) { + hal.console->printf("Unable to query buffer %u: %s(%d).\n", i, + strerror(errno), errno); + return false; + } + + switch (_memtype) { + case V4L2_MEMORY_MMAP: + buffers[i].mem = mmap(0, buf.length, PROT_READ | PROT_WRITE, + MAP_SHARED, _fd, buf.m.offset); + if (buffers[i].mem == MAP_FAILED) { + hal.console->printf("Unable to map buffer %u: %s (%d)\n", i, + strerror(errno), errno); + return false; + } + buffers[i].size = buf.length; + break; + case V4L2_MEMORY_USERPTR: + ret = posix_memalign(&buffers[i].mem, getpagesize(), buf.length); + if (ret < 0) { + hal.console->printf("Unable to allocate buffer %u (%d)\n", i, + ret); + return false; + } + buffers[i].size = buf.length; + break; + default: + return false; + } + } + _nbufs = rb.count; + _buffers = buffers; + return true; +} + +bool VideoIn::set_format(uint32_t *width, uint32_t *height, uint32_t *format, + uint32_t *bytesperline, uint32_t *sizeimage) +{ + struct v4l2_format fmt; + int ret; + + memset(&fmt, 0, sizeof fmt); + fmt.type = V4L2_CAP_VIDEO_CAPTURE; + fmt.fmt.pix.width = *width; + fmt.fmt.pix.height = *height; + fmt.fmt.pix.pixelformat = *format; + fmt.fmt.pix.colorspace = V4L2_COLORSPACE_REC709; + + ret = ioctl(_fd, VIDIOC_S_FMT, &fmt); + if (ret < 0) { + hal.console->printf("VideoIn: unable to set format: %s (%d).\n", + strerror(errno), errno); + return false; + } + + /* warn if format different from the one that was supposed + * to be set + */ + if ((fmt.fmt.pix.pixelformat != *format) || + (fmt.fmt.pix.width != *width) || + (fmt.fmt.pix.height != *height)) { + hal.console->printf("format set to (%08x)" + "%ux%u buffer size %u field : %d\n", + fmt.fmt.pix.pixelformat, fmt.fmt.pix.width, + fmt.fmt.pix.height, fmt.fmt.pix.sizeimage, + fmt.fmt.pix.field); + } + + *width = fmt.fmt.pix.width; + *height = fmt.fmt.pix.height; + *format = fmt.fmt.pix.pixelformat; + *bytesperline = fmt.fmt.pix.bytesperline; + *sizeimage = fmt.fmt.pix.sizeimage; + + return true; +} + +bool VideoIn::set_crop(uint32_t left, uint32_t top, + uint32_t width, uint32_t height) +{ + struct v4l2_crop crop; + int ret; + + memset(&crop, 0, sizeof crop); + crop.c.top = top; + crop.c.left = left; + crop.c.width = width; + crop.c.height = height; + crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + + ret = ioctl(_fd, VIDIOC_S_CROP, &crop); + + if (ret < 0) { + hal.console->printf("VideoIn: unable to set crop: %s (%d).\n", + strerror(errno), errno); + return false; + } + + return true; +} + +void VideoIn::prepare_capture() +{ + unsigned int i; + + /* Queue the buffers. */ + for (i = 0; i < _nbufs; ++i) { + _queue_buffer(i); + } +} + +uint32_t VideoIn::_timeval_to_us(struct timeval& tv) +{ + return (1.0e6 * tv.tv_sec + tv.tv_usec); +} + +void VideoIn::_queue_buffer(int index) +{ + int ret; + struct v4l2_buffer buf; + memset(&buf, 0, sizeof buf); + buf.index = index; + buf.type = V4L2_CAP_VIDEO_CAPTURE; + buf.memory = _memtype; + buf.length = _buffers[index].size; + if (_memtype == V4L2_MEMORY_USERPTR) { + buf.m.userptr = (unsigned long) _buffers[index].mem; + } + ret = ioctl(_fd, VIDIOC_QBUF, &buf); + if (ret < 0) { + hal.console->printf("Unable to queue buffer : %s (%d).\n", + strerror(errno), errno); + } +} + +bool VideoIn::_set_streaming(bool enable) +{ + int type = V4L2_CAP_VIDEO_CAPTURE; + int ret; + + ret = ioctl(_fd, enable ? VIDIOC_STREAMON : VIDIOC_STREAMOFF, &type); + if (ret < 0) { + printf("Unable to %s streaming: %s (%d).\n", + enable ? "start" : "stop", strerror(errno), errno); + return false; + } + + return true; +} + +bool VideoIn::_dequeue_frame(Frame &frame) +{ + struct v4l2_buffer buf; + int ret; + + /* Dequeue a buffer. */ + memset(&buf, 0, sizeof buf); + buf.type = V4L2_CAP_VIDEO_CAPTURE; + buf.memory = _memtype; + ret = ioctl(_fd, VIDIOC_DQBUF, &buf); + if (ret < 0) { + if (errno != EIO) { + hal.console->printf("Unable to dequeue buffer: %s (%d).\n", + strerror(errno), errno); + return false; + } + buf.type = V4L2_CAP_VIDEO_CAPTURE; + buf.memory = _memtype; + if (_memtype == V4L2_MEMORY_USERPTR) { + buf.m.userptr = (unsigned long)_buffers[buf.index].mem; + } + } + + frame.data = _buffers[buf.index].mem; + frame.buf_index = buf.index; + frame.timestamp = _timeval_to_us(buf.timestamp); + frame.sequence = buf.sequence; + + return true; +} + +#endif diff --git a/libraries/AP_HAL_Linux/VideoIn.h b/libraries/AP_HAL_Linux/VideoIn.h new file mode 100644 index 0000000000..d1014c6e37 --- /dev/null +++ b/libraries/AP_HAL_Linux/VideoIn.h @@ -0,0 +1,69 @@ +/* + This program 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 3 of the License, or + (at your option) any later version. + + This program 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 this program. If not, see . + */ +#ifndef __VIDEOIN_H__ +#define __VIDEOIN_H__ + +#include "AP_HAL_Linux.h" +#include + +struct buffer { + unsigned int size; + void *mem; +}; + +class Linux::VideoIn { +public: + /* This structure implements the fields of the v4l2_pix_format struct + * that are considered useful for an optical flow application along + * with the v4l2_buffer fields timestamp and sequence*/ + class Frame { + friend class VideoIn; + public: + uint32_t timestamp = 0; + uint32_t sequence = 0; + void *data = NULL; + private: + uint32_t buf_index; + }; + + bool get_frame(Frame &frame); + void put_frame(Frame &frame); + void set_device_path(const char* path); + void init(); + bool open_device(const char *device_path, uint32_t memtype); + bool allocate_buffers(uint32_t nbufs); + bool set_format(uint32_t *width, uint32_t *height, uint32_t *format, + uint32_t *bytesperline, uint32_t *sizeimage); + bool set_crop(uint32_t left, uint32_t top, + uint32_t width, uint32_t height); + void prepare_capture(); + +private: + void _queue_buffer(int index); + bool _set_streaming(bool enable); + bool _dequeue_frame(Frame &frame); + uint32_t _timeval_to_us(struct timeval& tv); + int _fd = -1; + struct buffer *_buffers = NULL; + unsigned int _nbufs = 0; + bool _streaming = false; + uint32_t _width = 0; + uint32_t _height = 0; + uint32_t _format = 0; + uint32_t _bytesperline = 0; + uint32_t _sizeimage = 0; + uint32_t _memtype = V4L2_MEMORY_MMAP; +}; +#endif