introduce sem_queue

5 files changed, 89 insertions, 17 deletions

diff --git a/src/camera/veyeimx287m.cpp b/src/camera/veyeimx287m.cpp
index 6df5a3e..a97297d 100644
--- a/src/camera/veyeimx287m.cpp
+++ b/src/camera/veyeimx287m.cpp
@@ -602,16 +602,22 @@ void VeyeIMX287m::getFrameLoop(std::stop_token stopToken)
             auto &src = *(Image::radxa_data_t *) m_buffers[bufferIdx].mem;
             auto &dst = image->data;
             Image::copy(dst, src);
+            // image->rotate();
+            // const auto pixels = image->sharedPixels();
 
             get_elapsed_ns += t.nsecsElapsed();
         }
 
         ++processedCounter;
-        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();
+        m_sync.rawSemQueue.enqueue(bufferIdx);
+        // m_sync.freeRawBuffers.acquire();
+        // // TODO: check that queue cannot contain duplicate indices due to ioctl
+        // // TODO: separate copied images from raw buffers
+        // {
+        //     std::lock_guard l{m_sync.rawBufferMtx};
+        //     m_sync.rawBufferIndices.enqueue(bufferIdx);
+        // }
+        // m_sync.usedRawBuffers.release();
 
         // image->rotate();
         // const auto pixels = image->sharedPixels();
@@ -623,15 +629,25 @@ void VeyeIMX287m::rotateFrameLoop(std::stop_token stopToken)
     while (!stopToken.stop_requested()) {
         // std::cout << __func__ << std::endl;
 
-        m_sync.usedRawBuffers.acquire();
-        const auto idx = m_sync.rawBufferIndices.dequeue();
+        // m_sync.usedRawBuffers.acquire();
+        // size_t idx = std::numeric_limits<size_t>::max();
+        // {
+        //     std::lock_guard l{m_sync.rawBufferMtx};
+        //     idx = m_sync.rawBufferIndices.dequeue();
+        // }
+        const auto idx = m_sync.rawSemQueue.dequeue();
         const auto &image = m_buffers[idx].image;
         image->rotate();
         // const auto pixels = image->sharedPixels();
-        m_sync.freeRawBuffers.release();
+        m_lastProcessedIdx = idx;
+        // m_sync.freeRawBuffers.release();
 
+        m_sync.rotSemQueue.enqueue(idx);
         // m_sync.freeRotatedBuffers.acquire();
-        // m_sync.rotatedBufferIndices.enqueue(idx);
+        // {
+        //     std::lock_guard l{m_sync.rotatedBufferMtx};
+        //     m_sync.rotatedBufferIndices.enqueue(idx);
+        // }
         // m_sync.usedRotatedBuffers.release();
     }
 }
@@ -639,11 +655,16 @@ void VeyeIMX287m::rotateFrameLoop(std::stop_token stopToken)
 void VeyeIMX287m::calcPixelsLoop(std::stop_token stopToken)
 {
     while (!stopToken.stop_requested()) {
-        m_sync.usedRotatedBuffers.acquire();
-        const auto idx = m_sync.rotatedBufferIndices.dequeue();
+        // m_sync.usedRotatedBuffers.acquire();
+        // size_t idx{std::numeric_limits<size_t>::max()};
+        // {
+        //     std::lock_guard l{m_sync.rotatedBufferMtx};
+        //     idx = m_sync.rotatedBufferIndices.dequeue();
+        // }
+        const auto idx = m_sync.rotSemQueue.dequeue();
         const auto &image = m_buffers[idx].image;
         const auto pixels = image->sharedPixels();
-        m_sync.freeRotatedBuffers.release();
+        // m_sync.freeRotatedBuffers.release();
     }
 }
 
@@ -756,6 +777,7 @@ bool VeyeIMX287m::dequeueImageBuffer(size_t &imageIndex)
 
 bool VeyeIMX287m::getImage(Image *image)
 {
+    return false;
     if (!image) {
         std::cerr << __func__ << ": image is nullptr" << std::endl;
 
@@ -790,6 +812,12 @@ std::shared_ptr<Image> VeyeIMX287m::getImage()
 {
     std::shared_ptr<Image> result;
 
+    if (m_lastProcessedIdx != std::numeric_limits<size_t>::max()) {
+        return m_buffers[m_lastProcessedIdx].image;
+    }
+
+    return {};
+
     size_t bufferIdx{};
 
     if (!dequeueImageBuffer(bufferIdx)) {
diff --git a/src/camera/veyeimx287m.h b/src/camera/veyeimx287m.h
index 2b5c28c..a9311ba 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 {
@@ -112,15 +113,20 @@ private:
     {
         Semaphore()
         {
-            rawBufferIndices.reserve(BUFFER_COUNT);
+            // rawBufferIndices.reserve(BUFFER_COUNT);
             rotatedBufferIndices.reserve(BUFFER_COUNT);
         }
         const uint8_t maxSize{BUFFER_COUNT};
 
-        QQueue<size_t> rawBufferIndices;
-        QSemaphore freeRawBuffers{BUFFER_COUNT};
-        QSemaphore usedRawBuffers{};
+        sem_queue<size_t, BUFFER_COUNT> rawSemQueue;
+        sem_queue<size_t, BUFFER_COUNT> rotSemQueue;
 
+        // std::mutex rawBufferMtx;
+        // QQueue<size_t> rawBufferIndices;
+        // QSemaphore freeRawBuffers{BUFFER_COUNT};
+        // QSemaphore usedRawBuffers{};
+
+        std::mutex rotatedBufferMtx;
         QQueue<size_t> rotatedBufferIndices;
         QSemaphore freeRotatedBuffers{BUFFER_COUNT};
         QSemaphore usedRotatedBuffers{};
@@ -141,7 +147,8 @@ private:
     // 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_calcPixelsThreads[2];
+    size_t m_lastProcessedIdx{std::numeric_limits<size_t>::max()};
 
     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/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};
+};