Merge branch 'development' into test/easter_test

motion/thruster_interface_asv/src/thruster_interface_asv_driver_lib.cpp

@@ -117,6 +117,7 @@ int16_t *_interpolate_force_to_pwm(float *forces) {
 const int8_t _I2C_BUS = 1;
 const int16_t _I2C_ADDRESS = 0x21;
 const char *_I2C_DEVICE = "/dev/i2c-1";
+int _fileI2C = -1;
 void _send_pwm_to_ESCs(int16_t *pwm) {
   // Variables ----------
   // 4 PWM values of 16-bits
@@ -138,37 +139,14 @@ void _send_pwm_to_ESCs(int16_t *pwm) {
   }
 
   // Data sending ----------
-
-  int fileI2C = -1;
-
   try {
-    // Open I2C conection
-    int fileI2C = open(_I2C_DEVICE, O_RDWR);
-
-    // Error handling in case of edge cases with I2C
-    if (fileI2C < 0) {
-      throw std::runtime_error("ERROR: Couldn't opening I2C device");
-    }
-    if (ioctl(fileI2C, I2C_SLAVE, _I2C_ADDRESS) < 0) {
-      throw std::runtime_error("ERROR: Couldn't set I2C address");
-    }
     // Send the I2C message
-    if (write(fileI2C, messageInBytesPWM, dataSize) != dataSize) {
+    if (write(_fileI2C, messageInBytesPWM, dataSize) != dataSize) {
       throw std::runtime_error(
           "ERROR: Couldn't send data, ignoring message...");
     }
   } catch (const std::exception &error) {
     std::cerr << error.what() << std::endl;
-
-    // Close I2C connection if we connected to I2C
-    if (fileI2C >= 0) {
-      close(fileI2C);
-    }
-  }
-
-  // Close I2C connection if we connected to I2C
-  if (fileI2C >= 0) {
-    close(fileI2C);
   }
 }
 
@@ -194,6 +172,22 @@ void init(const std::string &pathToCSVFile, int8_t *thrusterMapping,
     _minPWM[i] = minPWM[i];
     _maxPWM[i] = maxPWM[i];
   }
+
+  // Connecting to the I2C
+  try {
+    // Open I2C conection
+    _fileI2C = open(_I2C_DEVICE, O_RDWR);
+
+    // Error handling in case of edge cases with I2C
+    if (_fileI2C < 0) {
+      throw std::runtime_error("ERROR: Couldn't opening I2C device");
+    }
+    if (ioctl(_fileI2C, I2C_SLAVE, _I2C_ADDRESS) < 0) {
+      throw std::runtime_error("ERROR: Couldn't set I2C address");
+    }
+  } catch (const std::exception &error) {
+    std::cerr << error.what() << std::endl;
+  }
 }
 
 // The main core functionality of interacting and controling the thrusters