Changed EC_READ_REAL and EC_WRITE_REAL to bitcasting. EC_READ_REAL an…

…d EC_WRITE_REAL aren't always available.
ICube-Robotics · Jul 2, 2024 · e554af2 · e554af2
1 parent 7dc0ccf
commit e554af2
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 5 deletions.
diff --git a/ethercat_interface/include/ethercat_interface/ec_pdo_channel_manager.hpp b/ethercat_interface/include/ethercat_interface/ec_pdo_channel_manager.hpp
@@ -66,7 +66,9 @@ class EcPdoChannelManager
     } else if (data_type == "int64") {
       last_value = static_cast<double>(EC_READ_S64(domain_address));
     } else if (data_type == "real32" || data_type == "float") {
-      last_value = static_cast<double>(EC_READ_REAL(domain_address));
+      uint32_t raw = EC_READ_U32(domain_address);
+      float value = *(float *)(const void *)&raw;
+      last_value = static_cast<double>(value);
     } else if (data_type == "bool") {
       last_value = (EC_READ_U8(domain_address) & data_mask) ? 1 : 0;
     } else {
@@ -95,7 +97,8 @@ 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") {
-      EC_WRITE_REAL(domain_address, static_cast<float>(value));
+      uint32_t raw = *(uint32_t *)(const void *)&value;
+      EC_WRITE_U32(domain_address, static_cast<uint32_t>(raw));
     } else {
       buffer_ = EC_READ_U8(domain_address);
       if (popcount(data_mask) == 1) {
@@ -196,7 +199,7 @@ class EcPdoChannelManager
       return 8;
     } else if (type == "int16" || type == "uint16") {
       return 16;
-    } else if (type == "int32" || type == "uint32") {
+    } else if (type == "int32" || type == "uint32" || type == "float" || type == "real32") {
       return 32;
     } else if (type == "int64" || type == "uint64") {
       return 64;

diff --git a/ethercat_interface/include/ethercat_interface/ec_sdo_manager.hpp b/ethercat_interface/include/ethercat_interface/ec_sdo_manager.hpp
@@ -52,7 +52,8 @@ class SdoConfigEntry
     } else if (data_type == "int64") {
       EC_WRITE_S64(buffer, static_cast<int64_t>(data));
     } else if (data_type == "real32" || data_type == "float") {
-      EC_WRITE_REAL(buffer, static_cast<float>(data));
+      uint32_t raw = *(uint32_t *)(const void *)&doubledata;
+      EC_WRITE_U32(buffer, static_cast<uint32_t>(raw));
     }
   }
 
@@ -81,7 +82,11 @@ class SdoConfigEntry
     }
     // value
     if (sdo_config["value"]) {
-      data = sdo_config["value"].as<int>();
+      if (data_type == "float" || data_type == "real32") {
+        doubledata = sdo_config["value"].as<double>();
+      } else {
+        data = sdo_config["value"].as<int>();
+      }
     } else {
       std::cerr << "sdo " << index << ": missing sdo value" << std::endl;
       return false;
@@ -99,6 +104,7 @@ class SdoConfigEntry
   uint8_t sub_index;
   std::string data_type;
   int data;
+  double doubledata;
 
 private:
   size_t type2bytes(std::string type)