381 lines
12 KiB
C++
381 lines
12 KiB
C++
#include "v4l2_capture.h"
|
|
#include <fcntl.h>
|
|
#include <unistd.h>
|
|
#include <errno.h>
|
|
#include <sys/ioctl.h>
|
|
#include <sys/mman.h>
|
|
#include <linux/videodev2.h>
|
|
#include <cstring>
|
|
#include <cstdio>
|
|
#include <algorithm>
|
|
|
|
// 四字符码构造宏 (使用 v4l2_fourcc 避免重复定义)
|
|
// 注意: 这里使用videodev2.h已有的v4l2_fourcc宏
|
|
|
|
namespace {
|
|
|
|
// 将 v4l2_capability 的 driver[16] / card[32] 转成 C 字符串 (v4l2_capability 使用 __u8)
|
|
inline std::string to_cstr(const __u8 *arr, size_t len) {
|
|
std::string s;
|
|
s.reserve(len);
|
|
for (size_t i = 0; i < len && arr[i] != '\0'; ++i) {
|
|
char c = static_cast<char>(arr[i]);
|
|
if (c >= 32 && c < 127) s += c; // 只保留可打印 ASCII
|
|
}
|
|
// 去除末尾空格
|
|
while (!s.empty() && s.back() == ' ') s.pop_back();
|
|
return s;
|
|
}
|
|
|
|
// 打印 v4l2 格式调试信息
|
|
void dumpFormat(struct v4l2_format &fmt, const char *msg) {
|
|
printf("%s: width=%u height=%u pixfmt=0x%08x '%c%c%c%c' bpl=%u size=%u\n",
|
|
msg,
|
|
fmt.fmt.pix.width,
|
|
fmt.fmt.pix.height,
|
|
fmt.fmt.pix.pixelformat,
|
|
(fmt.fmt.pix.pixelformat) & 0xFF,
|
|
(fmt.fmt.pix.pixelformat >> 8) & 0xFF,
|
|
(fmt.fmt.pix.pixelformat >> 16) & 0xFF,
|
|
(fmt.fmt.pix.pixelformat >> 24) & 0xFF,
|
|
fmt.fmt.pix.bytesperline,
|
|
fmt.fmt.pix.sizeimage);
|
|
}
|
|
|
|
} // namespace
|
|
|
|
V4l2Capture::~V4l2Capture() {
|
|
close();
|
|
}
|
|
|
|
bool V4l2Capture::open(const std::string &device) {
|
|
if (fd_ >= 0) {
|
|
return false;
|
|
}
|
|
fd_ = ::open(device.c_str(), O_RDWR | O_NONBLOCK, 0);
|
|
if (fd_ < 0) {
|
|
printf("V4l2Capture: open %s failed: %s\n", device.c_str(), strerror(errno));
|
|
return false;
|
|
}
|
|
|
|
// 检查设备能力
|
|
struct v4l2_capability cap;
|
|
if (ioctl(fd_, VIDIOC_QUERYCAP, &cap) < 0) {
|
|
printf("V4l2Capture: VIDIOC_QUERYCAP failed: %s\n", strerror(errno));
|
|
::close(fd_);
|
|
fd_ = -1;
|
|
return false;
|
|
}
|
|
printf("V4l2Capture: opened %s, driver='%s' card='%s', caps=0x%x\n",
|
|
device.c_str(),
|
|
to_cstr(cap.driver, sizeof(cap.driver)).c_str(),
|
|
to_cstr(cap.card, sizeof(cap.card)).c_str(),
|
|
cap.capabilities);
|
|
|
|
// 必须支持 V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING
|
|
if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
|
|
printf("V4l2Capture: not a video capture device\n");
|
|
::close(fd_);
|
|
fd_ = -1;
|
|
return false;
|
|
}
|
|
if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
|
|
printf("V4l2Capture: does not support streaming I/O\n");
|
|
::close(fd_);
|
|
fd_ = -1;
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
void V4l2Capture::close() {
|
|
if (streaming_) {
|
|
stopStreaming();
|
|
}
|
|
buffers_.clear();
|
|
if (fd_ >= 0) {
|
|
::close(fd_);
|
|
fd_ = -1;
|
|
}
|
|
}
|
|
|
|
bool V4l2Capture::setFormat(uint32_t width, uint32_t height, uint32_t pixelformat) {
|
|
if (fd_ < 0) return false;
|
|
|
|
struct v4l2_format fmt;
|
|
std::memset(&fmt, 0, sizeof(fmt));
|
|
fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
|
fmt.fmt.pix.width = width;
|
|
fmt.fmt.pix.height = height;
|
|
fmt.fmt.pix.pixelformat = pixelformat;
|
|
fmt.fmt.pix.field = V4L2_FIELD_NONE; // 逐行扫描
|
|
|
|
if (ioctl(fd_, VIDIOC_S_FMT, &fmt) < 0) {
|
|
printf("V4l2Capture: VIDIOC_S_FMT failed: %s\n", strerror(errno));
|
|
return false;
|
|
}
|
|
|
|
dumpFormat(fmt, "V4l2Capture: setFormat");
|
|
|
|
width_ = fmt.fmt.pix.width;
|
|
height_ = fmt.fmt.pix.height;
|
|
pixelformat_ = fmt.fmt.pix.pixelformat;
|
|
bytes_per_line_ = fmt.fmt.pix.bytesperline;
|
|
|
|
// 如果驱动不按请求设置, 给出警告
|
|
if (width_ != width || height_ != height) {
|
|
printf("V4l2Capture: warning: requested %ux%u but got %ux%u\n",
|
|
width, height, width_, height_);
|
|
}
|
|
if (pixelformat_ != pixelformat) {
|
|
char req[5] = {0}, got[5] = {0};
|
|
req[0] = pixelformat & 0xFF; req[1] = (pixelformat>>8)&0xFF;
|
|
req[2] = (pixelformat>>16)&0xFF; req[3] = (pixelformat>>24)&0xFF;
|
|
got[0] = pixelformat_ & 0xFF; got[1] = (pixelformat_>>8)&0xFF;
|
|
got[2] = (pixelformat_>>16)&0xFF; got[3] = (pixelformat_>>24)&0xFF;
|
|
printf("V4l2Capture: warning: requested '%s' but got '%s'\n", req, got);
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
bool V4l2Capture::setFps(uint32_t fps) {
|
|
if (fd_ < 0) return false;
|
|
|
|
struct v4l2_streamparm parm;
|
|
std::memset(&parm, 0, sizeof(parm));
|
|
parm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
|
|
|
if (ioctl(fd_, VIDIOC_G_PARM, &parm) < 0) {
|
|
printf("V4l2Capture: VIDIOC_G_PARM failed: %s\n", strerror(errno));
|
|
return false;
|
|
}
|
|
|
|
// 尝试设置 FPS. 需要 V4L2_CAP_TIMEPERFRAME 支持
|
|
if (parm.parm.capture.capability & V4L2_CAP_TIMEPERFRAME) {
|
|
parm.parm.capture.timeperframe.numerator = 1;
|
|
parm.parm.capture.timeperframe.denominator = fps;
|
|
if (ioctl(fd_, VIDIOC_S_PARM, &parm) < 0) {
|
|
printf("V4l2Capture: VIDIOC_S_PARM fps=%u failed: %s\n", fps, strerror(errno));
|
|
return false;
|
|
}
|
|
printf("V4l2Capture: set fps=%u (actual %u/%u)\n",
|
|
fps,
|
|
parm.parm.capture.timeperframe.denominator,
|
|
parm.parm.capture.timeperframe.numerator);
|
|
} else {
|
|
printf("V4l2Capture: device does not support V4L2_CAP_TIMEPERFRAME\n");
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool V4l2Capture::setControl(uint32_t id, int32_t value) {
|
|
if (fd_ < 0) return false;
|
|
|
|
struct v4l2_control ctrl;
|
|
std::memset(&ctrl, 0, sizeof(ctrl));
|
|
ctrl.id = id;
|
|
ctrl.value = value;
|
|
|
|
if (ioctl(fd_, VIDIOC_S_CTRL, &ctrl) < 0) {
|
|
// 有些控制可能是只读的或不存在, 打印警告但不失败
|
|
// 继续尝试 VIDIOC_TRY_CTRL 看是否可用
|
|
struct v4l2_ext_controls ext_ctrls = {};
|
|
struct v4l2_ext_control ext_ctrl = {};
|
|
ext_ctrl.id = id;
|
|
ext_ctrl.value = value;
|
|
ext_ctrls.count = 1;
|
|
ext_ctrls.controls = &ext_ctrl;
|
|
|
|
if (ioctl(fd_, VIDIOC_TRY_EXT_CTRLS, &ext_ctrls) < 0) {
|
|
printf("V4l2Capture: VIDIOC_S_CTRL id=0x%08x value=%d failed: %s (ignored)\n",
|
|
id, value, strerror(errno));
|
|
return false;
|
|
} else {
|
|
printf("V4l2Capture: VIDIOC_S_CTRL id=0x%08x value=%d not supported (ignored)\n", id, value);
|
|
return false;
|
|
}
|
|
}
|
|
printf("V4l2Capture: set control id=0x%08x value=%d\n", id, value);
|
|
return true;
|
|
}
|
|
|
|
bool V4l2Capture::initMmap(uint32_t buffer_count) {
|
|
if (fd_ < 0) return false;
|
|
|
|
// 请求 MMAP 缓冲区
|
|
struct v4l2_requestbuffers req;
|
|
std::memset(&req, 0, sizeof(req));
|
|
req.count = buffer_count;
|
|
req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
|
req.memory = V4L2_MEMORY_MMAP;
|
|
|
|
if (ioctl(fd_, VIDIOC_REQBUFS, &req) < 0) {
|
|
printf("V4l2Capture: VIDIOC_REQBUFS failed: %s\n", strerror(errno));
|
|
return false;
|
|
}
|
|
if (req.count < buffer_count) {
|
|
printf("V4l2Capture: only %u buffers available (requested %u)\n", req.count, buffer_count);
|
|
buffer_count = req.count;
|
|
}
|
|
printf("V4l2Capture: allocated %u MMAP buffers\n", (unsigned)req.count);
|
|
|
|
buffers_.resize(buffer_count);
|
|
for (uint32_t i = 0; i < buffer_count; ++i) {
|
|
struct v4l2_buffer buf;
|
|
std::memset(&buf, 0, sizeof(buf));
|
|
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
|
buf.memory = V4L2_MEMORY_MMAP;
|
|
buf.index = i;
|
|
|
|
if (ioctl(fd_, VIDIOC_QUERYBUF, &buf) < 0) {
|
|
printf("V4l2Capture: VIDIOC_QUERYBUF %u failed: %s\n", i, strerror(errno));
|
|
return false;
|
|
}
|
|
|
|
buffers_[i].length = buf.length;
|
|
buffers_[i].start = mmap(nullptr, buf.length,
|
|
PROT_READ | PROT_WRITE, MAP_SHARED,
|
|
fd_, buf.m.offset);
|
|
if (buffers_[i].start == MAP_FAILED) {
|
|
printf("V4l2Capture: mmap buffer %u failed: %s\n", i, strerror(errno));
|
|
return false;
|
|
}
|
|
buffers_[i].queued = false;
|
|
printf("V4l2Capture: buffer %u: addr=%p len=%zu\n",
|
|
i, buffers_[i].start, buffers_[i].length);
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool V4l2Capture::startStreaming() {
|
|
if (fd_ < 0 || streaming_) return false;
|
|
|
|
// 把所有缓冲区入队
|
|
for (uint32_t i = 0; i < buffers_.size(); ++i) {
|
|
struct v4l2_buffer buf;
|
|
std::memset(&buf, 0, sizeof(buf));
|
|
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
|
buf.memory = V4L2_MEMORY_MMAP;
|
|
buf.index = i;
|
|
buf.bytesused = 0; // 驱动会填充
|
|
|
|
if (ioctl(fd_, VIDIOC_QBUF, &buf) < 0) {
|
|
printf("V4l2Capture: VIDIOC_QBUF %u failed: %s\n", i, strerror(errno));
|
|
return false;
|
|
}
|
|
buffers_[i].queued = true;
|
|
}
|
|
|
|
// 启动流
|
|
int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
|
if (ioctl(fd_, VIDIOC_STREAMON, &type) < 0) {
|
|
printf("V4l2Capture: VIDIOC_STREAMON failed: %s\n", strerror(errno));
|
|
return false;
|
|
}
|
|
streaming_ = true;
|
|
printf("V4l2Capture: streaming started\n");
|
|
return true;
|
|
}
|
|
|
|
bool V4l2Capture::stopStreaming() {
|
|
if (fd_ < 0 || !streaming_) return false;
|
|
|
|
int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
|
if (ioctl(fd_, VIDIOC_STREAMOFF, &type) < 0) {
|
|
printf("V4l2Capture: VIDIOC_STREAMOFF failed: %s\n", strerror(errno));
|
|
return false;
|
|
}
|
|
streaming_ = false;
|
|
last_dequeued_ = -1;
|
|
printf("V4l2Capture: streaming stopped\n");
|
|
return true;
|
|
}
|
|
|
|
bool V4l2Capture::getFrame(Frame &frame, int timeout_ms) {
|
|
if (fd_ < 0 || !streaming_) return false;
|
|
|
|
fd_set fds;
|
|
FD_ZERO(&fds);
|
|
FD_SET(fd_, &fds);
|
|
|
|
struct timeval tv;
|
|
tv.tv_sec = timeout_ms / 1000;
|
|
tv.tv_usec = (timeout_ms % 1000) * 1000;
|
|
|
|
int r = select(fd_ + 1, &fds, nullptr, nullptr, &tv);
|
|
if (r < 0) {
|
|
if (errno == EINTR) return getFrame(frame, timeout_ms); // 重试
|
|
printf("V4l2Capture: select failed: %s\n", strerror(errno));
|
|
return false;
|
|
}
|
|
if (r == 0) {
|
|
// 超时
|
|
return false;
|
|
}
|
|
|
|
struct v4l2_buffer buf;
|
|
std::memset(&buf, 0, sizeof(buf));
|
|
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
|
buf.memory = V4L2_MEMORY_MMAP;
|
|
|
|
if (ioctl(fd_, VIDIOC_DQBUF, &buf) < 0) {
|
|
if (errno == EAGAIN || errno == EIO) {
|
|
return false; // 再试
|
|
}
|
|
printf("V4l2Capture: VIDIOC_DQBUF failed: %s\n", strerror(errno));
|
|
return false;
|
|
}
|
|
|
|
// 如果之前有未归还的缓冲区, 先归还
|
|
if (last_dequeued_ >= 0) {
|
|
releaseFrameByIndex(last_dequeued_);
|
|
}
|
|
|
|
uint32_t idx = buf.index;
|
|
if (idx >= buffers_.size()) {
|
|
printf("V4l2Capture: invalid buffer index %u\n", idx);
|
|
return false;
|
|
}
|
|
|
|
last_dequeued_ = idx;
|
|
if ((buf.flags & V4L2_BUF_FLAG_ERROR) || buf.bytesused > buffers_[idx].length) {
|
|
releaseFrameByIndex(idx);
|
|
last_dequeued_ = -1;
|
|
return false;
|
|
}
|
|
|
|
frame.data = buffers_[idx].start;
|
|
frame.size = buf.bytesused;
|
|
frame.width = width_;
|
|
frame.height = height_;
|
|
frame.pixelformat = pixelformat_;
|
|
frame.bytes_per_line = bytes_per_line_;
|
|
frame.sequence = buf.sequence;
|
|
frame.timestamp_us = (uint64_t)buf.timestamp.tv_sec * 1000000ULL + buf.timestamp.tv_usec;
|
|
frame.index = idx;
|
|
|
|
return true;
|
|
}
|
|
|
|
void V4l2Capture::releaseFrame(const Frame &frame) {
|
|
if (frame.index >= 0) {
|
|
releaseFrameByIndex(frame.index);
|
|
last_dequeued_ = -1;
|
|
}
|
|
}
|
|
|
|
void V4l2Capture::releaseFrameByIndex(int idx) {
|
|
if (idx < 0 || (uint32_t)idx >= buffers_.size()) return;
|
|
struct v4l2_buffer buf;
|
|
std::memset(&buf, 0, sizeof(buf));
|
|
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
|
buf.memory = V4L2_MEMORY_MMAP;
|
|
buf.index = idx;
|
|
if (ioctl(fd_, VIDIOC_QBUF, &buf) < 0) {
|
|
printf("V4l2Capture: VIDIOC_QBUF %d failed: %s\n", idx, strerror(errno));
|
|
}
|
|
buffers_[idx].queued = true;
|
|
}
|