Add real32 (Beckhoff float) and real64 (Beckhoff double) on jazzy

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));
@@ -79,7 +79,15 @@ class SdoConfigEntry
     }
     // value
     if (sdo_config["value"]) {
-      data = sdo_config["value"].as<int>();
+      if (data_type == "float" || data_type == "real32") {
+        float floatvalue = sdo_config["value"].as<float>();
+        data = *reinterpret_cast<int *>(&floatvalue);
+      } else if (data_type == "double" || data_type == "real64") {
+        double doublevalue = sdo_config["value"].as<double>();
+        data = *reinterpret_cast<int *>(&doublevalue);
+      } else {
+        data = sdo_config["value"].as<int>();
+      }
     } else {
       std::cerr << "sdo " << index << ": missing sdo value" << std::endl;
       return false;
@@ -105,9 +113,9 @@ class SdoConfigEntry
       return 1;
     } else if (type == "int16" || type == "uint16") {
       return 2;
-    } else if (type == "int32" || type == "uint32") {
+    } 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;
     }
     return 0;

ethercat_interface/src/ec_pdo_channel_manager.cpp

@@ -30,7 +30,9 @@ const std::vector<std::string> ec_pdo_channel_data_types = {
   "int8", "uint8",
   "int16", "uint16",
   "int32", "uint32",
-  "int64", "uint64"
+  "int64", "uint64",
+  "float", "real32",
+  "double", "real64"
 };
 
 const std::vector<uint8_t> ec_pdo_channel_data_bits = {
@@ -40,6 +42,8 @@ const std::vector<uint8_t> ec_pdo_channel_data_bits = {
   8, 8,
   16, 16,
   32, 32,
+  64, 64,
+  32, 32,
   64, 64
 };
 
@@ -181,6 +185,22 @@ double int64_read(uint8_t * domain_address, uint8_t /*data_mask*/)
   return static_cast<double>(EC_READ_S64(domain_address));
 }
 
+double real32_read(uint8_t * domain_address, uint8_t /*data_mask*/)
+{
+  uint32_t raw = EC_READ_U32(domain_address);
+  float value;
+  std::memcpy(&value, &raw, sizeof(value));
+  return value;
+}
+
+double real64_read(uint8_t * domain_address, uint8_t /*data_mask*/)
+{
+  uint64_t raw = EC_READ_U64(domain_address);
+  double value;
+  std::memcpy(&value, &raw, sizeof(value));
+  return value;
+}
+
 double bool_read(uint8_t * domain_address, uint8_t data_mask)
 {
   return (EC_READ_U8(domain_address) & data_mask) ? 1. : 0.;
@@ -199,6 +219,8 @@ const SingleReadFunctionType ec_pdo_single_read_functions[] = {
   int16_read, uint16_read,
   int32_read, uint32_read,
   int64_read, uint64_read,
+  real32_read, real32_read,
+  real64_read, real64_read,
   octet_read
 };
 
@@ -246,6 +268,21 @@ void int64_write(uint8_t * domain_address, double value, uint8_t /*data_mask*/)
   EC_WRITE_S64(domain_address, static_cast<int64_t>(value));
 }
 
+void real32_write(uint8_t * domain_address, double value, uint8_t /*data_mask*/)
+{
+  float f = static_cast<float>(value);
+  uint32_t raw;
+  std::memcpy(&raw, &f, sizeof(raw));
+  EC_WRITE_U32(domain_address, static_cast<uint32_t>(raw));
+}
+
+void real64_write(uint8_t * domain_address, double value, uint8_t /*data_mask*/)
+{
+  uint64_t raw;
+  std::memcpy(&raw, &value, sizeof(raw));
+  EC_WRITE_U64(domain_address, static_cast<uint64_t>(raw));
+}
+
 /** @brief Helper function that counts the number of bits in an octet */
 inline
 uint8_t count_bits(uint8_t octet)
@@ -299,6 +336,8 @@ const SingleWriteFunctionType ec_pdo_single_write_functions[] = {
   int16_write, uint16_write,
   int32_write, uint32_write,
   int64_write, uint64_write,
+  real32_write, real32_write,
+  real64_write, real64_write,
   octet_override
 };