broken img calc; change dir struct

4 files changed, 150 insertions, 57 deletions

diff --git a/src/camera/icamera.h b/src/camera/icamera.h
index 3fccc4b..11919bb 100644
--- a/src/camera/icamera.h
+++ b/src/camera/icamera.h
@@ -35,12 +35,14 @@ public:
     virtual bool setExposureTimeUs(int value) = 0;
     virtual bool setGain(int value) = 0;
     virtual bool setLaserLevel(int value) = 0;
+    virtual bool setSomething(int value) = 0;
 
 public:
     libcamera::Signal<std::shared_ptr<Pixels>> newPixels;
     libcamera::Signal<std::shared_ptr<Image>> newImage;
     std::function<void(std::shared_ptr<Pixels>)> newPixelsCallback;
-    std::function<void(std::shared_ptr<Image>)> newImageCallback;
+    // std::function<void(std::shared_ptr<Image>)> newImageCallback;
+    std::function<void(Image &)> newImageCallback;
 
 public:
     virtual bool startStream() = 0;
diff --git a/src/camera/innomakerov9281.cpp b/src/camera/innomakerov9281.cpp
index b7c5cd4..79ae0b4 100644
--- a/src/camera/innomakerov9281.cpp
+++ b/src/camera/innomakerov9281.cpp
@@ -10,6 +10,9 @@
 #include <unistd.h>
 
 #include <iostream>
+#include <span>
+
+#include <QElapsedTimer>
 
 #include "constants.h"
 // #include "rotaryencoder.h"
@@ -23,10 +26,13 @@
 
 #define DBG(fmt, args...) LOGD("%s:%d, " fmt, __FUNCTION__, __LINE__, ##args);
 
+extern uint64_t dq_elapsed_ns;
+extern uint64_t get_elapsed_ns;
 extern uint64_t sum_elapsed_ns;
 extern uint64_t corr_elapsed_ns;
 extern uint64_t max_elapsed_ns;
 extern uint64_t value_elapsed_ns;
+extern uint64_t rot_elapsed_ns;
 
 // constexpr char videoDevice[] = "/dev/video0";
 
@@ -37,6 +43,9 @@ InnoMakerOV9281::~InnoMakerOV9281()
     m_streamThread.request_stop();
     m_streamThread.join();
 
+    m_someThread.request_stop();
+    m_someThread.join();
+
     int ret{-1};
 
     for (int i = 0; i < BUFFER_COUNT; ++i)
@@ -74,43 +83,84 @@ std::vector<std::shared_ptr<ICamera> > InnoMakerOV9281::search()
     if (!cam->init())
         return {};
 
-    if (!cam->setExposureTimeUs(3000))
+    if (!cam->setExposureTimeUs(68000))
+        return {};
+
+    if (!cam->setLaserLevel(1))
+        return {};
+
+    if (!cam->setGain(2))
         return {};
 
-    if (!cam->setGain(3000))
+    if (!cam->setSomething(0)) {
         return {};
+    }
+
+    // m_someThread = std::jthread{[=](std::stop_token stopToken) {
+    //     std::cout << "InnoMakerOV9281: start stream" << std::endl;
+    //     sleep(5);
+
+    //     static int i = 0;
+    //     while (!stopToken.stop_requested()) {
+    //         cam->setSomething(i);
+    //         i -= 1;
+    //     }
+    // }};
 
     return {cam};
 }
 
 bool InnoMakerOV9281::startStream()
 {
+    int buffer_type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+    auto ret = ioctl(m_cam_fd, VIDIOC_STREAMON, &buffer_type);
+
+    if (ret != 0) {
+        DBG("ioctl(VIDIOC_STREAMON) failed %d(%s)", errno, strerror(errno));
+        return false;
+    }
+
     m_streamThread = std::jthread{[&](std::stop_token stopToken) {
         std::cout << "InnoMakerOV9281: start stream" << std::endl;
 
+        // Image image;
+        std::shared_ptr<Image> imageCopy = std::make_shared<Image>();
         while (!stopToken.stop_requested())
         {
-            std::shared_ptr<Image> image = std::make_shared<Image>();
-            getImage(*image);
-// FIXME: backup emit value
-#ifdef emit
-#undef emit
-            // std::cout << "emit new image" << std::endl << std::flush;
-            // newImage.emit(image);
-            if (newImageCallback)
-            {
+            size_t imageIndex{};
+
+            // if (!getImage(image))
+            if (!getImage(imageIndex))
+                break;
+
+            auto image = m_images[imageIndex];
+
+            // // FIXME: backup emit value
+            // #ifdef emit
+            // #undef emit
+            //             // std::cout << "emit new image" << std::endl << std::flush;
+            //             // newImage.emit(image);
+            if (newImageCallback) {
+                // memcpy(imageCopy.get(), &image, sizeof(Image));
+                // newImageCallback(imageCopy);
                 newImageCallback(image);
             }
-            // newPixels.emit(pixels);
-            image->rotate();
-            if (newPixelsCallback)
-            {
-                auto pixels = image->pixels();
+            // continue;
+            //             // // newPixels.emit(pixels);
+            // auto &img = *image;
+            // image->rotate();
+            // img.rotate();
+            image.rotate();
+            //             image.rotate();
+            if (newPixelsCallback) {
+                // auto pixels = image->pixels();
+                auto pixels = image.pixels();
                 newPixelsCallback(pixels);
             }
+            continue;
         }
-#define emit
-#endif
+        // #define emit
+        // #endif
         std::cout << "InnoMakerOV9281: stream interruption requested"
                   << std::endl;
     }};
@@ -170,6 +220,13 @@ bool InnoMakerOV9281::setLaserLevel(int value)
     return setCamParam(V4L2_CID_FLASH_TIMEOUT, value);
 }
 
+bool InnoMakerOV9281::setSomething(int value)
+{
+    std::cout << __func__ << ": " << value << std::endl << std::flush;
+
+    return setCamParam(V4L2_CID_FLASH_INTENSITY, value);
+}
+
 bool InnoMakerOV9281::setCamParam(unsigned int v4l2controlId, int value)
 {
     v4l2_control ctl{v4l2controlId, value};
@@ -182,6 +239,7 @@ bool InnoMakerOV9281::setCamParam(unsigned int v4l2controlId, int value)
                 "cannot set cam param: id - %d, error - '%s'\n",
                 v4l2controlId,
                 strerror(errno));
+        fflush(stderr);
 
         return false;
     }
@@ -195,6 +253,8 @@ bool InnoMakerOV9281::setCamParam(unsigned int v4l2controlId, int value)
                 v4l2controlId,
                 strerror(errno));
 
