1 files changed, 44 insertions, 9 deletions
diff --git a/src/camera/veye_i2c.cpp b/src/camera/veye_i2c.cpp
index 9d04d9b..c34da51 100644
--- a/src/camera/veye_i2c.cpp
+++ b/src/camera/veye_i2c.cpp
@@ -15,15 +15,17 @@
 bool veye::imx287m::i2cRead(int fd, uint8_t i2c_addr, uint16_t reg, uint32_t &value)
 {
     int err;
-    int i = 0;
-    uint8_t buf[2] = {reg >> 8, reg & 0xff};
+    uint8_t bufin[2] = {reg >> 8, reg & 0xff};
     uint8_t bufout[4] = {0};
     struct i2c_msg msgs[2] = {
-        {.addr = i2c_addr,
-         .flags = 0,
-         .len = 2,
-         // .buf = (uint8_t *) (&reg),
-         .buf = buf},
+        {
+            //
+            .addr = i2c_addr,
+            .flags = 0,
+            .len = 2,
+            .buf = bufin
+            //
+        },
         {
             .addr = i2c_addr,
             .flags = I2C_M_RD,
@@ -39,7 +41,7 @@ bool veye::imx287m::i2cRead(int fd, uint8_t i2c_addr, uint16_t reg, uint32_t &va
     err = ioctl(fd, I2C_RDWR, &msgset);
     //printf("Read i2c addr %02X\n", i2c_addr);
     if (err != msgset.nmsgs) {
-        std::cerr << "Read i2c err " << err << std::endl;
+        std::cerr << "read i2c err " << err << std::endl;
         return false;
     }
 
@@ -51,7 +53,8 @@ bool veye::imx287m::i2cRead(int fd, uint8_t i2c_addr, uint16_t reg, uint32_t &va
 
 bool veye::imx287m::test(uint32_t value)
 {
-    const std::string i2cDevName{"/dev/i2c-10"};
+    // 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);
 
@@ -104,3 +107,35 @@ bool veye::imx287m::i2cWrite(int fd, uint16_t reg, const uint32_t value)
 
     return true;
 }
+
+veye::imx287m::i2c::i2c(const std::string &name, const int address)
+    : m_name{name}
+    , m_address{address}
+{}
+
+veye::imx287m::i2c::~i2c()
+{
+    if (m_fd > 0) {
+        close(m_fd);
+    }
+}
+
+bool veye::imx287m::i2c::open()
+{
+    m_fd = ::open(m_name.c_str(), O_RDWR);
+
+    if (!m_fd) {
+        std::cerr << "cannot open i2c device " << m_name << ", error: " << strerror(errno)
+                  << std::endl;
+        return false;
+    }
+
+    if (ioctl(m_fd, I2C_SLAVE_FORCE, m_address) < 0) {
+        std::cerr << "cannot set i2c slave. dev: " << m_name << ", addr: " << m_address
+                  << ", error: " << strerror(errno) << std::endl;
+
+        return false;
+    }
+
+    return true;
+}