1x2x2 no crashes

3 files changed, 102 insertions, 128 deletions

diff --git a/src/camera/veyeimx287m.cpp b/src/camera/veyeimx287m.cpp
index a97297d..1301c66 100644
--- a/src/camera/veyeimx287m.cpp
+++ b/src/camera/veyeimx287m.cpp
@@ -221,7 +221,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!!.");
         }
@@ -514,7 +514,7 @@ bool VeyeIMX287m::initCam()
         return false;
     }
 
-    m_buffers.resize(rb.count);
+    m_rawBuffers.resize(rb.count);
 
     std::cout << "query buffers" << std::endl;
 
@@ -548,9 +548,9 @@ 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) {
+        if (m_rawBuffers[i].mem == MAP_FAILED) {
             std::cerr << "mmap() failed: " << errno << " (" << strerror(errno)
                       << ")" << std::endl;
             std::cerr << "length: " << length << std::endl;
@@ -559,7 +559,7 @@ bool VeyeIMX287m::initCam()
             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);
 
@@ -594,77 +594,54 @@ void VeyeIMX287m::getFrameLoop(std::stop_token stopToken)
             continue;
         }
 
-        const auto &image = m_buffers[bufferIdx].image;
+        // const auto &image = m_rawBuffers[bufferIdx].image;
 
         {
             t.start();
 
-            auto &src = *(Image::radxa_data_t *) m_buffers[bufferIdx].mem;
+            auto &src = *(Image::radxa_data_t *) m_rawBuffers[bufferIdx].mem;
+            const auto image = std::make_shared<Image>();
             auto &dst = image->data;
             Image::copy(dst, src);
             // image->rotate();
             // const auto pixels = image->sharedPixels();
 
             get_elapsed_ns += t.nsecsElapsed();
+            m_sync.rawSemQueue.enqueue(image);
         }
 
         ++processedCounter;
-        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();
+        // m_sync.rawSemQueue.enqueue(bufferIdx);
     }
 }
 
 void VeyeIMX287m::rotateFrameLoop(std::stop_token stopToken)
 {
     while (!stopToken.stop_requested()) {
-        // std::cout << __func__ << std::endl;
-
-        // 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;
+        // const auto idx = m_sync.rawSemQueue.dequeue();
+        // const auto &image = m_rawBuffers[idx].image;
+        const auto image = m_sync.rawSemQueue.dequeue();
         image->rotate();
-        // const auto pixels = image->sharedPixels();
-        m_lastProcessedIdx = idx;
-        // m_sync.freeRawBuffers.release();
 
-        m_sync.rotSemQueue.enqueue(idx);
-        // m_sync.freeRotatedBuffers.acquire();
-        // {
-        //     std::lock_guard l{m_sync.rotatedBufferMtx};
-        //     m_sync.rotatedBufferIndices.enqueue(idx);
-        // }
-        // m_sync.usedRotatedBuffers.release();
+        // m_lastProcessedImage = idx;
+        // m_lastProcessedImage = image;
+
+        // 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();
-        // 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 idx = m_sync.rotSemQueue.dequeue();
+        const auto image = m_sync.rotSemQueue.dequeue();
+        // const auto &image = m_rawBuffers[idx].image;
         const auto pixels = image->sharedPixels();
-        // m_sync.freeRotatedBuffers.release();
+        {
+            std::lock_guard l{m_lastImageMtx};
+            m_lastProcessedImage = image;
+        }
     }
 }
 
@@ -749,17 +726,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);
@@ -778,66 +755,80 @@ bool VeyeIMX287m::dequeueImageBuffer(size_t &imageIndex)
 bool VeyeIMX287m::getImage(Image *image)
 {
     return false;
-    if (!image) {
-        std::cerr << __func__ << ": image is nullptr" << std::endl;
+    // 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>();
+
+    // if (m_lastProcessedImage != std::numeric_limits<size_t>::max()) {
+    // if (m_lastProcessedImage) {
+    // return m_rawBuffers[m_lastProcessedImage].image;
+    // }
 
-    if (m_lastProcessedIdx != std::numeric_limits<size_t>::max()) {
-        return m_buffers[m_lastProcessedIdx].image;
+    {
+        std::lock_guard l{m_lastImageMtx};
+        result = m_lastProcessedImage;
+        // std::swap(result, m_lastProcessedImage);
     }
+    // return {};
+    // {
+    // return m_lastProcessedImage;
+    //     result = m_lastProcessedImage;
+    // }
+    return result;
 
-    return {};
+    // size_t bufferIdx{};
 
-    size_t bufferIdx{};
+    // if (!dequeueImageBuffer(bufferIdx)) {
+    //     std::cerr << "cannot dequeue" << std::endl;
+    //     return {};
+    // }
 
-    if (!dequeueImageBuffer(bufferIdx)) {
-        return {};
-    }
+    // // 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]};
 
-    // 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]};
+    //     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;
 
-        auto &src = *(Image::radxa_data_t *) m_buffers[bufferIdx].mem;
-        auto &dst = result->data;
-        Image::copy(dst, src);
-        get_elapsed_ns += t.nsecsElapsed();
-    }
-
-    return result;
+    // return result;
 }
diff --git a/src/camera/veyeimx287m.h b/src/camera/veyeimx287m.h
index a9311ba..b875bb7 100644
--- a/src/camera/veyeimx287m.h
+++ b/src/camera/veyeimx287m.h
@@ -92,7 +92,7 @@ private:
      */
     std::optional<int64_t> m_previousFrameCounter{};
 
-    static constexpr uint8_t BUFFER_COUNT{16};
+    static constexpr uint8_t BUFFER_COUNT{4};
 
     // std::array<Image, BUFFER_COUNT> m_images;
     /*!
@@ -105,35 +105,16 @@ private:
     struct buffer
     {
         void *mem{nullptr};
-        std::shared_ptr<Image> image{std::make_shared<Image>()};
+        // 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};
-
-        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{};
-
-        // 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;
@@ -148,7 +129,8 @@ private:
     // TODO: sync all loops somehow to guarantee frames order
     std::jthread m_rotateThreads[2];
     std::jthread m_calcPixelsThreads[2];
-    size_t m_lastProcessedIdx{std::numeric_limits<size_t>::max()};
+    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/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) {