merge and use 16 threadsradxa_imx287

6 files changed, 162 insertions, 112 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;
 }
diff --git a/src/camera/veyeimx287m.h b/src/camera/veyeimx287m.h
index 2b5c28c..e4c02b0 100644
--- a/src/camera/veyeimx287m.h
+++ b/src/camera/veyeimx287m.h
@@ -15,6 +15,7 @@
 #include "constants.h"
 #include "icamera.h"
 #include "image.h"
+#include "utils/sem_queue.h"
 
 namespace veye {
 namespace imx287m {
@@ -106,42 +107,30 @@ private:
         void *mem{nullptr};
         std::shared_ptr<Image> image{std::make_shared<Image>()};
     };
-    std::vector<buffer> m_buffers;
+    std::vector<buffer> m_rawBuffers;
 
     struct Semaphore
     {
-        Semaphore()
-        {
-            rawBufferIndices.reserve(BUFFER_COUNT);
-            rotatedBufferIndices.reserve(BUFFER_COUNT);
-        }
-        const uint8_t maxSize{BUFFER_COUNT};
-
-        QQueue<size_t> rawBufferIndices;
-        QSemaphore freeRawBuffers{BUFFER_COUNT};
-        QSemaphore usedRawBuffers{};
-
-        QQueue<size_t> rotatedBufferIndices;
-        QSemaphore freeRotatedBuffers{BUFFER_COUNT};
-        QSemaphore usedRotatedBuffers{};
-
-        // uint8_t bufferIdx{std::numeric_limits<decltype(bufferIdx)>::max()};
-        // std::binary_semaphore main2calc{0};
-        // std::binary_semaphore calc2main{0};
+        // sem_queue<std::shared_ptr<Image>, BUFFER_COUNT> rawSemQueue;
+        // sem_queue<size_t, BUFFER_COUNT> rawSemQueue;
+        // sem_queue<std::shared_ptr<Image>, BUFFER_COUNT> rotSemQueue;
+        // sem_queue<size_t, BUFFER_COUNT> rotSemQueue;
     } m_sync;
 
     std::mutex m_camMtx;
     /*!
      * \brief m_buffersQueue - queue of buffers which require extracting pixels
      */
-    std::queue<std::remove_const<decltype(BUFFER_COUNT)>> m_buffersQueue;
+    // std::queue<std::remove_const<decltype(BUFFER_COUNT)>> m_buffersQueue;
 
     std::jthread m_streamThread;
     std::jthread m_getThreads[1];
     // std::jthread m_getThreads[4];
     // TODO: sync all loops somehow to guarantee frames order
-    std::jthread m_rotateThreads[2];
-    std::jthread m_calcPixelsThreads[1];
+    std::jthread m_rotateThreads[0];
+    std::jthread m_calcPixelsThreads[0];
+    std::mutex m_lastImageMtx;
+    std::shared_ptr<Image> m_lastProcessedImage{};
 
     std::shared_ptr<veye::imx287m::i2c> m_i2c;
     std::shared_ptr<HttpServer> m_httpServer;
diff --git a/src/image.cpp b/src/image.cpp
index 4af3e53..b2cd535 100644
--- a/src/image.cpp
+++ b/src/image.cpp
@@ -31,6 +31,7 @@ float process_column(const Image::column_t &column)
 
     float result = std::numeric_limits<float>::quiet_NaN();
 
+    // constexpr uint32_t signalThreshold = 900; // = SKO * sqrt(patternSize)
     constexpr uint32_t signalThreshold = 900; // = SKO * sqrt(patternSize)
     static constexpr uint32_t patternOffset = patternSize - ((patternSize % 2 == 1) ? 1 : 0);
     constexpr uint32_t correlationSize = img_height - patternSize
diff --git a/src/imagealgos.cpp b/src/imagealgos.cpp
index a293803..ff80402 100644
--- a/src/imagealgos.cpp
+++ b/src/imagealgos.cpp
@@ -32,8 +32,9 @@ size_t pgm_save(Image *img, FILE *outfile, bool really_save)
 
     size_t n{0};
 
-    n += sprintf((char*)pgm_image, "P5\n%d %d\n%d\n",
-                 img->width, img->height, 0xFF);
+    // n += sprintf((char*)pgm_image, "P5\n%d %d\n%d\n",
+    //              img->width, img->height, 0xFF);
+    n += sprintf((char *) pgm_image, "P5\n%d %d\n%d\n", int(img_width), int(img_height), 0xFF);
 
     // #ifdef RADXA_ZERO_3E
     //     for (size_t row{0}; row < img_height; ++row) {
diff --git a/src/utils/sem_queue.cpp b/src/utils/sem_queue.cpp
new file mode 100644
index 0000000..a3ca6e6
--- /dev/null
+++ b/src/utils/sem_queue.cpp
@@ -0,0 +1 @@
+#include "sem_queue.h"
diff --git a/src/utils/sem_queue.h b/src/utils/sem_queue.h
new file mode 100644
index 0000000..6987d88
--- /dev/null
+++ b/src/utils/sem_queue.h
@@ -0,0 +1,35 @@
+#pragma once
+
+#include <mutex>
+#include <queue>
+#include <semaphore>
+
+template<typename T, size_t S>
+class sem_queue
+{
+public:
+    inline void enqueue(const T &value)
+    {
+        m_free.acquire();
+        std::lock_guard l{m_mtx};
+        m_queue.push(value);
+        m_used.release();
+    }
+
+    inline T dequeue()
+    {
+        m_used.acquire();
+        std::lock_guard l{m_mtx};
+        const auto result = m_queue.front();
+        m_queue.pop();
+        m_free.release();
+
+        return result;
+    }
+
+private:
+    std::queue<T> m_queue;
+    std::mutex m_mtx;
+    std::counting_semaphore<S> m_free{S};
+    std::counting_semaphore<S> m_used{0};
+};