merge and use 16 threadsradxa_imx287

1 files changed, 111 insertions, 88 deletions

diff --git a/src/camera/veyeimx287m.cpp b/src/camera/veyeimx287m.cpp
index c87480b..a6c8b93 100644
--- a/src/camera/veyeimx287m.cpp
+++ b/src/camera/veyeimx287m.cpp
@@ -222,7 +222,7 @@ VeyeIMX287m::~VeyeIMX287m()
         std::cout << "cannot stop stream" << std::endl;
     }
 
-    for (const auto &buffer : m_buffers) {
+    for (const auto &buffer : m_rawBuffers) {
         if (munmap(buffer.mem, radxa_raw_img_size) < 0) {
             DBG("Munmap failed!!.");
         }
@@ -502,7 +502,7 @@ bool VeyeIMX287m::initCam()
         return false;
     }
 
-    m_buffers.resize(rb.count);
+    m_rawBuffers.resize(rb.count);
 
     std::cout << "query buffers" << std::endl;
 
@@ -535,17 +535,18 @@ bool VeyeIMX287m::initCam()
 
         const auto length = buf.m.planes[0].length;
         const auto offset = buf.m.planes[0].m.mem_offset;
-        m_buffers[i].mem = mmap(0, length, PROT_READ | PROT_WRITE, MAP_SHARED, m_cam_fd, offset);
+        m_rawBuffers[i].mem = mmap(0, length, PROT_READ | PROT_WRITE, MAP_SHARED, m_cam_fd, offset);
 
-        if (m_buffers[i].mem == MAP_FAILED) {
-            std::cerr << "mmap() failed: " << errno << " (" << strerror(errno) << ")" << std::endl;
+        if (m_rawBuffers[i].mem == 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;
         }
 
-        printf("Buffer mapped at address %p.\n", m_buffers[i].mem);
+        printf("Buffer mapped at address %p.\n", m_rawBuffers[i].mem);
 
         ret = ioctl(m_cam_fd, VIDIOC_QBUF, &buf);
 
@@ -576,6 +577,9 @@ void VeyeIMX287m::getFrameLoop(std::stop_token stopToken)
     uint8_t threadIdx{0};
     std::array<QFuture<uint8_t>, BUFFER_COUNT> futures;
 
+    QThreadPool threadPool{};
+    threadPool.setMaxThreadCount(BUFFER_COUNT);
+
     while (!stopToken.stop_requested()) {
         size_t bufferIdx{std::numeric_limits<size_t>::max()};
 
@@ -583,71 +587,69 @@ void VeyeIMX287m::getFrameLoop(std::stop_token stopToken)
             continue;
         }
 
-        const uint8_t i = threadIdx % BUFFER_COUNT;
+        const uint8_t i = threadIdx % futures.size();
 
         futures[i].waitForFinished();
 
-        const auto &image = m_buffers[i].image;
-
         {
             t.start();
-
-            auto &src = *(Image::radxa_data_t *) m_buffers[i].mem;
-            auto &dst = image->data;
+            const auto &src = *(Image::radxa_data_t *) m_rawBuffers[bufferIdx].mem;
+            // const auto image = std::make_shared<Image>();
+            // const auto &image = m_rawBuffers[i].image;
+            auto &dst = m_rawBuffers[i].image->data;
             Image::copy(dst, src);
+            // image->rotate();
+            // const auto pixels = image->sharedPixels();
 
             get_elapsed_ns += t.nsecsElapsed();
+            // m_sync.rawSemQueue.enqueue(image);
         }
 
-        futures[i] = QtConcurrent::run([this, i]() {
-                         const auto image = m_buffers[i].image;
-                         image->rotate();
-                         return i;
-                     }).then([this](const uint8_t i) {
-            const auto image = m_buffers[i].image;
+        futures[i] = QtConcurrent::run(&threadPool,
+                                       [this, i]() {
+                                           const auto image = m_rawBuffers[i].image;
+                                           image->rotate();
+                                           const auto pixels = image->sharedPixels();
+                                           return i;
+                                       }) /*.then(&threadPool, [this](const uint8_t i) {
+            const auto image = m_rawBuffers[i].image;
             const auto pixels = image->sharedPixels();
             return i;
-        });
+        })*/
+            ;
 
         ++processedCounter;
         ++threadIdx;
-        // m_sync.freeRawBuffers.acquire();
-        // // TODO: check that queue cannot contain duplicate indices due to ioctl
-        // // TODO: separate copied images from raw buffers
-        // m_sync.rawBufferIndices.enqueue(bufferIdx);
-        // m_sync.usedRawBuffers.release();
-
-        // image->rotate();
-        // const auto pixels = image->sharedPixels();
     }
 }
 
 void VeyeIMX287m::rotateFrameLoop(std::stop_token stopToken)
 {
     while (!stopToken.stop_requested()) {
-        // std::cout << __func__ << std::endl;
+        // const auto idx = m_sync.rawSemQueue.dequeue();
+        // const auto &image = m_rawBuffers[idx].image;
+        // const auto image = m_sync.rawSemQueue.dequeue();
+        // image->rotate();
 
-        m_sync.usedRawBuffers.acquire();
-        const auto idx = m_sync.rawBufferIndices.dequeue();
-        const auto &image = m_buffers[idx].image;
-        image->rotate();
-        // const auto pixels = image->sharedPixels();
-        m_sync.freeRawBuffers.release();
+        // // m_lastProcessedImage = idx;
+        // // m_lastProcessedImage = image;
 
-        // m_sync.freeRotatedBuffers.acquire();
-        // m_sync.rotatedBufferIndices.enqueue(idx);
-        // m_sync.usedRotatedBuffers.release();
+        // // m_sync.rotSemQueue.enqueue(idx);
+        // m_sync.rotSemQueue.enqueue(image);
     }
 }
 
 void VeyeIMX287m::calcPixelsLoop(std::stop_token stopToken)
 {
     while (!stopToken.stop_requested()) {
-        m_sync.usedRotatedBuffers.acquire();
-        const auto idx = m_sync.rotatedBufferIndices.dequeue();
-        const auto &image = m_buffers[idx].image;
-        const auto pixels = image->sharedPixels();
-        m_sync.freeRotatedBuffers.release();
+        // // const auto idx = m_sync.rotSemQueue.dequeue();
+        // const auto image = m_sync.rotSemQueue.dequeue();
+        // // const auto &image = m_rawBuffers[idx].image;
+        // const auto pixels = image->sharedPixels();
+        // {
+        //     std::lock_guard l{m_lastImageMtx};
+        //     m_lastProcessedImage = image;
+        // }
     }
 }
 
@@ -732,17 +734,17 @@ bool VeyeIMX287m::dequeueImageBuffer(size_t &imageIndex)
 
     imageIndex = buf.index;
 
-    const auto &image = m_buffers[buf.index].image;
-    image->height = img_height;
-    image->width = img_width;
-    // TODO: fill
-    // image.counters.encoderPosition = RotaryEncoder::instance()->position();
-    image->counters.measurementCounter = buf.sequence;
+    // const auto &image = m_rawBuffers[buf.index].image;
+    // image->height = img_height;
+    // image->width = img_width;
+    // // TODO: fill
+    // // image.counters.encoderPosition = RotaryEncoder::instance()->position();
+    // image->counters.measurementCounter = buf.sequence;
 
     dropped_count += buf.sequence - m_previousFrameCounter.value_or(buf.sequence) - 1;
     m_previousFrameCounter = buf.sequence;
 
-    image->counters.timestampUs = buf.timestamp.tv_sec * 1000 * 1000 + buf.timestamp.tv_usec;
+    // image->counters.timestampUs = buf.timestamp.tv_sec * 1000 * 1000 + buf.timestamp.tv_usec;
 
     {
         std::lock_guard<std::mutex> lock(m_camMtx);
@@ -760,60 +762,81 @@ bool VeyeIMX287m::dequeueImageBuffer(size_t &imageIndex)
 
 bool VeyeIMX287m::getImage(Image *image)
 {
-    if (!image) {
-        std::cerr << __func__ << ": image is nullptr" << std::endl;
+    return false;
+    // if (!image) {
+    //     std::cerr << __func__ << ": image is nullptr" << std::endl;
 
-        return false;
-    }
+    //     return false;
+    // }
 
-    size_t bufferIdx{};
+    // size_t bufferIdx{};
 
-    if (!dequeueImageBuffer(bufferIdx)) {
-        return false;
-    }
+    // if (!dequeueImageBuffer(bufferIdx)) {
+    //     return false;
+    // }
 
-    // TODO: remove this bullshit. return ptr to image or copy image metainfo
-    // only, then copy data
-    // *image = std::move(m_images[bufferIdx]);
-    *image = std::move(*m_buffers[bufferIdx].image);
-    {
-        QElapsedTimer t;
-        t.start();
-        std::lock_guard lock{m_imageMutexes[bufferIdx]};
+    // // TODO: remove this bullshit. return ptr to image or copy image metainfo
+    // // only, then copy data
+    // // *image = std::move(m_images[bufferIdx]);
+    // *image = std::move(*m_rawBuffers[bufferIdx].image);
+    // {
+    //     QElapsedTimer t;
+    //     t.start();
+    //     std::lock_guard lock{m_imageMutexes[bufferIdx]};
 
-        auto &src = *(Image::radxa_data_t *) m_buffers[bufferIdx].mem;
-        auto &dst = image->data;
-        Image::copy(dst, src);
-        get_elapsed_ns += t.nsecsElapsed();
-    }
+    //     auto &src = *(Image::radxa_data_t *) m_rawBuffers[bufferIdx].mem;
+    //     auto &dst = image->data;
+    //     Image::copy(dst, src);
+    //     get_elapsed_ns += t.nsecsElapsed();
+    // }
 
-    return true;
+    // return true;
 }
 
 std::shared_ptr<Image> VeyeIMX287m::getImage()
 {
     std::shared_ptr<Image> result;
+    // std::shared_ptr<Image> result = std::make_shared<Image>();
 
-    size_t bufferIdx{};
+    // if (m_lastProcessedImage != std::numeric_limits<size_t>::max()) {
+    // if (m_lastProcessedImage) {
+    // return m_rawBuffers[m_lastProcessedImage].image;
+    // }
 
-    if (!dequeueImageBuffer(bufferIdx)) {
-        return {};
+    {
+        std::lock_guard l{m_lastImageMtx};
+        // result = m_lastProcessedImage;
+        // std::swap(result, m_lastProcessedImage);
     }
+    // return {};
+    // {
+    // return m_lastProcessedImage;
+    //     result = m_lastProcessedImage;
+    // }
+    return result;
 
-    // TODO: remove this bullshit. return ptr to image or copy image metainfo
-    // only, then copy data
-    // *image = std::move(m_images[bufferIdx]);
-    result = m_buffers[bufferIdx].image;
-    {
-        QElapsedTimer t;
-        t.start();
-        std::lock_guard lock{m_imageMutexes[bufferIdx]};
+    // size_t bufferIdx{};
 
-        auto &src = *(Image::radxa_data_t *) m_buffers[bufferIdx].mem;
-        auto &dst = result->data;
-        Image::copy(dst, src);
-        get_elapsed_ns += t.nsecsElapsed();
-    }
+    // if (!dequeueImageBuffer(bufferIdx)) {
+    //     std::cerr << "cannot dequeue" << std::endl;
+    //     return {};
+    // }
 
-    return result;
+    // // TODO: remove this bullshit. return ptr to image or copy image metainfo
+    // // only, then copy data
+    // // *image = std::move(m_images[bufferIdx]);
+    // // result = m_rawBuffers[bufferIdx].image;
+    // {
+    //     QElapsedTimer t;
+    //     t.start();
+    //     std::lock_guard lock{m_imageMutexes[bufferIdx]};
+
+    //     auto &src = *(Image::radxa_data_t *) m_rawBuffers[bufferIdx].mem;
+    //     auto &dst = result->data;
+    //     Image::copy(dst, src);
+    //     get_elapsed_ns += t.nsecsElapsed();
+    // }
+    // // std::cerr << "ok" << std::endl;
+
+    // return result;
 }