From e554af28ed0f00d8e6e3c4c89342f8c6904e6011 Mon Sep 17 00:00:00 2001
From: Jens Vanhooydonck <jensvanhooydonck@gmail.com>
Date: Tue, 2 Jul 2024 08:47:01 +0200
Subject: [PATCH] Changed EC_READ_REAL and EC_WRITE_REAL to bitcasting.
 EC_READ_REAL and EC_WRITE_REAL aren't always available.

---
 .../ethercat_interface/ec_pdo_channel_manager.hpp      |  9 ++++++---
 .../include/ethercat_interface/ec_sdo_manager.hpp      | 10 ++++++++--
 2 files changed, 14 insertions(+), 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
index 6da1650f..3c36b7a0 100644
--- 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
index 500f5f49..77bd5fa6 100644
--- 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)