Added mutex to MODBUS and I@c for mutithreading

lib/I2C/I2C_Driver.cpp

@@ -3,21 +3,20 @@
 namespace drivers
 {
 
-  I2C::I2C()
+  I2C::I2C(): m_initialized(true)
   {
     Wire.begin(I2C_SDA_PIN, I2C_SCL_PIN);
-    isInitialized = true;
   }
 
   I2C::~I2C()
   {
     Wire.end();
-    isInitialized = true;
+    m_initialized = false;
   }
 
   const bool I2C::read(const uint8_t deviceAddr, const uint8_t deviceReg, const uint8_t len, std::vector<uint8_t> &data)
   {
-    //busy.try_lock();
+    std::lock_guard<std::mutex> lock(m_mutex);
     Wire.beginTransmission(deviceAddr);
     Wire.write(deviceReg);
     switch (Wire.endTransmission(true))
@@ -45,13 +44,12 @@ namespace drivers
     {
       data[i] = static_cast<uint8_t>(Wire.read());
     }
-    //busy.unlock();
     return true;
   }
 
   const bool I2C::write(const uint8_t deviceAddr, const uint8_t deviceReg, const std::vector<uint8_t> &data)
   {
-    //busy.lock();
+    std::lock_guard<std::mutex> lock(m_mutex);
     Wire.beginTransmission(deviceAddr);
     Wire.write(deviceReg);
     for (auto d : data)
@@ -73,7 +71,6 @@ namespace drivers
       LOG_ERROR("Unknown Error");
       return false;
     }
-    //busy.unlock();
     return true;
   }