add support of inno-maker ov9281

9 files changed, 382 insertions, 34 deletions

diff --git a/src/calibration.cpp b/src/calibration.cpp
index 7fcdbcc..33423b3 100644
--- a/src/calibration.cpp
+++ b/src/calibration.cpp
@@ -82,9 +82,9 @@ void interpolate(
                   [](auto& column) { interpolate(column); });
 
     for (size_t i = 9471; i < 9472; i++) {
-        std::cout << "row #" << i << ": ";
+        std::cout << __func__ << ": row #" << i << ": ";
 
-        for (size_t j = 0; j < 1280; ++j) {
+        for (size_t j = 640 - 5; j < 640 + 5; ++j) {
             const auto& p = table->at(j).at(i);
             std::cout << p << ' ';
         }
diff --git a/src/camera/innomakerov9281.cpp b/src/camera/innomakerov9281.cpp
new file mode 100644
index 0000000..6d7aa4a
--- /dev/null
+++ b/src/camera/innomakerov9281.cpp
@@ -0,0 +1,280 @@
+#include "innomakerov9281.h"
+
+#include <errno.h>
+#include <fcntl.h>
+#include <linux/videodev2.h>
+#include <stdio.h>
+#include <string.h>
+#include <sys/ioctl.h>
+#include <sys/mman.h>
+#include <unistd.h>
+
+#include "constants.h"
+// #include "rotaryencoder.h"
+
+#define LOGD(...) \
+    do { \
+        printf(__VA_ARGS__); \
+        printf("\n"); \
+    } while (0)
+
+#define DBG(fmt, args...) LOGD("%s:%d, " fmt, __FUNCTION__, __LINE__, ##args);
+
+extern uint64_t sum_elapsed_ns;
+extern uint64_t corr_elapsed_ns;
+extern uint64_t max_elapsed_ns;
+extern uint64_t value_elapsed_ns;
+
+constexpr char videoDevice[] = "/dev/video0";
+
+InnoMakerOV9281::InnoMakerOV9281() {}
+
+InnoMakerOV9281::~InnoMakerOV9281()
+{
+    int ret{-1};
+
+    for (int i = 0; i < BUFFER_COUNT; ++i) {
+        ret = munmap(video_buffer_ptr[i], img_size);
+
+        if (ret < 0) {
+            DBG("Munmap failed!!.");
+        }
+    }
+
+    // std::cout << __func__ << std::endl;
+    if (m_cam_fd >= 0) {
+        int ret = close(m_cam_fd);
+
+        if (ret == -1) {
+            // std::cout << __func__
+            //           << ": cannot close camera: " << strerror(errno)
+            //           << std::endl;
+        }
+    };
+
+    // std::cout << __func__ << ": success" << std::endl;
+}
+
+bool InnoMakerOV9281::init()
+{
+    if (!openCam())
+        return false;
+
+    if (!selectCam())
+        return false;
+
+    if (!initCam())
+        return false;
+
+    return true;
+}
+
+bool InnoMakerOV9281::setExposureTimeMs(int value)
+{
+    return setCamParam(V4L2_CID_EXPOSURE, value);
+}
+
+bool InnoMakerOV9281::setGain(int value)
+{
+    return setCamParam(V4L2_CID_GAIN, value);
+}
+
+bool InnoMakerOV9281::setCamParam(unsigned int v4l2controlId, int value)
+{
+    v4l2_control ctl{v4l2controlId, value};
+
+    int ret = ioctl(m_cam_fd, VIDIOC_S_CTRL, &ctl);
+
+    if (ret < 0) {
+        fprintf(stderr,
+                "cannot set cam param: id - %d, error - '%s'\n",
+                v4l2controlId,
+                strerror(errno));
+
+        return false;
+    }
+
+    return true;
+}
+
+bool InnoMakerOV9281::openCam()
+{
+    m_cam_fd = open(videoDevice, O_RDWR);
+
+    if (m_cam_fd < 0) {
+        fprintf(stderr, "cannot open cam '%s', error: '%s'\n", videoDevice, strerror(errno));
+        return false;
+    }
+
+    return true;
+}
+
+bool InnoMakerOV9281::selectCam(int camIdx)
+{
+    int input = camIdx;
+    int ret = ioctl(m_cam_fd, VIDIOC_S_INPUT, &input);
+
+    if (ret < 0) {
+        fprintf(stderr, "cannot select cam: idx - %d, error - '%s'\n", camIdx, strerror(errno));
+
+        return false;
+    }
+
+    return true;
+}
+
+bool InnoMakerOV9281::initCam()
+{
+    v4l2_format format;
+    memset(&format, 0, sizeof(v4l2_format));
+    format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+    format.fmt.pix.pixelformat = V4L2_PIX_FMT_GREY;
+    format.fmt.pix.width = img_width;
+    format.fmt.pix.height = img_height;
+
+    int ret = ioctl(m_cam_fd, VIDIOC_TRY_FMT, &format);
+
+    if (ret < 0) {
+        fprintf(stderr, "cannot try cam format: error - '%s'\n", strerror(errno));
+
+        return false;
+    }
+
+    // TODO: remove this?
+    format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+    ret = ioctl(m_cam_fd, VIDIOC_S_FMT, &format);
+
+    if (ret < 0) {
+        fprintf(stderr, "cannot set cam format: error - '%s'\n", strerror(errno));
+
+        return false;
+    }
+
+    struct v4l2_requestbuffers request;
+    request.count = BUFFER_COUNT;
+    request.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+    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));
+
+        return false;
+    }
+
+    if (request.count < BUFFER_COUNT) {
+        fprintf(stderr, "cannot set cam request buffers\n");
+
+        return false;
+    }
+
+    struct v4l2_buffer buffer;
+    memset(&buffer, 0, sizeof(buffer));
+    buffer.type = request.type;
+    buffer.memory = V4L2_MEMORY_MMAP;
+
+    for (uint32_t i = 0; i < request.count; i++) {
+        buffer.index = i;
+        ret = ioctl(m_cam_fd, VIDIOC_QUERYBUF, &buffer);
+
+        if (ret < 0) {
+            DBG("ioctl(VIDIOC_QUERYBUF) failed %d(%s)", errno, strerror(errno));
+            return false;
+        }
+
+        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) {
+            DBG("mmap() failed %d(%s)", errno, strerror(errno));
+            return false;
+        }
+
+        buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+        buffer.memory = V4L2_MEMORY_MMAP;
+        buffer.index = i;
+        ret = ioctl(m_cam_fd, VIDIOC_QBUF, &buffer);
+        if (ret != 0) {
+            DBG("ioctl(VIDIOC_QBUF) 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)
+{
+    static struct timeval curr, prev;
+    static uint16_t counter = 0;
+    gettimeofday(&curr, NULL);
+    ++counter;
+
+    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
+
+    if (elapsedTime > 1000.) {
+        // fprintf(stderr, "fps: %d, sec: %d\n", counter, curr.tv_sec);
+        fprintf(stderr,
+                "sum: %d,\tcorr: %d,\tval: %d\n",
+                sum_elapsed_ns / 1000 / counter,
+                corr_elapsed_ns / 1000 / counter,
+                // max_elapsed_ns / 1000 / counter,
+                value_elapsed_ns / 1000 / counter);
+        sum_elapsed_ns = 0;
+        corr_elapsed_ns = 0;
+        max_elapsed_ns = 0;
+        value_elapsed_ns = 0;
+
+        counter = 0;
+        prev = curr;
+    }
+
+    int ret;
+    struct v4l2_buffer buffer;
+
+    memset(&buffer, 0, sizeof(buffer));
+    buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+    buffer.memory = V4L2_MEMORY_MMAP;
+    buffer.index = BUFFER_COUNT;
+
+    ret = ioctl(m_cam_fd, VIDIOC_DQBUF, &buffer);
+
+    if (ret != 0) {
+        DBG("ioctl(VIDIOC_DQBUF) failed %d(%s)", errno, strerror(errno));
+        return false;
+    }
+
+    if (buffer.index < 0 || buffer.index >= BUFFER_COUNT) {
+        DBG("invalid buffer index: %d", buffer.index);
+        return false;
+    }
+
+    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);
+
+    ret = ioctl(m_cam_fd, VIDIOC_QBUF, &buffer);
+
+    if (ret != 0) {
+        DBG("ioctl(VIDIOC_QBUF) failed %d(%s)", errno, strerror(errno));
+        return false;
+    }
+
+    return true;
+}
diff --git a/src/camera/innomakerov9281.h b/src/camera/innomakerov9281.h
new file mode 100644
index 0000000..bcafc12
--- /dev/null
+++ b/src/camera/innomakerov9281.h
@@ -0,0 +1,36 @@
+#pragma once
+
+#include <cstdint>
+
+#include "constants.h"
+#include "image.h"
+
+class InnoMakerOV9281
+{
+public:
+    using buffer_t = std::array<uint8_t, img_size>;
+
+public:
+    InnoMakerOV9281();
+    ~InnoMakerOV9281();
+
+public:
+    bool init();
+
+    bool setExposureTimeMs(int value);
+    bool setGain(int value);
+
+    bool getImage(Image &image);
+
+private:
+    bool setCamParam(unsigned int v4l2controlId, int value);
+    bool openCam();
+    bool selectCam(int camIdx = 0);
+    bool initCam();
+
+private:
+    int m_cam_fd{-1};
+    static constexpr uint8_t BUFFER_COUNT{3};
+    uint8_t *video_buffer_ptr[BUFFER_COUNT];
+    // buffer_t m_buf;
+};
diff --git a/src/camera/ov9281.cpp b/src/camera/ov9281.cpp
index 4d393a0..012eab6 100644
--- a/src/camera/ov9281.cpp
+++ b/src/camera/ov9281.cpp
@@ -18,9 +18,15 @@
 #include "rotaryencoder.h"
 #include "typedefs.h"
 
+#include <QElapsedTimer>
+
+QElapsedTimer timer;
+size_t fpsCounter{0};
+
 OV9281::OV9281(const std::shared_ptr<libcamera::Camera> &camera)
     : INIT_FIELD(camera)
 {
+    timer.start();
     std::cout << __func__ << ":\tid: " << m_camera->id();
 }
 
@@ -54,7 +60,7 @@ bool OV9281::init()
         return false;
     }
 
