26 #include <sys/types.h>
30 #include <sys/select.h>
32 #define FDLDEBUG(msg) LDEBUG('[' << itsDevName << ':' << itsFd << "] " << msg)
33 #define FDLINFO(msg) LINFO('[' << itsDevName << ':' << itsFd << "] " << msg)
34 #define FDLERROR(msg) LERROR('[' << itsDevName << ':' << itsFd << "] " << msg)
35 #define FDLFATAL(msg) LFATAL('[' << itsDevName << ':' << itsFd << "] " << msg)
38 #ifndef V4L2_MODE_VIDEO
39 #define V4L2_MODE_VIDEO 2
42 #ifdef JEVOIS_PLATFORM_A33
46 unsigned int v4l2sunxiFix(
unsigned int fcc)
51 case V4L2_PIX_FMT_YUYV:
return V4L2_PIX_FMT_YUYV;
54 case V4L2_PIX_FMT_GREY:
return V4L2_PIX_FMT_GREY;
57 case V4L2_PIX_FMT_SRGGB8:
return V4L2_PIX_FMT_SRGGB8;
60 case V4L2_PIX_FMT_RGB565:
return V4L2_PIX_FMT_RGB565;
62 case V4L2_PIX_FMT_MJPEG:
return V4L2_PIX_FMT_MJPEG;
64 case V4L2_PIX_FMT_BGR24:
return V4L2_PIX_FMT_BGR24;
72 #define V4L2_COLORSPACE_DEFAULT v4l2_colorspace(0)
77 itsDevName(devname), itsNbufs(nbufs), itsBuffers(nullptr), itsStreaming(false), itsFormatOk(false),
83 itsFd = open(devname.c_str(), O_RDWR | O_NONBLOCK, 0);
84 if (itsFd == -1)
LFATAL(
"Camera device open failed on " << devname);
88 struct v4l2_input inp = { };
91 try { XIOCTL_QUIET(itsFd, VIDIOC_ENUMINPUT, &inp); }
catch (...) {
break; }
92 if (inp.type == V4L2_INPUT_TYPE_CAMERA)
94 if (camidx == -1) camidx = inp.index;
95 FDLDEBUG(devname <<
": Input " << inp.index <<
" [" << inp.name <<
"] is a camera sensor");
96 }
else FDLDEBUG(devname <<
": Input " << inp.index <<
" [" << inp.name <<
"] is a NOT camera sensor");
100 if (camidx == -1)
FDLFATAL(
"No valid camera input");
103 XIOCTL(itsFd, VIDIOC_S_INPUT, &camidx);
106 struct v4l2_capability cap = { };
107 XIOCTL(itsFd, VIDIOC_QUERYCAP, &cap);
109 FDLINFO(
"V4L2 camera " << devname <<
" card " << cap.card <<
" bus " << cap.bus_info);
112 if (cap.capabilities & V4L2_CAP_VIDEO_CAPTURE_MPLANE) itsMplane =
true;
114 if ((cap.capabilities & V4L2_CAP_VIDEO_CAPTURE) == 0 && itsMplane ==
false)
115 FDLFATAL(devname <<
" is not a video capture device");
117 if ((cap.capabilities & V4L2_CAP_STREAMING) == 0)
118 FDLFATAL(devname <<
" does not support streaming");
121 static bool showfmts =
true;
122 if (dummy ==
false && showfmts)
124 struct v4l2_fmtdesc fmtdesc = { };
125 if (itsMplane) fmtdesc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
else fmtdesc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
128 try { XIOCTL_QUIET(itsFd, VIDIOC_ENUM_FMT, &fmtdesc); }
catch (...) {
break; }
129 FDLINFO(
"Supported format " << fmtdesc.index <<
" is [" << fmtdesc.description <<
"] fcc " << std::showbase <<
130 std::hex << fmtdesc.pixelformat <<
" [" <<
jevois::fccstr(fmtdesc.pixelformat) <<
']');
140 while (itsRunning.load() ==
false) std::this_thread::sleep_for(std::chrono::milliseconds(1));
153 itsRunning.store(
false);
158 std::unique_lock lck(itsMtx, std::chrono::seconds(5));
159 if (lck.owns_lock() ==
false) {
FDLERROR(
"Timeout trying to acquire camera lock");
continue; }
161 if (itsBuffers)
delete itsBuffers;
162 if (itsFd != -1) close(itsFd);
172 void jevois::CameraDevice::run()
181 itsRunning.store(
true);
182 LDEBUG(
"run() thread ready");
190 std::vector<size_t> doneidx;
193 while (itsRunning.load())
201 if (itsDoneIdx.empty() ==
false) itsDoneIdx.swap(doneidx);
204 std::unique_lock lck(itsMtx, std::chrono::seconds(5));
205 if (lck.owns_lock() ==
false)
FDLFATAL(
"Timeout trying to acquire camera lock");
208 if (itsBuffers) {
for (
size_t idx : doneidx)
try { itsBuffers->qbuf(idx); }
catch (...) { } }
213 if (itsStreaming.load() ==
false)
216 std::this_thread::sleep_for(std::chrono::milliseconds(5));
222 if (itsBuffers && itsBuffers->nqueued() < 4)
224 LERROR(
"Running out of camera buffers - your process() function is too slow - DROPPING FRAMES");
225 size_t keep = 12345678;
230 if (itsOutputImage.valid()) keep = itsOutputImage.bufindex;
234 itsBuffers->qbufallbutone(keep);
238 FD_ZERO(&rfds); FD_ZERO(&efds); FD_SET(itsFd, &rfds); FD_SET(itsFd, &efds);
239 tv.tv_sec = 0; tv.tv_usec = 5000;
241 int ret = select(itsFd + 1, &rfds,
nullptr, &efds, &tv);
242 if (ret == -1) {
FDLERROR(
"Select error");
if (errno == EINTR)
continue;
else PLFATAL(
"Error polling camera"); }
245 if (FD_ISSET(itsFd, &efds))
FDLFATAL(
"Camera device error");
247 if (FD_ISSET(itsFd, &rfds))
250 struct v4l2_buffer buf;
251 itsBuffers->dqbuf(buf);
255 img.
width = itsFormat.fmt.pix.width;
256 img.
height = itsFormat.fmt.pix.height;
257 img.
fmt = itsFormat.fmt.pix.pixelformat;
259 img.
buf = itsBuffers->get(buf.index);
271 if (itsOutputImage.valid()) itsDoneIdx.push_back(itsOutputImage.bufindex);
274 itsOutputImage = img;
276 LDEBUG(
"Captured image " << img.
bufindex <<
" ready for processing");
279 itsOutputCondVar.notify_all();
283 std::this_thread::sleep_for(std::chrono::milliseconds(5));
289 itsRunning.store(
false);
297 LDEBUG(
"Turning on camera stream");
301 if (itsFormatOk ==
false)
FDLFATAL(
"No valid capture format was set -- ABORT");
303 if (itsStreaming.load() || itsBuffers) {
FDLERROR(
"Stream is already on -- IGNORED");
return; }
305 itsStreaming.store(
false);
322 unsigned int nbuf = 10;
325 v4l2_buf_type btype = itsMplane ? V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE : V4L2_BUF_TYPE_VIDEO_CAPTURE;
327 FDLINFO(itsBuffers->size() <<
" buffers of " << itsBuffers->get(0)->length() <<
" bytes allocated");
330 itsBuffers->qbufall();
331 FDLDEBUG(
"All buffers queued to camera driver");
334 XIOCTL(itsFd, VIDIOC_STREAMON, &btype);
337 itsStreaming.store(
true);
348 itsStreaming.store(
false);
351 for (
int i = 0; i < 20; ++i) itsOutputCondVar.notify_all();
362 FDLDEBUG(
"Turning off camera stream");
369 std::unique_lock<std::timed_mutex> lk1(itsMtx, std::defer_lock);
370 std::unique_lock<std::timed_mutex> lk2(itsOutputMtx, std::defer_lock);
371 LDEBUG(
"Ready to double-lock...");
373 LDEBUG(
"Double-lock success.");
376 itsOutputImage.invalidate();
385 int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
386 try { XIOCTL_QUIET(itsFd, VIDIOC_STREAMOFF, &type); }
catch (...) { }
389 if (itsBuffers) {
delete itsBuffers; itsBuffers =
nullptr; }
393 for (
int i = 0; i < 20; ++i) itsOutputCondVar.notify_all();
403 if (itsConvertedOutputImage.valid())
406 std::unique_lock ulck(itsOutputMtx, std::chrono::seconds(5));
407 if (ulck.owns_lock() ==
false)
FDLFATAL(
"Timeout trying to acquire output lock");
409 if (itsOutputImage.valid() ==
false)
411 if (itsOutputCondVar.wait_for(ulck, std::chrono::milliseconds(2500),
412 [&]() { return itsOutputImage.valid() || itsStreaming.load() == false; }) ==
false)
413 throw std::runtime_error(
"Timeout waiting for camera frame or camera not streaming");
416 if (itsStreaming.load() ==
false)
throw std::runtime_error(
"Camera not streaming");
418 switch (itsFormat.fmt.pix.pixelformat)
422 default:
FDLFATAL(
"Oops, cannot convert captured image");
425 img = itsConvertedOutputImage;
426 img.
bufindex = itsOutputImage.bufindex;
427 itsOutputImage.invalidate();
432 std::unique_lock ulck(itsOutputMtx, std::chrono::seconds(5));
433 if (ulck.owns_lock() ==
false)
FDLFATAL(
"Timeout trying to acquire output lock");
435 if (itsOutputImage.valid() ==
false)
437 if (itsOutputCondVar.wait_for(ulck, std::chrono::milliseconds(2500),
438 [&]() { return itsOutputImage.valid() || itsStreaming.load() == false; }) ==
false)
439 throw std::runtime_error(
"Timeout waiting for camera frame or camera not streaming");
442 if (itsStreaming.load() ==
false)
throw std::runtime_error(
"Camera not streaming");
444 img = itsOutputImage;
448 LDEBUG(
"Camera image " << img.
bufindex <<
" handed over to processing");
456 if (itsStreaming.load() ==
false)
throw std::runtime_error(
"Camera done() rejected while not streaming");
468 float const fps,
unsigned int const cropw,
unsigned int const croph)
474 if (itsStreaming.load()) streamOff();
485 itsFormat.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
486 XIOCTL(itsFd, VIDIOC_G_FMT, &itsFormat);
490 itsFormat.fmt.pix_mp.width = capw;
491 itsFormat.fmt.pix_mp.height = caph;
492 itsFormat.fmt.pix_mp.pixelformat = fmt;
493 itsFormat.fmt.pix_mp.num_planes = 1;
495 itsFormat.fmt.pix_mp.field = V4L2_FIELD_NONE;
497 LDEBUG(
"Requesting multiplane video format " << itsFormat.fmt.pix.width <<
'x' << itsFormat.fmt.pix.height <<
' ' <<
501 itsFormat.fmt.pix.width = capw;
502 itsFormat.fmt.pix.height = caph;
503 itsFormat.fmt.pix.pixelformat = fmt;
505 itsFormat.fmt.pix.field = V4L2_FIELD_NONE;
510 itsFormat.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
511 XIOCTL(itsFd, VIDIOC_G_FMT, &itsFormat);
514 itsFormat.fmt.pix.width = capw;
515 itsFormat.fmt.pix.height = caph;
516 itsFormat.fmt.pix.pixelformat = fmt;
518 itsFormat.fmt.pix.field = V4L2_FIELD_NONE;
520 LDEBUG(
"Requesting video format " << itsFormat.fmt.pix.width <<
'x' << itsFormat.fmt.pix.height <<
' ' <<
527 XIOCTL_QUIET(itsFd, VIDIOC_S_FMT, &itsFormat);
533 ". Maybe the sensor does not support requested pixel type or resolution.");
539 itsFormat.fmt.pix.pixelformat = V4L2_PIX_FMT_SRGGB8;
540 XIOCTL_QUIET(itsFd, VIDIOC_S_FMT, &itsFormat);
547 itsFormat.fmt.pix.pixelformat = V4L2_PIX_FMT_GREY;
548 XIOCTL_QUIET(itsFd, VIDIOC_S_FMT, &itsFormat);
553 ". Maybe the sensor does not support requested pixel type or resolution.");
560 XIOCTL(itsFd, VIDIOC_G_FMT, &itsFormat);
563 #ifdef JEVOIS_PLATFORM_A33
564 itsFormat.fmt.pix.pixelformat = v4l2sunxiFix(itsFormat.fmt.pix.pixelformat);
567 FDLINFO(
"Camera set video format to " << itsFormat.fmt.pix.width <<
'x' << itsFormat.fmt.pix.height <<
' ' <<
573 if (itsFormat.fmt.pix_mp.width != capw ||
574 itsFormat.fmt.pix_mp.height != caph ||
575 itsFormat.fmt.pix_mp.pixelformat != fmt)
576 FDLFATAL(
"Camera did not accept the requested video format as specified");
580 if (itsFormat.fmt.pix.width != capw ||
581 itsFormat.fmt.pix.height != caph ||
582 (itsFormat.fmt.pix.pixelformat != fmt &&
583 (fmt != V4L2_PIX_FMT_YUYV ||
584 (itsFormat.fmt.pix.pixelformat != V4L2_PIX_FMT_SRGGB8 &&
585 itsFormat.fmt.pix.pixelformat != V4L2_PIX_FMT_GREY))))
586 FDLFATAL(
"Camera did not accept the requested video format as specified");
595 struct v4l2_cropcap cropcap = { };
596 cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
597 XIOCTL_QUIET(itsFd, VIDIOC_CROPCAP, &cropcap);
599 LDEBUG(
"Cropcap bounds " << cropcap.bounds.width <<
'x' << cropcap.bounds.height <<
600 " @ (" << cropcap.bounds.left <<
", " << cropcap.bounds.top <<
')');
601 LDEBUG(
"Cropcap defrect " << cropcap.defrect.width <<
'x' << cropcap.defrect.height <<
602 " @ (" << cropcap.defrect.left <<
", " << cropcap.defrect.top <<
')');
604 struct v4l2_crop crop = { };
605 crop.type = itsFormat.type;
606 if (capw == cropw && caph == croph)
607 crop.c = cropcap.defrect;
610 crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
611 crop.c.top = ((caph - croph) >> 1) & 0xfffc;
612 crop.c.left = ((capw - cropw) >> 1) & 0xfffc;
613 crop.c.width = cropw; crop.c.height = croph;
616 itsFormat.fmt.pix.width = cropw;
617 itsFormat.fmt.pix.height = croph;
620 XIOCTL_QUIET(itsFd, VIDIOC_S_CROP, &crop);
622 LINFO(
"Set cropping rectangle to " << crop.c.width <<
'x' << crop.c.height <<
623 " @ (" << crop.c.left <<
", " << crop.c.top <<
')');
625 catch (...) {
LERROR(
"Querying/setting crop rectangle not supported"); }
628 itsFormat.fmt.pix.width = cropw;
629 itsFormat.fmt.pix.height = croph;
632 itsConvertedOutputImage.invalidate();
633 if (itsMplane ==
false && fmt == V4L2_PIX_FMT_YUYV &&
634 (itsFormat.fmt.pix.pixelformat == V4L2_PIX_FMT_SRGGB8 || itsFormat.fmt.pix.pixelformat == V4L2_PIX_FMT_GREY))
637 itsConvertedOutputImage.width = itsFormat.fmt.pix.width;
638 itsConvertedOutputImage.height = itsFormat.fmt.pix.height;
639 itsConvertedOutputImage.fmt = V4L2_PIX_FMT_YUYV;
640 itsConvertedOutputImage.fps = itsFps;
641 itsConvertedOutputImage.buf = std::make_shared<jevois::VideoBuf>(-1, itsConvertedOutputImage.bytesize(), 0, -1);
648 struct v4l2_streamparm parms = { };
649 parms.type = itsFormat.type;
650 parms.parm.capture.timeperframe = jevois::VideoMapping::fpsToV4l2(fps);
652 XIOCTL(itsFd, VIDIOC_S_PARM, &parms);
654 LDEBUG(
"Set framerate to " << fps <<
" fps");
656 catch (...) {
LERROR(
"Setting frame rate to " << fps <<
" fps failed -- IGNORED"); }