works on radxa zero 3e, 420 fps

2 files changed, 378 insertions, 77 deletions

diff --git a/src/camera/veyeimx287m.cpp b/src/camera/veyeimx287m.cpp
index e89442c..1f9bc20 100644
--- a/src/camera/veyeimx287m.cpp
+++ b/src/camera/veyeimx287m.cpp
@@ -20,6 +20,159 @@
 #include "pixels.h"
 // #include "rotaryencoder.h"
 
+static const struct v4l2_format_info
+{
+    const char *name;
+    unsigned int fourcc;
+    unsigned char n_planes;
+} pixel_formats[] = {
+    {"RGB332", V4L2_PIX_FMT_RGB332, 1},
+    {"RGB444", V4L2_PIX_FMT_RGB444, 1},
+    {"ARGB444", V4L2_PIX_FMT_ARGB444, 1},
+    {"XRGB444", V4L2_PIX_FMT_XRGB444, 1},
+    {"RGB555", V4L2_PIX_FMT_RGB555, 1},
+    {"ARGB555", V4L2_PIX_FMT_ARGB555, 1},
+    {"XRGB555", V4L2_PIX_FMT_XRGB555, 1},
+    {"RGB565", V4L2_PIX_FMT_RGB565, 1},
+    {"RGB555X", V4L2_PIX_FMT_RGB555X, 1},
+    {"RGB565X", V4L2_PIX_FMT_RGB565X, 1},
+    {"BGR666", V4L2_PIX_FMT_BGR666, 1},
+    {"BGR24", V4L2_PIX_FMT_BGR24, 1},
+    {"RGB24", V4L2_PIX_FMT_RGB24, 1},
+    {"BGR32", V4L2_PIX_FMT_BGR32, 1},
+    {"ABGR32", V4L2_PIX_FMT_ABGR32, 1},
+    {"XBGR32", V4L2_PIX_FMT_XBGR32, 1},
+    {"RGB32", V4L2_PIX_FMT_RGB32, 1},
+    {"ARGB32", V4L2_PIX_FMT_ARGB32, 1},
+    {"XRGB32", V4L2_PIX_FMT_XRGB32, 1},
+    {"HSV24", V4L2_PIX_FMT_HSV24, 1},
+    {"HSV32", V4L2_PIX_FMT_HSV32, 1},
+    {"Y8", V4L2_PIX_FMT_GREY, 1},
+    {"Y10", V4L2_PIX_FMT_Y10, 1},
+    {"Y12", V4L2_PIX_FMT_Y12, 1},
+    {"Y16", V4L2_PIX_FMT_Y16, 1},
+    {"UYVY", V4L2_PIX_FMT_UYVY, 1},
+    {"VYUY", V4L2_PIX_FMT_VYUY, 1},
+    {"YUYV", V4L2_PIX_FMT_YUYV, 1},
+    {"YVYU", V4L2_PIX_FMT_YVYU, 1},
+    {"YUV32", V4L2_PIX_FMT_YUV32, 1},
+    {"AYUV32", V4L2_PIX_FMT_AYUV32, 1},
+    {"XYUV32", V4L2_PIX_FMT_XYUV32, 1},
+    {"VUYA32", V4L2_PIX_FMT_VUYA32, 1},
+    {"VUYX32", V4L2_PIX_FMT_VUYX32, 1},
+    {"YUVA32", V4L2_PIX_FMT_YUVA32, 1},
+    {"YUVX32", V4L2_PIX_FMT_YUVX32, 1},
+    {"NV12", V4L2_PIX_FMT_NV12, 1},
+    {"NV12M", V4L2_PIX_FMT_NV12M, 2},
+    {"NV21", V4L2_PIX_FMT_NV21, 1},
+    {"NV21M", V4L2_PIX_FMT_NV21M, 2},
+    {"NV16", V4L2_PIX_FMT_NV16, 1},
+    {"NV16M", V4L2_PIX_FMT_NV16M, 2},
+    {"NV61", V4L2_PIX_FMT_NV61, 1},
+    {"NV61M", V4L2_PIX_FMT_NV61M, 2},
+    {"NV24", V4L2_PIX_FMT_NV24, 1},
+    {"NV42", V4L2_PIX_FMT_NV42, 1},
+    {"YUV420M", V4L2_PIX_FMT_YUV420M, 3},
+    {"YUV422M", V4L2_PIX_FMT_YUV422M, 3},
+    {"YUV444M", V4L2_PIX_FMT_YUV444M, 3},
+    {"YVU420M", V4L2_PIX_FMT_YVU420M, 3},
+    {"YVU422M", V4L2_PIX_FMT_YVU422M, 3},
+    {"YVU444M", V4L2_PIX_FMT_YVU444M, 3},
+    {"SBGGR8", V4L2_PIX_FMT_SBGGR8, 1},
+    {"SGBRG8", V4L2_PIX_FMT_SGBRG8, 1},
+    {"SGRBG8", V4L2_PIX_FMT_SGRBG8, 1},
+    {"SRGGB8", V4L2_PIX_FMT_SRGGB8, 1},
+    {"SBGGR10_DPCM8", V4L2_PIX_FMT_SBGGR10DPCM8, 1},
+    {"SGBRG10_DPCM8", V4L2_PIX_FMT_SGBRG10DPCM8, 1},
+    {"SGRBG10_DPCM8", V4L2_PIX_FMT_SGRBG10DPCM8, 1},
+    {"SRGGB10_DPCM8", V4L2_PIX_FMT_SRGGB10DPCM8, 1},
+    {"SBGGR10", V4L2_PIX_FMT_SBGGR10, 1},
+    {"SGBRG10", V4L2_PIX_FMT_SGBRG10, 1},
+    {"SGRBG10", V4L2_PIX_FMT_SGRBG10, 1},
+    {"SRGGB10", V4L2_PIX_FMT_SRGGB10, 1},
+    {"SBGGR10P", V4L2_PIX_FMT_SBGGR10P, 1},
+    {"SGBRG10P", V4L2_PIX_FMT_SGBRG10P, 1},
+    {"SGRBG10P", V4L2_PIX_FMT_SGRBG10P, 1},
+    {"SRGGB10P", V4L2_PIX_FMT_SRGGB10P, 1},
+    {"SBGGR12", V4L2_PIX_FMT_SBGGR12, 1},
+    {"SGBRG12", V4L2_PIX_FMT_SGBRG12, 1},
+    {"SGRBG12", V4L2_PIX_FMT_SGRBG12, 1},
+    {"SRGGB12", V4L2_PIX_FMT_SRGGB12, 1},
+    {"SBGGR16", V4L2_PIX_FMT_SBGGR16, 1},
+    {"SGBRG16", V4L2_PIX_FMT_SGBRG16, 1},
+    {"SGRBG16", V4L2_PIX_FMT_SGRBG16, 1},
+    {"SRGGB16", V4L2_PIX_FMT_SRGGB16, 1},
+    {"IPU3_SBGGR10", V4L2_PIX_FMT_IPU3_SBGGR10, 1},
+    {"IPU3_SGBRG10", V4L2_PIX_FMT_IPU3_SGBRG10, 1},
+    {"IPU3_SGRBG10", V4L2_PIX_FMT_IPU3_SGRBG10, 1},
+    {"IPU3_SRGGB10", V4L2_PIX_FMT_IPU3_SRGGB10, 1},
+    {"IPU3_Y10", V4L2_PIX_FMT_IPU3_Y10, 1},
+    {"DV", V4L2_PIX_FMT_DV, 1},
+    {"MJPEG", V4L2_PIX_FMT_MJPEG, 1},
+    {"MPEG", V4L2_PIX_FMT_MPEG, 1},
+};
+
+static const struct
+{
+    const char *name;
+    enum v4l2_field field;
+} fields[] = {
+    {"any", V4L2_FIELD_ANY},
+    {"none", V4L2_FIELD_NONE},
+    {"top", V4L2_FIELD_TOP},
+    {"bottom", V4L2_FIELD_BOTTOM},
+    {"interlaced", V4L2_FIELD_INTERLACED},
+    {"seq-tb", V4L2_FIELD_SEQ_TB},
+    {"seq-bt", V4L2_FIELD_SEQ_BT},
+    {"alternate", V4L2_FIELD_ALTERNATE},
+    {"interlaced-tb", V4L2_FIELD_INTERLACED_TB},
+    {"interlaced-bt", V4L2_FIELD_INTERLACED_BT},
+};
+
+#define ARRAY_SIZE(a) (sizeof(a) / sizeof((a)[0]))
+
+static const char *v4l2_field_name(enum v4l2_field field)
+{
+    unsigned int i;
+
+    for (i = 0; i < ARRAY_SIZE(fields); ++i) {
+        if (fields[i].field == field)
+            return fields[i].name;
+    }
+
+    return "unknown";
+}
+
+static const struct v4l2_format_info *v4l2_format_by_fourcc(unsigned int fourcc)
+{
+    unsigned int i;
+
+    for (i = 0; i < ARRAY_SIZE(pixel_formats); ++i) {
+        if (pixel_formats[i].fourcc == fourcc)
+            return &pixel_formats[i];
+    }
+
+    return NULL;
+}
+static const char *v4l2_format_name(unsigned int fourcc)
+{
+    const struct v4l2_format_info *info;
+    static char name[5];
+    unsigned int i;
+
+    info = v4l2_format_by_fourcc(fourcc);
+    if (info)
+        return info->name;
+
+    for (i = 0; i < 4; ++i) {
+        name[i] = fourcc & 0xff;
+        fourcc >>= 8;
+    }
+
+    name[4] = '\0';
+    return name;
+}
+
 #define LOGD(...) \
     do { \
         printf(__VA_ARGS__); \
@@ -52,16 +205,26 @@ VeyeIMX287m::~VeyeIMX287m()
     m_streamThread.request_stop();
     m_streamThread.join();
 
-    int buffer_type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-    if (ioctl(m_cam_fd, VIDIOC_STREAMOFF, &buffer_type) == -1) {
+    // int buffer_type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+    const auto radxa_buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+    // if (ioctl(m_cam_fd, VIDIOC_STREAMOFF, &buffer_type) == -1) {
+    if (ioctl(m_cam_fd, VIDIOC_STREAMOFF, &radxa_buf_type) == -1) {
         std::cout << "cannot stop stream" << std::endl;
     }
 
+#ifdef RADXA_ZERO_3E
+    for (const auto buffer : buffers) {
+        if (munmap(buffer.mem[0], radxa_raw_img_size) < 0) {
+            DBG("Munmap failed!!.");
+        }
+    }
+#else
     for (const auto buffer : m_videoBuffers) {
         if (munmap(buffer, img_size) < 0) {
             DBG("Munmap failed!!.");
         }
     }
+#endif // RADXA_ZERO_3E
 
     if (m_cam_fd >= 0) {
         if (close(m_cam_fd) == -1) {
@@ -110,14 +273,19 @@ std::vector<std::shared_ptr<ICamera> > VeyeIMX287m::search()
 
 bool VeyeIMX287m::startStream()
 {
-    int buffer_type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-    auto ret = ioctl(m_cam_fd, VIDIOC_STREAMON, &buffer_type);
+    // int buffer_type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+    const auto radxa_buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+    // auto ret = ioctl(m_cam_fd, VIDIOC_STREAMON, &buffer_type);
+    auto ret = ioctl(m_cam_fd, VIDIOC_STREAMON, &radxa_buf_type);
 
     if (ret != 0) {
-        DBG("ioctl(VIDIOC_STREAMON) failed %d(%s)", errno, strerror(errno));
+        std::cerr << "ioctl(VIDIOC_STREAMON) failed: " << errno << " (" << strerror(errno) << ")"
+                  << std::endl;
         return false;
     }
 
+    std::cout << "stream started" << std::endl;
+
     // m_streamThread = std::jthread{&VeyeIMX287m::dequeueFrameLoop, this};
 
     for (auto &t : m_calcThreads) {
@@ -132,8 +300,8 @@ bool VeyeIMX287m::init()
     if (!openCam())
         return false;
 
-    if (!selectCam())
-        return false;
+    // if (!selectCam())
+    //     return false;
 
     if (!initCam())
         return false;
@@ -198,6 +366,10 @@ bool VeyeIMX287m::setSomething(int value)
 
 bool VeyeIMX287m::setCamParam(unsigned int v4l2controlId, int value)
 {
+    std::cout << "radxa: skip setCamParam" << std::endl;
+
+    return true;
+
     v4l2_control ctl{v4l2controlId, value};
 
     if (ioctl(m_cam_fd, VIDIOC_S_CTRL, &ctl) < 0) {
@@ -254,37 +426,87 @@ bool VeyeIMX287m::selectCam(int camIdx)
 
 bool VeyeIMX287m::initCam()
 {
-    v4l2_format format;
-    memset(&format, 0, sizeof(v4l2_format));
-    format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-    format.fmt.pix.pixelformat = V4L2_PIX_FMT_GREY;
-    format.fmt.pix.width = img_width;
-    format.fmt.pix.height = img_height;
+    int ret{-1};
+    constexpr bool radxa_zero_3et{true};
 
-    int ret = ioctl(m_cam_fd, VIDIOC_TRY_FMT, &format);
+    const auto radxa_buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
 
-    if (ret < 0) {
-        fprintf(stderr, "cannot try cam format: error - '%s'\n", strerror(errno));
+    if constexpr (!radxa_zero_3et) {
+        v4l2_format format;
+        memset(&format, 0, sizeof(v4l2_format));
+        format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+        format.fmt.pix.pixelformat = V4L2_PIX_FMT_GREY;
+        format.fmt.pix.width = img_width;
+        format.fmt.pix.height = img_height;
+
+        ret = ioctl(m_cam_fd, VIDIOC_TRY_FMT, &format);
+
+        if (ret < 0) {
+            fprintf(stderr, "cannot try cam format: error - '%s'\n", strerror(errno));
+
+            return false;
+        }
+
+        // TODO: remove this?
+        format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+        ret = ioctl(m_cam_fd, VIDIOC_S_FMT, &format);
+
+        if (ret < 0) {
+            fprintf(stderr, "cannot set cam format: error - '%s'\n", strerror(errno));
+
+            return false;
+        }
+    }
+
+    v4l2_format fmt;
+    memset(&fmt, 0, sizeof fmt);
+    fmt.type = radxa_buf_type;
+
+    if (ioctl(m_cam_fd, VIDIOC_G_FMT, &fmt) < 0) {
+        printf("Unable to get format: %s (%d).\n", strerror(errno), errno);
 
         return false;
     }
 
-    // TODO: remove this?
-    format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+    const auto width = fmt.fmt.pix_mp.width;
+    const auto height = fmt.fmt.pix_mp.height;
+    const int num_planes = fmt.fmt.pix_mp.num_planes;
 
-    ret = ioctl(m_cam_fd, VIDIOC_S_FMT, &format);
+    std::cout << "num_planes: " << num_planes << std::endl;
 
-    if (ret < 0) {
-        fprintf(stderr, "cannot set cam format: error - '%s'\n", strerror(errno));
+    if (num_planes != 1) {
+        std::cerr << "multiple planes are not supported" << std::endl;
 
         return false;
     }
 
-    struct v4l2_requestbuffers request;
-    request.count = BUFFER_COUNT;
-    request.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-    request.memory = V4L2_MEMORY_MMAP;
-    ret = ioctl(m_cam_fd, VIDIOC_REQBUFS, &request);
+    printf("Video format: %s (%08x) %ux%u field %s, %u planes: \n",
+           v4l2_format_name(fmt.fmt.pix_mp.pixelformat),
+           fmt.fmt.pix_mp.pixelformat,
+           fmt.fmt.pix_mp.width,
+           fmt.fmt.pix_mp.height,
+           v4l2_field_name((enum v4l2_field) fmt.fmt.pix_mp.field),
+           fmt.fmt.pix_mp.num_planes);
+
+    for (int i = 0; i < fmt.fmt.pix_mp.num_planes; i++) {
+        printf(" * Stride %u, buffer size %u\n",
+               fmt.fmt.pix_mp.plane_fmt[i].bytesperline,
+               fmt.fmt.pix_mp.plane_fmt[i].sizeimage);
+        fflush(stdout);
+    }
+
+    struct v4l2_requestbuffers rb;
+    memset(&rb, 0, sizeof rb);
+    rb.count = BUFFER_COUNT;
+    if constexpr (radxa_zero_3et) {
+        rb.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+    } else {
+        rb.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+    }
+    rb.memory = V4L2_MEMORY_MMAP;
+
+    ret = ioctl(m_cam_fd, VIDIOC_REQBUFS, &rb);
 
     if (ret < 0) {
         fprintf(stderr, "cannot set cam request buffers: ioctl error - '%s'\n", strerror(errno));
@@ -292,45 +514,97 @@ bool VeyeIMX287m::initCam()
         return false;
     }
 
-    if (request.count < BUFFER_COUNT) {
+    std::cout << "buffers requested" << std::endl;
+
+    if (rb.count < BUFFER_COUNT) {
         fprintf(stderr, "cannot set cam request buffers\n");
 
         return false;
     }
 
-    struct v4l2_buffer buffer;
-    memset(&buffer, 0, sizeof(buffer));
-    buffer.type = request.type;
-    buffer.memory = V4L2_MEMORY_MMAP;
+    std::cout << "buffers count is ok: " << rb.count << std::endl;
 
-    for (uint32_t i = 0; i < request.count; i++) {
-        buffer.index = i;
-        ret = ioctl(m_cam_fd, VIDIOC_QUERYBUF, &buffer);
+    buffers.resize(rb.count);
+
+    // memset(&buffer, 0, sizeof(buffer));
+    // buffer.type = request.type;
+    // buffer.memory = V4L2_MEMORY_MMAP;
+
+    std::cout << "query buffers" << std::endl;
+
+    for (uint32_t i = 0; i < rb.count; i++) {
+        std::cout << "-----------------------------------------------------" << std::endl;
+        struct v4l2_buffer buf;
+        struct v4l2_plane planes[VIDEO_MAX_PLANES];
+
+        memset(&buf, 0, sizeof buf);
+        memset(planes, 0, sizeof planes);
+
+        buf.index = i;
+        buf.type = rb.type;
+        buf.memory = V4L2_MEMORY_MMAP;
+        buf.length = VIDEO_MAX_PLANES;
+        buf.m.planes = planes;
+
+        std::cout << "run ioctl(VIDIOC_QUERYBUF) for buf #" << i << std::endl;
+        ret = ioctl(m_cam_fd, VIDIOC_QUERYBUF, &buf);
 
         if (ret < 0) {
-            DBG("ioctl(VIDIOC_QUERYBUF) failed %d(%s)", errno, strerror(errno));
+            // std::cout << "ioctl(VIDIOC_QUERYBUF) failed: " << errno << " " << std::endl;
+            std::cerr << "ioctl(VIDIOC_QUERYBUF) failed: " << errno << " (" << strerror(errno)
+                      << ")" << std::endl;
             return false;
         }
 
-        DBG("buffer.length: %d", buffer.length);
-        DBG("buffer.m.offset: %d", buffer.m.offset);
-        m_videoBuffers[i] = (uint8_t *)
-            mmap(NULL, buffer.length, PROT_READ | PROT_WRITE, MAP_SHARED, m_cam_fd, buffer.m.offset);
-        if (m_videoBuffers[i] == MAP_FAILED) {
-            DBG("mmap() failed %d(%s)", errno, strerror(errno));
+        std::cout << "ioctl(VIDIOC_QUERYBUF) is ok: " << std::endl;
+
+        std::cout << "buffer.length: " << buf.length << std::endl;
+        std::cout << "buffer.m.offset: " << buf.m.offset << std::endl;
+
+        buffers[i].idx = i;
+
+        const auto length = buf.m.planes[0].length;
+        const auto offset = buf.m.planes[0].m.mem_offset;
+        buffers[i].mem[0] = mmap(0, length, PROT_READ | PROT_WRITE, MAP_SHARED, m_cam_fd, offset);
+        if (buffers[i].mem[0] == MAP_FAILED) {
+            std::cerr << "mmap() failed: " << errno << " (" << strerror(errno) << ")" << std::endl;
+            std::cerr << "length: " << length << std::endl;
+            std::cerr << "offset: " << offset << std::endl;
             return false;
+
+            // m_videoBuffers[i] = (uint8_t *)
+            //     mmap(NULL, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, m_cam_fd, buf.m.offset);
+            // if (m_videoBuffers[i] == MAP_FAILED) {
+            //     DBG("mmap() failed %d(%s)", errno, strerror(errno));
+            //     return false;
         }
 
-        buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-        buffer.memory = V4L2_MEMORY_MMAP;
-        buffer.index = i;
-        ret = ioctl(m_cam_fd, VIDIOC_QBUF, &buffer);
+        m_videoBuffers[i] = (uint8_t *) buffers[i].mem[0];
+
+        buffers[i].size[i] = length;
+        buffers[i].padding[i] = 0;
+
+        printf("Buffer %u/%u mapped at address %p.\n", buffers[i].idx, i, buffers[i].mem[0]);
+
+        // buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+        // buf.memory = V4L2_MEMORY_MMAP;
+        // buf.index = i;
+        ret = ioctl(m_cam_fd, VIDIOC_QBUF, &buf);
         if (ret != 0) {
-            DBG("ioctl(VIDIOC_QBUF) failed %d(%s)", errno, strerror(errno));
+            std::cerr << "ioctl(VIDIOC_QBUF) failed: " << errno << " (" << strerror(errno) << ")"
+                      << std::endl;
             return false;
         }
+
+        std::cout << "ioctl(VIDIOC_QBUF) is OK" << std::endl;
     }
 
+    fflush(stdout);
+    fflush(stderr);
+
+    // std::cout << "test return false" << std::endl;
+    // return false;
+
     // int buffer_type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
     // ret = ioctl(m_cam_fd, VIDIOC_STREAMON, &buffer_type);
     // if (ret != 0)
@@ -379,6 +653,7 @@ void VeyeIMX287m::calcFrameLoop(std::stop_token stopToken)
             // std::lock_guard buffer_lock{m_bufferMutexes[bufferIdx]};
             // get: 4100-4500
             memcpy(&image.data, m_videoBuffers[bufferIdx], img_size);
+            // memcpy(&image.data, buffers[bufferIdx].mem[0], img_size);
             // get: 5000-5100
             // memcpy_1by1<img_size / sizeof(ARRAY_TYPE)>((ARRAY_TYPE *) &image.data,
             //                                            (ARRAY_TYPE *) m_videoBuffers[bufferIdx]);
@@ -402,11 +677,14 @@ void VeyeIMX287m::calcFrameLoop(std::stop_token stopToken)
 
         image.rotate();
         // const auto pixels = image.pixels();
-        const auto pixels = image.sharedPixels();
+        // static const auto pixels = image.sharedPixels();
+
+        ++processedCounter;
+
 #pragma push_macro("emit")
 #undef emit
         // std::cout << "emit pixels" << std::endl;
-        newPixels.emit(pixels);
+        // newPixels.emit(pixels);
 #pragma pop_macro("emit")
         // const auto lines = pixelsToLines(*pixels);
         // continue;
@@ -435,14 +713,14 @@ bool VeyeIMX287m::dequeueImageBuffer(size_t &imageIndex)
                 counter,
                 dropped_count,
                 curr.tv_sec % 1000,
-                dq_elapsed_ns / 1000 / counter,
-                get_elapsed_ns / 1000 / counter,
-                rot_elapsed_ns / 1000 / counter,
-                pix_elapsed_ns / 1000 / counter,
-                sum_elapsed_ns / 1000 / counter,
-                corr_elapsed_ns / 1000 / counter,
-                // max_elapsed_ns / 1000 / counter,
-                value_elapsed_ns / 1000 / counter);
+                dq_elapsed_ns / 1000 / processedCounter,
+                get_elapsed_ns / 1000 / processedCounter,
+                rot_elapsed_ns / 1000 / processedCounter,
+                pix_elapsed_ns / 1000 / processedCounter,
+                sum_elapsed_ns / 1000 / processedCounter,
+                corr_elapsed_ns / 1000 / processedCounter,
+                // max_elapsed_ns / 1000 / processedCounter,
+                value_elapsed_ns / 1000 / processedCounter);
 
         dq_elapsed_ns = 0;
         get_elapsed_ns = 0;
@@ -455,18 +733,26 @@ bool VeyeIMX287m::dequeueImageBuffer(size_t &imageIndex)
         dropped_count = 0;
 
         counter = 0;
+        processedCounter = 0;
         prev = curr;
     }
 
     int ret;
-    struct v4l2_buffer buffer;
+    struct v4l2_buffer buf;
+    struct v4l2_plane planes[VIDEO_MAX_PLANES];
+
+    memset(&buf, 0, sizeof(buf));
+    memset(planes, 0, sizeof planes);
 
-    memset(&buffer, 0, sizeof(buffer));
-    buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-    buffer.memory = V4L2_MEMORY_MMAP;
+    const auto radxa_buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+    // buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+    buf.type = radxa_buf_type;
+    buf.memory = V4L2_MEMORY_MMAP;
+    buf.length = VIDEO_MAX_PLANES;
+    buf.m.planes = planes;
     static uint16_t requestIdx{0};
     // buffer.index = BUFFER_COUNT;
-    buffer.index = requestIdx++ % BUFFER_COUNT;
+    buf.index = requestIdx++ % BUFFER_COUNT;
 
     {
         QElapsedTimer t;
@@ -481,24 +767,25 @@ bool VeyeIMX287m::dequeueImageBuffer(size_t &imageIndex)
         //     }
         // }
 
-        auto oldIdx = buffer.index;
+        auto oldIdx = buf.index;
         // m_bufferMutexes[buffer.index].lock();
         std::lock_guard<std::mutex> lock(m_camMtx);
-        ret = ioctl(m_cam_fd, VIDIOC_DQBUF, &buffer);
+        ret = ioctl(m_cam_fd, VIDIOC_DQBUF, &buf);
         // m_bufferMutexes[buffer.index].unlock();
-        auto newIdx = buffer.index;
+        auto newIdx = buf.index;
 
         if (oldIdx != newIdx) {
             // std::cout << "AAAAAAAAAAAAAA " << oldIdx << ' ' << newIdx << std::endl;
         }
 
         if (ret != 0) {
-            DBG("ioctl(VIDIOC_DQBUF) failed %d(%s)", errno, strerror(errno));
+            std::cerr << "ioctl(VIDIOC_DQBUF) failed: " << errno << " (" << strerror(errno) << ")"
+                      << std::endl;
             return false;
         }
 
-        if (buffer.index < 0 || buffer.index >= BUFFER_COUNT) {
-            DBG("invalid buffer index: %d", buffer.index);
+        if (buf.index < 0 || buf.index >= BUFFER_COUNT) {
+            std::cerr << "invalid buffer index: " << buf.index << std::endl;
             return false;
         }
 
@@ -509,19 +796,20 @@ bool VeyeIMX287m::dequeueImageBuffer(size_t &imageIndex)
     }
 
     // auto &image = video_buffer_ptr[buffer.index];
-    imageIndex = buffer.index;
-    auto &image = m_images[buffer.index];
+    imageIndex = buf.index;
+    // std::cout << "index: " << imageIndex << std::endl;
+    auto &image = m_images[buf.index];
     image.height = img_height;
     image.width = img_width;
     // TODO: fill
     // image.counters.encoderPosition = RotaryEncoder::instance()->position();
-    image.counters.measurementCounter = buffer.sequence;
+    image.counters.measurementCounter = buf.sequence;
 
-    static int64_t prevCounter = buffer.sequence;
-    dropped_count += buffer.sequence - prevCounter - 1;
-    prevCounter = buffer.sequence;
+    static int64_t prevCounter = buf.sequence;
+    dropped_count += buf.sequence - prevCounter - 1;
+    prevCounter = buf.sequence;
 
-    image.counters.timestampUs = buffer.timestamp.tv_sec * 1000 * 1000 + buffer.timestamp.tv_usec;
+    image.counters.timestampUs = buf.timestamp.tv_sec * 1000 * 1000 + buf.timestamp.tv_usec;
 
     // {
     //     QElapsedTimer t;
@@ -533,7 +821,7 @@ bool VeyeIMX287m::dequeueImageBuffer(size_t &imageIndex)
 
     {
         std::lock_guard<std::mutex> lock(m_camMtx);
-        ret = ioctl(m_cam_fd, VIDIOC_QBUF, &buffer);
+        ret = ioctl(m_cam_fd, VIDIOC_QBUF, &buf);
     }
 
     if (ret != 0) {
diff --git a/src/camera/veyeimx287m.h b/src/camera/veyeimx287m.h
index 37da5b0..e1eb76d 100644
--- a/src/camera/veyeimx287m.h
+++ b/src/camera/veyeimx287m.h
@@ -1,7 +1,7 @@
 #pragma once
 
 #include <cstdint>
-
+#include <linux/videodev2.h>
 #include <queue>
 #include <thread>
 
@@ -57,17 +57,30 @@ private:
 private:
     int m_cam_fd{-1};
 
-    static constexpr uint8_t BUFFER_COUNT{8};
+    static constexpr uint8_t BUFFER_COUNT{16};
+    struct v4l2_plane planes[VIDEO_MAX_PLANES];
 
     std::array<std::mutex, BUFFER_COUNT> m_imageMutexes;
     std::array<Image, BUFFER_COUNT> m_images;
     std::array<std::mutex, BUFFER_COUNT> m_bufferMutexes;
     std::array<uint8_t *, BUFFER_COUNT> m_videoBuffers;
 
+    struct buffer
+    {
+        unsigned int idx;
+        unsigned int padding[VIDEO_MAX_PLANES];
+        unsigned int size[VIDEO_MAX_PLANES];
+        void *mem[VIDEO_MAX_PLANES];
+    };
+    std::vector<buffer> buffers;
+
     // std::mutex m_queueMtx;
     std::mutex m_camMtx;
     std::queue<size_t> m_buffersQueue;
 
     std::jthread m_streamThread;
+    // std::jthread m_calcThreads[1];
     std::jthread m_calcThreads[4];
+
+    uint32_t processedCounter{0};
 };