Enhancement/new thruster interface

asv_setup/config/robots/freya.yaml

@@ -50,3 +50,10 @@
           scaling: [ 1.0,  1.0,  1.0, 1.0, 1.0, 1.0]
 
     computer: "pc-debug"
+
+
+    thruster_interface:
+      publishing_rate: 1000 # [millisecond], Publishing 6 temperature values (ESC1, ESC2, ESC3, ESC4, AMBIENT1, AMBIENT2)
+                                  # and the status received from the arduino controlling the ESC
+      PWM_min: 1100 # Recommended : 1100 
+      PWM_max: 1900 # Recommended : 1900

motion/thruster_interface/include/thruster_interface/thruster_interface.hpp

@@ -1,31 +1,52 @@
-#pragma once
 
-#include <chrono>
 #include <cmath>
-#include <cstdint>
+#include <cstring> // Include for memcpy
 #include <fcntl.h>
 #include <fstream>
 #include <iostream>
 #include <linux/i2c-dev.h>
 #include <map>
 #include <sstream>
-#include <string>
 #include <sys/ioctl.h>
-#include <thread>
 #include <unistd.h>
 #include <vector>
+using namespace std;
 
-class ThrusterInterface {
-private:
-  std::map<double, double> pwm_table;
-  const int I2C_BUS = 1;
-  const int I2C_ADDRESS = 0x21;
-  const char *I2C_DEVICE = "/dev/i2c-1";
+#ifndef PWM_TABLE_HPP
+#define PWM_TABLE_HPP
+extern const char *I2C_DEVICE;
+extern int8_t i2c_slave_addr;
+extern std::map<float, float> pwm_table;
+#endif
+
+//--------------------------INITIALISATION--------------------------
+void init(int &file);
+
+//--------------------------SENDING--------------------------
+void send_status(int8_t status, int file);
+std::vector<uint8_t> pwm_to_bytes(std::vector<uint16_t> pwm_values);
+void send_pwm(std::vector<uint16_t> pwm_values, int file);
+
+//--------------------------RECEIVING--------------------------
+std::vector<float> readFloatsFromI2C(int file);
+uint8_t read_hardware_statusFromI2C(int file);
 
-  std::vector<uint8_t> pwm_to_bytes(const std::vector<int> &pwm_values);
+//--------------------------INTERPOLATION--------------------------
+void get_pwm_table();
+uint16_t interpolate(double force, int PWM_min, int PWM_max);
+std::vector<uint16_t> interpolate_all(std::vector<double> &force_values,
+                                      int PWM_min = 1100, int PWM_max = 1900);
+
+class I2C_Exception : public std::exception {
+private:
+  string message = " ";
 
 public:
-  ThrusterInterface(std::string mapping_file);
-  void publish_thrust_to_escs(std::vector<double> forces);
-  float interpolate(float force);
-};
+  I2C_Exception(string msg) { message = msg; }
+
+  const char *what() const
+
+      noexcept override {
+    return message.c_str();
+  }
+};

motion/thruster_interface/launch/thruster_interface.launch.py

@@ -4,13 +4,18 @@
 from launch_ros.actions import Node
 
 def generate_launch_description():
+    #Path to the YAML file
+    yaml_file_path = os.path.join(
+        get_package_share_directory("thruster_interface"),
+        "../../../../",                             # Go to the workspace
+        "src/vortex-asv/asv_setup/config/robots/",  # Go where the yaml file is located at
+        'freya.yaml'
+    )
     thruster_interface_node = Node(
             package='thruster_interface',
             executable='thruster_interface_node',
             name='thruster_interface_node',
-            parameters=[],
             output='screen',
+            parameters=[yaml_file_path],
         )
-    return LaunchDescription([
-        thruster_interface_node
-    ])
+    return LaunchDescription([thruster_interface_node])

motion/thruster_interface/src/thruster_interface.cpp

@@ -1,111 +1,160 @@
 #include <thruster_interface/thruster_interface.hpp>
 