-    m_config->orientation = libcamera::Orientation::Rotate90;
+    // m_config->orientation = libcamera::Orientation::Rotate90;
 
     libcamera::StreamConfiguration &streamConfig = m_config->at(0);
 
@@ -135,6 +141,14 @@ bool OV9281::applyConfig()
  */
 void OV9281::onRequestCompleted(libcamera::Request *completed_request)
 {
+    fpsCounter++;
+
+    if (timer.elapsed() > 1000) {
+        std::cout << "fps: " << fpsCounter << std::endl;
+        timer.restart();
+        fpsCounter = 0;
+    }
+
     using namespace libcamera;
 
     if (completed_request->status() == Request::RequestCancelled)
@@ -146,6 +160,7 @@ void OV9281::onRequestCompleted(libcamera::Request *completed_request)
 
     const auto &buffers = completed_request->buffers();
 
+#if 1
     for (auto [stream, buffer] : buffers)
     {
         const auto &streamConfig = stream->configuration();
@@ -170,13 +185,12 @@ void OV9281::onRequestCompleted(libcamera::Request *completed_request)
             img->height = imageSize.height;
 
             memcpy(img->data, data, size);
-            img->dataSize = size;
-            img->stride = stride;
+            // img->dataSize = size;
+            // img->stride = stride;
             img->pixelFormat = pixelFormat;
             img->counters.measurementCounter = metadata.sequence;
             img->counters.timestampUs = metadata.timestamp / 1000;
             img->counters.encoderPosition = RotaryEncoder::instance()->position();
-
             img->rotate();
 
             auto pixels = img->pixels();
@@ -185,12 +199,13 @@ void OV9281::onRequestCompleted(libcamera::Request *completed_request)
             if (!pixels) {
                 std::cerr << "emit empty pixels" << std::endl;
             }
+            newImage.emit(img);
             newPixels.emit(pixels);
 #define emit
 #endif
         }
     }
