mirror of https://github.com/ArduPilot/ardupilot
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.
This commit is contained in:
Julien BERAUD
Andrew Tridgell
parent
24d20f89c4
commit
25df4d03ff
|
|
@ -0,0 +1,313 @@
|
||||||
|
/*
|
||||||
|
This class has been implemented based on
|
||||||
|
yavta -- Yet Another V4L2 Test Application written by:
|
||||||
|
Laurent Pinchart <laurent.pinchart@ideasonboard.com>
|
||||||
|
|
||||||
|
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 <http://www.gnu.org/licenses/>.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <AP_HAL/AP_HAL.h>
|
||||||
|
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
|
||||||
|
#include "VideoIn.h"
|
||||||
|
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include <fcntl.h>
|
||||||
|
#include <unistd.h>
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <errno.h>
|
||||||
|
#include <sched.h>
|
||||||
|
#include <time.h>
|
||||||
|
#include <pthread.h>
|
||||||
|
#include <sys/ioctl.h>
|
||||||
|
#include <sys/mman.h>
|
||||||
|
#include <sys/select.h>
|
||||||
|
#include <sys/stat.h>
|
||||||
|
#include <sys/time.h>
|
||||||
|
|
||||||
|
#include <linux/videodev2.h>
|
||||||
|
|
||||||
|
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
|
||||||
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||||
|
*/
|
||||||
|
#ifndef __VIDEOIN_H__
|
||||||
|
#define __VIDEOIN_H__
|
||||||
|
|
||||||
|
#include "AP_HAL_Linux.h"
|
||||||
|
#include <linux/videodev2.h>
|
||||||
|
|
||||||
|
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
|
||||||
Loading…
Reference in New Issue