-ThrusterInterface::ThrusterInterface(std::string mapping_file) {
-  // Open the data file
-  std::ifstream data(mapping_file);
-
-  if (!data.is_open()) {
-    std::cerr << "Unable to open file\n";
-    exit(1);
-  }
-
-  std::string line;
-  // Ignore the header line
-  std::getline(data, line);
+const char *I2C_DEVICE = "/dev/i2c-1";
+int8_t i2c_slave_addr = 0x21; // change later to 0x21
+std::map<float, float> pwm_table;
 
-  while (std::getline(data, line)) {
-    std::istringstream stream(line);
-
-    std::string force_str, pwm_str;
+//--------------------------FUNCTIONS--------------------------
 
-    std::getline(stream, force_str, '\t');
-    std::getline(stream, pwm_str);
+//--------------------------INITIALISATION--------------------------
 
-    double force = std::stod(force_str);
-    double pwm = std::stod(pwm_str);
+void init(int &file) {
+  if ((file = open(I2C_DEVICE, O_RDWR)) < 0) {
+    throw I2C_Exception("error, could not connect to i2c bus");
+  } else {
+    std::cout << "connected to i2c bus!!" << std::endl;
+  }
 
-    pwm_table[force] = pwm;
+  if (ioctl(file, I2C_SLAVE, i2c_slave_addr) < 0) {
+    throw I2C_Exception("error, could not set address");
+  } else {
+    std::cout << "i2c adress set" << std::endl;
   }
 }
 
-float ThrusterInterface::interpolate(float force) {
-  auto upper_bound = pwm_table.upper_bound(force);
-  if (upper_bound == pwm_table.end()) {
-    --upper_bound;
-  }
-  auto lower_bound = std::prev(upper_bound);
+//--------------------------SENDING--------------------------
 
-  if (lower_bound == pwm_table.end()) {
-    return upper_bound->second;
+void send_status(int8_t status, int file) {
+  if (write(file, &status, 1) != 1) {
+    throw I2C_Exception("error, could not send status");
+  } else {
+    std::cout << "status data has been sent" << std::endl;
   }
+}
 
-  float force1 = lower_bound->first;
-  float force2 = upper_bound->first;
-
-  float pwm1 = lower_bound->second;
-  float pwm2 = upper_bound->second;
-
-  if (force1 == force2) {
-    return pwm1;
+void send_pwm(std::vector<uint16_t> pwm_values, int file) {
+  std::vector<uint8_t> bytes = pwm_to_bytes(pwm_values);
+  if (static_cast<size_t>(write(file, bytes.data(), bytes.size())) !=
+      bytes.size()) {
+    throw I2C_Exception("error, could not send PWM data");
+  } else {
+    std::cout << "PWM data has been sent" << std::endl;
   }
-
-  int pwm_signal = std::round(
-      pwm1 + ((force - force1) * (pwm2 - pwm1)) / (force2 - force1) + 0.5);
-
-  return pwm_signal;
 }
 
-std::vector<uint8_t>
-ThrusterInterface::pwm_to_bytes(const std::vector<int> &pwm_values) {
+std::vector<uint8_t> pwm_to_bytes(std::vector<uint16_t> pwm_values) {
 
   std::vector<uint8_t> bytes;
-  for (const auto &val : pwm_values) {
-    // Ensure the value is in the correct range and cast to an integer
-    int pwm_int = static_cast<int>(
-        std::min(std::max(val, 1400), 1600)); // min 1100, max 1900
 
+  for (size_t i = 0; i < pwm_values.size(); ++i) {
     // Split the integer into most significant byte (MSB) and least significant
     // byte (LSB)
-    uint8_t msb = (pwm_int >> 8) & 0xFF;
-    uint8_t lsb = pwm_int & 0xFF;
-
+    uint8_t msb = (pwm_values[i] >> 8) & 0xFF;
+    uint8_t lsb = pwm_values[i] & 0xFF;
     // Add MSB and LSB to the bytes vector
     bytes.push_back(msb);
     bytes.push_back(lsb);
   }
   return bytes;
 }
 
-void ThrusterInterface::publish_thrust_to_escs(std::vector<double> forces) {
-  std::vector<int> pwm_values;
-  for (auto force : forces) {
-    pwm_values.push_back(interpolate(force));
+//--------------------------RECEIVING--------------------------
+
+std::vector<float> readFloatsFromI2C(int file) {
+  // Define byte length
+  int num_floats = 6;
+  int byte_length = num_floats * sizeof(float);
+  std::vector<float> floats(num_floats);
+
+  // Read a block of bytes from the I2C device
+  std::vector<uint8_t> data(byte_length);
+  if (read(file, data.data(), byte_length) != byte_length) {
+    throw I2C_Exception("Failed to read data from the I2C device");
+    return floats; // Returns empty vector if reading fails
   }
+  // Convert the byte array to a list of floats
+  memcpy(floats.data(), data.data(), byte_length);
+  return floats;
+}
 
-  std::vector<uint8_t> pwm_bytes = pwm_to_bytes(pwm_values);
-  int data_size = pwm_bytes.size();
+uint8_t read_hardware_statusFromI2C(int file) {
+  uint8_t hardware_status;
+  if (read(file, &hardware_status, 1) != 1) {
+    throw I2C_Exception("Failed to read hardware status from the I2C device");
+  }
+  return hardware_status;
+}
 
-  // DEBUG
-  // for (auto pwm : pwm_values) {
-  //    std::cout << pwm << " ";
-  //}
-  // std::cout << std::endl;
+//--------------------------INTERPOLATION--------------------------
 
-  int file = open(I2C_DEVICE, O_RDWR);
-  if (file < 0) {
-    std::cerr << "Error opening device\n";
+void get_pwm_table() {
+  // Open the data file
+  std::ifstream file("src/vortex-asv/motion/thruster_interface/config/"
+                     "ThrustMe_P1000_force_mapping.csv");
+  if (!file.is_open()) {
+    std::cerr << "Unable to open PWM table\n";
     exit(1);
   }
 
-  if (ioctl(file, I2C_SLAVE, I2C_ADDRESS) < 0) {
-    std::cerr << "Error setting I2C address\n";
-    exit(1);
+  std::string line;
+  // Ignore the header line
+  std::getline(file, line);
+  // read line by line
+  while (std::getline(file, line)) {
+    std::istringstream stream(line);
+    std::string force_str, pwm_str;
+    std::getline(stream, force_str, '\t'); // read the line until a tab is found
+    std::getline(stream, pwm_str);         // read the rest of the line
+    // convert into float
+    float force = std::stod(force_str);
+    float pwm = std::stod(pwm_str);
+    // add values to the the table
+    pwm_table[force] = pwm;
   }
+  file.close();
+}
+
+uint16_t interpolate(double force, int PWM_min, int PWM_max) {
+  uint16_t pwm;
+  if (pwm_table.empty()) {
+    get_pwm_table();
+  }
+  // Find the first element with a key not less than force
+  auto it = pwm_table.lower_bound(force);
+  if (it == pwm_table.begin()) { // If the force is less than or equal to the
+                                 // smallest force in the table
+    pwm = it->second;
+  } else if (it == pwm_table.end()) { // If the force is greater than or equal
+                                      // to the largest force in the table
+    --it;
+    pwm = it->second;
+  } else {
+    // Linear interpolation
+    auto prev = std::prev(it); // Get the element with the next smaller key
+    double force1 = prev->first;
+    double force2 = it->first;
+    double pwm1 = prev->second;
+    double pwm2 = it->second;
+    pwm = pwm1 + (pwm2 - pwm1) * (force - force1) / (force2 - force1);
+  }
+  // check if the calculated pwm values are within the defined limits [PWM_min,
+  // PWM_max].
+  if (pwm < PWM_min) {
+    return PWM_min;
+  } else if (pwm > PWM_max) {
+    return PWM_max;
+  } else {
+    return pwm;
+  }
+}
 
-  // Send the I2C message
-  if (write(file, pwm_bytes.data(), data_size) != data_size) {
-    std::cerr << "Error sending data, ignoring message...\n";
+std::vector<uint16_t> interpolate_all(std::vector<double> &force_values,
+                                      int PWM_min, int PWM_max) {
+  std::vector<uint16_t> interpolatedVector;
+  // Interpolate each value in the input vector
+  for (const auto &force : force_values) {
+    interpolatedVector.push_back(interpolate(force, PWM_min, PWM_max));
   }
 
-  close(file);
+  return interpolatedVector;
 }