-
+#endif
     const libcamera::ControlList &metadata = completed_request->metadata();
     const ControlInfoMap &control_map = m_camera->controls();
     // const ControlIdMap & ctrlIdMap = control_map.idmap();
@@ -207,11 +222,11 @@ void OV9281::onRequestCompleted(libcamera::Request *completed_request)
     static auto lastControls = completed_request->controls();
 
     completed_request->reuse(Request::ReuseBuffers);
-    completed_request->controls().set(libcamera::controls::AeEnable, false);
-    completed_request->controls()
-        .set(libcamera::controls::draft ::NoiseReductionMode,
-             libcamera::controls::draft ::NoiseReductionModeEnum ::
-                 NoiseReductionModeHighQuality);
+    // completed_request->controls().set(libcamera::controls::AeEnable, false);
+    // completed_request->controls()
+    //     .set(libcamera::controls::draft ::NoiseReductionMode,
+    //          libcamera::controls::draft ::NoiseReductionModeEnum ::
+    //              NoiseReductionModeHighQuality);
 
     completed_request->controls().set(libcamera::controls::ExposureTime,
                                       m_exposureTime);
diff --git a/src/camera/ov9281.h b/src/camera/ov9281.h
index f70db2f..e989d96 100644
--- a/src/camera/ov9281.h
+++ b/src/camera/ov9281.h
@@ -37,6 +37,7 @@ public:
     // TODO: image->pixels in separate thread
     // TODO: respect sender/receiver threads
     libcamera::Signal<std::shared_ptr<Pixels>> newPixels;
