introduce calc pixels thread; rotation still requires 2 threads

4 files changed, 78 insertions, 82 deletions

diff --git a/src/camera/veyeimx287m.cpp b/src/camera/veyeimx287m.cpp
index 04ee7d2..6df5a3e 100644
--- a/src/camera/veyeimx287m.cpp
+++ b/src/camera/veyeimx287m.cpp
@@ -16,7 +16,6 @@
 // orpheus
 #include "camera/veye_i2c.h"
 #include "constants.h"
-#include "pixels.h"
 #include "protocols/httpserver.h"
 #include "veyeimx287m_types.h"
 
@@ -197,6 +196,11 @@ VeyeIMX287m::VeyeIMX287m() {}
 
 VeyeIMX287m::~VeyeIMX287m()
 {
+    for (auto &t : m_calcPixelsThreads) {
+        t.request_stop();
+        t.join();
+    }
+
     for (auto &t : m_rotateThreads) {
         t.request_stop();
         t.join();
@@ -279,6 +283,10 @@ bool VeyeIMX287m::startStream()
         t = std::jthread{&VeyeIMX287m::rotateFrameLoop, this};
     }
 
+    for (auto &t : m_calcPixelsThreads) {
+        t = std::jthread{&VeyeIMX287m::calcPixelsLoop, this};
+    }
+
     std::cout << __func__ << " - OK" << std::endl;
 
     return true;
@@ -453,8 +461,6 @@ bool VeyeIMX287m::initCam()
         return false;
     }
 
-    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;
 
     std::cout << "num_planes: " << num_planes << std::endl;
@@ -553,9 +559,6 @@ bool VeyeIMX287m::initCam()
             return false;
         }
 
-        m_buffers[i].size = length;
-        m_buffers[i].padding = 0;
-
         printf("Buffer mapped at address %p.\n", m_buffers[i].mem);
 
         ret = ioctl(m_cam_fd, VIDIOC_QBUF, &buf);
@@ -580,13 +583,6 @@ bool VeyeIMX287m::initI2C()
     return m_i2c != nullptr && m_i2c->open();
 }
 
-// bool VeyeIMX287m::initHttpServer()
-// {
-//     m_httpServer = std::make_shared<HttpServer>(this);
-
-//     return m_httpServer != nullptr;
-// }
-
 void VeyeIMX287m::getFrameLoop(std::stop_token stopToken)
 {
     QElapsedTimer t;
@@ -598,14 +594,6 @@ void VeyeIMX287m::getFrameLoop(std::stop_token stopToken)
             continue;
         }
 
-        // std::cout << __func__ << " acquire" << std::endl;
-        // std::cout << __func__ << " acquired" << std::endl;
-        // ++processedCounter;
-        // continue;
-
-        // std::lock_guard lock{m_imageMutexes[bufferIdx]};
-
-        // auto &image = m_images[bufferIdx];
         const auto &image = m_buffers[bufferIdx].image;
 
         {
@@ -618,44 +606,49 @@ void VeyeIMX287m::getFrameLoop(std::stop_token stopToken)
             get_elapsed_ns += t.nsecsElapsed();
         }
 
-        // m_receiverCalculatorSem.main2calc.release();
-        // std::cout << __func__ << ": main2calc.release" << std::endl;
-        // m_receiverCalculatorSem.bufferIdx = bufferIdx;
-        // m_receiverCalculatorSem.calc2main.acquire();
-        // std::cout << __func__ << ": calc2main.acquire" << std::endl;
-        // std::cout << __func__ << " released" << std::endl;
-
         ++processedCounter;