+        fflush(stderr);
+
         return false;
     }
 
@@ -277,11 +337,8 @@ bool InnoMakerOV9281::initCam()
     request.memory = V4L2_MEMORY_MMAP;
     ret = ioctl(m_cam_fd, VIDIOC_REQBUFS, &request);
 
-    if (ret < 0)
-    {
-        fprintf(stderr,
-                "cannot set cam request buffers: ioctl error - '%s'\n",
-                strerror(errno));
+    if (ret < 0) {
+        fprintf(stderr, "cannot set cam request buffers: ioctl error - '%s'\n", strerror(errno));
 
         return false;
     }
@@ -311,14 +368,9 @@ bool InnoMakerOV9281::initCam()
 
         DBG("buffer.length: %d", buffer.length);
         DBG("buffer.m.offset: %d", buffer.m.offset);
-        video_buffer_ptr[i] = (uint8_t *) mmap(NULL,
-                                               buffer.length,
-                                               PROT_READ | PROT_WRITE,
-                                               MAP_SHARED,
-                                               m_cam_fd,
-                                               buffer.m.offset);
-        if (video_buffer_ptr[i] == MAP_FAILED)
-        {
+        video_buffer_ptr[i] = (uint8_t *)
+            mmap(NULL, buffer.length, PROT_READ | PROT_WRITE, MAP_SHARED, m_cam_fd, buffer.m.offset);
+        if (video_buffer_ptr[i] == MAP_FAILED) {
             DBG("mmap() failed %d(%s)", errno, strerror(errno));
             return false;
         }
@@ -334,20 +386,23 @@ bool InnoMakerOV9281::initCam()
         }
     }
 
-    int buffer_type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-    ret = ioctl(m_cam_fd, VIDIOC_STREAMON, &buffer_type);
-    if (ret != 0)
-    {
-        DBG("ioctl(VIDIOC_STREAMON) failed %d(%s)", errno, strerror(errno));
-        return false;
-    }
+    // int buffer_type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+    // ret = ioctl(m_cam_fd, VIDIOC_STREAMON, &buffer_type);
+    // if (ret != 0)
+    // {
+    //     DBG("ioctl(VIDIOC_STREAMON) failed %d(%s)", errno, strerror(errno));
+    //     return false;
+    // }
 
     DBG("cam init done.");
 
     return true;
 }
 
