Working SDO + altered casting of floats + added doubles

ICube-Robotics · Jul 2, 2024 · 2a5d776 · 2a5d776
1 parent e554af2
commit 2a5d776
Show file tree

Hide file tree

Showing 3 changed files with 20 additions and 9 deletions.
diff --git a/ethercat_generic_plugins/ethercat_generic_slave/src/generic_ec_slave.cpp b/ethercat_generic_plugins/ethercat_generic_slave/src/generic_ec_slave.cpp
@@ -27,7 +27,7 @@ size_t type2bytes(std::string type)
     return 2;
   } else if (type == "int32" || type == "uint32" || type == "float" || type == "real32") {
     return 4;
-  } else if (type == "int64" || type == "uint64") {
+  } else if (type == "int64" || type == "uint64" || type == "double" || type == "real64") {
     return 8;
   }
 }

diff --git a/ethercat_interface/include/ethercat_interface/ec_pdo_channel_manager.hpp b/ethercat_interface/include/ethercat_interface/ec_pdo_channel_manager.hpp
@@ -67,7 +67,11 @@ class EcPdoChannelManager
       last_value = static_cast<double>(EC_READ_S64(domain_address));
     } else if (data_type == "real32" || data_type == "float") {
       uint32_t raw = EC_READ_U32(domain_address);
-      float value = *(float *)(const void *)&raw;
+      float value = *(float *)&raw;
+      last_value = static_cast<double>(value);
+    } else if (data_type == "real64" || data_type == "double") {
+      uint64_t raw = EC_READ_U64(domain_address);
+      double value = *(double *)&raw;
       last_value = static_cast<double>(value);
     } else if (data_type == "bool") {
       last_value = (EC_READ_U8(domain_address) & data_mask) ? 1 : 0;
@@ -97,8 +101,12 @@ class EcPdoChannelManager
     } else if (data_type == "int64") {
       EC_WRITE_S64(domain_address, static_cast<int64_t>(value));
     } else if (data_type == "real32" || data_type == "float") {
-      uint32_t raw = *(uint32_t *)(const void *)&value;
+      float f = (float)value;
+      uint32_t raw = *(uint32_t *)&f;
       EC_WRITE_U32(domain_address, static_cast<uint32_t>(raw));
+    } else if (data_type == "real64" || data_type == "double") {
+      uint32_t raw = *(uint32_t *)&value;
+      EC_WRITE_U64(domain_address, static_cast<uint64_t>(raw));
     } else {
       buffer_ = EC_READ_U8(domain_address);
       if (popcount(data_mask) == 1) {
@@ -201,7 +209,7 @@ class EcPdoChannelManager
       return 16;
     } else if (type == "int32" || type == "uint32" || type == "float" || type == "real32") {
       return 32;
-    } else if (type == "int64" || type == "uint64") {
+    } else if (type == "int64" || type == "uint64" || type == "double" || type == "real64") {
       return 64;
     } else if (type.find("bit") != std::string::npos) {
       std::string n_bits = type.substr(type.find("bit") + 3);

diff --git a/ethercat_interface/include/ethercat_interface/ec_sdo_manager.hpp b/ethercat_interface/include/ethercat_interface/ec_sdo_manager.hpp
@@ -43,11 +43,11 @@ class SdoConfigEntry
       EC_WRITE_U16(buffer, static_cast<uint16_t>(data));
     } else if (data_type == "int16") {
       EC_WRITE_S16(buffer, static_cast<int16_t>(data));
-    } else if (data_type == "uint32") {
+    } else if (data_type == "uint32" || data_type == "real32" || data_type == "float") {
       EC_WRITE_U32(buffer, static_cast<uint32_t>(data));
     } else if (data_type == "int32") {
       EC_WRITE_S32(buffer, static_cast<int32_t>(data));
-    } else if (data_type == "uint64") {
+    } else if (data_type == "uint64" || data_type == "real64" || data_type == "double") {
       EC_WRITE_U64(buffer, static_cast<uint64_t>(data));
     } else if (data_type == "int64") {
       EC_WRITE_S64(buffer, static_cast<int64_t>(data));
@@ -83,7 +83,11 @@ class SdoConfigEntry
     // value
     if (sdo_config["value"]) {
       if (data_type == "float" || data_type == "real32") {
-        doubledata = sdo_config["value"].as<double>();
+        float floatvalue = sdo_config["value"].as<float>();
+        data = *(int *)&floatvalue;
+      } else if (data_type == "double" || data_type == "real64") {
+        float doublevalue = sdo_config["value"].as<double>();
+        data = *(int *)&doublevalue;
       } else {
         data = sdo_config["value"].as<int>();
       }
@@ -104,7 +108,6 @@ class SdoConfigEntry
   uint8_t sub_index;
   std::string data_type;
   int data;
-  double doubledata;
 
 private:
   size_t type2bytes(std::string type)
@@ -115,7 +118,7 @@ class SdoConfigEntry
       return 2;
     } else if (type == "int32" || type == "uint32" || type == "float" || type == "real32") {
       return 4;
-    } else if (type == "int64" || type == "uint64") {
+    } else if (type == "int64" || type == "uint64" || type == "double" || type == "real64") {
       return 8;
     }
   }