-        // return;
-
-        image->rotate();
-        const auto pixels = image->sharedPixels();
+        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)
 {
-    return;
     while (!stopToken.stop_requested()) {
-        std::cout << __func__ << std::endl;
-
-        m_receiverCalculatorSem.main2calc.acquire();
-        std::cout << __func__ << ": main2calc.acquired" << std::endl;
-        const auto idx = m_receiverCalculatorSem.bufferIdx;
-        m_receiverCalculatorSem.calc2main.release();
-        std::cout << __func__ << ": calc2main.released" << std::endl;
+        // std::cout << __func__ << std::endl;
 
+        m_sync.usedRawBuffers.acquire();
+        const auto idx = m_sync.rawBufferIndices.dequeue();
         const auto &image = m_buffers[idx].image;
         image->rotate();
-        std::cout << "rotated" << std::endl;
+        // const auto pixels = image->sharedPixels();
+        m_sync.freeRawBuffers.release();
+
+        // m_sync.freeRotatedBuffers.acquire();
+        // m_sync.rotatedBufferIndices.enqueue(idx);
+        // m_sync.usedRotatedBuffers.release();
+    }
+}
+
+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();
     }
 }
 
 // TODO: check if some of buffers are being overritten during processing
 bool VeyeIMX287m::dequeueImageBuffer(size_t &imageIndex)