+    libcamera::Signal<std::shared_ptr<Image>> newImage;
 
 private:
     explicit OV9281(const std::shared_ptr<libcamera::Camera> &camera);
@@ -51,7 +52,7 @@ private:
 
     // constants
 private:
-    static inline constexpr auto pixelFormat{libcamera::formats::R16};
+    static inline constexpr auto pixelFormat{libcamera::formats::R8};
     static inline constexpr unsigned int bufferCount{2};
     static inline constexpr size_t desiredFPS{144};
 
@@ -64,5 +65,5 @@ private:
     std::unique_ptr<libcamera::FrameBufferAllocator> m_allocator{nullptr};
 
     // TODO: set exposureTime from outside
-    int32_t m_exposureTime{1000};
+    int32_t m_exposureTime{100};
 };
diff --git a/src/constants.h b/src/constants.h
index 1c451ab..b7fd7bc 100644
--- a/src/constants.h
+++ b/src/constants.h
@@ -3,10 +3,13 @@
 #include <cstddef>
 #include <cstdint>
 
+// TODO: get rid of qstring
 #include <QString>
 
 constexpr size_t img_width = 1280;
 constexpr size_t img_height = 800;
+constexpr size_t img_size = img_width * img_height;
+
 constexpr uint32_t patternSize = 16;
 
 constexpr float hardcodedZRangeMm{175.f};
