428 lines
12 KiB
C++
428 lines
12 KiB
C++
/*
|
|
|
|
(c) 2016-2023 Carl Philipp Klemm <carl@uvos.xyz>
|
|
(c) 2014 Séverin Lemaignan <severin.lemaignan@epfl.ch>
|
|
(c) 2008 Hans de Goede <hdegoede@redhat.com> for yuyv_to_rgb24
|
|
|
|
This program is free software; you can redistribute it and/or modify it
|
|
under the terms of the GNU Lesser General Public License as published by
|
|
the Free Software Foundation; either version 2.1 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 Lesser General Public
|
|
License for more details.
|
|
|
|
You should have received a copy of the GNU Lesser General Public License
|
|
along with this program; if not, write to the Free Software Foundation,
|
|
Inc., 51 Franklin Street, Suite 500, Boston, MA 02110-1335 USA
|
|
|
|
*/
|
|
|
|
#include <stdlib.h>
|
|
#include <assert.h>
|
|
#include <fcntl.h> /* low-level i/o */
|
|
#include <unistd.h>
|
|
#include <errno.h>
|
|
#include <string.h> // strerrno
|
|
#include <sys/stat.h>
|
|
#include <sys/types.h>
|
|
#include <sys/time.h>
|
|
#include <sys/mman.h>
|
|
#include <sys/ioctl.h>
|
|
|
|
#include <stdexcept>
|
|
|
|
#include <linux/videodev2.h>
|
|
|
|
#include "webcam.h"
|
|
#include "jpeg_img.h"
|
|
|
|
#define CLEAR(x) memset(&(x), 0, sizeof(x))
|
|
|
|
using namespace std;
|
|
|
|
static int xioctl(int fh, unsigned long int request, void *arg)
|
|
{
|
|
int r;
|
|
|
|
do {
|
|
r = ioctl(fh, request, arg);
|
|
} while (-1 == r && EINTR == errno);
|
|
|
|
return r;
|
|
}
|
|
|
|
#define CLIP(color) (unsigned char)(((color) > 0xFF) ? 0xff : (((color) < 0) ? 0 : (color)))
|
|
|
|
static void v4lconvert_yuyv_to_rgb24(const unsigned char *src,
|
|
unsigned char *dest,
|
|
int width, int height,
|
|
int stride)
|
|
{
|
|
int j;
|
|
|
|
while (--height >= 0) {
|
|
for (j = 0; j + 1 < width; j += 2) {
|
|
int u = src[1];
|
|
int v = src[3];
|
|
int u1 = (((u - 128) << 7) + (u - 128)) >> 6;
|
|
int rg = (((u - 128) << 1) + (u - 128) +
|
|
((v - 128) << 2) + ((v - 128) << 1)) >> 3;
|
|
int v1 = (((v - 128) << 1) + (v - 128)) >> 1;
|
|
|
|
*dest++ = CLIP(src[0] + v1);
|
|
*dest++ = CLIP(src[0] - rg);
|
|
*dest++ = CLIP(src[0] + u1);
|
|
|
|
*dest++ = CLIP(src[2] + v1);
|
|
*dest++ = CLIP(src[2] - rg);
|
|
*dest++ = CLIP(src[2] + u1);
|
|
src += 4;
|
|
}
|
|
src += stride - (width * 2);
|
|
}
|
|
}
|
|
|
|
Webcam::Webcam(const string& device, int width, int height, bool mjpeg, bool passthrough) :
|
|
device(device),
|
|
xres(width),
|
|
yres(height),
|
|
passthrough_format(passthrough),
|
|
format(mjpeg ? V4L2_PIX_FMT_MJPEG : V4L2_PIX_FMT_YUYV)
|
|
{
|
|
open_device();
|
|
init_device();
|
|
// xres and yres are set to the actual resolution provided by the cam
|
|
|
|
// frame stored as RGB888 (ie, RGB24)
|
|
rgb_frame.width = xres;
|
|
rgb_frame.height = yres;
|
|
rgb_frame.channels = 3;
|
|
rgb_frame.data = std::shared_ptr<unsigned char>(new unsigned char[rgb_frame.size()], std::default_delete<unsigned char[]>());
|
|
|
|
start_capturing();
|
|
}
|
|
|
|
Webcam::~Webcam()
|
|
{
|
|
stop_capturing();
|
|
uninit_device();
|
|
close_device();
|
|
}
|
|
|
|
const Image& Webcam::frame(int timeout)
|
|
{
|
|
for (;;) {
|
|
fd_set fds;
|
|
struct timeval tv;
|
|
int r;
|
|
|
|
FD_ZERO(&fds);
|
|
FD_SET(fd, &fds);
|
|
|
|
/* Timeout. */
|
|
tv.tv_sec = timeout;
|
|
tv.tv_usec = 0;
|
|
|
|
r = select(fd + 1, &fds, NULL, NULL, &tv);
|
|
|
|
if (-1 == r) {
|
|
if (EINTR == errno)
|
|
continue;
|
|
throw runtime_error("select");
|
|
}
|
|
|
|
if (0 == r) {
|
|
throw runtime_error(device + ": select timeout");
|
|
}
|
|
int idx = read_frame();
|
|
if (idx != -1) {
|
|
if(!passthrough_format)
|
|
{
|
|
if (format == V4L2_PIX_FMT_YUYV)
|
|
{
|
|
v4lconvert_yuyv_to_rgb24((unsigned char *) buffers[idx].data,
|
|
rgb_frame.data.get(),
|
|
xres,
|
|
yres,
|
|
stride);
|
|
}
|
|
else if(format == V4L2_PIX_FMT_MJPEG)
|
|
{
|
|
rgb_frame = decompressJpegImage((unsigned char *) buffers[idx].data, buffers[idx].size);
|
|
}
|
|
else
|
|
{
|
|
assert(false);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if(format == V4L2_PIX_FMT_YUYV)
|
|
{
|
|
rgb_frame.setBuffer(std::shared_ptr<unsigned char>(new unsigned char[buffers[idx].size], std::default_delete<unsigned char[]>()),
|
|
buffers[idx].size, Image::FORMAT_YUYV);
|
|
}
|
|
else if(format == V4L2_PIX_FMT_MJPEG)
|
|
{
|
|
rgb_frame.setBuffer(std::shared_ptr<unsigned char>(new unsigned char[buffers[idx].size], std::default_delete<unsigned char[]>()),
|
|
buffers[idx].size, Image::FORMAT_RGB);
|
|
}
|
|
else
|
|
{
|
|
assert(false);
|
|
}
|
|
memcpy(rgb_frame.data.get(), buffers[idx].data, buffers[idx].size);
|
|
}
|
|
return rgb_frame;
|
|
}
|
|
/* EAGAIN - continue select loop. */
|
|
}
|
|
|
|
}
|
|
|
|
bool Webcam::read_frame()
|
|
{
|
|
|
|
struct v4l2_buffer buf;
|
|
|
|
CLEAR(buf);
|
|
|
|
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
|
buf.memory = V4L2_MEMORY_MMAP;
|
|
|
|
if (-1 == xioctl(fd, VIDIOC_DQBUF, &buf)) {
|
|
switch (errno) {
|
|
case EAGAIN:
|
|
return -1;
|
|
|
|
case EIO:
|
|
/* Could ignore EIO, see spec. */
|
|
|
|
/* fall through */
|
|
|
|
default:
|
|
throw runtime_error("VIDIOC_DQBUF");
|
|
}
|
|
}
|
|
|
|
assert(buf.index < n_buffers);
|
|
|
|
if (-1 == xioctl(fd, VIDIOC_QBUF, &buf))
|
|
throw runtime_error("VIDIOC_QBUF");
|
|
|
|
return buf.index;
|
|
}
|
|
|
|
void Webcam::open_device(void)
|
|
{
|
|
struct stat st;
|
|
|
|
if (-1 == stat(device.c_str(), &st)) {
|
|
throw runtime_error(device + ": cannot identify! " + to_string(errno) + ": " + strerror(errno));
|
|
}
|
|
|
|
if (!S_ISCHR(st.st_mode)) {
|
|
throw runtime_error(device + " is no device");
|
|
}
|
|
|
|
fd = open(device.c_str(), O_RDWR /* required */ | O_NONBLOCK, 0);
|
|
|
|
if (-1 == fd) {
|
|
throw runtime_error(device + ": cannot open! " + to_string(errno) + ": " + strerror(errno));
|
|
}
|
|
}
|
|
|
|
|
|
void Webcam::init_mmap(void)
|
|
{
|
|
struct v4l2_requestbuffers req;
|
|
|
|
CLEAR(req);
|
|
|
|
req.count = 1;
|
|
req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
|
req.memory = V4L2_MEMORY_MMAP;
|
|
|
|
if (-1 == xioctl(fd, VIDIOC_REQBUFS, &req)) {
|
|
if (EINVAL == errno) {
|
|
throw runtime_error(device + " does not support memory mapping");
|
|
} else {
|
|
throw runtime_error("VIDIOC_REQBUFS");
|
|
}
|
|
}
|
|
|
|
/*if (req.count < 2) {
|
|
throw runtime_error(string("Insufficient buffer memory on ") + device);
|
|
}*/
|
|
|
|
buffers = (buffer*) calloc(req.count, sizeof(*buffers));
|
|
|
|
if (!buffers) {
|
|
throw runtime_error("Out of memory");
|
|
}
|
|
|
|
for (n_buffers = 0; n_buffers < req.count; ++n_buffers) {
|
|
struct v4l2_buffer buf;
|
|
|
|
CLEAR(buf);
|
|
|
|
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
|
buf.memory = V4L2_MEMORY_MMAP;
|
|
buf.index = n_buffers;
|
|
|
|
if (-1 == xioctl(fd, VIDIOC_QUERYBUF, &buf))
|
|
throw runtime_error("VIDIOC_QUERYBUF");
|
|
|
|
buffers[n_buffers].size = buf.length;
|
|
buffers[n_buffers].data =
|
|
mmap(NULL /* start anywhere */,
|
|
buf.length,
|
|
PROT_READ | PROT_WRITE /* required */,
|
|
MAP_SHARED /* recommended */,
|
|
fd, buf.m.offset);
|
|
|
|
if (MAP_FAILED == buffers[n_buffers].data)
|
|
throw runtime_error("mmap");
|
|
}
|
|
}
|
|
|
|
void Webcam::close_device(void)
|
|
{
|
|
if (-1 == close(fd))
|
|
throw runtime_error("close");
|
|
|
|
fd = -1;
|
|
}
|
|
|
|
void Webcam::init_device(void)
|
|
{
|
|
struct v4l2_capability cap;
|
|
struct v4l2_cropcap cropcap;
|
|
struct v4l2_crop crop;
|
|
struct v4l2_format fmt;
|
|
|
|
if (-1 == xioctl(fd, VIDIOC_QUERYCAP, &cap)) {
|
|
if (EINVAL == errno) {
|
|
throw runtime_error(device + " is no V4L2 device");
|
|
} else {
|
|
throw runtime_error("VIDIOC_QUERYCAP");
|
|
}
|
|
}
|
|
|
|
if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
|
|
throw runtime_error(device + " is no video capture device");
|
|
}
|
|
|
|
if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
|
|
throw runtime_error(device + " does not support streaming i/o");
|
|
}
|
|
|
|
/* Select video input, video standard and tune here. */
|
|
|
|
|
|
CLEAR(cropcap);
|
|
|
|
cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
|
|
|
if (0 == xioctl(fd, VIDIOC_CROPCAP, &cropcap)) {
|
|
crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
|
crop.c = cropcap.defrect; /* reset to default */
|
|
|
|
if (-1 == xioctl(fd, VIDIOC_S_CROP, &crop)) {
|
|
switch (errno) {
|
|
case EINVAL:
|
|
/* Cropping not supported. */
|
|
break;
|
|
default:
|
|
/* Errors ignored. */
|
|
break;
|
|
}
|
|
}
|
|
} else {
|
|
/* Errors ignored. */
|
|
}
|
|
|
|
|
|
CLEAR(fmt);
|
|
|
|
fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
|
if (force_format) {
|
|
fmt.fmt.pix.width = xres;
|
|
fmt.fmt.pix.height = yres;
|
|
fmt.fmt.pix.pixelformat = format;
|
|
fmt.fmt.pix.field = V4L2_FIELD_ANY;
|
|
|
|
if (-1 == xioctl(fd, VIDIOC_S_FMT, &fmt))
|
|
throw runtime_error("VIDIOC_S_FMT");
|
|
|
|
if (fmt.fmt.pix.pixelformat != format)
|
|
{
|
|
// note that libv4l2 (look for 'v4l-utils') provides helpers
|
|
// to manage conversions
|
|
throw runtime_error("Webcam does not support the requested format. Support for more formats need to be added! video format is "
|
|
+ std::to_string(fmt.fmt.pix.pixelformat));
|
|
}
|
|
|
|
/* Note VIDIOC_S_FMT may change width and height. */
|
|
xres = fmt.fmt.pix.width;
|
|
yres = fmt.fmt.pix.height;
|
|
|
|
stride = fmt.fmt.pix.bytesperline;
|
|
|
|
|
|
} else {
|
|
/* Preserve original settings as set by v4l2-ctl for example */
|
|
if (-1 == xioctl(fd, VIDIOC_G_FMT, &fmt))
|
|
throw runtime_error("VIDIOC_G_FMT");
|
|
}
|
|
|
|
init_mmap();
|
|
}
|
|
|
|
|
|
void Webcam::uninit_device(void)
|
|
{
|
|
unsigned int i;
|
|
|
|
for (i = 0; i < n_buffers; ++i)
|
|
if (-1 == munmap(buffers[i].data, buffers[i].size))
|
|
throw runtime_error("munmap");
|
|
|
|
free(buffers);
|
|
}
|
|
|
|
void Webcam::start_capturing(void)
|
|
{
|
|
unsigned int i;
|
|
enum v4l2_buf_type type;
|
|
|
|
for (i = 0; i < n_buffers; ++i) {
|
|
struct v4l2_buffer buf;
|
|
|
|
CLEAR(buf);
|
|
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
|
buf.memory = V4L2_MEMORY_MMAP;
|
|
buf.index = i;
|
|
|
|
if (-1 == xioctl(fd, VIDIOC_QBUF, &buf))
|
|
throw runtime_error("VIDIOC_QBUF");
|
|
}
|
|
type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
|
if (-1 == xioctl(fd, VIDIOC_STREAMON, &type))
|
|
throw runtime_error("VIDIOC_STREAMON");
|
|
}
|
|
|
|
void Webcam::stop_capturing(void)
|
|
{
|
|
enum v4l2_buf_type type;
|
|
|
|
type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
|
if (-1 == xioctl(fd, VIDIOC_STREAMOFF, &type))
|
|
throw runtime_error("VIDIOC_STREAMOFF");
|
|
}
|
|
|
|
|