1 files changed, 32 insertions, 46 deletions
diff --git a/src/camera/veye_i2c.cpp b/src/camera/veye_i2c.cpp
index c34da51..b9ad2fa 100644
--- a/src/camera/veye_i2c.cpp
+++ b/src/camera/veye_i2c.cpp
@@ -12,7 +12,9 @@
 
 #include "veyeimx287m_types.h"
 
-bool veye::imx287m::i2cRead(int fd, uint8_t i2c_addr, uint16_t reg, uint32_t &value)
+std::optional<uint32_t> veye::imx287m::i2c_read(int fd,
+                                                uint8_t i2c_addr,
+                                                uint16_t reg)
 {
     int err;
     uint8_t bufin[2] = {reg >> 8, reg & 0xff};
@@ -42,56 +44,16 @@ bool veye::imx287m::i2cRead(int fd, uint8_t i2c_addr, uint16_t reg, uint32_t &va
     //printf("Read i2c addr %02X\n", i2c_addr);
     if (err != msgset.nmsgs) {
         std::cerr << "read i2c err " << err << std::endl;
-        return false;
+        return {};
     }
 
-    value = ntohl(*(uint32_t *) bufout);
-    // fprintf(stderr, "addr %04x : value %08x \n", reg + i, value);
+    const auto result = ntohl(*(uint32_t *) bufout);
+    // fprintf(stderr, "addr %04x : value %08x \n", reg + i, result);
 
-    return true;
+    return result;
 }
 
-bool veye::imx287m::test(uint32_t value)
-{
-    // const std::string i2cDevName{"/dev/i2c-10"};
-    const std::string i2cDevName{"/dev/i2c-2"};
-    constexpr uint32_t i2cDevAddr{0x3b};
-    int fd = open(i2cDevName.c_str(), O_RDWR);
-
-    if (!fd) {
-        std::cerr << "cannot open i2c device " << i2cDevName << ", error: " << strerror(errno)
-                  << std::endl;
-        return false;
-    }
-
-    if (ioctl(fd, I2C_SLAVE_FORCE, i2cDevAddr) < 0) {
-        std::cerr << "cannot set i2c slave. dev: " << i2cDevName << ", addr: " << i2cDevAddr
-                  << ", error: " << strerror(errno) << std::endl;
-
-        return false;
-    }
-
-    uint32_t expTime{0};
-    // if (!i2cRead(fd, i2cDevAddr, (uint16_t) veye::imx287m::Register::ME_Time, expTime)) {
-    //     return false;
-    // }
-
-    // std::cout << "exp time is: " << expTime << std::endl;
-    std::cout << "set exp time to " << value;
-
-    if (!i2cWrite(fd, (uint16_t) veye::imx287m::Register::ME_Time, value)) {
-        return false;
-    }
-
-    if (!i2cRead(fd, i2cDevAddr, (uint16_t) veye::imx287m::Register::ME_Time, expTime)) {
-        return false;
-    }
-    std::cout << "\texp time is: " << expTime << std::endl;
-
-    return true;
-}
-
-bool veye::imx287m::i2cWrite(int fd, uint16_t reg, const uint32_t value)
+bool veye::imx287m::i2c_write(int fd, uint16_t reg, const uint32_t value)
 {
     uint8_t msg[] = {reg >> 8, reg & 0xff, value >> 24, value >> 16, value >> 8, value >> 0};
     int len = sizeof(msg) / sizeof(msg[0]);
@@ -120,8 +82,32 @@ veye::imx287m::i2c::~i2c()
     }
 }
 
+std::optional<uint32_t> veye::imx287m::i2c::read(uint16_t reg)
+{
+    if (m_fd == -1 && !open()) {
+        return {};
+    }
+
+    return i2c_read(m_fd, m_address, reg);
+}
+
+bool veye::imx287m::i2c::write(uint16_t reg, const uint32_t value)
+{
+    if (m_fd == -1 && !open()) {
+        return false;
+    }
+
+    return i2c_write(m_fd, reg, value);
+}
+
 bool veye::imx287m::i2c::open()
 {
+    if (m_fd != -1) {
+        std::cout << __func__ << ": " << m_name
+                  << " is already open, do nothing" << std::endl;
+        return true;
+    }
+
     m_fd = ::open(m_name.c_str(), O_RDWR);
 
     if (!m_fd) {