-// TODO: get Image from video_buffer_ptr
 {
     static struct timeval curr, prev;
     static uint16_t counter = 0;
@@ -664,6 +657,7 @@ bool VeyeIMX287m::dequeueImageBuffer(size_t &imageIndex)
     double elapsedTime = (curr.tv_sec - prev.tv_sec) * 1000.0; // sec to ms
     elapsedTime += (curr.tv_usec - prev.tv_usec) / 1000.0;     // us to ms
 
+    // TODO: move this shit to some beautiful place
     if (elapsedTime > 1000. && processedCounter != 0) {
         fprintf(stderr,
                 "fps: %d\tdropped: %lu sec: %ld "
@@ -703,9 +697,7 @@ bool VeyeIMX287m::dequeueImageBuffer(size_t &imageIndex)
     memset(&buf, 0, sizeof(buf));
     memset(planes, 0, sizeof planes);
 
-    const auto radxa_buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
-    // buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-    buf.type = radxa_buf_type;
+    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
     buf.memory = V4L2_MEMORY_MMAP;
     buf.length = VIDEO_MAX_PLANES;
     buf.m.planes = planes;
@@ -735,7 +727,7 @@ bool VeyeIMX287m::dequeueImageBuffer(size_t &imageIndex)
     }
 
     imageIndex = buf.index;
-    // auto &image = m_images[buf.index];
+
     const auto &image = m_buffers[buf.index].image;
     image->height = img_height;
     image->width = img_width;
@@ -743,10 +735,6 @@ bool VeyeIMX287m::dequeueImageBuffer(size_t &imageIndex)
     // image.counters.encoderPosition = RotaryEncoder::instance()->position();
     image->counters.measurementCounter = buf.sequence;
 
-    // FIXME: git rid of static vars
-    // if (not m_previousFrameCounter.has_value()) {
-    //     m_previousFrameCounter = buf.sequence;
-    // }
     dropped_count += buf.sequence - m_previousFrameCounter.value_or(buf.sequence) - 1;
     m_previousFrameCounter = buf.sequence;
 
diff --git a/src/camera/veyeimx287m.h b/src/camera/veyeimx287m.h
index ab7e3e5..2b5c28c 100644
--- a/src/camera/veyeimx287m.h
+++ b/src/camera/veyeimx287m.h
@@ -1,17 +1,20 @@
 #pragma once
 
+// c/cpp
 #include <cstdint>
 #include <linux/videodev2.h>
 #include <queue>
 #include <thread>
 
+// qt
+#include <QQueue>
 #include <QReadWriteLock>
 #include <QSemaphore>
 
+// orpheus
 #include "constants.h"
-#include "image.h"
-
 #include "icamera.h"
+#include "image.h"
 
 namespace veye {
 namespace imx287m {
@@ -75,6 +78,7 @@ private:
 
     void getFrameLoop(std::stop_token stopToken);
     void rotateFrameLoop(std::stop_token stopToken);
+    void calcPixelsLoop(std::stop_token stopToken);
 
 private:
     /*!
@@ -95,10 +99,10 @@ private:
      */
     std::array<std::mutex, BUFFER_COUNT> m_imageMutexes;
 
+    // TODO: split this
+    // there should be no chance of changing image by ioctl during futher processing
     struct buffer
     {
-        unsigned int padding{0};
-        unsigned int size{0};
         void *mem{nullptr};
         std::shared_ptr<Image> image{std::make_shared<Image>()};
     };
@@ -106,13 +110,26 @@ private:
 
     struct Semaphore
     {
+        Semaphore()
+        {
+            rawBufferIndices.reserve(BUFFER_COUNT);
+            rotatedBufferIndices.reserve(BUFFER_COUNT);
+        }
         const uint8_t maxSize{BUFFER_COUNT};
-        uint8_t bufferIdx{std::numeric_limits<decltype(bufferIdx)>::max()};
-        std::binary_semaphore main2calc{0};
-        std::binary_semaphore calc2main{0};
-    } m_receiverCalculatorSem;
 
-    // std::mutex m_queueMtx;
+        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};
+    } m_sync;
+
     std::mutex m_camMtx;
     /*!
      * \brief m_buffersQueue - queue of buffers which require extracting pixels
@@ -120,9 +137,11 @@ private:
     std::queue<std::remove_const<decltype(BUFFER_COUNT)>> m_buffersQueue;
 
     std::jthread m_streamThread;
-    // std::jthread m_getThreads[1];
-    std::jthread m_getThreads[4];
-    std::jthread m_rotateThreads[1];
+    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::shared_ptr<veye::imx287m::i2c> m_i2c;
     std::shared_ptr<HttpServer> m_httpServer;
diff --git a/src/image.h b/src/image.h
index d5fd953..33fb4d8 100644
--- a/src/image.h
+++ b/src/image.h
@@ -12,38 +12,27 @@ class Pixels;
 // TODO: template
 struct Image
 {
-    // Image(Image &other) { std::cerr << __func__ << ": don't copy me please" << std::endl; }
     Image() = default;
     Image(Image &other) = delete;
     Image(Image &&other) = delete;
     Image &operator=(Image &&other) = default;
-    // uint8_t data[img_height][img_width] = {{0}};
 
-#ifdef RADXA_ZERO_3E
     using radxa_row_t = std::array<uint8_t, radxa_raw_img_stride>;
     using radxa_data_t = std::array<radxa_row_t, img_height>;
-    // using row_t = std::array<uint8_t, radxa_raw_img_stride>;
-    using row_t = std::array<uint8_t, img_width>;
-    using data_t = std::array<row_t, img_height>;
-#else
+
     using row_t = std::array<uint8_t, img_width>;
     using data_t = std::array<row_t, img_height>;
-#endif // RADXA_ZERO_3E
+
     using rotated_row_t = std::array<uint8_t, img_height>;
     using column_t = rotated_row_t;
     using rotated_data_t = std::array<column_t, img_width>;
-    // data_t d;
+
     alignas(128) data_t data;
-    // alignas(128) std::array<uint8_t, 417792> rawData;
-    int width{0};
-    int height{0};
-    // data_t *data;
-    // uint8_t *data = {nullptr};
-    // uint8_t &dataAt(size_t row, size_t col);
-    // uint8_t rotated_cw[img_width][img_height] = {{0}};
-    rotated_data_t rotated_cw;
-    // size_t dataSize{0};
-    // unsigned int stride{0};
+    alignas(128) rotated_data_t rotated_cw;
+
+    uint16_t width{0};
+    uint16_t height{0};
+
     libcamera::PixelFormat pixelFormat{0};
     Counters counters{};
     bool hasInputData{false};
diff --git a/src/protocols/httpserver.cpp b/src/protocols/httpserver.cpp
index d11fdb0..3d5a0d5 100644
--- a/src/protocols/httpserver.cpp
+++ b/src/protocols/httpserver.cpp
@@ -117,7 +117,7 @@ QHttpServerResponse HttpServer::GET_pixels()
 
     std::shared_ptr<Image> image;
     {
-        const elapsed_logger logger{__func__};
+        // const elapsed_logger logger{__func__};
         /*const auto */ image = m_camera->getImage();
     }