-bool InnoMakerOV9281::getImage(Image &image)
+// Image &InnoMakerOV9281::getImage()
+// bool InnoMakerOV9281::getImage(Image &image)
+bool InnoMakerOV9281::getImage(size_t &imageIndex)
+// TODO: get Image from video_buffer_ptr
 {
     static struct timeval curr, prev;
     static uint16_t counter = 0;
@@ -359,17 +414,25 @@ bool InnoMakerOV9281::getImage(Image &image)
 
     if (elapsedTime > 1000.)
     {
-        fprintf(stderr, "fps: %d, sec: %d\n", counter, curr.tv_sec);
         fprintf(stderr,
-                "sum: %d,\tcorr: %d,\tval: %d\n",
+                "fps: %d\tsec: %d\tdq: %d\tget: %d\trot: %d\tsum: %d,\tcorr: %d,\tval: %d\n",
+                counter,
+                curr.tv_sec % 1000,
+                dq_elapsed_ns / 1000 / counter,
+                get_elapsed_ns / 1000 / counter,
+                rot_elapsed_ns / 1000 / counter,
                 sum_elapsed_ns / 1000 / counter,
                 corr_elapsed_ns / 1000 / counter,
                 // max_elapsed_ns / 1000 / counter,
                 value_elapsed_ns / 1000 / counter);
+
+        dq_elapsed_ns = 0;
+        get_elapsed_ns = 0;
         sum_elapsed_ns = 0;
         corr_elapsed_ns = 0;
         max_elapsed_ns = 0;
         value_elapsed_ns = 0;
+        rot_elapsed_ns = 0;
 
         counter = 0;
         prev = curr;
@@ -383,10 +446,14 @@ bool InnoMakerOV9281::getImage(Image &image)
     buffer.memory = V4L2_MEMORY_MMAP;
     buffer.index = BUFFER_COUNT;
 
-    ret = ioctl(m_cam_fd, VIDIOC_DQBUF, &buffer);
-
-    if (ret != 0)
     {
+        QElapsedTimer t;
+        t.start();
+        ret = ioctl(m_cam_fd, VIDIOC_DQBUF, &buffer);
+        dq_elapsed_ns += t.nsecsElapsed();
+    }
+
+    if (ret != 0) {
         DBG("ioctl(VIDIOC_DQBUF) failed %d(%s)", errno, strerror(errno));
         return false;
     }
@@ -397,19 +464,34 @@ bool InnoMakerOV9281::getImage(Image &image)
         return false;
     }
 
+    // auto &image = video_buffer_ptr[buffer.index];
+    imageIndex = buffer.index;
+    auto &image = m_images[buffer.index];
     image.height = img_height;
     image.width = img_width;
     // TODO: fill
     // image.counters.encoderPosition = RotaryEncoder::instance()->position();
     image.counters.measurementCounter = buffer.sequence;
-    image.counters.timestampUs = buffer.timestamp.tv_sec * 1000 * 1000 +
-                                 buffer.timestamp.tv_usec;
-    memcpy(image.data, video_buffer_ptr[buffer.index], img_size);
+    image.counters.timestampUs = buffer.timestamp.tv_sec * 1000 * 1000 + buffer.timestamp.tv_usec;
+
+    {
+        QElapsedTimer t;
+        t.start();
+        memcpy(&image.data, video_buffer_ptr[buffer.index], img_size);
+        // std::cout << (void *) video_buffer_ptr[buffer.index] << std::endl;
+        get_elapsed_ns += t.nsecsElapsed();
+    }
+
+    static bool done{false};
+
+    if (!done) {
+        // memcpy(image.data.data(), video_buffer_ptr[buffer.index], img_size);
+        done = true;
+    }
 
     ret = ioctl(m_cam_fd, VIDIOC_QBUF, &buffer);
 
-    if (ret != 0)
-    {
+    if (ret != 0) {
         DBG("ioctl(VIDIOC_QBUF) failed %d(%s)", errno, strerror(errno));
         return false;
     }
diff --git a/src/camera/innomakerov9281.h b/src/camera/innomakerov9281.h
index ccfe1ac..7f2d206 100644
--- a/src/camera/innomakerov9281.h
+++ b/src/camera/innomakerov9281.h
@@ -33,7 +33,11 @@ public:
 
     bool setLaserLevel(int value) override;
 
-    bool getImage(Image &image);
+    bool setSomething(int value) override;
+
+    // bool getImage(Image &image);
+    bool getImage(size_t &image);
+    // Image &getImage();
 
 public:
     libcamera::Signal<std::shared_ptr<Pixels>> newPixels;
@@ -48,7 +52,12 @@ private:
 private:
     int m_cam_fd{-1};
     static constexpr uint8_t BUFFER_COUNT{3};
+    // #ifdef USER_PTR
+    Image m_images[BUFFER_COUNT];
+    // #else
     uint8_t *video_buffer_ptr[BUFFER_COUNT];
+    // #endif
     // buffer_t m_buf;
     std::jthread m_streamThread;
+    static inline std::jthread m_someThread;
 };
diff --git a/src/camera/ov9281.cpp b/src/camera/ov9281.cpp
index 1a324e4..5b6936d 100644
--- a/src/camera/ov9281.cpp
+++ b/src/camera/ov9281.cpp
@@ -259,14 +259,14 @@ std::vector<std::shared_ptr<OV9281>> OV9281::search(
     {
         auto id = camera->id();
         auto c = manager->get(id);
-        auto ov9281 = std::shared_ptr<OV9281>(new OV9281(c));
+        // auto ov9281 = std::shared_ptr<OV9281>(new OV9281(c));
 
-        if (!ov9281->init())
-        {
-            continue;
-        }
+        // if (!ov9281->init())
+        // {
+        //     continue;
+        // }
 
-        result.push_back(ov9281);
+        // result.push_back(ov9281);
     }
 
     return result;