1 files changed, 280 insertions, 0 deletions
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;
+}