289 lines
9.0 KiB
C++
289 lines
9.0 KiB
C++
#include "v4l2worker.h"
|
|
|
|
#include <fcntl.h>
|
|
#include <unistd.h>
|
|
#include <sys/ioctl.h>
|
|
#include <sys/mman.h>
|
|
#include <errno.h>
|
|
#include <string.h>
|
|
#include <linux/videodev2.h>
|
|
|
|
#include <QBuffer>
|
|
#include <QImageReader>
|
|
#include <QThread>
|
|
|
|
static int xioctl(int fd, unsigned long req, void *arg)
|
|
{
|
|
int r;
|
|
do { r = ::ioctl(fd, req, arg); } while (r == -1 && errno == EINTR);
|
|
return r;
|
|
}
|
|
|
|
V4l2Worker::V4l2Worker(QObject *parent)
|
|
: QObject(parent)
|
|
{}
|
|
|
|
V4l2Worker::~V4l2Worker()
|
|
{
|
|
stop();
|
|
}
|
|
|
|
// ── public slots ──────────────────────────────────────────────────────────────
|
|
|
|
void V4l2Worker::startCapture(const QString &device, int width, int height)
|
|
{
|
|
if (m_running.load())
|
|
stop();
|
|
|
|
if (!openDevice(device, width, height))
|
|
return;
|
|
|
|
m_running.store(true);
|
|
|
|
// Run the blocking capture loop in a plain std::thread so the Qt event
|
|
// loop of this worker object stays responsive to stopCapture() signals.
|
|
m_captureThread = std::thread([this] { captureLoop(); });
|
|
}
|
|
|
|
void V4l2Worker::stopCapture()
|
|
{
|
|
stop();
|
|
}
|
|
|
|
// ── internal stop (safe to call from any thread) ──────────────────────────────
|
|
|
|
void V4l2Worker::stop()
|
|
{
|
|
m_running.store(false);
|
|
if (m_captureThread.joinable())
|
|
m_captureThread.join(); // wait for captureLoop() to exit
|
|
closeDevice();
|
|
}
|
|
|
|
// ── capture loop (runs in m_captureThread) ────────────────────────────────────
|
|
|
|
void V4l2Worker::captureLoop()
|
|
{
|
|
v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
|
if (xioctl(m_fd, VIDIOC_STREAMON, &type) == -1) {
|
|
emit errorOccurred(tr("VIDIOC_STREAMON failed: %1").arg(strerror(errno)));
|
|
m_running.store(false);
|
|
return;
|
|
}
|
|
|
|
while (m_running.load()) {
|
|
fd_set fds;
|
|
FD_ZERO(&fds);
|
|
FD_SET(m_fd, &fds);
|
|
timeval tv{0, 100000}; // 100 ms timeout → checks m_running 10x/sec
|
|
const int r = ::select(m_fd + 1, &fds, nullptr, nullptr, &tv);
|
|
if (r == 0) continue;
|
|
if (r == -1) {
|
|
if (errno == EINTR) continue;
|
|
emit errorOccurred(tr("select() failed: %1").arg(strerror(errno)));
|
|
break;
|
|
}
|
|
|
|
v4l2_buffer buf{};
|
|
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
|
buf.memory = V4L2_MEMORY_MMAP;
|
|
if (xioctl(m_fd, VIDIOC_DQBUF, &buf) == -1) {
|
|
if (errno == EAGAIN) continue;
|
|
emit errorOccurred(tr("VIDIOC_DQBUF failed: %1").arg(strerror(errno)));
|
|
break;
|
|
}
|
|
|
|
const QImage img = convertToImage(
|
|
m_buffers[buf.index].start, buf.bytesused, m_width, m_height);
|
|
if (!img.isNull())
|
|
emit newFrame(img);
|
|
|
|
if (xioctl(m_fd, VIDIOC_QBUF, &buf) == -1) {
|
|
emit errorOccurred(tr("VIDIOC_QBUF failed: %1").arg(strerror(errno)));
|
|
break;
|
|
}
|
|
}
|
|
|
|
xioctl(m_fd, VIDIOC_STREAMOFF, &type);
|
|
emit captureStopped();
|
|
}
|
|
|
|
// ── device open / close ───────────────────────────────────────────────────────
|
|
|
|
bool V4l2Worker::openDevice(const QString &device, int width, int height)
|
|
{
|
|
m_width = width;
|
|
m_height = height;
|
|
|
|
m_fd = ::open(device.toLocal8Bit().constData(), O_RDWR | O_NONBLOCK);
|
|
if (m_fd == -1) {
|
|
emit errorOccurred(tr("Cannot open %1: %2").arg(device, strerror(errno)));
|
|
return false;
|
|
}
|
|
|
|
v4l2_capability cap{};
|
|
if (xioctl(m_fd, VIDIOC_QUERYCAP, &cap) == -1) {
|
|
emit errorOccurred(tr("VIDIOC_QUERYCAP failed: %1").arg(strerror(errno)));
|
|
closeDevice();
|
|
return false;
|
|
}
|
|
if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
|
|
emit errorOccurred(tr("%1 is not a video capture device").arg(device));
|
|
closeDevice();
|
|
return false;
|
|
}
|
|
|
|
const uint32_t preferred[] = {
|
|
V4L2_PIX_FMT_MJPEG,
|
|
V4L2_PIX_FMT_YUYV,
|
|
V4L2_PIX_FMT_NV12,
|
|
0
|
|
};
|
|
|
|
m_pixFmt = 0;
|
|
for (int i = 0; preferred[i]; ++i) {
|
|
v4l2_format fmt{};
|
|
fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
|
fmt.fmt.pix.width = static_cast<uint32_t>(width);
|
|
fmt.fmt.pix.height = static_cast<uint32_t>(height);
|
|
fmt.fmt.pix.pixelformat = preferred[i];
|
|
fmt.fmt.pix.field = V4L2_FIELD_ANY;
|
|
if (xioctl(m_fd, VIDIOC_S_FMT, &fmt) == 0) {
|
|
m_pixFmt = fmt.fmt.pix.pixelformat;
|
|
m_width = static_cast<int>(fmt.fmt.pix.width);
|
|
m_height = static_cast<int>(fmt.fmt.pix.height);
|
|
break;
|
|
}
|
|
}
|
|
if (!m_pixFmt) {
|
|
emit errorOccurred(tr("No supported pixel format found on %1").arg(device));
|
|
closeDevice();
|
|
return false;
|
|
}
|
|
|
|
QString fmtName;
|
|
switch (m_pixFmt) {
|
|
case V4L2_PIX_FMT_MJPEG: fmtName = "MJPEG"; break;
|
|
case V4L2_PIX_FMT_YUYV: fmtName = "YUYV"; break;
|
|
case V4L2_PIX_FMT_NV12: fmtName = "NV12"; break;
|
|
default: fmtName = "?"; break;
|
|
}
|
|
|
|
if (!initMmap()) {
|
|
closeDevice();
|
|
return false;
|
|
}
|
|
|
|
emit captureStarted(device, m_width, m_height, fmtName);
|
|
return true;
|
|
}
|
|
|
|
void V4l2Worker::closeDevice()
|
|
{
|
|
freeMmap();
|
|
if (m_fd != -1) {
|
|
::close(m_fd);
|
|
m_fd = -1;
|
|
}
|
|
}
|
|
|
|
bool V4l2Worker::initMmap()
|
|
{
|
|
v4l2_requestbuffers req{};
|
|
req.count = MMAP_BUFFERS;
|
|
req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
|
req.memory = V4L2_MEMORY_MMAP;
|
|
if (xioctl(m_fd, VIDIOC_REQBUFS, &req) == -1) {
|
|
emit errorOccurred(tr("VIDIOC_REQBUFS failed: %1").arg(strerror(errno)));
|
|
return false;
|
|
}
|
|
m_nBuffers = static_cast<int>(req.count);
|
|
|
|
for (int i = 0; i < m_nBuffers; ++i) {
|
|
v4l2_buffer buf{};
|
|
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
|
buf.memory = V4L2_MEMORY_MMAP;
|
|
buf.index = static_cast<uint32_t>(i);
|
|
if (xioctl(m_fd, VIDIOC_QUERYBUF, &buf) == -1) {
|
|
emit errorOccurred(tr("VIDIOC_QUERYBUF failed: %1").arg(strerror(errno)));
|
|
return false;
|
|
}
|
|
m_buffers[i].length = buf.length;
|
|
m_buffers[i].start = ::mmap(nullptr, buf.length,
|
|
PROT_READ | PROT_WRITE, MAP_SHARED,
|
|
m_fd, buf.m.offset);
|
|
if (m_buffers[i].start == MAP_FAILED) {
|
|
emit errorOccurred(tr("mmap failed: %1").arg(strerror(errno)));
|
|
return false;
|
|
}
|
|
if (xioctl(m_fd, VIDIOC_QBUF, &buf) == -1) {
|
|
emit errorOccurred(tr("VIDIOC_QBUF failed: %1").arg(strerror(errno)));
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
void V4l2Worker::freeMmap()
|
|
{
|
|
for (int i = 0; i < m_nBuffers; ++i) {
|
|
if (m_buffers[i].start && m_buffers[i].start != MAP_FAILED) {
|
|
::munmap(m_buffers[i].start, m_buffers[i].length);
|
|
m_buffers[i].start = nullptr;
|
|
}
|
|
}
|
|
m_nBuffers = 0;
|
|
}
|
|
|
|
QImage V4l2Worker::convertToImage(const void *data, size_t size, int width, int height)
|
|
{
|
|
switch (m_pixFmt) {
|
|
case V4L2_PIX_FMT_MJPEG: {
|
|
QByteArray ba(static_cast<const char *>(data), static_cast<int>(size));
|
|
QBuffer buf(&ba);
|
|
QImageReader reader(&buf, "JPEG");
|
|
return reader.read();
|
|
}
|
|
case V4L2_PIX_FMT_YUYV:
|
|
return yuyv2Rgb(static_cast<const uchar *>(data), width, height);
|
|
case V4L2_PIX_FMT_NV12:
|
|
return nv122Rgb(static_cast<const uchar *>(data), width, height);
|
|
default:
|
|
return {};
|
|
}
|
|
}
|
|
|
|
QImage V4l2Worker::yuyv2Rgb(const uchar *src, int width, int height)
|
|
{
|
|
QImage img(width, height, QImage::Format_RGB32);
|
|
for (int y = 0; y < height; ++y) {
|
|
const uchar *row = src + y * width * 2;
|
|
QRgb *dst = reinterpret_cast<QRgb *>(img.scanLine(y));
|
|
for (int x = 0; x < width; x += 2) {
|
|
const int Y0 = row[0], U = row[1]-128, Y1 = row[2], V = row[3]-128;
|
|
row += 4;
|
|
auto clamp = [](int v) { return static_cast<uchar>(v<0?0:v>255?255:v); };
|
|
dst[x] = qRgb(clamp(Y0+1.402f*V), clamp(Y0-0.344f*U-0.714f*V), clamp(Y0+1.772f*U));
|
|
dst[x+1] = qRgb(clamp(Y1+1.402f*V), clamp(Y1-0.344f*U-0.714f*V), clamp(Y1+1.772f*U));
|
|
}
|
|
}
|
|
return img;
|
|
}
|
|
|
|
QImage V4l2Worker::nv122Rgb(const uchar *src, int width, int height)
|
|
{
|
|
QImage img(width, height, QImage::Format_RGB32);
|
|
const uchar *uvPlane = src + width * height;
|
|
auto clamp = [](int v) { return static_cast<uchar>(v<0?0:v>255?255:v); };
|
|
for (int y = 0; y < height; ++y) {
|
|
const uchar *yRow = src + y * width;
|
|
const uchar *uvRow = uvPlane + (y/2) * width;
|
|
QRgb *dst = reinterpret_cast<QRgb *>(img.scanLine(y));
|
|
for (int x = 0; x < width; ++x) {
|
|
const int Y = yRow[x], U = uvRow[x&~1]-128, V = uvRow[(x&~1)+1]-128;
|
|
dst[x] = qRgb(clamp(Y+1.402f*V), clamp(Y-0.344f*U-0.714f*V), clamp(Y+1.772f*U));
|
|
}
|
|
}
|
|
return img;
|
|
}
|