147 lines
8.9 KiB
C++
147 lines
8.9 KiB
C++
#include "camera.hpp"
|
|
#include "uyvy_converter.hpp"
|
|
#include <linux/videodev2.h>
|
|
#include <sys/eventfd.h>
|
|
#include <sys/ioctl.h>
|
|
#include <sys/mman.h>
|
|
#include <poll.h>
|
|
#include <fcntl.h>
|
|
#include <unistd.h>
|
|
#include <time.h>
|
|
#include <cerrno>
|
|
#include <cstring>
|
|
#include <limits>
|
|
#include <stdexcept>
|
|
|
|
namespace cms {
|
|
std::uint64_t monotonicNs() {
|
|
timespec time{};
|
|
if(clock_gettime(CLOCK_MONOTONIC,&time)!=0)throw std::runtime_error("clock_gettime failed");
|
|
return std::uint64_t(time.tv_sec)*1000000000ULL+time.tv_nsec;
|
|
}
|
|
namespace {
|
|
void checkedIoctl(int fd,unsigned long request,void* argument,char const* operation) {
|
|
int status;
|
|
do {status=ioctl(fd,request,argument);} while(status<0&&errno==EINTR);
|
|
if(status<0)throw std::runtime_error(std::string(operation)+": "+std::strerror(errno));
|
|
}
|
|
}
|
|
struct Camera::Impl {
|
|
struct Buffer {void* data{};std::size_t size{};};
|
|
CameraConfig config;
|
|
int fd{-1},stopFd{-1};bool streaming{};
|
|
std::uint32_t stride{},lastSequence{};bool haveSequence{};
|
|
std::uint64_t index{};
|
|
std::uint32_t consecutiveErrors{};
|
|
std::atomic<bool> stopped{false};
|
|
std::vector<Buffer> buffers;
|
|
std::unique_ptr<UyvyConverter> converter;
|
|
explicit Impl(CameraConfig c):config(std::move(c)) {
|
|
if(config.width<=0||config.width>8192||config.height<=0||config.height>8192||config.width%2||config.fps<=0||config.fps>240||config.buffers<2||config.buffers>32||config.timeout_ms<1)
|
|
throw std::invalid_argument("Invalid camera configuration");
|
|
try {
|
|
stopFd=eventfd(0,EFD_CLOEXEC|EFD_NONBLOCK);
|
|
if(stopFd<0)throw std::runtime_error("Cannot create camera stop event");
|
|
fd=open(config.device.c_str(),O_RDWR|O_NONBLOCK|O_CLOEXEC);
|
|
if(fd<0)throw std::runtime_error("Cannot open "+config.device+": "+std::strerror(errno));
|
|
v4l2_capability capability{};checkedIoctl(fd,VIDIOC_QUERYCAP,&capability,"VIDIOC_QUERYCAP");
|
|
auto caps=(capability.capabilities&V4L2_CAP_DEVICE_CAPS)?capability.device_caps:capability.capabilities;
|
|
if(!(caps&V4L2_CAP_VIDEO_CAPTURE)||!(caps&V4L2_CAP_STREAMING))throw std::runtime_error("Camera does not support single-plane streaming capture");
|
|
v4l2_format format{};format.type=V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
|
format.fmt.pix.width=config.width;format.fmt.pix.height=config.height;
|
|
format.fmt.pix.pixelformat=V4L2_PIX_FMT_UYVY;format.fmt.pix.field=V4L2_FIELD_NONE;
|
|
checkedIoctl(fd,VIDIOC_S_FMT,&format,"VIDIOC_S_FMT");
|
|
if(format.fmt.pix.pixelformat!=V4L2_PIX_FMT_UYVY||format.fmt.pix.width!=static_cast<unsigned>(config.width)||format.fmt.pix.height!=static_cast<unsigned>(config.height))
|
|
throw std::runtime_error("Camera rejected the configured UYVY resolution; refusing mismatched mask coordinates");
|
|
stride=format.fmt.pix.bytesperline;
|
|
if(stride<static_cast<unsigned>(config.width*2))throw std::runtime_error("Invalid camera row stride");
|
|
v4l2_streamparm params{};params.type=V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
|
if(ioctl(fd,VIDIOC_G_PARM,¶ms)==0&&(params.parm.capture.capability&V4L2_CAP_TIMEPERFRAME)) {
|
|
params.parm.capture.timeperframe.numerator=1;params.parm.capture.timeperframe.denominator=config.fps;
|
|
checkedIoctl(fd,VIDIOC_S_PARM,¶ms,"VIDIOC_S_PARM");
|
|
auto const& interval=params.parm.capture.timeperframe;
|
|
if(!interval.numerator||interval.denominator!=static_cast<unsigned>(config.fps)*interval.numerator)
|
|
throw std::runtime_error("Camera rejected requested frame rate");
|
|
}
|
|
v4l2_requestbuffers request{};request.count=config.buffers;
|
|
request.type=V4L2_BUF_TYPE_VIDEO_CAPTURE;request.memory=V4L2_MEMORY_MMAP;
|
|
checkedIoctl(fd,VIDIOC_REQBUFS,&request,"VIDIOC_REQBUFS");
|
|
if(request.count<2)throw std::runtime_error("Insufficient V4L2 buffers");
|
|
buffers.resize(request.count);
|
|
for(unsigned i=0;i<request.count;++i) {
|
|
v4l2_buffer buffer{};buffer.type=request.type;buffer.memory=request.memory;buffer.index=i;
|
|
checkedIoctl(fd,VIDIOC_QUERYBUF,&buffer,"VIDIOC_QUERYBUF");
|
|
if(buffer.length<std::size_t(stride)*config.height)throw std::runtime_error("Camera buffer smaller than negotiated frame");
|
|
buffers[i].size=buffer.length;
|
|
auto mapping=mmap(nullptr,buffer.length,PROT_READ|PROT_WRITE,MAP_SHARED,fd,buffer.m.offset);
|
|
if(mapping==MAP_FAILED)throw std::runtime_error("Camera mmap failed");
|
|
buffers[i].data=mapping;checkedIoctl(fd,VIDIOC_QBUF,&buffer,"VIDIOC_QBUF");
|
|
}
|
|
converter=std::make_unique<UyvyConverter>(config.width,config.height);
|
|
auto type=V4L2_BUF_TYPE_VIDEO_CAPTURE;checkedIoctl(fd,VIDIOC_STREAMON,&type,"VIDIOC_STREAMON");streaming=true;
|
|
} catch(...) {release();throw;}
|
|
}
|
|
~Impl(){release();}
|
|
void release() noexcept {
|
|
if(streaming){auto type=V4L2_BUF_TYPE_VIDEO_CAPTURE;ioctl(fd,VIDIOC_STREAMOFF,&type);streaming=false;}
|
|
for(auto const& b:buffers)if(b.data)munmap(b.data,b.size);
|
|
buffers.clear();
|
|
if(fd>=0){close(fd);fd=-1;}
|
|
if(stopFd>=0){close(stopFd);stopFd=-1;}
|
|
}
|
|
void stop() noexcept {
|
|
stopped=true;std::uint64_t one=1;
|
|
if(stopFd>=0){auto ignored=write(stopFd,&one,sizeof(one));(void)ignored;}
|
|
}
|
|
std::optional<CapturedFrame> readFrame() {
|
|
while(!stopped.load()) {
|
|
pollfd descriptors[2]={{fd,POLLIN,0},{stopFd,POLLIN,0}};
|
|
int ready=poll(descriptors,2,config.timeout_ms);
|
|
if(ready<0&&errno==EINTR)continue;
|
|
if(stopped.load()||(descriptors[1].revents&POLLIN))return {};
|
|
if(ready==0)throw std::runtime_error("Camera capture timeout: "+config.device);
|
|
if(ready<0||(descriptors[0].revents&(POLLERR|POLLHUP|POLLNVAL)))throw std::runtime_error("Camera disconnected or poll failed");
|
|
v4l2_buffer buffer{};buffer.type=V4L2_BUF_TYPE_VIDEO_CAPTURE;buffer.memory=V4L2_MEMORY_MMAP;
|
|
int status=ioctl(fd,VIDIOC_DQBUF,&buffer);
|
|
if(status<0&&(errno==EAGAIN||errno==EINTR))continue;
|
|
if(status<0)throw std::runtime_error("VIDIOC_DQBUF: "+std::string(std::strerror(errno)));
|
|
if(buffer.index>=buffers.size())throw std::runtime_error("Invalid V4L2 buffer index");
|
|
// The GMSL driver can mark its startup buffer as corrupt. Requeue
|
|
// it without conversion or inference; persistent corruption is fatal.
|
|
if(buffer.flags&V4L2_BUF_FLAG_ERROR) {
|
|
checkedIoctl(fd,VIDIOC_QBUF,&buffer,"VIDIOC_QBUF(error frame)");
|
|
if(++consecutiveErrors>=static_cast<unsigned>(config.fps))
|
|
throw std::runtime_error("Camera produced one second of consecutive error frames");
|
|
continue;
|
|
}
|
|
CapturedFrame frame;
|
|
try {
|
|
if(buffer.bytesused<std::size_t(stride)*config.height||buffer.bytesused>buffers[buffer.index].size)
|
|
throw std::runtime_error("Invalid camera frame: flags="+std::to_string(buffer.flags)+", used="+std::to_string(buffer.bytesused)+", required="+std::to_string(std::size_t(stride)*config.height)+", allocated="+std::to_string(buffers[buffer.index].size));
|
|
auto& m=frame.metadata;
|
|
m.discarded_error_frames=consecutiveErrors;consecutiveErrors=0;
|
|
m.driver_sequence=buffer.sequence;m.received_monotonic_ns=monotonicNs();
|
|
m.driver_timestamp_ns=std::uint64_t(buffer.timestamp.tv_sec)*1000000000ULL+std::uint64_t(buffer.timestamp.tv_usec)*1000;
|
|
m.driver_timestamp_monotonic=(buffer.flags&V4L2_BUF_FLAG_TIMESTAMP_MASK)==V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
|
|
m.capture_monotonic_ns=m.received_monotonic_ns; // Host dequeue clock; retain raw driver timestamp separately.
|
|
if(haveSequence) {
|
|
auto delta=static_cast<std::uint32_t>(buffer.sequence-lastSequence);
|
|
if(delta==0||delta>0x7fffffffU)throw std::runtime_error("Camera sequence reset or repeated frame");
|
|
m.skipped_driver_frames=delta-1;index+=delta;
|
|
}
|
|
m.frame_index=index;
|
|
lastSequence=buffer.sequence;haveSequence=true;
|
|
// Convert before requeuing. No published result references MMAP memory.
|
|
frame.image=converter->convert(buffers[buffer.index].data,stride);
|
|
} catch(...) {ioctl(fd,VIDIOC_QBUF,&buffer);throw;}
|
|
checkedIoctl(fd,VIDIOC_QBUF,&buffer,"VIDIOC_QBUF");return frame;
|
|
}
|
|
return {};
|
|
}
|
|
};
|
|
Camera::Camera(CameraConfig config):impl_(std::make_unique<Impl>(std::move(config))){}
|
|
Camera::~Camera()=default;
|
|
std::optional<CapturedFrame> Camera::read(){return impl_->readFrame();}
|
|
void Camera::requestStop() noexcept {impl_->stop();}
|
|
}
|