diff --git a/src/dumps.cpp b/src/dumps.cpp
index 43a839c..0a85c1b 100644
--- a/src/dumps.cpp
+++ b/src/dumps.cpp
@@ -53,6 +53,7 @@ QList<Pixels> openDump(
     qDebug() << "create results array" << filenames.size();
     auto resultOptionals = QScopedPointer(
         new QList<std::optional<Pixels>>(filenames.size()));
+    qDebug() << "created results array";
 
     QElapsedTimer t;
     t.start();
diff --git a/src/image.cpp b/src/image.cpp
index 7d1c824..ce814da 100644
--- a/src/image.cpp
+++ b/src/image.cpp
@@ -1,11 +1,20 @@
 #include "image.h"
 
+#include <QElapsedTimer>
+
 #include "macro.h"
 #include "pixels.h"
 
-float process_column(const uint16_t (&column)[])
+uint64_t sum_elapsed_ns = 0;
+uint64_t corr_elapsed_ns = 0;
+uint64_t max_elapsed_ns = 0;
+uint64_t value_elapsed_ns = 0;
+
+float process_column(const uint8_t (&column)[])
 {
     start_timer(process_column);
+    QElapsedTimer t;
+    t.start();
 
     float result = std::numeric_limits<float>::quiet_NaN();
 
@@ -24,23 +33,20 @@ float process_column(const uint16_t (&column)[])
     memset(correlation, 0, img_height * sizeof(correlation[0]));
     integralSum[0] = 0;
 
-    for (uint32_t i = 1; i < img_height; ++i)
-    {
-        // if (column[i] < 100)
-        // {
-        //     column[i] = 0;
-        // }
-
-        integralSum[i] = column[i] / 256 + integralSum[i - 1];
+    for (uint32_t i = 1; i < img_height; ++i) {
+        integralSum[i] = column[i] + integralSum[i - 1];
     }
+    sum_elapsed_ns += t.nsecsElapsed();
+    t.restart();
 
     for (uint32_t i = 0; i < correlationSize; ++i)
-        correlation[i + patternSize / 2] = column[i + patternSize / 2] / 256 *
-                                           (integralSum[i + patternOffset] -
-                                            integralSum[i]);
+        correlation[i + patternSize / 2] = column[i + patternSize / 2]
+                                           * (integralSum[i + patternOffset] - integralSum[i]);
 
-    for (uint32_t i = 3; i < img_height - 2; ++i)
-    {
+    corr_elapsed_ns += t.nsecsElapsed();
+    t.restart();
+
+    for (uint32_t i = 3; i < img_height - 2; ++i) {
         const auto sum = correlation[i - 1] + correlation[i] +
                          correlation[i + 1];
 
@@ -69,6 +75,9 @@ float process_column(const uint16_t (&column)[])
         }
     }
 
+    value_elapsed_ns += t.nsecsElapsed();
+    t.restart();
+
     result = (y2 != y1) ? (float(x1) - (float(y1) / (y2 - y1)))
                         : std::numeric_limits<float>::quiet_NaN();
 
@@ -81,6 +90,8 @@ void Image::rotate()
 
     using namespace std;
 
+#pragma omp parallel
+#pragma omp for
     for (size_t i = 0; i < img_height; ++i)
     {
         for (size_t j = 0; j < img_width; ++j)
@@ -99,8 +110,9 @@ std::shared_ptr<Pixels> Image::pixels() const
 
     start_timer(process_columns);
 
-    for (size_t i = 0; i < width; i++)
-    {
+#pragma omp chunk
+#pragma omp parallel for
+    for (size_t i = 0; i < img_width; i++) {
         result->pixels[i] = process_column(rotated_cw[i]);
     }
 
diff --git a/src/image.h b/src/image.h
index 2fff020..1ec1b93 100644
--- a/src/image.h
+++ b/src/image.h
@@ -9,10 +9,10 @@ struct Image
 {
     int width{0};
     int height{0};
-    uint16_t data[img_height][img_width] = {{0}};
-    uint16_t rotated_cw[img_width][img_height] = {{0}};
-    size_t dataSize{0};
-    unsigned int stride{0};
+    uint8_t data[img_height][img_width] = {{0}};
+    uint8_t rotated_cw[img_width][img_height] = {{0}};
+    // size_t dataSize{0};
+    // unsigned int stride{0};
     libcamera::PixelFormat pixelFormat{0};
     Counters counters{};