From 73a97c17747ff7f32ed9b562eec90b9770c6b5dd Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Sat, 15 Oct 2016 17:24:47 -0700 Subject: [PATCH] USBCamera: Finish implementing mode support. --- src/USBCameraImpl.cpp | 135 ++++++++++++++++++++++++++---------------- src/USBCameraImpl.h | 5 +- 2 files changed, 88 insertions(+), 52 deletions(-) diff --git a/src/USBCameraImpl.cpp b/src/USBCameraImpl.cpp index acaf3c6d4d..c8a044d2a3 100644 --- a/src/USBCameraImpl.cpp +++ b/src/USBCameraImpl.cpp @@ -617,44 +617,14 @@ void USBCameraImpl::DeviceConnect() { // Get or restore video mode if (!m_properties_cached) { DEBUG3("USB " << m_path << ": caching properties"); - DeviceCacheMode(); DeviceCacheProperties(); DeviceCacheVideoModes(); + DeviceCacheMode(); m_properties_cached = true; } else { DEBUG3("USB " << m_path << ": restoring video mode"); - struct v4l2_format vfmt; - std::memset(&vfmt, 0, sizeof(vfmt)); -#ifdef V4L2_CAP_EXT_PIX_FORMAT - vfmt.fmt.pix.priv = (m_capabilities & V4L2_CAP_EXT_PIX_FORMAT) != 0 - ? V4L2_PIX_FMT_PRIV_MAGIC - : 0; -#endif - vfmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; - switch (m_mode.pixelFormat) { - case VideoMode::kMJPEG: - vfmt.fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG; - break; - case VideoMode::kYUYV: - vfmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV; - break; - case VideoMode::kRGB565: - vfmt.fmt.pix.pixelformat = V4L2_PIX_FMT_RGB565; - break; - default: - WARNING("USB " << m_path << ": could not set format " - << m_mode.pixelFormat << ", defaulting to MJPEG"); - vfmt.fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG; - break; - } - vfmt.fmt.pix.width = m_mode.width; - vfmt.fmt.pix.height = m_mode.height; - vfmt.fmt.pix.field = V4L2_FIELD_ANY; - if (DoIoctl(fd, VIDIOC_S_FMT, &vfmt) != 0) { - WARNING("USB " << m_path << ": could not set format " - << m_mode.pixelFormat << " res " << m_mode.width << "x" - << m_mode.height); - } + DeviceSetMode(); + DeviceSetFPS(); // Restore settings DEBUG3("USB " << m_path << ": restoring settings"); @@ -786,22 +756,25 @@ void USBCameraImpl::DeviceProcessCommands() { m_modeSetFPS = true; } - // If the resolution changed, we need to disconnect and reconnect - if (newMode.width != m_mode.width || newMode.height != m_mode.height) { + // If the pixel format or resolution changed, we need to disconnect and + // reconnect + if (newMode.pixelFormat != m_mode.pixelFormat || + newMode.width != m_mode.width || newMode.height != m_mode.height) { + m_mode = newMode; lock.unlock(); bool wasStreaming = m_streaming; if (wasStreaming) DeviceStreamOff(); - m_mode = newMode; if (m_fd >= 0) { DeviceDisconnect(); DeviceConnect(); - DeviceSetFPS(); } if (wasStreaming) DeviceStreamOn(); lock.lock(); - } else { - // TODO + } else if (newMode.fps != m_mode.fps) { m_mode = newMode; + lock.unlock(); + DeviceSetFPS(); + lock.lock(); } } else if (msg->type == Message::kCmdSetProperty || msg->type == Message::kCmdSetPropertyStr) { @@ -880,15 +853,48 @@ done: m_responseCv.notify_all(); } +void USBCameraImpl::DeviceSetMode() { + int fd = m_fd.load(); + if (fd < 0) return; + + struct v4l2_format vfmt; + std::memset(&vfmt, 0, sizeof(vfmt)); +#ifdef V4L2_CAP_EXT_PIX_FORMAT + vfmt.fmt.pix.priv = (m_capabilities & V4L2_CAP_EXT_PIX_FORMAT) != 0 + ? V4L2_PIX_FMT_PRIV_MAGIC + : 0; +#endif + vfmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + switch (m_mode.pixelFormat) { + case VideoMode::kMJPEG: + vfmt.fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG; + break; + case VideoMode::kYUYV: + vfmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV; + break; + case VideoMode::kRGB565: + vfmt.fmt.pix.pixelformat = V4L2_PIX_FMT_RGB565; + break; + default: + WARNING("USB " << m_path << ": could not set format " + << m_mode.pixelFormat << ", defaulting to MJPEG"); + vfmt.fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG; + break; + } + vfmt.fmt.pix.width = m_mode.width; + vfmt.fmt.pix.height = m_mode.height; + vfmt.fmt.pix.field = V4L2_FIELD_ANY; + if (DoIoctl(fd, VIDIOC_S_FMT, &vfmt) != 0) { + WARNING("USB " << m_path << ": could not set format " + << m_mode.pixelFormat << " res " << m_mode.width << "x" + << m_mode.height); + } +} + void USBCameraImpl::DeviceSetFPS() { int fd = m_fd.load(); if (fd < 0) return; - int fps; - { - std::lock_guard lock(m_mutex); - fps = m_mode.fps; - } struct v4l2_streamparm parm; std::memset(&parm, 0, sizeof(parm)); parm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; @@ -896,7 +902,7 @@ void USBCameraImpl::DeviceSetFPS() { if ((parm.parm.capture.capability & V4L2_CAP_TIMEPERFRAME) == 0) return; std::memset(&parm, 0, sizeof(parm)); parm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; - parm.parm.capture.timeperframe = FPSToFract(fps); + parm.parm.capture.timeperframe = FPSToFract(m_mode.fps); if (DoIoctl(fd, VIDIOC_S_PARM, &parm) != 0) return; } @@ -934,6 +940,21 @@ void USBCameraImpl::DeviceCacheMode() { pixelFormat = VideoMode::kUnknown; break; } + int width = vfmt.fmt.pix.width; + int height = vfmt.fmt.pix.height; + + // Update format with user changes. + bool formatChanged = false; + if (m_modeSetPixelFormat && pixelFormat != m_mode.pixelFormat) { + formatChanged = true; + pixelFormat = static_cast(m_mode.pixelFormat); + } + if (m_modeSetResolution && + (width != m_mode.width || height != m_mode.height)) { + formatChanged = true; + width = m_mode.width; + height = m_mode.height; + } // Get FPS int fps = 0; @@ -945,12 +966,24 @@ void USBCameraImpl::DeviceCacheMode() { fps = FractToFPS(parm.parm.capture.timeperframe); } - // Save - std::lock_guard lock(m_mutex); - m_mode.pixelFormat = pixelFormat; - m_mode.width = vfmt.fmt.pix.width; - m_mode.height = vfmt.fmt.pix.height; - m_mode.fps = fps; + // Update FPS with user changes + bool fpsChanged = false; + if (m_modeSetFPS && fps != m_mode.fps) { + fpsChanged = true; + fps = m_mode.fps; + } + + // Save to global mode + { + std::lock_guard lock(m_mutex); + m_mode.pixelFormat = pixelFormat; + m_mode.width = width; + m_mode.height = height; + m_mode.fps = fps; + } + + if (formatChanged) DeviceSetMode(); + if (fpsChanged) DeviceSetFPS(); } void USBCameraImpl::DeviceCacheProperty(PropertyData&& prop) { diff --git a/src/USBCameraImpl.h b/src/USBCameraImpl.h index ace4cf27b2..9a28dfdebb 100644 --- a/src/USBCameraImpl.h +++ b/src/USBCameraImpl.h @@ -110,6 +110,8 @@ class USBCameraImpl : public SourceImpl { kError }; + Message(Type type_) : type(type_) {} + Type type; int data[4]; std::string dataStr; @@ -119,7 +121,7 @@ class USBCameraImpl : public SourceImpl { // Message pool access std::unique_ptr CreateMessage(Message::Type type) const { std::lock_guard lock(m_mutex); - if (m_messagePool.empty()) return llvm::make_unique(); + if (m_messagePool.empty()) return llvm::make_unique(type); auto rv = std::move(m_messagePool.back()); m_messagePool.pop_back(); rv->type = type; @@ -149,6 +151,7 @@ class USBCameraImpl : public SourceImpl { bool DeviceStreamOn(); bool DeviceStreamOff(); void DeviceProcessCommands(); + void DeviceSetMode(); void DeviceSetFPS(); void DeviceCacheMode(); void DeviceCacheProperty(PropertyData&& prop);