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